Add Ros joint state entity.

CMakeLists.txt

@@ -55,9 +55,20 @@ rosbuild_add_link_flags(ros_export ${JRL_MAL_LDFLAGS} ${DYNAMIC_GRAPH_LDFLAGS}
 target_link_libraries(ros_export
   ${JRL_MAL_LIBRARIES} ${DYNAMIC_GRAPH_LIBRARIES} ${SOT_CORE_LIBRARIES})
 
+rosbuild_add_library(ros_joint_state
+  src/ros_joint_state.cpp src/ros_joint_state.hh src/converter.hh src/sot_to_ros.hh)
+rosbuild_add_compile_flags(ros_joint_state ${JRL_MAL_CFLAGS} ${DYNAMIC_GRAPH_CFLAGS}
+  ${SOT_CORE_CFLAGS})
+rosbuild_add_link_flags(ros_joint_state ${JRL_MAL_LDFLAGS} ${DYNAMIC_GRAPH_LDFLAGS}
+  ${SOT_CORE_LDFLAGS})
+target_link_libraries(ros_joint_state
+  ${JRL_MAL_LIBRARIES} ${DYNAMIC_GRAPH_LIBRARIES} ${SOT_CORE_LIBRARIES})
+
+
 
 INSTALL(TARGETS ros_import DESTINATION lib)
 INSTALL(TARGETS ros_export DESTINATION lib)
+INSTALL(TARGETS ros_joint_state DESTINATION lib)
 
 INCLUDE(cmake/python.cmake)
 
@@ -88,4 +99,16 @@ SET_TARGET_PROPERTIES(dynamic_graph/ros/ros_export/wrap
   ${JRL_MAL_LDFLAGS} ${DYNAMIC_GRAPH_LDFLAGS} ${SOT_CORE_LDFLAGS}
   )
 
+DYNAMIC_GRAPH_PYTHON_MODULE("ros/ros_joint_state"
+  ros_joint_state
+  ros/ros_joint_state/wrap
+  )
+SET_TARGET_PROPERTIES(dynamic_graph/ros/ros_joint_state/wrap
+  PROPERTIES
+  COMPILE_FLAGS
+  ${JRL_MAL_CFLAGS} ${DYNAMIC_GRAPH_CFLAGS} ${SOT_CORE_CFLAGS}
+  LINK_FLAGS
+  ${JRL_MAL_LDFLAGS} ${DYNAMIC_GRAPH_LDFLAGS} ${SOT_CORE_LDFLAGS}
+  )
+
 ADD_SUBDIRECTORY(src)

manifest.xml

@@ -12,5 +12,6 @@
   <depend package="std_msgs"/>
   <depend package="roscpp"/>
   <depend package="geometry_msgs"/>
+  <depend package="sensor_msgs"/>
   <depend package="bullet"/>
 </package>

src/CMakeLists.txt

@@ -5,3 +5,4 @@ EXEC_PROGRAM(
   OUTPUT_VARIABLE PYTHON_SITELIB
   )
 PYTHON_INSTALL("dynamic_graph/ros" "__init__.py" "${PYTHON_SITELIB}")
+PYTHON_INSTALL("dynamic_graph/ros" "ros.py" "${PYTHON_SITELIB}")
\ No newline at end of file

src/dynamic_graph/ros/__init__.py

 from ros_import import RosImport
 from ros_export import RosExport
+from ros_joint_state import RosJointState
+
+from ros import Ros

src/dynamic_graph/ros/ros.py

+from ros_import import RosImport
+from ros_export import RosExport
+from ros_joint_state import RosJointState
+
+from dynamic_graph import plug
+
+class Ros(object):
+    device = None
+    rosImport = None
+    rosExport = None
+    rosJointState = None
+
+    def __init__(self, device, suffix = ''):
+        self.device = device
+        self.rosImport = RosImport('rosImport{0}'.format(suffix))
+        self.rosExport = RosExport('rosExport{0}'.format(suffix))
+        self.rosJointState = RosJointState('rosJointState{0}'.format(suffix))
+
+        plug(self.device.state, self.rosJointState.state)
+        self.device.after.addSignal('{0}.trigger'.format(self.rosImport.name))
+        self.device.after.addSignal('{0}.trigger'.format(self.rosJointState.name))

src/ros_joint_state.cpp

+#include <boost/bind.hpp>
+#include <boost/format.hpp>
+
+#include <dynamic-graph/factory.h>
+
+#include "ros_joint_state.hh"
+#include "sot_to_ros.hh"
+
+namespace dynamicgraph
+{
+  DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(RosJointState, "RosJointState");
+
+  static const char* rosInit()
+  {
+    int argc = 1;
+    char* arg0 = strdup("ros_joint_state");
+    char* argv[] = {arg0, 0};
+    ros::init(argc, argv, "ros_joint_state");
+    free (arg0);
+    return "dynamic_graph";
+  }
+
+  RosJointState::RosJointState (const std::string& n)
+    : Entity (n),
+      nh_ (rosInit ()),
+      state_ (0, MAKE_SIGNAL_STRING(name, true, "Vector", "state")),
+      publisher_ (nh_.advertise<sensor_msgs::JointState>("jointState", 5)),
+      jointState_ (),
+      trigger_ (boost::bind (&RosJointState::trigger, this, _1, _2),
+		sotNOSIGNAL,
+		MAKE_SIGNAL_STRING(name, true, "int", "trigger"))
+  {
+    signalRegistration (state_ << trigger_);
+    trigger_.setNeedUpdateFromAllChildren (true);
+
+    // Fill header.
+    jointState_.header.seq = 0;
+    jointState_.header.stamp.sec = 0;
+    jointState_.header.stamp.nsec = 0;
+    jointState_.header.frame_id = "odom";
+  }
+
+  RosJointState::~RosJointState ()
+  {}
+
+  int&
+  RosJointState::trigger (int& dummy, int t)
+  {
+    std::size_t s = state_.access (t).size ();
+
+    // Update header.
+    ++jointState_.header.seq;
+    jointState_.header.stamp.sec = 0;
+    jointState_.header.stamp.nsec = 0;
+
+    // Fill names if needed.
+    if (jointState_.name.size () != s)
+      {
+	boost::format fmtFreeFlyer ("free-flyer-%s");
+	boost::format fmtJoint ("joint-%s");
+	jointState_.name.resize (s);
+	for (std::size_t i = 0; i < s; ++i)
+	  if (i < 6)
+	    jointState_.name[i] = (fmtFreeFlyer % i).str ();
+	  else
+	    jointState_.name[i] = (fmtJoint % (i - 6)).str ();
+      }
+
+    // Fill position.
+    jointState_.position.resize (s);
+    for (std::size_t i = 0; i < s; ++i)
+      jointState_.position[i] = state_.access (t) (i);
+
+    publisher_.publish (jointState_);
+    return dummy;
+  }
+} // end of namespace dynamicgraph.

src/ros_joint_state.hh

+#ifndef DYNAMIC_GRAPH_JOINT_STATE_HH
+# define DYNAMIC_GRAPH_JOINT_STATE_HH
+# include <dynamic-graph/entity.h>
+# include <dynamic-graph/signal-ptr.h>
+# include <dynamic-graph/signal-time-dependent.h>
+
+# include <ros/ros.h>
+# include <sensor_msgs/JointState.h>
+
+# include "converter.hh"
+# include "sot_to_ros.hh"
+
+namespace dynamicgraph
+{
+  /// \brief Publish current robot configuration to ROS.
+  class RosJointState : public dynamicgraph::Entity
+  {
+    DYNAMIC_GRAPH_ENTITY_DECL();
+  public:
+    /// \brief Vector input signal.
+    typedef SignalPtr<ml::Vector, int> signalVectorIn_t;
+
+    RosJointState (const std::string& n);
+    virtual ~RosJointState ();
+
+    int& trigger (int&, int);
+  private:
+    ros::NodeHandle nh_;
+    signalVectorIn_t state_;
+    ros::Publisher publisher_;
+    sensor_msgs::JointState jointState_;
+    dynamicgraph::SignalTimeDependent<int,int> trigger_;
+  };
+} // end of namespace dynamicgraph.
+
+#endif //! DYNAMIC_GRAPH_JOINT_STATE_HH

tests/test_joint_state.py

+#!/usr/bin/python
+
+try:
+    from dynamic_graph.sot.dynamics.tools import *
+except:
+    print("This test requires sot-dynamic.")
+    exit(42)
+
+from dynamic_graph.ros import RosJointState
+
+rjs = RosJointState('rosjointstate')
+plug(robot.device.state, rjs.state)
+rjs.trigger.recompute(rjs.trigger.time + 1)