Clean and implement export entity.

CMakeLists.txt

@@ -34,13 +34,15 @@ rosbuild_genmsg()
 pkg_check_modules(JRL_MAL REQUIRED jrl-mal)
 pkg_check_modules(DYNAMIC_GRAPH REQUIRED dynamic-graph)
 
-rosbuild_add_library(ros_import src/ros_import.cpp)
+rosbuild_add_library(ros_import
+  src/ros_import.cpp src/ros_import.hh src/converter.hh src/sot_to_ros.hh)
 rosbuild_add_compile_flags(ros_import ${JRL_MAL_CFLAGS} ${DYNAMIC_GRAPH_CFLAGS})
 rosbuild_add_link_flags(ros_import ${JRL_MAL_LDFLAGS} ${DYNAMIC_GRAPH_LDFLAGS})
 target_link_libraries(ros_import
   ${JRL_MAL_LIBRARIES} ${DYNAMIC_GRAPH_LIBRARIES})
 
-rosbuild_add_library(ros_export src/ros_export.cpp)
+rosbuild_add_library(ros_export
+  src/ros_export.cpp src/ros_export.hh src/converter.hh src/sot_to_ros.hh)
 rosbuild_add_compile_flags(ros_export ${JRL_MAL_CFLAGS} ${DYNAMIC_GRAPH_CFLAGS})
 rosbuild_add_link_flags(ros_export ${JRL_MAL_LDFLAGS} ${DYNAMIC_GRAPH_LDFLAGS})
 target_link_libraries(ros_export

src/converter.hh

+#ifndef DYNAMIC_GRAPH_ROS_CONVERTER_HH
+# define DYNAMIC_GRAPH_ROS_CONVERTER_HH
+# include "sot_to_ros.hh"
+
+namespace dynamicgraph
+{
+  template <typename D, typename S>
+  void converter (D& dst, const S& src);
+
+  template <>
+  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)
+  {
+    dst = src.data;
+  }
+
+  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);
+  }
+
+
+} // end of namespace dynamicgraph.
+
+#endif //! DYNAMIC_GRAPH_ROS_CONVERTER_HH

src/ros_export.cpp

@@ -29,10 +29,13 @@ namespace dynamicgraph
       nh_ (rosInit ()),
       bindedSignal_ ()
   {
+    ros::AsyncSpinner spinner (1);
+    spinner.start ();
   }
 
   RosExport::~RosExport ()
   {
+    ros::waitForShutdown();
   }
 
   void RosExport::display (std::ostream& os) const

src/ros_export.hh

@@ -10,13 +10,16 @@
 
 # include <ros/ros.h>
 
+# include "converter.hh"
+# include "sot_to_ros.hh"
+
 namespace dynamicgraph
 {
   class RosExport : public dynamicgraph::Entity
   {
   public:
     typedef std::pair<boost::shared_ptr<dynamicgraph::SignalBase<int> >,
-		      boost::shared_ptr<ros::Publisher> >
+		      boost::shared_ptr<ros::Subscriber> >
       bindedSignal_t;
 
     static const std::string CLASS_NAME;
@@ -39,6 +42,13 @@ namespace dynamicgraph
     void list ();
     void clear ();
 
+    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);
+
     ros::NodeHandle nh_;
     std::map<std::string, bindedSignal_t> bindedSignal_;
   };

src/ros_export.hxx

 #ifndef DYNAMIC_GRAPH_ROS_EXPORT_HXX
 # define DYNAMIC_GRAPH_ROS_EXPORT_HXX
 # include <vector>
+# include <boost/bind.hpp>
 # include <jrl/mal/boost.hh>
 # include <std_msgs/Float64.h>
 # include "dynamic_graph/Matrix.h"
@@ -11,6 +12,44 @@ namespace ml = maal::boost;
 
 namespace dynamicgraph
 {
+  template <typename T>
+  void
+  RosExport::callback (boost::shared_ptr<dynamicgraph::SignalBase<int> > signal,
+		       const T& data)
+  {
+    typedef typename SotToRos<T>::sot_t 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>::signal_t signal_t;
+    typedef typename SotToRos<T>::callback_t callback_t;
+
+    // Initialize the bindedSignal object.
+    bindedSignal_t bindedSignal;
+
+    // Initialize the signal.
+    boost::format signalName ("RosExport(%1%)::%2%");
+    signalName % name % signal;
+
+    bindedSignal.first = boost::make_shared<signal_t>(0, signalName.str ());
+    signalRegistration (*bindedSignal.first);
+
+    // Initialize the publisher.
+    bindedSignal.second =
+      boost::make_shared<ros::Publisher>
+      (nh_.subscribe
+       (topic, 1, boost::bind (&RosExport::callback<T>,
+			       this, bindedSignal.first)));
+
+    bindedSignal_[signal] = bindedSignal;
+  }
 } // end of namespace dynamicgraph.
 
 #endif //! DYNAMIC_GRAPH_ROS_EXPORT_HXX

src/ros_import.hh

@@ -10,14 +10,11 @@
 
 # include <ros/ros.h>
 
+# include "converter.hh"
+# include "sot_to_ros.hh"
+
 namespace dynamicgraph
 {
-  template <typename SotType>
-  class SotToRos;
-
-  template <typename D, typename S>
-  void converter (D& dst, const S& src);
-
   class RosImport : public dynamicgraph::Entity
   {
   public:

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;
+# include "sot_to_ros.hh"
 
 namespace dynamicgraph
 {
-  template <>
-  struct SotToRos<double>
-  {
-    typedef double sot_t;
-    typedef std_msgs::Float64 ros_t;
-    typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t;
-    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)
-  {
-    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,

src/sot_to_ros.hh

+#ifndef DYNAMIC_GRAPH_ROS_SOT_TO_ROS_HH
+# define DYNAMIC_GRAPH_ROS_SOT_TO_ROS_HH
+# include <vector>
+# include <jrl/mal/boost.hh>
+# include <std_msgs/Float64.h>
+# include "dynamic_graph/Matrix.h"
+# include "dynamic_graph/Vector.h"
+
+namespace dynamicgraph
+{
+  namespace ml = maal::boost;
+
+  template <typename SotType>
+  class SotToRos;
+
+  template <>
+  struct SotToRos<double>
+  {
+    typedef double sot_t;
+    typedef std_msgs::Float64 ros_t;
+    typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t;
+    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;
+  };
+} // end of namespace dynamicgraph.
+
+#endif //! DYNAMIC_GRAPH_ROS_SOT_TO_ROS_HH