Compare revisions

c2c3026d · c2c3026d · c2c3026d · c2c3026d · c2c3026d · c2c3026d
--- a/src/ros_subscribe.hh
+++ b/src/ros_subscribe.hh
 #ifndef DYNAMIC_GRAPH_ROS_SUBSCRIBE_HH
 #define DYNAMIC_GRAPH_ROS_SUBSCRIBE_HH
-#include <map>
+#include <dynamic-graph/command.h>
-#include <boost/shared_ptr.hpp>
 #include <dynamic-graph/entity.h>
-#include <dynamic-graph/signal-time-dependent.h>
 #include <dynamic-graph/signal-ptr.h>
-#include <dynamic-graph/command.h>
+#include <dynamic-graph/signal-time-dependent.h>
-#include <sot/core/matrix-geometry.hh>
 #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"
@@ -48,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);
@@ -65,15 +64,20 @@ class RosSubscribe : public dynamicgraph::Entity {
  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;

--- a/src/ros_subscribe.hxx
+++ b/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 =
+    bindedSignalTimestamp.second = boost::make_shared<ros::Subscriber>(
-        boost::make_shared<ros::Subscriber>(RosSubscribe.nh().subscribe(topic, 1, callbackTimestamp));
+        RosSubscribe.nh().subscribe(topic, 1, callbackTimestamp));
    RosSubscribe.bindedSignal()[signalTimestamp] = bindedSignalTimestamp;
  }

--- a/src/ros_tf_listener-python-module-py.cc
+++ b/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)
+BOOST_PYTHON_MODULE(wrap) {
-{
  bp::import("dynamic_graph");
-  dg::python::exposeEntity<dg::RosTfListener, bp::bases<dg::Entity>, dg::python::AddCommands>()
+  dg::python::exposeEntity<dg::RosTfListener, bp::bases<dg::Entity>,
-    .def("add", &dg::RosTfListener::add,
+                           dg::python::AddCommands>()
-        "Add a signal containing the transform between two frames.",
+      .def("add", &dg::RosTfListener::add,
-        bp::args( "to_frame_name", "from_frame_name", "out_signal_name"))
+           "Add a signal containing the transform between two frames.",
-    .def("setMaximumDelay", &dg::RosTfListener::setMaximumDelay,
+           bp::args("to_frame_name", "from_frame_name", "out_signal_name"))
-        "Set the maximum time delay of the transform obtained from tf.",
+      .def("setMaximumDelay", &dg::RosTfListener::setMaximumDelay,
-        bp::args("signal_name", "delay_seconds"))
+           "Set the maximum time delay of the transform obtained from tf.",
-    ;
+           bp::args("signal_name", "delay_seconds"));
 }
--- a/src/ros_tf_listener.cpp
+++ b/src/ros_tf_listener.cpp
-#include <pinocchio/fwd.hpp>
-#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)
+sot::MatrixHomogeneous& TransformListenerData::getTransform(
-{
+    sot::MatrixHomogeneous& res, int time) {
  availableSig.recompute(time);
  bool available = availableSig.accessCopy();
@@ -18,17 +20,16 @@ sot::MatrixHomogeneous& TransformListenerData::getTransform(sot::MatrixHomogeneo
    return res;
  }
-  const geometry_msgs::TransformStamped::_transform_type::_rotation_type&
+  const geometry_msgs::TransformStamped::_transform_type::_rotation_type& quat =
-    quat = transform.transform.rotation;
+      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)
+bool& TransformListenerData::isAvailable(bool& available, int time) {
-{
  static const ros::Time origin(0);
  available = false;
  ros::Duration elapsed;
@@ -47,13 +48,13 @@ bool& TransformListenerData::isAvailable(bool& available, int time)
    if (!available) {
      std::ostringstream oss;
      oss << "Use failback " << signal.getName() << " at time " << time
-        << ". Time since last update of the transform: " << elapsed;
+          << ". 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 "
+    oss << "Unable to get transform " << signal.getName() << " at time " << time
-      << time << ": " << msg;
+        << ": " << msg;
    entity->SEND_WARNING_STREAM_MSG(oss.str());
    available = false;
  }

--- a/src/ros_tf_listener.hh
+++ b/src/ros_tf_listener.hh
 #ifndef DYNAMIC_GRAPH_ROS_TF_LISTENER_HH
 #define DYNAMIC_GRAPH_ROS_TF_LISTENER_HH
-#include <boost/bind.hpp>
+#include <dynamic-graph/command-bind.h>
-#include <tf2_ros/transform_listener.h>
-#include <geometry_msgs/TransformStamped.h>
 #include <dynamic-graph/entity.h>
-#include <dynamic-graph/signal-time-dependent.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 <sot/core/matrix-geometry.hh>
+#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 {
 class RosTfListener;
@@ -36,15 +33,14 @@ struct TransformListenerData {
  TransformListenerData(RosTfListener* e, tf2_ros::Buffer& b,
                        const std::string& to, const std::string& from,
                        const std::string& signame)
-      : entity(e)
+      : entity(e),
-      , buffer(b)
+        buffer(b),
-      , toFrame(to)
+        toFrame(to),
-      , fromFrame(from)
+        fromFrame(from),
-      , max_elapsed(0.5)
+        max_elapsed(0.5),
-      , availableSig(signame+"_available")
+        availableSig(signame + "_available"),
-      , signal(signame)
+        signal(signame),
-      , failbackSig(NULL, signame+"_failback")
+        failbackSig(NULL, signame + "_failback") {
-  {
    signal.setFunction(
        boost::bind(&TransformListenerData::getTransform, this, _1, _2));
@@ -52,7 +48,7 @@ struct TransformListenerData {
        boost::bind(&TransformListenerData::isAvailable, this, _1, _2));
    availableSig.setNeedUpdateFromAllChildren(true);
-    failbackSig.setConstant (sot::MatrixHomogeneous::Identity());
+    failbackSig.setConstant(sot::MatrixHomogeneous::Identity());
    signal.addDependencies(failbackSig << availableSig);
  }
@@ -69,20 +65,18 @@ class RosTfListener : public Entity {
  typedef internal::TransformListenerData TransformListenerData;
  RosTfListener(const std::string& _name)
-    : Entity(_name)
+      : Entity(_name), buffer(), listener(buffer, rosInit(), false) {}
-    , buffer()
-    , listener(buffer, rosInit(), false)
-  {}
-  ~RosTfListener()
+  ~RosTfListener() {
-  {
    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;
@@ -93,10 +87,10 @@ class RosTfListener : public Entity {
    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);
  }

--- a/src/ros_time-python.hh
+++ b/src/ros_time-python.hh
 #include "ros_time.hh"
-typedef boost::mpl::vector< dynamicgraph::RosTime > entities_t;
+typedef boost::mpl::vector<dynamicgraph::RosTime> entities_t;
--- a/src/ros_time.cpp
+++ b/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 {
@@ -23,7 +24,8 @@ const std::string RosTime::docstring_(
    "  boost::posix_time::ptime type.\n");
 RosTime::RosTime(const std::string& _name)
-    : Entity(_name), now_("RosTime(" + _name + ")::output(boost::posix_time::ptime)::time") {
+    : 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));

--- a/src/ros_time.hh
+++ b/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,13 +23,16 @@ 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> {};
+template <>
+struct signal_io<boost::posix_time::ptime>
+    : signal_io_unimplemented<boost::posix_time::ptime> {};
 }  // namespace dynamicgraph

--- a/src/sot_loader.cpp
+++ b/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_)
+      : N(N_), idx(0), iter(0), iters(N, 0), times(N, 0), ittimes(N, 0) {}
-    , idx(0)
-    , iter(0)
-    , iters(N, 0)
-    , times(N, 0)
-    , ittimes(N, 0)
-  {}
  void record(const double t, const double itt) {
-    iters  [idx] = iter;
+    iters[idx] = iter;
-    times  [idx] = t;
+    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
+        aof << iters[(idx + k) % N] << ' ' << times[(idx + k) % N] << ' '
-          << iters  [(idx + k) % N] << ' '
+            << ittimes[(idx + k) % N] << '\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,8 +107,7 @@ SotLoader::SotLoader()
  freeFlyerPose_.header.frame_id = "odom";
  freeFlyerPose_.child_frame_id = "base_link";
-  if (ros::param::get("/sot/tf_base_link",
+  if (ros::param::get("/sot/tf_base_link", freeFlyerPose_.child_frame_id)) {
-                      freeFlyerPose_.child_frame_id)) {
    ROS_INFO_STREAM("Publishing " << freeFlyerPose_.child_frame_id << " wrt "
                                  << freeFlyerPose_.header.frame_id);
  }
@@ -130,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);
@@ -148,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_);
 }
@@ -157,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 << ":";
@@ -170,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_
+    std::cerr << " angleControl_" << angleControl_.size() << " and nbOfJoints"
-              << " are different !" << std::endl;
+              << (unsigned int)nbOfJoints_ << " are different !" << std::endl;
    exit(-1);
  }
  // Publish the data.
@@ -181,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_);

--- a/src/sot_loader_basic.cpp
+++ b/src/sot_loader_basic.cpp
@@ -9,12 +9,13 @@
 /* --- INCLUDES ------------------------------------------------------------- */
 /* -------------------------------------------------------------------------- */
+#include <dynamic-graph/pool.h>
 #include <dynamic_graph_bridge/sot_loader.hh>
 #include "dynamic_graph_bridge/ros_init.hh"
 #include "dynamic_graph_bridge/ros_parameter.hh"
-#include <dynamic-graph/pool.h>
 // POSIX.1-2001
 #include <dlfcn.h>
@@ -22,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();
 }
@@ -36,17 +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;
@@ -66,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);
+    ROS_ASSERT(joint_state_parallel.getType() ==
-    std::cout << "Type of joint_state_parallel:" << joint_state_parallel.getType() << std::endl;
+               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] == '-') {
@@ -81,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();
@@ -103,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;
@@ -114,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_);
@@ -134,7 +148,8 @@ int SotLoaderBasic::parseOptions(int argc, char* argv[]) {
 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;
@@ -144,8 +159,9 @@ void SotLoaderBasic::Initialization() {
  dlerror();
  // Load the symbols.
-  createSotExternalInterface_t* createSot = reinterpret_cast<createSotExternalInterface_t*>(
+  createSotExternalInterface_t* createSot =
-      reinterpret_cast<long>(dlsym(sotRobotControllerLibrary_, "createSotExternalInterface")));
+      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';
@@ -165,8 +181,9 @@ void SotLoaderBasic::CleanUp() {
  // SignalCaster singleton could probably be destroyed.
  // Load the symbols.
-  destroySotExternalInterface_t* destroySot = reinterpret_cast<destroySotExternalInterface_t*>(
+  destroySotExternalInterface_t* destroySot =
-      reinterpret_cast<long>(dlsym(sotRobotControllerLibrary_, "destroySotExternalInterface")));
+      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';
@@ -180,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;
 }
--- a/src/sot_to_ros.cpp
+++ b/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<specific::Vector3, Vector> >::signalTypeName =
-const char* SotToRos<std::pair<sot::MatrixHomogeneous, Vector> >::signalTypeName = "MatrixHomo";
+    "Vector3Stamped";
-const char* SotToRos<std::pair<specific::Twist, Vector> >::signalTypeName = "Twist";
+const char*
+    SotToRos<std::pair<sot::MatrixHomogeneous, Vector> >::signalTypeName =
+        "MatrixHomo";
+const char* SotToRos<std::pair<specific::Twist, Vector> >::signalTypeName =
+    "Twist";
 }  // end of namespace dynamicgraph.
--- a/src/sot_to_ros.hh
+++ b/src/sot_to_ros.hh
 #ifndef DYNAMIC_GRAPH_ROS_SOT_TO_ROS_HH
 #define DYNAMIC_GRAPH_ROS_SOT_TO_ROS_HH
-#include <vector>
+#include <dynamic-graph/linear-algebra.h>
-#include <utility>
+#include <dynamic-graph/signal-ptr.h>
+#include <dynamic-graph/signal-time-dependent.h>
-#include <boost/format.hpp>
 #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>
+#define MAKE_SIGNAL_STRING(NAME, IS_INPUT, OUTPUT_TYPE, SIGNAL_NAME)  \
-#include <dynamic-graph/linear-algebra.h>
+  makeSignalString(typeid(*this).name(), NAME, IS_INPUT, OUTPUT_TYPE, \
-#include <dynamic-graph/signal-ptr.h>
+                   SIGNAL_NAME)
-#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)
 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,
+inline std::string makeSignalString(const std::string& className,
-                                    const std::string& signalType, const std::string& signalName) {
+                                    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.

--- a/tests/CMakeLists.txt
+++ b/tests/CMakeLists.txt
-IF(BUILD_PYTHON_INTERFACE)
+if(BUILD_PYTHON_INTERFACE)
-  # TODO: this test requires a ros master
+  # TODO: this test requires a ros master ADD_PYTHON_UNIT_TEST("py-import"
-  #ADD_PYTHON_UNIT_TEST("py-import" "tests/test_import.py")
+  # "tests/test_import.py")
-ENDIF(BUILD_PYTHON_INTERFACE)
+endif(BUILD_PYTHON_INTERFACE)
--- a/tests/test_import.py
+++ b/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("double", "doubleS", "doubleT")
-ri.add('vector', 'vectorS', 'vectorT')
+ri.add("vector", "vectorS", "vectorT")
-ri.add('matrix', 'matrixS', 'matrixT')
+ri.add("matrix", "matrixS", "matrixT")
-ri.doubleS.value = 42.
+ri.doubleS.value = 42.0
 ri.vectorS.value = (
-    42.,
+    42.0,
-    42.,
+    42.0,
 )
 ri.matrixS.value = (
    (
-        42.,
+        42.0,
-        42.,
+        42.0,
    ),
    (
-        42.,
+        42.0,
-        42.,
+        42.0,
    ),
 )

--- a/travis_custom/Dockerfile
+++ b/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
--- a/travis_custom/setup-opt-robotpkg.sh
+++ b/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
--- a/travis_custom/sudoers.txt
+++ b/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"
No results found