diff --git a/CMakeLists.txt b/CMakeLists.txt
index a65358c16e9c39a942b79f106f3f01e2fc5cee35..0eb6ba3896601515a2b12d13f97a18cef43f902b 100644
--- a/CMakeLists.txt
+++ b/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)
diff --git a/manifest.xml b/manifest.xml
index 6d8b1d9ffcd971a2e8f15eaab4323b4ed204db1b..cacb368c39a72a6fed5d660d7ebacc652d9de780 100644
--- a/manifest.xml
+++ b/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>
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 6226a8e32aca7980b37a8f4e4cafd9aa7a75eea0..4ab2d6f86dfe08d96da2512e672172c5907cfd94 100644
--- a/src/CMakeLists.txt
+++ b/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
diff --git a/src/dynamic_graph/ros/__init__.py b/src/dynamic_graph/ros/__init__.py
index 58fcc1695fd6dc95196b10e1871cccd2bbd1d2cc..99c5f33b67d7a87d7137420b9bb41b4701a3e8b5 100644
--- a/src/dynamic_graph/ros/__init__.py
+++ b/src/dynamic_graph/ros/__init__.py
@@ -1,2 +1,5 @@
from ros_import import RosImport
from ros_export import RosExport
+from ros_joint_state import RosJointState
+
+from ros import Ros
diff --git a/src/dynamic_graph/ros/ros.py b/src/dynamic_graph/ros/ros.py
new file mode 100644
index 0000000000000000000000000000000000000000..d6b5ba39e4171c5dd312190b1f16bf1f0c014237
--- /dev/null
+++ b/src/dynamic_graph/ros/ros.py
@@ -0,0 +1,21 @@
+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))
diff --git a/src/ros_joint_state.cpp b/src/ros_joint_state.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5f2af2ca836e62d71c3f91200632810dd774aae2
--- /dev/null
+++ b/src/ros_joint_state.cpp
@@ -0,0 +1,77 @@
+#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.
diff --git a/src/ros_joint_state.hh b/src/ros_joint_state.hh
new file mode 100644
index 0000000000000000000000000000000000000000..402081cb652b305784d6ba4fcb6509e0366b8943
--- /dev/null
+++ b/src/ros_joint_state.hh
@@ -0,0 +1,36 @@
+#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
diff --git a/tests/test_joint_state.py b/tests/test_joint_state.py
new file mode 100755
index 0000000000000000000000000000000000000000..48196400937dc36048eb10d7e3d36694d38b4499
--- /dev/null
+++ b/tests/test_joint_state.py
@@ -0,0 +1,13 @@
+#!/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)