Reuse the MultiThreaded::Executor to handle ROS Nodes.

Author: olivier-stasse

include/dynamic_graph_bridge/sot_loader.hh

@@ -80,7 +80,7 @@ class SotLoader : public SotLoaderBasic {
   geometry_msgs::msg::TransformStamped freeFlyerPose_;
 
  public:
-  SotLoader(const std::string &aNodeName=std::string("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

@@ -38,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_;
 
@@ -80,8 +83,9 @@ class SotLoaderBasic : public rclcpp::Node {
   // Ordered list of joint names describing the robot state.
   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/sot_loader.cpp

@@ -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,7 +196,6 @@ 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() {
@@ -230,11 +226,11 @@ 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 +246,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();
   }

src/sot_loader_basic.cpp

@@ -27,10 +27,13 @@ using namespace dynamicgraph::sot;
 namespace po = boost::program_options;
 
 SotLoaderBasic::SotLoaderBasic(const std::string& aNodeName)
-    : rclcpp::Node(aNodeName),
+    : ros_node_(nullptr),
       dynamic_graph_stopped_(true),
       sotController_(NULL),
       sotRobotControllerLibrary_(0) {
+
+  ros_node_ = dynamic_graph_bridge::get_ros_node(aNodeName);
+
   RCLCPP_INFO(rclcpp::get_logger("dynamic_graph_bridge::"),
               "Beginning of SotLoaderBasic");
   RCLCPP_INFO(rclcpp::get_logger("dynamic_graph_bridge"),
@@ -48,7 +51,7 @@ int SotLoaderBasic::initPublication() {
   RCLCPP_INFO(rclcpp::get_logger("dynamic_graph_bridge"),
               "SotLoaderBasic::initPublication - create joint_pub");
 
-  joint_pub_ =
+  joint_pub_ = ros_node_->
       create_publisher<sensor_msgs::msg::JointState>("joint_states", 1);
 
   RCLCPP_INFO(rclcpp::get_logger("dynamic_graph_bridge"),
@@ -62,19 +65,21 @@ void SotLoaderBasic::initializeServices() {
       "SotLoaderBasic::initializeServices - Ready to start dynamic graph.");
 
   using namespace std::placeholders;
-  service_start_ = create_service<std_srvs::srv::Empty>(
-      "start_dynamic_graph",
-      std::bind(&SotLoaderBasic::start_dg, this, std::placeholders::_1,
-                std::placeholders::_2));
+  service_start_ = ros_node_->
+      create_service<std_srvs::srv::Empty>(
+          "start_dynamic_graph",
+          std::bind(&SotLoaderBasic::start_dg, this, std::placeholders::_1,
+                    std::placeholders::_2));
   RCLCPP_INFO(rclcpp::get_logger("dynamic_graph_bridge"),
-              "SotLoaderBasic::initializeServices - started dynamic graph.");
+              "SotLoaderBasic::initializeServices - start_dynamic_graph.");
 
-  service_stop_ = create_service<std_srvs::srv::Empty>(
-      "stop_dynamic_graph",
-      std::bind(&SotLoaderBasic::stop_dg, this, std::placeholders::_1,
-                std::placeholders::_2));
+  service_stop_ = ros_node_->
+      create_service<std_srvs::srv::Empty>(
+          "stop_dynamic_graph",
+          std::bind(&SotLoaderBasic::stop_dg, this, std::placeholders::_1,
+                    std::placeholders::_2));
   RCLCPP_INFO(rclcpp::get_logger("dynamic_graph_bridge"),
-              "SotLoaderBasic::initializeServices - stopped dynamic graph.");
+              "SotLoaderBasic::initializeServices - stop_dynamic_graph.");
 
   parameter_server_read_robot_description();
 }
@@ -89,22 +94,22 @@ int SotLoaderBasic::readSotVectorStateParam() {
 
   // It is necessary to declare parameters first
   // before trying to access them.
-  if (not has_parameter("state_vector_map"))
-    declare_parameter("state_vector_map", std::vector<std::string>{});
-  if (not has_parameter("joint_state_parallel"))
-    declare_parameter("joint_state_parallel", std::vector<std::string>{});
+  if (not ros_node_->has_parameter("state_vector_map"))
+    ros_node_->declare_parameter("state_vector_map", std::vector<std::string>{});
+  if (not ros_node_->has_parameter("joint_state_parallel"))
+    ros_node_->declare_parameter("joint_state_parallel", std::vector<std::string>{});
 
   // Read the state vector of the robot
   // Defines the order in which the actuators are ordered
   try {
     std::string aParameterName("state_vector_map");
-    if (!get_parameter(aParameterName, stateVectorMap_)) {
+    if (!ros_node_->get_parameter(aParameterName, stateVectorMap_)) {
       logic_error aLogicError(
           "SotLoaderBasic::readSotVectorStateParam : State_vector_map is "
           "empty");
       throw aLogicError;
     }
-    RCLCPP_INFO(get_logger(), "state_vector_map parameter size %ld",
+    RCLCPP_INFO(ros_node_->get_logger(), "state_vector_map parameter size %ld",
                 stateVectorMap_.size());
   } catch (exception& e) {
     std::throw_with_nested(std::logic_error("Unable to call get_parameter"));
@@ -119,7 +124,7 @@ int SotLoaderBasic::readSotVectorStateParam() {
 
   std::string prefix("joint_state_parallel");
   std::map<std::string, rclcpp::Parameter> joint_state_parallel;
-  get_parameters(prefix, joint_state_parallel);
+  ros_node_->get_parameters(prefix, joint_state_parallel);
 
   // Iterates over the map joint_state_parallel
   for (std::map<std::string, rclcpp::Parameter>::iterator it_map_expression =
@@ -202,7 +207,9 @@ void SotLoaderBasic::loadController() {
   sotRobotControllerLibrary_ =
       dlopen(dynamicLibraryName_.c_str(), RTLD_LAZY | RTLD_GLOBAL);
   if (!sotRobotControllerLibrary_) {
-    std::cerr << "Cannot load library: " << dlerror() << '\n';
+    RCLCPP_ERROR(rclcpp::get_logger("dynamic_graph_bridge"),
+                 "Cannot load library: %s",
+                 dlerror());
     return;
   }
 
@@ -215,14 +222,17 @@ void SotLoaderBasic::loadController() {
           dlsym(sotRobotControllerLibrary_, "createSotExternalInterface")));
   const char* dlsym_error = dlerror();
   if (dlsym_error) {
-    std::cerr << "Cannot load symbol create: " << dlsym_error << " from "
-              << dynamicLibraryName_ << '\n';
+    RCLCPP_ERROR(rclcpp::get_logger("dynamic_graph_bridge"),
+                 "Cannot load symbol create: %s from %s",
+                 dlsym_error,
+                 dynamicLibraryName_.c_str() );
     return;
   }
 
   // Create robot-controller
   sotController_ = createSot();
-  cout << "Went out from loadController." << endl;
+  RCLCPP_INFO(rclcpp::get_logger("dynamic_graph_bridge"),
+              "Went out from loadController.");
 }
 
 void SotLoaderBasic::CleanUp() {
@@ -269,13 +279,13 @@ void SotLoaderBasic::stop_dg(
 }
 
 bool SotLoaderBasic::parameter_server_read_robot_description() {
-  if (!has_parameter("robot_description")) {
-    declare_parameter("robot_description", std::string(""));
+  if (!ros_node_->has_parameter("robot_description")) {
+    ros_node_->declare_parameter("robot_description", std::string(""));
   }
 
   std::string robot_description;
   std::string parameter_name("robot_description");
-  get_parameter(parameter_name, robot_description);
+  ros_node_->get_parameter(parameter_name, robot_description);
   if (robot_description.empty()) {
     RCLCPP_FATAL(rclcpp::get_logger("dynamic_graph_bridge"),
                  "Parameter robot_description is empty");