Preparation for tests implying python scripts and publishing topics.

README.md

@@ -30,9 +30,12 @@ cd sot_ws/src
 To build this package after installing all the dependencies:
 ```
 cd sot_ws
-colcon build --merge-install --packages-select dynamic_graph_bridge
+colcon build --merge-install --packages-select dynamic_graph_bridge --cmake-args ' -DPYTHON_STANDARD_LAYOUT:BOOL=ON'
 ```
 
+You need to install everything at the same level specifically for the python modules, and in addition it is necessary to install everything with the standard python layout.
+Note that the same option need to be used for all the packages.
+
 ### Test
 
 To test it:

include/dynamic_graph_bridge/ros.hpp

@@ -60,13 +60,32 @@ typedef rclcpp::Service<std_srvs::srv::Empty>::SharedPtr EmptyServicePtr;
 typedef std_srvs::srv::Empty EmptyServiceType;
 
 /**
- * @brief rosInit is a global method that uses the structure GlobalRos.
- * Its role is to create the ros::nodeHandle and the ros::spinner once at the
- * first call. It always returns a reference to the node hanlde.
- * @return the reference of GLOBAL_ROS_VAR.node_.
+ * @brief If the node named node_name does not exist create it with a reentrant
+ * group
+ * @param node_name
+ */
+void create_ros_node(std::string& node_name);
+
+/**
+ * @brief Get the node shared pointer providing the node name.
+ * @param node_name
+ * @return A pointer towards the node.
  */
 RosNodePtr get_ros_node(std::string node_name);
 
+/**
+ * @brief Returns a Reentrant Callback group initialized with the ros node
+ * specifed by node_name
+ * @param node_name
+ * @param Returns a shared pointer towards the described callback group of type
+ * reentrant.
+ */
+rclcpp::CallbackGroup::SharedPtr get_callback_group(std::string node_name);
+
+size_t ros_executor_get_nb_threads();
+
+void ros_display_list_of_nodes();
+
 /**
  * @brief Add a ros node to the global executor.
  *

include/dynamic_graph_bridge/ros_python_interpreter_server.hpp

@@ -85,8 +85,11 @@ class RosPythonInterpreterServer {
   /**
    * @brief start_ros_service advertize the "run_python_command" and
    * "run_python_scripts" ros services
+   * @param qos: Quality of Service
+   * @param callback_group: Callback group
    */
-  void start_ros_service();
+  void start_ros_service(const rclcpp::QoS& qos = rclcpp::ServicesQoS(),
+                         rclcpp::CallbackGroup::SharedPtr group = nullptr);
 
  protected:
   /**

include/dynamic_graph_bridge/sot_loader.hh

@@ -18,9 +18,6 @@
 // STL includes
 #include <map>
 
-// Pinocchio includes
-#include <pinocchio/fwd.hpp>
-
 // Boost includes
 #include <boost/foreach.hpp>
 #include <boost/format.hpp>
@@ -83,7 +80,7 @@ class SotLoader : public SotLoaderBasic {
   geometry_msgs::msg::TransformStamped freeFlyerPose_;
 
  public:
-  SotLoader();
+  SotLoader(const std::string &aNodeName = std::string("sot_loader"));
   virtual ~SotLoader();
 
   // \brief Create a thread for ROS and start the control loop.

include/dynamic_graph_bridge/sot_loader_basic.hh

@@ -18,9 +18,6 @@
 // STL includes
 #include <map>
 
-// Pinocchio includes
-#include <pinocchio/fwd.hpp>
-
 // Boost includes
 #include <boost/foreach.hpp>
 #include <boost/format.hpp>
@@ -41,8 +38,11 @@
 namespace po = boost::program_options;
 namespace dgs = dynamicgraph::sot;
 
-class SotLoaderBasic : public rclcpp::Node {
+class SotLoaderBasic {
  protected:
+  // RosNode
+  dynamic_graph_bridge::RosNodePtr ros_node_;
+
   // Dynamic graph is stopped.
   bool dynamic_graph_stopped_;
 
@@ -84,7 +84,8 @@ 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("sot_loader_basic"));
   virtual ~SotLoaderBasic();
 
   // \brief Read user input to extract the path of the SoT dynamic library.

src/CMakeLists.txt

@@ -106,7 +106,7 @@ if(BUILD_PYTHON_INTERFACE)
 endif(BUILD_PYTHON_INTERFACE)
 
 # Sot loader library
-add_library(sot_loader sot_loader.cpp sot_loader_basic.cpp)
+add_library(sot_loader SHARED sot_loader.cpp sot_loader_basic.cpp)
 target_link_libraries(
   sot_loader Boost::program_options ${catkin_LIBRARIES} ros_bridge
   dynamic_graph_bridge_msgs::dynamic_graph_bridge_msgs__rosidl_typesupport_cpp)

src/ros.cpp

@@ -33,7 +33,8 @@ namespace dynamic_graph_bridge {
 /**
  * @brief Shortcut.
  */
-typedef std::map<std::string, RosNodePtr> GlobalListOfRosNodeType;
+typedef std::tuple<RosNodePtr, rclcpp::CallbackGroup::SharedPtr> NodeInfo;
+typedef std::map<std::string, NodeInfo> GlobalListOfRosNodeType;
 
 /**
  * @brief GLOBAL_LIST_OF_ROS_NODE is global variable that acts as a singleton on
@@ -57,7 +58,7 @@ static GlobalListOfRosNodeType GLOBAL_LIST_OF_ROS_NODE;
  */
 class Executor {
  public:
-  Executor() : ros_executor_(rclcpp::ExecutorOptions(), 4) {
+  Executor() : ros_executor_(rclcpp::ExecutorOptions(), 30) {
     is_thread_running_ = false;
     is_spinning_ = false;
     list_node_added_.clear();
@@ -131,6 +132,13 @@ class Executor {
     }
   }
 
+  /**
+   * @brief Return the number of threads
+   */
+  size_t get_number_of_threads() {
+    return ros_executor_.get_number_of_threads();
+  }
+
  private:
   /**
    * @brief Thread callback function
@@ -227,7 +235,7 @@ bool ros_node_exists(std::string node_name) {
       GLOBAL_LIST_OF_ROS_NODE.end()) {
     return false;
   }
-  if (GLOBAL_LIST_OF_ROS_NODE.at(node_name) == nullptr) {
+  if (std::get<0>(GLOBAL_LIST_OF_ROS_NODE.at(node_name)) == nullptr) {
     return false;
   }
   return true;
@@ -241,19 +249,29 @@ ExecutorPtr get_ros_executor() {
   return EXECUTOR;
 }
 
-RosNodePtr get_ros_node(std::string node_name) {
-  ros_init();
+void create_ros_node(std::string& node_name) {
   if (!ros_node_exists(node_name)) {
-    GLOBAL_LIST_OF_ROS_NODE[node_name] = RosNodePtr(nullptr);
-  }
-  if (!GLOBAL_LIST_OF_ROS_NODE[node_name] ||
-      GLOBAL_LIST_OF_ROS_NODE[node_name].get() == nullptr) {
-    /** RosNode instanciation */
-    GLOBAL_LIST_OF_ROS_NODE[node_name] =
+    RosNodePtr a_ros_node_ptr =
         std::make_shared<RosNode>(node_name, "dynamic_graph_bridge");
+    rclcpp::CallbackGroup::SharedPtr a_cb_group =
+        a_ros_node_ptr->create_callback_group(
+            rclcpp::CallbackGroupType::Reentrant);
+    /** RosNode instanciation */
+    GLOBAL_LIST_OF_ROS_NODE[node_name] = NodeInfo(a_ros_node_ptr, a_cb_group);
   }
+}
+RosNodePtr get_ros_node(std::string node_name) {
+  ros_init();
+  create_ros_node(node_name);
   /** Return a reference to the node handle so any function can use it */
-  return GLOBAL_LIST_OF_ROS_NODE[node_name];
+  return std::get<0>(GLOBAL_LIST_OF_ROS_NODE[node_name]);
+}
+
+rclcpp::CallbackGroup::SharedPtr get_callback_group(std::string node_name) {
+  ros_init();
+  create_ros_node(node_name);
+  /** Return a reference to the node handle so any function can use it */
+  return std::get<1>(GLOBAL_LIST_OF_ROS_NODE[node_name]);
 }
 
 void ros_add_node_to_executor(const std::string& node_name) {
@@ -273,6 +291,22 @@ void ros_shutdown() {
   // rclcpp::shutdown();
 }
 
+size_t ros_executor_get_nb_threads() {
+  return get_ros_executor()->get_number_of_threads();
+}
+
+void ros_display_list_of_nodes() {
+  GlobalListOfRosNodeType::iterator ros_node_it =
+      GLOBAL_LIST_OF_ROS_NODE.begin();
+  while (ros_node_it != GLOBAL_LIST_OF_ROS_NODE.end()) {
+    RCLCPP_INFO(rclcpp::get_logger("dynamic_graph_bridge"),
+                "ros_display_list_of_nodes: %s/%s",
+                std::get<0>(ros_node_it->second)->get_namespace(),
+                std::get<0>(ros_node_it->second)->get_name());
+    ros_node_it++;
+  }
+}
+
 void ros_clean() {
   ros_stop_spinning();
   GlobalListOfRosNodeType::iterator ros_node_it =

src/ros_python_interpreter_server.cpp

@@ -25,18 +25,26 @@ RosPythonInterpreterServer::RosPythonInterpreterServer()
 
 RosPythonInterpreterServer::~RosPythonInterpreterServer() {}
 
-void RosPythonInterpreterServer::start_ros_service() {
+void RosPythonInterpreterServer::start_ros_service(
+    const rclcpp::QoS& qos, rclcpp::CallbackGroup::SharedPtr group) {
   run_python_command_callback_t runCommandCb =
       std::bind(&RosPythonInterpreterServer::runCommandCallback, this,
                 std::placeholders::_1, std::placeholders::_2);
   run_python_command_srv_ = ros_node_->create_service<RunPythonCommandSrvType>(
-      "run_python_command", runCommandCb);
+      "/dynamic_graph_bridge/run_python_command", runCommandCb, qos, group);
+  RCLCPP_INFO(
+      rclcpp::get_logger("dynamic_graph_bridge"),
+      "RosPythonInterpreterServer::start_ros_service - run_python_command...");
 
   run_python_file_callback_t runPythonFileCb =
       std::bind(&RosPythonInterpreterServer::runPythonFileCallback, this,
                 std::placeholders::_1, std::placeholders::_2);
   run_python_file_srv_ = ros_node_->create_service<RunPythonFileSrvType>(
-      "run_python_file", runPythonFileCb);
+      "/dynamic_graph_bridge/run_python_file", runPythonFileCb, qos, group);
+
+  RCLCPP_INFO(
+      rclcpp::get_logger("dynamic_graph_bridge"),
+      "RosPythonInterpreterServer::start_ros_service - run_python_file...");
 }
 
 void RosPythonInterpreterServer::runCommandCallback(
@@ -61,7 +69,11 @@ void RosPythonInterpreterServer::runCommandCallback(
 
 void RosPythonInterpreterServer::runPythonFileCallback(
     RunPythonFileRequestPtr req, RunPythonFileResponsePtr res) {
+  RCLCPP_INFO(rclcpp::get_logger("dynamic_graph_bridge"),
+              "RosPythonInterpreterServer::runPythonFileCallback- begin");
   run_python_file(req->input, res->result);
+  RCLCPP_INFO(rclcpp::get_logger("dynamic_graph_bridge"),
+              "RosPythonInterpreterServer::runPythonFileCallback- end");
 }
 
 void RosPythonInterpreterServer::run_python_command(const std::string& command,

src/sot_loader.cpp

@@ -56,8 +56,8 @@ struct DataToLog {
   }
 };
 
-SotLoader::SotLoader()
-    : SotLoaderBasic(),
+SotLoader::SotLoader(const std::string &aNodeName)
+    : SotLoaderBasic(aNodeName),
       sensorsIn_(),
       controlValues_(),
       angleEncoder_(),
@@ -83,10 +83,10 @@ void SotLoader::initializeServices() {
   freeFlyerPose_.header.frame_id = "odom";
   freeFlyerPose_.child_frame_id = "base_link";
 
-  if (not has_parameter("tf_base_link"))
-    declare_parameter("tf_base_link", std::string("base_link"));
+  if (not ros_node_->has_parameter("tf_base_link"))
+    ros_node_->declare_parameter("tf_base_link", std::string("base_link"));
 
-  if (get_parameter("tf_base_link", freeFlyerPose_.child_frame_id)) {
+  if (ros_node_->get_parameter("tf_base_link", freeFlyerPose_.child_frame_id)) {
     RCLCPP_INFO_STREAM(rclcpp::get_logger("dynamic_graph_bridge"),
                        "Publishing " << freeFlyerPose_.child_frame_id << " wrt "
                                      << freeFlyerPose_.header.frame_id);
@@ -98,7 +98,8 @@ void SotLoader::initializeServices() {
   control_.resize(static_cast<std::size_t>(nbOfJoints_));
 
   // Creates a publisher for the free flyer transform.
-  freeFlyerPublisher_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);
+  freeFlyerPublisher_ =
+      std::make_shared<tf2_ros::TransformBroadcaster>(ros_node_);
 }
 
 void SotLoader::fillSensors(map<string, dgs::SensorValues> &sensorsIn) {
@@ -164,9 +165,6 @@ void SotLoader::readControl(map<string, dgs::ControlValues> &controlValues) {
     joint_state_.position[i] = angleEncoder_[i];
   }
 
-  std::cerr << "parallel_joints_to_state_vector_.size(): "
-            << parallel_joints_to_state_vector_.size() << std::endl;
-
   for (unsigned int i = 0; i < parallel_joints_to_state_vector_.size(); i++) {
     joint_state_.position[i + nbOfJoints_std_s] =
         coefficient_parallel_joints_[i] *
@@ -184,7 +182,6 @@ void SotLoader::readControl(map<string, dgs::ControlValues> &controlValues) {
     throw anInvalidArgument;
   }
 
-  std::cerr << "Reached poseValue_" << std::endl;
   std::vector<double> poseValue = controlValues["baseff"].getValues();
 
   freeFlyerPose_.transform.translation.x = poseValue[0];
@@ -199,17 +196,19 @@ void SotLoader::readControl(map<string, dgs::ControlValues> &controlValues) {
   freeFlyerPose_.header.stamp = joint_state_.header.stamp;
   // Publish
   freeFlyerPublisher_->sendTransform(freeFlyerPose_);
-  std::cerr << "end of readControl" << std::endl;
 }
 
 void SotLoader::setup() {
+  if (sotController_ == NULL) return;
   fillSensors(sensorsIn_);
   sotController_->setupSetSensors(sensorsIn_);
   sotController_->getControl(controlValues_);
   readControl(controlValues_);
 }
 
 void SotLoader::oneIteration() {
+  if (sotController_ == NULL) return;
+
   fillSensors(sensorsIn_);
   try {
     sotController_->nominalSetSensors(sensorsIn_);
@@ -230,11 +229,13 @@ void SotLoader::workThreadLoader() {
   double periodd;
 
   /// Declare parameters
-  if (not has_parameter("dt")) declare_parameter<double>("dt", 0.01);
-  if (not has_parameter("paused")) declare_parameter<bool>("paused", false);
+  if (not ros_node_->has_parameter("dt"))
+    ros_node_->declare_parameter<double>("dt", 0.01);
+  if (not ros_node_->has_parameter("paused"))
+    ros_node_->declare_parameter<bool>("paused", false);
 
   //
-  get_parameter_or("dt", periodd, 0.001);
+  ros_node_->get_parameter_or("dt", periodd, 0.001);
   rclcpp::Rate rate(1 / periodd);  // 1 kHz
 
   DataToLog dataToLog(5000);
@@ -250,15 +251,15 @@ void SotLoader::workThreadLoader() {
   rclcpp::Time time;
   while (rclcpp::ok() && !isDynamicGraphStopped()) {
     // Check if the user did not paused geometric_simu
-    get_parameter_or("paused", paused, false);
+    ros_node_->get_parameter_or("paused", paused, false);
 
     if (!paused) {
       time = aClock.now();
       oneIteration();
 
       rclcpp::Duration d = aClock.now() - time;
-      dataToLog.record((time - timeOrigin).nanoseconds() * 1.0e9,
-                       d.nanoseconds() * 1.0e9);
+      dataToLog.record((double)((time - timeOrigin).nanoseconds()) * 1.0e9,
+                       (double)(d.nanoseconds()) * 1.0e9);
     }
     rate.sleep();
   }