Skip to content
Snippets Groups Projects

Compare revisions

Changes are shown as if the source revision was being merged into the target revision. Learn more about comparing revisions.

Source

Select target project
No results found

Target

Select target project
  • gsaurel/dynamic_graph_bridge
  • stack-of-tasks/dynamic_graph_bridge
2 results
Show changes
Showing
with 355 additions and 360 deletions
SET(plugins set(plugins ros_publish ros_subscribe ros_queued_subscribe ros_tf_listener
ros_publish ros_time)
ros_subscribe
ros_queued_subscribe
ros_tf_listener
ros_time
)
FOREACH(plugin ${plugins}) foreach(plugin ${plugins})
GET_FILENAME_COMPONENT(LIBRARY_NAME ${plugin} NAME) get_filename_component(LIBRARY_NAME ${plugin} NAME)
ADD_LIBRARY(${LIBRARY_NAME} SHARED ${plugin}.cpp ${plugin}.hh) add_library(${LIBRARY_NAME} SHARED ${plugin}.cpp ${plugin}.hh)
IF(SUFFIX_SO_VERSION) if(SUFFIX_SO_VERSION)
SET_TARGET_PROPERTIES(${LIBRARY_NAME} PROPERTIES SOVERSION ${PROJECT_VERSION}) set_target_properties(${LIBRARY_NAME} PROPERTIES SOVERSION
ENDIF(SUFFIX_SO_VERSION) ${PROJECT_VERSION})
endif(SUFFIX_SO_VERSION)
TARGET_LINK_LIBRARIES(${LIBRARY_NAME} ${${LIBRARY_NAME}_deps} ${catkin_LIBRARIES} ros_bridge) target_link_libraries(${LIBRARY_NAME} ${${LIBRARY_NAME}_deps}
PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} dynamic_graph_bridge_msgs) ${catkin_LIBRARIES} ros_bridge)
IF(NOT INSTALL_PYTHON_INTERFACE_ONLY) if(NOT INSTALL_PYTHON_INTERFACE_ONLY)
INSTALL(TARGETS ${LIBRARY_NAME} EXPORT ${TARGETS_EXPORT_NAME} install(
TARGETS ${LIBRARY_NAME}
EXPORT ${TARGETS_EXPORT_NAME}
DESTINATION ${DYNAMIC_GRAPH_PLUGINDIR}) DESTINATION ${DYNAMIC_GRAPH_PLUGINDIR})
ENDIF(NOT INSTALL_PYTHON_INTERFACE_ONLY) endif(NOT INSTALL_PYTHON_INTERFACE_ONLY)
IF(BUILD_PYTHON_INTERFACE) if(BUILD_PYTHON_INTERFACE)
STRING(REPLACE - _ PYTHON_LIBRARY_NAME ${LIBRARY_NAME}) string(REPLACE - _ PYTHON_LIBRARY_NAME ${LIBRARY_NAME})
if(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/${plugin}-python-module-py.cc") if(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/${plugin}-python-module-py.cc")
DYNAMIC_GRAPH_PYTHON_MODULE("ros/${PYTHON_LIBRARY_NAME}" dynamic_graph_python_module(
${LIBRARY_NAME} ${PROJECT_NAME}-${PYTHON_LIBRARY_NAME}-wrap "ros/${PYTHON_LIBRARY_NAME}" ${LIBRARY_NAME}
SOURCE_PYTHON_MODULE "${CMAKE_CURRENT_SOURCE_DIR}/${plugin}-python-module-py.cc") ${PROJECT_NAME}-${PYTHON_LIBRARY_NAME}-wrap SOURCE_PYTHON_MODULE
"${CMAKE_CURRENT_SOURCE_DIR}/${plugin}-python-module-py.cc")
elseif(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/${plugin}-python.hh") elseif(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/${plugin}-python.hh")
DYNAMIC_GRAPH_PYTHON_MODULE("ros/${PYTHON_LIBRARY_NAME}" dynamic_graph_python_module(
${LIBRARY_NAME} ${PROJECT_NAME}-${PYTHON_LIBRARY_NAME}-wrap "ros/${PYTHON_LIBRARY_NAME}" ${LIBRARY_NAME}
MODULE_HEADER "${CMAKE_CURRENT_SOURCE_DIR}/${plugin}-python.hh") ${PROJECT_NAME}-${PYTHON_LIBRARY_NAME}-wrap MODULE_HEADER
"${CMAKE_CURRENT_SOURCE_DIR}/${plugin}-python.hh")
endif() endif()
ENDIF(BUILD_PYTHON_INTERFACE) endif(BUILD_PYTHON_INTERFACE)
ENDFOREACH(plugin) endforeach(plugin)
target_link_libraries(ros_publish ros_bridge) target_link_libraries(ros_publish ros_bridge)
IF(BUILD_PYTHON_INTERFACE) if(BUILD_PYTHON_INTERFACE)
PYTHON_INSTALL_ON_SITE("dynamic_graph/ros" "__init__.py") python_install_on_site("dynamic_graph/ros" "__init__.py")
PYTHON_INSTALL_ON_SITE("dynamic_graph/ros" "ros.py") python_install_on_site("dynamic_graph/ros" "ros.py")
PYTHON_INSTALL_ON_SITE("dynamic_graph/ros" "dgcompleter.py") python_install_on_site("dynamic_graph/ros" "dgcompleter.py")
# ros_interperter library. # ros_interperter library.
add_library(ros_interpreter ros_interpreter.cpp) add_library(ros_interpreter ros_interpreter.cpp)
TARGET_LINK_LIBRARIES(ros_interpreter ros_bridge ${catkin_LIBRARIES} target_link_libraries(ros_interpreter ros_bridge ${catkin_LIBRARIES}
dynamic-graph-python::dynamic-graph-python) dynamic-graph-python::dynamic-graph-python)
PKG_CONFIG_USE_DEPENDENCY(ros_interpreter dynamic_graph_bridge_msgs)
install(TARGETS ros_interpreter install(
TARGETS ros_interpreter
EXPORT ${TARGETS_EXPORT_NAME} EXPORT ${TARGETS_EXPORT_NAME}
DESTINATION lib) DESTINATION lib)
ENDIF(BUILD_PYTHON_INTERFACE) endif(BUILD_PYTHON_INTERFACE)
# Stand alone embedded intepreter with a robot controller. # Stand alone embedded intepreter with a robot controller.
add_executable(geometric_simu geometric_simu.cpp sot_loader.cpp sot_loader_basic.cpp) add_executable(geometric_simu geometric_simu.cpp sot_loader.cpp
target_link_libraries(geometric_simu Boost::program_options ${CMAKE_DL_LIBS} ${catkin_LIBRARIES} ros_bridge) sot_loader_basic.cpp)
pkg_config_use_dependency(geometric_simu dynamic_graph_bridge_msgs) target_link_libraries(geometric_simu Boost::program_options ${CMAKE_DL_LIBS}
install(TARGETS geometric_simu ${catkin_LIBRARIES} ros_bridge)
DESTINATION lib/${PROJECT_NAME}) install(TARGETS geometric_simu DESTINATION lib/${PROJECT_NAME})
# Sot loader library # Sot loader library
add_library(sot_loader sot_loader.cpp sot_loader_basic.cpp) add_library(sot_loader sot_loader.cpp sot_loader_basic.cpp)
target_link_libraries(sot_loader Boost::program_options ${catkin_LIBRARIES} ros_bridge) target_link_libraries(sot_loader Boost::program_options ${catkin_LIBRARIES}
pkg_config_use_dependency(sot_loader dynamic_graph_bridge_msgs) ros_bridge)
install(TARGETS sot_loader EXPORT ${TARGETS_EXPORT_NAME} DESTINATION lib) install(
TARGETS sot_loader
EXPORT ${TARGETS_EXPORT_NAME}
DESTINATION lib)
#ifndef DYNAMIC_GRAPH_ROS_CONVERTER_HH #ifndef DYNAMIC_GRAPH_ROS_CONVERTER_HH
#define DYNAMIC_GRAPH_ROS_CONVERTER_HH #define DYNAMIC_GRAPH_ROS_CONVERTER_HH
#include <stdexcept> #include <ros/time.h>
#include "sot_to_ros.hh" #include <std_msgs/Header.h>
#include <boost/static_assert.hpp>
#include <boost/date_time/date.hpp> #include <boost/date_time/date.hpp>
#include <boost/date_time/posix_time/posix_time.hpp> #include <boost/date_time/posix_time/posix_time.hpp>
#include <boost/static_assert.hpp>
#include <stdexcept>
#include <ros/time.h> #include "sot_to_ros.hh"
#include <std_msgs/Header.h>
#define SOT_TO_ROS_IMPL(T) \ #define SOT_TO_ROS_IMPL(T) \
template <> \ template <> \
inline void converter(SotToRos<T>::ros_t& dst, const SotToRos<T>::sot_t& src) inline void converter(SotToRos<T>::ros_t& dst, \
const SotToRos<T>::sot_t& src)
#define ROS_TO_SOT_IMPL(T) \ #define ROS_TO_SOT_IMPL(T) \
template <> \ template <> \
inline void converter(SotToRos<T>::sot_t& dst, const SotToRos<T>::ros_t& src) inline void converter(SotToRos<T>::sot_t& dst, \
const SotToRos<T>::ros_t& src)
namespace dynamicgraph { namespace dynamicgraph {
inline void makeHeader(std_msgs::Header& header) { inline void makeHeader(std_msgs::Header& header) {
...@@ -99,7 +101,8 @@ SOT_TO_ROS_IMPL(Matrix) { ...@@ -99,7 +101,8 @@ SOT_TO_ROS_IMPL(Matrix) {
} }
ROS_TO_SOT_IMPL(Matrix) { ROS_TO_SOT_IMPL(Matrix) {
dst.resize(src.width, (unsigned int)src.data.size() / (unsigned int)src.width); dst.resize(src.width,
(unsigned int)src.data.size() / (unsigned int)src.width);
for (unsigned i = 0; i < src.data.size(); ++i) dst.data()[i] = src.data[i]; for (unsigned i = 0; i < src.data.size(); ++i) dst.data()[i] = src.data[i];
} }
...@@ -117,7 +120,8 @@ SOT_TO_ROS_IMPL(sot::MatrixHomogeneous) { ...@@ -117,7 +120,8 @@ SOT_TO_ROS_IMPL(sot::MatrixHomogeneous) {
} }
ROS_TO_SOT_IMPL(sot::MatrixHomogeneous) { ROS_TO_SOT_IMPL(sot::MatrixHomogeneous) {
sot::VectorQuaternion q(src.rotation.w, src.rotation.x, src.rotation.y, src.rotation.z); sot::VectorQuaternion q(src.rotation.w, src.rotation.x, src.rotation.y,
src.rotation.z);
dst.linear() = q.matrix(); dst.linear() = q.matrix();
// Copy the translation component. // Copy the translation component.
...@@ -128,7 +132,8 @@ ROS_TO_SOT_IMPL(sot::MatrixHomogeneous) { ...@@ -128,7 +132,8 @@ ROS_TO_SOT_IMPL(sot::MatrixHomogeneous) {
// Twist. // Twist.
SOT_TO_ROS_IMPL(specific::Twist) { SOT_TO_ROS_IMPL(specific::Twist) {
if (src.size() != 6) throw std::runtime_error("failed to convert invalid twist"); if (src.size() != 6)
throw std::runtime_error("failed to convert invalid twist");
dst.linear.x = src(0); dst.linear.x = src(0);
dst.linear.y = src(1); dst.linear.y = src(1);
dst.linear.z = src(2); dst.linear.z = src(2);
...@@ -163,7 +168,8 @@ ROS_TO_SOT_IMPL(specific::Twist) { ...@@ -163,7 +168,8 @@ ROS_TO_SOT_IMPL(specific::Twist) {
struct e_n_d__w_i_t_h__s_e_m_i_c_o_l_o_n struct e_n_d__w_i_t_h__s_e_m_i_c_o_l_o_n
DG_BRIDGE_TO_ROS_MAKE_STAMPED_IMPL(specific::Vector3, vector, ;); DG_BRIDGE_TO_ROS_MAKE_STAMPED_IMPL(specific::Vector3, vector, ;);
DG_BRIDGE_TO_ROS_MAKE_STAMPED_IMPL(sot::MatrixHomogeneous, transform, dst.child_frame_id = "";); DG_BRIDGE_TO_ROS_MAKE_STAMPED_IMPL(sot::MatrixHomogeneous, transform,
dst.child_frame_id = "";);
DG_BRIDGE_TO_ROS_MAKE_STAMPED_IMPL(specific::Twist, twist, ;); DG_BRIDGE_TO_ROS_MAKE_STAMPED_IMPL(specific::Twist, twist, ;);
/// \brief This macro generates a converter for a shared pointer on /// \brief This macro generates a converter for a shared pointer on
...@@ -172,11 +178,13 @@ DG_BRIDGE_TO_ROS_MAKE_STAMPED_IMPL(specific::Twist, twist, ;); ...@@ -172,11 +178,13 @@ DG_BRIDGE_TO_ROS_MAKE_STAMPED_IMPL(specific::Twist, twist, ;);
/// A converter for the underlying type is required. I.e. to /// A converter for the underlying type is required. I.e. to
/// convert a shared_ptr<T> to T', a converter from T to T' /// convert a shared_ptr<T> to T', a converter from T to T'
/// is required. /// is required.
#define DG_BRIDGE_MAKE_SHPTR_IMPL(T) \ #define DG_BRIDGE_MAKE_SHPTR_IMPL(T) \
template <> \ template <> \
inline void converter(SotToRos<T>::sot_t& dst, const boost::shared_ptr<SotToRos<T>::ros_t const>& src) { \ inline void converter( \
converter<SotToRos<T>::sot_t, SotToRos<T>::ros_t>(dst, *src); \ SotToRos<T>::sot_t& dst, \
} \ const boost::shared_ptr<SotToRos<T>::ros_t const>& src) { \
converter<SotToRos<T>::sot_t, SotToRos<T>::ros_t>(dst, *src); \
} \
struct e_n_d__w_i_t_h__s_e_m_i_c_o_l_o_n struct e_n_d__w_i_t_h__s_e_m_i_c_o_l_o_n
DG_BRIDGE_MAKE_SHPTR_IMPL(bool); DG_BRIDGE_MAKE_SHPTR_IMPL(bool);
...@@ -213,15 +221,17 @@ DG_BRIDGE_MAKE_STAMPED_IMPL(specific::Twist, twist, ;); ...@@ -213,15 +221,17 @@ DG_BRIDGE_MAKE_STAMPED_IMPL(specific::Twist, twist, ;);
/// a stamped type. I.e. A data associated with its timestamp. /// a stamped type. I.e. A data associated with its timestamp.
/// ///
/// FIXME: the timestamp is not yet forwarded to the dg signal. /// FIXME: the timestamp is not yet forwarded to the dg signal.
#define DG_BRIDGE_MAKE_STAMPED_SHPTR_IMPL(T, ATTRIBUTE, EXTRA) \ #define DG_BRIDGE_MAKE_STAMPED_SHPTR_IMPL(T, ATTRIBUTE, EXTRA) \
template <> \ template <> \
inline void converter(SotToRos<std::pair<T, Vector> >::sot_t& dst, \ inline void converter( \
const boost::shared_ptr<SotToRos<std::pair<T, Vector> >::ros_t const>& src) { \ SotToRos<std::pair<T, Vector> >::sot_t& dst, \
converter<SotToRos<T>::sot_t, SotToRos<T>::ros_t>(dst, src->ATTRIBUTE); \ const boost::shared_ptr<SotToRos<std::pair<T, Vector> >::ros_t const>& \
do { \ src) { \
EXTRA \ converter<SotToRos<T>::sot_t, SotToRos<T>::ros_t>(dst, src->ATTRIBUTE); \
} while (0); \ do { \
} \ EXTRA \
} while (0); \
} \
struct e_n_d__w_i_t_h__s_e_m_i_c_o_l_o_n struct e_n_d__w_i_t_h__s_e_m_i_c_o_l_o_n
DG_BRIDGE_MAKE_STAMPED_SHPTR_IMPL(specific::Vector3, vector, ;); DG_BRIDGE_MAKE_STAMPED_SHPTR_IMPL(specific::Vector3, vector, ;);
...@@ -251,7 +261,8 @@ typedef boost::posix_time::time_duration time_duration; ...@@ -251,7 +261,8 @@ typedef boost::posix_time::time_duration time_duration;
typedef boost::gregorian::date date; typedef boost::gregorian::date date;
boost::posix_time::ptime rosTimeToPtime(const ros::Time& rosTime) { boost::posix_time::ptime rosTimeToPtime(const ros::Time& rosTime) {
ptime time(date(1970, 1, 1), seconds(rosTime.sec) + microseconds(rosTime.nsec / 1000)); ptime time(date(1970, 1, 1),
seconds(rosTime.sec) + microseconds(rosTime.nsec / 1000));
return time; return time;
} }
...@@ -259,7 +270,8 @@ ros::Time pTimeToRostime(const boost::posix_time::ptime& time) { ...@@ -259,7 +270,8 @@ ros::Time pTimeToRostime(const boost::posix_time::ptime& time) {
static ptime timeStart(date(1970, 1, 1)); static ptime timeStart(date(1970, 1, 1));
time_duration diff = time - timeStart; time_duration diff = time - timeStart;
uint32_t sec = (unsigned int)diff.ticks() / (unsigned int)time_duration::rep_type::res_adjust(); uint32_t sec = (unsigned int)diff.ticks() /
(unsigned int)time_duration::rep_type::res_adjust();
uint32_t nsec = (unsigned int)diff.fractional_seconds(); uint32_t nsec = (unsigned int)diff.fractional_seconds();
return ros::Time(sec, nsec); return ros::Time(sec, nsec);
......
...@@ -3,3 +3,4 @@ from .ros import RosPublish as RosImport ...@@ -3,3 +3,4 @@ from .ros import RosPublish as RosImport
from .ros import RosSubscribe as RosExport from .ros import RosSubscribe as RosExport
from .ros_publish import RosPublish from .ros_publish import RosPublish
from .ros_subscribe import RosSubscribe from .ros_subscribe import RosSubscribe
from .ros_queued_subscribe import RosQueuedSubscribe
...@@ -62,18 +62,18 @@ class DGCompleter: ...@@ -62,18 +62,18 @@ class DGCompleter:
self.client(astr) self.client(astr)
astr = "readline.set_completer(aCompleter.complete)" astr = "readline.set_completer(aCompleter.complete)"
self.client(astr) self.client(astr)
astr = "readline.parse_and_bind(\"tab: complete\")" astr = 'readline.parse_and_bind("tab: complete")'
self.client(astr) self.client(astr)
def complete(self, text, state): def complete(self, text, state):
"""Return the next possible completion for 'text'. readline.parse_and_bind("tab: complete") """Return the next possible completion for 'text'.
readline.parse_and_bind("tab: complete")
This is called successively with state == 0, 1, 2, ... until it This is called successively with state == 0, 1, 2, ... until it
returns None. The completion should begin with 'text'. returns None. The completion should begin with 'text'.
""" """
astr = "aCompleter.complete(\"" + text + "\"," + str(state) + ")" astr = 'aCompleter.complete("' + text + '",' + str(state) + ")"
response = self.client(astr) response = self.client(astr)
res2 = ast.literal_eval(response.result) res2 = ast.literal_eval(response.result)
return res2 return res2
...@@ -12,13 +12,13 @@ class Ros(object): ...@@ -12,13 +12,13 @@ class Ros(object):
rosImport = None rosImport = None
rosExport = None rosExport = None
def __init__(self, robot, suffix=''): def __init__(self, robot, suffix=""):
self.robot = robot self.robot = robot
self.rosPublish = RosPublish('rosPublish{0}'.format(suffix)) self.rosPublish = RosPublish("rosPublish{0}".format(suffix))
self.rosSubscribe = RosSubscribe('rosSubscribe{0}'.format(suffix)) self.rosSubscribe = RosSubscribe("rosSubscribe{0}".format(suffix))
self.rosTime = RosTime('rosTime{0}'.format(suffix)) self.rosTime = RosTime("rosTime{0}".format(suffix))
self.robot.device.after.addSignal('{0}.trigger'.format(self.rosPublish.name)) self.robot.device.after.addSignal("{0}.trigger".format(self.rosPublish.name))
# aliases, for retro compatibility # aliases, for retro compatibility
self.rosImport = self.rosPublish self.rosImport = self.rosPublish
......
...@@ -5,17 +5,19 @@ ...@@ -5,17 +5,19 @@
* CNRS * CNRS
* *
*/ */
#include <iostream>
#include <ros/console.h> #include <ros/console.h>
#include <iostream>
#define ENABLE_RT_LOG #define ENABLE_RT_LOG
#include <dynamic-graph/real-time-logger.h> #include <dynamic-graph/real-time-logger.h>
#include <dynamic_graph_bridge/sot_loader.hh> #include <dynamic_graph_bridge/sot_loader.hh>
int main(int argc, char *argv[]) { int main(int argc, char *argv[]) {
::dynamicgraph::RealTimeLogger::instance() ::dynamicgraph::RealTimeLogger::instance().addOutputStream(
.addOutputStream(::dynamicgraph::LoggerStreamPtr_t(new dynamicgraph::LoggerIOStream(std::cout))); ::dynamicgraph::LoggerStreamPtr_t(
new dynamicgraph::LoggerIOStream(std::cout)));
ros::init(argc, argv, "sot_ros_encapsulator"); ros::init(argc, argv, "sot_ros_encapsulator");
SotLoader aSotLoader; SotLoader aSotLoader;
...@@ -27,6 +29,6 @@ int main(int argc, char *argv[]) { ...@@ -27,6 +29,6 @@ int main(int argc, char *argv[]) {
// Load dynamic library and run python prologue. // Load dynamic library and run python prologue.
aSotLoader.Initialization(); aSotLoader.Initialization();
ros::waitForShutdown (); ros::waitForShutdown();
return 0; return 0;
} }
#include <stdexcept> #include "dynamic_graph_bridge/ros_init.hh"
#include <boost/make_shared.hpp> #include <boost/make_shared.hpp>
#include <boost/shared_ptr.hpp> #include <boost/shared_ptr.hpp>
#include <stdexcept>
#include "dynamic_graph_bridge/ros_init.hh"
namespace dynamicgraph { namespace dynamicgraph {
struct GlobalRos { struct GlobalRos {
...@@ -17,7 +17,8 @@ struct GlobalRos { ...@@ -17,7 +17,8 @@ struct GlobalRos {
}; };
GlobalRos ros; GlobalRos ros;
ros::NodeHandle& rosInit(bool createAsyncSpinner, bool createMultiThreadedSpinner) { ros::NodeHandle& rosInit(bool createAsyncSpinner,
bool createMultiThreadedSpinner) {
if (!ros.nodeHandle) { if (!ros.nodeHandle) {
int argc = 1; int argc = 1;
char* arg0 = strdup("dynamic_graph_bridge"); char* arg0 = strdup("dynamic_graph_bridge");
...@@ -34,11 +35,14 @@ ros::NodeHandle& rosInit(bool createAsyncSpinner, bool createMultiThreadedSpinne ...@@ -34,11 +35,14 @@ ros::NodeHandle& rosInit(bool createAsyncSpinner, bool createMultiThreadedSpinne
// priority // priority
int oldThreadPolicy, newThreadPolicy; int oldThreadPolicy, newThreadPolicy;
struct sched_param oldThreadParam, newThreadParam; struct sched_param oldThreadParam, newThreadParam;
if (pthread_getschedparam(pthread_self(), &oldThreadPolicy, &oldThreadParam) == 0) { if (pthread_getschedparam(pthread_self(), &oldThreadPolicy,
&oldThreadParam) == 0) {
newThreadPolicy = SCHED_OTHER; newThreadPolicy = SCHED_OTHER;
newThreadParam = oldThreadParam; newThreadParam = oldThreadParam;
newThreadParam.sched_priority -= 5; // Arbitrary number, TODO: choose via param/file? newThreadParam.sched_priority -=
if (newThreadParam.sched_priority < sched_get_priority_min(newThreadPolicy)) 5; // Arbitrary number, TODO: choose via param/file?
if (newThreadParam.sched_priority <
sched_get_priority_min(newThreadPolicy))
newThreadParam.sched_priority = sched_get_priority_min(newThreadPolicy); newThreadParam.sched_priority = sched_get_priority_min(newThreadPolicy);
pthread_setschedparam(pthread_self(), newThreadPolicy, &newThreadParam); pthread_setschedparam(pthread_self(), newThreadPolicy, &newThreadParam);
......
...@@ -4,37 +4,49 @@ namespace dynamicgraph { ...@@ -4,37 +4,49 @@ namespace dynamicgraph {
static const int queueSize = 5; static const int queueSize = 5;
Interpreter::Interpreter(ros::NodeHandle& nodeHandle) Interpreter::Interpreter(ros::NodeHandle& nodeHandle)
: interpreter_(), nodeHandle_(nodeHandle), runCommandSrv_(), runPythonFileSrv_() {} : interpreter_(),
nodeHandle_(nodeHandle),
runCommandSrv_(),
runPythonFileSrv_() {}
void Interpreter::startRosService() { void Interpreter::startRosService() {
runCommandCallback_t runCommandCb = boost::bind(&Interpreter::runCommandCallback, this, _1, _2); runCommandCallback_t runCommandCb =
boost::bind(&Interpreter::runCommandCallback, this, _1, _2);
runCommandSrv_ = nodeHandle_.advertiseService("run_command", runCommandCb); runCommandSrv_ = nodeHandle_.advertiseService("run_command", runCommandCb);
runPythonFileCallback_t runPythonFileCb = boost::bind(&Interpreter::runPythonFileCallback, this, _1, _2); runPythonFileCallback_t runPythonFileCb =
runPythonFileSrv_ = nodeHandle_.advertiseService("run_script", runPythonFileCb); boost::bind(&Interpreter::runPythonFileCallback, this, _1, _2);
runPythonFileSrv_ =
nodeHandle_.advertiseService("run_script", runPythonFileCb);
} }
bool Interpreter::runCommandCallback(dynamic_graph_bridge_msgs::RunCommand::Request& req, bool Interpreter::runCommandCallback(
dynamic_graph_bridge_msgs::RunCommand::Response& res) { dynamic_graph_bridge_msgs::RunCommand::Request& req,
interpreter_.python(req.input, res.result, res.standardoutput, res.standarderror); dynamic_graph_bridge_msgs::RunCommand::Response& res) {
interpreter_.python(req.input, res.result, res.standardoutput,
res.standarderror);
return true; return true;
} }
bool Interpreter::runPythonFileCallback(dynamic_graph_bridge_msgs::RunPythonFile::Request& req, bool Interpreter::runPythonFileCallback(
dynamic_graph_bridge_msgs::RunPythonFile::Response& res) { dynamic_graph_bridge_msgs::RunPythonFile::Request& req,
dynamic_graph_bridge_msgs::RunPythonFile::Response& res) {
interpreter_.runPythonFile(req.input); interpreter_.runPythonFile(req.input);
res.result = "File parsed"; // FIX: It is just an echo, is there a way to res.result = "File parsed"; // FIX: It is just an echo, is there a way to
// have a feedback? // have a feedback?
return true; return true;
} }
void Interpreter::runCommand(const std::string& command, std::string& result, std::string& out, std::string& err) { void Interpreter::runCommand(const std::string& command, std::string& result,
std::string& out, std::string& err) {
interpreter_.python(command, result, out, err); interpreter_.python(command, result, out, err);
if (err.size() > 0) { if (err.size() > 0) {
ROS_ERROR(err.c_str()); ROS_ERROR(err.c_str());
} }
} }
void Interpreter::runPythonFile(std::string ifilename) { interpreter_.runPythonFile(ifilename); } void Interpreter::runPythonFile(std::string ifilename) {
interpreter_.runPythonFile(ifilename);
}
} // end of namespace dynamicgraph. } // end of namespace dynamicgraph.
#include <sot/core/robot-utils.hh> #include <pinocchio/fwd.hpp>
#include "pinocchio/multibody/model.hpp" // include pinocchio first
#include "pinocchio/parsers/urdf.hpp" //
#include <ros/ros.h>
#include <urdf_parser/urdf_parser.h>
#include <stdexcept>
#include <boost/make_shared.hpp> #include <boost/make_shared.hpp>
#include <boost/shared_ptr.hpp> #include <boost/shared_ptr.hpp>
#include <sot/core/robot-utils.hh>
#include <stdexcept>
#include <urdf_parser/urdf_parser.h>
#include <ros/ros.h>
#include "dynamic_graph_bridge/ros_parameter.hh" #include "dynamic_graph_bridge/ros_parameter.hh"
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/parsers/urdf.hpp"
namespace dynamicgraph { namespace dynamicgraph {
bool parameter_server_read_robot_description() bool parameter_server_read_robot_description() {
{
ros::NodeHandle nh; ros::NodeHandle nh;
if (!nh.hasParam("/robot_description")) if (!nh.hasParam("/robot_description")) {
{
ROS_ERROR("No /robot_description parameter"); ROS_ERROR("No /robot_description parameter");
return false; return false;
} }
std::string robot_description; std::string robot_description;
std::string parameter_name("/robot_description"); std::string parameter_name("/robot_description");
nh.getParam(parameter_name,robot_description); nh.getParam(parameter_name, robot_description);
std::string model_name("robot"); std::string model_name("robot");
...@@ -35,11 +35,10 @@ bool parameter_server_read_robot_description() ...@@ -35,11 +35,10 @@ bool parameter_server_read_robot_description()
aRobotUtil = sot::createRobotUtil(model_name); aRobotUtil = sot::createRobotUtil(model_name);
// If the creation is fine // If the creation is fine
if (aRobotUtil != sot::RefVoidRobotUtil()) if (aRobotUtil != sot::RefVoidRobotUtil()) {
{
// Then set the robot model. // Then set the robot model.
aRobotUtil->set_parameter(parameter_name,robot_description); aRobotUtil->set_parameter(parameter_name, robot_description);
ROS_INFO("Set parameter_name : %s.",parameter_name.c_str()); ROS_INFO("Set parameter_name : %s.", parameter_name.c_str());
// Everything went fine. // Everything went fine.
return true; return true;
} }
...@@ -48,7 +47,6 @@ bool parameter_server_read_robot_description() ...@@ -48,7 +47,6 @@ bool parameter_server_read_robot_description()
// Otherwise something went wrong. // Otherwise something went wrong.
return false; return false;
} }
} } // namespace dynamicgraph
#include <dynamic-graph/python/module.hh> #include <dynamic-graph/python/module.hh>
#include "ros_publish.hh" #include "ros_publish.hh"
namespace dg = dynamicgraph; namespace dg = dynamicgraph;
BOOST_PYTHON_MODULE(wrap) {
BOOST_PYTHON_MODULE(wrap)
{
bp::import("dynamic_graph"); bp::import("dynamic_graph");
dg::python::exposeEntity<dg::RosPublish, bp::bases<dg::Entity>, dg::python::AddCommands>() dg::python::exposeEntity<dg::RosPublish, bp::bases<dg::Entity>,
.def("clear", &dg::RosPublish::clear, "Remove all signals writing data to a ROS topic") dg::python::AddCommands>()
.def("rm", &dg::RosPublish::rm, "Remove a signal writing data to a ROS topic", .def("clear", &dg::RosPublish::clear,
bp::args("signal_name")) "Remove all signals writing data to a ROS topic")
.def("list", &dg::RosPublish::list, "List signals writing data to a ROS topic") .def("rm", &dg::RosPublish::rm,
; "Remove a signal writing data to a ROS topic",
bp::args("signal_name"))
.def("list", &dg::RosPublish::list,
"List signals writing data to a ROS topic");
} }
#include <stdexcept> #include "ros_publish.hh"
#include <dynamic-graph/command.h>
#include <dynamic-graph/factory.h>
#include <dynamic-graph/linear-algebra.h>
#include <ros/ros.h>
#include <std_msgs/Float64.h>
#include <std_msgs/UInt32.h>
#include <boost/assign.hpp> #include <boost/assign.hpp>
#include <boost/bind.hpp> #include <boost/bind.hpp>
...@@ -6,17 +13,9 @@ ...@@ -6,17 +13,9 @@
#include <boost/format.hpp> #include <boost/format.hpp>
#include <boost/function.hpp> #include <boost/function.hpp>
#include <boost/make_shared.hpp> #include <boost/make_shared.hpp>
#include <stdexcept>
#include <ros/ros.h>
#include <std_msgs/Float64.h>
#include <std_msgs/UInt32.h>
#include <dynamic-graph/factory.h>
#include <dynamic-graph/command.h>
#include <dynamic-graph/linear-algebra.h>
#include "dynamic_graph_bridge/ros_init.hh" #include "dynamic_graph_bridge/ros_init.hh"
#include "ros_publish.hh"
#define ENABLE_RT_LOG #define ENABLE_RT_LOG
#include <dynamic-graph/real-time-logger.h> #include <dynamic-graph/real-time-logger.h>
...@@ -30,7 +29,10 @@ namespace command { ...@@ -30,7 +29,10 @@ namespace command {
namespace rosPublish { namespace rosPublish {
Add::Add(RosPublish& entity, const std::string& docstring) Add::Add(RosPublish& entity, const std::string& docstring)
: Command(entity, boost::assign::list_of(Value::STRING)(Value::STRING)(Value::STRING), docstring) {} : Command(
entity,
boost::assign::list_of(Value::STRING)(Value::STRING)(Value::STRING),
docstring) {}
Value Add::doExecute() { Value Add::doExecute() {
RosPublish& entity = static_cast<RosPublish&>(owner()); RosPublish& entity = static_cast<RosPublish&>(owner());
...@@ -98,7 +100,8 @@ RosPublish::RosPublish(const std::string& n) ...@@ -98,7 +100,8 @@ RosPublish::RosPublish(const std::string& n)
clock_gettime(CLOCK_REALTIME, &nextPublicationRT_); clock_gettime(CLOCK_REALTIME, &nextPublicationRT_);
} }
} catch (const std::exception& exc) { } catch (const std::exception& exc) {
throw std::runtime_error("Failed to call ros::Time::now ():" + std::string(exc.what())); throw std::runtime_error("Failed to call ros::Time::now ():" +
std::string(exc.what()));
} }
signalRegistration(trigger_); signalRegistration(trigger_);
trigger_.setNeedUpdateFromAllChildren(true); trigger_.setNeedUpdateFromAllChildren(true);
...@@ -120,13 +123,16 @@ RosPublish::RosPublish(const std::string& n) ...@@ -120,13 +123,16 @@ RosPublish::RosPublish(const std::string& n)
RosPublish::~RosPublish() { aofs_.close(); } RosPublish::~RosPublish() { aofs_.close(); }
void RosPublish::display(std::ostream& os) const { os << CLASS_NAME << std::endl; } void RosPublish::display(std::ostream& os) const {
os << CLASS_NAME << std::endl;
}
void RosPublish::rm(const std::string& signal) { void RosPublish::rm(const std::string& signal) {
if (bindedSignal_.find(signal) == bindedSignal_.end()) return; if (bindedSignal_.find(signal) == bindedSignal_.end()) return;
if (signal == "trigger") { if (signal == "trigger") {
std::cerr << "The trigger signal should not be removed. Aborting." << std::endl; std::cerr << "The trigger signal should not be removed. Aborting."
<< std::endl;
return; return;
} }
...@@ -139,8 +145,8 @@ void RosPublish::rm(const std::string& signal) { ...@@ -139,8 +145,8 @@ void RosPublish::rm(const std::string& signal) {
std::vector<std::string> RosPublish::list() const { std::vector<std::string> RosPublish::list() const {
std::vector<std::string> result(bindedSignal_.size()); std::vector<std::string> result(bindedSignal_.size());
std::transform(bindedSignal_.begin(), bindedSignal_.end(), std::transform(bindedSignal_.begin(), bindedSignal_.end(), result.begin(),
result.begin(), [](const auto& pair) { return pair.first; }); [](const auto& pair) { return pair.first; });
return result; return result;
} }
......
#ifndef DYNAMIC_GRAPH_ROS_PUBLISH_HH #ifndef DYNAMIC_GRAPH_ROS_PUBLISH_HH
#define DYNAMIC_GRAPH_ROS_PUBLISH_HH #define DYNAMIC_GRAPH_ROS_PUBLISH_HH
#include <map> #include <dynamic-graph/command.h>
#include <boost/shared_ptr.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/thread/mutex.hpp>
#include <dynamic-graph/entity.h> #include <dynamic-graph/entity.h>
#include <dynamic-graph/signal-time-dependent.h> #include <dynamic-graph/signal-time-dependent.h>
#include <dynamic-graph/command.h> #include <realtime_tools/realtime_publisher.h>
#include <ros/ros.h> #include <ros/ros.h>
#include <realtime_tools/realtime_publisher.h> #include <boost/shared_ptr.hpp>
#include <boost/thread/mutex.hpp>
#include <boost/tuple/tuple.hpp>
#include <fstream>
#include <map>
#include "converter.hh" #include "converter.hh"
#include "sot_to_ros.hh" #include "sot_to_ros.hh"
#include <fstream>
namespace dynamicgraph { namespace dynamicgraph {
class RosPublish; class RosPublish;
...@@ -47,7 +44,9 @@ class RosPublish : public dynamicgraph::Entity { ...@@ -47,7 +44,9 @@ class RosPublish : public dynamicgraph::Entity {
public: public:
typedef boost::function<void(int)> callback_t; typedef boost::function<void(int)> callback_t;
typedef boost::tuple<boost::shared_ptr<dynamicgraph::SignalBase<int> >, callback_t> bindedSignal_t; typedef boost::tuple<boost::shared_ptr<dynamicgraph::SignalBase<int> >,
callback_t>
bindedSignal_t;
static const double ROS_JOINT_STATE_PUBLISHER_RATE; static const double ROS_JOINT_STATE_PUBLISHER_RATE;
...@@ -65,8 +64,11 @@ class RosPublish : public dynamicgraph::Entity { ...@@ -65,8 +64,11 @@ class RosPublish : public dynamicgraph::Entity {
int& trigger(int&, int); int& trigger(int&, int);
template <typename T> template <typename T>
void sendData(boost::shared_ptr<realtime_tools::RealtimePublisher<typename SotToRos<T>::ros_t> > publisher, void sendData(
boost::shared_ptr<typename SotToRos<T>::signalIn_t> signal, int time); boost::shared_ptr<
realtime_tools::RealtimePublisher<typename SotToRos<T>::ros_t> >
publisher,
boost::shared_ptr<typename SotToRos<T>::signalIn_t> signal, int time);
template <typename T> template <typename T>
void add(const std::string& signal, const std::string& topic); void add(const std::string& signal, const std::string& topic);
......
#ifndef DYNAMIC_GRAPH_ROS_PUBLISH_HXX #ifndef DYNAMIC_GRAPH_ROS_PUBLISH_HXX
#define DYNAMIC_GRAPH_ROS_PUBLISH_HXX #define DYNAMIC_GRAPH_ROS_PUBLISH_HXX
#include <vector>
#include <std_msgs/Float64.h> #include <std_msgs/Float64.h>
#include <vector>
#include "dynamic_graph_bridge_msgs/Matrix.h" #include "dynamic_graph_bridge_msgs/Matrix.h"
#include "dynamic_graph_bridge_msgs/Vector.h" #include "dynamic_graph_bridge_msgs/Vector.h"
#include "sot_to_ros.hh" #include "sot_to_ros.hh"
namespace dynamicgraph { namespace dynamicgraph {
template <> template <>
inline void RosPublish::sendData<std::pair<sot::MatrixHomogeneous, Vector> >( inline void RosPublish::sendData<std::pair<sot::MatrixHomogeneous, Vector> >(
boost::shared_ptr<realtime_tools::RealtimePublisher<SotToRos<std::pair<sot::MatrixHomogeneous, Vector> >::ros_t> > boost::shared_ptr<realtime_tools::RealtimePublisher<
SotToRos<std::pair<sot::MatrixHomogeneous, Vector> >::ros_t> >
publisher, publisher,
boost::shared_ptr<SotToRos<std::pair<sot::MatrixHomogeneous, Vector> >::signalIn_t> signal, int time) { boost::shared_ptr<
SotToRos<std::pair<sot::MatrixHomogeneous, Vector> >::signalIn_t>
signal,
int time) {
SotToRos<std::pair<sot::MatrixHomogeneous, Vector> >::ros_t result; SotToRos<std::pair<sot::MatrixHomogeneous, Vector> >::ros_t result;
if (publisher->trylock()) { if (publisher->trylock()) {
publisher->msg_.child_frame_id = "/dynamic_graph/world"; publisher->msg_.child_frame_id = "/dynamic_graph/world";
...@@ -23,8 +27,11 @@ inline void RosPublish::sendData<std::pair<sot::MatrixHomogeneous, Vector> >( ...@@ -23,8 +27,11 @@ inline void RosPublish::sendData<std::pair<sot::MatrixHomogeneous, Vector> >(
} }
template <typename T> template <typename T>
void RosPublish::sendData(boost::shared_ptr<realtime_tools::RealtimePublisher<typename SotToRos<T>::ros_t> > publisher, void RosPublish::sendData(
boost::shared_ptr<typename SotToRos<T>::signalIn_t> signal, int time) { boost::shared_ptr<
realtime_tools::RealtimePublisher<typename SotToRos<T>::ros_t> >
publisher,
boost::shared_ptr<typename SotToRos<T>::signalIn_t> signal, int time) {
typename SotToRos<T>::ros_t result; typename SotToRos<T>::ros_t result;
if (publisher->trylock()) { if (publisher->trylock()) {
converter(publisher->msg_, signal->access(time)); converter(publisher->msg_, signal->access(time));
...@@ -42,17 +49,19 @@ void RosPublish::add(const std::string& signal, const std::string& topic) { ...@@ -42,17 +49,19 @@ void RosPublish::add(const std::string& signal, const std::string& topic) {
// Initialize the publisher. // Initialize the publisher.
boost::shared_ptr<realtime_tools::RealtimePublisher<ros_t> > pubPtr = boost::shared_ptr<realtime_tools::RealtimePublisher<ros_t> > pubPtr =
boost::make_shared<realtime_tools::RealtimePublisher<ros_t> >(nh_, topic, 1); boost::make_shared<realtime_tools::RealtimePublisher<ros_t> >(nh_, topic,
1);
// Initialize the signal. // Initialize the signal.
boost::shared_ptr<signal_t> signalPtr( boost::shared_ptr<signal_t> signalPtr(new signal_t(
new signal_t(0, MAKE_SIGNAL_STRING(name, true, SotToRos<T>::signalTypeName, signal))); 0, MAKE_SIGNAL_STRING(name, true, SotToRos<T>::signalTypeName, signal)));
boost::get<0>(bindedSignal) = signalPtr; boost::get<0>(bindedSignal) = signalPtr;
SotToRos<T>::setDefault(*signalPtr); SotToRos<T>::setDefault(*signalPtr);
signalRegistration(*boost::get<0>(bindedSignal)); signalRegistration(*boost::get<0>(bindedSignal));
// Initialize the callback. // Initialize the callback.
callback_t callback = boost::bind(&RosPublish::sendData<T>, this, pubPtr, signalPtr, _1); callback_t callback =
boost::bind(&RosPublish::sendData<T>, this, pubPtr, signalPtr, _1);
boost::get<1>(bindedSignal) = callback; boost::get<1>(bindedSignal) = callback;
bindedSignal_[signal] = bindedSignal; bindedSignal_[signal] = bindedSignal;
......
#include <dynamic-graph/python/module.hh>
#include "ros_queued_subscribe.hh"
namespace dg = dynamicgraph;
BOOST_PYTHON_MODULE(wrap) {
bp::import("dynamic_graph");
dg::python::exposeEntity<dg::RosQueuedSubscribe, bp::bases<dg::Entity>,
dg::python::AddCommands>()
.def("clear", &dg::RosQueuedSubscribe::clear,
"Remove all signals reading data from a ROS topic")
.def("rm", &dg::RosQueuedSubscribe::rm,
"Remove a signal reading data from a ROS topic",
bp::args("signal_name"))
.def("list", &dg::RosQueuedSubscribe::list,
"List signals reading data from a ROS topic")
.def("listTopics", &dg::RosQueuedSubscribe::listTopics,
"List subscribed topics from ROS in the same order as list command")
.def("clearQueue", &dg::RosQueuedSubscribe::clearQueue,
"Empty the queue of a given signal", bp::args("signal_name"))
.def("queueSize", &dg::RosQueuedSubscribe::queueSize,
"Return the queue size of a given signal", bp::args("signal_name"))
.def("readQueue", &dg::RosQueuedSubscribe::readQueue,
"Whether signals should read values from the queues, and when.",
bp::args("start_time"));
}
#include "ros_queued_subscribe.hh"
typedef boost::mpl::vector< dynamicgraph::RosQueuedSubscribe > entities_t;
...@@ -4,59 +4,31 @@ ...@@ -4,59 +4,31 @@
// //
// //
#include <boost/assign.hpp> #include "ros_queued_subscribe.hh"
#include <boost/bind.hpp>
#include <boost/format.hpp>
#include <boost/function.hpp>
#include <boost/make_shared.hpp>
#include <dynamic-graph/factory.h>
#include <ros/ros.h> #include <ros/ros.h>
#include <std_msgs/Float64.h> #include <std_msgs/Float64.h>
#include <std_msgs/UInt32.h> #include <std_msgs/UInt32.h>
#include <dynamic-graph/factory.h> #include <boost/assign.hpp>
#include <boost/bind.hpp>
#include <boost/format.hpp>
#include <boost/function.hpp>
#include <boost/make_shared.hpp>
#include "dynamic_graph_bridge/ros_init.hh" #include "dynamic_graph_bridge/ros_init.hh"
#include "ros_queued_subscribe.hh"
namespace dynamicgraph { namespace dynamicgraph {
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(RosQueuedSubscribe, "RosQueuedSubscribe"); DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(RosQueuedSubscribe, "RosQueuedSubscribe");
namespace command { namespace command {
namespace rosQueuedSubscribe { namespace rosQueuedSubscribe {
Clear::Clear(RosQueuedSubscribe& entity, const std::string& docstring)
: Command(entity, std::vector<Value::Type>(), docstring) {}
Value Clear::doExecute() {
RosQueuedSubscribe& entity = static_cast<RosQueuedSubscribe&>(owner());
entity.clear();
return Value();
}
ClearQueue::ClearQueue(RosQueuedSubscribe& entity, const std::string& docstring)
: Command(entity, boost::assign::list_of(Value::STRING), docstring) {}
Value ClearQueue::doExecute() {
RosQueuedSubscribe& entity = static_cast<RosQueuedSubscribe&>(owner());
std::vector<Value> values = getParameterValues();
const std::string& signal = values[0].value();
entity.clearQueue(signal);
return Value();
}
List::List(RosQueuedSubscribe& entity, const std::string& docstring)
: Command(entity, std::vector<Value::Type>(), docstring) {}
Value List::doExecute() {
RosQueuedSubscribe& entity = static_cast<RosQueuedSubscribe&>(owner());
return Value(entity.list());
}
Add::Add(RosQueuedSubscribe& entity, const std::string& docstring) Add::Add(RosQueuedSubscribe& entity, const std::string& docstring)
: Command(entity, boost::assign::list_of(Value::STRING)(Value::STRING)(Value::STRING), docstring) {} : Command(
entity,
boost::assign::list_of(Value::STRING)(Value::STRING)(Value::STRING),
docstring) {}
Value Add::doExecute() { Value Add::doExecute() {
RosQueuedSubscribe& entity = static_cast<RosQueuedSubscribe&>(owner()); RosQueuedSubscribe& entity = static_cast<RosQueuedSubscribe&>(owner());
...@@ -84,42 +56,6 @@ Value Add::doExecute() { ...@@ -84,42 +56,6 @@ Value Add::doExecute() {
throw std::runtime_error("bad type"); throw std::runtime_error("bad type");
return Value(); return Value();
} }
Rm::Rm(RosQueuedSubscribe& entity, const std::string& docstring)
: Command(entity, boost::assign::list_of(Value::STRING), docstring) {}
Value Rm::doExecute() {
RosQueuedSubscribe& entity = static_cast<RosQueuedSubscribe&>(owner());
std::vector<Value> values = getParameterValues();
const std::string& signal = values[0].value();
entity.rm(signal);
return Value();
}
QueueSize::QueueSize(RosQueuedSubscribe& entity, const std::string& docstring)
: Command(entity, boost::assign::list_of(Value::STRING), docstring) {}
Value QueueSize::doExecute() {
RosQueuedSubscribe& entity = static_cast<RosQueuedSubscribe&>(owner());
std::vector<Value> values = getParameterValues();
const std::string& signal = values[0].value();
return Value((unsigned)entity.queueSize(signal));
}
ReadQueue::ReadQueue(RosQueuedSubscribe& entity, const std::string& docstring)
: Command(entity, boost::assign::list_of(Value::INT), docstring) {}
Value ReadQueue::doExecute() {
RosQueuedSubscribe& entity = static_cast<RosQueuedSubscribe&>(owner());
std::vector<Value> values = getParameterValues();
int read = values[0].value();
entity.readQueue(read);
return Value();
}
} // namespace rosQueuedSubscribe } // namespace rosQueuedSubscribe
} // end of namespace command. } // end of namespace command.
...@@ -129,7 +65,10 @@ const std::string RosQueuedSubscribe::docstring_( ...@@ -129,7 +65,10 @@ const std::string RosQueuedSubscribe::docstring_(
" Use command \"add\" to subscribe to a new signal.\n"); " Use command \"add\" to subscribe to a new signal.\n");
RosQueuedSubscribe::RosQueuedSubscribe(const std::string& n) RosQueuedSubscribe::RosQueuedSubscribe(const std::string& n)
: dynamicgraph::Entity(n), nh_(rosInit(true)), bindedSignal_(), readQueue_(-1) { : dynamicgraph::Entity(n),
nh_(rosInit(true)),
bindedSignal_(),
readQueue_(-1) {
std::string docstring = std::string docstring =
"\n" "\n"
" Add a signal reading data from a ROS topic\n" " Add a signal reading data from a ROS topic\n"
...@@ -141,58 +80,13 @@ RosQueuedSubscribe::RosQueuedSubscribe(const std::string& n) ...@@ -141,58 +80,13 @@ RosQueuedSubscribe::RosQueuedSubscribe(const std::string& n)
" - topic: the topic name in ROS.\n" " - topic: the topic name in ROS.\n"
"\n"; "\n";
addCommand("add", new command::rosQueuedSubscribe::Add(*this, docstring)); addCommand("add", new command::rosQueuedSubscribe::Add(*this, docstring));
docstring =
"\n"
" Remove a signal reading data from a ROS topic\n"
"\n"
" Input:\n"
" - name of the signal to remove (see method list for the list of "
"signals).\n"
"\n";
addCommand("rm", new command::rosQueuedSubscribe::Rm(*this, docstring));
docstring =
"\n"
" Remove all signals reading data from a ROS topic\n"
"\n"
" No input:\n"
"\n";
addCommand("clear", new command::rosQueuedSubscribe::Clear(*this, docstring));
docstring =
"\n"
" List signals reading data from a ROS topic\n"
"\n"
" No input:\n"
"\n";
addCommand("list", new command::rosQueuedSubscribe::List(*this, docstring));
docstring =
"\n"
" Empty the queue of a given signal\n"
"\n"
" Input is:\n"
" - name of the signal (see method list for the list of signals).\n"
"\n";
addCommand("clearQueue", new command::rosQueuedSubscribe::ClearQueue(*this, docstring));
docstring =
"\n"
" Return the queue size of a given signal\n"
"\n"
" Input is:\n"
" - name of the signal (see method list for the list of signals).\n"
"\n";
addCommand("queueSize", new command::rosQueuedSubscribe::QueueSize(*this, docstring));
docstring =
"\n"
" Whether signals should read values from the queues, and when.\n"
"\n"
" Input is:\n"
" - int (dynamic graph time at which the reading begin).\n"
"\n";
addCommand("readQueue", new command::rosQueuedSubscribe::ReadQueue(*this, docstring));
} }
RosQueuedSubscribe::~RosQueuedSubscribe() {} RosQueuedSubscribe::~RosQueuedSubscribe() {}
void RosQueuedSubscribe::display(std::ostream& os) const { os << CLASS_NAME << std::endl; } void RosQueuedSubscribe::display(std::ostream& os) const {
os << CLASS_NAME << std::endl;
}
void RosQueuedSubscribe::rm(const std::string& signal) { void RosQueuedSubscribe::rm(const std::string& signal) {
std::string signalTs = signal + "Timestamp"; std::string signalTs = signal + "Timestamp";
...@@ -206,13 +100,17 @@ void RosQueuedSubscribe::rm(const std::string& signal) { ...@@ -206,13 +100,17 @@ void RosQueuedSubscribe::rm(const std::string& signal) {
} }
} }
std::string RosQueuedSubscribe::list() { std::vector<std::string> RosQueuedSubscribe::list() {
std::string result("["); std::vector<std::string> result(bindedSignal_.size());
for (std::map<std::string, bindedSignal_t>::const_iterator it = bindedSignal_.begin(); it != bindedSignal_.end(); std::transform(bindedSignal_.begin(), bindedSignal_.end(), result.begin(),
it++) { [](const auto& pair) { return pair.first; });
result += "'" + it->first + "',"; return result;
} }
result += "]";
std::vector<std::string> RosQueuedSubscribe::listTopics() {
std::vector<std::string> result(topics_.size());
std::transform(topics_.begin(), topics_.end(), result.begin(),
[](const auto& pair) { return pair.second; });
return result; return result;
} }
...@@ -231,7 +129,8 @@ void RosQueuedSubscribe::clearQueue(const std::string& signal) { ...@@ -231,7 +129,8 @@ void RosQueuedSubscribe::clearQueue(const std::string& signal) {
} }
std::size_t RosQueuedSubscribe::queueSize(const std::string& signal) const { std::size_t RosQueuedSubscribe::queueSize(const std::string& signal) const {
std::map<std::string, bindedSignal_t>::const_iterator _bs = bindedSignal_.find(signal); std::map<std::string, bindedSignal_t>::const_iterator _bs =
bindedSignal_.find(signal);
if (_bs != bindedSignal_.end()) { if (_bs != bindedSignal_.end()) {
return _bs->second->size(); return _bs->second->size();
} }
......
...@@ -6,19 +6,17 @@ ...@@ -6,19 +6,17 @@
#ifndef DYNAMIC_GRAPH_ROS_QUEUED_SUBSCRIBE_HH #ifndef DYNAMIC_GRAPH_ROS_QUEUED_SUBSCRIBE_HH
#define DYNAMIC_GRAPH_ROS_QUEUED_SUBSCRIBE_HH #define DYNAMIC_GRAPH_ROS_QUEUED_SUBSCRIBE_HH
#include <map> #include <dynamic-graph/command.h>
#include <dynamic-graph/entity.h>
#include <dynamic-graph/signal-ptr.h>
#include <dynamic-graph/signal-time-dependent.h>
#include <ros/ros.h>
#include <boost/shared_ptr.hpp> #include <boost/shared_ptr.hpp>
#include <boost/thread/mutex.hpp> #include <boost/thread/mutex.hpp>
#include <map>
#include <dynamic-graph/entity.h>
#include <dynamic-graph/signal-time-dependent.h>
#include <dynamic-graph/signal-ptr.h>
#include <dynamic-graph/command.h>
#include <sot/core/matrix-geometry.hh> #include <sot/core/matrix-geometry.hh>
#include <ros/ros.h>
#include "converter.hh" #include "converter.hh"
#include "sot_to_ros.hh" #include "sot_to_ros.hh"
...@@ -38,12 +36,6 @@ using ::dynamicgraph::command::Value; ...@@ -38,12 +36,6 @@ using ::dynamicgraph::command::Value;
} }
ROS_QUEUED_SUBSCRIBE_MAKE_COMMAND(Add); ROS_QUEUED_SUBSCRIBE_MAKE_COMMAND(Add);
ROS_QUEUED_SUBSCRIBE_MAKE_COMMAND(Clear);
ROS_QUEUED_SUBSCRIBE_MAKE_COMMAND(List);
ROS_QUEUED_SUBSCRIBE_MAKE_COMMAND(Rm);
ROS_QUEUED_SUBSCRIBE_MAKE_COMMAND(ClearQueue);
ROS_QUEUED_SUBSCRIBE_MAKE_COMMAND(QueueSize);
ROS_QUEUED_SUBSCRIBE_MAKE_COMMAND(ReadQueue);
#undef ROS_QUEUED_SUBSCRIBE_MAKE_COMMAND #undef ROS_QUEUED_SUBSCRIBE_MAKE_COMMAND
...@@ -77,7 +69,11 @@ struct BindedSignal : BindedSignalBase { ...@@ -77,7 +69,11 @@ struct BindedSignal : BindedSignalBase {
typedef typename buffer_t::size_type size_type; typedef typename buffer_t::size_type size_type;
BindedSignal(RosQueuedSubscribe* e) BindedSignal(RosQueuedSubscribe* e)
: BindedSignalBase(e), frontIdx(0), backIdx(0), buffer(BufferSize), init(false) {} : BindedSignalBase(e),
frontIdx(0),
backIdx(0),
buffer(BufferSize),
init(false) {}
~BindedSignal() { ~BindedSignal() {
signal.reset(); signal.reset();
clear(); clear();
...@@ -142,24 +138,32 @@ class RosQueuedSubscribe : public dynamicgraph::Entity { ...@@ -142,24 +138,32 @@ class RosQueuedSubscribe : public dynamicgraph::Entity {
void display(std::ostream& os) const; void display(std::ostream& os) const;
void rm(const std::string& signal); void rm(const std::string& signal);
std::string list(); std::vector<std::string> list();
std::vector<std::string> listTopics();
void clear(); void clear();
void clearQueue(const std::string& signal); void clearQueue(const std::string& signal);
void readQueue(int beginReadingAt); void readQueue(int beginReadingAt);
std::size_t queueSize(const std::string& signal) const; std::size_t queueSize(const std::string& signal) const;
template <typename T> template <typename T>
void add(const std::string& type, const std::string& signal, const std::string& topic); void add(const std::string& type, const std::string& signal,
const std::string& topic);
std::map<std::string, bindedSignal_t>& bindedSignal() { return bindedSignal_; } std::map<std::string, bindedSignal_t>& bindedSignal() {
return bindedSignal_;
}
std::map<std::string, std::string>& topics() { return topics_; }
ros::NodeHandle& nh() { return nh_; } ros::NodeHandle& nh() { return nh_; }
template <typename R, typename S> template <typename R, typename S>
void callback(boost::shared_ptr<dynamicgraph::SignalPtr<S, int> > signal, const R& data); void callback(boost::shared_ptr<dynamicgraph::SignalPtr<S, int> > signal,
const R& data);
template <typename R> template <typename R>
void callbackTimestamp(boost::shared_ptr<dynamicgraph::SignalPtr<ptime, int> > signal, const R& data); void callbackTimestamp(
boost::shared_ptr<dynamicgraph::SignalPtr<ptime, int> > signal,
const R& data);
template <typename T> template <typename T>
friend class internal::Add; friend class internal::Add;
...@@ -168,6 +172,7 @@ class RosQueuedSubscribe : public dynamicgraph::Entity { ...@@ -168,6 +172,7 @@ class RosQueuedSubscribe : public dynamicgraph::Entity {
static const std::string docstring_; static const std::string docstring_;
ros::NodeHandle& nh_; ros::NodeHandle& nh_;
std::map<std::string, bindedSignal_t> bindedSignal_; std::map<std::string, bindedSignal_t> bindedSignal_;
std::map<std::string, std::string> topics_;
int readQueue_; int readQueue_;
// Signal<bool, int> readQueue_; // Signal<bool, int> readQueue_;
......
...@@ -8,14 +8,16 @@ ...@@ -8,14 +8,16 @@
#define DYNAMIC_GRAPH_ROS_QUEUED_SUBSCRIBE_HXX #define DYNAMIC_GRAPH_ROS_QUEUED_SUBSCRIBE_HXX
#define ENABLE_RT_LOG #define ENABLE_RT_LOG
#include <vector>
#include <boost/bind.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>
#include <dynamic-graph/real-time-logger.h>
#include <dynamic-graph/signal-caster.h>
#include <dynamic-graph/linear-algebra.h> #include <dynamic-graph/linear-algebra.h>
#include <dynamic-graph/real-time-logger.h>
#include <dynamic-graph/signal-cast-helper.h> #include <dynamic-graph/signal-cast-helper.h>
#include <dynamic-graph/signal-caster.h>
#include <std_msgs/Float64.h> #include <std_msgs/Float64.h>
#include <boost/bind.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>
#include <vector>
#include "dynamic_graph_bridge_msgs/Matrix.h" #include "dynamic_graph_bridge_msgs/Matrix.h"
#include "dynamic_graph_bridge_msgs/Vector.h" #include "dynamic_graph_bridge_msgs/Vector.h"
...@@ -25,8 +27,8 @@ static const int BUFFER_SIZE = 1 << 10; ...@@ -25,8 +27,8 @@ static const int BUFFER_SIZE = 1 << 10;
template <typename T> template <typename T>
struct Add { struct Add {
void operator()(RosQueuedSubscribe& rosSubscribe, const std::string& type, const std::string& signal, void operator()(RosQueuedSubscribe& rosSubscribe, const std::string& type,
const std::string& topic) { const std::string& signal, const std::string& topic) {
typedef typename SotToRos<T>::sot_t sot_t; typedef typename SotToRos<T>::sot_t sot_t;
typedef typename SotToRos<T>::ros_const_ptr_t ros_const_ptr_t; typedef typename SotToRos<T>::ros_const_ptr_t ros_const_ptr_t;
typedef BindedSignal<sot_t, BUFFER_SIZE> BindedSignal_t; typedef BindedSignal<sot_t, BUFFER_SIZE> BindedSignal_t;
...@@ -46,14 +48,17 @@ struct Add { ...@@ -46,14 +48,17 @@ struct Add {
// Initialize the subscriber. // Initialize the subscriber.
typedef boost::function<void(const ros_const_ptr_t& data)> callback_t; typedef boost::function<void(const ros_const_ptr_t& data)> callback_t;
callback_t callback = boost::bind(&BindedSignal_t::template writer<ros_const_ptr_t>, bs, _1); callback_t callback =
boost::bind(&BindedSignal_t::template writer<ros_const_ptr_t>, bs, _1);
// Keep 50 messages in queue, but only 20 are sent every 100ms // Keep 50 messages in queue, but only 20 are sent every 100ms
// -> No message should be lost because of a full buffer // -> No message should be lost because of a full buffer
bs->subscriber = boost::make_shared<ros::Subscriber>(rosSubscribe.nh().subscribe(topic, BUFFER_SIZE, callback)); bs->subscriber = boost::make_shared<ros::Subscriber>(
rosSubscribe.nh().subscribe(topic, BUFFER_SIZE, callback));
RosQueuedSubscribe::bindedSignal_t bindedSignal(bs); RosQueuedSubscribe::bindedSignal_t bindedSignal(bs);
rosSubscribe.bindedSignal()[signal] = bindedSignal; rosSubscribe.bindedSignal()[signal] = bindedSignal;
rosSubscribe.topics()[signal] = topic;
} }
}; };
...@@ -85,7 +90,8 @@ T& BindedSignal<T, N>::reader(T& data, int time) { ...@@ -85,7 +90,8 @@ T& BindedSignal<T, N>::reader(T& data, int time) {
// synchronize with method clear: // synchronize with method clear:
// If reading from the list cannot be done, then return last value. // If reading from the list cannot be done, then return last value.
boost::mutex::scoped_lock lock(rmutex, boost::try_to_lock); boost::mutex::scoped_lock lock(rmutex, boost::try_to_lock);
if (!lock.owns_lock() || entity->readQueue_ == -1 || time < entity->readQueue_) { if (!lock.owns_lock() || entity->readQueue_ == -1 ||
time < entity->readQueue_) {
data = last; data = last;
} else { } else {
if (empty()) if (empty())
...@@ -101,7 +107,8 @@ T& BindedSignal<T, N>::reader(T& data, int time) { ...@@ -101,7 +107,8 @@ T& BindedSignal<T, N>::reader(T& data, int time) {
} // end of namespace internal. } // end of namespace internal.
template <typename T> template <typename T>
void RosQueuedSubscribe::add(const std::string& type, const std::string& signal, const std::string& topic) { void RosQueuedSubscribe::add(const std::string& type, const std::string& signal,
const std::string& topic) {
internal::Add<T>()(*this, type, signal, topic); internal::Add<T>()(*this, type, signal, topic);
} }
} // end of namespace dynamicgraph. } // end of namespace dynamicgraph.
......
#include <dynamic-graph/python/module.hh> #include <dynamic-graph/python/module.hh>
#include "ros_subscribe.hh" #include "ros_subscribe.hh"
namespace dg = dynamicgraph; namespace dg = dynamicgraph;
BOOST_PYTHON_MODULE(wrap) {
BOOST_PYTHON_MODULE(wrap)
{
bp::import("dynamic_graph"); bp::import("dynamic_graph");
dg::python::exposeEntity<dg::RosSubscribe, bp::bases<dg::Entity>, dg::python::AddCommands>() dg::python::exposeEntity<dg::RosSubscribe, bp::bases<dg::Entity>,
.def("clear", &dg::RosSubscribe::clear, "Remove all signals reading data from a ROS topic") dg::python::AddCommands>()
.def("rm", &dg::RosSubscribe::rm, "Remove a signal reading data from a ROS topic", .def("clear", &dg::RosSubscribe::clear,
bp::args("signal_name")) "Remove all signals reading data from a ROS topic")
.def("list", &dg::RosSubscribe::list, "List signals reading data from a ROS topic") .def("rm", &dg::RosSubscribe::rm,
; "Remove a signal reading data from a ROS topic",
bp::args("signal_name"))
.def("list", &dg::RosSubscribe::list,
"List signals reading data from a ROS topic");
} }
#include <boost/assign.hpp> #include "ros_subscribe.hh"
#include <boost/bind.hpp>
#include <boost/format.hpp>
#include <boost/function.hpp>
#include <boost/make_shared.hpp>
#include <dynamic-graph/factory.h>
#include <ros/ros.h> #include <ros/ros.h>
#include <std_msgs/Float64.h> #include <std_msgs/Float64.h>
#include <std_msgs/UInt32.h> #include <std_msgs/UInt32.h>
#include <dynamic-graph/factory.h> #include <boost/assign.hpp>
#include <boost/bind.hpp>
#include <boost/format.hpp>
#include <boost/function.hpp>
#include <boost/make_shared.hpp>
#include "dynamic_graph_bridge/ros_init.hh" #include "dynamic_graph_bridge/ros_init.hh"
#include "ros_subscribe.hh"
namespace dynamicgraph { namespace dynamicgraph {
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(RosSubscribe, "RosSubscribe"); DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(RosSubscribe, "RosSubscribe");
...@@ -19,7 +19,10 @@ DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(RosSubscribe, "RosSubscribe"); ...@@ -19,7 +19,10 @@ DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(RosSubscribe, "RosSubscribe");
namespace command { namespace command {
namespace rosSubscribe { namespace rosSubscribe {
Add::Add(RosSubscribe& entity, const std::string& docstring) Add::Add(RosSubscribe& entity, const std::string& docstring)
: Command(entity, boost::assign::list_of(Value::STRING)(Value::STRING)(Value::STRING), docstring) {} : Command(
entity,
boost::assign::list_of(Value::STRING)(Value::STRING)(Value::STRING),
docstring) {}
Value Add::doExecute() { Value Add::doExecute() {
RosSubscribe& entity = static_cast<RosSubscribe&>(owner()); RosSubscribe& entity = static_cast<RosSubscribe&>(owner());
...@@ -63,7 +66,8 @@ const std::string RosSubscribe::docstring_( ...@@ -63,7 +66,8 @@ const std::string RosSubscribe::docstring_(
"\n" "\n"
" Use command \"add\" to subscribe to a new signal.\n"); " Use command \"add\" to subscribe to a new signal.\n");
RosSubscribe::RosSubscribe(const std::string& n) : dynamicgraph::Entity(n), nh_(rosInit(true)), bindedSignal_() { RosSubscribe::RosSubscribe(const std::string& n)
: dynamicgraph::Entity(n), nh_(rosInit(true)), bindedSignal_() {
std::string docstring = std::string docstring =
"\n" "\n"
" Add a signal reading data from a ROS topic\n" " Add a signal reading data from a ROS topic\n"
...@@ -81,7 +85,9 @@ RosSubscribe::RosSubscribe(const std::string& n) : dynamicgraph::Entity(n), nh_( ...@@ -81,7 +85,9 @@ RosSubscribe::RosSubscribe(const std::string& n) : dynamicgraph::Entity(n), nh_(
RosSubscribe::~RosSubscribe() {} RosSubscribe::~RosSubscribe() {}
void RosSubscribe::display(std::ostream& os) const { os << CLASS_NAME << std::endl; } void RosSubscribe::display(std::ostream& os) const {
os << CLASS_NAME << std::endl;
}
void RosSubscribe::rm(const std::string& signal) { void RosSubscribe::rm(const std::string& signal) {
std::string signalTs = signal + "Timestamp"; std::string signalTs = signal + "Timestamp";
...@@ -97,8 +103,8 @@ void RosSubscribe::rm(const std::string& signal) { ...@@ -97,8 +103,8 @@ void RosSubscribe::rm(const std::string& signal) {
std::vector<std::string> RosSubscribe::list() { std::vector<std::string> RosSubscribe::list() {
std::vector<std::string> result(bindedSignal_.size()); std::vector<std::string> result(bindedSignal_.size());
std::transform(bindedSignal_.begin(), bindedSignal_.end(), std::transform(bindedSignal_.begin(), bindedSignal_.end(), result.begin(),
result.begin(), [](const auto& pair) { return pair.first; }); [](const auto& pair) { return pair.first; });
return result; return result;
} }
......