From 74b7309f100327ba868b87c0ee61a10b5b9f9c9c Mon Sep 17 00:00:00 2001
From: Olivier Stasse <ostasse@laas.fr>
Date: Wed, 15 Mar 2023 18:32:04 +0100
Subject: [PATCH] Add ros_publish test + external interface.
---
CMakeLists.txt | 6 +-
tests/CMakeLists.txt | 25 +-
tests/impl_test_sot_external_interface.cpp | 40 ++-
tests/impl_test_sot_external_interface.hh | 24 +-
tests/impl_test_sot_mock_device.cpp | 399 +++++++++++++++++++++
tests/impl_test_sot_mock_device.hh | 99 +++++
tests/python_scripts/simple_ros_publish.py | 10 +
tests/test_ros_interpreter.cpp | 17 +
tests/test_sot_loader.cpp | 6 +-
9 files changed, 609 insertions(+), 17 deletions(-)
create mode 100644 tests/impl_test_sot_mock_device.cpp
create mode 100644 tests/impl_test_sot_mock_device.hh
create mode 100644 tests/python_scripts/simple_ros_publish.py
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 989035c..c813255 100644
--- a/CMakeLists.txt
+++ b/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)
diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt
index d1bce31..9d4a98d 100644
--- a/tests/CMakeLists.txt
+++ b/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})
diff --git a/tests/impl_test_sot_external_interface.cpp b/tests/impl_test_sot_external_interface.cpp
index 2b35ee9..e0ea9d5 100644
--- a/tests/impl_test_sot_external_interface.cpp
+++ b/tests/impl_test_sot_external_interface.cpp
@@ -1,6 +1,29 @@
#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> &) {
diff --git a/tests/impl_test_sot_external_interface.hh b/tests/impl_test_sot_external_interface.hh
index 119a844..d84ae7b 100644
--- a/tests/impl_test_sot_external_interface.hh
+++ b/tests/impl_test_sot_external_interface.hh
@@ -1,13 +1,19 @@
#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
diff --git a/tests/impl_test_sot_mock_device.cpp b/tests/impl_test_sot_mock_device.cpp
new file mode 100644
index 0000000..c79e5b9
--- /dev/null
+++ b/tests/impl_test_sot_mock_device.cpp
@@ -0,0 +1,399 @@
+/*
+ * 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 */
diff --git a/tests/impl_test_sot_mock_device.hh b/tests/impl_test_sot_mock_device.hh
new file mode 100644
index 0000000..6cc3b82
--- /dev/null
+++ b/tests/impl_test_sot_mock_device.hh
@@ -0,0 +1,99 @@
+/*
+ * 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_*/
diff --git a/tests/python_scripts/simple_ros_publish.py b/tests/python_scripts/simple_ros_publish.py
new file mode 100644
index 0000000..93768af
--- /dev/null
+++ b/tests/python_scripts/simple_ros_publish.py
@@ -0,0 +1,10 @@
+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"))
diff --git a/tests/test_ros_interpreter.cpp b/tests/test_ros_interpreter.cpp
index 11b06e9..0dfc919 100644
--- a/tests/test_ros_interpreter.cpp
+++ b/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, "");
+}
diff --git a/tests/test_sot_loader.cpp b/tests/test_sot_loader.cpp
index 1dfe8fd..59ca291 100644
--- a/tests/test_sot_loader.cpp
+++ b/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);
--
GitLab