From a2c6441f07f1008404dac286caeb8d60db76d908 Mon Sep 17 00:00:00 2001 From: Harsh Deshpande Date: Sun, 3 May 2020 23:41:46 +0200 Subject: [PATCH] Implemented action bridge Squash of upstream PR: https://github.com/ros2/ros1_bridge/pull/256 Co-authored-by: tomoya Co-authored-by: Geoffrey Biggs Signed-off-by: Victor Lopez Signed-off-by: Harsh Deshpande --- CMakeLists.txt | 12 +- README.md | 48 +++ include/ros1_bridge/action_factory.hpp | 414 ++++++++++++++++++++ include/ros1_bridge/bridge.hpp | 6 + include/ros1_bridge/factory_interface.hpp | 11 + package.xml | 4 + resource/get_factory.cpp.em | 19 +- resource/get_mappings.cpp.em | 14 + resource/interface_factories.cpp.em | 119 +++++- resource/pkg_factories.cpp.em | 19 + resource/pkg_factories.hpp.em | 9 + ros1_bridge/__init__.py | 446 ++++++++++++++++++++-- src/action_bridge.cpp | 72 ++++ src/dynamic_bridge.cpp | 336 +++++++++++++++- 14 files changed, 1498 insertions(+), 31 deletions(-) create mode 100644 include/ros1_bridge/action_factory.hpp create mode 100644 src/action_bridge.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 0f7855a4..4b85edbe 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -16,6 +16,7 @@ find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(rmw_implementation_cmake REQUIRED) find_package(std_msgs REQUIRED) +find_package(rclcpp_action REQUIRED) # find ROS 1 packages set(cmake_extras_files cmake/find_ros1_package.cmake cmake/find_ros1_interface_packages.cmake) @@ -43,6 +44,7 @@ if(NOT ros1_roscpp_FOUND) endif() find_ros1_package(std_msgs REQUIRED) +find_ros1_package(actionlib REQUIRED) # Dependency that we should only look for if ROS 1 is installed (it's not present on a ROS 2 # system; see https://github.com/ros2/ros1_bridge/pull/331#issuecomment-1188111510) @@ -146,7 +148,7 @@ foreach(package_name ${ros2_interface_packages}) file(TO_CMAKE_PATH "${interface_file}" interface_name) get_filename_component(interface_basename "${interface_name}" NAME_WE) # skipping actions and request and response messages of services - if(NOT "${interface_name}" MATCHES "^(msg|srv)/" OR "${interface_basename}" MATCHES "_(Request|Response)$") + if(NOT "${interface_name}" MATCHES "^(msg|srv|action)/" OR "${interface_basename}" MATCHES "_(Request|Response)$") continue() endif() string(REPLACE "/" "__" interface_name "${interface_name}") @@ -227,6 +229,7 @@ ament_target_dependencies(${PROJECT_NAME} ${prefixed_ros1_message_packages} ${ros2_interface_packages} "rclcpp" + "rclcpp_action" "ros1_roscpp" "ros1_std_msgs") @@ -242,6 +245,13 @@ custom_executable(static_bridge target_link_libraries(static_bridge ${PROJECT_NAME}) +custom_executable(action_bridge + "src/action_bridge.cpp" + ROS1_DEPENDENCIES + TARGET_DEPENDENCIES ${ros2_interface_packages}) +target_link_libraries(action_bridge + ${PROJECT_NAME}) + custom_executable(parameter_bridge "src/parameter_bridge.cpp" ROS1_DEPENDENCIES diff --git a/README.md b/README.md index 99fc110b..c6cb7e30 100644 --- a/README.md +++ b/README.md @@ -527,3 +527,51 @@ topics: ``` Note that the `qos` section can be omitted entirely and options not set are left default. + +# Action bridge + +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 the `action_bridge` node are: +`direction`: from client (`ros1` or `ros2`) +e.g.: +- `ROS1` client to `ROS2` server --> `direction` = `ros1` +- `ROS2` client to `ROS1` server --> `direction` = `ros2` + +`package`: package of the `ROS1` server node +`type`: action interface type of `ROS1` +`name`: action name + +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 -- ROS 1 action server +source /setup.bash +rosrun actionlib_tutorials fibonacci_server + +# Terminal 3 -- ROS 2 action client +source /setup.bash +ros2 run action_tutorials_cpp fibonacci_action_client 20 +``` + +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 -- ROS 2 action server +source /setup.bash +ros2 run action_tutorials_cpp fibonacci_action_server + +# Terminal 3 -- ROS 1 action client +source /setup.bash +rosrun actionlib_tutorials fibonacci_client 20 +``` + +`dynamic_bridge` has been extended to handle actions as well. diff --git a/include/ros1_bridge/action_factory.hpp b/include/ros1_bridge/action_factory.hpp new file mode 100644 index 00000000..88785b25 --- /dev/null +++ b/include/ros1_bridge/action_factory.hpp @@ -0,0 +1,414 @@ +// Copyright 2020 Fraunhofer IPA +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef ROS1_BRIDGE__ACTION_FACTORY_HPP_ +#define ROS1_BRIDGE__ACTION_FACTORY_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#ifdef __clang__ +#pragma clang diagnostic push +#endif +#include +#include // Need this for the goal state. Need a better solution +#include +#include +#ifdef __clang__ +#pragma clang diagnostic pop +#endif + +// include ROS 2 +#include + +#include "factory_interface.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace ros1_bridge +{ + +template +class ActionFactory_1_2 : public ActionFactoryInterface +{ +public: + using ROS1Server = typename actionlib::ActionServer; + using ROS1GoalHandle = typename actionlib::ActionServer::GoalHandle; + using ROS1Goal = typename actionlib::ActionServer::Goal; + using ROS1Feedback = typename actionlib::ActionServer::Feedback; + using ROS1Result = typename actionlib::ActionServer::Result; + using ROS2Goal = typename ROS2_T::Goal; + using ROS2Feedback = typename ROS2_T::Feedback; + using ROS2Result = typename ROS2_T::Result; + using ROS2ClientGoalHandle = typename rclcpp_action::ClientGoalHandle::SharedPtr; + using ROS2ClientSharedPtr = typename rclcpp_action::Client::SharedPtr; + using ROS2SendGoalOptions = typename rclcpp_action::Client::SendGoalOptions; + using ROS2GoalHandle = typename rclcpp_action::Client::GoalHandle::SharedPtr; + + virtual void create_server_client( + ros::NodeHandle ros1_node, rclcpp::Node::SharedPtr ros2_node, + const std::string action_name) + { + this->ros1_node_ = ros1_node; + this->ros2_node_ = ros2_node; + + server_ = std::make_shared( + ros1_node, action_name, + std::bind(&ActionFactory_1_2::goal_cb, this, std::placeholders::_1), + std::bind(&ActionFactory_1_2::cancel_cb, this, std::placeholders::_1), false); + server_->start(); + client_ = rclcpp_action::create_client(ros2_node, action_name); + } + + virtual void shutdown() + { + std::lock_guard lock(mutex_); + for (auto goal : goals_) { + std::thread([handler = goal.second]() mutable {handler->cancel();}).detach(); + } + server_.reset(); + } + + void cancel_cb(ROS1GoalHandle gh1) + { + // try to find goal and cancel it + std::lock_guard lock(mutex_); + auto it = goals_.find(gh1.getGoalID().id); + if (it != goals_.end()) { + std::thread([handler = it->second]() mutable {handler->cancel();}).detach(); + } + } + + void goal_cb(ROS1GoalHandle gh1) + { + const std::string goal_id = gh1.getGoalID().id; + + // create a new handler for the goal + std::shared_ptr handler; + 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_STREAM(ros2_node_->get_logger(), "Sending goal " << goal_id); + std::thread( + [handler, goal_id, this]() mutable { + // execute the goal remotely + handler->handle(); + + // clean-up + std::lock_guard lock(mutex_); + goals_.erase(goal_id); + }).detach(); + } + +private: + class GoalHandler + { + public: + void cancel() + { + std::lock_guard lock(mutex_gh_); + canceled_ = true; + if (gh2_) { // cancel goal if possible + auto fut = client_->async_cancel_goal(gh2_); + } + } + + void handle() + { + auto goal1 = gh1_.getGoal(); + ROS2Goal goal2; + translate_goal_1_to_2(*gh1_.getGoal(), goal2); + + if (!client_->wait_for_action_server(std::chrono::seconds(1))) { + RCLCPP_INFO(this->logger_, "Action server not available after waiting"); + gh1_.setRejected(); + return; + } + + std::shared_future gh2_future; + 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; + } + + gh1_.setAccepted(); + + { + std::lock_guard lock(mutex_gh_); + gh2_ = goal_handle; + + if (canceled_) { // cancel was called in between + auto fut = client_->async_cancel_goal(gh2_); + } + } + }; + + send_goal_ops.feedback_callback = [this](ROS2GoalHandle, auto feedback2) mutable { + ROS1Feedback feedback1; + translate_feedback_2_to_1(feedback1, *feedback2); + gh1_.publishFeedback(feedback1); + }; + + // send goal to ROS2 server, set-up feedback + gh2_future = client_->async_send_goal(goal2, send_goal_ops); + + auto future_result = client_->async_get_result(gh2_future.get()); + auto res2 = future_result.get(); + + ROS1Result res1; + translate_result_2_to_1(res1, *(res2.result)); + + std::lock_guard lock(mutex_gh_); + if (res2.code == rclcpp_action::ResultCode::SUCCEEDED) { + gh1_.setSucceeded(res1); + } else if (res2.code == rclcpp_action::ResultCode::CANCELED) { + gh1_.setCanceled(res1); + } else { + gh1_.setAborted(res1); + } + } + + 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_gh_; + }; + + ros::NodeHandle ros1_node_; + rclcpp::Node::SharedPtr ros2_node_; + + std::shared_ptr> server_; + ROS2ClientSharedPtr client_; + + std::mutex mutex_; + std::map> goals_; + + static void translate_goal_1_to_2(const ROS1Goal &, ROS2Goal &); + static void translate_result_2_to_1(ROS1Result &, const ROS2Result &); + static void translate_feedback_2_to_1(ROS1Feedback &, const ROS2Feedback &); +}; + +template +class ActionFactory_2_1 : public ActionFactoryInterface +{ +public: + using ROS2ServerGoalHandle = typename rclcpp_action::ServerGoalHandle; + using ROS1ClientGoalHandle = typename actionlib::ActionClient::GoalHandle; + using ROS2Goal = typename ROS2_T::Goal; + using ROS1Client = typename actionlib::ActionClient; + using ROS1Goal = typename actionlib::ActionServer::Goal; + using ROS1Feedback = typename actionlib::ActionServer::Feedback; + using ROS1Result = typename actionlib::ActionServer::Result; + using ROS1State = + actionlib::SimpleClientGoalState; // There is no 'ClientGoalState'. Better solution? + using ROS2Feedback = typename ROS2_T::Feedback; + using ROS2Result = typename ROS2_T::Result; + using ROS2ServerSharedPtr = typename rclcpp_action::Server::SharedPtr; + using ROS2SendGoalOptions = typename rclcpp_action::Client::SendGoalOptions; + + virtual void create_server_client( + ros::NodeHandle ros1_node, rclcpp::Node::SharedPtr ros2_node, + const std::string action_name) + { + this->ros1_node_ = ros1_node; + this->ros2_node_ = ros2_node; + client_ = std::make_shared(ros1_node, action_name); + + server_ = rclcpp_action::create_server( + ros2_node->get_node_base_interface(), ros2_node->get_node_clock_interface(), + ros2_node->get_node_logging_interface(), ros2_node->get_node_waitables_interface(), + action_name, + std::bind( + &ActionFactory_2_1::handle_goal, this, std::placeholders::_1, + std::placeholders::_2), + std::bind(&ActionFactory_2_1::handle_cancel, this, std::placeholders::_1), + std::bind(&ActionFactory_2_1::handle_accepted, this, std::placeholders::_1)); + } + + virtual void shutdown() + { + std::lock_guard lock(mutex_); + for (auto goal : goals_) { + std::thread([handler = goal.second]() mutable {handler->cancel();}).detach(); + } + server_.reset(); + } + + // ROS2 callbacks + rclcpp_action::GoalResponse handle_goal( + const rclcpp_action::GoalUUID & uuid, + std::shared_ptr goal) + { + (void)uuid; + (void)goal; + if (!client_->waitForActionServerToStart(ros::Duration(1))) { + RCLCPP_INFO(ros2_node_->get_logger(), "Action server not available after waiting"); + return rclcpp_action::GoalResponse::REJECT; + } + + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + } + + rclcpp_action::CancelResponse handle_cancel(std::shared_ptr gh2) + { + // try to find goal and cancel it + std::lock_guard lock(mutex_); + auto it = goals_.find(get_goal_id_hash(gh2->get_goal_id())); + if (it != goals_.end()) { + std::thread([handler = it->second]() mutable {handler->cancel();}).detach(); + } + + return rclcpp_action::CancelResponse::ACCEPT; + } + + void handle_accepted(std::shared_ptr gh2) + { + std::size_t goal_id = get_goal_id_hash(gh2->get_goal_id()); + std::shared_ptr handler; + handler = std::make_shared(gh2, client_); + std::lock_guard lock(mutex_); + goals_.insert(std::make_pair(goal_id, handler)); + + RCLCPP_INFO(ros2_node_->get_logger(), "Sending goal"); + std::thread( + [handler, goal_id, this]() mutable { + // execute the goal remotely + handler->handle(); + + // clean-up + std::lock_guard lock(mutex_); + goals_.erase(goal_id); + }).detach(); + } + +private: + class GoalHandler + { + public: + void cancel() + { + std::lock_guard lock(mutex_); + canceled_ = true; + if (gh1_) { + // cancel goal if possible + gh1_->cancel(); + } + } + void handle() + { + auto goal2 = gh2_->get_goal(); + ROS1Goal goal1; + translate_goal_2_to_1(*goal2, goal1); + + if (gh2_->is_canceling()) { + auto result = std::make_shared(); + gh2_->canceled(result); + return; + } + + std::condition_variable cond_result; + std::atomic result_ready(false); + + auto goal_handle = client_->sendGoal( + goal1, + [this, &result_ready, + &cond_result](ROS1ClientGoalHandle goal_handle) mutable // transition_cb + { + ROS_INFO("Goal [%s]", goal_handle.getCommState().toString().c_str()); + if (goal_handle.getCommState() == actionlib::CommState::RECALLING) { + // cancelled before being processed + auto result2 = std::make_shared(); + gh2_->canceled(result2); + return; + } else if (goal_handle.getCommState() == actionlib::CommState::ACTIVE) { + std::lock_guard lock(mutex_); + gh1_ = std::make_shared(goal_handle); + } else if (goal_handle.getCommState() == actionlib::CommState::DONE) { + 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()); + if (goal_handle.getTerminalState() == actionlib::TerminalState::SUCCEEDED) { + gh2_->succeed(result2); + } else { + gh2_->abort(result2); + } + result_ready = true; + cond_result.notify_one(); + return; + } + }, + [this](ROS1ClientGoalHandle goal_handle, auto feedback1) mutable // feedback_cb + { + (void)goal_handle; + auto feedback2 = std::make_shared(); + translate_feedback_1_to_2(*feedback2, *feedback1); + gh2_->publish_feedback(feedback2); + }); + + std::mutex mutex_result; + std::unique_lock lck(mutex_result); + 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) {} + + private: + std::shared_ptr gh1_; + std::shared_ptr gh2_; + std::shared_ptr client_; + bool canceled_; // cancel was called + std::mutex mutex_; + }; + + std::size_t get_goal_id_hash(const rclcpp_action::GoalUUID & uuid) + { + return std::hash{}(uuid); + } + + ros::NodeHandle ros1_node_; + rclcpp::Node::SharedPtr ros2_node_; + + std::shared_ptr client_; + ROS2ServerSharedPtr server_; + + std::mutex mutex_; + std::map> goals_; + + static void translate_goal_2_to_1(const ROS2Goal &, ROS1Goal &); + static void translate_result_1_to_2(ROS2Result &, const ROS1Result &); + static void translate_feedback_1_to_2(ROS2Feedback &, const ROS1Feedback &); +}; + +} // namespace ros1_bridge + +#endif // ROS1_BRIDGE__ACTION_FACTORY_HPP_ diff --git a/include/ros1_bridge/bridge.hpp b/include/ros1_bridge/bridge.hpp index c4d79dfc..5ccef637 100755 --- a/include/ros1_bridge/bridge.hpp +++ b/include/ros1_bridge/bridge.hpp @@ -64,6 +64,9 @@ get_all_message_mappings_2to1(); std::multimap get_all_service_mappings_2to1(); +std::multimap +get_all_action_mappings_2to1(); + std::shared_ptr get_factory( const std::string & ros1_type_name, @@ -72,6 +75,9 @@ get_factory( std::unique_ptr get_service_factory(const std::string &, const std::string &, const std::string &); +std::unique_ptr +get_action_factory(const std::string &, const std::string &, const std::string &); + Bridge1to2Handles create_bridge_from_1_to_2( ros::NodeHandle ros1_node, diff --git a/include/ros1_bridge/factory_interface.hpp b/include/ros1_bridge/factory_interface.hpp index 9a0f796d..8da33498 100755 --- a/include/ros1_bridge/factory_interface.hpp +++ b/include/ros1_bridge/factory_interface.hpp @@ -144,6 +144,17 @@ class ServiceFactoryInterface ros::NodeHandle &, rclcpp::Node::SharedPtr, const std::string &) = 0; }; +class ActionFactoryInterface +{ +public: + virtual void create_server_client( + ros::NodeHandle ros1_node, + rclcpp::Node::SharedPtr ros2_node, + const std::string action_name) = 0; + + virtual void shutdown() = 0; +}; + } // namespace ros1_bridge #endif // ROS1_BRIDGE__FACTORY_INTERFACE_HPP_ diff --git a/package.xml b/package.xml index 589c1036..356e7fe6 100644 --- a/package.xml +++ b/package.xml @@ -23,10 +23,12 @@ rosidl_parser builtin_interfaces + actionlib libboost-dev pkg-config python3-yaml rclcpp + rclcpp_action rcpputils rcutils rmw_implementation_cmake @@ -35,9 +37,11 @@ pkg-config + actionlib builtin_interfaces python3-yaml rclcpp + rclcpp_action rcpputils rcutils std_msgs diff --git a/resource/get_factory.cpp.em b/resource/get_factory.cpp.em index 722d1852..758844df 100644 --- a/resource/get_factory.cpp.em +++ b/resource/get_factory.cpp.em @@ -65,8 +65,25 @@ 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; } +std::unique_ptr get_action_factory(const std::string & ros_id, const std::string & package_name, const std::string & action_name) +{ +@[if not ros2_package_names]@ + (void)ros_id; + (void)package_name; + (void)action_name; +@[else]@ + std::unique_ptr factory; +@[end if]@ +@[for ros2_package_name in sorted(ros2_package_names)]@ + factory = get_action_factory_@(ros2_package_name)(ros_id, package_name, action_name); + if (factory) { + return factory; + } +@[end for]@ + return nullptr; +} + } // namespace ros1_bridge diff --git a/resource/get_mappings.cpp.em b/resource/get_mappings.cpp.em index 386f9cc7..e58a4cf3 100644 --- a/resource/get_mappings.cpp.em +++ b/resource/get_mappings.cpp.em @@ -89,4 +89,18 @@ get_all_service_mappings_2to1() return mappings; } +std::multimap +get_all_action_mappings_2to1() +{ + static std::multimap mappings = { +@[for a in actions]@ + { + "@(a['ros2_package'])/action/@(a['ros2_name'])", // ROS 2 + "@(a['ros1_package'])/@(a['ros1_name'])" // ROS 1 + }, +@[end for]@ + }; + return mappings; +} + } // namespace ros1_bridge diff --git a/resource/interface_factories.cpp.em b/resource/interface_factories.cpp.em index f3e536e4..b831b47f 100644 --- a/resource/interface_factories.cpp.em +++ b/resource/interface_factories.cpp.em @@ -46,6 +46,16 @@ from rosidl_parser.definition import UnboundedString #include <@(service["ros2_package"])/srv/@(camel_case_to_lower_case_underscore(service["ros2_name"])).hpp> @[end for]@ +// include ROS 1 actions +@[for action in mapped_actions]@ +#include <@(action["ros1_package"])/@(action["ros1_name"])Action.h> +@[end for]@ + +// include ROS 2 actions +@[for action in mapped_actions]@ +#include <@(action["ros2_package"])/action/@(camel_case_to_lower_case_underscore(action["ros2_name"])).hpp> +@[end for]@ + namespace ros1_bridge { @@ -104,6 +114,41 @@ get_service_factory_@(ros2_package_name)__@(interface_type)__@(interface.message return nullptr; } @ + +std::unique_ptr +get_action_factory_@(ros2_package_name)__@(interface_type)__@(interface.message_name)(const std::string & ros_id, const std::string & package_name, const std::string & action_name) +{ +@[if not mapped_actions]@ + (void)ros_id; + (void)package_name; + (void)action_name; +@[end if]@ +@[for action in mapped_actions]@ + if ( + ros_id == "ros1" && + package_name == "@(action["ros1_package"])" && + action_name == "@(action["ros1_name"])" + ) { + return std::unique_ptr(new ActionFactory_2_1< + @(action["ros1_package"])::@(action["ros1_name"])Action, + @(action["ros2_package"])::action::@(action["ros2_name"]) + >); + } + else if + ( + ros_id == "ros2" && + package_name == "@(action["ros2_package"])" && + action_name == "action/@(action["ros2_name"])" + ) { + return std::unique_ptr(new ActionFactory_1_2< + @(action["ros1_package"])::@(action["ros1_name"])Action, + @(action["ros2_package"])::action::@(action["ros2_name"]) + >); + } +@[end for]@ + return nullptr; +} +@ // conversion functions for available interfaces @[for m in mapped_msgs]@ @@ -319,10 +364,14 @@ void ServiceFactory< auto & @(field["ros2"]["name"])2 = req2.@(field["ros2"]["name"]); @[ end if]@ @[ if field["basic"]]@ - @(field["ros2"]["name"])@(to) = @(field["ros1"]["name"])@(frm); +@[ if field["ros2"]["type"].startswith("builtin_interfaces") ]@ + ros1_bridge::convert_@(frm)_to_@(to)(@(field["ros" + frm]["name"])@(frm), @(field["ros" + to]["name"])@(to)); +@[ else]@ + @(field["ros" + to]["name"])@(to) = @(field["ros" + frm]["name"])@(frm); +@[ end if]@ @[ else]@ Factory<@(field["ros1"]["cpptype"]),@(field["ros2"]["cpptype"])>::convert_@(frm)_to_@(to)(@ -@(field["ros2"]["name"])@(frm), @(field["ros1"]["name"])@(to)); +@(field["ros" + frm]["name"])@(frm), @(field["ros" + to]["name"])@(to)); @[ end if]@ @[ if field["array"]]@ } @@ -332,6 +381,72 @@ void ServiceFactory< @[ end for]@ @[ end for]@ +@[end for]@ + +@[for action in mapped_actions]@ +@[ for frm, to in [("1", "2"), ("2", "1")]]@ +@[ for type in ["Goal", "Result", "Feedback"]]@ +template <> +@[ if type == "Goal"]@ +@{ +frm_, to_ = frm, to +const_1 = "const " +const_2 = "" +}@ +@[ else]@ +@{ +frm_, to_ = to, frm +const_1 = "" +const_2 = "const " +}@ +@[ end if]@ +void ActionFactory_@(frm_)_@(to_)< +@(action["ros1_package"])::@(action["ros1_name"])Action, +@(action["ros2_package"])::action::@(action["ros2_name"]) +>::translate_@(type.lower())_@(frm)_to_@(to)( +@[ if type == "Goal"]@ + @(const_1)ROS@(frm)@(type) &@(type.lower())@(frm), + @(const_2)ROS@(to)@(type) &@(type.lower())@(to)) +@[ else]@ + @(const_1)ROS@(to)@(type) &@(type.lower())@(to), + @(const_2)ROS@(frm)@(type) &@(type.lower())@(frm)) +@[ end if]@ +{ +@[ for field in action["fields"][type.lower()]]@ +@[ if field["array"]]@ + @(type.lower())@(to).@(field["ros" + to]["name"]).resize(@(type.lower())@(frm).@(field["ros" + frm]["name"]).size()); + auto @(field["ros" + frm]["name"])@(frm)_it = @(type.lower())@(frm).@(field["ros" + frm]["name"]).begin(); + auto @(field["ros" + to]["name"])@(to)_it = @(type.lower())@(to).@(field["ros" + to]["name"]).begin(); + while ( + @(field["ros" + frm]["name"])@(frm)_it != @(type.lower())@(frm).@(field["ros" + frm]["name"]).end() && + @(field["ros" + to]["name"])@(to)_it != @(type.lower())@(to).@(field["ros" + to]["name"]).end() + ) { + auto & @(field["ros" + frm]["name"])@(frm) = *(@(field["ros" + frm]["name"])@(frm)_it++); + auto & @(field["ros" + to]["name"])@(to) = *(@(field["ros" + to]["name"])@(to)_it++); +@[ else]@ + auto & @(field["ros" + frm]["name"])@(frm) = @(type.lower())@(frm).@(field["ros" + frm]["name"]); + auto & @(field["ros" + to]["name"])@(to) = @(type.lower())@(to).@(field["ros" + to]["name"]); +@[ end if]@ +@[ if field["basic"]]@ +@[ if field["ros2"]["type"].startswith("builtin_interfaces") ]@ + ros1_bridge::convert_@(frm)_to_@(to)(@(field["ros" + frm]["name"])@(frm), @(field["ros" + to]["name"])@(to)); +@[ else]@ + @(field["ros" + to]["name"])@(to) = @(field["ros" + frm]["name"])@(frm); +@[ end if]@ +@[ else]@ + Factory<@(field["ros1"]["cpptype"]),@(field["ros2"]["cpptype"])>::convert_@(frm)_to_@(to)(@(field["ros" + frm]["name"])@(frm), @(field["ros" + to]["name"])@(to)); +@[end if]@ +@[ if field["array"]]@ + } +@[ end if]@ + +@[ end for]@ +} + +@[ end for]@ + +@[ end for]@ + @[end for]@ } // namespace ros1_bridge diff --git a/resource/pkg_factories.cpp.em b/resource/pkg_factories.cpp.em index c8d2ab21..52266f87 100644 --- a/resource/pkg_factories.cpp.em +++ b/resource/pkg_factories.cpp.em @@ -58,4 +58,23 @@ get_service_factory_@(ros2_package_name)(const std::string & ros_id, const std:: @[end for]@ return nullptr; } + +std::unique_ptr +get_action_factory_@(ros2_package_name)(const std::string & ros_id, const std::string & package_name, const std::string & action_name) +{ +@[if not ros2_action_types]@ + (void)ros_id; + (void)package_name; + (void)action_name; +@[else]@ + std::unique_ptr factory; +@[end if]@ +@[for a in ros2_action_types]@ + factory = get_action_factory_@(ros2_package_name)__action__@(a.message_name)(ros_id, package_name, action_name); + if (factory) { + return factory; + } +@[end for]@ + return nullptr; +} } // namespace ros1_bridge diff --git a/resource/pkg_factories.hpp.em b/resource/pkg_factories.hpp.em index 670d4fd2..af325bd1 100644 --- a/resource/pkg_factories.hpp.em +++ b/resource/pkg_factories.hpp.em @@ -28,6 +28,7 @@ from ros1_bridge import camel_case_to_lower_case_underscore #include #include +#include // include ROS 1 messages @[for ros1_msg in mapped_ros1_msgs]@ @@ -58,6 +59,14 @@ std::unique_ptr get_service_factory_@(ros2_package_name)__srv__@(s.message_name)(const std::string & ros_id, const std::string & package_name, const std::string & service_name); @[end for]@ +std::unique_ptr +get_action_factory_@(ros2_package_name)(const std::string & ros_id, const std::string & package_name, const std::string & action_name); +@[for a in ros2_action_types]@ + +std::unique_ptr +get_action_factory_@(ros2_package_name)__action__@(a.message_name)(const std::string & ros_id, const std::string & package_name, const std::string & action_name); +@[end for]@ + // conversion functions for available interfaces @[for m in mappings]@ diff --git a/ros1_bridge/__init__.py b/ros1_bridge/__init__.py index 7cc78d14..97dc89cb 100644 --- a/ros1_bridge/__init__.py +++ b/ros1_bridge/__init__.py @@ -20,7 +20,13 @@ import ament_index_python from catkin_pkg.package import parse_package # ROS 1 imports +try: + from cStringIO import StringIO # Python 2.x +except ImportError: + from io import StringIO # Python 3.x + import genmsg +from genmsg.base import COMMENTCHAR, IODELIM import genmsg.msg_loader import rosidl_adapter.parser @@ -75,14 +81,17 @@ def generate_cpp(output_path, template_dir): for m in data['mappings']} data.update( generate_services(rospack, message_string_pairs=message_string_pairs)) + data.update(generate_actions( + rospack, message_string_pairs=message_string_pairs)) template_file = os.path.join(template_dir, 'get_mappings.cpp.em') output_file = os.path.join(output_path, 'get_mappings.cpp') data_for_template = { - 'mappings': data['mappings'], 'services': data['services']} + 'mappings': data['mappings'], 'services': data['services'], 'actions': data['actions']} expand_template(template_file, data_for_template, output_file) - unique_package_names = set(data['ros2_package_names_msg'] + data['ros2_package_names_srv']) + unique_package_names = set( + data['ros2_package_names_msg'] + data['ros2_package_names_srv']) # skip builtin_interfaces since there is a custom implementation unique_package_names -= {'builtin_interfaces'} data['ros2_package_names'] = list(unique_package_names) @@ -108,6 +117,9 @@ def generate_cpp(output_path, template_dir): 'ros2_srv_types': [ s for s in data['all_ros2_srvs'] if s.package_name == ros2_package_name], + 'ros2_action_types': [ + s for s in data['all_ros2_actions'] + if s.package_name == ros2_package_name], # forward declaration of template specializations 'mappings': [ m for m in data['mappings'] @@ -123,6 +135,7 @@ def generate_cpp(output_path, template_dir): # call interface specific factory functions 'ros2_msg_types': data_pkg_hpp['ros2_msg_types'], 'ros2_srv_types': data_pkg_hpp['ros2_srv_types'], + 'ros2_action_types': data_pkg_hpp['ros2_action_types'], } template_file = os.path.join(template_dir, 'pkg_factories.cpp.em') output_file = os.path.join( @@ -130,7 +143,8 @@ def generate_cpp(output_path, template_dir): expand_template(template_file, data_pkg_cpp, output_file) for interface_type, interfaces in zip( - ['msg', 'srv'], [data['all_ros2_msgs'], data['all_ros2_srvs']] + ['msg', 'srv', 'action'], [data['all_ros2_msgs'], + data['all_ros2_srvs'], data['all_ros2_actions']] ): for interface in interfaces: if interface.package_name != ros2_package_name: @@ -141,6 +155,7 @@ def generate_cpp(output_path, template_dir): 'interface': interface, 'mapped_msgs': [], 'mapped_services': [], + 'mapped_actions': [], } if interface_type == 'msg': data_idl_cpp['mapped_msgs'] += [ @@ -152,7 +167,14 @@ def generate_cpp(output_path, template_dir): s for s in data['services'] if s['ros2_package'] == ros2_package_name and s['ros2_name'] == interface.message_name] - template_file = os.path.join(template_dir, 'interface_factories.cpp.em') + if interface_type == 'action': + data_idl_cpp['mapped_actions'] += [ + s for s in data['actions'] + if s['ros2_package'] == ros2_package_name and + s['ros2_name'] == interface.message_name] + + template_file = os.path.join( + template_dir, 'interface_factories.cpp.em') output_file = os.path.join( output_path, '%s__%s__%s__factories.cpp' % (ros2_package_name, interface_type, interface.message_name)) @@ -163,8 +185,10 @@ def generate_messages(rospack=None): ros1_msgs = get_ros1_messages(rospack=rospack) ros2_package_names, ros2_msgs, mapping_rules = get_ros2_messages() - package_pairs = determine_package_pairs(ros1_msgs, ros2_msgs, mapping_rules) - message_pairs = determine_message_pairs(ros1_msgs, ros2_msgs, package_pairs, mapping_rules) + package_pairs = determine_package_pairs( + ros1_msgs, ros2_msgs, mapping_rules) + message_pairs = determine_message_pairs( + ros1_msgs, ros2_msgs, package_pairs, mapping_rules) mappings = [] # add custom mapping for builtin_interfaces @@ -184,7 +208,8 @@ def generate_messages(rospack=None): msg_idx.ros2_put(ros2_msg) for ros1_msg, ros2_msg in message_pairs: - mapping = determine_field_mapping(ros1_msg, ros2_msg, mapping_rules, msg_idx) + mapping = determine_field_mapping( + ros1_msg, ros2_msg, mapping_rules, msg_idx) if mapping: mappings.append(mapping) @@ -214,7 +239,8 @@ def generate_messages(rospack=None): ('%s/%s' % (m.ros1_msg.package_name, m.ros1_msg.message_name), '%s/%s' % (m.ros2_msg.package_name, m.ros2_msg.message_name)), file=sys.stderr) for d in m.depends_on_ros2_messages: - print(' -', '%s/%s' % (d.package_name, d.message_name), file=sys.stderr) + print(' -', '%s/%s' % + (d.package_name, d.message_name), file=sys.stderr) print(file=sys.stderr) return { @@ -239,6 +265,19 @@ def generate_services(rospack=None, message_string_pairs=None): } +def generate_actions(rospack=None, message_string_pairs=None): + ros1_actions = get_ros1_actions(rospack) + ros2_pkgs, ros2_actions, mapping_rules = get_ros2_actions() + + actions = determine_common_actions( + ros1_actions, ros2_actions, mapping_rules, message_string_pairs=message_string_pairs) + return { + 'actions': actions, + 'ros2_package_names_actions': ros2_pkgs, + 'all_ros2_actions': ros2_actions, + } + + def get_ros1_messages(rospack=None): if not rospack: rospack = rospkg.RosPack() @@ -284,7 +323,8 @@ def get_ros2_messages(): continue if 'mapping_rules' not in export.attributes: continue - rule_file = os.path.join(package_path, export.attributes['mapping_rules']) + rule_file = os.path.join( + package_path, export.attributes['mapping_rules']) with open(rule_file, 'r') as h: content = yaml.safe_load(h) if not isinstance(content, list): @@ -346,7 +386,8 @@ def get_ros2_services(): continue if 'mapping_rules' not in export.attributes: continue - rule_file = os.path.join(package_path, export.attributes['mapping_rules']) + rule_file = os.path.join( + package_path, export.attributes['mapping_rules']) with open(rule_file, 'r') as h: content = yaml.safe_load(h) if not isinstance(content, list): @@ -363,6 +404,73 @@ def get_ros2_services(): return pkgs, srvs, rules +# rosmsg.py +def iterate_action_packages(rospack): + subdir = 'action' + pkgs = rospack.list() + for p in pkgs: + package_paths = rosmsg._get_package_paths(p, rospack) + for package_path in package_paths: + d = os.path.join(package_path, subdir) + if os.path.isdir(d): + yield p, d + + +def get_ros1_actions(rospack=None): + if not rospack: + rospack = rospkg.RosPack() + actions = [] + # actual function: rosmsg.iterate_packages(rospack, rosmsg.MODE_MSG)) + pkgs = sorted(x for x in iterate_action_packages(rospack)) + for package_name, path in pkgs: + for action_name in rosmsg._list_types(path, 'msg', '.action'): + actions.append(Message(package_name, action_name, path)) + return actions + + +def get_ros2_actions(): + pkgs = [] + actions = [] + rules = [] + resource_type = 'rosidl_interfaces' + resources = ament_index_python.get_resources(resource_type) + for package_name, prefix_path in resources.items(): + pkgs.append(package_name) + resource, _ = ament_index_python.get_resource( + resource_type, package_name) + interfaces = resource.splitlines() + action_names = { + i[7:-7] + for i in interfaces + if i.startswith('action/') and i[-7:] in ('.idl', '.action')} + for action_name in sorted(action_names): + actions.append(Message(package_name, action_name, prefix_path)) + package_path = os.path.join(prefix_path, 'share', package_name) + pkg = parse_package(package_path) + for export in pkg.exports: + if export.tagname != 'ros1_bridge': + continue + if 'mapping_rules' not in export.attributes: + continue + rule_file = os.path.join( + package_path, export.attributes['mapping_rules']) + with open(rule_file, 'r') as h: + content = yaml.safe_load(h) + if not isinstance(content, list): + print( + "The content of the mapping rules in '%s' is not a list" % rule_file, + file=sys.stderr) + continue + for data in content: + if (all(n not in data for n in ('ros1_message_name', 'ros2_message_name', + 'ros1_service_name', 'ros2_service_name'))): + try: + rules.append(ActionMappingRule(data, package_name)) + except Exception as e: + print('%s' % str(e), file=sys.stderr) + return pkgs, actions, rules + + class Message: __slots__ = [ 'package_name', @@ -415,7 +523,8 @@ def __init__(self, data, expected_package_name): len(data) == (2 + int('enable_foreign_mappings' in data)) ) else: - raise Exception('Ignoring a rule without a ros1_package_name and/or ros2_package_name') + raise Exception( + 'Ignoring a rule without a ros1_package_name and/or ros2_package_name') def is_package_mapping(self): return self.package_mapping @@ -506,6 +615,52 @@ def __str__(self): return 'ServiceMappingRule(%s <-> %s)' % (self.ros1_package_name, self.ros2_package_name) +class ActionMappingRule(MappingRule): + __slots__ = [ + 'ros1_action_name', + 'ros2_action_name', + 'goal_fields_1_to_2', + 'result_fields_1_to_2', + 'feedback_fields_1_to_2', + ] + + def __init__(self, data, expected_package_name): + super().__init__(data, expected_package_name) + self.ros1_action_name = None + self.ros2_action_name = None + self.goal_fields_1_to_2 = None + self.result_fields_1_to_2 = None + self.feedback_fields_1_to_2 = None + if all(n in data for n in ('ros1_action_name', 'ros2_action_name')): + self.ros1_action_name = data['ros1_action_name'] + self.ros2_action_name = data['ros2_action_name'] + expected_keys = 4 + if 'goal_fields_1_to_2' in data: + self.goal_fields_1_to_2 = OrderedDict() + for ros1_field_name, ros2_field_name in data['goal_fields_1_to_2'].items(): + self.goal_fields_1_to_2[ros1_field_name] = ros2_field_name + expected_keys += 1 + if 'result_fields_1_to_2' in data: + self.result_fields_1_to_2 = OrderedDict() + for ros1_field_name, ros2_field_name in data['result_fields_1_to_2'].items(): + self.result_fields_1_to_2[ros1_field_name] = ros2_field_name + expected_keys += 1 + if 'feedback_fields_1_to_2' in data: + self.feedback_fields_1_to_2 = OrderedDict() + for ros1_field_name, ros2_field_name in data['feedback_fields_1_to_2'].items(): + self.feedback_fields_1_to_2[ros1_field_name] = ros2_field_name + expected_keys += 1 + elif len(data) > expected_keys: + raise RuntimeError( + 'Mapping for package %s contains unknown field(s)' % self.ros2_package_name) + elif len(data) > 2: + raise RuntimeError( + 'Mapping for package %s contains unknown field(s)' % self.ros2_package_name) + + def __str__(self): + return 'ActionMappingRule(%s, %s)' % (self.ros1_package_name, self.ros2_package_name) + + def determine_package_pairs(ros1_msgs, ros2_msgs, mapping_rules): pairs = [] # determine package names considered equal between ROS 1 and ROS 2 @@ -639,12 +794,18 @@ def determine_common_services( ros2_type = str(ros2_fields[direction][i].type) ros1_name = ros1_field[1] ros2_name = ros2_fields[direction][i].name - if ros1_type != ros2_type or ros1_name != ros2_name: - # 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: - match = False - break + if ros1_name != ros2_name: + # if the names do not match, check first if the types are builtin + 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_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 + not ros2_type.startswith('builtin_interfaces')): + match = False + break output[direction].append({ 'basic': False if '/' in ros1_type else True, 'array': True if '[]' in ros1_type else False, @@ -670,6 +831,105 @@ def determine_common_services( return services +def determine_common_actions( + ros1_actions, ros2_actions, mapping_rules, message_string_pairs=None +): + if message_string_pairs is None: + message_string_pairs = set() + + pairs = [] + actions = [] + for ros1_action in ros1_actions: + for ros2_action in ros2_actions: + if ros1_action.package_name == ros2_action.package_name: + if ros1_action.message_name == ros2_action.message_name: + pairs.append((ros1_action, ros2_action)) + + for rule in mapping_rules: + for ros1_action in ros1_actions: + for ros2_action in ros2_actions: + if rule.ros1_package_name == ros1_action.package_name and \ + rule.ros2_package_name == ros2_action.package_name: + if rule.ros1_action_name is None and rule.ros2_action_name is None: + if ros1_action.message_name == ros2_action.message_name: + pairs.append((ros1_action, ros2_action)) + else: + if ( + rule.ros1_action_name == ros1_action.message_name and + rule.ros2_action_name == ros2_action.message_name + ): + pairs.append((ros1_action, ros2_action)) + + for pair in pairs: + ros1_spec = load_ros1_action(pair[0]) + ros2_spec = load_ros2_action(pair[1]) + ros1_fields = { + 'goal': ros1_spec.goal.fields(), + 'result': ros1_spec.result.fields(), + 'feedback': ros1_spec.feedback.fields() + } + ros2_fields = { + 'goal': ros2_spec.goal.fields, + 'result': ros2_spec.result.fields, + 'feedback': ros2_spec.feedback.fields + } + output = { + 'goal': [], + 'result': [], + 'feedback': [] + } + match = True + for direction in ['goal', 'result', 'feedback']: + if len(ros1_fields[direction]) != len(ros2_fields[direction]): + match = False + break + for i, ros1_field in enumerate(ros1_fields[direction]): + ros1_type = ros1_field[0] + ros2_type = str(ros2_fields[direction][i].type) + ros1_name = ros1_field[1] + ros2_name = ros2_fields[direction][i].name + if ros1_name != ros2_name: + # if the message types have a custom mapping their names + # might not be equal, therefore check the message pairs + + 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_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 + if ((ros1_type, ros2_type) not in message_string_pairs and + not ros2_type.startswith('builtin_interfaces') and + 'GripperCommand' not in ros2_type): + match = False + break + output[direction].append({ + 'basic': False if '/' in ros1_type else True, + 'array': True if '[]' in ros1_type else False, + 'ros1': { + 'name': ros1_name, + 'type': ros1_type.rstrip('[]'), + 'cpptype': ros1_type.rstrip('[]').replace('/', '::') + }, + 'ros2': { + 'name': ros2_name, + 'type': ros2_type.rstrip('[]'), + 'cpptype': ros2_type.rstrip('[]').replace('/', '::msg::') + } + }) + if match: + actions.append({ + 'ros1_name': pair[0].message_name, + 'ros2_name': pair[1].message_name, + 'ros1_package': pair[0].package_name, + 'ros2_package': pair[1].package_name, + 'fields': output + }) + return actions + + def update_ros1_field_information(ros1_field, package_name): parts = ros1_field.base_type.split('/') assert len(parts) in [1, 2] @@ -704,11 +964,14 @@ def consume_field(field): selected_fields.append(field) fields = ros1_field_selection.split('.') - current_field = [f for f in parent_ros1_spec.parsed_fields() if f.name == fields[0]][0] + current_field = [f for f in parent_ros1_spec.parsed_fields() + if f.name == fields[0]][0] consume_field(current_field) for field in fields[1:]: - parent_ros1_spec = load_ros1_message(msg_idx.ros1_get_from_field(current_field)) - current_field = [f for f in parent_ros1_spec.parsed_fields() if f.name == field][0] + parent_ros1_spec = load_ros1_message( + msg_idx.ros1_get_from_field(current_field)) + current_field = [ + f for f in parent_ros1_spec.parsed_fields() if f.name == field][0] consume_field(current_field) return tuple(selected_fields) @@ -766,7 +1029,8 @@ def determine_field_mapping(ros1_msg, ros2_msg, mapping_rules, msg_idx): for ros1_field_selection, ros2_field_selection in rule.fields_1_to_2.items(): try: ros1_selected_fields = \ - get_ros1_selected_fields(ros1_field_selection, ros1_spec, msg_idx) + get_ros1_selected_fields( + ros1_field_selection, ros1_spec, msg_idx) except IndexError: print( "A manual mapping refers to an invalid field '%s' " % ros1_field_selection + @@ -776,7 +1040,8 @@ def determine_field_mapping(ros1_msg, ros2_msg, mapping_rules, msg_idx): continue try: ros2_selected_fields = \ - get_ros2_selected_fields(ros2_field_selection, ros2_spec, msg_idx) + get_ros2_selected_fields( + ros2_field_selection, ros2_spec, msg_idx) except IndexError: print( "A manual mapping refers to an invalid field '%s' " % ros2_field_selection + @@ -826,7 +1091,8 @@ def determine_field_mapping(ros1_msg, ros2_msg, mapping_rules, msg_idx): def load_ros1_message(ros1_msg): msg_context = genmsg.MsgContext.create_default() - message_path = os.path.join(ros1_msg.prefix_path, ros1_msg.message_name + '.msg') + message_path = os.path.join( + ros1_msg.prefix_path, ros1_msg.message_name + '.msg') try: spec = genmsg.msg_loader.load_msg_from_file( msg_context, message_path, '%s/%s' % (ros1_msg.package_name, ros1_msg.message_name)) @@ -837,10 +1103,123 @@ def load_ros1_message(ros1_msg): def load_ros1_service(ros1_srv): srv_context = genmsg.MsgContext.create_default() - srv_path = os.path.join(ros1_srv.prefix_path, ros1_srv.message_name + '.srv') + srv_path = os.path.join(ros1_srv.prefix_path, + ros1_srv.message_name + '.srv') srv_name = '%s/%s' % (ros1_srv.package_name, ros1_srv.message_name) try: - spec = genmsg.msg_loader.load_srv_from_file(srv_context, srv_path, srv_name) + spec = genmsg.msg_loader.load_srv_from_file( + srv_context, srv_path, srv_name) + except genmsg.InvalidMsgSpec: + return None + return spec + + +# genmsg/actions.py + +class ActionSpec(object): + + def __init__(self, goal, result, feedback, text, full_name='', short_name='', package=''): + + alt_package, alt_short_name = genmsg.package_resource_name(full_name) + if not package: + package = alt_package + if not short_name: + short_name = alt_short_name + + self.goal = goal + self.result = result + self.feedback = feedback + self.text = text + self.full_name = full_name + self.short_name = short_name + self.package = package + + def __eq__(self, other): + if not other or not isinstance(other, ActionSpec): + return False + return self.goal == other.goal and \ + self.result == other.result and \ + self.feedback == other.feedback and \ + self.text == other.text and \ + self.full_name == other.full_name and \ + self.short_name == other.short_name and \ + self.package == other.package + + def __ne__(self, other): + if not other or not isinstance(other, ActionSpec): + return True + return not self.__eq__(other) + + def __repr__(self): + return 'ActionSpec[%s, %s, %s]' % (repr(self.goal), repr(self.result), repr(self.feedback)) + + +# genmsg.msg_loader + +def load_action_from_string(msg_context, text, full_name): + """ + Load :class:`ActionSpec` from the .action file. + + :param msg_context: :class:`MsgContext` instance to load goal/reresult/feedback messages into. + :param text: .msg text , ``str`` + :param package_name: context to use for msg type name, i.e. the package name, + or '' to use local naming convention. ``str`` + :returns: :class:`ActionSpec` instance + :raises :exc:`InvalidMsgSpec` If syntax errors or other problems are detected in file + """ + text_goal = StringIO() + text_result = StringIO() + text_feedback = StringIO() + count = 0 + accum = text_goal + for l in text.split('\n'): + s = l.split(COMMENTCHAR)[0].strip() # strip comments + if s.startswith(IODELIM): # lenient, by request + if count == 0: + accum = text_result + count = 1 + else: + accum = text_feedback + else: + accum.write(s+'\n') + + # create separate MsgSpec objects for each half of file + msg_goal = genmsg.msg_loader.load_msg_from_string( + msg_context, text_goal.getvalue(), '%sGoal' % (full_name)) + msg_result = genmsg.msg_loader.load_msg_from_string( + msg_context, text_result.getvalue(), '%sResult' % (full_name)) + msg_feedback = genmsg.msg_loader.load_msg_from_string( + msg_context, text_feedback.getvalue(), '%sFeedback' % (full_name)) + return ActionSpec(msg_goal, msg_result, msg_feedback, text, full_name) + + +# genmsg.msg_loader +def load_action_from_file(msg_context, file_path, full_name): + """ + Convert the .action representation in the file to a :class:`MsgSpec` instance. + + NOTE: this will register the message in the *msg_context*. + :param file_path: path of file to load from, ``str`` + :returns: :class:`MsgSpec` instance + :raises: :exc:`InvalidMsgSpec`: if syntax errors or other problems are detected in file + """ + with open(file_path, 'r') as f: + text = f.read() + + spec = load_action_from_string(msg_context, text, full_name) + # msg_context.set_file('%sRequest' % (full_name), file_path) + # msg_context.set_file('%sResponse' % (full_name), file_path) + return spec + + +def load_ros1_action(ros1_action): + msg_context = genmsg.MsgContext.create_default() + message_path = os.path.join( + ros1_action.prefix_path, ros1_action.message_name + '.action') + try: + spec = load_action_from_file( + msg_context, message_path, '%s/%s' % + (ros1_action.package_name, ros1_action.message_name)) except genmsg.InvalidMsgSpec: return None return spec @@ -893,13 +1272,28 @@ def load_ros2_service(ros2_srv): ros2_srv.prefix_path, 'share', ros2_srv.package_name, 'srv', ros2_srv.message_name + '.srv') try: - spec = rosidl_adapter.parser.parse_service_file(ros2_srv.package_name, srv_path) + spec = rosidl_adapter.parser.parse_service_file( + ros2_srv.package_name, srv_path) except rosidl_adapter.parser.InvalidSpecification: return None return spec +def load_ros2_action(ros2_action): + 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, action_path) + except rosidl_adapter.parser.InvalidSpecification: + print('Invalid spec') + return None + return spec + # make field types hashable + + def FieldHash(self): return self.name.__hash__() diff --git a/src/action_bridge.cpp b/src/action_bridge.cpp new file mode 100644 index 00000000..16f2c881 --- /dev/null +++ b/src/action_bridge.cpp @@ -0,0 +1,72 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +// include ROS 1 +#ifdef __clang__ +#pragma clang diagnostic push +#endif +#include "ros/ros.h" +#ifdef __clang__ +#pragma clang diagnostic pop +#endif + +// include ROS 2 +#include "rclcpp/rclcpp.hpp" +#include "ros1_bridge/bridge.hpp" + +int main(int argc, char * argv[]) +{ + // ROS 2 node + // must be before ROS1, because ros::init consumes args like __name and we cannot remap the node + rclcpp::init(argc, argv); + auto ros2_node = rclcpp::Node::make_shared("ros_bridge"); + + // ROS 1 node + ros::init(argc, argv, "ros_bridge"); + ros::NodeHandle ros1_node; + + std::string dir = argv[1]; + std::string package = argv[2]; + std::string type = argv[3]; + std::string name = argv[4]; + + std::cout << dir << " " << package << " " << type << " " << name << std::endl; + + + 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); + } catch (std::runtime_error & e) { + fprintf(stderr, "Failed to created a bridge: %s\n", e.what()); + } + } else { + fprintf(stderr, "Failed to create a factory\n"); + } + + // ROS 1 asynchronous spinner + ros::AsyncSpinner async_spinner(1); + async_spinner.start(); + + // ROS 2 spinning loop + rclcpp::executors::SingleThreadedExecutor executor; + while (ros1_node.ok() && rclcpp::ok()) { + executor.spin_node_once(ros2_node, std::chrono::milliseconds(1000)); + } + + return 0; +} diff --git a/src/dynamic_bridge.cpp b/src/dynamic_bridge.cpp index b87d1bac..1baa31dd 100644 --- a/src/dynamic_bridge.cpp +++ b/src/dynamic_bridge.cpp @@ -19,6 +19,7 @@ #include #include #include +#include // NOLINT // include ROS 1 #ifdef __clang__ @@ -40,6 +41,7 @@ #include "ros1_bridge/bridge.hpp" #include "ros1_bridge/command_parser_utils.hpp" +#include "ros1_bridge/action_factory.hpp" std::mutex g_bridge_mutex; @@ -119,6 +121,15 @@ bool parse_command_options( } else { printf("No service type conversion pairs supported.\n"); } + mappings_2to1 = ros1_bridge::get_all_action_mappings_2to1(); + if (mappings_2to1.size() > 0) { + printf("Supported ROS 2 <=> ROS 1 action type conversion pairs:\n"); + for (auto & pair : mappings_2to1) { + printf(" - '%s' (ROS 2) <=> '%s' (ROS 1)\n", pair.first.c_str(), pair.second.c_str()); + } + } else { + printf("No action type conversion pairs supported.\n"); + } return false; } @@ -170,10 +181,16 @@ void update_bridge( const std::map & ros2_subscribers, const std::map> & ros1_services, const std::map> & ros2_services, + const std::map> & ros1_action_servers, + const std::map> & ros2_action_servers, std::map & bridges_1to2, std::map & bridges_2to1, std::map & service_bridges_1_to_2, std::map & service_bridges_2_to_1, + std::map> & action_bridges_1_to_2, + std::map> & action_bridges_2_to_1, bool bridge_all_1to2_topics, bool bridge_all_2to1_topics) { std::lock_guard lock(g_bridge_mutex); @@ -419,6 +436,83 @@ void update_bridge( ++it; } } + + // create bridges for ros1 actions + for (auto & ros1_action : ros1_action_servers) { + auto & name = ros1_action.first; + auto & details = ros1_action.second; + if ( + action_bridges_1_to_2.find(name) == action_bridges_1_to_2.end() && + action_bridges_2_to_1.find(name) == action_bridges_2_to_1.end()) + { + auto factory = ros1_bridge::get_action_factory( + "ros1", details.at("package"), details.at("type")); + if (factory) { + try { + factory->create_server_client(ros1_node, ros2_node, name); + action_bridges_2_to_1[name] = std::move(factory); + printf("Created 2 to 1 bridge for action %s\n", name.data()); + } catch (std::runtime_error & e) { + fprintf(stderr, "Failed to created a bridge: %s\n", e.what()); + } + } + } + } + + // create bridges for ros2 actions + for (auto & ros2_action : ros2_action_servers) { + auto & name = ros2_action.first; + auto & details = ros2_action.second; + if ( + action_bridges_1_to_2.find(name) == action_bridges_1_to_2.end() && + action_bridges_2_to_1.find(name) == action_bridges_2_to_1.end()) + { + auto factory = ros1_bridge::get_action_factory( + "ros2", details.at("package"), details.at("type")); + if (factory) { + try { + factory->create_server_client(ros1_node, ros2_node, name); + action_bridges_1_to_2[name] = std::move(factory); + printf("Created 1 to 2 bridge for action %s\n", name.data()); + } catch (std::runtime_error & e) { + fprintf(stderr, "Failed to created a bridge: %s\n", e.what()); + } + } + } + } + + // remove obsolete ros1 actions + for (auto it = action_bridges_2_to_1.begin(); it != action_bridges_2_to_1.end(); ) { + if (ros1_action_servers.find(it->first) == ros1_action_servers.end()) { + printf("Removed 2 to 1 bridge for action %s\n", it->first.data()); + try { + it->second->shutdown(); + it->second.reset(); + it = action_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()); + } + } else { + ++it; + } + } + + // remove obsolete ros2 actions + for (auto it = action_bridges_1_to_2.begin(); it != action_bridges_1_to_2.end(); ) { + if (ros2_action_servers.find(it->first) == ros2_action_servers.end()) { + printf("Removed 1 to 2 bridge for action %s\n", it->first.data()); + try { + // it->second.server.shutdown(); + it->second->shutdown(); + it->second.reset(); + it = action_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()); + } + } else { + ++it; + } + } } void get_ros1_service_info( @@ -495,6 +589,209 @@ void get_ros1_service_info( ros1_services[key]["name"] = std::string(t.begin() + t.find("/") + 1, t.end()); } +inline bool is_action_topic( + std::map> & actions, + std::map & action_nums, const bool is_action_type, + const std::string topic_name, const std::string topic_name_ends_with, + const std::string type, const std::string type_ends_with, bool is_ros2 = false) +{ + // check if the topic name and topic types are as expected + if (boost::algorithm::ends_with(topic_name.c_str(), topic_name_ends_with.c_str()) && + boost::algorithm::ends_with(type.c_str(), type_ends_with.c_str())) + { + // extract action name from topic name + std::string name = topic_name.substr(0, topic_name.find(topic_name_ends_with.c_str())); + if (actions.find(name) == actions.end()) { + actions[name]["package"] = ""; + actions[name]["type"] = ""; + action_nums[name] = 0; + } + + // e.g.: topic type of '/fibonacci/goal' is 'actionlib_tutorials/FibonacciActionGoal' + // Thus, package name is action type is 'actionlib_tutorials' and + // action type is 'Fibonacci' + if (!type.empty() && is_action_type) { + std::string pkg_name = type.substr(0, type.find("/")); + std::string action_type = + type.substr( + type.find_last_of("/") + 1, + type.length() - (type.find_last_of("/") + type_ends_with.length() + 1)); + actions[name]["package"] = pkg_name; + if (is_ros2) { + actions[name]["type"] = "action/" + action_type; + } else { + actions[name]["type"] = action_type; + } + } + + action_nums[name] += 1; + + return true; + } + return false; +} + +// if topics 'goal' with type 'ActionGoal' and 'cancel' with type 'GoalID' are pubs, then it is an +// action client +// equivalent ROS2 action pkg and type can be retrieved from get_mappings.cpp +void get_active_ros1_actions( + std::map publishers, + std::map subscribers, + std::map> & active_ros1_action_servers, + std::map> & active_ros1_action_clients) +{ + // check if the topics end with 'goal', 'result', 'cancel', 'status' + + // find topics that end with goal and cancel, find corresponding result, status and feedback + // in the other map + std::map::iterator it; + std::map::iterator it_num; + // store count of pubs and subs for each action + std::map action_server_nums, action_client_nums; + + for (it = publishers.begin(); it != publishers.end(); it++) { + // check for action client + if ( + is_action_topic( + active_ros1_action_clients, action_client_nums, false, + it->first.c_str(), "/cancel", it->second.c_str(), "/GoalID")) + { + continue; + } else if ( // NOLINT + is_action_topic( + active_ros1_action_clients, action_client_nums, true, + it->first.c_str(), "/goal", it->second.c_str(), "ActionGoal")) + { + continue; + } else if ( // NOLINT // check for action server + is_action_topic( + active_ros1_action_servers, action_server_nums, true, + it->first.c_str(), "/feedback", it->second.c_str(), + "ActionFeedback")) + { + continue; + } + if ( + is_action_topic( + active_ros1_action_servers, action_server_nums, false, + it->first.c_str(), "/result", it->second.c_str(), "ActionResult")) + { + continue; + } else if ( // NOLINT + is_action_topic( + active_ros1_action_servers, action_server_nums, false, + it->first.c_str(), "/status", it->second.c_str(), + "GoalStatusArray")) + { + continue; + } + } + + // subscribers do not report their types, but use it to confirm action + for (it = subscribers.begin(); it != subscribers.end(); it++) { + // check for action server + if ( + is_action_topic( + active_ros1_action_servers, action_server_nums, false, + it->first.c_str(), "/cancel", it->second.c_str(), "")) + { + continue; + } else if ( // NOLINT + is_action_topic( + active_ros1_action_servers, action_server_nums, false, + it->first.c_str(), "/goal", it->second.c_str(), "")) + { + continue; + } else if ( // NOLINT // check for action client + is_action_topic( + active_ros1_action_clients, action_client_nums, false, + it->first.c_str(), "/feedback", it->second.c_str(), "")) + { + continue; + } else if ( // NOLINT + is_action_topic( + active_ros1_action_clients, action_client_nums, false, + it->first.c_str(), "/result", it->second.c_str(), "")) + { + continue; + } else if ( // NOLINT + is_action_topic( + active_ros1_action_clients, action_client_nums, false, + it->first.c_str(), "/status", it->second.c_str(), "")) + { + continue; + } + } + + for (it_num = action_client_nums.begin(); it_num != action_client_nums.end(); it_num++) { + if (it_num->second != 5) { + active_ros1_action_clients.erase(it_num->first); + } + } + for (it_num = action_server_nums.begin(); it_num != action_server_nums.end(); it_num++) { + if (it_num->second != 5) { + active_ros1_action_servers.erase(it_num->first); + } + } +} + +// how does ros2 action list determine active interfaces? +// ref: opt/ros/foxy/lib/python3.8/site-packages/ros2action/verb/list.py +// https://github.com/ros2/rcl/blob/master/rcl_action/src/rcl_action/graph.c +void get_active_ros2_actions( + const std::map active_ros2_publishers, + const std::map active_ros2_subscribers, + std::map> & active_ros2_action_servers, + std::map> & active_ros2_action_clients) +{ + std::map::const_iterator it; + std::map::iterator it_num; + std::map action_server_nums, action_client_nums; + for (it = active_ros2_publishers.begin(); it != active_ros2_publishers.end(); it++) { + if ( + is_action_topic( + active_ros2_action_servers, action_server_nums, true, it->first.c_str(), + "/_action/feedback", it->second.c_str(), "_FeedbackMessage", true)) + { + continue; + } else if ( // NOLINT + is_action_topic( + active_ros2_action_servers, action_server_nums, false, + it->first.c_str(), "/_action/status", it->second.c_str(), + "GoalStatusArray"), true) + { + continue; + } + } + for (it = active_ros2_subscribers.begin(); it != active_ros2_subscribers.end(); it++) { + if ( + is_action_topic( + active_ros2_action_clients, action_client_nums, true, + it->first.c_str(), "/_action/feedback", it->second.c_str(), + "_FeedbackMessage", true)) + { + continue; + } else if ( // NOLINT + is_action_topic( + active_ros2_action_clients, action_client_nums, false, + it->first.c_str(), "/_action/status", it->second.c_str(), + "GoalStatusArray", true)) + { + continue; + } + } + for (it_num = action_client_nums.begin(); it_num != action_client_nums.end(); it_num++) { + if (it_num->second != 2) { + active_ros2_action_clients.erase(it_num->first); + } + } + for (it_num = action_server_nums.begin(); it_num != action_server_nums.end(); it_num++) { + if (it_num->second != 2) { + active_ros2_action_servers.erase(it_num->first); + } + } +} + int main(int argc, char * argv[]) { bool output_topic_introspection; @@ -528,11 +825,16 @@ int main(int argc, char * argv[]) std::map ros2_subscribers; std::map> ros1_services; std::map> ros2_services; - + std::map> ros1_action_servers; + std::map> ros1_action_clients; + std::map> ros2_action_servers; + std::map> ros2_action_clients; std::map bridges_1to2; std::map bridges_2to1; std::map service_bridges_1_to_2; std::map service_bridges_2_to_1; + std::map> action_bridges_1_to_2; + std::map> action_bridges_2_to_1; // setup polling of ROS 1 master auto ros1_poll = [ @@ -541,7 +843,10 @@ int main(int argc, char * argv[]) &ros2_publishers, &ros2_subscribers, &bridges_1to2, &bridges_2to1, &ros1_services, &ros2_services, + &ros1_action_servers, &ros1_action_clients, + &ros2_action_servers, &ros2_action_clients, &service_bridges_1_to_2, &service_bridges_2_to_1, + &action_bridges_1_to_2, &action_bridges_2_to_1, &output_topic_introspection, &bridge_all_1to2_topics, &bridge_all_2to1_topics ](const ros::TimerEvent &) -> void @@ -644,6 +949,20 @@ int main(int argc, char * argv[]) } } + // check actions + std::map> active_ros1_action_servers, + active_ros1_action_clients; + get_active_ros1_actions( + current_ros1_publishers, current_ros1_subscribers, + active_ros1_action_servers, active_ros1_action_clients); + + { + std::lock_guard lock(g_bridge_mutex); + ros1_services = active_ros1_services; + ros1_action_servers = active_ros1_action_servers; + ros1_action_clients = active_ros1_action_clients; + } + if (output_topic_introspection) { printf("\n"); } @@ -659,8 +978,10 @@ int main(int argc, char * argv[]) ros1_publishers, ros1_subscribers, ros2_publishers, ros2_subscribers, ros1_services, ros2_services, + 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, bridge_all_1to2_topics, bridge_all_2to1_topics); }; @@ -676,8 +997,11 @@ 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, &bridges_1to2, &bridges_2to1, &service_bridges_1_to_2, &service_bridges_2_to_1, + &action_bridges_1_to_2, &action_bridges_2_to_1, &output_topic_introspection, &bridge_all_1to2_topics, &bridge_all_2to1_topics, &already_ignored_topics, &already_ignored_services @@ -803,9 +1127,17 @@ int main(int argc, char * argv[]) } } + std::map> active_ros2_action_servers, + active_ros2_action_clients; + get_active_ros2_actions( + current_ros2_publishers, current_ros2_subscribers, + active_ros2_action_servers, active_ros2_action_clients); + { std::lock_guard lock(g_bridge_mutex); ros2_services = active_ros2_services; + ros2_action_servers = active_ros2_action_servers; + ros2_action_clients = active_ros2_action_clients; } if (output_topic_introspection) { @@ -823,8 +1155,10 @@ int main(int argc, char * argv[]) ros1_publishers, ros1_subscribers, ros2_publishers, ros2_subscribers, ros1_services, ros2_services, + 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, bridge_all_1to2_topics, bridge_all_2to1_topics); };