Making pre-commit happy.

Author: olivier-stasse

include/dynamic_graph_bridge/ros_parameter.hpp

@@ -84,7 +84,7 @@ class SotLoaderBasic : public rclcpp::Node {
   std::vector<std::string> stateVectorMap_;
 
  public:
-  SotLoaderBasic(const std::string & aNodeName=std::string("SotLoaderBasic"));
+  SotLoaderBasic(const std::string& aNodeName = std::string("SotLoaderBasic"));
   virtual ~SotLoaderBasic();
 
   // \brief Read user input to extract the path of the SoT dynamic library.
@@ -124,6 +124,9 @@ class SotLoaderBasic : public rclcpp::Node {
 
   // \brief Specify the name of the dynamic library.
   void setDynamicLibraryName(std::string& afilename);
+
+  // \brief Read the parameters of the node
+  bool parameter_server_read_robot_description();
 };
 
 #endif /* _SOT_LOADER_BASIC_HH_ */

include/dynamic_graph_bridge/sot_loader_basic.hh

@@ -5,7 +5,6 @@
 
 # Main Library
 set(${PROJECT_NAME}_HEADERS
-    ${PROJECT_SOURCE_DIR}/include/${PROJECT_NAME}/ros_parameter.hpp
     ${PROJECT_SOURCE_DIR}/include/${PROJECT_NAME}/ros_python_interpreter_client.hpp
     ${PROJECT_SOURCE_DIR}/include/${PROJECT_NAME}/ros_python_interpreter_server.hpp
     ${PROJECT_SOURCE_DIR}/src/dg_ros_mapping.hpp
@@ -16,7 +15,6 @@ set(${PROJECT_NAME}_SOURCES
     ${PROJECT_SOURCE_DIR}/src/ros_python_interpreter_client.cpp #
     ${PROJECT_SOURCE_DIR}/src/ros_python_interpreter_server.cpp #
     ${PROJECT_SOURCE_DIR}/src/ros.cpp #
-    ${PROJECT_SOURCE_DIR}/src/ros_parameter.cpp #
 )
 add_library(ros_bridge SHARED ${${PROJECT_NAME}_SOURCES}
                               ${${PROJECT_NAME}_HEADERS})
@@ -64,13 +62,13 @@ foreach(plugin ${plugins})
                                                             ${PROJECT_VERSION})
   endif(SUFFIX_SO_VERSION)
   target_link_libraries(
-     ${plugin_library_name} ${${plugin_library_name}_deps} 
-     ros_bridge
-     dynamic_graph_bridge_msgs::dynamic_graph_bridge_msgs__rosidl_typesupport_cpp
-     rclcpp::rclcpp
-     rcl_interfaces::rcl_interfaces__rosidl_typesupport_cpp
-     std_srvs::std_srvs__rosidl_typesupport_cpp
-  )
+    ${plugin_library_name}
+    ${${plugin_library_name}_deps}
+    ros_bridge
+    dynamic_graph_bridge_msgs::dynamic_graph_bridge_msgs__rosidl_typesupport_cpp
+    rclcpp::rclcpp
+    rcl_interfaces::rcl_interfaces__rosidl_typesupport_cpp
+    std_srvs::std_srvs__rosidl_typesupport_cpp)
   target_include_directories(
     ${plugin_library_name}
     PUBLIC $<INSTALL_INTERFACE:include>

src/CMakeLists.txt

@@ -13,9 +13,14 @@
 
 #include <dynamic-graph/pool.h>
 
+#include <boost/make_shared.hpp>
+#include <boost/shared_ptr.hpp>
 #include <exception>
+#include <sot/core/robot-utils.hh>
+#include <stdexcept>
 
-#include "dynamic_graph_bridge/ros_parameter.hpp"
+#include "pinocchio/multibody/model.hpp"
+#include "pinocchio/parsers/urdf.hpp"
 
 // POSIX.1-2001
 #include <dlfcn.h>
@@ -24,15 +29,15 @@ using namespace std;
 using namespace dynamicgraph::sot;
 namespace po = boost::program_options;
 
-SotLoaderBasic::SotLoaderBasic(const std::string &aNodeName)
+SotLoaderBasic::SotLoaderBasic(const std::string& aNodeName)
     : rclcpp::Node(aNodeName),
       dynamic_graph_stopped_(true),
+      sotController_(NULL),
       sotRobotControllerLibrary_(0) {
   RCLCPP_INFO(rclcpp::get_logger("dynamic_graph_bridge::"),
               "Beginning of SotLoaderBasic");
-  //  nh_ = dynamic_graph_bridge::get_ros_node("SotLoaderBasic");
   RCLCPP_INFO(rclcpp::get_logger("dynamic_graph_bridge"),
-              "End of SotLoaderBasic");  
+              "End of SotLoaderBasic");
 }
 
 SotLoaderBasic::~SotLoaderBasic() {
@@ -41,23 +46,23 @@ SotLoaderBasic::~SotLoaderBasic() {
 
 void SotLoaderBasic::initialize() {}
 
-//rclcpp::Node::SharedPtr SotLoaderBasic::returnsNodeHandle() { return nh_; }
 int SotLoaderBasic::initPublication() {
   // Prepare message to be published
   RCLCPP_INFO(rclcpp::get_logger("dynamic_graph_bridge"),
               "SotLoaderBasic::initPublication - create joint_pub");
-  
+
   joint_pub_ =
       create_publisher<sensor_msgs::msg::JointState>("joint_states", 1);
 
   RCLCPP_INFO(rclcpp::get_logger("dynamic_graph_bridge"),
-              "SotLoaderBasic::initPublication - after create joint_pub");  
+              "SotLoaderBasic::initPublication - after create joint_pub");
   return 0;
 }
 
 void SotLoaderBasic::initializeServices() {
-  RCLCPP_INFO(rclcpp::get_logger("dynamic_graph_bridge"),
-              "SotLoaderBasic::initializeServices - Ready to start dynamic graph.");
+  RCLCPP_INFO(
+      rclcpp::get_logger("dynamic_graph_bridge"),
+      "SotLoaderBasic::initializeServices - Ready to start dynamic graph.");
 
   using namespace std::placeholders;
   service_start_ = create_service<std_srvs::srv::Empty>(
@@ -74,8 +79,7 @@ void SotLoaderBasic::initializeServices() {
   RCLCPP_INFO(rclcpp::get_logger("dynamic_graph_bridge"),
               "SotLoaderBasic::initializeServices - stopped dynamic graph.");
 
-  rclcpp::Node::SharedPtr a_node_ptr(this);
-  dynamicgraph::parameter_server_read_robot_description(a_node_ptr);
+  parameter_server_read_robot_description();
 }
 
 void SotLoaderBasic::setDynamicLibraryName(std::string& afilename) {
@@ -106,8 +110,7 @@ int SotLoaderBasic::readSotVectorStateParam() {
     RCLCPP_INFO(get_logger(), "state_vector_map parameter size %ld",
                 stateVectorMap_.size());
   } catch (exception& e) {
-    std::throw_with_nested(
-        std::logic_error("Unable to call nh_->get_parameter"));
+    std::throw_with_nested(std::logic_error("Unable to call get_parameter"));
   }
 
   nbOfJoints_ = static_cast<int>(stateVectorMap_.size());
@@ -233,17 +236,19 @@ 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")));
-  const char* dlsym_error = dlerror();
-  if (dlsym_error) {
-    std::cerr << "Cannot load symbol destroy: " << dlsym_error << '\n';
-    return;
-  }
+  if (sotController_ != NULL) {
+    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';
+      return;
+    }
 
-  destroySot(sotController_);
-  sotController_ = NULL;
+    destroySot(sotController_);
+    sotController_ = NULL;
+  }
 
   /// Uncount the number of access to this library.
   try {
@@ -265,3 +270,44 @@ void SotLoaderBasic::stop_dg(
     std::shared_ptr<std_srvs::srv::Empty::Response>) {
   dynamic_graph_stopped_ = true;
 }
+
+bool SotLoaderBasic::parameter_server_read_robot_description() {
+  if (!has_parameter("robot_description")) {
+    declare_parameter("robot_description", std::string(""));
+  }
+
+  std::string robot_description;
+  std::string parameter_name("robot_description");
+  get_parameter(parameter_name, robot_description);
+  if (robot_description.empty()) {
+    RCLCPP_FATAL(rclcpp::get_logger("dynamic_graph_bridge"),
+                 "Parameter robot_description is empty");
+    return false;
+  }
+
+  std::string model_name("robot");
+
+  // Search for the robot util related to robot_name.
+  RobotUtilShrPtr aRobotUtil = getRobotUtil(model_name);
+  // If does not exist then it is created.
+  if (aRobotUtil == RefVoidRobotUtil())
+    aRobotUtil = createRobotUtil(model_name);
+
+  // If the creation is fine
+  if (aRobotUtil != RefVoidRobotUtil()) {
+    // Then set the robot model.
+    aRobotUtil->set_parameter(parameter_name, robot_description);
+    RCLCPP_INFO(
+        rclcpp::get_logger("dynamic_graph_bridge"),
+        "parameter_server_read_robot_description : Set parameter_name : %s.",
+        parameter_name.c_str());
+    // Everything went fine.
+    return true;
+  }
+  RCLCPP_FATAL(rclcpp::get_logger("dynamic_graph_bridge"),
+               "Wrong initialization of parameter_name %s",
+               parameter_name.c_str());
+
+  // Otherwise something went wrong.
+  return false;
+}

src/ros_parameter.cpp

@@ -15,33 +15,35 @@ if(BUILD_TESTING)
   set_target_properties(impl_test_sot_mock_device PROPERTIES INSTALL_RPATH
                                                              $ORIGIN)
   target_link_libraries(
-    impl_test_sot_mock_device PUBLIC sot-core::sot-core
-    dynamic-graph-python::dynamic-graph-python
-    ros_bridge
-  )
+    impl_test_sot_mock_device
+    PUBLIC sot-core::sot-core dynamic-graph-python::dynamic-graph-python
+           ros_bridge)
 
   install(
     TARGETS impl_test_sot_mock_device
     EXPORT ${TARGETS_EXPORT_NAME}
     DESTINATION ${DYNAMIC_GRAPH_PLUGINDIR})
 
-  file(MAKE_DIRECTORY
-    ${PROJECT_BINARY_DIR}/tests/dynamic_graph/ros/tests/impl_test_sot_mock_device)
-  
+  file(
+    MAKE_DIRECTORY
+    ${PROJECT_BINARY_DIR}/tests/dynamic_graph/ros/tests/impl_test_sot_mock_device
+  )
+
   dynamic_graph_python_module(
-    "${PYTHON_DIR}/ros/tests/impl_test_sot_mock_device" impl_test_sot_mock_device
-    dynamic_graph_bridge-impl_test_sot_mock_device-wrap SOURCE_PYTHON_MODULE
+    "${PYTHON_DIR}/ros/tests/impl_test_sot_mock_device"
+    impl_test_sot_mock_device
+    dynamic_graph_bridge-impl_test_sot_mock_device-wrap
+    SOURCE_PYTHON_MODULE
     "${CMAKE_CURRENT_SOURCE_DIR}/impl_test_sot_mock_device-python-module-py.cc")
 
   # Library for sot_external_interface
   add_library(impl_test_library SHARED impl_test_sot_external_interface.cpp)
-  set_target_properties(impl_test_library PROPERTIES INSTALL_RPATH
-    $ORIGIN)
+  set_target_properties(impl_test_library PROPERTIES INSTALL_RPATH $ORIGIN)
 
   target_link_libraries(impl_test_library PUBLIC impl_test_sot_mock_device
                                                  sot-core::sot-core)
   ament_target_dependencies(impl_test_library PUBLIC dynamic_graph_bridge_msgs
-    rclcpp rcl_interfaces std_srvs)
+                            rclcpp rcl_interfaces std_srvs)
 
   install(
     TARGETS impl_test_library
@@ -99,7 +101,11 @@ if(BUILD_TESTING)
       PRIVATE ${PROJECT_SOURCE_DIR}/include)
 
     # Link the dependecies to it.
+    #
     target_link_libraries(test_${test_name} ros_bridge)
+    set(current_test_name test_${test_name})
+    ament_target_dependencies(${current_test_name} dynamic_graph_bridge_msgs
+                              rclcpp rcl_interfaces std_srvs)
 
     # add some preprocessor variable
     target_compile_definitions(