Fix Ros import class.

CMakeLists.txt

@@ -36,6 +36,7 @@ pkg_check_modules(DYNAMIC_GRAPH REQUIRED dynamic-graph)
 pkg_check_modules(SOT_CORE REQUIRED sot-core)
 
 rosbuild_add_library(ros_import
+  src/sot_to_ros.cpp
   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}
   ${SOT_CORE_CFLAGS})
@@ -45,6 +46,7 @@ target_link_libraries(ros_import
   ${JRL_MAL_LIBRARIES} ${DYNAMIC_GRAPH_LIBRARIES} ${SOT_CORE_LIBRARIES})
 
 rosbuild_add_library(ros_export
+  src/sot_to_ros.cpp
   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}
   ${SOT_CORE_CFLAGS})

src/ros_import.cpp

@@ -2,6 +2,7 @@
 
 #include <boost/assign.hpp>
 #include <boost/bind.hpp>
+#include <boost/foreach.hpp>
 #include <boost/format.hpp>
 #include <boost/function.hpp>
 #include <boost/make_shared.hpp>
@@ -119,8 +120,14 @@ namespace dynamicgraph
   RosImport::RosImport (const std::string& n)
     : dynamicgraph::Entity(n),
       nh_ (rosInit ()),
-      bindedSignal_ ()
+      bindedSignal_ (),
+      trigger_ (boost::bind (&RosImport::trigger, this, _1, _2),
+		sotNOSIGNAL,
+		MAKE_SIGNAL_STRING(name, true, "int", "trigger"))
   {
+    signalRegistration (trigger_);
+    trigger_.setNeedUpdateFromAllChildren (true);
+
     std::string docstring;
     addCommand ("add",
 		new command::rosImport::Add
@@ -160,4 +167,16 @@ namespace dynamicgraph
     bindedSignal_.clear ();
   }
 
+  int& RosImport::trigger (int& dummy, int t)
+  {
+    typedef std::map<std::string, bindedSignal_t>::iterator iterator_t;
+
+    for (iterator_t it = bindedSignal_.begin ();
+	 it != bindedSignal_.end (); ++it)
+      {
+	boost::get<2>(it->second) (t);
+      }
+    return dummy;
+  }
+
 } // end of namespace dynamicgraph.

src/ros_import.hh

@@ -4,6 +4,7 @@
 # include <map>
 
 # include <boost/shared_ptr.hpp>
+# include <boost/tuple/tuple.hpp>
 
 # include <dynamic-graph/entity.h>
 # include <dynamic-graph/signal-time-dependent.h>
@@ -50,9 +51,13 @@ namespace dynamicgraph
   {
     DYNAMIC_GRAPH_ENTITY_DECL();
   public:
-    typedef std::pair<boost::shared_ptr<dynamicgraph::SignalBase<int> >,
-		      boost::shared_ptr<ros::Publisher> >
-      bindedSignal_t;
+    typedef boost::function<void (int)> callback_t;
+
+    typedef boost::tuple<
+      boost::shared_ptr<dynamicgraph::SignalBase<int> >,
+      boost::shared_ptr<ros::Publisher>,
+      callback_t>
+    bindedSignal_t;
 
     RosImport (const std::string& n);
     virtual ~RosImport ();
@@ -64,9 +69,12 @@ namespace dynamicgraph
     void list ();
     void clear ();
 
+    int& trigger (int&, int);
+
     template <typename T>
-    T& sendData (boost::shared_ptr<ros::Publisher> publisher,
-		 T& data, int time);
+    void sendData (boost::shared_ptr<ros::Publisher> publisher,
+		   boost::shared_ptr<typename SotToRos<T>::signal_t> signal,
+		   int time);
 
     template <typename T>
     void add (const std::string& signal, const std::string& topic);
@@ -74,6 +82,7 @@ namespace dynamicgraph
   private:
     ros::NodeHandle nh_;
     std::map<std::string, bindedSignal_t> bindedSignal_;
+    dynamicgraph::SignalTimeDependent<int,int> trigger_;
   };
 } // end of namespace dynamicgraph.
 

src/ros_import.hxx

@@ -12,14 +12,14 @@
 namespace dynamicgraph
 {
   template <typename T>
-  T&
+  void
   RosImport::sendData (boost::shared_ptr<ros::Publisher> publisher,
-		       T& data, int time)
+		       boost::shared_ptr<typename SotToRos<T>::signal_t> signal,
+		       int time)
   {
     typename SotToRos<T>::ros_t result;
-    converter (result, data);
+    converter (result, signal->access (time));
     publisher->publish (result);
-    return data;
   }
 
   template <typename T>
@@ -28,27 +28,29 @@ namespace dynamicgraph
     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 publisher.
-    bindedSignal.second =
+    boost::get<1> (bindedSignal) =
       boost::make_shared<ros::Publisher> (nh_.advertise<ros_t>(topic, 1));
 
     // Initialize the signal.
-    boost::format signalName ("RosImport(%1%)::%2%");
-    signalName % name % signal;
-
-    callback_t signalCallback = boost::bind
-      (&RosImport::sendData<sot_t>, this, bindedSignal.second, _1, _2);
-
-    boost::shared_ptr<signal_t> signalPtr = boost::make_shared<signal_t>
-      (signalCallback, sotNOSIGNAL, signalName.str ());
-    signalPtr->setNeedUpdateFromAllChildren (true);
-    bindedSignal.first = signalPtr;
-    signalRegistration (*bindedSignal.first);
+    boost::shared_ptr<signal_t> signalPtr =
+      boost::make_shared<signal_t>
+      (MAKE_SIGNAL_STRING(name, true, SotToRos<T>::signalTypeName, signal));
+    boost::get<0> (bindedSignal) = signalPtr;
+    signalRegistration (*boost::get<0> (bindedSignal));
+
+    // Initialize the callback.
+    callback_t callback = boost::bind
+      (&RosImport::sendData<T>,
+       this,
+       boost::get<1> (bindedSignal),
+       signalPtr,
+       _1);
+    boost::get<2> (bindedSignal) = callback;
 
     bindedSignal_[signal] = bindedSignal;
   }

src/sot_to_ros.cpp

+#include "sot_to_ros.hh"
+
+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<sot::MatrixHomogeneous>::signalTypeName = "MatrixHomo";
+  const char* SotToRos
+  <std::pair<sot::MatrixHomogeneous, ml::Vector> >::signalTypeName
+  = "MatrixHomo";
+
+} // end of namespace dynamicgraph.

src/sot_to_ros.hh

@@ -3,6 +3,8 @@
 # include <vector>
 # include <utility>
 
+# include <boost/format.hpp>
+
 # include <jrl/mal/boost.hh>
 # include <std_msgs/Float64.h>
 # include "dynamic_graph_bridge/Matrix.h"
@@ -15,6 +17,10 @@
 # include <dynamic-graph/signal-ptr.h>
 # include <sot/core/matrix-homogeneous.hh>
 
+# define MAKE_SIGNAL_STRING(NAME, IS_INPUT, OUTPUT_TYPE, SIGNAL_NAME)	\
+  makeSignalString (typeid (*this).name (), NAME,			\
+		    IS_INPUT, OUTPUT_TYPE, SIGNAL_NAME)
+
 namespace dynamicgraph
 {
   namespace ml = maal::boost;
@@ -34,9 +40,11 @@ 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 dynamicgraph::Signal<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 <>
@@ -48,6 +56,8 @@ namespace dynamicgraph
     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 <>
@@ -59,6 +69,8 @@ namespace dynamicgraph
     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 <>
@@ -70,6 +82,8 @@ namespace dynamicgraph
     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;
   };
 
   // Stamped transformation
@@ -82,8 +96,25 @@ namespace dynamicgraph
     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;
   };
 
+  inline std::string
+  makeSignalString (const std::string& className,
+		    const std::string& instanceName,
+		    bool isInputSignal,
+		    const std::string& signalType,
+		    const std::string& signalName)
+  {
+    boost::format fmt ("%s(%s)::%s(%s)::%s");
+    fmt % className
+      % instanceName
+      % (isInputSignal ? "input" : "output")
+      % signalType
+      % signalName;
+    return fmt.str ();
+  }
 } // end of namespace dynamicgraph.
 
 #endif //! DYNAMIC_GRAPH_ROS_SOT_TO_ROS_HH

tests/test_import.py

+#!/usr/bin/python
+
+from dynamic_graph.ros import RosImport
+
+ri = RosImport('rosimport')
+
+ri.add('double', 'doubleS', 'doubleT')
+ri.add('vector', 'vectorS', 'vectorT')
+ri.add('matrix', 'matrixS', 'matrixT')
+
+ri.doubleS.value = 42.
+ri.vectorS.value = (42., 42.,)
+ri.matrixS.value = ((42., 42.,),(42., 42.,),)
+
+ri.trigger.recompute(ri.trigger.time + 1)