diff --git a/CMakeLists.txt b/CMakeLists.txt index b285ccf184642fe3c1dfe70809ecf384abc07b81..4384e5415b923d1063d0020300d33796c8c8ba14 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -66,8 +66,8 @@ include(cmake/python.cmake) include_directories(${DYNAMIC_GRAPH_include_DIRS}) link_directories(${DYNAMIC_GRAPH_LIBRARY_DIRS}) -compile_plugin(ros_import) -compile_plugin(ros_export) +compile_plugin(ros_publish) +compile_plugin(ros_subscribe) compile_plugin(ros_time) compile_plugin(ros_joint_state) diff --git a/src/dynamic_graph/ros/__init__.py b/src/dynamic_graph/ros/__init__.py index dc9d0eab021620080159d0310e8c085fde0cd72a..3a14f188bdab8b5d4fb1cd7bcd3a3589871f8bd1 100644 --- a/src/dynamic_graph/ros/__init__.py +++ b/src/dynamic_graph/ros/__init__.py @@ -1,6 +1,12 @@ -from ros_import import RosImport -from ros_export import RosExport +from ros_publish import RosPublish +from ros_subscribe import RosSubscribe from ros_joint_state import RosJointState from robot_model import RosRobotModel from ros import Ros + +# aliases, for retro compatibility +ros_import = ros_publish +ros_export = ros_subscribe +from ros import RosPublish as RosImport +from ros import RosSubscribe as RosExport diff --git a/src/dynamic_graph/ros/ros.py b/src/dynamic_graph/ros/ros.py index cfc9c28190952dc1d7de4a4d0a4b059b5194a9d9..9a0b9730789fb9d52ab25eb826beb482b3b1eff2 100644 --- a/src/dynamic_graph/ros/ros.py +++ b/src/dynamic_graph/ros/ros.py @@ -1,5 +1,5 @@ -from ros_import import RosImport -from ros_export import RosExport +from ros_publish import RosPublish +from ros_subscribe import RosSubscribe from ros_joint_state import RosJointState from ros_time import RosTime @@ -7,20 +7,28 @@ from dynamic_graph import plug class Ros(object): device = None + rosPublish = None + rosSubscribe = None + rosJointState = None + + # aliases, for retro compatibility rosImport = None rosExport = None - rosJointState = None def __init__(self, robot, suffix = ''): self.robot = robot - self.rosImport = RosImport('rosImport{0}'.format(suffix)) - self.rosExport = RosExport('rosExport{0}'.format(suffix)) + self.rosPublish = RosPublish('rosPublish{0}'.format(suffix)) + self.rosSubscribe = RosSubscribe('rosSubscribe{0}'.format(suffix)) self.rosJointState = RosJointState('rosJointState{0}'.format(suffix)) self.rosJointState.retrieveJointNames(self.robot.dynamic.name) self.rosTime = RosTime ('rosTime{0}'.format(suffix)) plug(self.robot.device.state, self.rosJointState.state) self.robot.device.after.addSignal( - '{0}.trigger'.format(self.rosImport.name)) + '{0}.trigger'.format(self.rosPublish.name)) self.robot.device.after.addSignal( '{0}.trigger'.format(self.rosJointState.name)) + + # aliases, for retro compatibility + self.rosImport=self.rosPublish + self.rosExport=self.rosSubscribe diff --git a/src/ros_import.cpp b/src/ros_publish.cpp similarity index 78% rename from src/ros_import.cpp rename to src/ros_publish.cpp index 88279b0c17518566bf9173cf5c35b5e7464f2659..c7bc2122d1bea94043a6042677917082c176d393 100644 --- a/src/ros_import.cpp +++ b/src/ros_publish.cpp @@ -15,18 +15,18 @@ #include <dynamic-graph/command.h> #include "dynamic_graph_bridge/ros_init.hh" -#include "ros_import.hh" +#include "ros_publish.hh" namespace dynamicgraph { - DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(RosImport, "RosImport"); + DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(RosPublish, "RosPublish"); namespace command { - namespace rosImport + namespace rosPublish { Clear::Clear - (RosImport& entity, const std::string& docstring) + (RosPublish& entity, const std::string& docstring) : Command (entity, std::vector<Value::Type> (), @@ -35,15 +35,15 @@ namespace dynamicgraph Value Clear::doExecute () { - RosImport& entity = - static_cast<RosImport&> (owner ()); + RosPublish& entity = + static_cast<RosPublish&> (owner ()); entity.clear (); return Value (); } List::List - (RosImport& entity, const std::string& docstring) + (RosPublish& entity, const std::string& docstring) : Command (entity, std::vector<Value::Type> (), @@ -52,13 +52,13 @@ namespace dynamicgraph Value List::doExecute () { - RosImport& entity = - static_cast<RosImport&> (owner ()); + RosPublish& entity = + static_cast<RosPublish&> (owner ()); return Value (entity.list ()); } Add::Add - (RosImport& entity, const std::string& docstring) + (RosPublish& entity, const std::string& docstring) : Command (entity, boost::assign::list_of @@ -68,8 +68,8 @@ namespace dynamicgraph Value Add::doExecute () { - RosImport& entity = - static_cast<RosImport&> (owner ()); + RosPublish& entity = + static_cast<RosPublish&> (owner ()); std::vector<Value> values = getParameterValues (); const std::string& type = values[0].value (); @@ -103,7 +103,7 @@ namespace dynamicgraph } Rm::Rm - (RosImport& entity, const std::string& docstring) + (RosPublish& entity, const std::string& docstring) : Command (entity, boost::assign::list_of (Value::STRING), @@ -112,8 +112,8 @@ namespace dynamicgraph Value Rm::doExecute () { - RosImport& entity = - static_cast<RosImport&> (owner ()); + RosPublish& entity = + static_cast<RosPublish&> (owner ()); std::vector<Value> values = getParameterValues (); const std::string& signal = values[0].value (); entity.rm (signal); @@ -122,17 +122,17 @@ namespace dynamicgraph } // end of errorEstimator. } // end of namespace command. - const std::string RosImport::docstring_ - ("Import ROS topics as dynamic-graph signals.\n" + const std::string RosPublish::docstring_ + ("Publish dynamic-graph signals as ROS topics.\n" "\n" - " Use command \"add\" to import a new ROS topic.\n"); + " Use command \"add\" to publish a new ROS topic.\n"); - RosImport::RosImport (const std::string& n) + RosPublish::RosPublish (const std::string& n) : dynamicgraph::Entity(n), - // rosImport do not use callback so do not create a useless spinner. + // RosPublish do not use callback so do not create a useless spinner. nh_ (rosInit (false)), bindedSignal_ (), - trigger_ (boost::bind (&RosImport::trigger, this, _1, _2), + trigger_ (boost::bind (&RosPublish::trigger, this, _1, _2), sotNOSIGNAL, MAKE_SIGNAL_STRING(name, true, "int", "trigger")), rate_ (ROS_JOINT_STATE_PUBLISHER_RATE), @@ -159,7 +159,7 @@ namespace dynamicgraph " - topic: the topic name in ROS.\n" "\n"; addCommand ("add", - new command::rosImport::Add + new command::rosPublish::Add (*this, docstring)); docstring = "\n" @@ -169,7 +169,7 @@ namespace dynamicgraph " - name of the signal to remove (see method list for the list of signals).\n" "\n"; addCommand ("rm", - new command::rosImport::Rm + new command::rosPublish::Rm (*this, docstring)); docstring = "\n" @@ -178,7 +178,7 @@ namespace dynamicgraph " No input:\n" "\n"; addCommand ("clear", - new command::rosImport::Clear + new command::rosPublish::Clear (*this, docstring)); docstring = "\n" @@ -187,25 +187,25 @@ namespace dynamicgraph " No input:\n" "\n"; addCommand ("list", - new command::rosImport::List + new command::rosPublish::List (*this, docstring)); } - RosImport::~RosImport () + RosPublish::~RosPublish () { } - void RosImport::display (std::ostream& os) const + void RosPublish::display (std::ostream& os) const { os << CLASS_NAME << std::endl; } - void RosImport::rm (const std::string& signal) + void RosPublish::rm (const std::string& signal) { bindedSignal_.erase (signal); } - std::string RosImport::list () const + std::string RosPublish::list () const { std::string result("["); for (std::map<std::string, bindedSignal_t>::const_iterator it = @@ -216,12 +216,12 @@ namespace dynamicgraph return result; } - void RosImport::clear () + void RosPublish::clear () { bindedSignal_.clear (); } - int& RosImport::trigger (int& dummy, int t) + int& RosPublish::trigger (int& dummy, int t) { typedef std::map<std::string, bindedSignal_t>::iterator iterator_t; @@ -237,7 +237,7 @@ namespace dynamicgraph return dummy; } - std::string RosImport::getDocString () const + std::string RosPublish::getDocString () const { return docstring_; } diff --git a/src/ros_import.hh b/src/ros_publish.hh similarity index 78% rename from src/ros_import.hh rename to src/ros_publish.hh index 632bfd55ce44247b537a328954dc7364fbe78979..03a564599f988c25ca29ca43b6feea125bb4ac32 100644 --- a/src/ros_import.hh +++ b/src/ros_publish.hh @@ -1,5 +1,5 @@ -#ifndef DYNAMIC_GRAPH_ROS_IMPORT_HH -# define DYNAMIC_GRAPH_ROS_IMPORT_HH +#ifndef DYNAMIC_GRAPH_ROS_PUBLISH_HH +# define DYNAMIC_GRAPH_ROS_PUBLISH_HH # include <iostream> # include <map> @@ -19,37 +19,37 @@ namespace dynamicgraph { - class RosImport; + class RosPublish; namespace command { - namespace rosImport + namespace rosPublish { using ::dynamicgraph::command::Command; using ::dynamicgraph::command::Value; -# define ROS_IMPORT_MAKE_COMMAND(CMD) \ +# define ROS_PUBLISH_MAKE_COMMAND(CMD) \ class CMD : public Command \ { \ public: \ - CMD (RosImport& entity, \ + CMD (RosPublish& entity, \ const std::string& docstring); \ virtual Value doExecute (); \ } - ROS_IMPORT_MAKE_COMMAND(Add); - ROS_IMPORT_MAKE_COMMAND(Clear); - ROS_IMPORT_MAKE_COMMAND(List); - ROS_IMPORT_MAKE_COMMAND(Rm); + ROS_PUBLISH_MAKE_COMMAND(Add); + ROS_PUBLISH_MAKE_COMMAND(Clear); + ROS_PUBLISH_MAKE_COMMAND(List); + ROS_PUBLISH_MAKE_COMMAND(Rm); -#undef ROS_IMPORT_MAKE_COMMAND +#undef ROS_PUBLISH_MAKE_COMMAND } // end of namespace errorEstimator. } // end of namespace command. /// \brief Publish dynamic-graph information into ROS. - class RosImport : public dynamicgraph::Entity + class RosPublish : public dynamicgraph::Entity { DYNAMIC_GRAPH_ENTITY_DECL(); public: @@ -62,8 +62,8 @@ namespace dynamicgraph static const double ROS_JOINT_STATE_PUBLISHER_RATE = 1. / 100.; - RosImport (const std::string& n); - virtual ~RosImport (); + RosPublish (const std::string& n); + virtual ~RosPublish (); virtual std::string getDocString () const; void display (std::ostream& os) const; @@ -97,5 +97,5 @@ namespace dynamicgraph }; } // end of namespace dynamicgraph. -# include "ros_import.hxx" -#endif //! DYNAMIC_GRAPH_ROS_IMPORT_HH +# include "ros_publish.hxx" +#endif //! DYNAMIC_GRAPH_ROS_PUBLISH_HH diff --git a/src/ros_import.hxx b/src/ros_publish.hxx similarity index 89% rename from src/ros_import.hxx rename to src/ros_publish.hxx index 1ac821aebeceab374b76197e9f6a372d20fa85c2..9d28a31b31965f20477c85310c403242a7f2e9c9 100644 --- a/src/ros_import.hxx +++ b/src/ros_publish.hxx @@ -1,5 +1,5 @@ -#ifndef DYNAMIC_GRAPH_ROS_IMPORT_HXX -# define DYNAMIC_GRAPH_ROS_IMPORT_HXX +#ifndef DYNAMIC_GRAPH_ROS_PUBLISH_HXX +# define DYNAMIC_GRAPH_ROS_PUBLISH_HXX # include <vector> # include <std_msgs/Float64.h> @@ -14,7 +14,7 @@ namespace dynamicgraph { template <> inline void - RosImport::sendData + RosPublish::sendData <std::pair<sot::MatrixHomogeneous, ml::Vector> > (boost::shared_ptr <realtime_tools::RealtimePublisher @@ -38,7 +38,7 @@ namespace dynamicgraph template <typename T> void - RosImport::sendData + RosPublish::sendData (boost::shared_ptr <realtime_tools::RealtimePublisher <typename SotToRos<T>::ros_t> > publisher, @@ -54,7 +54,7 @@ namespace dynamicgraph } template <typename T> - void RosImport::add (const std::string& signal, const std::string& topic) + void RosPublish::add (const std::string& signal, const std::string& topic) { typedef typename SotToRos<T>::sot_t sot_t; typedef typename SotToRos<T>::ros_t ros_t; @@ -81,7 +81,7 @@ namespace dynamicgraph // Initialize the callback. callback_t callback = boost::bind - (&RosImport::sendData<T>, + (&RosPublish::sendData<T>, this, pubPtr, signalPtr, @@ -93,4 +93,4 @@ namespace dynamicgraph } // end of namespace dynamicgraph. -#endif //! DYNAMIC_GRAPH_ROS_IMPORT_HXX +#endif //! DYNAMIC_GRAPH_ROS_PUBLISH_HXX diff --git a/src/ros_export.cpp b/src/ros_subscribe.cpp similarity index 76% rename from src/ros_export.cpp rename to src/ros_subscribe.cpp index aafa3e182aa6275905de23fc6459cabd0e0c421e..36791596de20c3469e18c817db94f756abd5ba8c 100644 --- a/src/ros_export.cpp +++ b/src/ros_subscribe.cpp @@ -11,18 +11,18 @@ #include <dynamic-graph/factory.h> #include "dynamic_graph_bridge/ros_init.hh" -#include "ros_export.hh" +#include "ros_subscribe.hh" namespace dynamicgraph { - DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(RosExport, "RosExport"); + DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(RosSubscribe, "RosSubscribe"); namespace command { - namespace rosExport + namespace rosSubscribe { Clear::Clear - (RosExport& entity, const std::string& docstring) + (RosSubscribe& entity, const std::string& docstring) : Command (entity, std::vector<Value::Type> (), @@ -31,15 +31,15 @@ namespace dynamicgraph Value Clear::doExecute () { - RosExport& entity = - static_cast<RosExport&> (owner ()); + RosSubscribe& entity = + static_cast<RosSubscribe&> (owner ()); entity.clear (); return Value (); } List::List - (RosExport& entity, const std::string& docstring) + (RosSubscribe& entity, const std::string& docstring) : Command (entity, std::vector<Value::Type> (), @@ -48,13 +48,13 @@ namespace dynamicgraph Value List::doExecute () { - RosExport& entity = - static_cast<RosExport&> (owner ()); + RosSubscribe& entity = + static_cast<RosSubscribe&> (owner ()); return Value (entity.list ()); } Add::Add - (RosExport& entity, const std::string& docstring) + (RosSubscribe& entity, const std::string& docstring) : Command (entity, boost::assign::list_of @@ -64,8 +64,8 @@ namespace dynamicgraph Value Add::doExecute () { - RosExport& entity = - static_cast<RosExport&> (owner ()); + RosSubscribe& entity = + static_cast<RosSubscribe&> (owner ()); std::vector<Value> values = getParameterValues (); const std::string& type = values[0].value (); @@ -100,7 +100,7 @@ namespace dynamicgraph } Rm::Rm - (RosExport& entity, const std::string& docstring) + (RosSubscribe& entity, const std::string& docstring) : Command (entity, boost::assign::list_of (Value::STRING), @@ -109,8 +109,8 @@ namespace dynamicgraph Value Rm::doExecute () { - RosExport& entity = - static_cast<RosExport&> (owner ()); + RosSubscribe& entity = + static_cast<RosSubscribe&> (owner ()); std::vector<Value> values = getParameterValues (); const std::string& signal = values[0].value (); entity.rm (signal); @@ -119,12 +119,12 @@ namespace dynamicgraph } // end of errorEstimator. } // end of namespace command. - const std::string RosExport::docstring_ - ("Export dynamic-graph signals as ROS topics.\n" + const std::string RosSubscribe::docstring_ + ("Subscribe to a ROS topics and convert it into a dynamic-graph signals.\n" "\n" - " Use command \"add\" to export a new signal.\n"); + " Use command \"add\" to subscribe to a new signal.\n"); - RosExport::RosExport (const std::string& n) + RosSubscribe::RosSubscribe (const std::string& n) : dynamicgraph::Entity(n), nh_ (rosInit (true)), bindedSignal_ () @@ -141,7 +141,7 @@ namespace dynamicgraph " - topic: the topic name in ROS.\n" "\n"; addCommand ("add", - new command::rosExport::Add + new command::rosSubscribe::Add (*this, docstring)); docstring = "\n" @@ -151,7 +151,7 @@ namespace dynamicgraph " - name of the signal to remove (see method list for the list of signals).\n" "\n"; addCommand ("rm", - new command::rosExport::Rm + new command::rosSubscribe::Rm (*this, docstring)); docstring = "\n" @@ -160,7 +160,7 @@ namespace dynamicgraph " No input:\n" "\n"; addCommand ("clear", - new command::rosExport::Clear + new command::rosSubscribe::Clear (*this, docstring)); docstring = "\n" @@ -169,24 +169,24 @@ namespace dynamicgraph " No input:\n" "\n"; addCommand ("list", - new command::rosExport::List + new command::rosSubscribe::List (*this, docstring)); } - RosExport::~RosExport () + RosSubscribe::~RosSubscribe () {} - void RosExport::display (std::ostream& os) const + void RosSubscribe::display (std::ostream& os) const { os << CLASS_NAME << std::endl; } - void RosExport::rm (const std::string& signal) + void RosSubscribe::rm (const std::string& signal) { bindedSignal_.erase (signal); } - std::string RosExport::list () + std::string RosSubscribe::list () { std::string result("["); for (std::map<std::string, bindedSignal_t>::const_iterator it = @@ -197,12 +197,12 @@ namespace dynamicgraph return result; } - void RosExport::clear () + void RosSubscribe::clear () { bindedSignal_.clear (); } - std::string RosExport::getDocString () const + std::string RosSubscribe::getDocString () const { return docstring_; } diff --git a/src/ros_export.hh b/src/ros_subscribe.hh similarity index 78% rename from src/ros_export.hh rename to src/ros_subscribe.hh index bae0fac276f07798493fa7cee5108aeb2597df6f..480686c67524297d78777723c6eb3ab2dbbc8f86 100644 --- a/src/ros_export.hh +++ b/src/ros_subscribe.hh @@ -1,5 +1,5 @@ -#ifndef DYNAMIC_GRAPH_ROS_EXPORT_HH -# define DYNAMIC_GRAPH_ROS_EXPORT_HH +#ifndef DYNAMIC_GRAPH_ROS_SUBSCRIBE_HH +# define DYNAMIC_GRAPH_ROS_SUBSCRIBE_HH # include <iostream> # include <map> @@ -18,30 +18,30 @@ namespace dynamicgraph { - class RosExport; + class RosSubscribe; namespace command { - namespace rosExport + namespace rosSubscribe { using ::dynamicgraph::command::Command; using ::dynamicgraph::command::Value; -# define ROS_EXPORT_MAKE_COMMAND(CMD) \ +# define ROS_SUBSCRIBE_MAKE_COMMAND(CMD) \ class CMD : public Command \ { \ public: \ - CMD (RosExport& entity, \ + CMD (RosSubscribe& entity, \ const std::string& docstring); \ virtual Value doExecute (); \ } - ROS_EXPORT_MAKE_COMMAND(Add); - ROS_EXPORT_MAKE_COMMAND(Clear); - ROS_EXPORT_MAKE_COMMAND(List); - ROS_EXPORT_MAKE_COMMAND(Rm); + ROS_SUBSCRIBE_MAKE_COMMAND(Add); + ROS_SUBSCRIBE_MAKE_COMMAND(Clear); + ROS_SUBSCRIBE_MAKE_COMMAND(List); + ROS_SUBSCRIBE_MAKE_COMMAND(Rm); -#undef ROS_EXPORT_MAKE_COMMAND +#undef ROS_SUBSCRIBE_MAKE_COMMAND } // end of namespace errorEstimator. } // end of namespace command. @@ -54,7 +54,7 @@ namespace dynamicgraph } // end of internal namespace. /// \brief Publish ROS information in the dynamic-graph. - class RosExport : public dynamicgraph::Entity + class RosSubscribe : public dynamicgraph::Entity { DYNAMIC_GRAPH_ENTITY_DECL(); typedef boost::posix_time::ptime ptime; @@ -63,8 +63,8 @@ namespace dynamicgraph boost::shared_ptr<ros::Subscriber> > bindedSignal_t; - RosExport (const std::string& n); - virtual ~RosExport (); + RosSubscribe (const std::string& n); + virtual ~RosSubscribe (); virtual std::string getDocString () const; void display (std::ostream& os) const; @@ -107,5 +107,5 @@ namespace dynamicgraph }; } // end of namespace dynamicgraph. -# include "ros_export.hxx" -#endif //! DYNAMIC_GRAPH_ROS_EXPORT_HH +# include "ros_subscribe.hxx" +#endif //! DYNAMIC_GRAPH_ROS_SUBSCRIBE_HH diff --git a/src/ros_export.hxx b/src/ros_subscribe.hxx similarity index 64% rename from src/ros_export.hxx rename to src/ros_subscribe.hxx index 82ad056691aa4e302ce92cd3736d2122383313d0..3cdd85519fdac8ef0182ff4fb53b01de0f077445 100644 --- a/src/ros_export.hxx +++ b/src/ros_subscribe.hxx @@ -1,5 +1,5 @@ -#ifndef DYNAMIC_GRAPH_ROS_EXPORT_HXX -# define DYNAMIC_GRAPH_ROS_EXPORT_HXX +#ifndef DYNAMIC_GRAPH_ROS_SUBSCRIBE_HXX +# define DYNAMIC_GRAPH_ROS_SUBSCRIBE_HXX # include <vector> # include <boost/bind.hpp> # include <boost/date_time/posix_time/posix_time.hpp> @@ -17,7 +17,7 @@ namespace dynamicgraph { template <typename R, typename S> void - RosExport::callback + RosSubscribe::callback (boost::shared_ptr<dynamicgraph::SignalPtr<S, int> > signal, const R& data) { @@ -29,7 +29,7 @@ namespace dynamicgraph template <typename R> void - RosExport::callbackTimestamp + RosSubscribe::callbackTimestamp (boost::shared_ptr<dynamicgraph::SignalPtr<ptime, int> > signal, const R& data) { @@ -43,7 +43,7 @@ namespace dynamicgraph template <typename T> struct Add { - void operator () (RosExport& rosExport, + void operator () (RosSubscribe& RosSubscribe, const std::string& signal, const std::string& topic) { @@ -52,36 +52,36 @@ namespace dynamicgraph typedef typename SotToRos<T>::signalIn_t signal_t; // Initialize the bindedSignal object. - RosExport::bindedSignal_t bindedSignal; + RosSubscribe::bindedSignal_t bindedSignal; // Initialize the signal. - boost::format signalName ("RosExport(%1%)::%2%"); - signalName % rosExport.getName () % signal; + boost::format signalName ("RosSubscribe(%1%)::%2%"); + signalName % RosSubscribe.getName () % signal; boost::shared_ptr<signal_t> signal_ (new signal_t (0, signalName.str ())); SotToRos<T>::setDefault(*signal_); bindedSignal.first = signal_; - rosExport.signalRegistration (*bindedSignal.first); + RosSubscribe.signalRegistration (*bindedSignal.first); // Initialize the subscriber. typedef boost::function<void (const ros_const_ptr_t& data)> callback_t; callback_t callback = boost::bind - (&RosExport::callback<ros_const_ptr_t, sot_t>, - &rosExport, signal_, _1); + (&RosSubscribe::callback<ros_const_ptr_t, sot_t>, + &RosSubscribe, signal_, _1); bindedSignal.second = boost::make_shared<ros::Subscriber> - (rosExport.nh ().subscribe (topic, 1, callback)); + (RosSubscribe.nh ().subscribe (topic, 1, callback)); - rosExport.bindedSignal ()[signal] = bindedSignal; + RosSubscribe.bindedSignal ()[signal] = bindedSignal; } }; template <typename T> struct Add<std::pair<T, ml::Vector> > { - void operator () (RosExport& rosExport, + void operator () (RosSubscribe& RosSubscribe, const std::string& signal, const std::string& topic) { @@ -92,72 +92,72 @@ namespace dynamicgraph typedef typename SotToRos<type_t>::signalIn_t signal_t; // Initialize the bindedSignal object. - RosExport::bindedSignal_t bindedSignal; + RosSubscribe::bindedSignal_t bindedSignal; // Initialize the signal. - boost::format signalName ("RosExport(%1%)::%2%"); - signalName % rosExport.getName () % signal; + boost::format signalName ("RosSubscribe(%1%)::%2%"); + signalName % RosSubscribe.getName () % signal; boost::shared_ptr<signal_t> signal_ (new signal_t (0, signalName.str ())); SotToRos<T>::setDefault(*signal_); bindedSignal.first = signal_; - rosExport.signalRegistration (*bindedSignal.first); + RosSubscribe.signalRegistration (*bindedSignal.first); // Initialize the publisher. typedef boost::function<void (const ros_const_ptr_t& data)> callback_t; callback_t callback = boost::bind - (&RosExport::callback<ros_const_ptr_t, sot_t>, - &rosExport, signal_, _1); + (&RosSubscribe::callback<ros_const_ptr_t, sot_t>, + &RosSubscribe, signal_, _1); bindedSignal.second = boost::make_shared<ros::Subscriber> - (rosExport.nh ().subscribe (topic, 1, callback)); + (RosSubscribe.nh ().subscribe (topic, 1, callback)); - rosExport.bindedSignal ()[signal] = bindedSignal; + RosSubscribe.bindedSignal ()[signal] = bindedSignal; // Timestamp. - typedef dynamicgraph::SignalPtr<RosExport::ptime, int> + typedef dynamicgraph::SignalPtr<RosSubscribe::ptime, int> signalTimestamp_t; std::string signalTimestamp = (boost::format ("%1%%2%") % signal % "Timestamp").str (); // Initialize the bindedSignal object. - RosExport::bindedSignal_t bindedSignalTimestamp; + RosSubscribe::bindedSignal_t bindedSignalTimestamp; // Initialize the signal. - boost::format signalNameTimestamp ("RosExport(%1%)::%2%"); - signalNameTimestamp % rosExport.name % signalTimestamp; + boost::format signalNameTimestamp ("RosSubscribe(%1%)::%2%"); + signalNameTimestamp % RosSubscribe.name % signalTimestamp; boost::shared_ptr<signalTimestamp_t> signalTimestamp_ (new signalTimestamp_t (0, signalNameTimestamp.str ())); - RosExport::ptime zero (rosTimeToPtime (ros::Time (0, 0))); + RosSubscribe::ptime zero (rosTimeToPtime (ros::Time (0, 0))); signalTimestamp_->setConstant (zero); bindedSignalTimestamp.first = signalTimestamp_; - rosExport.signalRegistration (*bindedSignalTimestamp.first); + RosSubscribe.signalRegistration (*bindedSignalTimestamp.first); // Initialize the publisher. typedef boost::function<void (const ros_const_ptr_t& data)> callback_t; callback_t callbackTimestamp = boost::bind - (&RosExport::callbackTimestamp<ros_const_ptr_t>, - &rosExport, signalTimestamp_, _1); + (&RosSubscribe::callbackTimestamp<ros_const_ptr_t>, + &RosSubscribe, signalTimestamp_, _1); bindedSignalTimestamp.second = boost::make_shared<ros::Subscriber> - (rosExport.nh ().subscribe (topic, 1, callbackTimestamp)); + (RosSubscribe.nh ().subscribe (topic, 1, callbackTimestamp)); - rosExport.bindedSignal ()[signalTimestamp] = bindedSignalTimestamp; + RosSubscribe.bindedSignal ()[signalTimestamp] = bindedSignalTimestamp; } }; } // end of namespace internal. template <typename T> - void RosExport::add (const std::string& signal, const std::string& topic) + void RosSubscribe::add (const std::string& signal, const std::string& topic) { internal::Add<T> () (*this, signal, topic); } } // end of namespace dynamicgraph. -#endif //! DYNAMIC_GRAPH_ROS_EXPORT_HXX +#endif //! DYNAMIC_GRAPH_ROS_SUBSCRIBE_HXX