Add support for vector3 and add tf_broadcaster.

manifest.xml

@@ -16,6 +16,7 @@
   <depend package="geometry_msgs"/>
   <depend package="sensor_msgs"/>
   <depend package="bullet"/>
+  <depend package="tf"/>
 
   <depend package="realtime_tools"/>
 </package>

src/converter.hh

@@ -68,6 +68,22 @@ namespace dynamicgraph
       dst.elementAt (i) =  src.data[i];
   }
 
+  // Vector3
+  SOT_TO_ROS_IMPL(specific::Vector3)
+  {
+    assert (src.size () == 3);
+    dst.x =  src.elementAt (0);
+    dst.y =  src.elementAt (1);
+    dst.z =  src.elementAt (2);
+  }
+
+  ROS_TO_SOT_IMPL(specific::Vector3)
+  {
+    dst.resize (3);
+    dst.elementAt (0) =  src.x;
+    dst.elementAt (1) =  src.y;
+    dst.elementAt (2) =  src.z;
+  }
 
   // Matrix
   SOT_TO_ROS_IMPL(ml::Matrix)
@@ -163,6 +179,7 @@ namespace dynamicgraph
   }									\
   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(sot::MatrixHomogeneous, transform,
 				     dst.child_frame_id = "";);
   DG_BRIDGE_TO_ROS_MAKE_STAMPED_IMPL(specific::Twist, twist, ;);
@@ -186,6 +203,7 @@ namespace dynamicgraph
 
   DG_BRIDGE_MAKE_SHPTR_IMPL(double);
   DG_BRIDGE_MAKE_SHPTR_IMPL(ml::Vector);
+  DG_BRIDGE_MAKE_SHPTR_IMPL(specific::Vector3);
   DG_BRIDGE_MAKE_SHPTR_IMPL(ml::Matrix);
   DG_BRIDGE_MAKE_SHPTR_IMPL(sot::MatrixHomogeneous);
   DG_BRIDGE_MAKE_SHPTR_IMPL(specific::Twist);
@@ -205,6 +223,7 @@ namespace dynamicgraph
   }									\
   struct e_n_d__w_i_t_h__s_e_m_i_c_o_l_o_n
 
+  DG_BRIDGE_MAKE_STAMPED_IMPL(specific::Vector3, vector, ;);
   DG_BRIDGE_MAKE_STAMPED_IMPL(sot::MatrixHomogeneous, transform, ;);
   DG_BRIDGE_MAKE_STAMPED_IMPL(specific::Twist, twist, ;);
 
@@ -224,6 +243,7 @@ namespace dynamicgraph
   }									\
   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(sot::MatrixHomogeneous, transform, ;);
   DG_BRIDGE_MAKE_STAMPED_SHPTR_IMPL(specific::Twist, twist, ;);
 

src/dynamic_graph/ros/ros.py

@@ -10,12 +10,28 @@ class Ros(object):
     rosExport = None
     rosJointState = None
 
-    def __init__(self, device, suffix = ''):
-        self.device = device
+    def __init__(self, robot, suffix = ''):
+        self.robot = robot
         self.rosImport = RosImport('rosImport{0}'.format(suffix))
         self.rosExport = RosExport('rosExport{0}'.format(suffix))
         self.rosJointState = RosJointState('rosJointState{0}'.format(suffix))
 
-        plug(self.device.state, self.rosJointState.state)
-        self.device.after.addSignal('{0}.trigger'.format(self.rosImport.name))
-        self.device.after.addSignal('{0}.trigger'.format(self.rosJointState.name))
+        plug(self.robot.device.state, self.rosJointState.state)
+        self.robot.device.after.addSignal('{0}.trigger'.format(self.rosImport.name))
+        self.robot.device.after.addSignal('{0}.trigger'.format(self.rosJointState.name))
+
+        # Export base_footprint aka dynamic-graph world frame.
+        self.rosImport.add('matrixHomoStamped', 'base_footprint', 'base_footprint')
+        plug(robot.dynamic.signal('left-ankle'), self.rosImport.base_footprint)
+
+        # Export base_link aka waist frame.
+        self.rosImport.add('matrixHomoStamped', 'base_link', 'base_link')
+        plug(robot.dynamic.waist, self.rosImport.base_link)
+
+        # Export center of mass position.
+        self.rosImport.add('vector3Stamped', 'com', 'com')
+        plug(robot.dynamic.com, self.rosImport.com)
+
+        # Export ZMP position.
+        self.rosImport.add('vector3Stamped', 'zmp', 'zmp')
+        plug(robot.dynamic.zmp, self.rosImport.zmp)

src/ros_export.cpp

@@ -78,6 +78,10 @@ namespace dynamicgraph
 	  entity.add<ml::Matrix> (signal, topic);
 	else if (type == "vector")
 	  entity.add<ml::Vector> (signal, topic);
+	else if (type == "vector3")
+	  entity.add<specific::Vector3> (signal, topic);
+	else if (type == "vector3Stamped")
+	  entity.add<std::pair<specific::Vector3, ml::Vector> > (signal, topic);
 	else if (type == "matrixHomo")
 	  entity.add<sot::MatrixHomogeneous> (signal, topic);
 	else if (type == "matrixHomoStamped")

src/ros_import.cpp

@@ -82,6 +82,10 @@ namespace dynamicgraph
 	  entity.add<ml::Matrix> (signal, topic);
 	else if (type == "vector")
 	  entity.add<ml::Vector> (signal, topic);
+	else if (type == "vector3")
+	  entity.add<specific::Vector3> (signal, topic);
+	else if (type == "vector3Stamped")
+	  entity.add<std::pair<specific::Vector3, ml::Vector> > (signal, topic);
 	else if (type == "matrixHomo")
 	  entity.add<sot::MatrixHomogeneous> (signal, topic);
 	else if (type == "matrixHomoStamped")

src/ros_import.hxx

@@ -12,6 +12,30 @@
 
 namespace dynamicgraph
 {
+  template <>
+  inline void
+  RosImport::sendData
+  <std::pair<sot::MatrixHomogeneous, ml::Vector> >
+  (boost::shared_ptr
+   <realtime_tools::RealtimePublisher
+   <SotToRos
+   <std::pair<sot::MatrixHomogeneous, ml::Vector> >::ros_t> > publisher,
+   boost::shared_ptr
+   <SotToRos
+   <std::pair<sot::MatrixHomogeneous, ml::Vector> >::signalIn_t> signal,
+   int time)
+  {
+    SotToRos
+      <std::pair
+      <sot::MatrixHomogeneous, ml::Vector> >::ros_t result;
+    if (publisher->trylock ())
+      {
+	publisher->msg_.child_frame_id = "/dynamic_graph/world";
+	converter (publisher->msg_, signal->access (time));
+	publisher->unlockAndPublish ();
+      }
+  }
+
   template <typename T>
   void
   RosImport::sendData

src/sot_to_ros.cpp

@@ -6,9 +6,13 @@ namespace dynamicgraph
   const char* SotToRos<double>::signalTypeName = "Double";
   const char* SotToRos<ml::Matrix>::signalTypeName = "Matrix";
   const char* SotToRos<ml::Vector>::signalTypeName = "Vector";
+  const char* SotToRos<specific::Vector3>::signalTypeName = "Vector3";
   const char* SotToRos<sot::MatrixHomogeneous>::signalTypeName = "MatrixHomo";
   const char* SotToRos<specific::Twist>::signalTypeName = "Twist";
   const char* SotToRos
+  <std::pair<specific::Vector3, ml::Vector> >::signalTypeName
+  = "Vector3Stamped";
+  const char* SotToRos
   <std::pair<sot::MatrixHomogeneous, ml::Vector> >::signalTypeName
   = "MatrixHomo";
   const char* SotToRos

src/sot_to_ros.hh

@@ -14,6 +14,7 @@
 # include "geometry_msgs/TransformStamped.h"
 # include "geometry_msgs/Twist.h"
 # include "geometry_msgs/TwistStamped.h"
+# include "geometry_msgs/Vector3Stamped.h"
 
 # include <dynamic-graph/signal-time-dependent.h>
 # include <dynamic-graph/signal-ptr.h>
@@ -34,6 +35,7 @@ namespace dynamicgraph
   /// For instance a vector of six elements vs a twist coordinate.
   namespace specific
   {
+    class Vector3 {};
     class Twist {};
   } // end of namespace specific.
 
@@ -107,6 +109,27 @@ namespace dynamicgraph
     }
   };
 
+  template <>
+  struct SotToRos<specific::Vector3>
+  {
+    typedef ml::Vector sot_t;
+    typedef geometry_msgs::Vector3 ros_t;
+    typedef geometry_msgs::Vector3ConstPtr 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;
+      v.resize (0);
+      s.setConstant (v);
+    }
+  };
+
   template <>
   struct SotToRos<sot::MatrixHomogeneous>
   {
@@ -148,6 +171,26 @@ namespace dynamicgraph
     }
   };
 
+  // Stamped vector3
+  template <>
+  struct SotToRos<std::pair<specific::Vector3, ml::Vector> >
+  {
+    typedef ml::Vector sot_t;
+    typedef geometry_msgs::Vector3Stamped ros_t;
+    typedef geometry_msgs::Vector3StampedConstPtr 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);
+    }
+  };
+
   // Stamped transformation
   template <>
   struct SotToRos<std::pair<sot::MatrixHomogeneous, ml::Vector> >