Fix implementation.

src/converter.hh

@@ -8,22 +8,22 @@ namespace dynamicgraph
   void converter (D& dst, const S& src);
 
   template <>
-  void converter (SotToRos<double>::ros_t& dst,
-		  const SotToRos<double>::sot_t& src)
+  inline void converter (SotToRos<double>::ros_t& dst,
+			 const SotToRos<double>::sot_t& src)
   {
     dst.data = src;
   }
 
   template <>
-  void converter (SotToRos<double>::sot_t& dst,
-		  const SotToRos<double>::ros_t& src)
+  inline void converter (SotToRos<double>::sot_t& dst,
+			 const SotToRos<double>::ros_const_ptr_t& src)
   {
-    dst = src.data;
+    dst = src->data;
   }
 
   template <>
-  void converter (SotToRos<ml::Matrix>::ros_t& dst,
-		  const SotToRos<ml::Matrix>::sot_t& src)
+  inline 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 ());
@@ -32,8 +32,8 @@ namespace dynamicgraph
   }
 
   template <>
-  void converter (SotToRos<ml::Vector>::ros_t& dst,
-		  const SotToRos<ml::Vector>::sot_t& src)
+  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)

src/ros_export.cpp

@@ -64,11 +64,13 @@ namespace dynamicgraph
       {
 	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";
       }

src/ros_export.hh

@@ -45,9 +45,15 @@ namespace dynamicgraph
     template <typename T>
     void add (const std::string& signal, const std::string& topic);
 
-    template <typename T>
-    void callback (boost::shared_ptr<dynamicgraph::SignalBase<int> > signal,
-		   const T& message);
+    // template <typename R, typename S>
+    // void callback
+    // (boost::shared_ptr<dynamicgraph::SignalTimeDependent<S, int> > signal,
+    //  const R& data);
+
+    template <typename R, typename S>
+    void callback
+    (boost::shared_ptr<dynamicgraph::SignalTimeDependent<S, int> > signal,
+     const R& data);
 
     ros::NodeHandle nh_;
     std::map<std::string, bindedSignal_t> bindedSignal_;

src/ros_export.hxx

@@ -12,24 +12,37 @@ namespace ml = maal::boost;
 
 namespace dynamicgraph
 {
-  template <typename T>
+  // template <typename R, typename S>
+  // void
+  // RosExport::callback
+  // (boost::shared_ptr<dynamicgraph::SignalTimeDependent<S, int> > signal,
+  //  const R& data)
+  // {
+  //   // typedef S sot_t;
+  //   // sot_t value;
+  //   // converter (value, data);
+  //   // (*signal) (value);
+  // }
+
+  template <typename R, typename S>
   void
-  RosExport::callback (boost::shared_ptr<dynamicgraph::SignalBase<int> > signal,
-		       const T& data)
+  RosExport::callback
+  (boost::shared_ptr<dynamicgraph::SignalTimeDependent<S, int> > signal,
+   const R& data)
   {
-    typedef typename SotToRos<T>::sot_t sot_t;
+    typedef S sot_t;
     sot_t value;
     converter (value, data);
     (*signal) (value);
   }
 
+
   template <typename T>
   void RosExport::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;
+    typedef typename SotToRos<T>::ros_const_ptr_t ros_const_ptr_t;
     typedef typename SotToRos<T>::signal_t signal_t;
-    typedef typename SotToRos<T>::callback_t callback_t;
 
     // Initialize the bindedSignal object.
     bindedSignal_t bindedSignal;
@@ -38,15 +51,19 @@ namespace dynamicgraph
     boost::format signalName ("RosExport(%1%)::%2%");
     signalName % name % signal;
 
-    bindedSignal.first = boost::make_shared<signal_t>(0, signalName.str ());
+    boost::shared_ptr<signal_t> signal_ =
+      boost::make_shared<signal_t>(0, signalName.str ());
+    bindedSignal.first = signal_;
     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>,
+       this, signal_, _1);
+
     bindedSignal.second =
-      boost::make_shared<ros::Publisher>
-      (nh_.subscribe
-       (topic, 1, boost::bind (&RosExport::callback<T>,
-			       this, bindedSignal.first)));
+      boost::make_shared<ros::Subscriber> (nh_.subscribe (topic, 1, callback));
 
     bindedSignal_[signal] = bindedSignal;
   }

src/sot_to_ros.hh

@@ -18,6 +18,7 @@ namespace dynamicgraph
   {
     typedef double sot_t;
     typedef std_msgs::Float64 ros_t;
+    typedef std_msgs::Float64ConstPtr ros_const_ptr_t;
     typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t;
     typedef boost::function<sot_t& (sot_t&, int)> callback_t;
   };
@@ -27,6 +28,7 @@ namespace dynamicgraph
   {
     typedef ml::Matrix sot_t;
     typedef dynamic_graph::Matrix ros_t;
+    typedef dynamic_graph::MatrixConstPtr ros_const_ptr_t;
     typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t;
     typedef boost::function<sot_t& (sot_t&, int)> callback_t;
   };
@@ -36,6 +38,7 @@ namespace dynamicgraph
   {
     typedef ml::Vector sot_t;
     typedef dynamic_graph::Vector ros_t;
+    typedef dynamic_graph::VectorConstPtr ros_const_ptr_t;
     typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t;
     typedef boost::function<sot_t& (sot_t&, int)> callback_t;
   };