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") 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 ``` diff --git a/include/ros1_bridge/action_factory.hpp b/include/ros1_bridge/action_factory.hpp index 3a67ebad..de2233ae 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 @@ -91,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(); @@ -103,11 +103,11 @@ 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_, 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_1_2::goal_cb Sending goal"); std::thread( [handler, goal_id, this]() mutable { // execute the goal remotely @@ -131,6 +131,7 @@ class ActionFactory_1_2 : public ActionFactoryInterface auto fut = client_->async_cancel_goal(gh2_); } } + void handle() { auto goal1 = gh1_.getGoal(); @@ -138,33 +139,60 @@ 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(this->logger_, "Action server not available after waiting"); gh1_.setRejected(); 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 { - 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) { + 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::sleep_for(std::chrono::milliseconds(100)); + counter++; + } + auto goal_handle = gh2_future.get(); + if (!goal_handle) { + gh1_.setRejected(); // goal was not accepted by remote server + return; + } - gh1_.setAccepted(); + gh1_.setAccepted(); - { - std::lock_guard 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) { + RCLCPP_ERROR_STREAM( + this->logger_, + "Caught a future_error with code '" << e.code() << + "' Message: '" << e.what() << "'"); + throw; } }; @@ -193,13 +221,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_; }; @@ -272,7 +301,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; } @@ -295,11 +324,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.reset(new GoalHandler(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 @@ -344,7 +373,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(); @@ -357,7 +389,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 { @@ -381,13 +416,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_; }; diff --git a/resource/get_factory.cpp.em b/resource/get_factory.cpp.em index 3e8f0e48..758844df 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,8 +83,7 @@ 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 factory; + return nullptr; } } // namespace ros1_bridge 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 diff --git a/src/action_bridge.cpp b/src/action_bridge.cpp index bb0185b1..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__ @@ -46,14 +45,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()); } 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,