src/ros_subscribe-python-module-py.cc

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

src/ros_subscribe.cpp

-#include <boost/assign.hpp>
-#include <boost/bind.hpp>
-#include <boost/format.hpp>
-#include <boost/function.hpp>
-#include <boost/make_shared.hpp>
+#include "ros_subscribe.hh"
 
+#include <dynamic-graph/factory.h>
 #include <ros/ros.h>
 #include <std_msgs/Float64.h>
 #include <std_msgs/UInt32.h>
 
-#include <dynamic-graph/factory.h>
+#include <boost/assign.hpp>
+#include <boost/bind.hpp>
+#include <boost/format.hpp>
+#include <boost/function.hpp>
+#include <boost/make_shared.hpp>
 
 #include "dynamic_graph_bridge/ros_init.hh"
-#include "ros_subscribe.hh"
 
 namespace dynamicgraph {
 DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(RosSubscribe, "RosSubscribe");
 
 namespace command {
 namespace rosSubscribe {
-Clear::Clear(RosSubscribe& entity, const std::string& docstring)
-    : Command(entity, std::vector<Value::Type>(), docstring) {}
-
-Value Clear::doExecute() {
-  RosSubscribe& entity = static_cast<RosSubscribe&>(owner());
-
-  entity.clear();
-  return Value();
-}
-
-List::List(RosSubscribe& entity, const std::string& docstring)
-    : Command(entity, std::vector<Value::Type>(), docstring) {}
-
-Value List::doExecute() {
-  RosSubscribe& entity = static_cast<RosSubscribe&>(owner());
-  return Value(entity.list());
-}
-
 Add::Add(RosSubscribe& entity, const std::string& docstring)
-    : Command(entity, boost::assign::list_of(Value::STRING)(Value::STRING)(Value::STRING), docstring) {}
+    : Command(
+          entity,
+          boost::assign::list_of(Value::STRING)(Value::STRING)(Value::STRING),
+          docstring) {}
 
 Value Add::doExecute() {
   RosSubscribe& entity = static_cast<RosSubscribe&>(owner());
@@ -73,17 +58,6 @@ Value Add::doExecute() {
     throw std::runtime_error("bad type");
   return Value();
 }
-
-Rm::Rm(RosSubscribe& entity, const std::string& docstring)
-    : Command(entity, boost::assign::list_of(Value::STRING), docstring) {}
-
-Value Rm::doExecute() {
-  RosSubscribe& entity = static_cast<RosSubscribe&>(owner());
-  std::vector<Value> values = getParameterValues();
-  const std::string& signal = values[0].value();
-  entity.rm(signal);
-  return Value();
-}
 }  // namespace rosSubscribe
 }  // end of namespace command.
 
@@ -92,7 +66,8 @@ const std::string RosSubscribe::docstring_(
     "\n"
     "  Use command \"add\" to subscribe to a new signal.\n");
 
-RosSubscribe::RosSubscribe(const std::string& n) : dynamicgraph::Entity(n), nh_(rosInit(true)), bindedSignal_() {
+RosSubscribe::RosSubscribe(const std::string& n)
+    : dynamicgraph::Entity(n), nh_(rosInit(true)), bindedSignal_() {
   std::string docstring =
       "\n"
       "  Add a signal reading data from a ROS topic\n"
@@ -106,34 +81,13 @@ RosSubscribe::RosSubscribe(const std::string& n) : dynamicgraph::Entity(n), nh_(
       "    - topic:  the topic name in ROS.\n"
       "\n";
   addCommand("add", new command::rosSubscribe::Add(*this, docstring));
-  docstring =
-      "\n"
-      "  Remove a signal reading data from 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::rosSubscribe::Rm(*this, docstring));
-  docstring =
-      "\n"
-      "  Remove all signals reading data from a ROS topic\n"
-      "\n"
-      "  No input:\n"
-      "\n";
-  addCommand("clear", new command::rosSubscribe::Clear(*this, docstring));
-  docstring =
-      "\n"
-      "  List signals reading data from a ROS topic\n"
-      "\n"
-      "  No input:\n"
-      "\n";
-  addCommand("list", new command::rosSubscribe::List(*this, docstring));
 }
 
 RosSubscribe::~RosSubscribe() {}
 
-void RosSubscribe::display(std::ostream& os) const { os << CLASS_NAME << std::endl; }
+void RosSubscribe::display(std::ostream& os) const {
+  os << CLASS_NAME << std::endl;
+}
 
 void RosSubscribe::rm(const std::string& signal) {
   std::string signalTs = signal + "Timestamp";
@@ -147,13 +101,10 @@ void RosSubscribe::rm(const std::string& signal) {
   }
 }
 
-std::string RosSubscribe::list() {
-  std::string result("[");
-  for (std::map<std::string, bindedSignal_t>::const_iterator it = bindedSignal_.begin(); it != bindedSignal_.end();
-       it++) {
-    result += "'" + it->first + "',";
-  }
-  result += "]";
+std::vector<std::string> RosSubscribe::list() {
+  std::vector<std::string> result(bindedSignal_.size());
+  std::transform(bindedSignal_.begin(), bindedSignal_.end(), result.begin(),
+                 [](const auto& pair) { return pair.first; });
   return result;
 }
 

src/ros_subscribe.hh

 #ifndef DYNAMIC_GRAPH_ROS_SUBSCRIBE_HH
 #define DYNAMIC_GRAPH_ROS_SUBSCRIBE_HH
-#include <map>
-
-#include <boost/shared_ptr.hpp>
-
+#include <dynamic-graph/command.h>
 #include <dynamic-graph/entity.h>
-#include <dynamic-graph/signal-time-dependent.h>
 #include <dynamic-graph/signal-ptr.h>
-#include <dynamic-graph/command.h>
-#include <sot/core/matrix-geometry.hh>
-
+#include <dynamic-graph/signal-time-dependent.h>
 #include <ros/ros.h>
 
+#include <boost/shared_ptr.hpp>
+#include <map>
+#include <sot/core/matrix-geometry.hh>
+
 #include "converter.hh"
 #include "sot_to_ros.hh"
 
@@ -31,9 +29,6 @@ using ::dynamicgraph::command::Value;
   }
 
 ROS_SUBSCRIBE_MAKE_COMMAND(Add);
-ROS_SUBSCRIBE_MAKE_COMMAND(Clear);
-ROS_SUBSCRIBE_MAKE_COMMAND(List);
-ROS_SUBSCRIBE_MAKE_COMMAND(Rm);
 
 #undef ROS_SUBSCRIBE_MAKE_COMMAND
 
@@ -51,7 +46,8 @@ class RosSubscribe : public dynamicgraph::Entity {
   typedef boost::posix_time::ptime ptime;
 
  public:
-  typedef std::pair<boost::shared_ptr<dynamicgraph::SignalBase<int> >, boost::shared_ptr<ros::Subscriber> >
+  typedef std::pair<boost::shared_ptr<dynamicgraph::SignalBase<int> >,
+                    boost::shared_ptr<ros::Subscriber> >
       bindedSignal_t;
 
   RosSubscribe(const std::string& n);
@@ -62,21 +58,26 @@ class RosSubscribe : public dynamicgraph::Entity {
 
   void add(const std::string& signal, const std::string& topic);
   void rm(const std::string& signal);
-  std::string list();
+  std::vector<std::string> list();
   void clear();
 
   template <typename T>
   void add(const std::string& signal, const std::string& topic);
 
-  std::map<std::string, bindedSignal_t>& bindedSignal() { return bindedSignal_; }
+  std::map<std::string, bindedSignal_t>& bindedSignal() {
+    return bindedSignal_;
+  }
 
   ros::NodeHandle& nh() { return nh_; }
 
   template <typename R, typename S>
-  void callback(boost::shared_ptr<dynamicgraph::SignalPtr<S, int> > signal, const R& data);
+  void callback(boost::shared_ptr<dynamicgraph::SignalPtr<S, int> > signal,
+                const R& data);
 
   template <typename R>
-  void callbackTimestamp(boost::shared_ptr<dynamicgraph::SignalPtr<ptime, int> > signal, const R& data);
+  void callbackTimestamp(
+      boost::shared_ptr<dynamicgraph::SignalPtr<ptime, int> > signal,
+      const R& data);
 
   template <typename T>
   friend class internal::Add;

src/ros_subscribe.hxx

 #ifndef DYNAMIC_GRAPH_ROS_SUBSCRIBE_HXX
 #define DYNAMIC_GRAPH_ROS_SUBSCRIBE_HXX
-#include <vector>
-#include <boost/bind.hpp>
-#include <boost/date_time/posix_time/posix_time.hpp>
-#include <dynamic-graph/signal-caster.h>
 #include <dynamic-graph/linear-algebra.h>
 #include <dynamic-graph/signal-cast-helper.h>
+#include <dynamic-graph/signal-caster.h>
 #include <std_msgs/Float64.h>
+
+#include <boost/bind.hpp>
+#include <boost/date_time/posix_time/posix_time.hpp>
+#include <vector>
+
 #include "dynamic_graph_bridge_msgs/Matrix.h"
 #include "dynamic_graph_bridge_msgs/Vector.h"
 #include "ros_time.hh"
@@ -15,7 +17,8 @@ namespace dg = dynamicgraph;
 
 namespace dynamicgraph {
 template <typename R, typename S>
-void RosSubscribe::callback(boost::shared_ptr<dynamicgraph::SignalPtr<S, int> > signal, const R& data) {
+void RosSubscribe::callback(
+    boost::shared_ptr<dynamicgraph::SignalPtr<S, int> > signal, const R& data) {
   typedef S sot_t;
   sot_t value;
   converter(value, data);
@@ -23,7 +26,9 @@ void RosSubscribe::callback(boost::shared_ptr<dynamicgraph::SignalPtr<S, int> >
 }
 
 template <typename R>
-void RosSubscribe::callbackTimestamp(boost::shared_ptr<dynamicgraph::SignalPtr<ptime, int> > signal, const R& data) {
+void RosSubscribe::callbackTimestamp(
+    boost::shared_ptr<dynamicgraph::SignalPtr<ptime, int> > signal,
+    const R& data) {
   ptime time = rosTimeToPtime(data->header.stamp);
   signal->setConstant(time);
 }
@@ -31,7 +36,8 @@ void RosSubscribe::callbackTimestamp(boost::shared_ptr<dynamicgraph::SignalPtr<p
 namespace internal {
 template <typename T>
 struct Add {
-  void operator()(RosSubscribe& RosSubscribe, const std::string& signal, const std::string& topic) {
+  void operator()(RosSubscribe& RosSubscribe, const std::string& signal,
+                  const std::string& topic) {
     typedef typename SotToRos<T>::sot_t sot_t;
     typedef typename SotToRos<T>::ros_const_ptr_t ros_const_ptr_t;
     typedef typename SotToRos<T>::signalIn_t signal_t;
@@ -50,9 +56,12 @@ struct Add {
 
     // Initialize the subscriber.
     typedef boost::function<void(const ros_const_ptr_t& data)> callback_t;
-    callback_t callback = boost::bind(&RosSubscribe::callback<ros_const_ptr_t, sot_t>, &RosSubscribe, signal_, _1);
+    callback_t callback =
+        boost::bind(&RosSubscribe::callback<ros_const_ptr_t, sot_t>,
+                    &RosSubscribe, signal_, _1);
 
-    bindedSignal.second = boost::make_shared<ros::Subscriber>(RosSubscribe.nh().subscribe(topic, 1, callback));
+    bindedSignal.second = boost::make_shared<ros::Subscriber>(
+        RosSubscribe.nh().subscribe(topic, 1, callback));
 
     RosSubscribe.bindedSignal()[signal] = bindedSignal;
   }
@@ -60,7 +69,8 @@ struct Add {
 
 template <typename T>
 struct Add<std::pair<T, dg::Vector> > {
-  void operator()(RosSubscribe& RosSubscribe, const std::string& signal, const std::string& topic) {
+  void operator()(RosSubscribe& RosSubscribe, const std::string& signal,
+                  const std::string& topic) {
     typedef std::pair<T, dg::Vector> type_t;
 
     typedef typename SotToRos<type_t>::sot_t sot_t;
@@ -81,15 +91,19 @@ struct Add<std::pair<T, dg::Vector> > {
 
     // Initialize the publisher.
     typedef boost::function<void(const ros_const_ptr_t& data)> callback_t;
-    callback_t callback = boost::bind(&RosSubscribe::callback<ros_const_ptr_t, sot_t>, &RosSubscribe, signal_, _1);
+    callback_t callback =
+        boost::bind(&RosSubscribe::callback<ros_const_ptr_t, sot_t>,
+                    &RosSubscribe, signal_, _1);
 
-    bindedSignal.second = boost::make_shared<ros::Subscriber>(RosSubscribe.nh().subscribe(topic, 1, callback));
+    bindedSignal.second = boost::make_shared<ros::Subscriber>(
+        RosSubscribe.nh().subscribe(topic, 1, callback));
 
     RosSubscribe.bindedSignal()[signal] = bindedSignal;
 
     // Timestamp.
     typedef dynamicgraph::SignalPtr<RosSubscribe::ptime, int> signalTimestamp_t;
-    std::string signalTimestamp = (boost::format("%1%%2%") % signal % "Timestamp").str();
+    std::string signalTimestamp =
+        (boost::format("%1%%2%") % signal % "Timestamp").str();
 
     // Initialize the bindedSignal object.
     RosSubscribe::bindedSignal_t bindedSignalTimestamp;
@@ -98,7 +112,8 @@ struct Add<std::pair<T, dg::Vector> > {
     boost::format signalNameTimestamp("RosSubscribe(%1%)::%2%");
     signalNameTimestamp % RosSubscribe.name % signalTimestamp;
 
-    boost::shared_ptr<signalTimestamp_t> signalTimestamp_(new signalTimestamp_t(0, signalNameTimestamp.str()));
+    boost::shared_ptr<signalTimestamp_t> signalTimestamp_(
+        new signalTimestamp_t(0, signalNameTimestamp.str()));
 
     RosSubscribe::ptime zero(rosTimeToPtime(ros::Time(0, 0)));
     signalTimestamp_->setConstant(zero);
@@ -108,10 +123,11 @@ struct Add<std::pair<T, dg::Vector> > {
     // Initialize the publisher.
     typedef boost::function<void(const ros_const_ptr_t& data)> callback_t;
     callback_t callbackTimestamp =
-        boost::bind(&RosSubscribe::callbackTimestamp<ros_const_ptr_t>, &RosSubscribe, signalTimestamp_, _1);
+        boost::bind(&RosSubscribe::callbackTimestamp<ros_const_ptr_t>,
+                    &RosSubscribe, signalTimestamp_, _1);
 
-    bindedSignalTimestamp.second =
-        boost::make_shared<ros::Subscriber>(RosSubscribe.nh().subscribe(topic, 1, callbackTimestamp));
+    bindedSignalTimestamp.second = boost::make_shared<ros::Subscriber>(
+        RosSubscribe.nh().subscribe(topic, 1, callbackTimestamp));
 
     RosSubscribe.bindedSignal()[signalTimestamp] = bindedSignalTimestamp;
   }

src/ros_tf_listener-python-module-py.cc

+#include <dynamic-graph/python/module.hh>
+
+#include "ros_tf_listener.hh"
+
+namespace dg = dynamicgraph;
+
+BOOST_PYTHON_MODULE(wrap) {
+  bp::import("dynamic_graph");
+
+  dg::python::exposeEntity<dg::RosTfListener, bp::bases<dg::Entity>,
+                           dg::python::AddCommands>()
+      .def("add", &dg::RosTfListener::add,
+           "Add a signal containing the transform between two frames.",
+           bp::args("to_frame_name", "from_frame_name", "out_signal_name"))
+      .def("setMaximumDelay", &dg::RosTfListener::setMaximumDelay,
+           "Set the maximum time delay of the transform obtained from tf.",
+           bp::args("signal_name", "delay_seconds"));
+}

src/ros_tf_listener.cpp

-#include "dynamic_graph_bridge/ros_init.hh"
 #include "ros_tf_listener.hh"
 
 #include <dynamic-graph/factory.h>
 
+#include <pinocchio/fwd.hpp>
+
+#include "dynamic_graph_bridge/ros_init.hh"
+
 namespace dynamicgraph {
 namespace internal {
-sot::MatrixHomogeneous& TransformListenerData::getTransform(sot::MatrixHomogeneous& res, int time) {
-  static const ros::Time origin(0);
-  bool useFailback = false;
-  try {
-    transform = buffer.lookupTransform(toFrame, fromFrame, origin);
-    ros::Duration elapsed (ros::Time::now() - transform.header.stamp);
-    useFailback = elapsed > max_elapsed;
-  } catch (const tf2::TransformException& ex) {
-    std::ostringstream oss;
-    oss << "Enable to get transform at time " << time << ": " << ex.what();
-    entity->SEND_WARNING_STREAM_MSG(oss.str());
-    useFailback = true;
-  }
+sot::MatrixHomogeneous& TransformListenerData::getTransform(
+    sot::MatrixHomogeneous& res, int time) {
+  availableSig.recompute(time);
+
+  bool available = availableSig.accessCopy();
 
-  if (useFailback)
-  {
+  if (!available) {
     failbackSig.recompute(time);
     res = failbackSig.accessCopy();
     return res;
   }
 
-  const geometry_msgs::TransformStamped::_transform_type::_rotation_type&
-    quat = transform.transform.rotation;
+  const geometry_msgs::TransformStamped::_transform_type::_rotation_type& quat =
+      transform.transform.rotation;
   const geometry_msgs::TransformStamped::_transform_type::_translation_type&
-    trans = transform.transform.translation;
+      trans = transform.transform.translation;
   res.linear() = sot::Quaternion(quat.w, quat.x, quat.y, quat.z).matrix();
   res.translation() << trans.x, trans.y, trans.z;
   return res;
 }
+
+bool& TransformListenerData::isAvailable(bool& available, int time) {
+  static const ros::Time origin(0);
+  available = false;
+  ros::Duration elapsed;
+  std::string msg;
+
+  if (buffer.canTransform(toFrame, fromFrame, origin, &msg)) {
+    transform = buffer.lookupTransform(toFrame, fromFrame, origin);
+    if (transform.header.stamp == origin) {
+      // This is likely a TF2 static transform.
+      available = true;
+    } else {
+      elapsed = ros::Time::now() - transform.header.stamp;
+      available = (elapsed <= max_elapsed);
+    }
+
+    if (!available) {
+      std::ostringstream oss;
+      oss << "Use failback " << signal.getName() << " at time " << time
+          << ". Time since last update of the transform: " << elapsed;
+      entity->SEND_INFO_STREAM_MSG(oss.str());
+    }
+  } else {
+    std::ostringstream oss;
+    oss << "Unable to get transform " << signal.getName() << " at time " << time
+        << ": " << msg;
+    entity->SEND_WARNING_STREAM_MSG(oss.str());
+    available = false;
+  }
+  return available;
+}
+
 }  // namespace internal
 
 DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(RosTfListener, "RosTfListener");

src/ros_tf_listener.hh

 #ifndef DYNAMIC_GRAPH_ROS_TF_LISTENER_HH
 #define DYNAMIC_GRAPH_ROS_TF_LISTENER_HH
 
-#include <boost/bind.hpp>
-
-#include <tf2_ros/transform_listener.h>
-#include <geometry_msgs/TransformStamped.h>
-
+#include <dynamic-graph/command-bind.h>
 #include <dynamic-graph/entity.h>
-#include <dynamic-graph/signal.h>
 #include <dynamic-graph/signal-ptr.h>
-#include <dynamic-graph/command-bind.h>
+#include <dynamic-graph/signal-time-dependent.h>
+#include <geometry_msgs/TransformStamped.h>
+#include <tf2_ros/transform_listener.h>
 
+#include <boost/bind.hpp>
+#include <dynamic_graph_bridge/ros_init.hh>
 #include <sot/core/matrix-geometry.hh>
 
 namespace dynamicgraph {
@@ -18,34 +17,44 @@ class RosTfListener;
 
 namespace internal {
 struct TransformListenerData {
-  typedef Signal<sot::MatrixHomogeneous, int> signal_t;
-  typedef SignalPtr<sot::MatrixHomogeneous, int> signalIn_t;
+  typedef SignalTimeDependent<bool, int> AvailableSignal_t;
+  typedef SignalTimeDependent<sot::MatrixHomogeneous, int> MatrixSignal_t;
+  typedef SignalPtr<sot::MatrixHomogeneous, int> DefaultSignal_t;
 
   RosTfListener* entity;
   tf2_ros::Buffer& buffer;
   const std::string toFrame, fromFrame;
   geometry_msgs::TransformStamped transform;
   ros::Duration max_elapsed;
-  signal_t signal;
-  signalIn_t failbackSig;
+  AvailableSignal_t availableSig;
+  MatrixSignal_t signal;
+  DefaultSignal_t failbackSig;
 
   TransformListenerData(RosTfListener* e, tf2_ros::Buffer& b,
                         const std::string& to, const std::string& from,
                         const std::string& signame)
-      : entity(e)
-      , buffer(b)
-      , toFrame(to)
-      , fromFrame(from)
-      , max_elapsed(0.5)
-      , signal(signame)
-      , failbackSig(NULL, signame+"_failback")
-  {
+      : entity(e),
+        buffer(b),
+        toFrame(to),
+        fromFrame(from),
+        max_elapsed(0.5),
+        availableSig(signame + "_available"),
+        signal(signame),
+        failbackSig(NULL, signame + "_failback") {
     signal.setFunction(
         boost::bind(&TransformListenerData::getTransform, this, _1, _2));
-    failbackSig.setConstant (sot::MatrixHomogeneous::Identity());
+
+    availableSig.setFunction(
+        boost::bind(&TransformListenerData::isAvailable, this, _1, _2));
+    availableSig.setNeedUpdateFromAllChildren(true);
+
+    failbackSig.setConstant(sot::MatrixHomogeneous::Identity());
+    signal.addDependencies(failbackSig << availableSig);
   }
 
   sot::MatrixHomogeneous& getTransform(sot::MatrixHomogeneous& res, int time);
+
+  bool& isAvailable(bool& isAvailable, int time);
 };
 }  // namespace internal
 
@@ -55,53 +64,33 @@ class RosTfListener : public Entity {
  public:
   typedef internal::TransformListenerData TransformListenerData;
 
-  RosTfListener(const std::string& name)
-    : Entity(name)
-    , buffer()
-    , listener(buffer, rosInit(), false)
-  {
-    std::string docstring =
-        "\n"
-        "  Add a signal containing the transform between two frames.\n"
-        "\n"
-        "  Input:\n"
-        "    - to  : frame name\n"
-        "    - from: frame name,\n"
-        "    - signalName: the signal name in dynamic-graph"
-        "\n";
-    addCommand("add", command::makeCommandVoid3(*this, &RosTfListener::add, docstring));
-    docstring =
-        "\n"
-        "  Set the maximum time delay of the transform obtained from tf.\n"
-        "\n"
-        "  Input:\n"
-        "    - signalName: the signal name,\n"
-        "    - delay  : in seconds\n"
-        "\n";
-    addCommand("setMaximumDelay", command::makeCommandVoid2(*this, &RosTfListener::setMaximumDelay, docstring));
-  }
+  RosTfListener(const std::string& _name)
+      : Entity(_name), buffer(), listener(buffer, rosInit(), false) {}
 
   ~RosTfListener() {
-    for (Map_t::const_iterator _it = listenerDatas.begin(); _it != listenerDatas.end(); ++_it) delete _it->second;
+    for (const auto& pair : listenerDatas) delete pair.second;
   }
 
-  void add(const std::string& to, const std::string& from, const std::string& signame) {
+  void add(const std::string& to, const std::string& from,
+           const std::string& signame) {
     if (listenerDatas.find(signame) != listenerDatas.end())
-      throw std::invalid_argument("A signal " + signame + " already exists in RosTfListener " + getName());
+      throw std::invalid_argument("A signal " + signame +
+                                  " already exists in RosTfListener " +
+                                  getName());
 
     boost::format signalName("RosTfListener(%1%)::output(MatrixHomo)::%2%");
     signalName % getName() % signame;
 
     TransformListenerData* tld =
         new TransformListenerData(this, buffer, to, from, signalName.str());
-    signalRegistration(tld->signal << tld->failbackSig);
+    signalRegistration(tld->signal << tld->availableSig << tld->failbackSig);
     listenerDatas[signame] = tld;
   }
 
-  void setMaximumDelay(const std::string& signame, const double& max_elapsed)
-  {
+  void setMaximumDelay(const std::string& signame, const double& max_elapsed) {
     if (listenerDatas.count(signame) == 0)
-      throw std::invalid_argument("No signal " + signame + " in RosTfListener " + getName());
+      throw std::invalid_argument("No signal " + signame +
+                                  " in RosTfListener " + getName());
     listenerDatas[signame]->max_elapsed = ros::Duration(max_elapsed);
   }
 

src/ros_time-python.hh

+#include "ros_time.hh"
+
+typedef boost::mpl::vector<dynamicgraph::RosTime> entities_t;

src/ros_time.cpp

@@ -3,11 +3,12 @@
 ///
 /// Author: Florent Lamiraux
 
+#include "ros_time.hh"
+
 #include <dynamic-graph/factory.h>
-#include <dynamic-graph/signal-caster.h>
 #include <dynamic-graph/signal-cast-helper.h>
+#include <dynamic-graph/signal-caster.h>
 
-#include "ros_time.hh"
 #include "converter.hh"
 
 namespace dynamicgraph {
@@ -22,8 +23,9 @@ const std::string RosTime::docstring_(
     "  Signal \"time\" provides time as given by ros::time as\n"
     "  boost::posix_time::ptime type.\n");
 
-RosTime::RosTime(const std::string& name)
-    : Entity(name), now_("RosTime(" + name + ")::output(boost::posix_time::ptime)::time") {
+RosTime::RosTime(const std::string& _name)
+    : Entity(_name),
+      now_("RosTime(" + _name + ")::output(boost::posix_time::ptime)::time") {
   signalRegistration(now_);
   now_.setConstant(rosTimeToPtime(ros::Time::now()));
   now_.setFunction(boost::bind(&RosTime::update, this, _1, _2));

src/ros_time.hh

@@ -6,10 +6,11 @@
 #ifndef DYNAMIC_GRAPH_ROS_TIME_HH
 #define DYNAMIC_GRAPH_ROS_TIME_HH
 
+#include <dynamic-graph/entity.h>
+#include <dynamic-graph/signal.h>
 #include <ros/time.h>
+
 #include <boost/date_time/posix_time/posix_time_types.hpp>
-#include <dynamic-graph/signal.h>
-#include <dynamic-graph/entity.h>
 
 namespace dynamicgraph {
 
@@ -22,12 +23,17 @@ class RosTime : public dynamicgraph::Entity {
   virtual std::string getDocString() const;
 
  protected:
-  boost::posix_time::ptime& update(boost::posix_time::ptime& time, const int& t);
+  boost::posix_time::ptime& update(boost::posix_time::ptime& time,
+                                   const int& t);
 
  private:
   static const std::string docstring_;
 };  // class RosTime
 
+template <>
+struct signal_io<boost::posix_time::ptime>
+    : signal_io_unimplemented<boost::posix_time::ptime> {};
+
 }  // namespace dynamicgraph
 
 #endif  // DYNAMIC_GRAPH_ROS_TIME_HH

src/sot_loader.cpp

@@ -10,16 +10,14 @@
 /* -------------------------------------------------------------------------- */
 
 #include <ros/rate.h>
+
 #include <dynamic_graph_bridge/sot_loader.hh>
+
 #include "dynamic_graph_bridge/ros_init.hh"
 
 // POSIX.1-2001
 #include <dlfcn.h>
 
-#include <boost/thread/condition.hpp>
-
-boost::condition_variable cond;
-
 using namespace std;
 using namespace dynamicgraph::sot;
 namespace po = boost::program_options;
@@ -33,17 +31,11 @@ struct DataToLog {
   std::vector<double> ittimes;
 
   DataToLog(std::size_t N_)
-    : N(N_)
-    , idx(0)
-    , iter(0)
-    , iters(N, 0)
-    , times(N, 0)
-    , ittimes(N, 0)
-  {}
+      : N(N_), idx(0), iter(0), iters(N, 0), times(N, 0), ittimes(N, 0) {}
 
   void record(const double t, const double itt) {
-    iters  [idx] = iter;
-    times  [idx] = t;
+    iters[idx] = iter;
+    times[idx] = t;
     ittimes[idx] = itt;
     ++idx;
     ++iter;
@@ -57,10 +49,8 @@ struct DataToLog {
     std::ofstream aof(oss.str().c_str());
     if (aof.is_open()) {
       for (std::size_t k = 0; k < N; ++k) {
-        aof
-          << iters  [(idx + k) % N] << ' '
-          << times  [(idx + k) % N] << ' '
-          << ittimes[(idx + k) % N] << '\n';
+        aof << iters[(idx + k) % N] << ' ' << times[(idx + k) % N] << ' '
+            << ittimes[(idx + k) % N] << '\n';
       }
     }
     aof.close();
@@ -68,11 +58,11 @@ struct DataToLog {
 };
 
 void workThreadLoader(SotLoader *aSotLoader) {
-  ros::Rate rate(1000); // 1 kHz
+  ros::Rate rate(1000);  // 1 kHz
   if (ros::param::has("/sot_controller/dt")) {
     double periodd;
     ros::param::get("/sot_controller/dt", periodd);
-    rate = ros::Rate(1/periodd);
+    rate = ros::Rate(1 / periodd);
   }
   DataToLog dataToLog(5000);
 
@@ -98,7 +88,6 @@ void workThreadLoader(SotLoader *aSotLoader) {
     rate.sleep();
   }
   dataToLog.save("/tmp/geometric_simu");
-  cond.notify_all();
   ros::waitForShutdown();
 }
 
@@ -118,6 +107,10 @@ SotLoader::SotLoader()
 
   freeFlyerPose_.header.frame_id = "odom";
   freeFlyerPose_.child_frame_id = "base_link";
+  if (ros::param::get("/sot/tf_base_link", freeFlyerPose_.child_frame_id)) {
+    ROS_INFO_STREAM("Publishing " << freeFlyerPose_.child_frame_id << " wrt "
+                                  << freeFlyerPose_.header.frame_id);
+  }
 }
 
 SotLoader::~SotLoader() {
@@ -125,7 +118,9 @@ SotLoader::~SotLoader() {
   thread_.join();
 }
 
-void SotLoader::startControlLoop() { thread_ = boost::thread(workThreadLoader, this); }
+void SotLoader::startControlLoop() {
+  thread_ = boost::thread(workThreadLoader, this);
+}
 
 void SotLoader::initializeRosNode(int argc, char *argv[]) {
   SotLoaderBasic::initializeRosNode(argc, argv);
@@ -143,7 +138,8 @@ void SotLoader::fillSensors(map<string, dgs::SensorValues> &sensorsIn) {
   assert(angleControl_.size() == angleEncoder_.size());
 
   sensorsIn["joints"].setName("angle");
-  for (unsigned int i = 0; i < angleControl_.size(); i++) angleEncoder_[i] = angleControl_[i];
+  for (unsigned int i = 0; i < angleControl_.size(); i++)
+    angleEncoder_[i] = angleControl_[i];
   sensorsIn["joints"].setValues(angleEncoder_);
 }
 
@@ -152,7 +148,8 @@ void SotLoader::readControl(map<string, dgs::ControlValues> &controlValues) {
   angleControl_ = controlValues["control"].getValues();
 
   // Debug
-  std::map<std::string, dgs::ControlValues>::iterator it = controlValues.begin();
+  std::map<std::string, dgs::ControlValues>::iterator it =
+      controlValues.begin();
   sotDEBUG(30) << "ControlValues to be broadcasted:" << std::endl;
   for (; it != controlValues.end(); it++) {
     sotDEBUG(30) << it->first << ":";
@@ -165,8 +162,8 @@ void SotLoader::readControl(map<string, dgs::ControlValues> &controlValues) {
 
   // Check if the size if coherent with the robot description.
   if (angleControl_.size() != (unsigned int)nbOfJoints_) {
-    std::cerr << " angleControl_" << angleControl_.size() << " and nbOfJoints" << (unsigned int)nbOfJoints_
-              << " are different !" << std::endl;
+    std::cerr << " angleControl_" << angleControl_.size() << " and nbOfJoints"
+              << (unsigned int)nbOfJoints_ << " are different !" << std::endl;
     exit(-1);
   }
   // Publish the data.
@@ -176,7 +173,8 @@ void SotLoader::readControl(map<string, dgs::ControlValues> &controlValues) {
   }
   for (unsigned int i = 0; i < parallel_joints_to_state_vector_.size(); i++) {
     joint_state_.position[i + nbOfJoints_] =
-        coefficient_parallel_joints_[i] * angleControl_[parallel_joints_to_state_vector_[i]];
+        coefficient_parallel_joints_[i] *
+        angleControl_[parallel_joints_to_state_vector_[i]];
   }
 
   joint_pub_.publish(joint_state_);
@@ -211,8 +209,8 @@ void SotLoader::oneIteration() {
   try {
     sotController_->nominalSetSensors(sensorsIn_);
     sotController_->getControl(controlValues_);
-  } catch (std::exception &e) {
-    throw e;
+  } catch (std::exception &) {
+    throw;
   }
 
   readControl(controlValues_);

src/sot_loader_basic.cpp

@@ -9,10 +9,12 @@
 /* --- INCLUDES ------------------------------------------------------------- */
 /* -------------------------------------------------------------------------- */
 
+#include <dynamic-graph/pool.h>
+
 #include <dynamic_graph_bridge/sot_loader.hh>
-#include "dynamic_graph_bridge/ros_init.hh"
 
-#include <dynamic-graph/pool.h>
+#include "dynamic_graph_bridge/ros_init.hh"
+#include "dynamic_graph_bridge/ros_parameter.hh"
 
 // POSIX.1-2001
 #include <dlfcn.h>
@@ -21,7 +23,8 @@ using namespace std;
 using namespace dynamicgraph::sot;
 namespace po = boost::program_options;
 
-SotLoaderBasic::SotLoaderBasic() : dynamic_graph_stopped_(true), sotRobotControllerLibrary_(0) {
+SotLoaderBasic::SotLoaderBasic()
+    : dynamic_graph_stopped_(true), sotRobotControllerLibrary_(0) {
   readSotVectorStateParam();
   initPublication();
 }
@@ -35,15 +38,21 @@ int SotLoaderBasic::initPublication() {
   return 0;
 }
 
-void SotLoaderBasic::initializeRosNode(int, char* []) {
+void SotLoaderBasic::initializeRosNode(int, char*[]) {
   ROS_INFO("Ready to start dynamic graph.");
   ros::NodeHandle n;
-  service_start_ = n.advertiseService("start_dynamic_graph", &SotLoaderBasic::start_dg, this);
+  service_start_ = n.advertiseService("start_dynamic_graph",
+                                      &SotLoaderBasic::start_dg, this);
+
+  service_stop_ =
+      n.advertiseService("stop_dynamic_graph", &SotLoaderBasic::stop_dg, this);
 
-  service_stop_ = n.advertiseService("stop_dynamic_graph", &SotLoaderBasic::stop_dg, this);
+  dynamicgraph::parameter_server_read_robot_description();
 }
 
-void SotLoaderBasic::setDynamicLibraryName(std::string& afilename) { dynamicLibraryName_ = afilename; }
+void SotLoaderBasic::setDynamicLibraryName(std::string& afilename) {
+  dynamicLibraryName_ = afilename;
+}
 
 int SotLoaderBasic::readSotVectorStateParam() {
   std::map<std::string, int> from_state_name_to_state_vector;
@@ -63,12 +72,16 @@ int SotLoaderBasic::readSotVectorStateParam() {
   if (ros::param::has("/sot/joint_state_parallel")) {
     XmlRpc::XmlRpcValue joint_state_parallel;
     n.getParam("/sot/joint_state_parallel", joint_state_parallel);
-    ROS_ASSERT(joint_state_parallel.getType() == XmlRpc::XmlRpcValue::TypeStruct);
-    std::cout << "Type of joint_state_parallel:" << joint_state_parallel.getType() << std::endl;
+    ROS_ASSERT(joint_state_parallel.getType() ==
+               XmlRpc::XmlRpcValue::TypeStruct);
+    std::cout << "Type of joint_state_parallel:"
+              << joint_state_parallel.getType() << std::endl;
 
-    for (XmlRpc::XmlRpcValue::iterator it = joint_state_parallel.begin(); it != joint_state_parallel.end(); it++) {
+    for (XmlRpc::XmlRpcValue::iterator it = joint_state_parallel.begin();
+         it != joint_state_parallel.end(); it++) {
       XmlRpc::XmlRpcValue local_value = it->second;
-      std::string final_expression, map_expression = static_cast<string>(local_value);
+      std::string final_expression,
+          map_expression = static_cast<string>(local_value);
       double final_coefficient = 1.0;
       // deal with sign
       if (map_expression[0] == '-') {
@@ -78,7 +91,8 @@ int SotLoaderBasic::readSotVectorStateParam() {
         final_expression = map_expression;
 
       std::cout << it->first.c_str() << ": " << final_coefficient << std::endl;
-      from_parallel_name_to_state_vector_name[it->first.c_str()] = final_expression;
+      from_parallel_name_to_state_vector_name[it->first.c_str()] =
+          final_expression;
       coefficient_parallel_joints_.push_back(final_coefficient);
     }
     nbOfParallelJoints_ = from_parallel_name_to_state_vector_name.size();
@@ -100,10 +114,12 @@ int SotLoaderBasic::readSotVectorStateParam() {
   // and build map from parallel name to state vector
   int i = 0;
   parallel_joints_to_state_vector_.resize(nbOfParallelJoints_);
-  for (std::map<std::string, std::string>::iterator it = from_parallel_name_to_state_vector_name.begin();
+  for (std::map<std::string, std::string>::iterator it =
+           from_parallel_name_to_state_vector_name.begin();
        it != from_parallel_name_to_state_vector_name.end(); it++, i++) {
     joint_state_.name[i + nbOfJoints_] = it->first.c_str();
-    parallel_joints_to_state_vector_[i] = from_state_name_to_state_vector[it->second];
+    parallel_joints_to_state_vector_[i] =
+        from_state_name_to_state_vector[it->second];
   }
 
   return 0;
@@ -111,7 +127,8 @@ int SotLoaderBasic::readSotVectorStateParam() {
 
 int SotLoaderBasic::parseOptions(int argc, char* argv[]) {
   po::options_description desc("Allowed options");
-  desc.add_options()("help", "produce help message")("input-file", po::value<string>(), "library to load");
+  desc.add_options()("help", "produce help message")(
+      "input-file", po::value<string>(), "library to load");
 
   po::store(po::parse_command_line(argc, argv, desc), vm_);
   po::notify(vm_);
@@ -126,13 +143,13 @@ int SotLoaderBasic::parseOptions(int argc, char* argv[]) {
   } else
     dynamicLibraryName_ = vm_["input-file"].as<string>();
 
-  Initialization();
   return 0;
 }
 
 void SotLoaderBasic::Initialization() {
   // Load the SotRobotBipedController library.
-  sotRobotControllerLibrary_ = dlopen(dynamicLibraryName_.c_str(), RTLD_LAZY | RTLD_GLOBAL);
+  sotRobotControllerLibrary_ =
+      dlopen(dynamicLibraryName_.c_str(), RTLD_LAZY | RTLD_GLOBAL);
   if (!sotRobotControllerLibrary_) {
     std::cerr << "Cannot load library: " << dlerror() << '\n';
     return;
@@ -142,8 +159,9 @@ void SotLoaderBasic::Initialization() {
   dlerror();
 
   // Load the symbols.
-  createSotExternalInterface_t* createSot = reinterpret_cast<createSotExternalInterface_t*>(
-      reinterpret_cast<long>(dlsym(sotRobotControllerLibrary_, "createSotExternalInterface")));
+  createSotExternalInterface_t* createSot =
+      reinterpret_cast<createSotExternalInterface_t*>(reinterpret_cast<long>(
+          dlsym(sotRobotControllerLibrary_, "createSotExternalInterface")));
   const char* dlsym_error = dlerror();
   if (dlsym_error) {
     std::cerr << "Cannot load symbol create: " << dlsym_error << '\n';
@@ -163,8 +181,9 @@ void SotLoaderBasic::CleanUp() {
   // SignalCaster singleton could probably be destroyed.
 
   // Load the symbols.
-  destroySotExternalInterface_t* destroySot = reinterpret_cast<destroySotExternalInterface_t*>(
-      reinterpret_cast<long>(dlsym(sotRobotControllerLibrary_, "destroySotExternalInterface")));
+  destroySotExternalInterface_t* destroySot =
+      reinterpret_cast<destroySotExternalInterface_t*>(reinterpret_cast<long>(
+          dlsym(sotRobotControllerLibrary_, "destroySotExternalInterface")));
   const char* dlsym_error = dlerror();
   if (dlsym_error) {
     std::cerr << "Cannot load symbol destroy: " << dlsym_error << '\n';
@@ -178,12 +197,14 @@ void SotLoaderBasic::CleanUp() {
   dlclose(sotRobotControllerLibrary_);
 }
 
-bool SotLoaderBasic::start_dg(std_srvs::Empty::Request&, std_srvs::Empty::Response&) {
+bool SotLoaderBasic::start_dg(std_srvs::Empty::Request&,
+                              std_srvs::Empty::Response&) {
   dynamic_graph_stopped_ = false;
   return true;
 }
 
-bool SotLoaderBasic::stop_dg(std_srvs::Empty::Request&, std_srvs::Empty::Response&) {
+bool SotLoaderBasic::stop_dg(std_srvs::Empty::Request&,
+                             std_srvs::Empty::Response&) {
   dynamic_graph_stopped_ = true;
   return true;
 }

src/sot_to_ros.cpp

@@ -12,8 +12,12 @@ const char* SotToRos<Vector>::signalTypeName = "Vector";
 const char* SotToRos<specific::Vector3>::signalTypeName = "Vector3";
 const char* SotToRos<sot::MatrixHomogeneous>::signalTypeName = "MatrixHomo";
 const char* SotToRos<specific::Twist>::signalTypeName = "Twist";
-const char* SotToRos<std::pair<specific::Vector3, Vector> >::signalTypeName = "Vector3Stamped";
-const char* SotToRos<std::pair<sot::MatrixHomogeneous, Vector> >::signalTypeName = "MatrixHomo";
-const char* SotToRos<std::pair<specific::Twist, Vector> >::signalTypeName = "Twist";
+const char* SotToRos<std::pair<specific::Vector3, Vector> >::signalTypeName =
+    "Vector3Stamped";
+const char*
+    SotToRos<std::pair<sot::MatrixHomogeneous, Vector> >::signalTypeName =
+        "MatrixHomo";
+const char* SotToRos<std::pair<specific::Twist, Vector> >::signalTypeName =
+    "Twist";
 
 }  // end of namespace dynamicgraph.

src/sot_to_ros.hh

 #ifndef DYNAMIC_GRAPH_ROS_SOT_TO_ROS_HH
 #define DYNAMIC_GRAPH_ROS_SOT_TO_ROS_HH
-#include <vector>
-#include <utility>
-
-#include <boost/format.hpp>
-
+#include <dynamic-graph/linear-algebra.h>
+#include <dynamic-graph/signal-ptr.h>
+#include <dynamic-graph/signal-time-dependent.h>
 #include <std_msgs/Bool.h>
 #include <std_msgs/Float64.h>
-#include <std_msgs/UInt32.h>
 #include <std_msgs/Int32.h>
 #include <std_msgs/String.h>
+#include <std_msgs/UInt32.h>
+
+#include <boost/format.hpp>
+#include <sot/core/matrix-geometry.hh>
+#include <utility>
+#include <vector>
+
 #include "dynamic_graph_bridge_msgs/Matrix.h"
 #include "dynamic_graph_bridge_msgs/Vector.h"
-
 #include "geometry_msgs/Transform.h"
 #include "geometry_msgs/TransformStamped.h"
 #include "geometry_msgs/Twist.h"
 #include "geometry_msgs/TwistStamped.h"
 #include "geometry_msgs/Vector3Stamped.h"
 
-#include <dynamic-graph/signal-time-dependent.h>
-#include <dynamic-graph/linear-algebra.h>
-#include <dynamic-graph/signal-ptr.h>
-#include <sot/core/matrix-geometry.hh>
-
-#define MAKE_SIGNAL_STRING(NAME, IS_INPUT, OUTPUT_TYPE, SIGNAL_NAME) \
-  makeSignalString(typeid(*this).name(), NAME, IS_INPUT, OUTPUT_TYPE, SIGNAL_NAME)
+#define MAKE_SIGNAL_STRING(NAME, IS_INPUT, OUTPUT_TYPE, SIGNAL_NAME)  \
+  makeSignalString(typeid(*this).name(), NAME, IS_INPUT, OUTPUT_TYPE, \
+                   SIGNAL_NAME)
 
 namespace dynamicgraph {
 
@@ -300,10 +299,14 @@ struct SotToRos<std::pair<specific::Twist, Vector> > {
   }
 };
 
-inline std::string makeSignalString(const std::string& className, const std::string& instanceName, bool isInputSignal,
-                                    const std::string& signalType, const std::string& signalName) {
+inline std::string makeSignalString(const std::string& className,
+                                    const std::string& instanceName,
+                                    bool isInputSignal,
+                                    const std::string& signalType,
+                                    const std::string& signalName) {
   boost::format fmt("%s(%s)::%s(%s)::%s");
-  fmt % className % instanceName % (isInputSignal ? "input" : "output") % signalType % signalName;
+  fmt % className % instanceName % (isInputSignal ? "input" : "output") %
+      signalType % signalName;
   return fmt.str();
 }
 }  // end of namespace dynamicgraph.

srv/RunPythonFile.srv

-string input
----
-string result

tests/CMakeLists.txt

-IF(BUILD_PYTHON_INTERFACE)
-  # TODO: this test requires a ros master
-  #ADD_PYTHON_UNIT_TEST("py-import" "tests/test_import.py")
-ENDIF(BUILD_PYTHON_INTERFACE)
+if(BUILD_PYTHON_INTERFACE)
+  # TODO: this test requires a ros master ADD_PYTHON_UNIT_TEST("py-import"
+  # "tests/test_import.py")
+endif(BUILD_PYTHON_INTERFACE)

tests/test_import.py

@@ -2,25 +2,25 @@
 
 from dynamic_graph.ros import RosImport
 
-ri = RosImport('rosimport')
+ri = RosImport("rosimport")
 
-ri.add('double', 'doubleS', 'doubleT')
-ri.add('vector', 'vectorS', 'vectorT')
-ri.add('matrix', 'matrixS', 'matrixT')
+ri.add("double", "doubleS", "doubleT")
+ri.add("vector", "vectorS", "vectorT")
+ri.add("matrix", "matrixS", "matrixT")
 
-ri.doubleS.value = 42.
+ri.doubleS.value = 42.0
 ri.vectorS.value = (
-    42.,
-    42.,
+    42.0,
+    42.0,
 )
 ri.matrixS.value = (
     (
-        42.,
-        42.,
+        42.0,
+        42.0,
     ),
     (
-        42.,
-        42.,
+        42.0,
+        42.0,
     ),
 )
 

travis_custom/Dockerfile

-# Pull ros docker image
-FROM ros:indigo-ros-base
-
-# Prepapre sudo environment
-RUN apt-get update && \
-      apt-get -y install sudo && \
-      apt-get -y install curl
-
-RUN useradd -m docker && echo "docker:docker" | chpasswd && adduser docker sudo
-
-USER root
-ADD /travis_custom/sudoers.txt /etc/sudoers
-ADD /travis_custom/setup-opt-robotpkg.sh /home/docker/setup-opt-robotpkg.sh
-RUN chmod 440 /etc/sudoers
-
-USER docker
-CMD /bin/bash
-# Add robotpkg binary repo
-
-RUN sudo sh -c "echo \"deb [arch=amd64] http://robotpkg.openrobots.org/wip/packages/debian/pub trusty robotpkg\" >> /etc/apt/sources.list "
-RUN sudo sh -c "echo \"deb [arch=amd64] http://robotpkg.openrobots.org/packages/debian/pub trusty robotpkg\" >> /etc/apt/sources.list "
-
-RUN curl http://robotpkg.openrobots.org/packages/debian/robotpkg.key | sudo apt-key add -
-
-# Update the reference to packages
-RUN sudo apt-get update
-RUN more /etc/apt/sources.list
-#
-RUN sudo apt-get install -y g++ python2.7 python2.7-dev
-RUN sudo apt-get install -y cppcheck doxygen libboost-system-dev libboost-test-dev libboost-filesystem-dev libboost-program-options-dev libeigen3-dev libtinyxml-dev
-RUN sudo apt-get install -y ros-indigo-tf ros-indigo-tf2-bullet ros-indigo-realtime-tools 
-RUN sudo apt-get install -y robotpkg-pinocchio 
-RUN sudo apt-get install -y robotpkg-dynamic-graph-v3 robotpkg-py27-dynamic-graph-v3 robotpkg-dynamic-graph-bridge-msgs
-RUN sudo apt-get install -y robotpkg-sot-core-v3 robotpkg-py27-sot-tools-v3 robotpkg-sot-dynamic-pinocchio-v3
-RUN sudo apt-get install -y libboost-python-dev robotpkg-py27-eigenpy python2.7-dev python-numpy python-sphinx
-RUN env
-
-
-

travis_custom/setup-opt-robotpkg.sh

-#!/bin/bash
-export ROBOTPKG_BASE=/opt/openrobots
-export PATH=$PATH:$ROBOTPKG_BASE/sbin:$ROBOTPKG_BASE/bin
-export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:$ROBOTPKG_BASE/lib:$ROBOTPKG_BASE/lib/plugin:$ROBOTPKG_BASE/lib64
-export PYTHONPATH=$PYTHONPATH:$ROBOTPKG_BASE/lib/python2.7/site-packages:$ROBOTPKG_BASE/lib/python2.7/dist-packages
-export PKG_CONFIG_PATH=$PKG_CONFIG_PATH:$ROBOTPKG_BASE/lib/pkgconfig/
-export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:$ROBOTPKG_BASE/share:$ROBOTPKG_BASE/stacks
-export CMAKE_PREFIX_PATH=$CMAKE_PREFIX_PATH:$ROBOTPKG_BASE

travis_custom/sudoers.txt

-root ALL=(ALL) ALL
-docker ALL=(ALL) NOPASSWD: ALL
-Defaults    env_reset
-Defaults    secure_path="/usr/local/sbin:/usr/local/bin:/usr/sbin:/usr/bin:/sbin:/bin"