Start implementing dynamic-graph signal export into ROS.

.gitignore

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

CMakeLists.txt

@@ -28,3 +28,8 @@ set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
 #rosbuild_link_boost(${PROJECT_NAME} thread)
 #rosbuild_add_executable(example examples/example.cpp)
 #target_link_libraries(example ${PROJECT_NAME})
+
+rosbuild_add_library(ros_import src/ros_import.cpp)
+target_link_libraries(ros_import dynamic-graph)
+
+# rosbuild_add_library(sot_to_ros src/sot_to_ros.cpp)

manifest.xml

 <package>
   <description brief="dynamic_graph">
 
-     dynamic_graph
+     Dynamic graph bindings.
 
   </description>
   <author>Thomas Moulard</author>
@@ -9,6 +9,8 @@
   <review status="unreviewed" notes=""/>
   <url>http://ros.org/wiki/dynamic_graph</url>
 
-</package>
-
+  <depend package="std_msgs"/>
+  <depend package="roscpp"/>
 
+  <rosdep name="dynamic-graph"/>
+</package>

src/ros_import.cpp

+#include <boost/bind.hpp>
+#include <boost/format.hpp>
+#include <boost/function.hpp>
+#include <boost/make_shared.hpp>
+
+#include <ros/ros.h>
+#include <std_msgs/Float64.h>
+
+#include <dynamic-graph/factory.h>
+
+#include "ros_import.hh"
+
+namespace dynamicgraph
+{
+  DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(RosImport, "RosImport");
+
+  RosImport::RosImport (const std::string& n)
+    : dynamicgraph::Entity(n),
+      nh_ (),
+      bindedSignal_ ()
+  {
+    int argc = 1;
+    char* arg0 = strdup("ros_import");
+    char* argv[] = {arg0, 0};
+    ros::init(argc, argv, "ros_import");
+    free (arg0);
+
+    nh_ = ros::NodeHandle ("dynamic_graph");
+
+    //ros::Publisher blob_pub = n.advertise<hueblob::Blob>("blob", 1000);
+
+    // ros::Rate loop_rate(10);
+
+    // while (ros::ok())
+    //   {
+    // 	hueblob::Blob blob;
+    // 	blob_pub.publish(blob);
+
+    // 	ros::spinOnce();
+    // 	loop_rate.sleep();
+    //   }
+  }
+
+  RosImport::~RosImport ()
+  {
+  }
+
+  void RosImport::display (std::ostream& os) const
+  {
+    os << CLASS_NAME << std::endl;
+  }
+
+  void RosImport::commandLine (const std::string& cmdLine,
+			       std::istringstream& cmdArgs,
+			       std::ostream& os)
+  {
+    std::string signal;
+    std::string topic;
+
+    if (cmdLine == "help")
+      {
+	os << "RosImport: "<< std::endl
+	   << "  - add <SIGNAL> <TOPIC>" << std::endl;
+	Entity::commandLine (cmdLine, cmdArgs, os);
+      }
+    else if (cmdLine == "add")
+      {
+	cmdArgs >> signal >> topic;
+	add<double> (signal, topic);
+      }
+    else if (cmdLine == "rm")
+      {
+	cmdArgs >> signal >> topic;
+	rm (signal, topic);
+      }
+    else if (cmdLine == "clear")
+      clear ();
+    else if (cmdLine == "list")
+      list ();
+    else
+      Entity::commandLine (cmdLine, cmdArgs, os);
+  }
+
+  const std::string& RosImport::getClassName ()
+  {
+    return CLASS_NAME;
+  }
+
+  void RosImport::rm (const std::string& signal, const std::string& topic)
+  {
+  }
+
+  void RosImport::list ()
+  {
+    std::cout << CLASS_NAME << std::endl;
+  }
+
+  void RosImport::clear ()
+  {
+    bindedSignal_.clear ();
+  }
+
+} // end of namespace dynamicgraph.

src/ros_import.hh

+#ifndef DYNAMIC_GRAPH_ROS_IMPORT_HH
+# define DYNAMIC_GRAPH_ROS_IMPORT_HH
+# include <iostream>
+
+# include <boost/shared_ptr.hpp>
+
+# include <dynamic-graph/entity.h>
+# include <dynamic-graph/signal-time-dependent.h>
+
+# include <ros/ros.h>
+
+
+//FIXME: remove me.
+#include <std_msgs/Float64.h>
+
+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;
+};
+
+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;
+  }
+
+  class RosImport : public dynamicgraph::Entity
+  {
+  public:
+    typedef std::pair<boost::shared_ptr<dynamicgraph::SignalBase<int> >,
+		      boost::shared_ptr<ros::Publisher> >
+      bindedSignal_t;
+
+    static const std::string CLASS_NAME;
+
+    RosImport (const std::string& n);
+    virtual ~RosImport ();
+
+    void display (std::ostream& os) const;
+    virtual void
+    commandLine (const std::string& cmdLine,
+		 std::istringstream& cmdArgs,
+		 std::ostream& os);
+
+
+    virtual const std::string& getClassName();
+
+  private:
+    void add (const std::string& signal, const std::string& topic);
+    void rm (const std::string& signal, const std::string& topic);
+    void list ();
+    void clear ();
+
+    template <typename T>
+    T& sendData (boost::shared_ptr<ros::Publisher> publisher, T& data, int time)
+    {
+      typename SotToRos<T>::ros_t result;
+      converter (result, data);
+      publisher->publish (result);
+      return data;
+    }
+
+    template <typename T>
+    void 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 publisher.
+      bindedSignal.second =
+	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);
+
+      bindedSignal.first = boost::make_shared<signal_t>
+	(signalCallback, sotNOSIGNAL, signalName.str ());
+      signalRegistration (*bindedSignal.first);
+
+      bindedSignal_.push_back(bindedSignal);
+    }
+
+
+    ros::NodeHandle nh_;
+
+    std::vector<bindedSignal_t> bindedSignal_;
+  };
+} // end of namespace dynamicgraph.
+
+#endif //! DYNAMIC_GRAPH_ROS_IMPORT_HH