From 099e7e1507390a70ebac56ba24f6884ac3246116 Mon Sep 17 00:00:00 2001 From: Harsh Deshpande Date: Fri, 25 Nov 2022 08:49:58 +0100 Subject: [PATCH 01/19] Update CMakeLists.txt Co-authored-by: Geoffrey Biggs Signed-off-by: Harsh Deshpande --- CMakeLists.txt | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 01d44b56..97fc39d2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -191,7 +191,8 @@ add_library(${PROJECT_NAME} SHARED ament_target_dependencies(${PROJECT_NAME} ${prefixed_ros1_message_packages} ${ros2_interface_packages} - "rclcpp" "rclcpp_action" + "rclcpp" + "rclcpp_action" "ros1_roscpp" "ros1_std_msgs") From 553db795b06eeafb462bf335615b7ab05e2eccfb Mon Sep 17 00:00:00 2001 From: Harsh Deshpande Date: Fri, 25 Nov 2022 08:50:43 +0100 Subject: [PATCH 02/19] Update README.md Co-authored-by: Geoffrey Biggs Signed-off-by: Harsh Deshpande --- README.md | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/README.md b/README.md index 49263fdf..4c75bb3d 100644 --- a/README.md +++ b/README.md @@ -505,9 +505,9 @@ Note that the `qos` section can be omitted entirely and options not set are left # Action bridge -This bridge extends the `ros1_bridge` to action interface. The bridge works in both directions, meaning an action goal can be sent from ROS1 client to ROS2 server, or from ROS2 client to ROS1 server. +This bridge extends the `ros1_bridge` to support actions. The bridge works in both directions, meaning an action goal can be sent from ROS 1 client to ROS 2 server, or from ROS 2 client to ROS 1 server. -The arguments for `action_bridge` node are: +The arguments for the `action_bridge` node are: `direction`: from client (`ros1` or `ros2`) e.g.: - `ROS1` client to `ROS2` server --> `direction` = `ros1` @@ -517,34 +517,34 @@ e.g.: `type`: action interface type of `ROS1` `name`: action name -For sending goals from ROS2 action client to ROS1 action server +For sending goals from a ROS 2 action client to a ROS 1 action server ``` # Terminal 1 -- action bridge # Make sure roscore is already running source /setup.bash ros2 run ros1_bridge action_bridge ros1 actionlib_tutorials Fibonacci fibonacci -# Terminal 2 -- ROS1 action server +# Terminal 2 -- ROS 1 action server source /setup.bash rosrun actionlib_tutorials fibonacci_server -# Terminal 3 -- ROS2 action client +# Terminal 3 -- ROS 2 action client source /setup.bash ros2 run action_tutorials_cpp fibonacci_action_client 20 ``` -For sending goals from ROS1 action client to ROS2 action server +For sending goals from a ROS 1 action client to a ROS 2 action server ``` # Terminal 1 -- action bridge # Make sure roscore is already running source /setup.bash ros2 run ros1_bridge action_bridge ros2 action_tutorials_interfaces action/Fibonacci fibonacci -# Terminal 2 -- ROS2 action server +# Terminal 2 -- ROS 2 action server source /setup.bash ros2 run action_tutorials_cpp fibonacci_action_server -# Terminal 3 -- ROS1 action client +# Terminal 3 -- ROS 1 action client source /setup.bash rosrun actionlib_tutorials fibonacci_client 20 ``` From 9048f326ebe82cb5284aafbfda110ee8831c486a Mon Sep 17 00:00:00 2001 From: Harsh Deshpande Date: Fri, 25 Nov 2022 08:54:28 +0100 Subject: [PATCH 03/19] Update include/ros1_bridge/action_factory.hpp Co-authored-by: Geoffrey Biggs Signed-off-by: Harsh Deshpande --- include/ros1_bridge/action_factory.hpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/include/ros1_bridge/action_factory.hpp b/include/ros1_bridge/action_factory.hpp index 3a67ebad..03cd5413 100644 --- a/include/ros1_bridge/action_factory.hpp +++ b/include/ros1_bridge/action_factory.hpp @@ -122,7 +122,7 @@ class ActionFactory_1_2 : public ActionFactoryInterface private: class GoalHandler { -public: + public: void cancel() { std::lock_guard lock(mutex_); @@ -131,6 +131,7 @@ class ActionFactory_1_2 : public ActionFactoryInterface auto fut = client_->async_cancel_goal(gh2_); } } + void handle() { auto goal1 = gh1_.getGoal(); @@ -196,7 +197,7 @@ class ActionFactory_1_2 : public ActionFactoryInterface GoalHandler(ROS1GoalHandle & gh1, ROS2ClientSharedPtr & client) : gh1_(gh1), gh2_(nullptr), client_(client), canceled_(false) {} -private: + private: ROS1GoalHandle gh1_; ROS2ClientGoalHandle gh2_; ROS2ClientSharedPtr client_; @@ -314,7 +315,7 @@ class ActionFactory_2_1 : public ActionFactoryInterface private: class GoalHandler { -public: + public: void cancel() { std::lock_guard lock(mutex_); @@ -384,7 +385,7 @@ class ActionFactory_2_1 : public ActionFactoryInterface GoalHandler(std::shared_ptr & gh2, std::shared_ptr & client) : gh2_(gh2), client_(client), canceled_(false) {} -private: + private: std::shared_ptr gh1_; std::shared_ptr gh2_; std::shared_ptr client_; From ab8e4d2439195fc218eccfe041a2385733ffe6a2 Mon Sep 17 00:00:00 2001 From: Harsh Deshpande Date: Fri, 25 Nov 2022 08:58:25 +0100 Subject: [PATCH 04/19] Update resource/get_factory.cpp.em Co-authored-by: Geoffrey Biggs Signed-off-by: Harsh Deshpande --- resource/get_factory.cpp.em | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/resource/get_factory.cpp.em b/resource/get_factory.cpp.em index 3e8f0e48..a1745ebf 100644 --- a/resource/get_factory.cpp.em +++ b/resource/get_factory.cpp.em @@ -85,7 +85,7 @@ std::unique_ptr get_action_factory(const std::string & r } @[end for]@ // fprintf(stderr, "No template specialization for the service %s:%s/%s\n", ros_id.data(), package_name.data(), service_name.data()); - return factory; + return null_ptr; } } // namespace ros1_bridge From 03e9a135e32071e9cb9b59cde9d180070a43b010 Mon Sep 17 00:00:00 2001 From: Harsh Deshpande Date: Fri, 25 Nov 2022 08:59:41 +0100 Subject: [PATCH 05/19] Update ros1_bridge/__init__.py Co-authored-by: Geoffrey Biggs Signed-off-by: Harsh Deshpande --- ros1_bridge/__init__.py | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/ros1_bridge/__init__.py b/ros1_bridge/__init__.py index efda0160..407215e5 100644 --- a/ros1_bridge/__init__.py +++ b/ros1_bridge/__init__.py @@ -814,10 +814,10 @@ def determine_common_services( ros2_name = ros2_fields[direction][i].name if ros1_name != ros2_name: # if the names do not match, check first if the types are builtin - ros1_is_buildin = genmsg.msgs.is_builtin(genmsg.msgs.bare_msg_type(ros1_type)) - ros2_is_buildin = ros2_fields[direction][i].type.is_primitive_type() + ros1_is_builtin = genmsg.msgs.is_builtin(genmsg.msgs.bare_msg_type(ros1_type)) + ros2_is_builtin = ros2_fields[direction][i].type.is_primitive_type() # Check if types are primitive types - if not ros1_is_buildin or not ros2_is_buildin or ros1_type != ros2_type: + if not ros1_is_builtin or not ros2_is_builtin or ros1_type != ros2_type: # if the message types have a custom mapping their names # might not be equal, therefore check the message pairs if ((ros1_type, ros2_type) not in message_string_pairs and @@ -910,11 +910,11 @@ def determine_common_actions( # if the message types have a custom mapping their names # might not be equal, therefore check the message pairs - ros1_is_buildin = genmsg.msgs.is_builtin(genmsg.msgs.bare_msg_type(ros1_type)) - ros2_is_buildin = ros2_fields[direction][i].type.is_primitive_type() + ros1_is_builtin = genmsg.msgs.is_builtin(genmsg.msgs.bare_msg_type(ros1_type)) + ros2_is_builtin = ros2_fields[direction][i].type.is_primitive_type() # Check if types are primitive types - if not ros1_is_buildin or not ros2_is_buildin or ros1_type != ros2_type: + if not ros1_is_builtin or not ros2_is_builtin or ros1_type != ros2_type: # the check for 'builtin_interfaces' should be removed # once merged with __init__.py # It seems to handle it already @@ -1296,12 +1296,12 @@ def load_ros2_service(ros2_srv): def load_ros2_action(ros2_action): - actiom_path = os.path.join( + action_path = os.path.join( ros2_action.prefix_path, 'share', ros2_action.package_name, 'action', ros2_action.message_name + '.action') try: spec = rosidl_adapter.parser.parse_action_file( - ros2_action.package_name, actiom_path) + ros2_action.package_name, action_path) except rosidl_adapter.parser.InvalidSpecification: print('Invalid spec') return None From a988cce6d252f1d384c16cc913adcb0225839ec4 Mon Sep 17 00:00:00 2001 From: Harsh Deshpande Date: Fri, 25 Nov 2022 09:04:16 +0100 Subject: [PATCH 06/19] Update src/action_bridge.cpp Co-authored-by: Geoffrey Biggs Signed-off-by: Harsh Deshpande --- src/action_bridge.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/action_bridge.cpp b/src/action_bridge.cpp index bb0185b1..14c35d62 100644 --- a/src/action_bridge.cpp +++ b/src/action_bridge.cpp @@ -46,14 +46,12 @@ int main(int argc, char * argv[]) std::cout << dir << " " << package << " " << type << " " << name << std::endl; - // ros1 example_tutorials Fibonacci fibonacci auto factory = ros1_bridge::get_action_factory(dir, package, type); if (factory) { printf("created action factory\n"); try { factory->create_server_client(ros1_node, ros2_node, name); - // 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()); } From ef4c49e02a8437418824a22dca054d2e9718f75f Mon Sep 17 00:00:00 2001 From: Harsh Deshpande Date: Mon, 27 Mar 2023 10:04:38 +0200 Subject: [PATCH 07/19] risk leaking if the reset call crashes somehow Signed-off-by: Harsh Deshpande --- include/ros1_bridge/action_factory.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/include/ros1_bridge/action_factory.hpp b/include/ros1_bridge/action_factory.hpp index 03cd5413..af8d41ba 100644 --- a/include/ros1_bridge/action_factory.hpp +++ b/include/ros1_bridge/action_factory.hpp @@ -103,7 +103,7 @@ class ActionFactory_1_2 : public ActionFactoryInterface // create a new handler for the goal std::shared_ptr handler; - handler.reset(new GoalHandler(gh1, client_)); + handler = std::make_shared(gh1, client_)); std::lock_guard lock(mutex_); goals_.insert(std::make_pair(goal_id, handler)); @@ -296,7 +296,7 @@ class ActionFactory_2_1 : public ActionFactoryInterface { std::size_t goal_id = get_goal_id_hash(gh2->get_goal_id()); std::shared_ptr handler; - handler.reset(new GoalHandler(gh2, client_)); + handler = std::make_shared(gh2, client_)); std::lock_guard lock(mutex_); goals_.insert(std::make_pair(goal_id, handler)); From 0bcdfd541aa9700e0d55de615675dd0a19b77bdb Mon Sep 17 00:00:00 2001 From: Harsh Deshpande Date: Mon, 27 Mar 2023 10:10:50 +0200 Subject: [PATCH 08/19] Change cout to use RCLCPP logging macros. Signed-off-by: Harsh Deshpande --- include/ros1_bridge/action_factory.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/include/ros1_bridge/action_factory.hpp b/include/ros1_bridge/action_factory.hpp index af8d41ba..9dfc2a42 100644 --- a/include/ros1_bridge/action_factory.hpp +++ b/include/ros1_bridge/action_factory.hpp @@ -139,7 +139,7 @@ class ActionFactory_1_2 : public ActionFactoryInterface translate_goal_1_to_2(*gh1_.getGoal(), goal2); if (!client_->wait_for_action_server(std::chrono::seconds(1))) { - std::cout << "Action server not available after waiting" << std::endl; + RCLCPP_INFO(ros2_node_->get_logger(), "Action server not available after waiting"); gh1_.setRejected(); return; } @@ -273,7 +273,7 @@ class ActionFactory_2_1 : public ActionFactoryInterface (void)uuid; (void)goal; if (!client_->waitForActionServerToStart(ros::Duration(1))) { - std::cout << "Action server not available after waiting" << std::endl; + RCLCPP_INFO(ros2_node_->get_logger(), "Action server not available after waiting"); return rclcpp_action::GoalResponse::REJECT; } From 71cea80a8ee2c20026d8906df408955fc1949aa7 Mon Sep 17 00:00:00 2001 From: Harsh Deshpande Date: Mon, 27 Mar 2023 10:16:06 +0200 Subject: [PATCH 09/19] remove commented out code Signed-off-by: Harsh Deshpande --- resource/get_factory.cpp.em | 2 -- 1 file changed, 2 deletions(-) diff --git a/resource/get_factory.cpp.em b/resource/get_factory.cpp.em index a1745ebf..955fdef6 100644 --- a/resource/get_factory.cpp.em +++ b/resource/get_factory.cpp.em @@ -65,7 +65,6 @@ std::unique_ptr get_service_factory(const std::string & return factory; } @[end for]@ - // fprintf(stderr, "No template specialization for the service %s:%s/%s\n", ros_id.data(), package_name.data(), service_name.data()); return factory; } @@ -84,7 +83,6 @@ std::unique_ptr get_action_factory(const std::string & r return factory; } @[end for]@ - // fprintf(stderr, "No template specialization for the service %s:%s/%s\n", ros_id.data(), package_name.data(), service_name.data()); return null_ptr; } From 2a4e7b5f390eca2887c2e647aaf175b965175b51 Mon Sep 17 00:00:00 2001 From: Harsh Deshpande Date: Mon, 27 Mar 2023 12:48:28 +0200 Subject: [PATCH 10/19] fix parenthesis Signed-off-by: Harsh Deshpande --- include/ros1_bridge/action_factory.hpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/include/ros1_bridge/action_factory.hpp b/include/ros1_bridge/action_factory.hpp index 9dfc2a42..864a7772 100644 --- a/include/ros1_bridge/action_factory.hpp +++ b/include/ros1_bridge/action_factory.hpp @@ -27,7 +27,6 @@ #ifdef __clang__ #pragma clang diagnostic push -#pragma clang diagnostic ignored "-Wunused-parameter" #endif #include #include // Need this for the goal state. Need a better solution @@ -103,7 +102,7 @@ class ActionFactory_1_2 : public ActionFactoryInterface // create a new handler for the goal std::shared_ptr handler; - handler = std::make_shared(gh1, client_)); + handler = std::make_shared(gh1, client_); std::lock_guard lock(mutex_); goals_.insert(std::make_pair(goal_id, handler)); @@ -296,7 +295,7 @@ class ActionFactory_2_1 : public ActionFactoryInterface { std::size_t goal_id = get_goal_id_hash(gh2->get_goal_id()); std::shared_ptr handler; - handler = std::make_shared(gh2, client_)); + handler = std::make_shared(gh2, client_); std::lock_guard lock(mutex_); goals_.insert(std::make_pair(goal_id, handler)); From 2916013998343b7cd54188b72b74a6eedcfa3015 Mon Sep 17 00:00:00 2001 From: Harsh Deshpande Date: Mon, 27 Mar 2023 12:53:35 +0200 Subject: [PATCH 11/19] remove unused params Signed-off-by: Harsh Deshpande --- src/action_bridge.cpp | 1 - src/dynamic_bridge.cpp | 8 ++------ 2 files changed, 2 insertions(+), 7 deletions(-) diff --git a/src/action_bridge.cpp b/src/action_bridge.cpp index 14c35d62..16f2c881 100644 --- a/src/action_bridge.cpp +++ b/src/action_bridge.cpp @@ -17,7 +17,6 @@ // include ROS 1 #ifdef __clang__ #pragma clang diagnostic push -#pragma clang diagnostic ignored "-Wunused-parameter" #endif #include "ros/ros.h" #ifdef __clang__ diff --git a/src/dynamic_bridge.cpp b/src/dynamic_bridge.cpp index 2a5841a7..8c9ec341 100644 --- a/src/dynamic_bridge.cpp +++ b/src/dynamic_bridge.cpp @@ -109,9 +109,7 @@ void update_bridge( const std::map> & ros1_services, const std::map> & ros2_services, const std::map> & ros1_action_servers, - const std::map> & ros1_action_clients, const std::map> & ros2_action_servers, - const std::map> & ros2_action_clients, std::map & bridges_1to2, std::map & bridges_2to1, std::map & service_bridges_1_to_2, @@ -763,8 +761,7 @@ int main(int argc, char * argv[]) ros1_publishers, ros1_subscribers, ros2_publishers, ros2_subscribers, ros1_services, ros2_services, - ros1_action_servers, ros1_action_clients, - ros2_action_servers, ros2_action_clients, + ros1_action_servers, ros2_action_servers, bridges_1to2, bridges_2to1, service_bridges_1_to_2, service_bridges_2_to_1, action_bridges_1_to_2, action_bridges_2_to_1, @@ -830,8 +827,7 @@ int main(int argc, char * argv[]) ros1_publishers, ros1_subscribers, ros2_publishers, ros2_subscribers, ros1_services, ros2_services, - ros1_action_servers, ros1_action_clients, - ros2_action_servers, ros2_action_clients, + ros1_action_servers, ros2_action_servers, bridges_1to2, bridges_2to1, service_bridges_1_to_2, service_bridges_2_to_1, action_bridges_1_to_2, action_bridges_2_to_1, From bf61c2959ff30013a69d124ee6ee8f21c4e436c2 Mon Sep 17 00:00:00 2001 From: Harsh Deshpande Date: Mon, 27 Mar 2023 12:53:51 +0200 Subject: [PATCH 12/19] fix nullptr keyword Signed-off-by: Harsh Deshpande --- resource/get_factory.cpp.em | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/resource/get_factory.cpp.em b/resource/get_factory.cpp.em index 955fdef6..758844df 100644 --- a/resource/get_factory.cpp.em +++ b/resource/get_factory.cpp.em @@ -83,7 +83,7 @@ std::unique_ptr get_action_factory(const std::string & r return factory; } @[end for]@ - return null_ptr; + return nullptr; } } // namespace ros1_bridge From 9d58aac1ce5c61b800588b851d753cae81dc495e Mon Sep 17 00:00:00 2001 From: Harsh Deshpande Date: Mon, 27 Mar 2023 13:01:23 +0200 Subject: [PATCH 13/19] remove outdated comments Signed-off-by: Harsh Deshpande --- include/ros1_bridge/action_factory.hpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/include/ros1_bridge/action_factory.hpp b/include/ros1_bridge/action_factory.hpp index 864a7772..f5162b99 100644 --- a/include/ros1_bridge/action_factory.hpp +++ b/include/ros1_bridge/action_factory.hpp @@ -143,10 +143,7 @@ class ActionFactory_1_2 : public ActionFactoryInterface return; } - // goal_response_callback signature changed after foxy, this implementation - // works with both std::shared_future gh2_future; - // Changes as per Dashing auto send_goal_ops = ROS2SendGoalOptions(); send_goal_ops.goal_response_callback = [this, &gh2_future](std::shared_future gh2) mutable { From e855ecb165bb8982b67e7dd422a3f4d04155e94d Mon Sep 17 00:00:00 2001 From: Harsh Deshpande Date: Mon, 27 Mar 2023 17:20:13 +0200 Subject: [PATCH 14/19] pass logger to GoalHandler Signed-off-by: Harsh Deshpande --- include/ros1_bridge/action_factory.hpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/include/ros1_bridge/action_factory.hpp b/include/ros1_bridge/action_factory.hpp index f5162b99..fd386a56 100644 --- a/include/ros1_bridge/action_factory.hpp +++ b/include/ros1_bridge/action_factory.hpp @@ -102,7 +102,7 @@ class ActionFactory_1_2 : public ActionFactoryInterface // create a new handler for the goal std::shared_ptr handler; - handler = std::make_shared(gh1, client_); + handler = std::make_shared(gh1, client_, ros2_node_->get_logger()); std::lock_guard lock(mutex_); goals_.insert(std::make_pair(goal_id, handler)); @@ -138,7 +138,7 @@ class ActionFactory_1_2 : public ActionFactoryInterface translate_goal_1_to_2(*gh1_.getGoal(), goal2); if (!client_->wait_for_action_server(std::chrono::seconds(1))) { - RCLCPP_INFO(ros2_node_->get_logger(), "Action server not available after waiting"); + RCLCPP_INFO(this->logger_, "Action server not available after waiting"); gh1_.setRejected(); return; } @@ -190,13 +190,14 @@ class ActionFactory_1_2 : public ActionFactoryInterface } } - GoalHandler(ROS1GoalHandle & gh1, ROS2ClientSharedPtr & client) - : gh1_(gh1), gh2_(nullptr), client_(client), canceled_(false) {} + GoalHandler(ROS1GoalHandle & gh1, ROS2ClientSharedPtr & client, rclcpp::Logger logger) + : gh1_(gh1), gh2_(nullptr), client_(client), logger_(logger), canceled_(false) {} private: ROS1GoalHandle gh1_; ROS2ClientGoalHandle gh2_; ROS2ClientSharedPtr client_; + rclcpp::Logger logger_; bool canceled_; // cancel was called std::mutex mutex_; }; From 1b4097753726011e810192ef298a3c2f8f9c2ebd Mon Sep 17 00:00:00 2001 From: fmessmer Date: Tue, 11 Jul 2023 16:57:39 +0200 Subject: [PATCH 15/19] fix code style divergence --- include/ros1_bridge/action_factory.hpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/include/ros1_bridge/action_factory.hpp b/include/ros1_bridge/action_factory.hpp index fd386a56..7e1e2e76 100644 --- a/include/ros1_bridge/action_factory.hpp +++ b/include/ros1_bridge/action_factory.hpp @@ -121,7 +121,7 @@ class ActionFactory_1_2 : public ActionFactoryInterface private: class GoalHandler { - public: +public: void cancel() { std::lock_guard lock(mutex_); @@ -193,7 +193,7 @@ class ActionFactory_1_2 : public ActionFactoryInterface GoalHandler(ROS1GoalHandle & gh1, ROS2ClientSharedPtr & client, rclcpp::Logger logger) : gh1_(gh1), gh2_(nullptr), client_(client), logger_(logger), canceled_(false) {} - private: +private: ROS1GoalHandle gh1_; ROS2ClientGoalHandle gh2_; ROS2ClientSharedPtr client_; @@ -312,7 +312,7 @@ class ActionFactory_2_1 : public ActionFactoryInterface private: class GoalHandler { - public: +public: void cancel() { std::lock_guard lock(mutex_); @@ -382,7 +382,7 @@ class ActionFactory_2_1 : public ActionFactoryInterface GoalHandler(std::shared_ptr & gh2, std::shared_ptr & client) : gh2_(gh2), client_(client), canceled_(false) {} - private: +private: std::shared_ptr gh1_; std::shared_ptr gh2_; std::shared_ptr client_; From d915538c6eb963ee07adf9211490f732a737c42c Mon Sep 17 00:00:00 2001 From: fmessmer Date: Fri, 22 Sep 2023 10:03:57 +0200 Subject: [PATCH 16/19] more logging via RCLCPP_INFO --- include/ros1_bridge/action_factory.hpp | 26 +++++++++++++++++++------- 1 file changed, 19 insertions(+), 7 deletions(-) diff --git a/include/ros1_bridge/action_factory.hpp b/include/ros1_bridge/action_factory.hpp index 7e1e2e76..e4ffa0fe 100644 --- a/include/ros1_bridge/action_factory.hpp +++ b/include/ros1_bridge/action_factory.hpp @@ -90,6 +90,7 @@ class ActionFactory_1_2 : public ActionFactoryInterface { // try to find goal and cancel it std::lock_guard lock(mutex_); + RCLCPP_INFO(ros2_node_->get_logger(), "ActionFactory_1_2::cancel_cb"); auto it = goals_.find(gh1.getGoalID().id); if (it != goals_.end()) { std::thread([handler = it->second]() mutable {handler->cancel();}).detach(); @@ -99,6 +100,7 @@ class ActionFactory_1_2 : public ActionFactoryInterface void goal_cb(ROS1GoalHandle gh1) { const std::string goal_id = gh1.getGoalID().id; + RCLCPP_INFO(ros2_node_->get_logger(), "ActionFactory_1_2::goal_cb"); // create a new handler for the goal std::shared_ptr handler; @@ -106,7 +108,7 @@ class ActionFactory_1_2 : public ActionFactoryInterface std::lock_guard lock(mutex_); goals_.insert(std::make_pair(goal_id, handler)); - RCLCPP_INFO(ros2_node_->get_logger(), "Sending goal"); + RCLCPP_INFO(ros2_node_->get_logger(), "ActionFactory_1_2::goal_cb Sending goal"); std::thread( [handler, goal_id, this]() mutable { // execute the goal remotely @@ -293,11 +295,11 @@ class ActionFactory_2_1 : public ActionFactoryInterface { std::size_t goal_id = get_goal_id_hash(gh2->get_goal_id()); std::shared_ptr handler; - handler = std::make_shared(gh2, client_); + handler = std::make_shared(gh2, client_, ros2_node_->get_logger()); std::lock_guard lock(mutex_); goals_.insert(std::make_pair(goal_id, handler)); - RCLCPP_INFO(ros2_node_->get_logger(), "Sending goal"); + RCLCPP_INFO(ros2_node_->get_logger(), "ActionFactory_2_1::handle_accepted Sending goal"); std::thread( [handler, goal_id, this]() mutable { // execute the goal remotely @@ -342,7 +344,10 @@ class ActionFactory_2_1 : public ActionFactoryInterface [this, &result_ready, &cond_result](ROS1ClientGoalHandle goal_handle) mutable // transition_cb { - ROS_INFO("Goal [%s]", goal_handle.getCommState().toString().c_str()); + RCLCPP_INFO( + this->logger_, + "ActionFactory_2_1::handle Goal [%s]", + goal_handle.getCommState().toString().c_str()); if (goal_handle.getCommState() == actionlib::CommState::RECALLING) { // cancelled before being processed auto result2 = std::make_shared(); @@ -355,7 +360,10 @@ class ActionFactory_2_1 : public ActionFactoryInterface auto result2 = std::make_shared(); auto result1 = goal_handle.getResult(); translate_result_1_to_2(*result2, *result1); - ROS_INFO("Goal [%s]", goal_handle.getTerminalState().toString().c_str()); + RCLCPP_INFO( + this->logger_, + "ActionFactory_2_1::handle Goal [%s]", + goal_handle.getTerminalState().toString().c_str()); if (goal_handle.getTerminalState() == actionlib::TerminalState::SUCCEEDED) { gh2_->succeed(result2); } else { @@ -379,13 +387,17 @@ class ActionFactory_2_1 : public ActionFactoryInterface cond_result.wait(lck, [&result_ready] {return result_ready.load();}); } - GoalHandler(std::shared_ptr & gh2, std::shared_ptr & client) - : gh2_(gh2), client_(client), canceled_(false) {} + GoalHandler( + std::shared_ptr & gh2, + std::shared_ptr & client, + rclcpp::Logger logger) + : gh2_(gh2), client_(client), logger_(logger), canceled_(false) {} private: std::shared_ptr gh1_; std::shared_ptr gh2_; std::shared_ptr client_; + rclcpp::Logger logger_; bool canceled_; // cancel was called std::mutex mutex_; }; From 08fc3bcf03b023114d111e4130c6b10f69615808 Mon Sep 17 00:00:00 2001 From: Benjamin Maidel Date: Thu, 21 Sep 2023 11:23:33 +0200 Subject: [PATCH 17/19] handle invalid std::shared_future reference --- include/ros1_bridge/action_factory.hpp | 48 ++++++++++++++++++++------ 1 file changed, 37 insertions(+), 11 deletions(-) diff --git a/include/ros1_bridge/action_factory.hpp b/include/ros1_bridge/action_factory.hpp index e4ffa0fe..086137a9 100644 --- a/include/ros1_bridge/action_factory.hpp +++ b/include/ros1_bridge/action_factory.hpp @@ -149,21 +149,47 @@ class ActionFactory_1_2 : public ActionFactoryInterface auto send_goal_ops = ROS2SendGoalOptions(); send_goal_ops.goal_response_callback = [this, &gh2_future](std::shared_future gh2) mutable { - auto goal_handle = gh2_future.get(); - if (!goal_handle) { - gh1_.setRejected(); // goal was not accepted by remote server - return; - } + try { + // this is a workaround for the underlying actionlib implementation bug + // this callback function should never get called the the shared_future + // is not ready/valid + int counter = 0; + while(!gh2_future.valid()) + { + if( counter >= 10) + { + std::cout<<"std::shared_future not valid for more than 1.0 seconds. " + "Rejecting action goal."< lock(mutex_); - gh2_ = goal_handle; + { + std::lock_guard lock(mutex_); + gh2_ = goal_handle; - if (canceled_) { // cancel was called in between - auto fut = client_->async_cancel_goal(gh2_); + if (canceled_) { // cancel was called in between + auto fut = client_->async_cancel_goal(gh2_); + } } + } catch (const std::future_error& e) { + std::cout << "Caught a future_error with code \"" << e.code() + << "\"\nMessage: \"" << e.what() << "\"\n"; + throw; } }; From f64801ac4d2d4e9e0d6f64cc9f305ddddcddcef6 Mon Sep 17 00:00:00 2001 From: Benjamin Maidel Date: Thu, 21 Sep 2023 14:57:11 +0200 Subject: [PATCH 18/19] Use rclcpp logger --- include/ros1_bridge/action_factory.hpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/include/ros1_bridge/action_factory.hpp b/include/ros1_bridge/action_factory.hpp index 086137a9..62edb8e8 100644 --- a/include/ros1_bridge/action_factory.hpp +++ b/include/ros1_bridge/action_factory.hpp @@ -158,15 +158,15 @@ class ActionFactory_1_2 : public ActionFactoryInterface { if( counter >= 10) { - std::cout<<"std::shared_future not valid for more than 1.0 seconds. " - "Rejecting action goal."<logger_, "std::shared_future not valid for more than 1.0 seconds. " + "Rejecting action goal."); gh1_.setRejected(); return; } - std::cout<<"std::shared_future not valid. " - "This indicates a bug in the actionlib implementation. " - "goal_reponse_callback should only get called when the future is valid. " - "Waiting and retrying..."<logger_, "std::shared_future not valid. " + "This indicates a bug in the actionlib implementation. " + "goal_reponse_callback should only get called when the future is valid. " + "Waiting and retrying..."); rclcpp::sleep_for(std::chrono::milliseconds(100)); counter++; } @@ -187,8 +187,8 @@ class ActionFactory_1_2 : public ActionFactoryInterface } } } catch (const std::future_error& e) { - std::cout << "Caught a future_error with code \"" << e.code() - << "\"\nMessage: \"" << e.what() << "\"\n"; + RCLCPP_ERROR_STREAM(this->logger_, "Caught a future_error with code '" << e.code() + << "' Message: '" << e.what()<<"'"); throw; } }; From 8ddb3c2f0386c802f5c452665aa973f3d09694d0 Mon Sep 17 00:00:00 2001 From: fmessmer Date: Fri, 22 Sep 2023 10:41:03 +0200 Subject: [PATCH 19/19] fix format --- include/ros1_bridge/action_factory.hpp | 33 ++++++++++++++------------ 1 file changed, 18 insertions(+), 15 deletions(-) diff --git a/include/ros1_bridge/action_factory.hpp b/include/ros1_bridge/action_factory.hpp index 62edb8e8..de2233ae 100644 --- a/include/ros1_bridge/action_factory.hpp +++ b/include/ros1_bridge/action_factory.hpp @@ -100,7 +100,6 @@ class ActionFactory_1_2 : public ActionFactoryInterface void goal_cb(ROS1GoalHandle gh1) { const std::string goal_id = gh1.getGoalID().id; - RCLCPP_INFO(ros2_node_->get_logger(), "ActionFactory_1_2::goal_cb"); // create a new handler for the goal std::shared_ptr handler; @@ -151,22 +150,24 @@ class ActionFactory_1_2 : public ActionFactoryInterface [this, &gh2_future](std::shared_future gh2) mutable { try { // this is a workaround for the underlying actionlib implementation bug - // this callback function should never get called the the shared_future + // this callback function should never get called the the shared_future // is not ready/valid int counter = 0; - while(!gh2_future.valid()) - { - if( counter >= 10) - { - RCLCPP_ERROR(this->logger_, "std::shared_future not valid for more than 1.0 seconds. " - "Rejecting action goal."); + while (!gh2_future.valid()) { + if (counter >= 10) { + RCLCPP_ERROR( + this->logger_, + "std::shared_future not valid for more than 1.0 seconds. " + "Rejecting action goal."); gh1_.setRejected(); return; } - RCLCPP_WARN(this->logger_, "std::shared_future not valid. " - "This indicates a bug in the actionlib implementation. " - "goal_reponse_callback should only get called when the future is valid. " - "Waiting and retrying..."); + RCLCPP_WARN( + this->logger_, + "std::shared_future not valid. " + "This indicates a bug in the actionlib implementation. " + "goal_reponse_callback should only get called when the future is valid. " + "Waiting and retrying..."); rclcpp::sleep_for(std::chrono::milliseconds(100)); counter++; } @@ -186,9 +187,11 @@ class ActionFactory_1_2 : public ActionFactoryInterface auto fut = client_->async_cancel_goal(gh2_); } } - } catch (const std::future_error& e) { - RCLCPP_ERROR_STREAM(this->logger_, "Caught a future_error with code '" << e.code() - << "' Message: '" << e.what()<<"'"); + } catch (const std::future_error & e) { + RCLCPP_ERROR_STREAM( + this->logger_, + "Caught a future_error with code '" << e.code() << + "' Message: '" << e.what() << "'"); throw; } };