From b0c3768ec0e279ae638e314d7de863cc5ee06842 Mon Sep 17 00:00:00 2001
From: Thomas Moulard <thomas.moulard@gmail.com>
Date: Mon, 14 Nov 2011 11:29:24 +0100
Subject: [PATCH] Add Twist support and clean code.
---
src/converter.hh | 267 ++++++++++++++++++++++++++++-----------------
src/ros_export.cpp | 5 +
src/ros_export.hxx | 4 +-
src/ros_import.cpp | 4 +
src/ros_import.hxx | 1 +
src/sot_to_ros.cpp | 4 +
src/sot_to_ros.hh | 88 +++++++++++++++
7 files changed, 273 insertions(+), 100 deletions(-)
diff --git a/src/converter.hh b/src/converter.hh
index bd4443f..4b12fc9 100644
--- a/src/converter.hh
+++ b/src/converter.hh
@@ -1,12 +1,37 @@
#ifndef DYNAMIC_GRAPH_ROS_CONVERTER_HH
# define DYNAMIC_GRAPH_ROS_CONVERTER_HH
+# include <stdexcept>
# include "sot_to_ros.hh"
+# include <boost/static_assert.hpp>
+
+# include <ros/time.h>
+# include <std_msgs/Header.h>
+
# include <LinearMath/btMatrix3x3.h>
# include <LinearMath/btQuaternion.h>
+# define SOT_TO_ROS_IMPL(T) \
+ template <> \
+ inline void \
+ converter (SotToRos<T>::ros_t& dst, const SotToRos<T>::sot_t& src)
+
+# define ROS_TO_SOT_IMPL(T) \
+ template <> \
+ inline void \
+ converter (SotToRos<T>::sot_t& dst, const SotToRos<T>::ros_t& src)
+
namespace dynamicgraph
{
+ inline
+ void
+ makeHeader(std_msgs::Header& header)
+ {
+ header.seq = 0;
+ header.stamp = ros::Time::now ();
+ header.frame_id = "/dynamic_graph/world";
+ }
+
/// \brief Handle ROS <-> dynamic-graph conversion.
///
/// Implements all ROS/dynamic-graph conversions required by the
@@ -17,23 +42,35 @@ namespace dynamicgraph
template <typename D, typename S>
void converter (D& dst, const S& src);
- template <>
- inline void converter (SotToRos<double>::ros_t& dst,
- const SotToRos<double>::sot_t& src)
+ // Double
+ SOT_TO_ROS_IMPL(double)
{
dst.data = src;
}
- template <>
- inline void converter (SotToRos<double>::sot_t& dst,
- const SotToRos<double>::ros_const_ptr_t& src)
+ ROS_TO_SOT_IMPL(double)
+ {
+ dst = src.data;
+ }
+
+ // Vector
+ SOT_TO_ROS_IMPL(ml::Vector)
+ {
+ dst.data.resize (src.size ());
+ for (unsigned i = 0; i < src.size (); ++i)
+ dst.data[i] = src.elementAt (i);
+ }
+
+ ROS_TO_SOT_IMPL(ml::Vector)
{
- dst = src->data;
+ dst.resize (src.data.size ());
+ for (unsigned i = 0; i < src.data.size (); ++i)
+ dst.elementAt (i) = src.data[i];
}
- template <>
- inline void converter (SotToRos<ml::Matrix>::ros_t& dst,
- const SotToRos<ml::Matrix>::sot_t& src)
+
+ // Matrix
+ SOT_TO_ROS_IMPL(ml::Matrix)
{
dst.width = src.nbRows ();
dst.data.resize (src.nbCols () * src.nbRows ());
@@ -41,9 +78,15 @@ namespace dynamicgraph
dst.data[i] = src.elementAt (i);
}
- template <>
- inline void converter (SotToRos<sot::MatrixHomogeneous>::ros_t& dst,
- const SotToRos<sot::MatrixHomogeneous>::sot_t& src)
+ ROS_TO_SOT_IMPL(ml::Matrix)
+ {
+ dst.resize (src.width, src.data.size () / src.width);
+ for (unsigned i = 0; i < src.data.size (); ++i)
+ dst.elementAt (i) = src.data[i];
+ }
+
+ // Homogeneous matrix.
+ SOT_TO_ROS_IMPL(sot::MatrixHomogeneous)
{
btMatrix3x3 rotation;
btQuaternion quaternion;
@@ -62,49 +105,7 @@ 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)
- {
- dst.data.resize (src.size ());
- for (unsigned i = 0; i < src.size (); ++i)
- dst.data[i] = src.elementAt (i);
- }
-
- template <>
- inline void converter
- (SotToRos<ml::Vector>::sot_t& dst,
- const SotToRos<ml::Vector>::ros_t& src)
- {
- dst.resize (src.data.size ());
- for (unsigned i = 0; i < src.data.size (); ++i)
- dst.elementAt (i) = src.data[i];
- }
-
- template <>
- inline void converter
- (SotToRos<ml::Matrix>::sot_t& dst,
- const SotToRos<ml::Matrix>::ros_t& src)
- {
- dst.resize (src.width, src.data.size () / src.width);
- for (unsigned i = 0; i < src.data.size (); ++i)
- dst.elementAt (i) = src.data[i];
- }
-
- template <>
- inline void converter
- (SotToRos<sot::MatrixHomogeneous>::sot_t& dst,
- const SotToRos<sot::MatrixHomogeneous>::ros_t& src)
+ ROS_TO_SOT_IMPL(sot::MatrixHomogeneous)
{
btQuaternion quaternion
(src.rotation.x, src.rotation.y, src.rotation.z, src.rotation.w);
@@ -121,58 +122,128 @@ 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,
- const boost::shared_ptr<SotToRos<ml::Vector>::ros_t const>& src)
+ // Twist.
+ SOT_TO_ROS_IMPL(specific::Twist)
{
- converter
- <SotToRos<ml::Vector>::sot_t,
- SotToRos<ml::Vector>::ros_t> (dst, *src);
+ if (src.size () != 6)
+ throw std::runtime_error ("failed to convert invalid twist");
+ dst.linear.x = src (0);
+ dst.linear.y = src (1);
+ dst.linear.z = src (2);
+ dst.angular.x = src (3);
+ dst.angular.y = src (4);
+ dst.angular.z = src (5);
}
- template <>
- inline void converter
- (SotToRos<ml::Matrix>::sot_t& dst,
- const boost::shared_ptr<SotToRos<ml::Matrix>::ros_t const>& src)
+ ROS_TO_SOT_IMPL(specific::Twist)
{
- converter
- <SotToRos<ml::Matrix>::sot_t,
- SotToRos<ml::Matrix>::ros_t> (dst, *src);
+ dst.resize (6);
+ dst (0) = src.linear.x;
+ dst (1) = src.linear.y;
+ dst (2) = src.linear.z;
+ dst (3) = src.angular.x;
+ dst (4) = src.angular.y;
+ dst (5) = src.angular.z;
}
- template <>
- inline void converter
- (SotToRos<sot::MatrixHomogeneous>::sot_t& dst,
- const boost::shared_ptr<SotToRos<sot::MatrixHomogeneous>::ros_t const>& src)
- {
- converter
- <SotToRos<sot::MatrixHomogeneous>::sot_t,
- 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)
+ /// \brief This macro generates a converter for a stamped type from
+ /// dynamic-graph to ROS. I.e. A data associated with its
+ /// timestamp.
+# define DG_BRIDGE_TO_ROS_MAKE_STAMPED_IMPL(T, ATTRIBUTE, EXTRA) \
+ template <> \
+ inline void converter \
+ (SotToRos<std::pair<T, ml::Vector> >::ros_t& dst, \
+ const SotToRos<std::pair<T, ml::Vector> >::sot_t& src) \
+ { \
+ makeHeader(dst.header); \
+ converter<SotToRos<T>::ros_t, SotToRos<T>::sot_t> (dst.ATTRIBUTE, src); \
+ do { EXTRA } while (0); \
+ } \
+ struct e_n_d__w_i_t_h__s_e_m_i_c_o_l_o_n
+
+ DG_BRIDGE_TO_ROS_MAKE_STAMPED_IMPL(sot::MatrixHomogeneous, transform,
+ dst.child_frame_id = "";);
+ DG_BRIDGE_TO_ROS_MAKE_STAMPED_IMPL(specific::Twist, twist, ;);
+
+
+ /// \brief This macro generates a converter for a shared pointer on
+ /// a ROS type to a dynamic-graph type.
+ ///
+ /// A converter for the underlying type is required. I.e. to
+ /// convert a shared_ptr<T> to T', a converter from T to T'
+ /// is required.
+# define DG_BRIDGE_MAKE_SHPTR_IMPL(T) \
+ template <> \
+ inline void converter \
+ (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
+
+ DG_BRIDGE_MAKE_SHPTR_IMPL(double);
+ DG_BRIDGE_MAKE_SHPTR_IMPL(ml::Vector);
+ DG_BRIDGE_MAKE_SHPTR_IMPL(ml::Matrix);
+ DG_BRIDGE_MAKE_SHPTR_IMPL(sot::MatrixHomogeneous);
+ DG_BRIDGE_MAKE_SHPTR_IMPL(specific::Twist);
+
+ /// \brief This macro generates a converter for a stamped type.
+ /// I.e. A data associated with its timestamp.
+ ///
+ /// FIXME: the timestamp is not yet forwarded to the dg signal.
+# define DG_BRIDGE_MAKE_STAMPED_IMPL(T, ATTRIBUTE, EXTRA) \
+ template <> \
+ inline void converter \
+ (SotToRos<std::pair<T, ml::Vector> >::sot_t& dst, \
+ const SotToRos<std::pair<T, ml::Vector> >::ros_t& src) \
+ { \
+ converter<SotToRos<T>::sot_t, SotToRos<T>::ros_t> (dst, src.ATTRIBUTE); \
+ do { EXTRA } while (0); \
+ } \
+ struct e_n_d__w_i_t_h__s_e_m_i_c_o_l_o_n
+
+ DG_BRIDGE_MAKE_STAMPED_IMPL(sot::MatrixHomogeneous, transform, ;);
+ DG_BRIDGE_MAKE_STAMPED_IMPL(specific::Twist, twist, ;);
+
+ /// \brief This macro generates a converter for a shared pointer on
+ /// a stamped type. I.e. A data associated with its timestamp.
+ ///
+ /// FIXME: the timestamp is not yet forwarded to the dg signal.
+# define DG_BRIDGE_MAKE_STAMPED_SHPTR_IMPL(T, ATTRIBUTE, EXTRA) \
+ template <> \
+ inline void converter \
+ (SotToRos<std::pair<T, ml::Vector> >::sot_t& dst, \
+ const boost::shared_ptr \
+ <SotToRos<std::pair<T, ml::Vector> >::ros_t const>& src) \
+ { \
+ converter<SotToRos<T>::sot_t, SotToRos<T>::ros_t> (dst, src->ATTRIBUTE); \
+ do { EXTRA } while (0); \
+ } \
+ struct e_n_d__w_i_t_h__s_e_m_i_c_o_l_o_n
+
+ DG_BRIDGE_MAKE_STAMPED_SHPTR_IMPL(sot::MatrixHomogeneous, transform, ;);
+ DG_BRIDGE_MAKE_STAMPED_SHPTR_IMPL(specific::Twist, twist, ;);
+
+
+ /// \brief If an impossible/unimplemented conversion is required, fail.
+ ///
+ /// IMPORTANT, READ ME:
+ ///
+ /// If the compiler generates an error in the following function,
+ /// this is /normal/.
+ ///
+ /// This error means that either you try to use an undefined
+ /// conversion. You can either fix your code or provide the wanted
+ /// conversion by updating this header.
+ template <typename U, typename V>
+ inline void converter (U& dst, V& src)
{
- converter
- <SotToRos<sot::MatrixHomogeneous>::sot_t,
- SotToRos<sot::MatrixHomogeneous>::ros_t> (dst, src->transform);
+ // This will always fail if instantiated.
+ BOOST_STATIC_ASSERT (sizeof (U) == 0);
}
-
-
} // end of namespace dynamicgraph.
#endif //! DYNAMIC_GRAPH_ROS_CONVERTER_HH
diff --git a/src/ros_export.cpp b/src/ros_export.cpp
index d28ff28..f5f6e53 100644
--- a/src/ros_export.cpp
+++ b/src/ros_export.cpp
@@ -82,6 +82,11 @@ namespace dynamicgraph
else if (type == "matrixHomoStamped")
entity.add<std::pair<sot::MatrixHomogeneous, ml::Vector> >
(signal, topic);
+ else if (type == "Twist")
+ entity.add<specific::Twist> (signal, topic);
+ else if (type == "TwistStamped")
+ entity.add<std::pair<specific::Twist, ml::Vector> >
+ (signal, topic);
else
throw std::runtime_error("bad type");
return Value ();
diff --git a/src/ros_export.hxx b/src/ros_export.hxx
index 64fe3c5..0293e67 100644
--- a/src/ros_export.hxx
+++ b/src/ros_export.hxx
@@ -58,7 +58,7 @@ namespace dynamicgraph
boost::shared_ptr<signal_t> signal_
(new signal_t (0, signalName.str ()));
- signal_->setConstant (sot_t ());
+ SotToRos<T>::setDefault(*signal_);
bindedSignal.first = signal_;
rosExport.signalRegistration (*bindedSignal.first);
@@ -98,7 +98,7 @@ namespace dynamicgraph
boost::shared_ptr<signal_t> signal_
(new signal_t (0, signalName.str ()));
- signal_->setConstant (sot_t ());
+ SotToRos<T>::setDefault(*signal_);
bindedSignal.first = signal_;
rosExport.signalRegistration (*bindedSignal.first);
diff --git a/src/ros_import.cpp b/src/ros_import.cpp
index a6990e6..556c69c 100644
--- a/src/ros_import.cpp
+++ b/src/ros_import.cpp
@@ -86,6 +86,10 @@ namespace dynamicgraph
else if (type == "matrixHomoStamped")
entity.add<std::pair<sot::MatrixHomogeneous, ml::Vector> >
(signal, topic);
+ else if (type == "twist")
+ entity.add<specific::Twist> (signal, topic);
+ else if (type == "twistStamped")
+ entity.add<std::pair<specific::Twist, ml::Vector> > (signal, topic);
else
throw std::runtime_error("bad type");
return Value ();
diff --git a/src/ros_import.hxx b/src/ros_import.hxx
index 146714f..8555565 100644
--- a/src/ros_import.hxx
+++ b/src/ros_import.hxx
@@ -51,6 +51,7 @@ namespace dynamicgraph
boost::make_shared<signal_t>
(MAKE_SIGNAL_STRING(name, true, SotToRos<T>::signalTypeName, signal));
boost::get<0> (bindedSignal) = signalPtr;
+ SotToRos<T>::setDefault(*signalPtr);
signalRegistration (*boost::get<0> (bindedSignal));
// Initialize the callback.
diff --git a/src/sot_to_ros.cpp b/src/sot_to_ros.cpp
index 89df1c0..bfdbc79 100644
--- a/src/sot_to_ros.cpp
+++ b/src/sot_to_ros.cpp
@@ -7,8 +7,12 @@ namespace dynamicgraph
const char* SotToRos<ml::Matrix>::signalTypeName = "Matrix";
const char* SotToRos<ml::Vector>::signalTypeName = "Vector";
const char* SotToRos<sot::MatrixHomogeneous>::signalTypeName = "MatrixHomo";
+ const char* SotToRos<specific::Twist>::signalTypeName = "Twist";
const char* SotToRos
<std::pair<sot::MatrixHomogeneous, ml::Vector> >::signalTypeName
= "MatrixHomo";
+ const char* SotToRos
+ <std::pair<specific::Twist, ml::Vector> >::signalTypeName
+ = "Twist";
} // end of namespace dynamicgraph.
diff --git a/src/sot_to_ros.hh b/src/sot_to_ros.hh
index bf5752f..8dbbb35 100644
--- a/src/sot_to_ros.hh
+++ b/src/sot_to_ros.hh
@@ -12,6 +12,8 @@
# include "geometry_msgs/Transform.h"
# include "geometry_msgs/TransformStamped.h"
+# include "geometry_msgs/Twist.h"
+# include "geometry_msgs/TwistStamped.h"
# include <dynamic-graph/signal-time-dependent.h>
# include <dynamic-graph/signal-ptr.h>
@@ -25,6 +27,16 @@ namespace dynamicgraph
{
namespace ml = maal::boost;
+ /// \brief Types dedicated to identify pairs of (dg,ros) data.
+ ///
+ /// They do not contain any data but allow to make the difference
+ /// between different types with the same structure.
+ /// For instance a vector of six elements vs a twist coordinate.
+ namespace specific
+ {
+ class Twist {};
+ } // end of namespace specific.
+
/// \brief SotToRos trait type.
///
/// This trait provides types associated to a dynamic-graph type:
@@ -45,6 +57,12 @@ namespace dynamicgraph
typedef boost::function<sot_t& (sot_t&, int)> callback_t;
static const char* signalTypeName;
+
+ template <typename S>
+ static void setDefault(S& s)
+ {
+ s.setConstant (0.);
+ }
};
template <>
@@ -58,6 +76,14 @@ namespace dynamicgraph
typedef boost::function<sot_t& (sot_t&, int)> callback_t;
static const char* signalTypeName;
+
+ template <typename S>
+ static void setDefault(S& s)
+ {
+ ml::Matrix m;
+ m.resize(0, 0);
+ s.setConstant (m);
+ }
};
template <>
@@ -71,6 +97,14 @@ namespace dynamicgraph
typedef boost::function<sot_t& (sot_t&, int)> callback_t;
static const char* signalTypeName;
+
+ template <typename S>
+ static void setDefault(S& s)
+ {
+ ml::Vector v;
+ v.resize (0);
+ s.setConstant (v);
+ }
};
template <>
@@ -84,6 +118,34 @@ namespace dynamicgraph
typedef boost::function<sot_t& (sot_t&, int)> callback_t;
static const char* signalTypeName;
+
+ template <typename S>
+ static void setDefault(S& s)
+ {
+ sot::MatrixHomogeneous m;
+ s.setConstant (m);
+ }
+ };
+
+ template <>
+ struct SotToRos<specific::Twist>
+ {
+ typedef ml::Vector sot_t;
+ typedef geometry_msgs::Twist ros_t;
+ typedef geometry_msgs::TwistConstPtr 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;
+
+ static const char* signalTypeName;
+
+ template <typename S>
+ static void setDefault(S& s)
+ {
+ ml::Vector v (6);
+ v.setZero ();
+ s.setConstant (v);
+ }
};
// Stamped transformation
@@ -98,6 +160,32 @@ namespace dynamicgraph
typedef boost::function<sot_t& (sot_t&, int)> callback_t;
static const char* signalTypeName;
+
+ template <typename S>
+ static void setDefault(S& s)
+ {
+ SotToRos<sot_t>::setDefault(s);
+ }
+ };
+
+ // Stamped twist
+ template <>
+ struct SotToRos<std::pair<specific::Twist, ml::Vector> >
+ {
+ typedef ml::Vector sot_t;
+ typedef geometry_msgs::TwistStamped ros_t;
+ typedef geometry_msgs::TwistStampedConstPtr 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;
+
+ static const char* signalTypeName;
+
+ template <typename S>
+ static void setDefault(S& s)
+ {
+ SotToRos<sot_t>::setDefault(s);
+ }
};
inline std::string
--
GitLab