From ba32b220fd0300473498faa325ffecf35a73b36a Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Mon, 24 May 2021 17:53:26 +0800 Subject: [PATCH 01/10] support services run with multiple threads Signed-off-by: Chen Lihui --- include/ros1_bridge/factory.hpp | 6 ++++-- include/ros1_bridge/factory_interface.hpp | 2 ++ src/dynamic_bridge.cpp | 25 ++++++++++++++++------- 3 files changed, 24 insertions(+), 9 deletions(-) diff --git a/include/ros1_bridge/factory.hpp b/include/ros1_bridge/factory.hpp index f2d1e34b..eee828ba 100755 --- a/include/ros1_bridge/factory.hpp +++ b/include/ros1_bridge/factory.hpp @@ -328,7 +328,8 @@ class ServiceFactory : public ServiceFactoryInterface ros::NodeHandle & ros1_node, rclcpp::Node::SharedPtr ros2_node, const std::string & name) { ServiceBridge1to2 bridge; - bridge.client = ros2_node->create_client(name); + bridge.group = ros2_node->create_callback_group(rclcpp::CallbackGroupType::Reentrant); + bridge.client = ros2_node->create_client(name, rmw_qos_profile_services_default, bridge.group); auto m = &ServiceFactory::forward_1_to_2; auto f = std::bind( m, this, bridge.client, ros2_node->get_logger(), std::placeholders::_1, @@ -351,7 +352,8 @@ class ServiceFactory : public ServiceFactoryInterface f = std::bind( m, this, bridge.client, ros2_node->get_logger(), std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); - bridge.server = ros2_node->create_service(name, f); + bridge.group = ros2_node->create_callback_group(rclcpp::CallbackGroupType::Reentrant); + bridge.server = ros2_node->create_service(name, f, rmw_qos_profile_services_default, bridge.group); return bridge; } diff --git a/include/ros1_bridge/factory_interface.hpp b/include/ros1_bridge/factory_interface.hpp index ee568d4d..4ee60c35 100755 --- a/include/ros1_bridge/factory_interface.hpp +++ b/include/ros1_bridge/factory_interface.hpp @@ -33,11 +33,13 @@ namespace ros1_bridge struct ServiceBridge1to2 { ros::ServiceServer server; + rclcpp::CallbackGroup::SharedPtr group; rclcpp::ClientBase::SharedPtr client; }; struct ServiceBridge2to1 { + rclcpp::CallbackGroup::SharedPtr group; rclcpp::ServiceBase::SharedPtr server; ros::ServiceClient client; }; diff --git a/src/dynamic_bridge.cpp b/src/dynamic_bridge.cpp index cd2db0c3..59cc53d1 100644 --- a/src/dynamic_bridge.cpp +++ b/src/dynamic_bridge.cpp @@ -136,7 +136,8 @@ void update_bridge( std::map & bridges_2to1, std::map & service_bridges_1_to_2, std::map & service_bridges_2_to_1, - bool bridge_all_1to2_topics, bool bridge_all_2to1_topics) + bool bridge_all_1to2_topics, bool bridge_all_2to1_topics, + std::vector & invalid_callback_group) { std::lock_guard lock(g_bridge_mutex); @@ -353,6 +354,8 @@ void update_bridge( if (ros1_services.find(it->first) == ros1_services.end()) { printf("Removed 2 to 1 bridge for service %s\n", it->first.data()); try { + // store the callback group to ensure that it is deleted after the node + invalid_callback_group.push_back(it->second.group); it = service_bridges_2_to_1.erase(it); } catch (std::runtime_error & e) { fprintf(stderr, "There was an error while removing 2 to 1 bridge: %s\n", e.what()); @@ -368,6 +371,8 @@ void update_bridge( printf("Removed 1 to 2 bridge for service %s\n", it->first.data()); try { it->second.server.shutdown(); + // store the callback group to ensure that it is deleted after the node + invalid_callback_group.push_back(it->second.group); it = service_bridges_1_to_2.erase(it); } catch (std::runtime_error & e) { fprintf(stderr, "There was an error while removing 1 to 2 bridge: %s\n", e.what()); @@ -484,6 +489,7 @@ int main(int argc, char * argv[]) std::map bridges_2to1; std::map service_bridges_1_to_2; std::map service_bridges_2_to_1; + std::vector invalid_callback_group; // setup polling of ROS 1 master auto ros1_poll = [ @@ -494,7 +500,8 @@ int main(int argc, char * argv[]) &ros1_services, &ros2_services, &service_bridges_1_to_2, &service_bridges_2_to_1, &output_topic_introspection, - &bridge_all_1to2_topics, &bridge_all_2to1_topics + &bridge_all_1to2_topics, &bridge_all_2to1_topics, + &invalid_callback_group ](const ros::TimerEvent &) -> void { // collect all topics names which have at least one publisher or subscriber beside this bridge @@ -612,7 +619,8 @@ int main(int argc, char * argv[]) ros1_services, ros2_services, bridges_1to2, bridges_2to1, service_bridges_1_to_2, service_bridges_2_to_1, - bridge_all_1to2_topics, bridge_all_2to1_topics); + bridge_all_1to2_topics, bridge_all_2to1_topics, + invalid_callback_group); }; auto ros1_poll_timer = ros1_node.createTimer(ros::Duration(1.0), ros1_poll); @@ -631,7 +639,8 @@ int main(int argc, char * argv[]) &service_bridges_1_to_2, &service_bridges_2_to_1, &output_topic_introspection, &bridge_all_1to2_topics, &bridge_all_2to1_topics, - &already_ignored_topics, &already_ignored_services + &already_ignored_topics, &already_ignored_services, + &invalid_callback_group ]() -> void { auto ros2_topics = ros2_node->get_topic_names_and_types(); @@ -776,7 +785,8 @@ int main(int argc, char * argv[]) ros1_services, ros2_services, bridges_1to2, bridges_2to1, service_bridges_1_to_2, service_bridges_2_to_1, - bridge_all_1to2_topics, bridge_all_2to1_topics); + bridge_all_1to2_topics, bridge_all_2to1_topics, + invalid_callback_group); }; auto ros2_poll_timer = ros2_node->create_wall_timer( @@ -784,13 +794,14 @@ int main(int argc, char * argv[]) // ROS 1 asynchronous spinner - ros::AsyncSpinner async_spinner(1); + ros::AsyncSpinner async_spinner(0); async_spinner.start(); // ROS 2 spinning loop - rclcpp::executors::SingleThreadedExecutor executor; + rclcpp::executors::MultiThreadedExecutor executor; while (ros1_node.ok() && rclcpp::ok()) { executor.spin_node_once(ros2_node); + invalid_callback_group.clear(); } return 0; From 16c85a66e51fd2f90a1812ee815073d24caa6b4f Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Tue, 25 May 2021 09:20:18 +0800 Subject: [PATCH 02/10] fix unscrutify Signed-off-by: Chen Lihui --- include/ros1_bridge/factory.hpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/include/ros1_bridge/factory.hpp b/include/ros1_bridge/factory.hpp index eee828ba..1738cf4c 100755 --- a/include/ros1_bridge/factory.hpp +++ b/include/ros1_bridge/factory.hpp @@ -329,7 +329,9 @@ class ServiceFactory : public ServiceFactoryInterface { ServiceBridge1to2 bridge; bridge.group = ros2_node->create_callback_group(rclcpp::CallbackGroupType::Reentrant); - bridge.client = ros2_node->create_client(name, rmw_qos_profile_services_default, bridge.group); + bridge.client = ros2_node->create_client( + name, rmw_qos_profile_services_default, + bridge.group); auto m = &ServiceFactory::forward_1_to_2; auto f = std::bind( m, this, bridge.client, ros2_node->get_logger(), std::placeholders::_1, @@ -353,7 +355,9 @@ class ServiceFactory : public ServiceFactoryInterface m, this, bridge.client, ros2_node->get_logger(), std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); bridge.group = ros2_node->create_callback_group(rclcpp::CallbackGroupType::Reentrant); - bridge.server = ros2_node->create_service(name, f, rmw_qos_profile_services_default, bridge.group); + bridge.server = ros2_node->create_service( + name, f, rmw_qos_profile_services_default, + bridge.group); return bridge; } From f980aba345adbcc390bda687d8adb46527446599 Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Tue, 25 May 2021 10:49:34 +0800 Subject: [PATCH 03/10] use only one custom callback with reentrant for service, client and subsciption Signed-off-by: Chen Lihui --- include/ros1_bridge/factory.hpp | 21 +++++++++++++++++---- include/ros1_bridge/factory_interface.hpp | 2 -- src/dynamic_bridge.cpp | 23 +++++++---------------- 3 files changed, 24 insertions(+), 22 deletions(-) diff --git a/include/ros1_bridge/factory.hpp b/include/ros1_bridge/factory.hpp index 1738cf4c..fe8c8f12 100755 --- a/include/ros1_bridge/factory.hpp +++ b/include/ros1_bridge/factory.hpp @@ -33,6 +33,20 @@ namespace ros1_bridge { +static rclcpp::CallbackGroup::SharedPtr get_callback_group(rclcpp::Node::SharedPtr ros2_node) +{ + auto node_base = ros2_node->get_node_base_interface(); + auto group = node_base->get_default_callback_group(); + auto callback_groups = node_base->get_callback_groups(); + if (callback_groups.size() > 1) { + auto group_lock = callback_groups[1].lock(); + if (group_lock) { + group = group_lock; + } + } + return group; +} + template class Factory : public FactoryInterface { @@ -145,6 +159,7 @@ class Factory : public FactoryInterface ros1_pub, ros1_type_name_, ros2_type_name_, node->get_logger(), ros2_pub); rclcpp::SubscriptionOptions options; options.ignore_local_publications = true; + options.callback_group = ros1_bridge::get_callback_group(node); return node->create_subscription( topic_name, qos, callback, options); } @@ -328,10 +343,9 @@ class ServiceFactory : public ServiceFactoryInterface ros::NodeHandle & ros1_node, rclcpp::Node::SharedPtr ros2_node, const std::string & name) { ServiceBridge1to2 bridge; - bridge.group = ros2_node->create_callback_group(rclcpp::CallbackGroupType::Reentrant); bridge.client = ros2_node->create_client( name, rmw_qos_profile_services_default, - bridge.group); + ros1_bridge::get_callback_group(ros2_node)); auto m = &ServiceFactory::forward_1_to_2; auto f = std::bind( m, this, bridge.client, ros2_node->get_logger(), std::placeholders::_1, @@ -354,10 +368,9 @@ class ServiceFactory : public ServiceFactoryInterface f = std::bind( m, this, bridge.client, ros2_node->get_logger(), std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); - bridge.group = ros2_node->create_callback_group(rclcpp::CallbackGroupType::Reentrant); bridge.server = ros2_node->create_service( name, f, rmw_qos_profile_services_default, - bridge.group); + ros1_bridge::get_callback_group(ros2_node)); return bridge; } diff --git a/include/ros1_bridge/factory_interface.hpp b/include/ros1_bridge/factory_interface.hpp index 4ee60c35..ee568d4d 100755 --- a/include/ros1_bridge/factory_interface.hpp +++ b/include/ros1_bridge/factory_interface.hpp @@ -33,13 +33,11 @@ namespace ros1_bridge struct ServiceBridge1to2 { ros::ServiceServer server; - rclcpp::CallbackGroup::SharedPtr group; rclcpp::ClientBase::SharedPtr client; }; struct ServiceBridge2to1 { - rclcpp::CallbackGroup::SharedPtr group; rclcpp::ServiceBase::SharedPtr server; ros::ServiceClient client; }; diff --git a/src/dynamic_bridge.cpp b/src/dynamic_bridge.cpp index 59cc53d1..e8e1c7d3 100644 --- a/src/dynamic_bridge.cpp +++ b/src/dynamic_bridge.cpp @@ -136,8 +136,7 @@ void update_bridge( std::map & bridges_2to1, std::map & service_bridges_1_to_2, std::map & service_bridges_2_to_1, - bool bridge_all_1to2_topics, bool bridge_all_2to1_topics, - std::vector & invalid_callback_group) + bool bridge_all_1to2_topics, bool bridge_all_2to1_topics) { std::lock_guard lock(g_bridge_mutex); @@ -354,8 +353,6 @@ void update_bridge( if (ros1_services.find(it->first) == ros1_services.end()) { printf("Removed 2 to 1 bridge for service %s\n", it->first.data()); try { - // store the callback group to ensure that it is deleted after the node - invalid_callback_group.push_back(it->second.group); it = service_bridges_2_to_1.erase(it); } catch (std::runtime_error & e) { fprintf(stderr, "There was an error while removing 2 to 1 bridge: %s\n", e.what()); @@ -371,8 +368,6 @@ void update_bridge( printf("Removed 1 to 2 bridge for service %s\n", it->first.data()); try { it->second.server.shutdown(); - // store the callback group to ensure that it is deleted after the node - invalid_callback_group.push_back(it->second.group); it = service_bridges_1_to_2.erase(it); } catch (std::runtime_error & e) { fprintf(stderr, "There was an error while removing 1 to 2 bridge: %s\n", e.what()); @@ -472,6 +467,8 @@ int main(int argc, char * argv[]) rclcpp::init(argc, argv); auto ros2_node = rclcpp::Node::make_shared("ros_bridge"); + // a callback group for creating ros2 entity (client, service and subscription) later + auto callback_group = ros2_node->create_callback_group(rclcpp::CallbackGroupType::Reentrant); // ROS 1 node ros::init(argc, argv, "ros_bridge"); @@ -489,7 +486,6 @@ int main(int argc, char * argv[]) std::map bridges_2to1; std::map service_bridges_1_to_2; std::map service_bridges_2_to_1; - std::vector invalid_callback_group; // setup polling of ROS 1 master auto ros1_poll = [ @@ -500,8 +496,7 @@ int main(int argc, char * argv[]) &ros1_services, &ros2_services, &service_bridges_1_to_2, &service_bridges_2_to_1, &output_topic_introspection, - &bridge_all_1to2_topics, &bridge_all_2to1_topics, - &invalid_callback_group + &bridge_all_1to2_topics, &bridge_all_2to1_topics ](const ros::TimerEvent &) -> void { // collect all topics names which have at least one publisher or subscriber beside this bridge @@ -619,8 +614,7 @@ int main(int argc, char * argv[]) ros1_services, ros2_services, bridges_1to2, bridges_2to1, service_bridges_1_to_2, service_bridges_2_to_1, - bridge_all_1to2_topics, bridge_all_2to1_topics, - invalid_callback_group); + bridge_all_1to2_topics, bridge_all_2to1_topics); }; auto ros1_poll_timer = ros1_node.createTimer(ros::Duration(1.0), ros1_poll); @@ -639,8 +633,7 @@ int main(int argc, char * argv[]) &service_bridges_1_to_2, &service_bridges_2_to_1, &output_topic_introspection, &bridge_all_1to2_topics, &bridge_all_2to1_topics, - &already_ignored_topics, &already_ignored_services, - &invalid_callback_group + &already_ignored_topics, &already_ignored_services ]() -> void { auto ros2_topics = ros2_node->get_topic_names_and_types(); @@ -785,8 +778,7 @@ int main(int argc, char * argv[]) ros1_services, ros2_services, bridges_1to2, bridges_2to1, service_bridges_1_to_2, service_bridges_2_to_1, - bridge_all_1to2_topics, bridge_all_2to1_topics, - invalid_callback_group); + bridge_all_1to2_topics, bridge_all_2to1_topics); }; auto ros2_poll_timer = ros2_node->create_wall_timer( @@ -801,7 +793,6 @@ int main(int argc, char * argv[]) rclcpp::executors::MultiThreadedExecutor executor; while (ros1_node.ok() && rclcpp::ok()) { executor.spin_node_once(ros2_node); - invalid_callback_group.clear(); } return 0; From 50ba40f502b9641182b22c18cc01d553b4833472 Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Tue, 25 May 2021 17:17:05 +0800 Subject: [PATCH 04/10] fix ros2 callback of ros1_bridge running with multiple threads Signed-off-by: Chen Lihui --- src/dynamic_bridge.cpp | 17 ++++++++++++----- 1 file changed, 12 insertions(+), 5 deletions(-) diff --git a/src/dynamic_bridge.cpp b/src/dynamic_bridge.cpp index e8e1c7d3..258c3fe4 100644 --- a/src/dynamic_bridge.cpp +++ b/src/dynamic_bridge.cpp @@ -781,9 +781,17 @@ int main(int argc, char * argv[]) bridge_all_1to2_topics, bridge_all_2to1_topics); }; - auto ros2_poll_timer = ros2_node->create_wall_timer( - std::chrono::seconds(1), ros2_poll); + auto check_ros1_flag = [&ros1_node] { + if (!ros1_node.ok()) { + rclcpp::shutdown(); + } + }; + auto ros2_poll_timer = ros2_node->create_wall_timer( + std::chrono::seconds(1), [&ros2_poll, &check_ros1_flag] { + ros2_poll(); + check_ros1_flag(); + }); // ROS 1 asynchronous spinner ros::AsyncSpinner async_spinner(0); @@ -791,9 +799,8 @@ int main(int argc, char * argv[]) // ROS 2 spinning loop rclcpp::executors::MultiThreadedExecutor executor; - while (ros1_node.ok() && rclcpp::ok()) { - executor.spin_node_once(ros2_node); - } + executor.add_node(ros2_node); + executor.spin(); return 0; } From bb062af0b13474c70455b822807909223fa5e0de Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Thu, 27 May 2021 10:06:35 +0800 Subject: [PATCH 05/10] ensure that the message data of each topic is received in order and resotre the original main loop logic Signed-off-by: Chen Lihui --- include/ros1_bridge/factory.hpp | 32 +++++++++++++++++++++++++------- src/dynamic_bridge.cpp | 19 ++++++------------- 2 files changed, 31 insertions(+), 20 deletions(-) diff --git a/include/ros1_bridge/factory.hpp b/include/ros1_bridge/factory.hpp index fe8c8f12..cf532d16 100755 --- a/include/ros1_bridge/factory.hpp +++ b/include/ros1_bridge/factory.hpp @@ -33,17 +33,35 @@ namespace ros1_bridge { -static rclcpp::CallbackGroup::SharedPtr get_callback_group(rclcpp::Node::SharedPtr ros2_node) +static rclcpp::CallbackGroup::SharedPtr get_callback_group( + rclcpp::Node::SharedPtr ros2_node, + const std::string & topic_name = "") { auto node_base = ros2_node->get_node_base_interface(); auto group = node_base->get_default_callback_group(); - auto callback_groups = node_base->get_callback_groups(); - if (callback_groups.size() > 1) { - auto group_lock = callback_groups[1].lock(); - if (group_lock) { - group = group_lock; + if (topic_name.empty()) { + auto callback_groups = node_base->get_callback_groups(); + if (callback_groups.size() > 1) { + auto group_lock = callback_groups[1].lock(); + if (group_lock) { + group = group_lock; + } } + return group; + } + + typedef std::map TopicCallbackGroupMap; + static TopicCallbackGroupMap s_topic_callbackgroups; + auto iter = s_topic_callbackgroups.find(topic_name); + if (iter != s_topic_callbackgroups.end()) { + return iter->second; } + + // use CallbackGroup with MutuallyExclusive for each topic + // to ensure that the message data of each topic is received in order + group = ros2_node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + s_topic_callbackgroups.insert({topic_name, group}); + return group; } @@ -159,7 +177,7 @@ class Factory : public FactoryInterface ros1_pub, ros1_type_name_, ros2_type_name_, node->get_logger(), ros2_pub); rclcpp::SubscriptionOptions options; options.ignore_local_publications = true; - options.callback_group = ros1_bridge::get_callback_group(node); + options.callback_group = ros1_bridge::get_callback_group(node, topic_name); return node->create_subscription( topic_name, qos, callback, options); } diff --git a/src/dynamic_bridge.cpp b/src/dynamic_bridge.cpp index 258c3fe4..a6365d49 100644 --- a/src/dynamic_bridge.cpp +++ b/src/dynamic_bridge.cpp @@ -467,7 +467,7 @@ int main(int argc, char * argv[]) rclcpp::init(argc, argv); auto ros2_node = rclcpp::Node::make_shared("ros_bridge"); - // a callback group for creating ros2 entity (client, service and subscription) later + // a callback group for creating ros2 entity (client, service) later auto callback_group = ros2_node->create_callback_group(rclcpp::CallbackGroupType::Reentrant); // ROS 1 node @@ -781,17 +781,9 @@ int main(int argc, char * argv[]) bridge_all_1to2_topics, bridge_all_2to1_topics); }; - auto check_ros1_flag = [&ros1_node] { - if (!ros1_node.ok()) { - rclcpp::shutdown(); - } - }; - auto ros2_poll_timer = ros2_node->create_wall_timer( - std::chrono::seconds(1), [&ros2_poll, &check_ros1_flag] { - ros2_poll(); - check_ros1_flag(); - }); + std::chrono::seconds(1), ros2_poll); + // ROS 1 asynchronous spinner ros::AsyncSpinner async_spinner(0); @@ -799,8 +791,9 @@ int main(int argc, char * argv[]) // ROS 2 spinning loop rclcpp::executors::MultiThreadedExecutor executor; - executor.add_node(ros2_node); - executor.spin(); + while (ros1_node.ok() && rclcpp::ok()) { + executor.spin_node_once(ros2_node); + } return 0; } From 597a205b090467f46ddf6ef4387d2350baeebbea Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Mon, 31 May 2021 15:19:51 +0800 Subject: [PATCH 06/10] use custom callbackqueue instead of ros1 global callbackqueue to fix 'Not all nodes were finished before finishing the context' since ros2 PublisherBase passed into ros1 subscriber as parameter which might be freed after ros2 context. Signed-off-by: Chen Lihui --- include/ros1_bridge/factory.hpp | 1 + src/dynamic_bridge.cpp | 5 ++++- 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/include/ros1_bridge/factory.hpp b/include/ros1_bridge/factory.hpp index cf532d16..b5f9d77e 100755 --- a/include/ros1_bridge/factory.hpp +++ b/include/ros1_bridge/factory.hpp @@ -16,6 +16,7 @@ #define ROS1_BRIDGE__FACTORY_HPP_ #include +#include #include #include #include diff --git a/src/dynamic_bridge.cpp b/src/dynamic_bridge.cpp index a6365d49..eb93ff98 100644 --- a/src/dynamic_bridge.cpp +++ b/src/dynamic_bridge.cpp @@ -25,6 +25,7 @@ # pragma clang diagnostic push # pragma clang diagnostic ignored "-Wunused-parameter" #endif +#include "ros/callback_queue.h" #include "ros/ros.h" #ifdef __clang__ # pragma clang diagnostic pop @@ -473,6 +474,8 @@ int main(int argc, char * argv[]) // ROS 1 node ros::init(argc, argv, "ros_bridge"); ros::NodeHandle ros1_node; + ros::CallbackQueue queue; + ros1_node.setCallbackQueue(&queue); // mapping of available topic names to type names std::map ros1_publishers; @@ -786,7 +789,7 @@ int main(int argc, char * argv[]) // ROS 1 asynchronous spinner - ros::AsyncSpinner async_spinner(0); + ros::AsyncSpinner async_spinner(0, &queue); async_spinner.start(); // ROS 2 spinning loop From 23d065f6d56c1afbe3f8c297d929a6014555a725 Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Mon, 31 May 2021 16:35:02 +0800 Subject: [PATCH 07/10] adjust creating callbackgroup location and rename the variable Signed-off-by: Chen Lihui --- include/ros1_bridge/factory.hpp | 14 +++++--------- src/dynamic_bridge.cpp | 8 +++----- 2 files changed, 8 insertions(+), 14 deletions(-) diff --git a/include/ros1_bridge/factory.hpp b/include/ros1_bridge/factory.hpp index b5f9d77e..4fa8d7db 100755 --- a/include/ros1_bridge/factory.hpp +++ b/include/ros1_bridge/factory.hpp @@ -41,14 +41,10 @@ static rclcpp::CallbackGroup::SharedPtr get_callback_group( auto node_base = ros2_node->get_node_base_interface(); auto group = node_base->get_default_callback_group(); if (topic_name.empty()) { - auto callback_groups = node_base->get_callback_groups(); - if (callback_groups.size() > 1) { - auto group_lock = callback_groups[1].lock(); - if (group_lock) { - group = group_lock; - } - } - return group; + // create a callback group with Reentrant for creating ros2 clients and services + static auto s_shared_callbackgroup = + ros2_node->create_callback_group(rclcpp::CallbackGroupType::Reentrant); + return s_shared_callbackgroup; } typedef std::map TopicCallbackGroupMap; @@ -58,7 +54,7 @@ static rclcpp::CallbackGroup::SharedPtr get_callback_group( return iter->second; } - // use CallbackGroup with MutuallyExclusive for each topic + // create one CallbackGroup with MutuallyExclusive for each topic // to ensure that the message data of each topic is received in order group = ros2_node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); s_topic_callbackgroups.insert({topic_name, group}); diff --git a/src/dynamic_bridge.cpp b/src/dynamic_bridge.cpp index eb93ff98..2d2bb53f 100644 --- a/src/dynamic_bridge.cpp +++ b/src/dynamic_bridge.cpp @@ -468,14 +468,12 @@ int main(int argc, char * argv[]) rclcpp::init(argc, argv); auto ros2_node = rclcpp::Node::make_shared("ros_bridge"); - // a callback group for creating ros2 entity (client, service) later - auto callback_group = ros2_node->create_callback_group(rclcpp::CallbackGroupType::Reentrant); // ROS 1 node ros::init(argc, argv, "ros_bridge"); ros::NodeHandle ros1_node; - ros::CallbackQueue queue; - ros1_node.setCallbackQueue(&queue); + ros::CallbackQueue ros1_callback_queue; + ros1_node.setCallbackQueue(&ros1_callback_queue); // mapping of available topic names to type names std::map ros1_publishers; @@ -789,7 +787,7 @@ int main(int argc, char * argv[]) // ROS 1 asynchronous spinner - ros::AsyncSpinner async_spinner(0, &queue); + ros::AsyncSpinner async_spinner(0, &ros1_callback_queue); async_spinner.start(); // ROS 2 spinning loop From f96ce5907bc23c7d913c1a98d87f39be15730231 Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Tue, 1 Jun 2021 11:02:06 +0800 Subject: [PATCH 08/10] update new tag version with v0.2 for ros-tooling/setup-ros Signed-off-by: Chen Lihui --- .github/workflows/main.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index 1d6aa37b..e6aed2ef 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -7,7 +7,7 @@ jobs: runs-on: ubuntu-latest steps: - uses: actions/checkout@v2 - - uses: ros-tooling/setup-ros@v0.1 + - uses: ros-tooling/setup-ros@v0.2 with: required-ros-distributions: "noetic rolling" - name: Build and test ros1-bridge From 879dfb8b24c326503a4906b0337d35e1f988a88c Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Tue, 8 Jun 2021 11:38:56 +0800 Subject: [PATCH 09/10] add option and refactor some code based on review Co-authored-by: Tomoya Fujita Signed-off-by: Chen Lihui --- include/ros1_bridge/bridge.hpp | 9 ++- include/ros1_bridge/factory.hpp | 66 +++++++++++--------- include/ros1_bridge/factory_interface.hpp | 15 +++-- src/bridge.cpp | 16 +++-- src/dynamic_bridge.cpp | 73 +++++++++++++++++------ 5 files changed, 120 insertions(+), 59 deletions(-) diff --git a/include/ros1_bridge/bridge.hpp b/include/ros1_bridge/bridge.hpp index c0485df9..7fb7614d 100755 --- a/include/ros1_bridge/bridge.hpp +++ b/include/ros1_bridge/bridge.hpp @@ -104,7 +104,8 @@ create_bridge_from_2_to_1( const std::string & ros1_type_name, const std::string & ros1_topic_name, size_t publisher_queue_size, - rclcpp::PublisherBase::SharedPtr ros2_pub = nullptr); + rclcpp::PublisherBase::SharedPtr ros2_pub = nullptr, + bool custom_callback_group = false); Bridge2to1Handles create_bridge_from_2_to_1( @@ -116,7 +117,8 @@ create_bridge_from_2_to_1( const std::string & ros1_type_name, const std::string & ros1_topic_name, size_t publisher_queue_size, - rclcpp::PublisherBase::SharedPtr ros2_pub = nullptr); + rclcpp::PublisherBase::SharedPtr ros2_pub = nullptr, + bool custom_callback_group = false); BridgeHandles create_bidirectional_bridge( @@ -125,7 +127,8 @@ create_bidirectional_bridge( const std::string & ros1_type_name, const std::string & ros2_type_name, const std::string & topic_name, - size_t queue_size = 10); + size_t queue_size = 10, + bool custom_callback_group = false); } // namespace ros1_bridge diff --git a/include/ros1_bridge/factory.hpp b/include/ros1_bridge/factory.hpp index 4fa8d7db..35ea046b 100755 --- a/include/ros1_bridge/factory.hpp +++ b/include/ros1_bridge/factory.hpp @@ -39,25 +39,23 @@ static rclcpp::CallbackGroup::SharedPtr get_callback_group( const std::string & topic_name = "") { auto node_base = ros2_node->get_node_base_interface(); - auto group = node_base->get_default_callback_group(); - if (topic_name.empty()) { - // create a callback group with Reentrant for creating ros2 clients and services - static auto s_shared_callbackgroup = - ros2_node->create_callback_group(rclcpp::CallbackGroupType::Reentrant); - return s_shared_callbackgroup; - } + rclcpp::CallbackGroup::SharedPtr group = nullptr; - typedef std::map TopicCallbackGroupMap; - static TopicCallbackGroupMap s_topic_callbackgroups; - auto iter = s_topic_callbackgroups.find(topic_name); - if (iter != s_topic_callbackgroups.end()) { + typedef std::map CallbackGroupMap; + static CallbackGroupMap s_callbackgroups; + auto iter = s_callbackgroups.find(topic_name); + if (iter != s_callbackgroups.end()) { return iter->second; } - // create one CallbackGroup with MutuallyExclusive for each topic - // to ensure that the message data of each topic is received in order - group = ros2_node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - s_topic_callbackgroups.insert({topic_name, group}); + group = ros2_node->create_callback_group( + topic_name.empty() ? + // create a shared callback group with Reentrant for creating all ros2 clients and services + rclcpp::CallbackGroupType::Reentrant : + // create one CallbackGroup with MutuallyExclusive for each topic + // to ensure that the message data of each topic is received in order + rclcpp::CallbackGroupType::MutuallyExclusive); + s_callbackgroups.insert({topic_name, group}); return group; } @@ -139,10 +137,12 @@ class Factory : public FactoryInterface const std::string & topic_name, size_t queue_size, ros::Publisher ros1_pub, - rclcpp::PublisherBase::SharedPtr ros2_pub = nullptr) + rclcpp::PublisherBase::SharedPtr ros2_pub = nullptr, + bool custom_callback_group = false) { auto qos = rclcpp::SensorDataQoS(rclcpp::KeepLast(queue_size)); - return create_ros2_subscriber(node, topic_name, qos, ros1_pub, ros2_pub); + return create_ros2_subscriber( + node, topic_name, qos, ros1_pub, ros2_pub, custom_callback_group); } rclcpp::SubscriptionBase::SharedPtr @@ -151,12 +151,13 @@ class Factory : public FactoryInterface const std::string & topic_name, const rmw_qos_profile_t & qos, ros::Publisher ros1_pub, - rclcpp::PublisherBase::SharedPtr ros2_pub = nullptr) + rclcpp::PublisherBase::SharedPtr ros2_pub = nullptr, + bool custom_callback_group = false) { auto rclcpp_qos = rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos)); rclcpp_qos.get_rmw_qos_profile() = qos; return create_ros2_subscriber( - node, topic_name, rclcpp_qos, ros1_pub, ros2_pub); + node, topic_name, rclcpp_qos, ros1_pub, ros2_pub, custom_callback_group); } rclcpp::SubscriptionBase::SharedPtr @@ -165,7 +166,8 @@ class Factory : public FactoryInterface const std::string & topic_name, const rclcpp::QoS & qos, ros::Publisher ros1_pub, - rclcpp::PublisherBase::SharedPtr ros2_pub = nullptr) + rclcpp::PublisherBase::SharedPtr ros2_pub = nullptr, + bool custom_callback_group = false) { std::function< void(const typename ROS2_T::SharedPtr msg, const rclcpp::MessageInfo & msg_info)> callback; @@ -174,7 +176,9 @@ class Factory : public FactoryInterface ros1_pub, ros1_type_name_, ros2_type_name_, node->get_logger(), ros2_pub); rclcpp::SubscriptionOptions options; options.ignore_local_publications = true; - options.callback_group = ros1_bridge::get_callback_group(node, topic_name); + if (custom_callback_group) { + options.callback_group = ros1_bridge::get_callback_group(node, topic_name); + } return node->create_subscription( topic_name, qos, callback, options); } @@ -355,12 +359,16 @@ class ServiceFactory : public ServiceFactoryInterface } ServiceBridge1to2 service_bridge_1_to_2( - ros::NodeHandle & ros1_node, rclcpp::Node::SharedPtr ros2_node, const std::string & name) + ros::NodeHandle & ros1_node, rclcpp::Node::SharedPtr ros2_node, const std::string & name, + bool custom_callback_group = false) { ServiceBridge1to2 bridge; + rclcpp::CallbackGroup::SharedPtr group = nullptr; + if (custom_callback_group) { + group = ros1_bridge::get_callback_group(ros2_node); + } bridge.client = ros2_node->create_client( - name, rmw_qos_profile_services_default, - ros1_bridge::get_callback_group(ros2_node)); + name, rmw_qos_profile_services_default, group); auto m = &ServiceFactory::forward_1_to_2; auto f = std::bind( m, this, bridge.client, ros2_node->get_logger(), std::placeholders::_1, @@ -370,7 +378,8 @@ class ServiceFactory : public ServiceFactoryInterface } ServiceBridge2to1 service_bridge_2_to_1( - ros::NodeHandle & ros1_node, rclcpp::Node::SharedPtr ros2_node, const std::string & name) + ros::NodeHandle & ros1_node, rclcpp::Node::SharedPtr ros2_node, const std::string & name, + bool custom_callback_group = false) { ServiceBridge2to1 bridge; bridge.client = ros1_node.serviceClient(name); @@ -383,9 +392,12 @@ class ServiceFactory : public ServiceFactoryInterface f = std::bind( m, this, bridge.client, ros2_node->get_logger(), std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); + rclcpp::CallbackGroup::SharedPtr group = nullptr; + if (custom_callback_group) { + group = ros1_bridge::get_callback_group(ros2_node); + } bridge.server = ros2_node->create_service( - name, f, rmw_qos_profile_services_default, - ros1_bridge::get_callback_group(ros2_node)); + name, f, rmw_qos_profile_services_default, group); return bridge; } diff --git a/include/ros1_bridge/factory_interface.hpp b/include/ros1_bridge/factory_interface.hpp index ee568d4d..9d4b1cd6 100755 --- a/include/ros1_bridge/factory_interface.hpp +++ b/include/ros1_bridge/factory_interface.hpp @@ -90,7 +90,8 @@ class FactoryInterface const std::string & topic_name, size_t queue_size, ros::Publisher ros1_pub, - rclcpp::PublisherBase::SharedPtr ros2_pub = nullptr) = 0; + rclcpp::PublisherBase::SharedPtr ros2_pub = nullptr, + bool custom_callback_group = false) = 0; virtual rclcpp::SubscriptionBase::SharedPtr @@ -99,7 +100,8 @@ class FactoryInterface const std::string & topic_name, const rmw_qos_profile_t & qos_profile, ros::Publisher ros1_pub, - rclcpp::PublisherBase::SharedPtr ros2_pub = nullptr) = 0; + rclcpp::PublisherBase::SharedPtr ros2_pub = nullptr, + bool custom_callback_group = false) = 0; virtual rclcpp::SubscriptionBase::SharedPtr @@ -108,7 +110,8 @@ class FactoryInterface const std::string & topic_name, const rclcpp::QoS & qos, ros::Publisher ros1_pub, - rclcpp::PublisherBase::SharedPtr ros2_pub = nullptr) = 0; + rclcpp::PublisherBase::SharedPtr ros2_pub = nullptr, + bool custom_callback_group = false) = 0; virtual void @@ -123,10 +126,12 @@ class ServiceFactoryInterface { public: virtual ServiceBridge1to2 service_bridge_1_to_2( - ros::NodeHandle &, rclcpp::Node::SharedPtr, const std::string &) = 0; + ros::NodeHandle &, rclcpp::Node::SharedPtr, const std::string &, + bool custom_callback_group = false) = 0; virtual ServiceBridge2to1 service_bridge_2_to_1( - ros::NodeHandle &, rclcpp::Node::SharedPtr, const std::string &) = 0; + ros::NodeHandle &, rclcpp::Node::SharedPtr, const std::string &, + bool custom_callback_group = false) = 0; }; } // namespace ros1_bridge diff --git a/src/bridge.cpp b/src/bridge.cpp index b8f0fa83..80efd87c 100755 --- a/src/bridge.cpp +++ b/src/bridge.cpp @@ -76,7 +76,8 @@ create_bridge_from_2_to_1( const std::string & ros1_type_name, const std::string & ros1_topic_name, size_t publisher_queue_size, - rclcpp::PublisherBase::SharedPtr ros2_pub) + rclcpp::PublisherBase::SharedPtr ros2_pub, + bool custom_callback_group) { auto subscriber_qos = rclcpp::SensorDataQoS(rclcpp::KeepLast(subscriber_queue_size)); return create_bridge_from_2_to_1( @@ -88,7 +89,8 @@ create_bridge_from_2_to_1( ros1_type_name, ros1_topic_name, publisher_queue_size, - ros2_pub); + ros2_pub, + custom_callback_group); } Bridge2to1Handles @@ -101,14 +103,15 @@ create_bridge_from_2_to_1( const std::string & ros1_type_name, const std::string & ros1_topic_name, size_t publisher_queue_size, - rclcpp::PublisherBase::SharedPtr ros2_pub) + rclcpp::PublisherBase::SharedPtr ros2_pub, + bool custom_callback_group) { auto factory = get_factory(ros1_type_name, ros2_type_name); auto ros1_pub = factory->create_ros1_publisher( ros1_node, ros1_topic_name, publisher_queue_size); auto ros2_sub = factory->create_ros2_subscriber( - ros2_node, ros2_topic_name, subscriber_qos, ros1_pub, ros2_pub); + ros2_node, ros2_topic_name, subscriber_qos, ros1_pub, ros2_pub, custom_callback_group); Bridge2to1Handles handles; handles.ros2_subscriber = ros2_sub; @@ -123,7 +126,8 @@ create_bidirectional_bridge( const std::string & ros1_type_name, const std::string & ros2_type_name, const std::string & topic_name, - size_t queue_size) + size_t queue_size, + bool custom_callback_group) { RCLCPP_INFO( ros2_node->get_logger(), "create bidirectional bridge for topic %s", @@ -135,7 +139,7 @@ create_bidirectional_bridge( handles.bridge2to1 = create_bridge_from_2_to_1( ros2_node, ros1_node, ros2_type_name, topic_name, queue_size, ros1_type_name, topic_name, queue_size, - handles.bridge1to2.ros2_publisher); + handles.bridge1to2.ros2_publisher, custom_callback_group); return handles; } diff --git a/src/dynamic_bridge.cpp b/src/dynamic_bridge.cpp index 2d2bb53f..de2ba7a9 100644 --- a/src/dynamic_bridge.cpp +++ b/src/dynamic_bridge.cpp @@ -71,7 +71,8 @@ bool get_flag_option(const std::vector & args, const std::string & bool parse_command_options( int argc, char ** argv, bool & output_topic_introspection, - bool & bridge_all_1to2_topics, bool & bridge_all_2to1_topics) + bool & bridge_all_1to2_topics, bool & bridge_all_2to1_topics, + bool & multi_threads) { std::vector args(argv, argv + argc); @@ -89,6 +90,8 @@ bool parse_command_options( ss << "a matching subscriber." << std::endl; ss << " --bridge-all-2to1-topics: Bridge all ROS 2 topics to ROS 1, whether or not there is "; ss << "a matching subscriber." << std::endl; + ss << " --multi-threads: Bridge with multiple threads for spinner of ROS 1 and ROS 2."; + ss << std::endl; std::cout << ss.str(); return false; } @@ -120,6 +123,7 @@ bool parse_command_options( bool bridge_all_topics = get_flag_option(args, "--bridge-all-topics"); bridge_all_1to2_topics = bridge_all_topics || get_flag_option(args, "--bridge-all-1to2-topics"); bridge_all_2to1_topics = bridge_all_topics || get_flag_option(args, "--bridge-all-2to1-topics"); + multi_threads = get_flag_option(args, "--multi-threads"); return true; } @@ -137,7 +141,8 @@ void update_bridge( std::map & bridges_2to1, std::map & service_bridges_1_to_2, std::map & service_bridges_2_to_1, - bool bridge_all_1to2_topics, bool bridge_all_2to1_topics) + bool bridge_all_1to2_topics, bool bridge_all_2to1_topics, + bool multi_threads = false) { std::lock_guard lock(g_bridge_mutex); @@ -257,7 +262,9 @@ void update_bridge( bridge.bridge_handles = ros1_bridge::create_bridge_from_2_to_1( ros2_node, ros1_node, bridge.ros2_type_name, topic_name, 10, - bridge.ros1_type_name, topic_name, 10); + bridge.ros1_type_name, topic_name, 10, + nullptr, + multi_threads); } catch (std::runtime_error & e) { fprintf( stderr, @@ -319,7 +326,8 @@ void update_bridge( "ros1", details.at("package"), details.at("name")); if (factory) { try { - service_bridges_2_to_1[name] = factory->service_bridge_2_to_1(ros1_node, ros2_node, name); + service_bridges_2_to_1[name] = factory->service_bridge_2_to_1( + ros1_node, ros2_node, name, multi_threads); printf("Created 2 to 1 bridge for service %s\n", name.data()); } catch (std::runtime_error & e) { fprintf(stderr, "Failed to created a bridge: %s\n", e.what()); @@ -340,7 +348,8 @@ void update_bridge( "ros2", details.at("package"), details.at("name")); if (factory) { try { - service_bridges_1_to_2[name] = factory->service_bridge_1_to_2(ros1_node, ros2_node, name); + service_bridges_1_to_2[name] = factory->service_bridge_1_to_2( + ros1_node, ros2_node, name, multi_threads); printf("Created 1 to 2 bridge for service %s\n", name.data()); } catch (std::runtime_error & e) { fprintf(stderr, "Failed to created a bridge: %s\n", e.what()); @@ -458,8 +467,10 @@ int main(int argc, char * argv[]) bool output_topic_introspection; bool bridge_all_1to2_topics; bool bridge_all_2to1_topics; + bool multi_threads; if (!parse_command_options( - argc, argv, output_topic_introspection, bridge_all_1to2_topics, bridge_all_2to1_topics)) + argc, argv, output_topic_introspection, bridge_all_1to2_topics, bridge_all_2to1_topics, + multi_threads)) { return 0; } @@ -472,8 +483,12 @@ int main(int argc, char * argv[]) // ROS 1 node ros::init(argc, argv, "ros_bridge"); ros::NodeHandle ros1_node; - ros::CallbackQueue ros1_callback_queue; - ros1_node.setCallbackQueue(&ros1_callback_queue); + std::unique_ptr ros1_callback_queue = nullptr; + if (multi_threads) { + ros1_callback_queue = std::make_unique(); + ros1_node.setCallbackQueue(ros1_callback_queue.get()); + } + // mapping of available topic names to type names std::map ros1_publishers; @@ -497,7 +512,8 @@ int main(int argc, char * argv[]) &ros1_services, &ros2_services, &service_bridges_1_to_2, &service_bridges_2_to_1, &output_topic_introspection, - &bridge_all_1to2_topics, &bridge_all_2to1_topics + &bridge_all_1to2_topics, &bridge_all_2to1_topics, + multi_threads ](const ros::TimerEvent &) -> void { // collect all topics names which have at least one publisher or subscriber beside this bridge @@ -615,7 +631,8 @@ int main(int argc, char * argv[]) ros1_services, ros2_services, bridges_1to2, bridges_2to1, service_bridges_1_to_2, service_bridges_2_to_1, - bridge_all_1to2_topics, bridge_all_2to1_topics); + bridge_all_1to2_topics, bridge_all_2to1_topics, + multi_threads); }; auto ros1_poll_timer = ros1_node.createTimer(ros::Duration(1.0), ros1_poll); @@ -634,7 +651,8 @@ int main(int argc, char * argv[]) &service_bridges_1_to_2, &service_bridges_2_to_1, &output_topic_introspection, &bridge_all_1to2_topics, &bridge_all_2to1_topics, - &already_ignored_topics, &already_ignored_services + &already_ignored_topics, &already_ignored_services, + multi_threads ]() -> void { auto ros2_topics = ros2_node->get_topic_names_and_types(); @@ -779,22 +797,41 @@ int main(int argc, char * argv[]) ros1_services, ros2_services, bridges_1to2, bridges_2to1, service_bridges_1_to_2, service_bridges_2_to_1, - bridge_all_1to2_topics, bridge_all_2to1_topics); + bridge_all_1to2_topics, bridge_all_2to1_topics, + multi_threads); + }; + + auto check_ros1_flag = [&ros1_node] { + if (!ros1_node.ok()) { + rclcpp::shutdown(); + } }; auto ros2_poll_timer = ros2_node->create_wall_timer( - std::chrono::seconds(1), ros2_poll); + std::chrono::seconds(1), [&ros2_poll, &check_ros1_flag] { + ros2_poll(); + check_ros1_flag(); + }); // ROS 1 asynchronous spinner - ros::AsyncSpinner async_spinner(0, &ros1_callback_queue); - async_spinner.start(); + std::unique_ptr async_spinner = nullptr; + if (!multi_threads) { + async_spinner = std::make_unique(1); + } else { + async_spinner = std::make_unique(0, ros1_callback_queue.get()); + } + async_spinner->start(); // ROS 2 spinning loop - rclcpp::executors::MultiThreadedExecutor executor; - while (ros1_node.ok() && rclcpp::ok()) { - executor.spin_node_once(ros2_node); + std::unique_ptr executor = nullptr; + if (!multi_threads) { + executor = std::make_unique(); + } else { + executor = std::make_unique(); } + executor->add_node(ros2_node); + executor->spin(); return 0; } From 0d41087557fe56052bbe408f00ebae058371a2d8 Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Tue, 8 Jun 2021 15:23:09 +0800 Subject: [PATCH 10/10] update test for --multi-threads option Signed-off-by: Chen Lihui --- CMakeLists.txt | 73 +++++++++++-------- .../test_services_across_dynamic_bridge.py.in | 3 +- test/test_topics_across_dynamic_bridge.py.in | 3 +- 3 files changed, 48 insertions(+), 31 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index b303dc77..af79cdc6 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -223,35 +223,44 @@ endif() macro(targets) set(TEST_BRIDGE_RMW ${rmw_implementation}) - configure_file( - test/test_topics_across_dynamic_bridge.py.in - test_topics_across_dynamic_bridge${target_suffix}.py.genexp - @ONLY - ) - file(GENERATE - OUTPUT "${CMAKE_CURRENT_BINARY_DIR}/test_topics_across_dynamic_bridge${target_suffix}_$.py" - INPUT "${CMAKE_CURRENT_BINARY_DIR}/test_topics_across_dynamic_bridge${target_suffix}.py.genexp" - ) - add_launch_test( - "${CMAKE_CURRENT_BINARY_DIR}/test_topics_across_dynamic_bridge${target_suffix}_$.py" - TARGET test_topics_across_dynamic_bridge${target_suffix} - ENV RMW_IMPLEMENTATION=${rmw_implementation} - TIMEOUT 60) - - configure_file( - test/test_services_across_dynamic_bridge.py.in - test_services_across_dynamic_bridge${target_suffix}.py.genexp - @ONLY - ) - file(GENERATE - OUTPUT "${CMAKE_CURRENT_BINARY_DIR}/test_services_across_dynamic_bridge${target_suffix}_$.py" - INPUT "${CMAKE_CURRENT_BINARY_DIR}/test_services_across_dynamic_bridge${target_suffix}.py.genexp" - ) - add_launch_test( - "${CMAKE_CURRENT_BINARY_DIR}/test_services_across_dynamic_bridge${target_suffix}_$.py" - TARGET test_services_across_dynamic_bridge${target_suffix} - ENV RMW_IMPLEMENTATION=${rmw_implementation} - TIMEOUT 60) + list(LENGTH DYNAMIC_BRIDGE_TYPE count) + math(EXPR count "${count}-1") + foreach(i RANGE ${count}) + list(GET DYNAMIC_BRIDGE_TYPE ${i} bridge_type) + list(GET DYNAMIC_BRIDGE_ARG ${i} bridge_arg) + set(TEST_BRIDGE_DYNAMIC_BRIDGE_ARG "${bridge_arg}") + + configure_file( + test/test_topics_across_dynamic_bridge.py.in + test_topics_across_dynamic_bridge${target_suffix}_${bridge_type}.py.genexp + @ONLY + ) + file(GENERATE + OUTPUT "${CMAKE_CURRENT_BINARY_DIR}/test_topics_across_dynamic_bridge${target_suffix}_${bridge_type}_$.py" + INPUT "${CMAKE_CURRENT_BINARY_DIR}/test_topics_across_dynamic_bridge${target_suffix}_${bridge_type}.py.genexp" + ) + add_launch_test( + "${CMAKE_CURRENT_BINARY_DIR}/test_topics_across_dynamic_bridge${target_suffix}_${bridge_type}_$.py" + TARGET test_topics_across_dynamic_bridge${target_suffix}_${bridge_type} + ENV RMW_IMPLEMENTATION=${rmw_implementation} + TIMEOUT 60) + + configure_file( + test/test_services_across_dynamic_bridge.py.in + test_services_across_dynamic_bridge${target_suffix}_${bridge_type}.py.genexp + @ONLY + ) + file(GENERATE + OUTPUT "${CMAKE_CURRENT_BINARY_DIR}/test_services_across_dynamic_bridge${target_suffix}_${bridge_type}_$.py" + INPUT "${CMAKE_CURRENT_BINARY_DIR}/test_services_across_dynamic_bridge${target_suffix}_${bridge_type}.py.genexp" + ) + add_launch_test( + "${CMAKE_CURRENT_BINARY_DIR}/test_services_across_dynamic_bridge${target_suffix}_${bridge_type}_$.py" + TARGET test_services_across_dynamic_bridge${target_suffix}_${bridge_type} + ENV RMW_IMPLEMENTATION=${rmw_implementation} + TIMEOUT 60) + endforeach() + endmacro() if(TEST_ROS1_BRIDGE) @@ -266,6 +275,12 @@ if(TEST_ROS1_BRIDGE) set(TEST_BRIDGE_ROS1_CLIENT "$") set(TEST_BRIDGE_ROS1_SERVER "$") + list(APPEND DYNAMIC_BRIDGE_TYPE "st") + list(APPEND DYNAMIC_BRIDGE_ARG " ") + + list(APPEND DYNAMIC_BRIDGE_TYPE "mt") + list(APPEND DYNAMIC_BRIDGE_ARG "--multi-threads") + call_for_each_rmw_implementation(targets) endif() diff --git a/test/test_services_across_dynamic_bridge.py.in b/test/test_services_across_dynamic_bridge.py.in index 757c94a4..5e72257f 100644 --- a/test/test_services_across_dynamic_bridge.py.in +++ b/test/test_services_across_dynamic_bridge.py.in @@ -27,6 +27,7 @@ TEST_BRIDGE_ROSCORE = '@TEST_BRIDGE_ROSCORE@' TEST_BRIDGE_ROS1_CLIENT = '@TEST_BRIDGE_ROS1_CLIENT@' TEST_BRIDGE_ROS1_SERVER = '@TEST_BRIDGE_ROS1_SERVER@' TEST_BRIDGE_DYNAMIC_BRIDGE = '@TEST_BRIDGE_DYNAMIC_BRIDGE@' +TEST_BRIDGE_DYNAMIC_BRIDGE_ARG = '@TEST_BRIDGE_DYNAMIC_BRIDGE_ARG@' TEST_BRIDGE_ROS2_CLIENT = '@TEST_BRIDGE_ROS2_CLIENT@' TEST_BRIDGE_ROS2_SERVER = '@TEST_BRIDGE_ROS2_SERVER@' @@ -50,7 +51,7 @@ def generate_test_description(test_name, server_cmd, client_cmd): # dynamic bridge rosbridge_process = ExecuteProcess( - cmd=[TEST_BRIDGE_ROS1_ENV, TEST_BRIDGE_DYNAMIC_BRIDGE], + cmd=[TEST_BRIDGE_ROS1_ENV, TEST_BRIDGE_DYNAMIC_BRIDGE, TEST_BRIDGE_DYNAMIC_BRIDGE_ARG], name=test_name + '__dynamic_bridge', ) launch_description.add_action(rosbridge_process) diff --git a/test/test_topics_across_dynamic_bridge.py.in b/test/test_topics_across_dynamic_bridge.py.in index b50691bc..3b103e49 100644 --- a/test/test_topics_across_dynamic_bridge.py.in +++ b/test/test_topics_across_dynamic_bridge.py.in @@ -33,6 +33,7 @@ TEST_BRIDGE_ROSCORE = '@TEST_BRIDGE_ROSCORE@' TEST_BRIDGE_ROS1_TALKER = ['rosrun', 'roscpp_tutorials', 'talker'] TEST_BRIDGE_ROS1_LISTENER = ['rosrun', 'rospy_tutorials', 'listener'] TEST_BRIDGE_DYNAMIC_BRIDGE = '@TEST_BRIDGE_DYNAMIC_BRIDGE@' +TEST_BRIDGE_DYNAMIC_BRIDGE_ARG = '@TEST_BRIDGE_DYNAMIC_BRIDGE_ARG@' TEST_BRIDGE_ROS2_TALKER = get_executable_path( package_name='demo_nodes_cpp', executable_name='talker') TEST_BRIDGE_ROS2_LISTENER = get_executable_path( @@ -59,7 +60,7 @@ def generate_test_description(test_name, talker_cmd, listener_cmd, logs_stream): # dynamic bridge rosbridge_process = ExecuteProcess( - cmd=[TEST_BRIDGE_ROS1_ENV, TEST_BRIDGE_DYNAMIC_BRIDGE], + cmd=[TEST_BRIDGE_ROS1_ENV, TEST_BRIDGE_DYNAMIC_BRIDGE, TEST_BRIDGE_DYNAMIC_BRIDGE_ARG], name=test_name + '__dynamic_bridge', ) launch_description.add_action(rosbridge_process)