Modifies the ROS Node map in a map of tuple which includes:

- The ROS Node
- A related callback group with reentrant type.

Author: olivier-stasse

include/dynamic_graph_bridge/ros.hpp

@@ -60,13 +60,28 @@ 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();

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();
@@ -234,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;
@@ -248,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 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 GLOBAL_LIST_OF_ROS_NODE[node_name];
+  return std::get<1>(GLOBAL_LIST_OF_ROS_NODE[node_name]);
 }
 
 void ros_add_node_to_executor(const std::string& node_name) {
@@ -290,8 +301,8 @@ void ros_display_list_of_nodes() {
   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",
-                ros_node_it->second->get_namespace(),
-                ros_node_it->second->get_name());
+                std::get<0>(ros_node_it->second)->get_namespace(),
+                std::get<0>(ros_node_it->second)->get_name());
     ros_node_it++;
   }
 }