msg/Matrix.msg

-float64[] data
-uint32 width

msg/Vector.msg

-float64[] data

package.xml

+<package format="2">
+  <name>dynamic_graph_bridge</name>
+  <version>3.4.5</version>
+  <description>
+
+     ROS bindings for dynamic graph.
+
+  </description>
+  <maintainer email="guilhem.saurel@laas.fr">guilhem.saurel@laas.fr</maintainer>
+  <author email="hpp@laas.fr">hpp@laas.fr</author>
+
+  <license>LGPL</license>
+  <url>http://ros.org/wiki/dynamic_graph_bridge</url>
+
+  <export>
+    <rosdoc config="rosdoc.yaml" />
+  </export>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>std_msgs</build_depend>
+  <build_depend>std_srvs</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>tf2_ros</build_depend>
+  <build_depend>realtime_tools</build_depend>
+  <build_depend>message_generation</build_depend>
+  <build_depend>message_runtime</build_depend>
+  <build_depend>dynamic-graph</build_depend>
+  <build_depend>dynamic-graph-python</build_depend>
+  <build_depend>sot-core</build_depend>
+  <build_depend>sot-dynamic-pinocchio</build_depend>
+  <build_depend>dynamic_graph_bridge_msgs</build_depend>
+
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>std_srvs</exec_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>tf2_ros</exec_depend>
+  <exec_depend>realtime_tools</exec_depend>
+  <exec_depend>message_generation</exec_depend>
+  <exec_depend>message_runtime</exec_depend>
+  <exec_depend>dynamic-graph</exec_depend>
+  <exec_depend>dynamic-graph-python</exec_depend>
+  <exec_depend>sot-core</exec_depend>
+  <exec_depend>sot-dynamic-pinocchio</exec_depend>
+  <exec_depend>dynamic_graph_bridge_msgs</exec_depend>
+
+</package>

pyproject.toml

+[tool.black]
+exclude = "cmake"

rosdoc.yaml

+- builder: doxygen
+  name: C++ API
+  output_dir: c++
+  file_patterns: '*.c *.cpp *.h *.cc *.hh *.dox'

scripts/robot_pose_publisher

+#!/usr/bin/env python
+#
+# Listens to TransformStamped messages and publish them to tf
+#
+
+import rospy
+
+import tf
+import sensor_msgs.msg
+
+frame = ""
+childFrame = ""
+
+
+def pose_broadcaster(msg):
+    # DEPRECATED. Robot Pose is already being published
+    translation = msg.position[0:3]
+    rotation = tf.transformations.quaternion_from_euler(
+        msg.position[3], msg.position[4], msg.position[5]
+    )
+    tfbr = tf.TransformBroadcaster()
+    tfbr.sendTransform(translation, rotation, rospy.Time.now(), childFrame, frame)
+
+
+if __name__ == "__main__":
+    rospy.init_node("robot_pose_publisher", anonymous=True)
+
+    frame = rospy.get_param("~frame", "odom")
+    childFrame = rospy.get_param("~child_frame", "base_link")
+    topic = rospy.get_param("~topic", "joint_states")
+
+    rospy.Subscriber(topic, sensor_msgs.msg.JointState, pose_broadcaster)
+    rospy.spin()

scripts/run_command

+#!/usr/bin/env python
+
+import rospy
+
+import dynamic_graph  # noqa: F401
+import dynamic_graph_bridge_msgs.srv
+import sys
+import code
+from code import InteractiveConsole
+
+import os
+
+from dynamic_graph.ros.dgcompleter import DGCompleter
+
+try:
+    import readline
+except ImportError:
+    print("Module readline not available.")
+
+# Enable a History
+HISTFILE = "%s/.pyhistory" % os.environ["HOME"]
+
+
+def savehist():
+    readline.write_history_file(HISTFILE)
+
+
+class RosShell(InteractiveConsole):
+    def __init__(self):
+        self.cache = ""
+        InteractiveConsole.__init__(self)
+
+        rospy.loginfo("waiting for service...")
+        rospy.wait_for_service("run_command")
+        self.client = rospy.ServiceProxy(
+            "run_command", dynamic_graph_bridge_msgs.srv.RunCommand, True
+        )
+        rospy.wait_for_service("run_script")
+        self.scriptClient = rospy.ServiceProxy(
+            "run_script", dynamic_graph_bridge_msgs.srv.RunPythonFile, True
+        )
+
+        readline.set_completer(DGCompleter(self.client).complete)
+        readline.parse_and_bind("tab: complete")
+
+        # Read the existing history if there is one
+        if os.path.exists(HISTFILE):
+            readline.read_history_file(HISTFILE)
+
+        # Set maximum number of items that will be written to the history file
+        readline.set_history_length(300)
+
+    import atexit
+
+    atexit.register(savehist)
+
+    def runcode(self, code, retry=True):
+        source = self.cache[:-1]
+        self.cache = ""
+        if source != "":
+            try:
+                if not self.client:
+                    print("Connection to remote server lost. Reconnecting...")
+                    self.client = rospy.ServiceProxy(
+                        "run_command", dynamic_graph_bridge_msgs.srv.RunCommand, True
+                    )
+                    if retry:
+                        print("Retrying previous command...")
+                        self.cache = source
+                        return self.runcode(code, False)
+                response = self.client(str(source))
+                if response.standardoutput != "":
+                    print(response.standardoutput[:-1])
+                if response.standarderror != "":
+                    print(response.standarderror[:-1])
+                elif response.result != "None":
+                    print(response.result)
+            except rospy.ServiceException:
+                print("Connection to remote server lost. Reconnecting...")
+                self.client = rospy.ServiceProxy(
+                    "run_command", dynamic_graph_bridge_msgs.srv.RunCommand, True
+                )
+                if retry:
+                    print("Retrying previous command...")
+                    self.cache = source
+                    self.runcode(code, False)
+
+    def runsource(self, source, filename="<input>", symbol="single"):
+        try:
+            c = code.compile_command(source, filename, symbol)
+            if c:
+                return self.runcode(c)
+            else:
+                return True
+        except (SyntaxError, OverflowError):
+            self.showsyntaxerror()
+            self.cache = ""
+            return False
+
+    def push(self, line):
+        self.cache += line + "\n"
+        return InteractiveConsole.push(self, line)
+
+
+if __name__ == "__main__":
+    import optparse
+    import os.path
+
+    rospy.init_node("run_command", argv=sys.argv)
+    sys.argv = rospy.myargv(argv=None)
+    parser = optparse.OptionParser(usage="\n\t%prog [options]")
+    (options, args) = parser.parse_args(sys.argv[1:])
+
+    sh = RosShell()
+
+    if args[:]:
+        infile = args[0]
+        if os.path.isfile(infile):
+            if not sh.client:
+                print("Connection to remote server has been lost.")
+                sys.exit(1)
+            response = sh.scriptClient(os.path.abspath(infile))
+            if not response:
+                print("Error while file parsing ")
+        else:
+            print("Provided file does not exist: %s" % (infile))
+    else:
+        sh.interact("Interacting with remote server.")

scripts/tf_publisher

+#!/usr/bin/env python
+#
+# This script looks for a particular tf transformation
+# and publish it as a TransformStamped topic.
+# This may be useful to insert tf frames into dynamic-graph
+# through dynamic_graph_bridge.
+#
+
+import logging
+
+import rospy
+
+import tf
+import geometry_msgs.msg
+
+
+def main():
+    rospy.init_node("tf_publisher", anonymous=True)
+
+    frame = rospy.get_param("~frame", "")
+    childFrame = rospy.get_param("~child_frame", "")
+    topic = rospy.get_param("~topic", "")
+    rateSeconds = rospy.get_param("~rate", 5)
+
+    if not frame or not childFrame or not topic:
+        logging.error("frame, childFrame and topic are required parameters")
+        return
+
+    rate = rospy.Rate(rateSeconds)
+    tl = tf.TransformListener()
+    pub = rospy.Publisher(topic, geometry_msgs.msg.TransformStamped)
+
+    transform = geometry_msgs.msg.TransformStamped()
+    transform.header.frame_id = frame
+    transform.child_frame_id = childFrame
+
+    ok = False
+    while not rospy.is_shutdown() and not ok:
+        try:
+            tl.waitForTransform(childFrame, frame, rospy.Time(), rospy.Duration(0.1))
+            ok = True
+        except tf.Exception:
+            logging.warning("waiting for tf transform")
+            ok = False
+
+    while not rospy.is_shutdown():
+        time = tl.getLatestCommonTime(frame, childFrame)
+        (p, q) = tl.lookupTransform(childFrame, frame, time)
+        transform.header.seq += 1
+        transform.header.stamp = time
+
+        transform.transform.translation.x = p[0]
+        transform.transform.translation.y = p[1]
+        transform.transform.translation.z = p[2]
+
+        transform.transform.rotation.x = q[0]
+        transform.transform.rotation.y = q[1]
+        transform.transform.rotation.z = q[2]
+        transform.transform.rotation.w = q[3]
+
+        pub.publish(transform)
+        rate.sleep()
+
+
+main()

setup.cfg

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

src/CMakeLists.txt

+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 "sot_to_ros.hh"
-
-namespace dynamicgraph
-{
-  template <typename D, typename S>
-  void converter (D& dst, const S& src);
-
-  template <>
-  void converter (SotToRos<double>::ros_t& dst,
-		  const SotToRos<double>::sot_t& src)
-  {
-    dst.data = src;
-  }
+#define DYNAMIC_GRAPH_ROS_CONVERTER_HH
+#include <ros/time.h>
+#include <std_msgs/Header.h>
 
-  template <>
-  void converter (SotToRos<double>::sot_t& dst,
-		  const SotToRos<double>::ros_t& src)
-  {
-    dst = src.data;
-  }
+#include <boost/date_time/date.hpp>
+#include <boost/date_time/posix_time/posix_time.hpp>
+#include <boost/static_assert.hpp>
+#include <stdexcept>
 
-  template <>
-  void converter (SotToRos<ml::Matrix>::ros_t& dst,
-		  const SotToRos<ml::Matrix>::sot_t& src)
-  {
-    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);
-  }
+#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; }
 
-  template <>
-  void converter (SotToRos<ml::Vector>::ros_t& dst,
-		  const SotToRos<ml::Vector>::sot_t& src)
-  {
-    dst.data.resize (src.size ());
-    for (unsigned i = 0; i < src.size (); ++i)
-      dst.data[i] =  src.elementAt (i);
+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);
+    }
   }
+}
+
+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); \
+  }                                                               \
+  struct e_n_d__w_i_t_h__s_e_m_i_c_o_l_o_n
+
+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, 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 = (unsigned int)diff.ticks() /
+                 (unsigned int)time_duration::rep_type::res_adjust();
+  uint32_t nsec = (unsigned int)diff.fractional_seconds();
 
-} // end of namespace dynamicgraph.
+  return ros::Time(sec, nsec);
+}
+}  // end of namespace dynamicgraph.
 
-#endif //! DYNAMIC_GRAPH_ROS_CONVERTER_HH
+#endif  //! DYNAMIC_GRAPH_ROS_CONVERTER_HH

src/dynamic_graph/ros/__init__.py

+# 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_time import RosTime
+
+
+class Ros(object):
+    device = None
+    rosPublish = None
+    rosSubscribe = None
+
+    # aliases, for retro compatibility
+    rosImport = None
+    rosExport = None
+
+    def __init__(self, robot, suffix=""):
+        self.robot = robot
+        self.rosPublish = RosPublish("rosPublish{0}".format(suffix))
+        self.rosSubscribe = RosSubscribe("rosSubscribe{0}".format(suffix))
+        self.rosTime = RosTime("rosTime{0}".format(suffix))
+
+        self.robot.device.after.addSignal("{0}.trigger".format(self.rosPublish.name))
+
+        # aliases, for retro compatibility
+        self.rosImport = self.rosPublish
+        self.rosExport = self.rosSubscribe

src/geometric_simu.cpp

+/*
+ * Copyright 2011,
+ * Olivier Stasse,
+ *
+ * CNRS
+ *
+ */
+#include <ros/console.h>
+
+#include <iostream>
+
+#define ENABLE_RT_LOG
+#include <dynamic-graph/real-time-logger.h>
+
+#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)));
+
+  ros::init(argc, argv, "sot_ros_encapsulator");
+  SotLoader aSotLoader;
+  if (aSotLoader.parseOptions(argc, argv) < 0) return -1;
+
+  // 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();
+
+  ros::waitForShutdown();
+  return 0;
+}

src/ros_export.cpp

-#include <boost/bind.hpp>
-#include <boost/format.hpp>
-#include <boost/function.hpp>
-#include <boost/make_shared.hpp>
-
-#include <ros/ros.h>
-#include <std_msgs/Float64.h>
-
-#include <dynamic-graph/factory.h>
-
-#include "ros_export.hh"
-
-namespace dynamicgraph
-{
-  DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(RosExport, "RosExport");
-
-  const char* rosInit()
-  {
-    int argc = 1;
-    char* arg0 = strdup("ros_export");
-    char* argv[] = {arg0, 0};
-    ros::init(argc, argv, "ros_export");
-    free (arg0);
-    return "dynamic_graph";
-  }
-
-  RosExport::RosExport (const std::string& n)
-    : dynamicgraph::Entity(n),
-      nh_ (rosInit ()),
-      bindedSignal_ ()
-  {
-    ros::AsyncSpinner spinner (1);
-    spinner.start ();
-  }
-
-  RosExport::~RosExport ()
-  {
-    ros::waitForShutdown();
-  }
-
-  void RosExport::display (std::ostream& os) const
-  {
-    os << CLASS_NAME << std::endl;
-  }
-
-  void RosExport::commandLine (const std::string& cmdLine,
-			       std::istringstream& cmdArgs,
-			       std::ostream& os)
-  {
-    std::string type;
-    std::string signal;
-    std::string topic;
-
-    if (cmdLine == "help")
-      {
-	os << "RosExport: "<< std::endl
-	   << "  - add <TYPE> <SIGNAL> <TOPIC>" << std::endl
-	   << "  - rm <SIGNAL>" << std::endl
-	   << "  - clear" << std::endl
-	   << "  - list" << std::endl;
-	Entity::commandLine (cmdLine, cmdArgs, os);
-      }
-    else if (cmdLine == "add")
-      {
-	cmdArgs >> type >> signal >> topic;
-	if (type == "double")
-	  ;
-	else if (type == "matrix")
-	  ;
-	else if (type == "vector")
-	  ;
-	else
-	  throw "bad type";
-      }
-    else if (cmdLine == "rm")
-      {
-	cmdArgs >> signal;
-	rm (signal);
-      }
-    else if (cmdLine == "clear")
-      clear ();
-    else if (cmdLine == "list")
-      list ();
-    else
-      Entity::commandLine (cmdLine, cmdArgs, os);
-  }
-
-  const std::string& RosExport::getClassName ()
-  {
-    return CLASS_NAME;
-  }
-
-  void RosExport::rm (const std::string& signal)
-  {
-    bindedSignal_.erase (signal);
-  }
-
-  void RosExport::list ()
-  {
-    std::cout << CLASS_NAME << std::endl;
-  }
-
-  void RosExport::clear ()
-  {
-    bindedSignal_.clear ();
-  }
-
-} // end of namespace dynamicgraph.

src/ros_export.hh

-#ifndef DYNAMIC_GRAPH_ROS_EXPORT_HH
-# define DYNAMIC_GRAPH_ROS_EXPORT_HH
-# include <iostream>
-# include <map>
-
-# include <boost/shared_ptr.hpp>
-
-# include <dynamic-graph/entity.h>
-# include <dynamic-graph/signal-time-dependent.h>
-
-# include <ros/ros.h>
-
-# include "converter.hh"
-# include "sot_to_ros.hh"
-
-namespace dynamicgraph
-{
-  class RosExport : public dynamicgraph::Entity
-  {
-  public:
-    typedef std::pair<boost::shared_ptr<dynamicgraph::SignalBase<int> >,
-		      boost::shared_ptr<ros::Subscriber> >
-      bindedSignal_t;
-
-    static const std::string CLASS_NAME;
-
-    RosExport (const std::string& n);
-    virtual ~RosExport ();
-
-    void display (std::ostream& os) const;
-    virtual void
-    commandLine (const std::string& cmdLine,
-		 std::istringstream& cmdArgs,
-		 std::ostream& os);
-
-
-    virtual const std::string& getClassName ();
-
-  private:
-    void add (const std::string& signal, const std::string& topic);
-    void rm (const std::string& signal);
-    void list ();
-    void clear ();
-
-    template <typename T>
-    void add (const std::string& signal, const std::string& topic);
-
-    template <typename T>
-    void callback (boost::shared_ptr<dynamicgraph::SignalBase<int> > signal,
-		   const T& message);
-
-    ros::NodeHandle nh_;
-    std::map<std::string, bindedSignal_t> bindedSignal_;
-  };
-} // end of namespace dynamicgraph.
-
-# include "ros_export.hxx"
-#endif //! DYNAMIC_GRAPH_ROS_EXPORT_HH

src/ros_export.hxx

-#ifndef DYNAMIC_GRAPH_ROS_EXPORT_HXX
-# define DYNAMIC_GRAPH_ROS_EXPORT_HXX
-# include <vector>
-# include <boost/bind.hpp>
-# include <jrl/mal/boost.hh>
-# include <std_msgs/Float64.h>
-# include "dynamic_graph/Matrix.h"
-# include "dynamic_graph/Vector.h"
-
-
-namespace ml = maal::boost;
-
-namespace dynamicgraph
-{
-  template <typename T>
-  void
-  RosExport::callback (boost::shared_ptr<dynamicgraph::SignalBase<int> > signal,
-		       const T& data)
-  {
-    typedef typename SotToRos<T>::sot_t sot_t;
-    sot_t value;
-    converter (value, data);
-    (*signal) (value);
-  }
-
-  template <typename T>
-  void RosExport::add (const std::string& signal, const std::string& topic)
-  {
-    typedef typename SotToRos<T>::sot_t sot_t;
-    typedef typename SotToRos<T>::ros_t ros_t;
-    typedef typename SotToRos<T>::signal_t signal_t;
-    typedef typename SotToRos<T>::callback_t callback_t;
-
-    // Initialize the bindedSignal object.
-    bindedSignal_t bindedSignal;
-
-    // Initialize the signal.
-    boost::format signalName ("RosExport(%1%)::%2%");
-    signalName % name % signal;
-
-    bindedSignal.first = boost::make_shared<signal_t>(0, signalName.str ());
-    signalRegistration (*bindedSignal.first);
-
-    // Initialize the publisher.
-    bindedSignal.second =
-      boost::make_shared<ros::Publisher>
-      (nh_.subscribe
-       (topic, 1, boost::bind (&RosExport::callback<T>,
-			       this, bindedSignal.first)));
-
-    bindedSignal_[signal] = bindedSignal;
-  }
-} // end of namespace dynamicgraph.
-
-#endif //! DYNAMIC_GRAPH_ROS_EXPORT_HXX

src/ros_import.cpp

-#include <boost/bind.hpp>
-#include <boost/format.hpp>
-#include <boost/function.hpp>
-#include <boost/make_shared.hpp>
-
-#include <ros/ros.h>
-#include <std_msgs/Float64.h>
-
-#include <dynamic-graph/factory.h>
-
-#include "ros_import.hh"
-
-namespace dynamicgraph
-{
-  DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(RosImport, "RosImport");
-
-  const char* rosInit()
-  {
-    int argc = 1;
-    char* arg0 = strdup("ros_import");
-    char* argv[] = {arg0, 0};
-    ros::init(argc, argv, "ros_import");
-    free (arg0);
-    return "dynamic_graph";
-  }
-
-  RosImport::RosImport (const std::string& n)
-    : dynamicgraph::Entity(n),
-      nh_ (rosInit ()),
-      bindedSignal_ ()
-  {
-  }
-
-  RosImport::~RosImport ()
-  {
-  }
-
-  void RosImport::display (std::ostream& os) const
-  {
-    os << CLASS_NAME << std::endl;
-  }
-
-  void RosImport::commandLine (const std::string& cmdLine,
-			       std::istringstream& cmdArgs,
-			       std::ostream& os)
-  {
-    std::string type;
-    std::string signal;
-    std::string topic;
-
-    if (cmdLine == "help")
-      {
-	os << "RosImport: "<< std::endl
-	   << "  - add <TYPE> <SIGNAL> <TOPIC>" << std::endl
-	   << "  - rm <SIGNAL>" << std::endl
-	   << "  - clear" << std::endl
-	   << "  - list" << std::endl;
-	Entity::commandLine (cmdLine, cmdArgs, os);
-      }
-    else if (cmdLine == "add")
-      {
-	cmdArgs >> type >> signal >> topic;
-	if (type == "double")
-	  add<double> (signal, topic);
-	else if (type == "matrix")
-	  add<ml::Matrix> (signal, topic);
-	else if (type == "vector")
-	  add<ml::Vector> (signal, topic);
-	else
-	  throw "bad type";
-      }
-    else if (cmdLine == "rm")
-      {
-	cmdArgs >> signal;
-	rm (signal);
-      }
-    else if (cmdLine == "clear")
-      clear ();
-    else if (cmdLine == "list")
-      list ();
-    else
-      Entity::commandLine (cmdLine, cmdArgs, os);
-  }
-
-  const std::string& RosImport::getClassName ()
-  {
-    return CLASS_NAME;
-  }
-
-  void RosImport::rm (const std::string& signal)
-  {
-    bindedSignal_.erase (signal);
-  }
-
-  void RosImport::list ()
-  {
-    std::cout << CLASS_NAME << std::endl;
-  }
-
-  void RosImport::clear ()
-  {
-    bindedSignal_.clear ();
-  }
-
-} // end of namespace dynamicgraph.

src/ros_import.hh

-#ifndef DYNAMIC_GRAPH_ROS_IMPORT_HH
-# define DYNAMIC_GRAPH_ROS_IMPORT_HH
-# include <iostream>
-# include <map>
-
-# include <boost/shared_ptr.hpp>
-
-# include <dynamic-graph/entity.h>
-# include <dynamic-graph/signal-time-dependent.h>
-
-# include <ros/ros.h>
-
-# include "converter.hh"
-# include "sot_to_ros.hh"
-
-namespace dynamicgraph
-{
-  class RosImport : public dynamicgraph::Entity
-  {
-  public:
-    typedef std::pair<boost::shared_ptr<dynamicgraph::SignalBase<int> >,
-		      boost::shared_ptr<ros::Publisher> >
-      bindedSignal_t;
-
-    static const std::string CLASS_NAME;
-
-    RosImport (const std::string& n);
-    virtual ~RosImport ();
-
-    void display (std::ostream& os) const;
-    virtual void
-    commandLine (const std::string& cmdLine,
-		 std::istringstream& cmdArgs,
-		 std::ostream& os);
-
-
-    virtual const std::string& getClassName ();
-
-  private:
-    void add (const std::string& signal, const std::string& topic);
-    void rm (const std::string& signal);
-    void list ();
-    void clear ();
-
-    template <typename T>
-    T& sendData (boost::shared_ptr<ros::Publisher> publisher,
-		 T& data, int time);
-
-    template <typename T>
-    void add (const std::string& signal, const std::string& topic);
-
-    ros::NodeHandle nh_;
-    std::map<std::string, bindedSignal_t> bindedSignal_;
-  };
-} // end of namespace dynamicgraph.
-
-# include "ros_import.hxx"
-#endif //! DYNAMIC_GRAPH_ROS_IMPORT_HH