diff --git a/src/converter.hh b/src/converter.hh
index 93c84c9d5fe1d3fc322d30ddf82cdc2beab34119..bd4443fa01be52db58ad9b0a8df95e8cf8d44cb2 100644
--- a/src/converter.hh
+++ b/src/converter.hh
@@ -7,6 +7,13 @@
namespace dynamicgraph
{
+ /// \brief Handle ROS <-> dynamic-graph conversion.
+ ///
+ /// Implements all ROS/dynamic-graph conversions required by the
+ /// bridge.
+ ///
+ ///Relies on the SotToRos type trait to make sure valid pair of
+ /// types are used.
template <typename D, typename S>
void converter (D& dst, const S& src);
@@ -55,6 +62,16 @@ namespace dynamicgraph
dst.rotation.w = quaternion.w ();
}
+ template <>
+ inline void converter
+ (SotToRos<std::pair<sot::MatrixHomogeneous, ml::Vector> >::ros_t& dst,
+ const SotToRos<std::pair<sot::MatrixHomogeneous, ml::Vector> >::sot_t& src)
+ {
+ converter
+ <SotToRos<sot::MatrixHomogeneous>::ros_t,
+ SotToRos<sot::MatrixHomogeneous>::sot_t> (dst.transform, src);
+ }
+
template <>
inline void converter (SotToRos<ml::Vector>::ros_t& dst,
const SotToRos<ml::Vector>::sot_t& src)
@@ -104,6 +121,16 @@ namespace dynamicgraph
dst(2, 3) = src.translation.z;
}
+ template <>
+ inline void converter
+ (SotToRos<std::pair<sot::MatrixHomogeneous, ml::Vector> >::sot_t& dst,
+ const SotToRos<std::pair<sot::MatrixHomogeneous, ml::Vector> >::ros_t& src)
+ {
+ converter
+ <SotToRos<sot::MatrixHomogeneous>::sot_t,
+ SotToRos<sot::MatrixHomogeneous>::ros_t> (dst, src.transform);
+ }
+
template <>
inline void converter
(SotToRos<ml::Vector>::sot_t& dst,
@@ -134,6 +161,17 @@ namespace dynamicgraph
SotToRos<sot::MatrixHomogeneous>::ros_t> (dst, *src);
}
+ template <>
+ inline void converter
+ (SotToRos<std::pair<sot::MatrixHomogeneous, ml::Vector> >::sot_t& dst,
+ const boost::shared_ptr
+ <SotToRos<std::pair<sot::MatrixHomogeneous, ml::Vector> >::ros_t const>& src)
+ {
+ converter
+ <SotToRos<sot::MatrixHomogeneous>::sot_t,
+ SotToRos<sot::MatrixHomogeneous>::ros_t> (dst, src->transform);
+ }
+
} // end of namespace dynamicgraph.
diff --git a/src/ros_export.cpp b/src/ros_export.cpp
index 8cd110f618e75bc0945da51b6e35f315c698fc1d..51b7cb7ad0b78c01bc07661b2cd0a779b6e611bb 100644
--- a/src/ros_export.cpp
+++ b/src/ros_export.cpp
@@ -79,6 +79,9 @@ namespace dynamicgraph
entity.add<ml::Vector> (signal, topic);
else if (type == "matrixHomo")
entity.add<sot::MatrixHomogeneous> (signal, topic);
+ else if (type == "matrixHomoStamped")
+ entity.add<std::pair<sot::MatrixHomogeneous, ml::Vector> >
+ (signal, topic);
else
throw std::runtime_error("bad type");
return Value ();
diff --git a/src/ros_export.hh b/src/ros_export.hh
index b93083491e66f7fb58f777024e0a9349bf65a761..800df9cbf73211b1ba0235c033d800f1c192bf8d 100644
--- a/src/ros_export.hh
+++ b/src/ros_export.hh
@@ -47,6 +47,13 @@ namespace dynamicgraph
} // end of namespace command.
+ namespace internal
+ {
+ template <typename T>
+ struct Add;
+ } // end of internal namespace.
+
+ /// \brief Publish ROS information in the dynamic-graph.
class RosExport : public dynamicgraph::Entity
{
DYNAMIC_GRAPH_ENTITY_DECL();
@@ -68,12 +75,30 @@ namespace dynamicgraph
template <typename T>
void add (const std::string& signal, const std::string& topic);
- private:
+ std::map<std::string, bindedSignal_t>&
+ bindedSignal ()
+ {
+ return bindedSignal_;
+ }
+
+ ros::NodeHandle& nh ()
+ {
+ return nh_;
+ }
+
template <typename R, typename S>
void callback
(boost::shared_ptr<dynamicgraph::SignalPtr<S, int> > signal,
const R& data);
+ template <typename R>
+ void callbackTimestamp
+ (boost::shared_ptr<dynamicgraph::SignalPtr<ml::Vector, int> > signal,
+ const R& data);
+
+ template <typename T>
+ friend class internal::Add;
+ private:
ros::NodeHandle nh_;
std::map<std::string, bindedSignal_t> bindedSignal_;
ros::AsyncSpinner spinner_;
diff --git a/src/ros_export.hxx b/src/ros_export.hxx
index 0970efb5f783d1d7c21086ef869eb1d373472244..be9eab92c9c23b8eff8a11351f17f698fefd0956 100644
--- a/src/ros_export.hxx
+++ b/src/ros_export.hxx
@@ -7,8 +7,6 @@
# include "dynamic_graph_bridge/Matrix.h"
# include "dynamic_graph_bridge/Vector.h"
-#include <iostream>//FIXME:
-
namespace ml = ::maal::boost;
namespace dynamicgraph
@@ -25,36 +23,139 @@ namespace dynamicgraph
signal->setConstant (value);
}
+ template <typename R>
+ void
+ RosExport::callbackTimestamp
+ (boost::shared_ptr<dynamicgraph::SignalPtr<ml::Vector, int> > signal,
+ const R& data)
+ {
+ ml::Vector time (2);
+ time (0) = data->header.stamp.sec;
+ // Convert nanoseconds into microseconds (i.e. timeval structure).
+ time (1) = data->header.stamp.nsec / 1000.;
+ signal->setConstant(time);
+ }
- template <typename T>
- void RosExport::add (const std::string& signal, const std::string& topic)
+ namespace internal
{
- typedef typename SotToRos<T>::sot_t sot_t;
- typedef typename SotToRos<T>::ros_const_ptr_t ros_const_ptr_t;
- typedef typename SotToRos<T>::signalIn_t signal_t;
+ template <typename T>
+ struct Add
+ {
+ void operator () (RosExport& rosExport,
+ const std::string& signal,
+ const std::string& topic)
+ {
+ typedef typename SotToRos<T>::sot_t sot_t;
+ typedef typename SotToRos<T>::ros_const_ptr_t ros_const_ptr_t;
+ typedef typename SotToRos<T>::signalIn_t signal_t;
+
+ // Initialize the bindedSignal object.
+ RosExport::bindedSignal_t bindedSignal;
+
+ // Initialize the signal.
+ boost::format signalName ("RosExport(%1%)::%2%");
+ signalName % rosExport.getName () % signal;
+
+ boost::shared_ptr<signal_t> signal_
+ (new signal_t (0, signalName.str ()));
+ signal_->setConstant (sot_t ());
+ bindedSignal.first = signal_;
+ rosExport.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);
+
+ bindedSignal.second =
+ boost::make_shared<ros::Subscriber>
+ (rosExport.nh ().subscribe (topic, 1, callback));
+
+ rosExport.bindedSignal ()[signal] = bindedSignal;
+ }
+ };
+
+ template <typename T>
+ struct Add<std::pair<T, ml::Vector> >
+ {
+ void operator () (RosExport& rosExport,
+ const std::string& signal,
+ const std::string& topic)
+ {
+ typedef std::pair<T, ml::Vector> type_t;
- // Initialize the bindedSignal object.
- bindedSignal_t bindedSignal;
+ typedef typename SotToRos<type_t>::sot_t sot_t;
+ typedef typename SotToRos<type_t>::ros_const_ptr_t ros_const_ptr_t;
+ typedef typename SotToRos<type_t>::signalIn_t signal_t;
- // Initialize the signal.
- boost::format signalName ("RosExport(%1%)::%2%");
- signalName % name % signal;
+ // Initialize the bindedSignal object.
+ RosExport::bindedSignal_t bindedSignal;
- boost::shared_ptr<signal_t> signal_ (new signal_t (0, signalName.str ()));
- signal_->setConstant (sot_t ());
- bindedSignal.first = signal_;
- signalRegistration (*bindedSignal.first);
+ // Initialize the signal.
+ boost::format signalName ("RosExport(%1%)::%2%");
+ signalName % rosExport.getName () % signal;
- // 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>,
- this, signal_, _1);
+ boost::shared_ptr<signal_t> signal_
+ (new signal_t (0, signalName.str ()));
+ signal_->setConstant (sot_t ());
+ bindedSignal.first = signal_;
+ rosExport.signalRegistration (*bindedSignal.first);
- bindedSignal.second =
- boost::make_shared<ros::Subscriber> (nh_.subscribe (topic, 1, callback));
+ // 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);
- bindedSignal_[signal] = bindedSignal;
+ bindedSignal.second =
+ boost::make_shared<ros::Subscriber>
+ (rosExport.nh ().subscribe (topic, 1, callback));
+
+ rosExport.bindedSignal ()[signal] = bindedSignal;
+
+
+ // Timestamp.
+ typedef dynamicgraph::SignalPtr<ml::Vector, int>
+ signalTimestamp_t;
+ std::string signalTimestamp =
+ (boost::format ("%1%%2%") % signal % "Timestamp").str ();
+
+ // Initialize the bindedSignal object.
+ RosExport::bindedSignal_t bindedSignalTimestamp;
+
+ // Initialize the signal.
+ boost::format signalNameTimestamp ("RosExport(%1%)::%2%");
+ signalNameTimestamp % rosExport.name % signalTimestamp;
+
+ boost::shared_ptr<signalTimestamp_t> signalTimestamp_
+ (new signalTimestamp_t (0, signalNameTimestamp.str ()));
+
+ ml::Vector zero (2);
+ zero.setZero ();
+ signalTimestamp_->setConstant (zero);
+ bindedSignalTimestamp.first = signalTimestamp_;
+ rosExport.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);
+
+ bindedSignalTimestamp.second =
+ boost::make_shared<ros::Subscriber>
+ (rosExport.nh ().subscribe (topic, 1, callbackTimestamp));
+
+ rosExport.bindedSignal ()[signalTimestamp] = bindedSignalTimestamp;
+ }
+ };
+ } // end of namespace internal.
+
+ template <typename T>
+ void RosExport::add (const std::string& signal, const std::string& topic)
+ {
+ internal::Add<T> () (*this, signal, topic);
}
} // end of namespace dynamicgraph.
diff --git a/src/ros_import.hh b/src/ros_import.hh
index 6f2a17a8d68ec3e6a0abdbeec848b9dc4c4ddedf..e45695a8bc6ac0ee6dd6f3d4e10e524197a8ccb1 100644
--- a/src/ros_import.hh
+++ b/src/ros_import.hh
@@ -45,6 +45,7 @@ namespace dynamicgraph
} // end of namespace command.
+ /// \brief Publish dynamic-graph information into ROS.
class RosImport : public dynamicgraph::Entity
{
DYNAMIC_GRAPH_ENTITY_DECL();
diff --git a/src/sot_to_ros.hh b/src/sot_to_ros.hh
index 2008815370920d6c23c642832e6763736d09b994..c228799bbd04046c259ed76a825a6d3e347793f5 100644
--- a/src/sot_to_ros.hh
+++ b/src/sot_to_ros.hh
@@ -1,12 +1,15 @@
#ifndef DYNAMIC_GRAPH_ROS_SOT_TO_ROS_HH
# define DYNAMIC_GRAPH_ROS_SOT_TO_ROS_HH
# include <vector>
+# include <utility>
+
# include <jrl/mal/boost.hh>
# include <std_msgs/Float64.h>
# include "dynamic_graph_bridge/Matrix.h"
# include "dynamic_graph_bridge/Vector.h"
# include "geometry_msgs/Transform.h"
+# include "geometry_msgs/TransformStamped.h"
# include <dynamic-graph/signal-time-dependent.h>
# include <dynamic-graph/signal-ptr.h>
@@ -16,6 +19,12 @@ namespace dynamicgraph
{
namespace ml = maal::boost;
+ /// \brief SotToRos trait type.
+ ///
+ /// This trait provides types associated to a dynamic-graph type:
+ /// - ROS corresponding type,
+ /// - signal type / input signal type,
+ /// - ROS callback type.
template <typename SotType>
class SotToRos;
@@ -63,6 +72,18 @@ namespace dynamicgraph
typedef boost::function<sot_t& (sot_t&, int)> callback_t;
};
+ // Stamped transformation
+ template <>
+ struct SotToRos<std::pair<sot::MatrixHomogeneous, ml::Vector> >
+ {
+ typedef sot::MatrixHomogeneous sot_t;
+ typedef geometry_msgs::TransformStamped ros_t;
+ typedef geometry_msgs::TransformStampedConstPtr ros_const_ptr_t;
+ typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t;
+ typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
+ typedef boost::function<sot_t& (sot_t&, int)> callback_t;
+ };
+
} // end of namespace dynamicgraph.
#endif //! DYNAMIC_GRAPH_ROS_SOT_TO_ROS_HH