Cleaning

Author: olivier-stasse

CMakeLists.txt

@@ -24,8 +24,6 @@ set(CATKIN_ENABLE_TESTING OFF)
 # JRL-cmakemodule setup
 include(cmake/base.cmake)
 include(cmake/boost.cmake)
-include(cmake/python.cmake)
-include(cmake/ros.cmake)
 
 # Project definition
 compute_project_args(PROJECT_ARGS LANGUAGES CXX)
@@ -44,13 +42,13 @@ find_package(std_srvs REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(tf2_ros REQUIRED)
+find_package(urdfdom REQUIRED)
 
 add_project_dependency(Boost REQUIRED COMPONENTS program_options)
 add_project_dependency(dynamic_graph_bridge_msgs 0.3.0 REQUIRED)
 add_project_dependency(sot-core REQUIRED)
 
 if(BUILD_PYTHON_INTERFACE)
-  findpython()
   search_for_boost_python()
   string(REGEX REPLACE "-" "_" PY_NAME ${PROJECT_NAME})
   add_project_dependency(dynamic-graph-python 4.0.0 REQUIRED)

include/dynamic_graph_bridge/ros.hpp

@@ -9,6 +9,8 @@
 
 #include <memory>
 
+#include <boost/chrono.hpp>
+
 // ROS includes
 #include "dynamic_graph_bridge_msgs/srv/run_python_command.hpp"
 #include "dynamic_graph_bridge_msgs/srv/run_python_file.hpp"

include/dynamic_graph_bridge/sot_loader_basic.hh

@@ -41,7 +41,7 @@
 namespace po = boost::program_options;
 namespace dgs = dynamicgraph::sot;
 
-class SotLoaderBasic {
+class SotLoaderBasic : public rclcpp::Node {
  protected:
   // Dynamic graph is stopped.
   bool dynamic_graph_stopped_;
@@ -73,9 +73,6 @@ class SotLoaderBasic {
   // Joint state publication.
   rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr joint_pub_;
 
-  // Node reference
-  rclcpp::Node::SharedPtr nh_;
-
   // Joint state to be published.
   sensor_msgs::msg::JointState joint_state_;
 
@@ -87,7 +84,7 @@ class SotLoaderBasic {
   std::vector<std::string> stateVectorMap_;
 
  public:
-  SotLoaderBasic();
+  SotLoaderBasic(const std::string & aNodeName=std::string("SotLoaderBasic"));
   virtual ~SotLoaderBasic();
 
   // \brief Read user input to extract the path of the SoT dynamic library.

src/CMakeLists.txt

@@ -38,7 +38,9 @@ ament_target_dependencies(
   geometry_msgs
   sensor_msgs
   tf2_ros
-  rcutils)
+  rcutils
+  urdfdom)
+
 if(SUFFIX_SO_VERSION)
   set_target_properties(ros_bridge PROPERTIES SOVERSION ${PROJECT_VERSION})
 endif(SUFFIX_SO_VERSION)
@@ -62,9 +64,12 @@ foreach(plugin ${plugins})
                                                             ${PROJECT_VERSION})
   endif(SUFFIX_SO_VERSION)
   target_link_libraries(
-    ${plugin_library_name} ${${plugin_library_name}_deps} ${catkin_LIBRARIES}
-    ros_bridge
-    dynamic_graph_bridge_msgs::dynamic_graph_bridge_msgs__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}

src/dg_ros_mapping.cpp

@@ -66,22 +66,25 @@ DG_TO_ROS_IMPL(DgRosMappingMatrix) {
   // For simplicity and easiness of maintenance we implement a slower version
   // of the copy here.
   ros_dst.width = static_cast<unsigned int>(dg_src.rows());
-  ros_dst.data.resize(dg_src.size());
-  for (int row = 0; row < dg_src.rows(); ++row) {
+  ros_dst.data.resize(static_cast<std::size_t>(dg_src.size()));
+  for (int row = 0 ; row < dg_src.rows(); ++row) {
     for (int col = 0; col < dg_src.cols(); ++col) {
-      ros_dst.data[row * ros_dst.width + col] = dg_src(row, col);
+      ros_dst.data[static_cast<unsigned int>(row) * ros_dst.width +
+                   static_cast<unsigned int>(col)] =
+          dg_src(row, col);
     }
   }
 }
 ROS_TO_DG_IMPL(DgRosMappingMatrix) {
   // For simplicity and easiness of maintenance we implement a slower version
   // of the copy here.
-  int rows = ros_src.width;
+  int rows = static_cast<int>(ros_src.width);
   int cols = static_cast<int>(ros_src.data.size()) / static_cast<int>(rows);
   dg_dst.resize(rows, cols);
   for (int row = 0; row < dg_dst.rows(); ++row) {
     for (int col = 0; col < dg_dst.cols(); ++col) {
-      dg_dst(row, col) = ros_src.data[row * rows + col];
+      dg_dst(row, col) =
+          ros_src.data[static_cast<std::vector<double>::size_type>(row * rows + col)];
     }
   }
 }

src/dg_ros_mapping.hpp

@@ -8,11 +8,15 @@
  */
 #pragma once
 
+#include "fwd.hpp"
+
 // Standard includes
 #include <sstream>
 #include <utility>
 #include <vector>
 
+#include <boost/chrono.hpp>
+
 // Dynamic Graph types.
 #include <dynamic-graph/linear-algebra.h>
 #include <dynamic-graph/signal-ptr.h>
@@ -107,7 +111,7 @@ class DgRosMapping {
   /** @brief Output signal type. */
   typedef dg::SignalTimeDependent<dg_t, dg::sigtime_t> signal_out_t;
   /** @brief Output signal type. */
-  typedef dg::SignalTimeDependent<timestamp_t, dg::sigtime_t>
+  typedef dg::SignalTimeDependent<dynamic_graph_bridge::timestamp_t, dg::sigtime_t>
       signal_timestamp_out_t;
   /** @brief Input signal type. */
   typedef dg::SignalPtr<dg_t, dg::sigtime_t> signal_in_t;
@@ -126,7 +130,7 @@ class DgRosMapping {
    */
   template <typename SignalTypePtr>
   static void set_default(SignalTypePtr s) {
-    s->setConstant(DgRosMapping<RosType, DgType>::default_value);
+    s->setConstant(default_value);
   }
 
   /**
@@ -135,17 +139,17 @@ class DgRosMapping {
    * @param s
    */
   static void set_default(dg_t& d) {
-    d = DgRosMapping<RosType, DgType>::default_value;
+    d = default_value;
   }
 
   /**
-   * @brief Convert ROS time to std::chrono.
+   * @brief Convert ROS time to boost::chrono.
    *
    * @param ros_time
    * @return timestamp_t
    */
   static timestamp_t from_ros_time(rclcpp::Time ros_time) {
-    return epoch_time() + std::chrono::nanoseconds(ros_time.nanoseconds());
+    return epoch_time() + boost::chrono::nanoseconds(ros_time.nanoseconds());
   }
 
   /**
@@ -154,7 +158,7 @@ class DgRosMapping {
    * @return timestamp_t
    */
   static timestamp_t epoch_time() {
-    return std::chrono::time_point<std::chrono::high_resolution_clock>{};
+    return boost::chrono::time_point<boost::chrono::high_resolution_clock>{};
   }
 
   /**

src/fwd.hpp

@@ -12,14 +12,16 @@
 
 #include <chrono>
 #include <ostream>
+#include <iostream>
+#include <boost/chrono.hpp>
 
 namespace dynamic_graph_bridge {
 /** @brief Time stamp type. */
-typedef std::chrono::time_point<std::chrono::high_resolution_clock> timestamp_t;
+typedef boost::chrono::time_point<boost::chrono::high_resolution_clock> timestamp_t;
 
 }  // namespace dynamic_graph_bridge
 
-namespace dynamicgraph {
+//namespace dynamicgraph {
 /**
  * @brief out stream the time stamp data.
  *
@@ -30,13 +32,16 @@ namespace dynamicgraph {
  * For clang this function needs to be forward declared before the template
  * using it. This is more in accordance to the standard.
  */
-inline std::ostream &operator<<(
-    std::ostream &os, const dynamic_graph_bridge::timestamp_t &time_stamp) {
-  std::chrono::time_point<std::chrono::high_resolution_clock,
-                          std::chrono::milliseconds>
+std::ostream &operator<<(
+    std::ostream &os,
+    const dynamic_graph_bridge::timestamp_t &time_stamp) {
+  boost::chrono::time_point<boost::chrono::high_resolution_clock,
+                          boost::chrono::milliseconds>
       time_stamp_nanosec =
-          std::chrono::time_point_cast<std::chrono::milliseconds>(time_stamp);
+      boost::chrono::time_point_cast<boost::chrono::milliseconds>(time_stamp);
   os << time_stamp_nanosec.time_since_epoch().count();
   return os;
 }
-}  // namespace dynamicgraph
+
+
+//}  // namespace dynamicgraph

src/ros_parameter.cpp

@@ -38,7 +38,7 @@ bool parameter_server_read_robot_description(rclcpp::Node::SharedPtr nh) {
     // Then set the robot model.
     aRobotUtil->set_parameter(parameter_name, robot_description);
     RCLCPP_INFO(rclcpp::get_logger("dynamic_graph_bridge"),
-                "Set parameter_name : %s.", parameter_name.c_str());
+                "parameter_server_read_robot_description : Set parameter_name : %s.", parameter_name.c_str());
     // Everything went fine.
     return true;
   }

src/ros_python_interpreter_client.cpp

@@ -71,7 +71,7 @@ std::string RosPythonInterpreterClient::run_python_command(
       return_string += response.get()->result;
     }
   } catch (const std::exception& ex) {
-    RCLCPP_INFO(rclcpp::get_logger("RosPythonInterpreterClient"), ex.what());
+    RCLCPP_INFO(rclcpp::get_logger("RosPythonInterpreterClient"), "%s", ex.what());
     connect_to_rosservice_run_python_command(timeout_connection_s_);
   } catch (...) {
     RCLCPP_INFO(rclcpp::get_logger("RosPythonInterpreterClient"),

src/ros_python_interpreter_server.cpp

@@ -14,7 +14,6 @@
 #include <memory>
 
 namespace dynamic_graph_bridge {
-static const int queueSize = 5;
 
 RosPythonInterpreterServer::RosPythonInterpreterServer()
     : interpreter_(),