Skip to content
Snippets Groups Projects
Commit c4032210 authored by Francois Keith's avatar Francois Keith
Browse files

Rename ros_import into ros_publish, and ros_export into ros_subscribe.

The old names are kept for now, but will disappear in the
future.
parent 50c4909a
Branches
Tags
No related merge requests found
......@@ -69,8 +69,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)
......
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
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
......@@ -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_;
}
......
#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
#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
......@@ -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_;
}
......
#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
#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
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment