Add ros_publish test + external interface.

Author: olivier-stasse

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

@@ -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> &) {

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