Bind vector and matrix MAL types.

.gitignore

 lib/
 msg/lisp/
 msg_gen/
-src/hueblob/
+src/dynamic_graph/
 bin/

CMakeLists.txt

@@ -17,7 +17,7 @@ set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
 set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
 
 #uncomment if you have defined messages
-#rosbuild_genmsg()
+rosbuild_genmsg()
 #uncomment if you have defined services
 #rosbuild_gensrv()
 

manifest.xml

@@ -12,5 +12,6 @@
   <depend package="std_msgs"/>
   <depend package="roscpp"/>
 
+  <rosdep name="jrl-mal"/>
   <rosdep name="dynamic-graph"/>
 </package>

msg/Matrix.msg

+float64[] data
+uint32 width

msg/Vector.msg

+float64[] data

src/ros_import.cpp

@@ -56,8 +56,15 @@ namespace dynamicgraph
       }
     else if (cmdLine == "add")
       {
-	cmdArgs >> signal >> topic;
-	add<double> (signal, topic);
+	cmdArgs >> type >> signal >> topic;
+	if (type == "double")
+	  add<double> (signal, topic);
+	else if (type == "matrix")
+	  add<ml::Matrix> (signal, topic);
+	else if (type == "vector")
+	  add<ml::Vector> (signal, topic);
+	else
+	  throw "bad type";
       }
     else if (cmdLine == "rm")
       {

src/ros_import.hh

 #ifndef DYNAMIC_GRAPH_ROS_IMPORT_HH
 # define DYNAMIC_GRAPH_ROS_IMPORT_HH
 # include <iostream>
+# include <map>
 
 # include <boost/shared_ptr.hpp>
 

src/ros_import.hxx

 #ifndef DYNAMIC_GRAPH_ROS_IMPORT_HXX
 # define DYNAMIC_GRAPH_ROS_IMPORT_HXX
+# include <vector>
+# include <jrl/mal/boost.hh>
 # include <std_msgs/Float64.h>
+# include "dynamic_graph/Matrix.h"
+# include "dynamic_graph/Vector.h"
+
+
+namespace ml = maal::boost;
 
 namespace dynamicgraph
 {
@@ -13,6 +20,25 @@ namespace dynamicgraph
     typedef boost::function<sot_t& (sot_t&, int)> callback_t;
   };
 
+  template <>
+  struct SotToRos<ml::Matrix>
+  {
+    typedef ml::Matrix sot_t;
+    typedef dynamic_graph::Matrix ros_t;
+    typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t;
+    typedef boost::function<sot_t& (sot_t&, int)> callback_t;
+  };
+
+  template <>
+  struct SotToRos<ml::Vector>
+  {
+    typedef ml::Vector sot_t;
+    typedef dynamic_graph::Vector ros_t;
+    typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t;
+    typedef boost::function<sot_t& (sot_t&, int)> callback_t;
+  };
+
+
   template <>
   void converter (SotToRos<double>::ros_t& dst,
 		  const SotToRos<double>::sot_t& src)
@@ -20,6 +46,26 @@ namespace dynamicgraph
     dst.data = src;
   }
 
+  template <>
+  void converter (SotToRos<ml::Matrix>::ros_t& dst,
+		  const SotToRos<ml::Matrix>::sot_t& src)
+  {
+    dst.width = src.nbRows ();
+    dst.data.resize (src.nbCols () * src.nbRows ());
+    for (unsigned i = 0; i < src.nbCols () * src.nbRows (); ++i)
+      dst.data[i] =  src.elementAt (i);
+  }
+
+  template <>
+  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 <typename T>
   T&
   RosImport::sendData (boost::shared_ptr<ros::Publisher> publisher,