From baa5b68a052c1db932436f9ccb1776d88c7dd23c Mon Sep 17 00:00:00 2001
From: Olivier Stasse <ostasse@laas.fr>
Date: Sun, 3 Dec 2023 21:23:42 +0100
Subject: [PATCH] Modifies the ROS Node map in a map of tuple which includes: -
The ROS Node - A related callback group with reentrant type.
---
include/dynamic_graph_bridge/ros.hpp | 23 +++++++++++++---
src/ros.cpp | 39 ++++++++++++++++++----------
2 files changed, 44 insertions(+), 18 deletions(-)
diff --git a/include/dynamic_graph_bridge/ros.hpp b/include/dynamic_graph_bridge/ros.hpp
index b11acb3..547761b 100644
--- a/include/dynamic_graph_bridge/ros.hpp
+++ b/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();
diff --git a/src/ros.cpp b/src/ros.cpp
index 0f8f215..1283673 100644
--- a/src/ros.cpp
+++ b/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++;
}
}
--
GitLab