setup.cfg

+[flake8]
+exclude = cmake
+max-line-length = 88
+extend-ignore = E203

src/CMakeLists.txt

-PYTHON_INSTALL("dynamic_graph/ros" "__init__.py" "${PYTHON_SITELIB}")
-PYTHON_INSTALL("dynamic_graph/ros" "ros.py" "${PYTHON_SITELIB}")
\ No newline at end of file
+set(plugins ros_publish ros_subscribe ros_queued_subscribe ros_tf_listener
+            ros_time)
+
+foreach(plugin ${plugins})
+  get_filename_component(LIBRARY_NAME ${plugin} NAME)
+  add_library(${LIBRARY_NAME} SHARED ${plugin}.cpp ${plugin}.hh)
+
+  if(SUFFIX_SO_VERSION)
+    set_target_properties(${LIBRARY_NAME} PROPERTIES SOVERSION
+                                                     ${PROJECT_VERSION})
+  endif(SUFFIX_SO_VERSION)
+
+  target_link_libraries(${LIBRARY_NAME} ${${LIBRARY_NAME}_deps}
+                        ${catkin_LIBRARIES} ros_bridge)
+
+  if(NOT INSTALL_PYTHON_INTERFACE_ONLY)
+    install(
+      TARGETS ${LIBRARY_NAME}
+      EXPORT ${TARGETS_EXPORT_NAME}
+      DESTINATION ${DYNAMIC_GRAPH_PLUGINDIR})
+  endif(NOT INSTALL_PYTHON_INTERFACE_ONLY)
+
+  if(BUILD_PYTHON_INTERFACE)
+    string(REPLACE - _ PYTHON_LIBRARY_NAME ${LIBRARY_NAME})
+    if(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/${plugin}-python-module-py.cc")
+      dynamic_graph_python_module(
+        "ros/${PYTHON_LIBRARY_NAME}" ${LIBRARY_NAME}
+        ${PROJECT_NAME}-${PYTHON_LIBRARY_NAME}-wrap SOURCE_PYTHON_MODULE
+        "${CMAKE_CURRENT_SOURCE_DIR}/${plugin}-python-module-py.cc")
+    elseif(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/${plugin}-python.hh")
+      dynamic_graph_python_module(
+        "ros/${PYTHON_LIBRARY_NAME}" ${LIBRARY_NAME}
+        ${PROJECT_NAME}-${PYTHON_LIBRARY_NAME}-wrap MODULE_HEADER
+        "${CMAKE_CURRENT_SOURCE_DIR}/${plugin}-python.hh")
+    endif()
+  endif(BUILD_PYTHON_INTERFACE)
+endforeach(plugin)
+
+target_link_libraries(ros_publish ros_bridge)
+
+if(BUILD_PYTHON_INTERFACE)
+  python_install_on_site("dynamic_graph/ros" "__init__.py")
+  python_install_on_site("dynamic_graph/ros" "ros.py")
+  python_install_on_site("dynamic_graph/ros" "dgcompleter.py")
+
+  # ros_interperter library.
+  add_library(ros_interpreter ros_interpreter.cpp)
+  target_link_libraries(ros_interpreter ros_bridge ${catkin_LIBRARIES}
+                        dynamic-graph-python::dynamic-graph-python)
+
+  install(
+    TARGETS ros_interpreter
+    EXPORT ${TARGETS_EXPORT_NAME}
+    DESTINATION lib)
+endif(BUILD_PYTHON_INTERFACE)
+
+# Stand alone embedded intepreter with a robot controller.
+add_executable(geometric_simu geometric_simu.cpp sot_loader.cpp
+                              sot_loader_basic.cpp)
+target_link_libraries(geometric_simu Boost::program_options ${CMAKE_DL_LIBS}
+                      ${catkin_LIBRARIES} ros_bridge)
+install(TARGETS geometric_simu DESTINATION lib/${PROJECT_NAME})
+
+# Sot loader library
+add_library(sot_loader sot_loader.cpp sot_loader_basic.cpp)
+target_link_libraries(sot_loader Boost::program_options ${catkin_LIBRARIES}
+                      ros_bridge)
+install(
+  TARGETS sot_loader
+  EXPORT ${TARGETS_EXPORT_NAME}
+  DESTINATION lib)

src/converter.hh

 #ifndef DYNAMIC_GRAPH_ROS_CONVERTER_HH
-# define DYNAMIC_GRAPH_ROS_CONVERTER_HH
-# include <stdexcept>
-# include "sot_to_ros.hh"
-
-# include <boost/static_assert.hpp>
-# include <boost/date_time/date.hpp>
-# include <boost/date_time/posix_time/posix_time.hpp>
-
-# include <ros/time.h>
-# include <std_msgs/Header.h>
-
-# include <LinearMath/btMatrix3x3.h>
-# include <LinearMath/btQuaternion.h>
-
-# define SOT_TO_ROS_IMPL(T)						\
-  template <>								\
-  inline void								\
-  converter (SotToRos<T>::ros_t& dst, const SotToRos<T>::sot_t& src)
-
-# define ROS_TO_SOT_IMPL(T)						\
-  template <>								\
-  inline void								\
-  converter (SotToRos<T>::sot_t& dst, const SotToRos<T>::ros_t& src)
-
-namespace dynamicgraph
-{
-  inline
-  void
-  makeHeader(std_msgs::Header& header)
-  {
-    header.seq = 0;
-    header.stamp = ros::Time::now ();
-    header.frame_id = "/dynamic_graph/world";
+#define DYNAMIC_GRAPH_ROS_CONVERTER_HH
+#include <ros/time.h>
+#include <std_msgs/Header.h>
+
+#include <boost/date_time/date.hpp>
+#include <boost/date_time/posix_time/posix_time.hpp>
+#include <boost/static_assert.hpp>
+#include <stdexcept>
+
+#include "sot_to_ros.hh"
+
+#define SOT_TO_ROS_IMPL(T)                       \
+  template <>                                    \
+  inline void converter(SotToRos<T>::ros_t& dst, \
+                        const SotToRos<T>::sot_t& src)
+
+#define ROS_TO_SOT_IMPL(T)                       \
+  template <>                                    \
+  inline void converter(SotToRos<T>::sot_t& dst, \
+                        const SotToRos<T>::ros_t& src)
+
+namespace dynamicgraph {
+inline void makeHeader(std_msgs::Header& header) {
+  header.seq = 0;
+  header.stamp = ros::Time::now();
+  header.frame_id = "/dynamic_graph/world";
+}
+
+/// \brief Handle ROS <-> dynamic-graph conversion.
+///
+/// Implements all ROS/dynamic-graph conversions required by the
+/// bridge.
+///
+/// Relies on the SotToRos type trait to make sure valid pair of
+/// types are used.
+template <typename D, typename S>
+void converter(D& dst, const S& src);
+
+// Boolean
+SOT_TO_ROS_IMPL(bool) { dst.data = src; }
+
+ROS_TO_SOT_IMPL(bool) { dst = src.data; }
+
+// Double
+SOT_TO_ROS_IMPL(double) { dst.data = src; }
+
+ROS_TO_SOT_IMPL(double) { dst = src.data; }
+
+// Int
+SOT_TO_ROS_IMPL(int) { dst.data = src; }
+
+ROS_TO_SOT_IMPL(int) { dst = src.data; }
+
+// Unsigned
+SOT_TO_ROS_IMPL(unsigned int) { dst.data = src; }
+
+ROS_TO_SOT_IMPL(unsigned int) { dst = src.data; }
+
+// String
+SOT_TO_ROS_IMPL(std::string) { dst.data = src; }
+
+ROS_TO_SOT_IMPL(std::string) { dst = src.data; }
+
+// Vector
+SOT_TO_ROS_IMPL(Vector) {
+  dst.data.resize(src.size());
+  for (int i = 0; i < src.size(); ++i) dst.data[i] = src(i);
+}
+
+ROS_TO_SOT_IMPL(Vector) {
+  dst.resize(src.data.size());
+  for (unsigned int i = 0; i < src.data.size(); ++i) dst(i) = src.data[i];
+}
+
+// Vector3
+SOT_TO_ROS_IMPL(specific::Vector3) {
+  if (src.size() > 0) {
+    dst.x = src(0);
+    if (src.size() > 1) {
+      dst.y = src(1);
+      if (src.size() > 2) dst.z = src(2);
+    }
   }
-
-  /// \brief Handle ROS <-> dynamic-graph conversion.
-  ///
-  /// Implements all ROS/dynamic-graph conversions required by the
-  /// bridge.
-  ///
-  ///Relies on the SotToRos type trait to make sure valid pair of
-  /// types are used.
-  template <typename D, typename S>
-  void converter (D& dst, const S& src);
-
-  // Double
-  SOT_TO_ROS_IMPL(double)
-  {
-    dst.data = src;
-  }
-
-  ROS_TO_SOT_IMPL(double)
-  {
-    dst = src.data;
-  }
-
-  // Unsigned
-  SOT_TO_ROS_IMPL(unsigned int)
-  {
-    dst.data = src;
-  }
-
-  ROS_TO_SOT_IMPL(unsigned int)
-  {
-    dst = src.data;
-  }
-
-  // Vector
-  SOT_TO_ROS_IMPL(ml::Vector)
-  {
-    dst.data.resize (src.size ());
-    for (unsigned i = 0; i < src.size (); ++i)
-      dst.data[i] =  src.elementAt (i);
-  }
-
-  ROS_TO_SOT_IMPL(ml::Vector)
-  {
-    dst.resize (src.data.size ());
-    for (unsigned i = 0; i < src.data.size (); ++i)
-      dst.elementAt (i) =  src.data[i];
-  }
-
-  // Vector3
-  SOT_TO_ROS_IMPL(specific::Vector3)
-  {
-    if (src.size () > 0)
-      {
-	dst.x =  src.elementAt (0);
-	if (src.size () > 1)
-	  {
-	    dst.y =  src.elementAt (1);
-	    if (src.size () > 2)
-	      dst.z =  src.elementAt (2);
-	  }
-      }
-  }
-
-  ROS_TO_SOT_IMPL(specific::Vector3)
-  {
-    dst.resize (3);
-    dst.elementAt (0) =  src.x;
-    dst.elementAt (1) =  src.y;
-    dst.elementAt (2) =  src.z;
-  }
-
-  // Matrix
-  SOT_TO_ROS_IMPL(ml::Matrix)
-  {
-    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);
-  }
-
-  ROS_TO_SOT_IMPL(ml::Matrix)
-  {
-    dst.resize (src.width, src.data.size () / src.width);
-    for (unsigned i = 0; i < src.data.size (); ++i)
-      dst.elementAt (i) =  src.data[i];
-  }
-
-  // Homogeneous matrix.
-  SOT_TO_ROS_IMPL(sot::MatrixHomogeneous)
-  {
-    btMatrix3x3 rotation;
-    btQuaternion quaternion;
-    for(unsigned i = 0; i < 3; ++i)
-      for(unsigned j = 0; j < 3; ++j)
-	rotation[i][j] = src (i, j);
-    rotation.getRotation (quaternion);
-
-    dst.translation.x = src (0, 3);
-    dst.translation.y = src (1, 3);
-    dst.translation.z = src (2, 3);
-
-    dst.rotation.x = quaternion.x ();
-    dst.rotation.y = quaternion.y ();
-    dst.rotation.z = quaternion.z ();
-    dst.rotation.w = quaternion.w ();
-  }
-
-  ROS_TO_SOT_IMPL(sot::MatrixHomogeneous)
-  {
-    btQuaternion quaternion
-      (src.rotation.x, src.rotation.y, src.rotation.z, src.rotation.w);
-    btMatrix3x3 rotation (quaternion);
-
-    // Copy the rotation component.
-    for(unsigned i = 0; i < 3; ++i)
-      for(unsigned j = 0; j < 3; ++j)
-	dst (i, j) = rotation[i][j];
-
-    // Copy the translation component.
-    dst(0, 3) = src.translation.x;
-    dst(1, 3) = src.translation.y;
-    dst(2, 3) = src.translation.z;
-  }
-
-
-  // Twist.
-  SOT_TO_ROS_IMPL(specific::Twist)
-  {
-    if (src.size () != 6)
-      throw std::runtime_error ("failed to convert invalid twist");
-    dst.linear.x = src (0);
-    dst.linear.y = src (1);
-    dst.linear.z = src (2);
-    dst.angular.x = src (3);
-    dst.angular.y = src (4);
-    dst.angular.z = src (5);
-  }
-
-  ROS_TO_SOT_IMPL(specific::Twist)
-  {
-    dst.resize (6);
-    dst (0) = src.linear.x;
-    dst (1) = src.linear.y;
-    dst (2) = src.linear.z;
-    dst (3) = src.angular.x;
-    dst (4) = src.angular.y;
-    dst (5) = src.angular.z;
-  }
-
-
-  /// \brief This macro generates a converter for a stamped type from
-  /// dynamic-graph to ROS.  I.e. A data associated with its
-  /// timestamp.
-# define DG_BRIDGE_TO_ROS_MAKE_STAMPED_IMPL(T, ATTRIBUTE, EXTRA)	\
-  template <>								\
-  inline void converter							\
-  (SotToRos<std::pair<T, ml::Vector> >::ros_t& dst,			\
-   const SotToRos<std::pair<T, ml::Vector> >::sot_t& src)		\
-  {									\
-    makeHeader(dst.header);						\
-    converter<SotToRos<T>::ros_t, SotToRos<T>::sot_t> (dst.ATTRIBUTE, src); \
-    do { EXTRA } while (0);						\
-  }									\
+}
+
+ROS_TO_SOT_IMPL(specific::Vector3) {
+  dst.resize(3);
+  dst(0) = src.x;
+  dst(1) = src.y;
+  dst(2) = src.z;
+}
+
+// Matrix
+SOT_TO_ROS_IMPL(Matrix) {
+  // TODO: Confirm Ros Matrix Storage order. It changes the RosMatrix to
+  // ColMajor.
+  dst.width = (unsigned int)src.rows();
+  dst.data.resize(src.cols() * src.rows());
+  for (int i = 0; i < src.cols() * src.rows(); ++i) dst.data[i] = src.data()[i];
+}
+
+ROS_TO_SOT_IMPL(Matrix) {
+  dst.resize(src.width,
+             (unsigned int)src.data.size() / (unsigned int)src.width);
+  for (unsigned i = 0; i < src.data.size(); ++i) dst.data()[i] = src.data[i];
+}
+
+// Homogeneous matrix.
+SOT_TO_ROS_IMPL(sot::MatrixHomogeneous) {
+  sot::VectorQuaternion q(src.linear());
+  dst.translation.x = src.translation()(0);
+  dst.translation.y = src.translation()(1);
+  dst.translation.z = src.translation()(2);
+
+  dst.rotation.x = q.x();
+  dst.rotation.y = q.y();
+  dst.rotation.z = q.z();
+  dst.rotation.w = q.w();
+}
+
+ROS_TO_SOT_IMPL(sot::MatrixHomogeneous) {
+  sot::VectorQuaternion q(src.rotation.w, src.rotation.x, src.rotation.y,
+                          src.rotation.z);
+  dst.linear() = q.matrix();
+
+  // Copy the translation component.
+  dst.translation()(0) = src.translation.x;
+  dst.translation()(1) = src.translation.y;
+  dst.translation()(2) = src.translation.z;
+}
+
+// Twist.
+SOT_TO_ROS_IMPL(specific::Twist) {
+  if (src.size() != 6)
+    throw std::runtime_error("failed to convert invalid twist");
+  dst.linear.x = src(0);
+  dst.linear.y = src(1);
+  dst.linear.z = src(2);
+  dst.angular.x = src(3);
+  dst.angular.y = src(4);
+  dst.angular.z = src(5);
+}
+
+ROS_TO_SOT_IMPL(specific::Twist) {
+  dst.resize(6);
+  dst(0) = src.linear.x;
+  dst(1) = src.linear.y;
+  dst(2) = src.linear.z;
+  dst(3) = src.angular.x;
+  dst(4) = src.angular.y;
+  dst(5) = src.angular.z;
+}
+
+/// \brief This macro generates a converter for a stamped type from
+/// dynamic-graph to ROS.  I.e. A data associated with its
+/// timestamp.
+#define DG_BRIDGE_TO_ROS_MAKE_STAMPED_IMPL(T, ATTRIBUTE, EXTRA)              \
+  template <>                                                                \
+  inline void converter(SotToRos<std::pair<T, Vector> >::ros_t& dst,         \
+                        const SotToRos<std::pair<T, Vector> >::sot_t& src) { \
+    makeHeader(dst.header);                                                  \
+    converter<SotToRos<T>::ros_t, SotToRos<T>::sot_t>(dst.ATTRIBUTE, src);   \
+    do {                                                                     \
+      EXTRA                                                                  \
+    } while (0);                                                             \
+  }                                                                          \
   struct e_n_d__w_i_t_h__s_e_m_i_c_o_l_o_n
 
-  DG_BRIDGE_TO_ROS_MAKE_STAMPED_IMPL(specific::Vector3, vector, ;);
-  DG_BRIDGE_TO_ROS_MAKE_STAMPED_IMPL(sot::MatrixHomogeneous, transform,
-				     dst.child_frame_id = "";);
-  DG_BRIDGE_TO_ROS_MAKE_STAMPED_IMPL(specific::Twist, twist, ;);
-
-
-  /// \brief This macro generates a converter for a shared pointer on
-  ///        a ROS type to a dynamic-graph type.
-  ///
-  ///        A converter for the underlying type is required.  I.e. to
-  ///        convert a shared_ptr<T> to T', a converter from T to T'
-  ///        is required.
-# define DG_BRIDGE_MAKE_SHPTR_IMPL(T)					\
-  template <>								\
-  inline void converter							\
-  (SotToRos<T>::sot_t& dst,						\
-   const boost::shared_ptr<SotToRos<T>::ros_t const>& src)		\
-  {									\
-    converter<SotToRos<T>::sot_t, SotToRos<T>::ros_t> (dst, *src);	\
-  }									\
+DG_BRIDGE_TO_ROS_MAKE_STAMPED_IMPL(specific::Vector3, vector, ;);
+DG_BRIDGE_TO_ROS_MAKE_STAMPED_IMPL(sot::MatrixHomogeneous, transform,
+                                   dst.child_frame_id = "";);
+DG_BRIDGE_TO_ROS_MAKE_STAMPED_IMPL(specific::Twist, twist, ;);
+
+/// \brief This macro generates a converter for a shared pointer on
+///        a ROS type to a dynamic-graph type.
+///
+///        A converter for the underlying type is required.  I.e. to
+///        convert a shared_ptr<T> to T', a converter from T to T'
+///        is required.
+#define DG_BRIDGE_MAKE_SHPTR_IMPL(T)                              \
+  template <>                                                     \
+  inline void converter(                                          \
+      SotToRos<T>::sot_t& dst,                                    \
+      const boost::shared_ptr<SotToRos<T>::ros_t const>& src) {   \
+    converter<SotToRos<T>::sot_t, SotToRos<T>::ros_t>(dst, *src); \
+  }                                                               \
   struct e_n_d__w_i_t_h__s_e_m_i_c_o_l_o_n
 
-  DG_BRIDGE_MAKE_SHPTR_IMPL(double);
-  DG_BRIDGE_MAKE_SHPTR_IMPL(unsigned int);
-  DG_BRIDGE_MAKE_SHPTR_IMPL(ml::Vector);
-  DG_BRIDGE_MAKE_SHPTR_IMPL(specific::Vector3);
-  DG_BRIDGE_MAKE_SHPTR_IMPL(ml::Matrix);
-  DG_BRIDGE_MAKE_SHPTR_IMPL(sot::MatrixHomogeneous);
-  DG_BRIDGE_MAKE_SHPTR_IMPL(specific::Twist);
-
-  /// \brief This macro generates a converter for a stamped type.
-  /// I.e. A data associated with its timestamp.
-  ///
-  /// FIXME: the timestamp is not yet forwarded to the dg signal.
-# define DG_BRIDGE_MAKE_STAMPED_IMPL(T, ATTRIBUTE, EXTRA)		\
-  template <>								\
-  inline void converter							\
-  (SotToRos<std::pair<T, ml::Vector> >::sot_t& dst,			\
-   const SotToRos<std::pair<T, ml::Vector> >::ros_t& src)		\
-  {									\
-    converter<SotToRos<T>::sot_t, SotToRos<T>::ros_t> (dst, src.ATTRIBUTE); \
-    do { EXTRA } while (0);						\
-  }									\
+DG_BRIDGE_MAKE_SHPTR_IMPL(bool);
+DG_BRIDGE_MAKE_SHPTR_IMPL(double);
+DG_BRIDGE_MAKE_SHPTR_IMPL(int);
+DG_BRIDGE_MAKE_SHPTR_IMPL(unsigned int);
+DG_BRIDGE_MAKE_SHPTR_IMPL(std::string);
+DG_BRIDGE_MAKE_SHPTR_IMPL(Vector);
+DG_BRIDGE_MAKE_SHPTR_IMPL(specific::Vector3);
+DG_BRIDGE_MAKE_SHPTR_IMPL(Matrix);
+DG_BRIDGE_MAKE_SHPTR_IMPL(sot::MatrixHomogeneous);
+DG_BRIDGE_MAKE_SHPTR_IMPL(specific::Twist);
+
+/// \brief This macro generates a converter for a stamped type.
+/// I.e. A data associated with its timestamp.
+///
+/// FIXME: the timestamp is not yet forwarded to the dg signal.
+#define DG_BRIDGE_MAKE_STAMPED_IMPL(T, ATTRIBUTE, EXTRA)                     \
+  template <>                                                                \
+  inline void converter(SotToRos<std::pair<T, Vector> >::sot_t& dst,         \
+                        const SotToRos<std::pair<T, Vector> >::ros_t& src) { \
+    converter<SotToRos<T>::sot_t, SotToRos<T>::ros_t>(dst, src.ATTRIBUTE);   \
+    do {                                                                     \
+      EXTRA                                                                  \
+    } while (0);                                                             \
+  }                                                                          \
   struct e_n_d__w_i_t_h__s_e_m_i_c_o_l_o_n
 
-  DG_BRIDGE_MAKE_STAMPED_IMPL(specific::Vector3, vector, ;);
-  DG_BRIDGE_MAKE_STAMPED_IMPL(sot::MatrixHomogeneous, transform, ;);
-  DG_BRIDGE_MAKE_STAMPED_IMPL(specific::Twist, twist, ;);
-
-  /// \brief This macro generates a converter for a shared pointer on
-  /// a stamped type.  I.e. A data associated with its timestamp.
-  ///
-  /// FIXME: the timestamp is not yet forwarded to the dg signal.
-# define DG_BRIDGE_MAKE_STAMPED_SHPTR_IMPL(T, ATTRIBUTE, EXTRA)		\
-  template <>								\
-  inline void converter							\
-  (SotToRos<std::pair<T, ml::Vector> >::sot_t& dst,			\
-   const boost::shared_ptr						\
-   <SotToRos<std::pair<T, ml::Vector> >::ros_t const>& src)		\
-  {									\
-    converter<SotToRos<T>::sot_t, SotToRos<T>::ros_t> (dst, src->ATTRIBUTE); \
-    do { EXTRA } while (0);						\
-  }									\
+DG_BRIDGE_MAKE_STAMPED_IMPL(specific::Vector3, vector, ;);
+DG_BRIDGE_MAKE_STAMPED_IMPL(sot::MatrixHomogeneous, transform, ;);
+DG_BRIDGE_MAKE_STAMPED_IMPL(specific::Twist, twist, ;);
+
+/// \brief This macro generates a converter for a shared pointer on
+/// a stamped type.  I.e. A data associated with its timestamp.
+///
+/// FIXME: the timestamp is not yet forwarded to the dg signal.
+#define DG_BRIDGE_MAKE_STAMPED_SHPTR_IMPL(T, ATTRIBUTE, EXTRA)               \
+  template <>                                                                \
+  inline void converter(                                                     \
+      SotToRos<std::pair<T, Vector> >::sot_t& dst,                           \
+      const boost::shared_ptr<SotToRos<std::pair<T, Vector> >::ros_t const>& \
+          src) {                                                             \
+    converter<SotToRos<T>::sot_t, SotToRos<T>::ros_t>(dst, src->ATTRIBUTE);  \
+    do {                                                                     \
+      EXTRA                                                                  \
+    } while (0);                                                             \
+  }                                                                          \
   struct e_n_d__w_i_t_h__s_e_m_i_c_o_l_o_n
 
-  DG_BRIDGE_MAKE_STAMPED_SHPTR_IMPL(specific::Vector3, vector, ;);
-  DG_BRIDGE_MAKE_STAMPED_SHPTR_IMPL(sot::MatrixHomogeneous, transform, ;);
-  DG_BRIDGE_MAKE_STAMPED_SHPTR_IMPL(specific::Twist, twist, ;);
-
-
-  /// \brief If an impossible/unimplemented conversion is required, fail.
-  ///
-  /// IMPORTANT, READ ME:
-  ///
-  /// If the compiler generates an error in the following function,
-  /// this is /normal/.
-  ///
-  /// This error means that either you try to use an undefined
-  /// conversion.  You can either fix your code or provide the wanted
-  /// conversion by updating this header.
-  template <typename U, typename V>
-  inline void converter (U& dst, V& src)
-  {
-    // This will always fail if instantiated.
-    BOOST_STATIC_ASSERT (sizeof (U) == 0);
-  }
-
-  typedef boost::posix_time::ptime ptime;
-  typedef boost::posix_time::seconds seconds;
-  typedef boost::posix_time::microseconds microseconds;
-  typedef boost::posix_time::time_duration time_duration;
-  typedef boost::gregorian::date date;
-
-  boost::posix_time::ptime rosTimeToPtime (const ros::Time& rosTime)
-  {
-    ptime time (date(1970,1,1),	seconds (rosTime.sec) +
-		microseconds (rosTime.nsec/1000));
-    return time;
-  }
-
-  ros::Time pTimeToRostime (const boost::posix_time::ptime& time)
-  {
-    static ptime timeStart(date(1970,1,1));
-    time_duration diff = time - timeStart;
-
-    uint32_t sec = diff.ticks ()/time_duration::rep_type::res_adjust ();
-    uint32_t nsec = diff.fractional_seconds();
-
-    return ros::Time (sec, nsec);
-  }
-} // end of namespace dynamicgraph.
-
-#endif //! DYNAMIC_GRAPH_ROS_CONVERTER_HH
+DG_BRIDGE_MAKE_STAMPED_SHPTR_IMPL(specific::Vector3, vector, ;);
+DG_BRIDGE_MAKE_STAMPED_SHPTR_IMPL(sot::MatrixHomogeneous, transform, ;);
+DG_BRIDGE_MAKE_STAMPED_SHPTR_IMPL(specific::Twist, twist, ;);
+
+/// \brief If an impossible/unimplemented conversion is required, fail.
+///
+/// IMPORTANT, READ ME:
+///
+/// If the compiler generates an error in the following function,
+/// this is /normal/.
+///
+/// This error means that either you try to use an undefined
+/// conversion.  You can either fix your code or provide the wanted
+/// conversion by updating this header.
+template <typename U, typename V>
+inline void converter(U& dst, V& src) {
+  // This will always fail if instantiated.
+  BOOST_STATIC_ASSERT(sizeof(U) == 0);
+}
+
+typedef boost::posix_time::ptime ptime;
+typedef boost::posix_time::seconds seconds;
+typedef boost::posix_time::microseconds microseconds;
+typedef boost::posix_time::time_duration time_duration;
+typedef boost::gregorian::date date;
+
+boost::posix_time::ptime rosTimeToPtime(const ros::Time& rosTime) {
+  ptime time(date(1970, 1, 1),
+             seconds(rosTime.sec) + microseconds(rosTime.nsec / 1000));
+  return time;
+}
+
+ros::Time pTimeToRostime(const boost::posix_time::ptime& time) {
+  static ptime timeStart(date(1970, 1, 1));
+  time_duration diff = time - timeStart;
+
+  uint32_t sec = (unsigned int)diff.ticks() /
+                 (unsigned int)time_duration::rep_type::res_adjust();
+  uint32_t nsec = (unsigned int)diff.fractional_seconds();
+
+  return ros::Time(sec, nsec);
+}
+}  // end of namespace dynamicgraph.
+
+#endif  //! DYNAMIC_GRAPH_ROS_CONVERTER_HH

src/dynamic_graph/ros/__init__.py

-from ros_publish import RosPublish
-from ros_subscribe import RosSubscribe
-from ros_joint_state import RosJointState
-from robot_model import RosRobotModel
-
-from ros import Ros
-
-# aliases, for retro compatibility
-ros_import = ros_publish
-ros_export = ros_subscribe
-from ros import RosPublish as RosImport
-from ros import RosSubscribe as RosExport
+# flake8: noqa
+from .ros import RosPublish as RosImport
+from .ros import RosSubscribe as RosExport
+from .ros_publish import RosPublish
+from .ros_subscribe import RosSubscribe
+from .ros_queued_subscribe import RosQueuedSubscribe

src/dynamic_graph/ros/dgcompleter.py

+"""Word completion for GNU readline.
+
+The completer completes keywords, built-ins and globals in a selectable
+namespace (which defaults to __main__); when completing NAME.NAME..., it
+evaluates (!) the expression up to the last dot and completes its attributes.
+
+It's very cool to do "import sys" type "sys.", hit the completion key (twice),
+and see the list of names defined by the sys module!
+
+Tip: to use the tab key as the completion key, call
+
+    readline.parse_and_bind("tab: complete")
+
+Notes:
+
+- Exceptions raised by the completer function are *ignored* (and generally cause
+  the completion to fail).  This is a feature -- since readline sets the tty
+  device in raw (or cbreak) mode, printing a traceback wouldn't work well
+  without some complicated hoopla to save, reset and restore the tty state.
+
+- The evaluation of the NAME.NAME... form may cause arbitrary application
+  defined code to be executed if an object with a __getattr__ hook is found.
+  Since it is the responsibility of the application (or the user) to enable this
+  feature, I consider this an acceptable risk.  More complicated expressions
+  (e.g. function calls or indexing operations) are *not* evaluated.
+
+- GNU readline is also used by the built-in functions input() and
+raw_input(), and thus these also benefit/suffer from the completer
+features.  Clearly an interactive application can benefit by
+specifying its own completer function and using raw_input() for all
+its input.
+
+- When the original stdin is not a tty device, GNU readline is never
+  used, and this module (and the readline module) are silently inactive.
+
+"""
+
+import ast
+
+__all__ = ["DGCompleter"]
+
+
+class DGCompleter:
+    def __init__(self, client):
+        """Create a new completer for the command line.
+
+        Completer([client]) -> completer instance.
+
+        Client is a ROS proxy to dynamic_graph run_command service.
+
+        Completer instances should be used as the completion mechanism of
+        readline via the set_completer() call:
+
+        readline.set_completer(Completer(client).complete)
+        """
+        self.client = client
+        astr = "import readline"
+        self.client(astr)
+        astr = "from rlcompleter import Completer"
+        self.client(astr)
+        astr = "aCompleter=Completer()"
+        self.client(astr)
+        astr = "readline.set_completer(aCompleter.complete)"
+        self.client(astr)
+        astr = 'readline.parse_and_bind("tab: complete")'
+        self.client(astr)
+
+    def complete(self, text, state):
+        """Return the next possible completion for 'text'.
+        readline.parse_and_bind("tab: complete")
+
+        This is called successively with state == 0, 1, 2, ... until it
+        returns None.  The completion should begin with 'text'.
+
+        """
+        astr = 'aCompleter.complete("' + text + '",' + str(state) + ")"
+        response = self.client(astr)
+        res2 = ast.literal_eval(response.result)
+        return res2

src/dynamic_graph/ros/ros.py

-from ros_publish import RosPublish
-from ros_subscribe import RosSubscribe
-from ros_joint_state import RosJointState
-from ros_time import RosTime
+from .ros_publish import RosPublish
+from .ros_subscribe import RosSubscribe
+from .ros_time import RosTime
 
-from dynamic_graph import plug
 
 class Ros(object):
     device = None
     rosPublish = None
     rosSubscribe = None
-    rosJointState = None
 
     # aliases, for retro compatibility
     rosImport = None
     rosExport = None
 
-    def __init__(self, robot, suffix = ''):
+    def __init__(self, robot, suffix=""):
         self.robot = robot
-        self.rosPublish = RosPublish('rosPublish{0}'.format(suffix))
-        self.rosSubscribe = RosSubscribe('rosSubscribe{0}'.format(suffix))
-        self.rosJointState = RosJointState('rosJointState{0}'.format(suffix))
-        self.rosJointState.retrieveJointNames(self.robot.dynamic.name)
-        self.rosTime = RosTime ('rosTime{0}'.format(suffix))
+        self.rosPublish = RosPublish("rosPublish{0}".format(suffix))
+        self.rosSubscribe = RosSubscribe("rosSubscribe{0}".format(suffix))
+        self.rosTime = RosTime("rosTime{0}".format(suffix))
 
-        plug(self.robot.device.state, self.rosJointState.state)
-        self.robot.device.after.addSignal(
-            '{0}.trigger'.format(self.rosPublish.name))
-        self.robot.device.after.addSignal(
-            '{0}.trigger'.format(self.rosJointState.name))
+        self.robot.device.after.addSignal("{0}.trigger".format(self.rosPublish.name))
 
         # aliases, for retro compatibility
-        self.rosImport=self.rosPublish
-        self.rosExport=self.rosSubscribe
+        self.rosImport = self.rosPublish
+        self.rosExport = self.rosSubscribe

src/geometric_simu.cpp

@@ -4,64 +4,31 @@
  *
  * CNRS
  *
- * This file is part of dynamic_graph_bridge.
- * dynamic_graph_bridge is free software: you can redistribute it and/or
- * modify it under the terms of the GNU Lesser General Public License
- * as published by the Free Software Foundation, either version 3 of
- * the License, or (at your option) any later version.
- * dynamic_graph_bridge is distributed in the hope that it will be
- * useful, but WITHOUT ANY WARRANTY; without even the implied warranty
- * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU Lesser General Public License for more details.  You should
- * have received a copy of the GNU Lesser General Public License along
- * with dynamic_graph_bridge.  If not, see <http://www.gnu.org/licenses/>.
  */
-#include <iostream>
-#include <boost/thread/thread.hpp>
-#include <boost/thread/condition.hpp>
+#include <ros/console.h>
 
-#include <dynamic_graph_bridge/sot_loader.hh>
+#include <iostream>
 
-boost::condition_variable cond;
-boost::mutex mut;
+#define ENABLE_RT_LOG
+#include <dynamic-graph/real-time-logger.h>
 
-void workThread(SotLoader *aSotLoader)
-{
-  {
-    boost::lock_guard<boost::mutex> lock(mut);
-  }
-  while(aSotLoader->isDynamicGraphStopped())
-    {
-      usleep(5000);
-    }  
-  while(!aSotLoader->isDynamicGraphStopped())
-    {
-      aSotLoader->oneIteration();
-      usleep(5000);
-    }
-  cond.notify_all();
-  ros::waitForShutdown();
-}
+#include <dynamic_graph_bridge/sot_loader.hh>
 
+int main(int argc, char *argv[]) {
+  ::dynamicgraph::RealTimeLogger::instance().addOutputStream(
+      ::dynamicgraph::LoggerStreamPtr_t(
+          new dynamicgraph::LoggerIOStream(std::cout)));
 
-int main(int argc, char *argv[])
-{
-  
   ros::init(argc, argv, "sot_ros_encapsulator");
-
   SotLoader aSotLoader;
-  if (aSotLoader.parseOptions(argc,argv)<0)
-    return -1;
-  
-  ros::NodeHandle n;
-  ros::ServiceServer service = n.advertiseService("start_dynamic_graph", 
-                                                  &SotLoader::start_dg,
-                                                  &aSotLoader);
-  ROS_INFO("Ready to start dynamic graph.");
+  if (aSotLoader.parseOptions(argc, argv) < 0) return -1;
 
-  boost::thread thr(workThread,&aSotLoader);
+  // Advertize service "(start|stop)_dynamic_graph" and
+  // load parameter "robot_description in SoT.
+  aSotLoader.initializeRosNode(argc, argv);
+  // Load dynamic library and run python prologue.
+  aSotLoader.Initialization();
 
-  boost::unique_lock<boost::mutex> lock(mut);
-  cond.wait(lock);
-  ros::spin();
+  ros::waitForShutdown();
+  return 0;
 }

src/interpreter.cpp

-#include <dynamic_graph_bridge/ros_init.hh>
-#include <dynamic_graph_bridge/ros_interpreter.hh>
-
-int main ()
-{
-  // we spin explicitly so we do not need an async spinner here.
-  ros::NodeHandle& nodeHandle = dynamicgraph::rosInit (false);
-  dynamicgraph::Interpreter interpreter (nodeHandle);
-  ros::spin ();
-}

src/robot_model.cpp

-#include "robot_model.hh"
-
-#include <dynamic-graph/all-commands.h>
-#include <dynamic-graph/factory.h>
-#include <jrl/dynamics/urdf/parser.hh>
-
-#include "dynamic_graph_bridge/ros_init.hh"
-
-namespace dynamicgraph
-{
-
-RosRobotModel::RosRobotModel(const std::string& name)
-    : Dynamic(name,false),
-      jointsParameterName_("jrl_map"),
-      ns_("sot_controller")
-{
-    std::string docstring;
-
-    docstring =
-            "\n"
-            "  Load the robot model from the parameter server.\n"
-            "\n"
-            "  This is the recommended method.\n"
-            "\n";
-    addCommand("loadFromParameterServer", command::makeCommandVoid0(*this,&RosRobotModel::loadFromParameterServer,docstring));
-
-    docstring =
-            "\n"
-            "  Load the robot model from an URDF file.\n"
-            "\n";
-    addCommand("loadUrdf", command::makeCommandVoid1(*this,&RosRobotModel::loadUrdf,docstring));
-
-    docstring =
-            "\n"
-            "  Set the controller namespace."
-            "\n";
-    addCommand("setNamespace", command::makeCommandVoid1(*this,&RosRobotModel::setNamespace,docstring));
-
-    docstring =
-            "\n"
-            "  Get current configuration of the robot.\n"
-            "\n";
-    addCommand ("curConf", new command::Getter<RosRobotModel,Vector> (*this,&RosRobotModel::curConf,docstring));
-
-    docstring =
-            "\n"
-            "  Maps a link name in the URDF parser to actual robot link name.\n"
-            "\n";
-    addCommand ("addJointMapping", command::makeCommandVoid2(*this,&RosRobotModel::addJointMapping,docstring));
-}
-
-RosRobotModel::~RosRobotModel()
-{}
-
-void RosRobotModel::loadUrdf (const std::string& filename)
-{
-    jrl::dynamics::urdf::Parser parser;
-
-    std::map<std::string, std::string>::const_iterator it = specialJoints_.begin();
-    for (;it!=specialJoints_.end();++it) {
-        parser.specifyREPName(it->first, it->second);
-    }
-    rosInit (false);
-
-    m_HDR = parser.parse(filename);
-
-    ros::NodeHandle nh(ns_);
-
-    nh.setParam(jointsParameterName_, parser.JointsNamesByRank_);
-}
-
-void RosRobotModel::setNamespace (const std::string& ns)
-{
-    ns_ = ns;
-}
-
-void RosRobotModel::loadFromParameterServer()
-{
-    jrl::dynamics::urdf::Parser parser;
-
-    std::map<std::string, std::string>::const_iterator it = specialJoints_.begin();
-    for (;it!=specialJoints_.end();++it) {
-        parser.specifyREPName(it->first, it->second);
-    }
-
-    rosInit (false);
-    std::string robotDescription;
-    ros::param::param<std::string> ("/robot_description", robotDescription, "");
-
-    if (robotDescription.empty ())
-        throw std::runtime_error("No model available as ROS parameter. Fail.");
-
-    m_HDR = parser.parseStream (robotDescription);
-
-    ros::NodeHandle nh(ns_);
-
-    nh.setParam(jointsParameterName_, parser.JointsNamesByRank_);
-
-}
-
-namespace
-{
-
-vectorN convertVector(const ml::Vector& v)
-{
-    vectorN res (v.size());
-    for (unsigned i = 0; i < v.size(); ++i)
-        res[i] = v(i);
-    return res;
-}
-
-ml::Vector convertVector(const vectorN& v)
-{
-    ml::Vector res;
-    res.resize((unsigned int)v.size());
-    for (unsigned i = 0; i < v.size(); ++i)
-        res(i) = v[i];
-    return res;
-}
-
-} // end of anonymous namespace.
-
-Vector RosRobotModel::curConf() const
-{
-
-    // The first 6 dofs are associated to the Freeflyer frame
-    // Freeflyer reference frame should be the same as global
-    // frame so that operational point positions correspond to
-    // position in freeflyer frame.
-
-    XmlRpc::XmlRpcValue ffpose;
-    ros::NodeHandle nh(ns_);
-    std::string param_name = "ffpose";
-    if (nh.hasParam(param_name)){
-        nh.getParam(param_name, ffpose);
-        ROS_ASSERT(ffpose.getType() == XmlRpc::XmlRpcValue::TypeArray);
-        ROS_ASSERT(ffpose.size() == 6);
-        for (int32_t i = 0; i < ffpose.size(); ++i)
-        {
-            ROS_ASSERT(ffpose[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
-        }
-    }
-    else
-    {
-        ffpose.setSize(6);
-        for (int32_t i = 0; i < ffpose.size(); ++i)
-            ffpose[i] = 0.0;
-    }
-
-    if (!m_HDR )
-        throw std::runtime_error ("no robot loaded");
-    else {
-        vectorN currConf = m_HDR->currentConfiguration();
-        Vector res;
-        res = convertVector(currConf);
-
-        for (int32_t i = 0; i < ffpose.size(); ++i)
-            res(i) = static_cast<double>(ffpose[i]);
-
-        return res;
-    }
-}
-
-void
-RosRobotModel::addJointMapping(const std::string &link, const std::string &repName)
-{
-    specialJoints_[link] = repName;
-}
-
-DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(RosRobotModel, "RosRobotModel");
-} // end of namespace dynamicgraph.

src/robot_model.hh

-#ifndef DYNAMIC_GRAPH_BRIDGE_ROBOT_MODEL_HH
-# define DYNAMIC_GRAPH_BRIDGE_ROBOT_MODEL_HH
-
-# include <string>
-
-#include <sot-dynamic/dynamic.h>
-#include <dynamic-graph/linear-algebra.h>
-#include "XmlRpcValue.h"
-
-namespace dynamicgraph
-{
-  class RosRobotModel;
-
-  /// \brief This entity load either the current model available in
-  /// the robot_description parameter or a specified file and build
-  /// a Dynamic entity
-  ///
-  /// This relies on jrl_dynamics_urdf to load the model and jrl-dynamics
-  /// to realize the computation.
-  class RosRobotModel : public sot::Dynamic
-  {
-    DYNAMIC_GRAPH_ENTITY_DECL();
-  public:
-    RosRobotModel(const std::string& n);
-    virtual ~RosRobotModel();
-
-    void loadUrdf(const std::string& filename);
-    void setNamespace (const std::string& ns);
-    void loadFromParameterServer();
-    Vector curConf() const;
-
-    void addJointMapping(const std::string& link, const std::string& repName);
-
-  protected:
-
-    unsigned getDimension () const
-    {
-      if (!m_HDR)
-	throw std::runtime_error ("no robot loaded");
-      return m_HDR->numberDof();
-    }
-
-
-  private:
-    /// \brief Name of the parameter where the joints list will be published
-    std::string jointsParameterName_;
-
-    /// \brief Name of the controller namespace
-    std::string ns_;
-
-    /// \brief Special joints map for the parser
-    std::map<std::string, std::string> specialJoints_;
-  };
-} // end of namespace dynamicgraph.
-
-#endif //! DYNAMIC_GRAPH_BRIDGE_ROBOT_MODEL_HH

src/ros_init.cpp

-#include <stdexcept>
+#include "dynamic_graph_bridge/ros_init.hh"
+
 #include <boost/make_shared.hpp>
 #include <boost/shared_ptr.hpp>
+#include <stdexcept>
 
-#include "dynamic_graph_bridge/ros_init.hh"
-
-namespace dynamicgraph
-{
-  struct GlobalRos
-  {
-    ~GlobalRos ()
-    {
-      if (spinner)
-	spinner->stop ();
-      if (nodeHandle)
-	nodeHandle->shutdown ();
-    }
+namespace dynamicgraph {
+struct GlobalRos {
+  ~GlobalRos() {
+    if (spinner) spinner->stop();
+    if (nodeHandle) nodeHandle->shutdown();
+  }
 
-    boost::shared_ptr<ros::NodeHandle> nodeHandle;
-    boost::shared_ptr<ros::AsyncSpinner> spinner;
-    boost::shared_ptr<ros::MultiThreadedSpinner> mtSpinner;
-  };
-  GlobalRos ros;
+  boost::shared_ptr<ros::NodeHandle> nodeHandle;
+  boost::shared_ptr<ros::AsyncSpinner> spinner;
+  boost::shared_ptr<ros::MultiThreadedSpinner> mtSpinner;
+};
+GlobalRos ros;
 
-  ros::NodeHandle& rosInit (bool createAsyncSpinner, bool createMultiThreadedSpinner)
-  {
-    if (!ros.nodeHandle)
-      {
-	int argc = 1;
-	char* arg0 = strdup("dynamic_graph_bridge");
-	char* argv[] = {arg0, 0};
-	ros::init(argc, argv, "dynamic_graph_bridge");
-	free (arg0);
+ros::NodeHandle& rosInit(bool createAsyncSpinner,
+                         bool createMultiThreadedSpinner) {
+  if (!ros.nodeHandle) {
+    int argc = 1;
+    char* arg0 = strdup("dynamic_graph_bridge");
+    char* argv[] = {arg0, 0};
+    ros::init(argc, argv, "dynamic_graph_bridge");
+    free(arg0);
 
-	ros.nodeHandle = boost::make_shared<ros::NodeHandle> ("");
-      }
-    if (!ros.spinner && createAsyncSpinner)
-      {
-	ros.spinner = boost::make_shared<ros::AsyncSpinner> (4);
-	ros.spinner->start ();
-      }
-    else 
-      {
-	if (!ros.mtSpinner && createMultiThreadedSpinner)
-	  {
-	    ros.mtSpinner = boost::make_shared<ros::MultiThreadedSpinner>(4);
-	  }
-      }
-    return *ros.nodeHandle;
+    ros.nodeHandle = boost::make_shared<ros::NodeHandle>("");
   }
+  if (!ros.spinner && createAsyncSpinner) {
+    ros.spinner = boost::make_shared<ros::AsyncSpinner>(4);
 
-  ros::AsyncSpinner& spinner ()
-  {
-    if (!ros.spinner)
-      throw std::runtime_error ("spinner has not been created");
-    return *ros.spinner;
-  }
+    // Change the thread's scheduler from real-time to normal and reduce its
+    // priority
+    int oldThreadPolicy, newThreadPolicy;
+    struct sched_param oldThreadParam, newThreadParam;
+    if (pthread_getschedparam(pthread_self(), &oldThreadPolicy,
+                              &oldThreadParam) == 0) {
+      newThreadPolicy = SCHED_OTHER;
+      newThreadParam = oldThreadParam;
+      newThreadParam.sched_priority -=
+          5;  // Arbitrary number, TODO: choose via param/file?
+      if (newThreadParam.sched_priority <
+          sched_get_priority_min(newThreadPolicy))
+        newThreadParam.sched_priority = sched_get_priority_min(newThreadPolicy);
+
+      pthread_setschedparam(pthread_self(), newThreadPolicy, &newThreadParam);
+    }
+
+    // AsyncSpinners are created with the reduced priority
+    ros.spinner->start();
 
-  ros::MultiThreadedSpinner& mtSpinner ()
-  {
-    if (!ros.mtSpinner)
-      throw std::runtime_error ("spinner has not been created");
-    return *ros.mtSpinner;
+    // Switch the priority of the parent thread (this thread) back to real-time.
+    pthread_setschedparam(pthread_self(), oldThreadPolicy, &oldThreadParam);
+  } else {
+    if (!ros.mtSpinner && createMultiThreadedSpinner) {
+      // Seems not to be used.
+      // If we need to reduce its threads priority, it needs to be done before
+      // calling the MultiThreadedSpinner::spin() method
+      ros.mtSpinner = boost::make_shared<ros::MultiThreadedSpinner>(4);
+    }
   }
+  return *ros.nodeHandle;
+}
+
+ros::AsyncSpinner& spinner() {
+  if (!ros.spinner) throw std::runtime_error("spinner has not been created");
+  return *ros.spinner;
+}
+
+ros::MultiThreadedSpinner& mtSpinner() {
+  if (!ros.mtSpinner) throw std::runtime_error("spinner has not been created");
+  return *ros.mtSpinner;
+}
 
-} // end of namespace dynamicgraph.
+}  // end of namespace dynamicgraph.

src/ros_interpreter.cpp

 #include "dynamic_graph_bridge/ros_interpreter.hh"
 
-namespace dynamicgraph
-{
-  static const int queueSize = 5;
-
-  Interpreter::Interpreter (ros::NodeHandle& nodeHandle)
-    : interpreter_ (),
-      nodeHandle_ (nodeHandle),
-      runCommandSrv_ (),
-      runPythonFileSrv_ ()
-  {
+namespace dynamicgraph {
+static const int queueSize = 5;
+
+Interpreter::Interpreter(ros::NodeHandle& nodeHandle)
+    : interpreter_(),
+      nodeHandle_(nodeHandle),
+      runCommandSrv_(),
+      runPythonFileSrv_() {}
+
+void Interpreter::startRosService() {
+  runCommandCallback_t runCommandCb =
+      boost::bind(&Interpreter::runCommandCallback, this, _1, _2);
+  runCommandSrv_ = nodeHandle_.advertiseService("run_command", runCommandCb);
+
+  runPythonFileCallback_t runPythonFileCb =
+      boost::bind(&Interpreter::runPythonFileCallback, this, _1, _2);
+  runPythonFileSrv_ =
+      nodeHandle_.advertiseService("run_script", runPythonFileCb);
+}
+
+bool Interpreter::runCommandCallback(
+    dynamic_graph_bridge_msgs::RunCommand::Request& req,
+    dynamic_graph_bridge_msgs::RunCommand::Response& res) {
+  interpreter_.python(req.input, res.result, res.standardoutput,
+                      res.standarderror);
+  return true;
+}
+
+bool Interpreter::runPythonFileCallback(
+    dynamic_graph_bridge_msgs::RunPythonFile::Request& req,
+    dynamic_graph_bridge_msgs::RunPythonFile::Response& res) {
+  interpreter_.runPythonFile(req.input);
+  res.result = "File parsed";  // FIX: It is just an echo, is there a way to
+                               // have a feedback?
+  return true;
+}
+
+void Interpreter::runCommand(const std::string& command, std::string& result,
+                             std::string& out, std::string& err) {
+  interpreter_.python(command, result, out, err);
+  if (err.size() > 0) {
+    ROS_ERROR(err.c_str());
   }
+}
 
-  void Interpreter::startRosService ()
-  {
-    runCommandCallback_t runCommandCb =
-      boost::bind (&Interpreter::runCommandCallback, this, _1, _2);
-    runCommandSrv_ =
-      nodeHandle_.advertiseService ("run_command", runCommandCb);
-
-    runPythonFileCallback_t runPythonFileCb =
-      boost::bind (&Interpreter::runPythonFileCallback, this, _1, _2);
-    runPythonFileSrv_ =
-      nodeHandle_.advertiseService ("run_script", runPythonFileCb);
-  }
-
-  bool
-  Interpreter::runCommandCallback
-  (dynamic_graph_bridge_msgs::RunCommand::Request& req,
-   dynamic_graph_bridge_msgs::RunCommand::Response& res)
-  {
-    interpreter_.python(req.input, res.result, res.standardoutput, res.standarderror);
-    return true;
-  }
-
-  bool
-  Interpreter::runPythonFileCallback (dynamic_graph_bridge_msgs::RunPythonFile::Request& req,
-                                      dynamic_graph_bridge_msgs::RunPythonFile::Response& res)
-  {
-    interpreter_.runPythonFile(req.input);
-    res.result = "File parsed"; // FIX: It is just an echo, is there a way to have a feedback?
-    return true;
-  }
-
-  void Interpreter::runCommand
-  (const std::string & command, 
-   std::string &result,
-   std::string &out, 
-   std::string &err)
-  {
-    interpreter_.python(command, result, out, err);
-  }
-
-  void Interpreter::runPythonFile( std::string ifilename ){
-      interpreter_.runPythonFile(ifilename);
-  }
-
-} // end of namespace dynamicgraph.
+void Interpreter::runPythonFile(std::string ifilename) {
+  interpreter_.runPythonFile(ifilename);
+}
 
+}  // end of namespace dynamicgraph.

src/ros_joint_state.cpp

-#include <boost/assign.hpp>
-#include <boost/bind.hpp>
-#include <boost/format.hpp>
-
-#include <dynamic-graph/command.h>
-#include <dynamic-graph/factory.h>
-#include <dynamic-graph/pool.h>
-#include <sot-dynamic/dynamic.h>
-
-#include "dynamic_graph_bridge/ros_init.hh"
-#include "ros_joint_state.hh"
-#include "sot_to_ros.hh"
-
-namespace dynamicgraph
-{
-  DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(RosJointState, "RosJointState");
-  const double RosJointState::ROS_JOINT_STATE_PUBLISHER_RATE = 0.01;
-
-  namespace command
-  {
-    using ::dynamicgraph::command::Command;
-    using ::dynamicgraph::command::Value;
-
-    class RetrieveJointNames : public Command
-    {
-    public:
-      RetrieveJointNames (RosJointState& entity,
-			  const std::string& docstring);
-      virtual Value doExecute ();
-    };
-
-    RetrieveJointNames::RetrieveJointNames
-    (RosJointState& entity, const std::string& docstring)
-      : Command (entity, boost::assign::list_of (Value::STRING), docstring)
-    {}
-
-    namespace
-    {
-      void
-      buildJointNames (sensor_msgs::JointState& jointState, CjrlJoint* joint)
-      {
-	if (!joint)
-	  return;
-	// Ignore anchors.
-	if (joint->numberDof() != 0)
-	  {
-	    // If we only have one dof, the dof name is the joint name.
-	    if (joint->numberDof() == 1)
-	      {
-		jointState.name[joint->rankInConfiguration()] =
-		  joint->getName();
-	      }
-	    // ...otherwise, the dof name is the joint name on which
-	    // the dof id is appended.
-	    else
-	      for (unsigned i = 0; i < joint->numberDof(); ++i)
-		{
-		  boost::format fmt("%1%_%2%");
-		  fmt % joint->getName();
-		  fmt % i;
-		  jointState.name[joint->rankInConfiguration() + i] =
-		    fmt.str();
-		}
-	  }
-	for (unsigned i = 0; i < joint->countChildJoints (); ++i)
-	  buildJointNames (jointState, joint->childJoint (i));
-      }
-    } // end of anonymous namespace
-
-    Value RetrieveJointNames::doExecute ()
-    {
-      RosJointState& entity = static_cast<RosJointState&> (owner ());
-
-      std::vector<Value> values = getParameterValues ();
-      std::string name = values[0].value ();
-
-      if (!dynamicgraph::PoolStorage::getInstance ()->existEntity (name))
-	{
-	  std::cerr << "invalid entity name" << std::endl;
-	  return Value ();
-	}
-
-      dynamicgraph::sot::Dynamic* dynamic =
-	dynamic_cast<dynamicgraph::sot::Dynamic*>
-	(&dynamicgraph::PoolStorage::getInstance ()->getEntity (name));
-      if (!dynamic)
-	{
-	  std::cerr << "entity is not a Dynamic entity" << std::endl;
-	  return Value ();
-	}
-
-      CjrlHumanoidDynamicRobot* robot = dynamic->m_HDR;
-      if (!robot)
-	{
-	  std::cerr << "no robot in the dynamic entity" << std::endl;
-	  return Value ();
-	}
-
-      entity.jointState ().name.resize (robot->numberDof());
-      buildJointNames (entity.jointState (), robot->rootJoint());
-      return Value ();
-    }
-  } // end of namespace command.
-
-  RosJointState::RosJointState (const std::string& n)
-    : Entity (n),
-      // do not use callbacks, so do not create a useless spinner
-      nh_ (rosInit (false)),
-      state_ (0, MAKE_SIGNAL_STRING(name, true, "Vector", "state")),
-      publisher_ (nh_, "dynamic_graph/joint_states", 5),
-      jointState_ (),
-      trigger_ (boost::bind (&RosJointState::trigger, this, _1, _2),
-		sotNOSIGNAL,
-		MAKE_SIGNAL_STRING(name, true, "int", "trigger")),
-      rate_ (ROS_JOINT_STATE_PUBLISHER_RATE),
-      lastPublicated_ ()
-  {
-    try {
-      lastPublicated_ = ros::Time::now ();
-    } catch (const std::exception& exc) {
-      throw std::runtime_error ("Failed to call ros::Time::now ():" +
-				std::string (exc.what ()));
-    }
-    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 = "";
-
-    std::string docstring =
-      "\n"
-      "  Retrieve joint names using robot model contained in a Dynamic entity\n"
-      "\n"
-      "  Input:\n"
-      "    - dynamic entity name (i.e. robot.dynamic.name)\n"
-      "\n";
-    addCommand ("retrieveJointNames",
-		new command::RetrieveJointNames (*this, docstring));
-  }
-
-  RosJointState::~RosJointState ()
-  {}
-
-  int&
-  RosJointState::trigger (int& dummy, int t)
-  {
-    ros::Duration dt = ros::Time::now () - lastPublicated_;
-    if (dt > rate_ && publisher_.trylock ())
-      {
-	lastPublicated_ = ros::Time::now ();
-
-	// State size without the free floating.
-	std::size_t s = state_.access (t).size ();
-
-	// Safety check: if data are inconsistent, clear
-	// the joint names to avoid sending erroneous data.
-	// This should not happen unless you change
-	// the robot model at run-time.
-	if (s != jointState_.name.size())
-	  jointState_.name.clear();
-
-	// Update header.
-	++jointState_.header.seq;
-
-	ros::Time now = ros::Time::now ();
-	jointState_.header.stamp.sec = now.sec;
-	jointState_.header.stamp.nsec = now.nsec;
-
-	// Fill position.
-	jointState_.position.resize (s);
-	for (std::size_t i = 0; i < s; ++i)
-	  jointState_.position[i] = state_.access (t) (i);
-
-	publisher_.msg_ = jointState_;
-	publisher_.unlockAndPublish ();
-      }
-    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 <realtime_tools/realtime_publisher.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;
-
-    static const double ROS_JOINT_STATE_PUBLISHER_RATE;
-
-    RosJointState (const std::string& n);
-    virtual ~RosJointState ();
-
-    int& trigger (int&, int);
-
-    sensor_msgs::JointState& jointState ()
-    {
-      return jointState_;
-    }
-  private:
-    ros::NodeHandle& nh_;
-    signalVectorIn_t state_;
-    realtime_tools::RealtimePublisher<sensor_msgs::JointState> publisher_;
-    sensor_msgs::JointState jointState_;
-    dynamicgraph::SignalTimeDependent<int,int> trigger_;
-    ros::Duration rate_;
-    ros::Time lastPublicated_;
-  };
-} // end of namespace dynamicgraph.
-
-#endif //! DYNAMIC_GRAPH_JOINT_STATE_HH

src/ros_parameter.cpp

+#include <pinocchio/fwd.hpp>
+
+// include pinocchio first
+//
+#include <ros/ros.h>
+#include <urdf_parser/urdf_parser.h>
+
+#include <boost/make_shared.hpp>
+#include <boost/shared_ptr.hpp>
+#include <sot/core/robot-utils.hh>
+#include <stdexcept>
+
+#include "dynamic_graph_bridge/ros_parameter.hh"
+#include "pinocchio/multibody/model.hpp"
+#include "pinocchio/parsers/urdf.hpp"
+
+namespace dynamicgraph {
+bool parameter_server_read_robot_description() {
+  ros::NodeHandle nh;
+  if (!nh.hasParam("/robot_description")) {
+    ROS_ERROR("No /robot_description parameter");
+    return false;
+  }
+
+  std::string robot_description;
+  std::string parameter_name("/robot_description");
+  nh.getParam(parameter_name, robot_description);
+
+  std::string model_name("robot");
+
+  // Search for the robot util related to robot_name.
+  sot::RobotUtilShrPtr aRobotUtil = sot::getRobotUtil(model_name);
+  // If does not exist then it is created.
+  if (aRobotUtil == sot::RefVoidRobotUtil())
+    aRobotUtil = sot::createRobotUtil(model_name);
+
+  // If the creation is fine
+  if (aRobotUtil != sot::RefVoidRobotUtil()) {
+    // Then set the robot model.
+    aRobotUtil->set_parameter(parameter_name, robot_description);
+    ROS_INFO("Set parameter_name : %s.", parameter_name.c_str());
+    // Everything went fine.
+    return true;
+  }
+  ROS_ERROR("Wrong initialization of parameter_name %s",
+            parameter_name.c_str());
+
+  // Otherwise something went wrong.
+  return false;
+}
+
+}  // namespace dynamicgraph

src/ros_publish-python-module-py.cc

+#include <dynamic-graph/python/module.hh>
+
+#include "ros_publish.hh"
+
+namespace dg = dynamicgraph;
+
+BOOST_PYTHON_MODULE(wrap) {
+  bp::import("dynamic_graph");
+
+  dg::python::exposeEntity<dg::RosPublish, bp::bases<dg::Entity>,
+                           dg::python::AddCommands>()
+      .def("clear", &dg::RosPublish::clear,
+           "Remove all signals writing data to a ROS topic")
+      .def("rm", &dg::RosPublish::rm,
+           "Remove a signal writing data to a ROS topic",
+           bp::args("signal_name"))
+      .def("list", &dg::RosPublish::list,
+           "List signals writing data to a ROS topic");
+}

src/ros_publish.cpp

-#include <stdexcept>
+#include "ros_publish.hh"
+
+#include <dynamic-graph/command.h>
+#include <dynamic-graph/factory.h>
+#include <dynamic-graph/linear-algebra.h>
+#include <ros/ros.h>
+#include <std_msgs/Float64.h>
+#include <std_msgs/UInt32.h>
 
 #include <boost/assign.hpp>
 #include <boost/bind.hpp>
@@ -6,268 +13,182 @@
 #include <boost/format.hpp>
 #include <boost/function.hpp>
 #include <boost/make_shared.hpp>
-
-#include <ros/ros.h>
-#include <std_msgs/Float64.h>
-#include <std_msgs/UInt32.h>
-
-#include <dynamic-graph/factory.h>
-#include <dynamic-graph/command.h>
+#include <stdexcept>
 
 #include "dynamic_graph_bridge/ros_init.hh"
-#include "ros_publish.hh"
-
-namespace dynamicgraph
-{
-  DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(RosPublish, "RosPublish");
-  const double RosPublish::ROS_JOINT_STATE_PUBLISHER_RATE = 0.01;
-
-  namespace command
-  {
-    namespace rosPublish
-    {
-      Clear::Clear
-      (RosPublish& entity, const std::string& docstring)
-	: Command
-	  (entity,
-	   std::vector<Value::Type> (),
-	   docstring)
-      {}
-
-      Value Clear::doExecute ()
-      {
-	RosPublish& entity =
-	  static_cast<RosPublish&> (owner ());
-
-	entity.clear ();
-	return Value ();
-      }
-
-      List::List
-      (RosPublish& entity, const std::string& docstring)
-	: Command
-	  (entity,
-	   std::vector<Value::Type> (),
-	   docstring)
-      {}
-
-      Value List::doExecute ()
-      {
-	RosPublish& entity =
-	  static_cast<RosPublish&> (owner ());
-	return Value (entity.list ());
-      }
-
-      Add::Add
-      (RosPublish& entity, const std::string& docstring)
-	: Command
-	  (entity,
-	   boost::assign::list_of
-	   (Value::STRING) (Value::STRING) (Value::STRING),
-	   docstring)
-      {}
-
-      Value Add::doExecute ()
-      {
-	RosPublish& entity =
-	  static_cast<RosPublish&> (owner ());
-	std::vector<Value> values = getParameterValues ();
 
-	const std::string& type = values[0].value ();
-	const std::string& signal = values[1].value ();
-	const std::string& topic = values[2].value ();
-
-	if (type == "double")
-	  entity.add<double> (signal, topic);
-	else if (type == "unsigned")
-	  entity.add<unsigned int> (signal, topic);
-	else if (type == "matrix")
-	  entity.add<ml::Matrix> (signal, topic);
-	else if (type == "vector")
-	  entity.add<ml::Vector> (signal, topic);
-	else if (type == "vector3")
-	  entity.add<specific::Vector3> (signal, topic);
-	else if (type == "vector3Stamped")
-	  entity.add<std::pair<specific::Vector3, ml::Vector> > (signal, topic);
-	else if (type == "matrixHomo")
-	  entity.add<sot::MatrixHomogeneous> (signal, topic);
-	else if (type == "matrixHomoStamped")
-	  entity.add<std::pair<sot::MatrixHomogeneous, ml::Vector> >
-	    (signal, topic);
-	else if (type == "twist")
-	  entity.add<specific::Twist> (signal, topic);
-	else if (type == "twistStamped")
-	  entity.add<std::pair<specific::Twist, ml::Vector> > (signal, topic);
-	else
-	  throw std::runtime_error("bad type");
-	return Value ();
-      }
-
-      Rm::Rm
-      (RosPublish& entity, const std::string& docstring)
-	: Command
-	  (entity,
-	   boost::assign::list_of (Value::STRING),
-	   docstring)
-      {}
-
-      Value Rm::doExecute ()
-      {
-	RosPublish& entity =
-	  static_cast<RosPublish&> (owner ());
-	std::vector<Value> values = getParameterValues ();
-	const std::string& signal = values[0].value ();
-	entity.rm (signal);
-	return Value ();
-      }
-    } // end of errorEstimator.
-  } // end of namespace command.
-
-  const std::string RosPublish::docstring_
-  ("Publish dynamic-graph signals as ROS topics.\n"
-   "\n"
-   "  Use command \"add\" to publish a new ROS topic.\n");
-
-  RosPublish::RosPublish (const std::string& n)
+#define ENABLE_RT_LOG
+#include <dynamic-graph/real-time-logger.h>
+
+namespace dynamicgraph {
+
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(RosPublish, "RosPublish");
+const double RosPublish::ROS_JOINT_STATE_PUBLISHER_RATE = 0.01;
+
+namespace command {
+namespace rosPublish {
+
+Add::Add(RosPublish& entity, const std::string& docstring)
+    : Command(
+          entity,
+          boost::assign::list_of(Value::STRING)(Value::STRING)(Value::STRING),
+          docstring) {}
+
+Value Add::doExecute() {
+  RosPublish& entity = static_cast<RosPublish&>(owner());
+  std::vector<Value> values = getParameterValues();
+
+  const std::string& type = values[0].value();
+  const std::string& signal = values[1].value();
+  const std::string& topic = values[2].value();
+
+  if (type == "boolean")
+    entity.add<bool>(signal, topic);
+  else if (type == "double")
+    entity.add<double>(signal, topic);
+  else if (type == "unsigned")
+    entity.add<unsigned int>(signal, topic);
+  else if (type == "int")
+    entity.add<int>(signal, topic);
+  else if (type == "matrix")
+    entity.add<Matrix>(signal, topic);
+  else if (type == "vector")
+    entity.add<Vector>(signal, topic);
+  else if (type == "vector3")
+    entity.add<specific::Vector3>(signal, topic);
+  else if (type == "vector3Stamped")
+    entity.add<std::pair<specific::Vector3, Vector> >(signal, topic);
+  else if (type == "matrixHomo")
+    entity.add<sot::MatrixHomogeneous>(signal, topic);
+  else if (type == "matrixHomoStamped")
+    entity.add<std::pair<sot::MatrixHomogeneous, Vector> >(signal, topic);
+  else if (type == "twist")
+    entity.add<specific::Twist>(signal, topic);
+  else if (type == "twistStamped")
+    entity.add<std::pair<specific::Twist, Vector> >(signal, topic);
+  else if (type == "string")
+    entity.add<std::string>(signal, topic);
+  else
+    throw std::runtime_error("bad type");
+  return Value();
+}
+
+}  // namespace rosPublish
+}  // end of namespace command.
+
+const std::string RosPublish::docstring_(
+    "Publish dynamic-graph signals as ROS topics.\n"
+    "\n"
+    "  Use command \"add\" to publish a new ROS topic.\n");
+
+RosPublish::RosPublish(const std::string& n)
     : dynamicgraph::Entity(n),
       // RosPublish do not use callback so do not create a useless spinner.
-      nh_ (rosInit (false)),
-      bindedSignal_ (),
-      trigger_ (boost::bind (&RosPublish::trigger, this, _1, _2),
-		sotNOSIGNAL,
-		MAKE_SIGNAL_STRING(name, true, "int", "trigger")),
-      rate_ (ROS_JOINT_STATE_PUBLISHER_RATE),
-      lastPublicated_ ()
-  {
-    try {
-      lastPublicated_ = ros::Time::now ();
-    } catch (const std::exception& exc) {
-      throw std::runtime_error ("Failed to call ros::Time::now ():" +
-				std::string (exc.what ()));
+      nh_(rosInit(false)),
+      bindedSignal_(),
+      trigger_(boost::bind(&RosPublish::trigger, this, _1, _2), sotNOSIGNAL,
+               MAKE_SIGNAL_STRING(name, true, "int", "trigger")),
+      rate_(0, 10000000),
+      nextPublication_() {
+  aofs_.open("/tmp/ros_publish.txt");
+  dgADD_OSTREAM_TO_RTLOG(aofs_);
+
+  try {
+    if (ros::Time::isSimTime())
+      nextPublication_ = ros::Time::now();
+    else {
+      clock_gettime(CLOCK_REALTIME, &nextPublicationRT_);
     }
-    signalRegistration (trigger_);
-    trigger_.setNeedUpdateFromAllChildren (true);
+  } catch (const std::exception& exc) {
+    throw std::runtime_error("Failed to call ros::Time::now ():" +
+                             std::string(exc.what()));
+  }
+  signalRegistration(trigger_);
+  trigger_.setNeedUpdateFromAllChildren(true);
 
-    std::string docstring =
+  std::string docstring =
       "\n"
       "  Add a signal writing data to a ROS topic\n"
       "\n"
       "  Input:\n"
       "    - type: string among ['double', 'matrix', 'vector', 'vector3',\n"
-      "                          'vector3Stamped', 'matrixHomo', 'matrixHomoStamped',\n"
+      "                          'vector3Stamped', 'matrixHomo', "
+      "'matrixHomoStamped',\n"
       "                          'twist', 'twistStamped'],\n"
       "    - signal: the signal name in dynamic-graph,\n"
       "    - topic:  the topic name in ROS.\n"
       "\n";
-    addCommand ("add",
-		new command::rosPublish::Add
-		(*this, docstring));
-    docstring =
-      "\n"
-      "  Remove a signal writing data to a ROS topic\n"
-      "\n"
-      "  Input:\n"
-      "    - name of the signal to remove (see method list for the list of signals).\n"
-      "\n";
-    addCommand ("rm",
-		new command::rosPublish::Rm
-		(*this, docstring));
-    docstring =
-      "\n"
-      "  Remove all signals writing data to a ROS topic\n"
-      "\n"
-      "  No input:\n"
-      "\n";
-    addCommand ("clear",
-		new command::rosPublish::Clear
-		(*this, docstring));
-    docstring =
-      "\n"
-      "  List signals writing data to a ROS topic\n"
-      "\n"
-      "  No input:\n"
-      "\n";
-    addCommand ("list",
-		new command::rosPublish::List
-		(*this, docstring));
-  }
-
-  RosPublish::~RosPublish ()
-  {
-  }
+  addCommand("add", new command::rosPublish::Add(*this, docstring));
+}
 
-  void RosPublish::display (std::ostream& os) const
-  {
-    os << CLASS_NAME << std::endl;
-  }
+RosPublish::~RosPublish() { aofs_.close(); }
 
-  void RosPublish::rm (const std::string& signal)
-  {
-    if(bindedSignal_.find(signal) == bindedSignal_.end())
-      return;
+void RosPublish::display(std::ostream& os) const {
+  os << CLASS_NAME << std::endl;
+}
 
-    if(signal == "trigger")
-    {
-      std::cerr << "The trigger signal should not be removed. Aborting." << std::endl;
-      return;
-    }
+void RosPublish::rm(const std::string& signal) {
+  if (bindedSignal_.find(signal) == bindedSignal_.end()) return;
 
-    //lock the mutex to avoid deleting the signal during a call to trigger
-    while(! mutex_.try_lock() ){}
-    signalDeregistration(signal);
-    bindedSignal_.erase (signal);
-    mutex_.unlock();
+  if (signal == "trigger") {
+    std::cerr << "The trigger signal should not be removed. Aborting."
+              << std::endl;
+    return;
   }
 
-  std::string RosPublish::list () const
-  {
-    std::string result("[");
-    for (std::map<std::string, bindedSignal_t>::const_iterator it =
-	   bindedSignal_.begin (); it != bindedSignal_.end (); it++) {
-      result += "'" + it->first + "',";
+  // lock the mutex to avoid deleting the signal during a call to trigger
+  boost::mutex::scoped_lock lock(mutex_);
+
+  signalDeregistration(signal);
+  bindedSignal_.erase(signal);
+}
+
+std::vector<std::string> RosPublish::list() const {
+  std::vector<std::string> result(bindedSignal_.size());
+  std::transform(bindedSignal_.begin(), bindedSignal_.end(), result.begin(),
+                 [](const auto& pair) { return pair.first; });
+  return result;
+}
+
+void RosPublish::clear() {
+  std::map<std::string, bindedSignal_t>::iterator it = bindedSignal_.begin();
+  for (; it != bindedSignal_.end();) {
+    if (it->first != "trigger") {
+      rm(it->first);
+      it = bindedSignal_.begin();
+    } else {
+      ++it;
     }
-    result += "]";
-    return result;
- }
-
-  void RosPublish::clear ()
-  {
-    std::map<std::string, bindedSignal_t>::iterator it = bindedSignal_.begin();
-    for(; it!= bindedSignal_.end(); )
-    {
-      if (it->first != "trigger")
-      {
-        rm(it->first);
-        it = bindedSignal_.begin();
-      }
-      else
-      {
-          ++it;
-      }
+  }
+}
+
+int& RosPublish::trigger(int& dummy, int t) {
+  typedef std::map<std::string, bindedSignal_t>::iterator iterator_t;
+  ros::Time aTime;
+  if (ros::Time::isSimTime()) {
+    aTime = ros::Time::now();
+    if (aTime <= nextPublication_) return dummy;
+
+    nextPublication_ = aTime + rate_;
+  } else {
+    struct timespec aTimeRT;
+    clock_gettime(CLOCK_REALTIME, &aTimeRT);
+    nextPublicationRT_.tv_sec = aTimeRT.tv_sec + rate_.sec;
+    nextPublicationRT_.tv_nsec = aTimeRT.tv_nsec + rate_.nsec;
+    if (nextPublicationRT_.tv_nsec > 1000000000) {
+      nextPublicationRT_.tv_nsec -= 1000000000;
+      nextPublicationRT_.tv_sec += 1;
     }
   }
 
-  int& RosPublish::trigger (int& dummy, int t)
-  {
-    typedef std::map<std::string, bindedSignal_t>::iterator iterator_t;
-
-    ros::Duration dt = ros::Time::now () - lastPublicated_;
-    if (dt < rate_)
-      return dummy;
+  boost::mutex::scoped_lock lock(mutex_);
 
-    while(! mutex_.try_lock() ){}
-    for (iterator_t it = bindedSignal_.begin ();
-	 it != bindedSignal_.end (); ++it)
-      {
-	boost::get<1>(it->second) (t);
-      }
-    mutex_.unlock();
-    return dummy;
+  for (iterator_t it = bindedSignal_.begin(); it != bindedSignal_.end(); ++it) {
+    boost::get<1>(it->second)(t);
   }
+  return dummy;
+}
 
-  std::string RosPublish::getDocString () const
-  {
-    return docstring_;
-  }
+std::string RosPublish::getDocString() const { return docstring_; }
 
-} // end of namespace dynamicgraph.
+}  // end of namespace dynamicgraph.

src/ros_publish.hh

 #ifndef DYNAMIC_GRAPH_ROS_PUBLISH_HH
-# define DYNAMIC_GRAPH_ROS_PUBLISH_HH
-# include <iostream>
-# include <map>
-
-# include <boost/shared_ptr.hpp>
-# include <boost/tuple/tuple.hpp>
-# include <boost/interprocess/sync/interprocess_mutex.hpp>
-
-# include <dynamic-graph/entity.h>
-# include <dynamic-graph/signal-time-dependent.h>
-# include <dynamic-graph/command.h>
-
-# include <ros/ros.h>
-
-# include <realtime_tools/realtime_publisher.h>
-
-# include "converter.hh"
-# include "sot_to_ros.hh"
-
-namespace dynamicgraph
-{
-  class RosPublish;
-
-  namespace command
-  {
-    namespace rosPublish
-    {
-      using ::dynamicgraph::command::Command;
-      using ::dynamicgraph::command::Value;
-
-# define ROS_PUBLISH_MAKE_COMMAND(CMD)			\
-      class CMD : public Command			\
-      {							\
-      public:						\
-	CMD (RosPublish& entity,				\
-	     const std::string& docstring);		\
-	virtual Value doExecute ();			\
-      }
-
-      ROS_PUBLISH_MAKE_COMMAND(Add);
-      ROS_PUBLISH_MAKE_COMMAND(Clear);
-      ROS_PUBLISH_MAKE_COMMAND(List);
-      ROS_PUBLISH_MAKE_COMMAND(Rm);
+#define DYNAMIC_GRAPH_ROS_PUBLISH_HH
+#include <dynamic-graph/command.h>
+#include <dynamic-graph/entity.h>
+#include <dynamic-graph/signal-time-dependent.h>
+#include <realtime_tools/realtime_publisher.h>
+#include <ros/ros.h>
+
+#include <boost/shared_ptr.hpp>
+#include <boost/thread/mutex.hpp>
+#include <boost/tuple/tuple.hpp>
+#include <fstream>
+#include <map>
+
+#include "converter.hh"
+#include "sot_to_ros.hh"
+
+namespace dynamicgraph {
+class RosPublish;
+
+namespace command {
+namespace rosPublish {
+using ::dynamicgraph::command::Command;
+using ::dynamicgraph::command::Value;
+
+#define ROS_PUBLISH_MAKE_COMMAND(CMD)                      \
+  class CMD : public Command {                             \
+   public:                                                 \
+    CMD(RosPublish& entity, const std::string& docstring); \
+    virtual Value doExecute();                             \
+  }
+
+ROS_PUBLISH_MAKE_COMMAND(Add);
 
 #undef ROS_PUBLISH_MAKE_COMMAND
 
-    } // end of namespace errorEstimator.
-  } // end of namespace command.
-
-
-  /// \brief Publish dynamic-graph information into ROS.
-  class RosPublish : public dynamicgraph::Entity
-  {
-    DYNAMIC_GRAPH_ENTITY_DECL();
-  public:
-    typedef boost::function<void (int)> callback_t;
-
-    typedef boost::tuple<
-      boost::shared_ptr<dynamicgraph::SignalBase<int> >,
-      callback_t>
-    bindedSignal_t;
-
-    static const double ROS_JOINT_STATE_PUBLISHER_RATE;
-
-    RosPublish (const std::string& n);
-    virtual ~RosPublish ();
-
-    virtual std::string getDocString () const;
-    void display (std::ostream& os) const;
-
-    void add (const std::string& signal, const std::string& topic);
-    void rm (const std::string& signal);
-    std::string list () const;
-    void clear ();
-
-    int& trigger (int&, int);
-
-    template <typename T>
-    void
-    sendData
-    (boost::shared_ptr
-     <realtime_tools::RealtimePublisher
-     <typename SotToRos<T>::ros_t> > publisher,
-     boost::shared_ptr<typename SotToRos<T>::signalIn_t> signal,
-     int time);
-
-    template <typename T>
-    void add (const std::string& signal, const std::string& topic);
-
-  private:
-    static const std::string docstring_;
-    ros::NodeHandle& nh_;
-    std::map<std::string, bindedSignal_t> bindedSignal_;
-    dynamicgraph::SignalTimeDependent<int,int> trigger_;
-    ros::Duration rate_;
-    ros::Time lastPublicated_;
-    boost::interprocess::interprocess_mutex mutex_;
-  };
-} // end of namespace dynamicgraph.
-
-# include "ros_publish.hxx"
-#endif //! DYNAMIC_GRAPH_ROS_PUBLISH_HH
+}  // namespace rosPublish
+}  // end of namespace command.
+
+/// \brief Publish dynamic-graph information into ROS.
+class RosPublish : public dynamicgraph::Entity {
+  DYNAMIC_GRAPH_ENTITY_DECL();
+
+ public:
+  typedef boost::function<void(int)> callback_t;
+
+  typedef boost::tuple<boost::shared_ptr<dynamicgraph::SignalBase<int> >,
+                       callback_t>
+      bindedSignal_t;
+
+  static const double ROS_JOINT_STATE_PUBLISHER_RATE;
+
+  RosPublish(const std::string& n);
+  virtual ~RosPublish();
+
+  virtual std::string getDocString() const;
+  void display(std::ostream& os) const;
+
+  void add(const std::string& signal, const std::string& topic);
+  void rm(const std::string& signal);
+  std::vector<std::string> list() const;
+  void clear();
+
+  int& trigger(int&, int);
+
+  template <typename T>
+  void sendData(
+      boost::shared_ptr<
+          realtime_tools::RealtimePublisher<typename SotToRos<T>::ros_t> >
+          publisher,
+      boost::shared_ptr<typename SotToRos<T>::signalIn_t> signal, int time);
+
+  template <typename T>
+  void add(const std::string& signal, const std::string& topic);
+
+ private:
+  static const std::string docstring_;
+  ros::NodeHandle& nh_;
+  std::map<std::string, bindedSignal_t> bindedSignal_;
+  dynamicgraph::SignalTimeDependent<int, int> trigger_;
+  ros::Duration rate_;
+  ros::Time nextPublication_;
+  boost::mutex mutex_;
+  std::ofstream aofs_;
+  struct timespec nextPublicationRT_;
+};
+}  // end of namespace dynamicgraph.
+
+#include "ros_publish.hxx"
+#endif  //! DYNAMIC_GRAPH_ROS_PUBLISH_HH

src/ros_publish.hxx

 #ifndef DYNAMIC_GRAPH_ROS_PUBLISH_HXX
-# define DYNAMIC_GRAPH_ROS_PUBLISH_HXX
-# include <vector>
-# include <std_msgs/Float64.h>
+#define DYNAMIC_GRAPH_ROS_PUBLISH_HXX
+#include <std_msgs/Float64.h>
 
-# include "dynamic_graph_bridge_msgs/Matrix.h"
-# include "dynamic_graph_bridge_msgs/Vector.h"
+#include <vector>
 
-# include "sot_to_ros.hh"
+#include "dynamic_graph_bridge_msgs/Matrix.h"
+#include "dynamic_graph_bridge_msgs/Vector.h"
+#include "sot_to_ros.hh"
 
-# include <iostream>
-
-namespace dynamicgraph
-{
-  template <>
-  inline void
-  RosPublish::sendData
-  <std::pair<sot::MatrixHomogeneous, ml::Vector> >
-  (boost::shared_ptr
-   <realtime_tools::RealtimePublisher
-   <SotToRos
-   <std::pair<sot::MatrixHomogeneous, ml::Vector> >::ros_t> > publisher,
-   boost::shared_ptr
-   <SotToRos
-   <std::pair<sot::MatrixHomogeneous, ml::Vector> >::signalIn_t> signal,
-   int time)
-  {
-    SotToRos
-      <std::pair
-      <sot::MatrixHomogeneous, ml::Vector> >::ros_t result;
-    if (publisher->trylock ())
-      {
-	publisher->msg_.child_frame_id = "/dynamic_graph/world";
-	converter (publisher->msg_, signal->access (time));
-	publisher->unlockAndPublish ();
-      }
+namespace dynamicgraph {
+template <>
+inline void RosPublish::sendData<std::pair<sot::MatrixHomogeneous, Vector> >(
+    boost::shared_ptr<realtime_tools::RealtimePublisher<
+        SotToRos<std::pair<sot::MatrixHomogeneous, Vector> >::ros_t> >
+        publisher,
+    boost::shared_ptr<
+        SotToRos<std::pair<sot::MatrixHomogeneous, Vector> >::signalIn_t>
+        signal,
+    int time) {
+  SotToRos<std::pair<sot::MatrixHomogeneous, Vector> >::ros_t result;
+  if (publisher->trylock()) {
+    publisher->msg_.child_frame_id = "/dynamic_graph/world";
+    converter(publisher->msg_, signal->access(time));
+    publisher->unlockAndPublish();
   }
+}
 
-  template <typename T>
-  void
-  RosPublish::sendData
-  (boost::shared_ptr
-   <realtime_tools::RealtimePublisher
-   <typename SotToRos<T>::ros_t> > publisher,
-   boost::shared_ptr<typename SotToRos<T>::signalIn_t> signal,
-   int time)
-  {
-    typename SotToRos<T>::ros_t result;
-    if (publisher->trylock ())
-      {
-	converter (publisher->msg_, signal->access (time));
-	publisher->unlockAndPublish ();
-      }
+template <typename T>
+void RosPublish::sendData(
+    boost::shared_ptr<
+        realtime_tools::RealtimePublisher<typename SotToRos<T>::ros_t> >
+        publisher,
+    boost::shared_ptr<typename SotToRos<T>::signalIn_t> signal, int time) {
+  typename SotToRos<T>::ros_t result;
+  if (publisher->trylock()) {
+    converter(publisher->msg_, signal->access(time));
+    publisher->unlockAndPublish();
   }
+}
 
-  template <typename T>
-  void RosPublish::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>::signalIn_t signal_t;
+template <typename T>
+void RosPublish::add(const std::string& signal, const std::string& topic) {
+  typedef typename SotToRos<T>::ros_t ros_t;
+  typedef typename SotToRos<T>::signalIn_t signal_t;
 
-    // Initialize the bindedSignal object.
-    bindedSignal_t bindedSignal;
+  // Initialize the bindedSignal object.
+  bindedSignal_t bindedSignal;
 
-    // Initialize the publisher.
-    boost::shared_ptr
-      <realtime_tools::RealtimePublisher<ros_t> >
-      pubPtr =
-      boost::make_shared<realtime_tools::RealtimePublisher<ros_t> >
-      (nh_, topic, 1);
+  // Initialize the publisher.
+  boost::shared_ptr<realtime_tools::RealtimePublisher<ros_t> > pubPtr =
+      boost::make_shared<realtime_tools::RealtimePublisher<ros_t> >(nh_, topic,
+                                                                    1);
 
-    // Initialize the signal.
-    boost::shared_ptr<signal_t> signalPtr
-      (new signal_t
-       (0,
-	MAKE_SIGNAL_STRING(name, true, SotToRos<T>::signalTypeName, signal)));
-    boost::get<0> (bindedSignal) = signalPtr;
-    SotToRos<T>::setDefault(*signalPtr);
-    signalRegistration (*boost::get<0> (bindedSignal));
+  // Initialize the signal.
+  boost::shared_ptr<signal_t> signalPtr(new signal_t(
+      0, MAKE_SIGNAL_STRING(name, true, SotToRos<T>::signalTypeName, signal)));
+  boost::get<0>(bindedSignal) = signalPtr;
+  SotToRos<T>::setDefault(*signalPtr);
+  signalRegistration(*boost::get<0>(bindedSignal));
 
-    // Initialize the callback.
-    callback_t callback = boost::bind
-      (&RosPublish::sendData<T>,
-       this,
-       pubPtr,
-       signalPtr,
-       _1);
-    boost::get<1> (bindedSignal) = callback;
+  // Initialize the callback.
+  callback_t callback =
+      boost::bind(&RosPublish::sendData<T>, this, pubPtr, signalPtr, _1);
+  boost::get<1>(bindedSignal) = callback;
 
-    bindedSignal_[signal] = bindedSignal;
-  }
+  bindedSignal_[signal] = bindedSignal;
+}
 
-} // end of namespace dynamicgraph.
+}  // end of namespace dynamicgraph.
 
-#endif //! DYNAMIC_GRAPH_ROS_PUBLISH_HXX
+#endif  //! DYNAMIC_GRAPH_ROS_PUBLISH_HXX

src/ros_queued_subscribe-python-module-py.cc

+#include <dynamic-graph/python/module.hh>
+
+#include "ros_queued_subscribe.hh"
+
+namespace dg = dynamicgraph;
+
+BOOST_PYTHON_MODULE(wrap) {
+  bp::import("dynamic_graph");
+
+  dg::python::exposeEntity<dg::RosQueuedSubscribe, bp::bases<dg::Entity>,
+                           dg::python::AddCommands>()
+      .def("clear", &dg::RosQueuedSubscribe::clear,
+           "Remove all signals reading data from a ROS topic")
+      .def("rm", &dg::RosQueuedSubscribe::rm,
+           "Remove a signal reading data from a ROS topic",
+           bp::args("signal_name"))
+      .def("list", &dg::RosQueuedSubscribe::list,
+           "List signals reading data from a ROS topic")
+      .def("listTopics", &dg::RosQueuedSubscribe::listTopics,
+           "List subscribed topics from ROS in the same order as list command")
+      .def("clearQueue", &dg::RosQueuedSubscribe::clearQueue,
+           "Empty the queue of a given signal", bp::args("signal_name"))
+      .def("queueSize", &dg::RosQueuedSubscribe::queueSize,
+           "Return the queue size of a given signal", bp::args("signal_name"))
+      .def("readQueue", &dg::RosQueuedSubscribe::readQueue,
+           "Whether signals should read values from the queues, and when.",
+           bp::args("start_time"));
+}