Add ros_publish test + external interface.

CMakeLists.txt

@@ -31,9 +31,9 @@ include(cmake/ros.cmake)
 compute_project_args(PROJECT_ARGS LANGUAGES CXX)
 project(${PROJECT_NAME} ${PROJECT_ARGS})
 
-if(NOT CMAKE_CXX_STANDARD)
-  set(CMAKE_CXX_STANDARD 14)
-endif()
+#if(NOT CMAKE_CXX_STANDARD)
+set(CMAKE_CXX_STANDARD 17)
+#endif()
 
 cmake_policy(SET CMP0057 NEW)
 find_package(ament_cmake REQUIRED)

tests/CMakeLists.txt

@@ -11,12 +11,23 @@ if(BUILD_TESTING)
   ament_lint_auto_find_test_dependencies()
 
   # Library for sot_external_interface
-  add_library(impl_test_sot_external_interface SHARED
-              impl_test_sot_external_interface)
-
-  target_link_libraries(impl_test_sot_external_interface
-                        PUBLIC sot-core::sot-core)
-
+  add_library(impl_test_library
+    SHARED
+    impl_test_sot_external_interface.cpp
+    impl_test_sot_mock_device.cpp)
+
+  target_include_directories(impl_test_library
+    PUBLIC include
+    )
+  target_link_libraries(impl_test_library
+    PUBLIC sot-core::sot-core
+    )
+  ament_target_dependencies(impl_test_library PUBLIC
+    dynamic_graph_bridge_msgs
+    rclcpp
+    rcl_interfaces
+    std_srvs
+    )
   # Executable for SotLoaderBasic test
   add_executable(test_sot_loader_basic test_sot_loader_basic.cpp)
   target_include_directories(
@@ -42,7 +53,7 @@ if(BUILD_TESTING)
           DESTINATION lib/${PROJECT_NAME})
 
   # Install library for tests
-  install(TARGETS impl_test_sot_external_interface DESTINATION lib)
+  install(TARGETS impl_test_library DESTINATION lib)
 
   install(DIRECTORY launch urdf DESTINATION share/${PROJECT_NAME})
 

tests/impl_test_sot_external_interface.cpp

 #include "impl_test_sot_external_interface.hh"
 
-ImplTestSotExternalInterface::ImplTestSotExternalInterface() {
+#include "dynamic_graph_bridge/ros.hpp"
+
+using namespace dynamic_graph_bridge;
+
+ImplTestSotExternalInterface::ImplTestSotExternalInterface()
+    : device_(new ImplTestSotMockDevice("RobotName")) {
+  init();
+}
+
+ImplTestSotExternalInterface::ImplTestSotExternalInterface(
+    std::string RobotName)
+    : device_(new ImplTestSotMockDevice(RobotName)) {
+  init();
+}
+
+ImplTestSotExternalInterface::ImplTestSotExternalInterface(
+    const char RobotName[])
+    : device_(new ImplTestSotMockDevice(RobotName)) {
+  init();
+}
+
+ImplTestSotExternalInterface::~ImplTestSotExternalInterface() {}
+
+void ImplTestSotExternalInterface::init() {
   std::vector<double> ctrl_vector;
   ctrl_vector.resize(2);
 
@@ -19,9 +42,20 @@ ImplTestSotExternalInterface::ImplTestSotExternalInterface() {
   for (std::vector<double>::size_type i = 0; i < 6; i++) ctrl_vector[i] = 0.0;
   ctrl_vector[6] = 0.0;
   named_base_ff_vec_.setValues(ctrl_vector);
-}
 
-ImplTestSotExternalInterface::~ImplTestSotExternalInterface() {}
+  // rosInit is called here only to initialize ros.
+  // No spinner is initialized.
+  py_interpreter_srv_ =
+      boost::shared_ptr<dynamic_graph_bridge::RosPythonInterpreterServer>(
+          new dynamic_graph_bridge::RosPythonInterpreterServer());
+
+  RosNodePtr py_inter_ptr = get_ros_node("python_interpreter");
+  // Set the control time step parameter to 0.001
+  double ts = 0.001;
+
+  py_inter_ptr->declare_parameter<double>("/sot_controller/dt", ts);
+  device_->timeStep(ts);
+}
 
 void ImplTestSotExternalInterface::setupSetSensors(
     std::map<std::string, dynamicgraph::sot::SensorValues> &) {

tests/impl_test_sot_external_interface.hh

 #ifndef _DGB_IMPL_TEST_SOT_EXTERNAL_INTEFACE_HH_
 #define _DGB_IMPL_TEST_SOT_EXTERNAL_INTEFACE_HH_
 
+#include <dynamic_graph_bridge/ros_python_interpreter_server.hpp>
 #include <iostream>
 #include <sot/core/abstract-sot-external-interface.hh>
 
+#include "impl_test_sot_mock_device.hh"
+
+namespace dynamic_graph_bridge {
 class ImplTestSotExternalInterface
     : public dynamicgraph::sot::AbstractSotExternalInterface {
  public:
   ImplTestSotExternalInterface();
+  ImplTestSotExternalInterface(const char robotName[]);
+  ImplTestSotExternalInterface(std::string robotName);
   virtual ~ImplTestSotExternalInterface();
 
   virtual void setupSetSensors(
@@ -25,12 +31,28 @@ class ImplTestSotExternalInterface
 
   virtual void setNoIntegration(void);
 
+  /// Embedded python interpreter accessible via Corba/ros
+  boost::shared_ptr<dynamic_graph_bridge::RosPythonInterpreterServer>
+      py_interpreter_srv_;
+
  protected:
   // Named ctrl vector
   dynamicgraph::sot::ControlValues named_ctrl_vec_;
 
   // Named base free flyer vector
   dynamicgraph::sot::ControlValues named_base_ff_vec_;
-};
 
+  // Update output port with the control computed from the
+  // dynamic graph.
+  void updateRobotState(std::vector<double> &anglesIn);
+
+  /// Run a python command
+  void runPython(std::ostream &file, const std::string &command,
+                 dynamicgraph::Interpreter &interpreter);
+
+  void init();
+
+  ImplTestSotMockDevice *device_;
+};
+}  // namespace dynamic_graph_bridge
 #endif

tests/impl_test_sot_mock_device.cpp

+/*
+ * Copyright 2023, LAAS, CNRS
+ *
+ * Author:Olivier Stasse
+ *
+ * This file is part of dynamic_graph_bridge.
+ * This file is under the APACHE license 2.0
+ *
+ */
+#include <fstream>
+#include <map>
+
+#if DEBUG
+#define ODEBUG(x) std::cout << x << std::endl
+#else
+#define ODEBUG(x)
+#endif
+#define ODEBUG3(x) std::cout << x << std::endl
+
+#define DBGFILE "/tmp/impl_test_sot_mock_device.txt"
+
+#if 0
+#define RESETDEBUG5()                            \
+  {                                              \
+    std::ofstream DebugFile;                     \
+    DebugFile.open(DBGFILE, std::ofstream::out); \
+    DebugFile.close();                           \
+  }
+#define ODEBUG5FULL(x)                                               \
+  {                                                                  \
+    std::ofstream DebugFile;                                         \
+    DebugFile.open(DBGFILE, std::ofstream::app);                     \
+    DebugFile << __FILE__ << ":" << __FUNCTION__ << "(#" << __LINE__ \
+              << "):" << x << std::endl;                             \
+    DebugFile.close();                                               \
+  }
+#define ODEBUG5(x)                               \
+  {                                              \
+    std::ofstream DebugFile;                     \
+    DebugFile.open(DBGFILE, std::ofstream::app); \
+    DebugFile << x << std::endl;                 \
+    DebugFile.close();                           \
+  }
+
+#else
+// Void the macro
+#define RESETDEBUG5()
+#define ODEBUG5FULL(x)
+#define ODEBUG5(x)
+#endif
+
+#include <dynamic-graph/all-commands.h>
+#include <dynamic-graph/factory.h>
+
+#include <sot/core/debug.hh>
+
+#include "impl_test_sot_mock_device.hh"
+
+using namespace std;
+
+const double ImplTestSotMockDevice::TIMESTEP_DEFAULT = 0.001;
+
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(ImplTestSotMockDevice,
+                                   "ImplTestSotMockDevice");
+
+ImplTestSotMockDevice::ImplTestSotMockDevice(std::string RobotName)
+    : dgsot::Device(RobotName),
+      previousState_(),
+      baseff_(),
+      accelerometerSOUT_("Device(" + RobotName +
+                         ")::output(vector)::accelerometer"),
+      gyrometerSOUT_("Device(" + RobotName + ")::output(vector)::gyrometer"),
+      currentsSOUT_("Device(" + RobotName + ")::output(vector)::currents"),
+      joint_anglesSOUT_("Device(" + RobotName +
+                        ")::output(vector)::joint_angles"),
+      motor_anglesSOUT_("Device(" + RobotName +
+                        ")::output(vector)::motor_angles"),
+      p_gainsSOUT_("Device(" + RobotName + ")::output(vector)::p_gains"),
+      d_gainsSOUT_("Device(" + RobotName + ")::output(vector)::d_gains"),
+      dgforces_(6),
+      accelerometer_(3),
+      gyrometer_(3) {
+  RESETDEBUG5();
+  timestep_ = TIMESTEP_DEFAULT;
+  sotDEBUGIN(25);
+
+  for (int i = 0; i < 4; ++i) {
+    withForceSignals[i] = true;
+  }
+  signalRegistration(accelerometerSOUT_
+                     << gyrometerSOUT_ << currentsSOUT_ << joint_anglesSOUT_
+                     << motor_anglesSOUT_ << p_gainsSOUT_ << d_gainsSOUT_);
+  dg::Vector data(3);
+  data.setZero();
+  accelerometerSOUT_.setConstant(data);
+  gyrometerSOUT_.setConstant(data);
+  baseff_.resize(7);
+  dg::Vector dataForces(6);
+  dataForces.setZero();
+  for (int i = 0; i < 4; i++) forcesSOUT[i]->setConstant(dataForces);
+
+  using namespace dynamicgraph::command;
+  std::string docstring;
+  /* Command increment. */
+  docstring =
+      "\n"
+      "    Integrate dynamics for time step provided as input\n"
+      "\n"
+      "      take one floating point number as input\n"
+      "\n";
+  addCommand("increment",
+             makeCommandVoid1((Device&)*this, &Device::increment, docstring));
+
+  sotDEBUGOUT(25);
+}
+
+ImplTestSotMockDevice::~ImplTestSotMockDevice() {}
+
+void ImplTestSotMockDevice::setSensorsForce(
+    map<string, dgsot::SensorValues>& SensorsIn, int t) {
+  int map_sot_2_urdf[4] = {2, 0, 3, 1};
+  sotDEBUGIN(15);
+  map<string, dgsot::SensorValues>::iterator it;
+  it = SensorsIn.find("forces");
+  if (it != SensorsIn.end()) {
+    // Implements force recollection.
+    const vector<double>& forcesIn = it->second.getValues();
+    for (int i = 0; i < 4; ++i) {
+      sotDEBUG(15) << "Force sensor " << i << std::endl;
+      int idx_sensor = map_sot_2_urdf[i];
+      for (int j = 0; j < 6; ++j) {
+        dgforces_(j) = forcesIn[idx_sensor * 6 + j];
+        sotDEBUG(15) << "Force value " << j << ":" << dgforces_(j) << std::endl;
+      }
+      forcesSOUT[i]->setConstant(dgforces_);
+      forcesSOUT[i]->setTime(t);
+    }
+  }
+  sotDEBUGIN(15);
+}
+
+void ImplTestSotMockDevice::setSensorsIMU(
+    map<string, dgsot::SensorValues>& SensorsIn, int t) {
+  map<string, dgsot::SensorValues>::iterator it;
+  // TODO: Confirm if this can be made quaternion
+  it = SensorsIn.find("attitude");
+  if (it != SensorsIn.end()) {
+    const vector<double>& attitude = it->second.getValues();
+    for (unsigned int i = 0; i < 3; ++i)
+      for (unsigned int j = 0; j < 3; ++j) pose(i, j) = attitude[i * 3 + j];
+    attitudeSOUT.setConstant(pose);
+    attitudeSOUT.setTime(t);
+  }
+
+  it = SensorsIn.find("accelerometer_0");
+  if (it != SensorsIn.end()) {
+    const vector<double>& accelerometer =
+        SensorsIn["accelerometer_0"].getValues();
+    for (std::size_t i = 0; i < 3; ++i) accelerometer_(i) = accelerometer[i];
+    accelerometerSOUT_.setConstant(accelerometer_);
+    accelerometerSOUT_.setTime(t);
+  }
+
+  it = SensorsIn.find("gyrometer_0");
+  if (it != SensorsIn.end()) {
+    const vector<double>& gyrometer = SensorsIn["gyrometer_0"].getValues();
+    for (std::size_t i = 0; i < 3; ++i) gyrometer_(i) = gyrometer[i];
+    gyrometerSOUT_.setConstant(gyrometer_);
+    gyrometerSOUT_.setTime(t);
+  }
+}
+
+void ImplTestSotMockDevice::setSensorsEncoders(
+    map<string, dgsot::SensorValues>& SensorsIn, int t) {
+  map<string, dgsot::SensorValues>::iterator it;
+
+  it = SensorsIn.find("motor-angles");
+  if (it != SensorsIn.end()) {
+    const vector<double>& anglesIn = it->second.getValues();
+    dgRobotState_.resize(anglesIn.size() + 6);
+    motor_angles_.resize(anglesIn.size());
+    for (unsigned i = 0; i < 6; ++i) dgRobotState_(i) = 0.;
+    for (unsigned i = 0; i < anglesIn.size(); ++i) {
+      dgRobotState_(i + 6) = anglesIn[i];
+      motor_angles_(i) = anglesIn[i];
+    }
+    robotState_.setConstant(dgRobotState_);
+    robotState_.setTime(t);
+    motor_anglesSOUT_.setConstant(motor_angles_);
+    motor_anglesSOUT_.setTime(t);
+  }
+
+  it = SensorsIn.find("joint-angles");
+  if (it != SensorsIn.end()) {
+    const vector<double>& joint_anglesIn = it->second.getValues();
+    joint_angles_.resize(joint_anglesIn.size());
+    for (unsigned i = 0; i < joint_anglesIn.size(); ++i)
+      joint_angles_(i) = joint_anglesIn[i];
+    joint_anglesSOUT_.setConstant(joint_angles_);
+    joint_anglesSOUT_.setTime(t);
+  }
+}
+
+void ImplTestSotMockDevice::setSensorsVelocities(
+    map<string, dgsot::SensorValues>& SensorsIn, int t) {
+  map<string, dgsot::SensorValues>::iterator it;
+
+  it = SensorsIn.find("velocities");
+  if (it != SensorsIn.end()) {
+    const vector<double>& velocitiesIn = it->second.getValues();
+    dgRobotVelocity_.resize(velocitiesIn.size() + 6);
+    for (unsigned i = 0; i < 6; ++i) dgRobotVelocity_(i) = 0.;
+    for (unsigned i = 0; i < velocitiesIn.size(); ++i) {
+      dgRobotVelocity_(i + 6) = velocitiesIn[i];
+    }
+    robotVelocity_.setConstant(dgRobotVelocity_);
+    robotVelocity_.setTime(t);
+  }
+}
+
+void ImplTestSotMockDevice::setSensorsTorquesCurrents(
+    map<string, dgsot::SensorValues>& SensorsIn, int t) {
+  map<string, dgsot::SensorValues>::iterator it;
+  it = SensorsIn.find("torques");
+  if (it != SensorsIn.end()) {
+    const std::vector<double>& torques = SensorsIn["torques"].getValues();
+    torques_.resize(torques.size());
+    for (std::size_t i = 0; i < torques.size(); ++i) torques_(i) = torques[i];
+    pseudoTorqueSOUT.setConstant(torques_);
+    pseudoTorqueSOUT.setTime(t);
+  }
+
+  it = SensorsIn.find("currents");
+  if (it != SensorsIn.end()) {
+    const std::vector<double>& currents = SensorsIn["currents"].getValues();
+    currents_.resize(currents.size());
+    for (std::size_t i = 0; i < currents.size(); ++i)
+      currents_(i) = currents[i];
+    currentsSOUT_.setConstant(currents_);
+    currentsSOUT_.setTime(t);
+  }
+}
+
+void ImplTestSotMockDevice::setSensorsGains(
+    map<string, dgsot::SensorValues>& SensorsIn, int t) {
+  map<string, dgsot::SensorValues>::iterator it;
+  it = SensorsIn.find("p_gains");
+  if (it != SensorsIn.end()) {
+    const std::vector<double>& p_gains = SensorsIn["p_gains"].getValues();
+    p_gains_.resize(p_gains.size());
+    for (std::size_t i = 0; i < p_gains.size(); ++i) p_gains_(i) = p_gains[i];
+    p_gainsSOUT_.setConstant(p_gains_);
+    p_gainsSOUT_.setTime(t);
+  }
+
+  it = SensorsIn.find("d_gains");
+  if (it != SensorsIn.end()) {
+    const std::vector<double>& d_gains = SensorsIn["d_gains"].getValues();
+    d_gains_.resize(d_gains.size());
+    for (std::size_t i = 0; i < d_gains.size(); ++i) d_gains_(i) = d_gains[i];
+    d_gainsSOUT_.setConstant(d_gains_);
+    d_gainsSOUT_.setTime(t);
+  }
+}
+
+void ImplTestSotMockDevice::setSensors(
+    map<string, dgsot::SensorValues>& SensorsIn) {
+  sotDEBUGIN(25);
+  map<string, dgsot::SensorValues>::iterator it;
+  int t = stateSOUT.getTime() + 1;
+
+  setSensorsForce(SensorsIn, t);
+  setSensorsIMU(SensorsIn, t);
+  setSensorsEncoders(SensorsIn, t);
+  setSensorsVelocities(SensorsIn, t);
+  setSensorsTorquesCurrents(SensorsIn, t);
+  setSensorsGains(SensorsIn, t);
+
+  sotDEBUGOUT(25);
+}
+
+void ImplTestSotMockDevice::setupSetSensors(
+    map<string, dgsot::SensorValues>& SensorsIn) {
+  // The first time we read the sensors, we need to copy the state of the
+  // robot into the signal device.state.
+  setSensors(SensorsIn);
+  setState(robotState_(0));
+}
+
+void ImplTestSotMockDevice::nominalSetSensors(
+    map<string, dgsot::SensorValues>& SensorsIn) {
+  setSensors(SensorsIn);
+}
+
+void ImplTestSotMockDevice::cleanupSetSensors(
+    map<string, dgsot::SensorValues>& SensorsIn) {
+  setSensors(SensorsIn);
+}
+
+void ImplTestSotMockDevice::getControl(
+    map<string, dgsot::ControlValues>& controlOut) {
+  ODEBUG5FULL("start");
+  sotDEBUGIN(25);
+  vector<double> anglesOut, velocityOut;
+  anglesOut.resize(state_.size());
+  velocityOut.resize(state_.size());
+
+  // Integrate control
+  increment(timestep_);
+  sotDEBUG(25) << "state = " << state_ << std::endl;
+  sotDEBUG(25) << "diff  = "
+               << ((previousState_.size() == state_.size())
+                       ? (state_ - previousState_)
+                       : state_)
+               << std::endl;
+  ODEBUG5FULL("state = " << state_);
+  ODEBUG5FULL("diff  = " << ((previousState_.size() == state_.size())
+                                 ? (state_ - previousState_)
+                                 : state_));
+  previousState_ = state_;
+
+  // Specify the joint values for the controller.
+  // warning: we make here the asumption that the control signal contains the
+  // velocity of the freeflyer joint. This may change in the future.
+  if ((int)anglesOut.size() != state_.size() - 6) {
+    anglesOut.resize(state_.size() - 6);
+    velocityOut.resize(state_.size() - 6);
+  }
+
+  int time = controlSIN.getTime();
+  for (unsigned int i = 6; i < state_.size(); ++i) {
+    anglesOut[i - 6] = state_(i);
+    velocityOut[i - 6] = controlSIN(time)(i);
+  }
+  // Store in "control" the joint values
+  controlOut["control"].setValues(anglesOut);
+  // Store in "velocity" the joint velocity values
+  controlOut["velocity"].setValues(velocityOut);
+  // Read zmp reference from input signal if plugged
+  zmpSIN.recompute(time + 1);
+  // Express ZMP in free flyer reference frame
+  dg::Vector zmpGlobal(4);
+  for (unsigned int i = 0; i < 3; ++i) zmpGlobal(i) = zmpSIN(time + 1)(i);
+  zmpGlobal(3) = 1.;
+  dgsot::MatrixHomogeneous inversePose;
+
+  inversePose = freeFlyerPose().inverse(Eigen::Affine);
+  dg::Vector localZmp(4);
+  localZmp = inversePose.matrix() * zmpGlobal;
+  vector<double> ZMPRef(3);
+  for (unsigned int i = 0; i < 3; ++i) ZMPRef[i] = localZmp(i);
+
+  controlOut["zmp"].setName("zmp");
+  controlOut["zmp"].setValues(ZMPRef);
+
+  // Update position of freeflyer in global frame
+  Eigen::Vector3d transq_(freeFlyerPose().translation());
+  dg::sot::VectorQuaternion qt_(freeFlyerPose().linear());
+
+  // translation
+  for (int i = 0; i < 3; i++) baseff_[i] = transq_(i);
+
+  // rotation: quaternion
+  baseff_[3] = qt_.w();
+  baseff_[4] = qt_.x();
+  baseff_[5] = qt_.y();
+  baseff_[6] = qt_.z();
+
+  controlOut["baseff"].setValues(baseff_);
+  ODEBUG5FULL("end");
+  sotDEBUGOUT(25);
+}
+
+using namespace dynamicgraph::sot;
+
+namespace dynamicgraph {
+namespace sot {
+#ifdef WIN32
+const char* DebugTrace::DEBUG_FILENAME_DEFAULT = "c:/tmp/sot-core-traces.txt";
+#else   // WIN32
+const char* DebugTrace::DEBUG_FILENAME_DEFAULT = "/tmp/sot-core-traces.txt";
+#endif  // WIN32
+
+#ifdef VP_DEBUG
+#ifdef WIN32
+std::ofstream debugfile("C:/tmp/sot-core-traces.txt",
+                        std::ios::trunc& std::ios::out);
+#else   // WIN32
+std::ofstream debugfile("/tmp/sot-core-traces.txt",
+                        std::ios::trunc& std::ios::out);
+#endif  // WIN32
+#else   // VP_DEBUG
+
+std::ofstream debugfile;
+
+#endif  // VP_DEBUG
+
+} /* namespace sot */
+} /* namespace dynamicgraph */

tests/impl_test_sot_mock_device.hh

+/*
+ * Copyright 2023, LAAS, CNRS
+ *
+ * Author:Olivier Stasse
+ *
+ * This file is part of dynamic_graph_bridge.
+ * This file is under the APACHE license 2.0
+ *
+ */
+#ifndef _Impl_Test_SoT_Mock_Device_H_
+#define _Impl_Test_SoT_Mock_Device_H_
+
+#include <dynamic-graph/entity.h>
+#include <dynamic-graph/linear-algebra.h>
+#include <dynamic-graph/signal-ptr.h>
+#include <dynamic-graph/signal.h>
+
+#include <sot/core/abstract-sot-external-interface.hh>
+#include <sot/core/device.hh>
+#include <sot/core/matrix-geometry.hh>
+
+namespace dgsot = dynamicgraph::sot;
+namespace dg = dynamicgraph;
+
+class ImplTestSotMockDevice : public dgsot::Device {
+ public:
+  static const std::string CLASS_NAME;
+  static const double TIMESTEP_DEFAULT;
+
+  virtual const std::string &getClassName() const { return CLASS_NAME; }
+
+  ImplTestSotMockDevice(std::string RobotName);
+  virtual ~ImplTestSotMockDevice();
+
+  void setSensors(std::map<std::string, dgsot::SensorValues> &sensorsIn);
+
+  void setupSetSensors(std::map<std::string, dgsot::SensorValues> &sensorsIn);
+
+  void nominalSetSensors(std::map<std::string, dgsot::SensorValues> &sensorsIn);
+
+  void cleanupSetSensors(std::map<std::string, dgsot::SensorValues> &sensorsIn);
+
+  void getControl(std::map<std::string, dgsot::ControlValues> &anglesOut);
+
+  void timeStep(double ts) { timestep_ = ts; }
+
+ protected:
+  /// \brief Previous robot configuration.
+  dg::Vector previousState_;
+
+  /// Intermediate variables to avoid allocation during control
+  std::vector<double> baseff_;
+
+  /// Accelerations measured by accelerometers
+  dynamicgraph::Signal<dg::Vector, int> accelerometerSOUT_;
+  /// Rotation velocity measured by gyrometers
+  dynamicgraph::Signal<dg::Vector, int> gyrometerSOUT_;
+  /// motor currents
+  dynamicgraph::Signal<dg::Vector, int> currentsSOUT_;
+  /// joint angles
+  dynamicgraph::Signal<dg::Vector, int> joint_anglesSOUT_;
+  /// motor angles
+  dynamicgraph::Signal<dg::Vector, int> motor_anglesSOUT_;
+
+  /// proportional and derivative position-control gains
+  dynamicgraph::Signal<dg::Vector, int> p_gainsSOUT_;
+
+  dynamicgraph::Signal<dg::Vector, int> d_gainsSOUT_;
+
+  /// Protected methods for internal variables filling
+  void setSensorsForce(std::map<std::string, dgsot::SensorValues> &SensorsIn,
+                       int t);
+  void setSensorsIMU(std::map<std::string, dgsot::SensorValues> &SensorsIn,
+                     int t);
+  void setSensorsEncoders(std::map<std::string, dgsot::SensorValues> &SensorsIn,
+                          int t);
+  void setSensorsVelocities(
+      std::map<std::string, dgsot::SensorValues> &SensorsIn, int t);
+  void setSensorsTorquesCurrents(
+      std::map<std::string, dgsot::SensorValues> &SensorsIn, int t);
+  void setSensorsGains(std::map<std::string, dgsot::SensorValues> &SensorsIn,
+                       int t);
+
+  /// Intermediate variables to avoid allocation during control
+  dg::Vector dgforces_;
+  dg::Vector dgRobotState_;     // motor-angles
+  dg::Vector joint_angles_;     // joint-angles
+  dg::Vector motor_angles_;     // motor-angles
+  dg::Vector dgRobotVelocity_;  // motor velocities
+  dg::Vector velocities_;       // motor velocities
+  dgsot::MatrixRotation pose;
+  dg::Vector accelerometer_;
+  dg::Vector gyrometer_;
+  dg::Vector torques_;
+  dg::Vector currents_;
+  dg::Vector p_gains_;
+  dg::Vector d_gains_;
+};
+#endif /* _Impl_Test_SoT_Mock_Device_H_*/

tests/python_scripts/simple_ros_publish.py

+from dynamic_graph.ros.ros_publish import RosPublish
+
+# Create a topic from the SoT to the ROS world
+ros_publish = RosPublish("rosPublish")
+
+name="control"
+ros_publish.add("vector", name + "_ros", name)
+
+device_sot_mock= ImplTestSotMockDevice("device_sot_mock")
+plug(device_sot_mock.control, ros_import.signal(name + "_ros"))

tests/test_ros_interpreter.cpp

@@ -255,3 +255,20 @@ TEST_F(TestRosInterpreter, test_call_run_script_standarderror) {
   ASSERT_TRUE(found_first_part != std::string::npos);
   ASSERT_TRUE(found_second_part != std::string::npos);
 }
+
+TEST_F(TestRosInterpreter, test_call_run_script_ros_publish) {
+  /* Setup. */
+
+  // Create the ros python interpreter.
+  RosPythonInterpreterServer rpi;
+  rpi.start_ros_service();
+
+  // Create and call the clients.
+  std::string file_name =
+      TEST_CONFIG_PATH + std::string("simple_ros_publish.py");
+  std::string result = "";
+  start_run_python_script_ros_service(file_name, result);
+
+  /* Tests. */
+  ASSERT_EQ(result, "");
+}

tests/test_sot_loader.cpp

@@ -26,14 +26,14 @@ class MockSotLoaderTest : public ::testing::Test {
       char *argv[2];
       char argv1[30] = "mocktest";
       argv[0] = argv1;
-      char argv2[60] = "--input-file=libimpl_test_sot_external_interface.so";
+      char argv2[60] = "--input-file=libimpl_test_library.so";
       argv[1] = argv2;
       parseOptions(argc, argv);
 
-      std::string finalname("libimpl_test_sot_external_interface.so");
+      std::string finalname("libimpl_test_library.so");
       EXPECT_TRUE(finalname == dynamicLibraryName_);
 
-      // Performs initialization of libimpl_test_sot_external_interface.so
+      // Performs initialization of libimpl_test_library.so
       loadController();
       EXPECT_TRUE(sotRobotControllerLibrary_ != 0);
       EXPECT_TRUE(sotController_ != nullptr);