diff --git a/CMakeLists.txt b/CMakeLists.txt index 1686882..421b950 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -16,7 +16,10 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") endif() find_package(ament_cmake REQUIRED) +find_package(ament_cmake_python REQUIRED) + find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) find_package(rclcpp_components REQUIRED) find_package(rcutils REQUIRED) # Leverage rosbag2's generic type support utilities @@ -27,6 +30,16 @@ find_package(yaml_cpp_vendor REQUIRED) find_package(zstd_vendor REQUIRED) find_package(zstd REQUIRED) +find_package(Python3 REQUIRED COMPONENTS Interpreter) + +set(GENERATED_CODE "${CMAKE_CURRENT_BINARY_DIR}/generated_code.cpp") + +add_custom_command( + OUTPUT ${GENERATED_CODE} + COMMAND Python3::Interpreter ${CMAKE_CURRENT_SOURCE_DIR}/bin/generate_code.py --output ${GENERATED_CODE} + DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/bin/generate_code.py ${CMAKE_CURRENT_SOURCE_DIR}/resources/code_template.cpp.em +) + rosidl_generate_interfaces(${PROJECT_NAME} msg/CompressedMsg.msg ) @@ -40,8 +53,26 @@ add_library(${PROJECT_NAME}_lib SHARED src/${PROJECT_NAME}/qos_options.cpp src/${PROJECT_NAME}/service_bridge_options.cpp src/${PROJECT_NAME}/topic_bridge_options.cpp + src/${PROJECT_NAME}/action_bridge_options.cpp + + ${GENERATED_CODE} ) + +# generate per interface compilation units to keep the memory usage low +ament_index_get_resources(ros2_interface_packages "rosidl_interfaces") +# actionlib_msgs is deprecated, but we will quiet the warning until the bridge has support for +# ROS actions: https://github.com/ros2/design/issues/195 +foreach(package_name ${ros2_interface_packages}) + find_package(${package_name} QUIET REQUIRED) + message(STATUS "Found ${package_name}: ${${package_name}_VERSION} (${${package_name}_DIR})") + if("${package_name}" STREQUAL "builtin_interfaces") + continue() + endif() + list(APPEND interface_packages ${package_name}) +endforeach() + + target_include_directories(${PROJECT_NAME}_lib PUBLIC $ $ @@ -53,12 +84,14 @@ endif() ament_target_dependencies(${PROJECT_NAME}_lib rclcpp + rclcpp_action rclcpp_components rcutils rosbag2_cpp rosidl_typesupport_cpp yaml_cpp_vendor zstd + ${interface_packages} ) rosidl_get_typesupport_target(cpp_typesupport_target @@ -130,6 +163,7 @@ install(DIRECTORY examples launch ament_export_targets(export_${PROJECT_NAME}) ament_export_dependencies( rclcpp + rclcpp_action rclcpp_components rosbag2_cpp rcutils @@ -150,6 +184,7 @@ if(BUILD_TESTING) ) ament_target_dependencies(${PROJECT_NAME}_test_component_lib rclcpp + rclcpp_action rclcpp_components test_msgs ) diff --git a/bin/generate_code.py b/bin/generate_code.py new file mode 100755 index 0000000..ec76549 --- /dev/null +++ b/bin/generate_code.py @@ -0,0 +1,79 @@ +#!/usr/bin/env python3 + +import argparse +import os +from ament_index_python.packages import get_packages_with_prefixes +from rosidl_cmake import expand_template +import re + + +def camel_case_to_lower_case_underscore(value): + # insert an underscore before any upper case letter + # which is not followed by another upper case letter + value = re.sub('(.)([A-Z][a-z]+)', '\\1_\\2', value) + # insert an underscore before any upper case letter + # which is preseded by a lower case letter or number + value = re.sub('([a-z0-9])([A-Z])', '\\1_\\2', value) + return value.lower() + +def type_name_to_include(type_name): + package, interface_type, type = type_name.split("/") + type = camel_case_to_lower_case_underscore(type) + return "#include <" + package + "/" + interface_type + "/" + type + ".hpp>" + +def type_name_to_cpp_type(type_name): + package, interface_type, type = type_name.split("/") + return package + "::" + interface_type + "::" + type + +def get_types(): + ros2_packages = get_packages_with_prefixes() + + message_types = [] + service_types = [] + action_types = [] + + for package_name, prefix_path in ros2_packages.items(): + share_path = os.path.join(prefix_path, 'share', package_name) + # Collect message types + msg_path = os.path.join(share_path, 'msg') + if os.path.exists(msg_path): + for msg_file in os.listdir(msg_path): + if msg_file.endswith('.msg'): + msg_type = f"{package_name}/msg/{msg_file[:-4]}" + message_types.append(msg_type) + # Collect service types + srv_path = os.path.join(share_path, 'srv') + if os.path.exists(srv_path): + for srv_file in os.listdir(srv_path): + if srv_file.endswith('.srv'): + srv_type = f"{package_name}/srv/{srv_file[:-4]}" + service_types.append(srv_type) + # Collect action types + action_path = os.path.join(share_path, 'action') + if os.path.exists(action_path): + for action_file in os.listdir(action_path): + if action_file.endswith('.action'): + action_type = f"{package_name}/action/{action_file[:-7]}" + action_types.append(action_type) + + return message_types, service_types, action_types + +def main(): + parser = argparse.ArgumentParser() + parser.add_argument('--output', required=True) + args = parser.parse_args() + + message_types, service_types, action_types = get_types() + + template_file = os.path.join(os.path.dirname(__file__), '../resources/code_template.cpp.em') + expand_template(template_file, { + 'message_types': message_types, + 'service_types': service_types, + 'action_types': action_types, + 'camel_case_to_lower_case_underscore': camel_case_to_lower_case_underscore, + 'type_name_to_include': type_name_to_include, + 'type_name_to_cpp_type': type_name_to_cpp_type + }, args.output) + +if __name__ == '__main__': + main() diff --git a/examples/example_bridge_config.yaml b/examples/example_bridge_config.yaml index 9add192..51ccbd5 100644 --- a/examples/example_bridge_config.yaml +++ b/examples/example_bridge_config.yaml @@ -8,7 +8,7 @@ topics: type: rosgraph_msgs/msg/Clock # Bridge "/chatter" topic from doman ID 2 to domain ID 3 chatter: - type: example_interfaces/msg/String + type: std_msgs/msg/String # Override QoS reliablity setting to be best effort # This affects the domain bridge subscription and publisher # This does NOT affect the other "chatter" bridge below, which is for a different domain @@ -16,16 +16,16 @@ topics: reliability: best_effort # Also bridge "/chatter" topic from doman ID 2 to domain ID 4 chatter: - type: example_interfaces/msg/String + type: std_msgs/msg/String to_domain: 4 # Bridge reversed "/chatter" topic chatter: - type: example_interfaces/msg/String + type: std_msgs/msg/String # Reverse 'from' and 'to' domains (bridge from domain ID 3 to domain ID 2) reversed: True # Bridge bidirectional "/chatter" topic chatter: - type: example_interfaces/msg/String + type: std_msgs/msg/String from_domain: 7 to_domain: 8 # Bridge from domain ID 7 to domain ID 8 AND from domain ID 8 to domain ID 7 @@ -33,5 +33,18 @@ topics: # Bridge "/chatter" topic from doman ID 2 to domain ID 3, but as "/chitter" chatter: - type: example_interfaces/msg/String + type: std_msgs/msg/String remap: chitter + +services: + add_two_ints: + type: example_interfaces/srv/AddTwoInts + remap: add_two_numbers + from_domain: 1 + to_domain: 2 + +actions: + fibonacci: + type: action_tutorials_interfaces/action/Fibonacci + from_domain: 1 + to_domain: 2 diff --git a/include/domain_bridge/action_bridge.hpp b/include/domain_bridge/action_bridge.hpp new file mode 100644 index 0000000..abbaccd --- /dev/null +++ b/include/domain_bridge/action_bridge.hpp @@ -0,0 +1,63 @@ +#ifndef DOMAIN_BRIDGE__ACTION_BRIDGE_HPP_ +#define DOMAIN_BRIDGE__ACTION_BRIDGE_HPP_ + +#include +#include + +namespace domain_bridge +{ + +/// Info and ROS entities related to a action bridge +struct ActionBridge +{ + /// Name of the bridged action + std::string action_name; + + /// Name of the message type sent on the action + std::string type_name; + + /// Domain ID that the subscription uses + std::size_t from_domain_id; + + /// Domain ID that the publisher uses + std::size_t to_domain_id; + + /// Less-than operator. + /** + * Sort by 'from_domain_id', + * then by 'to_domain_id', + * then by 'action_name', + * then by 'type_name' + */ + bool operator<(const ActionBridge & other) const + { + if (from_domain_id < other.from_domain_id) { + return true; + } + if (from_domain_id > other.from_domain_id) { + return false; + } + if (to_domain_id < other.to_domain_id) { + return true; + } + if (to_domain_id > other.to_domain_id) { + return false; + } + int name_compare = action_name.compare(other.action_name); + if (name_compare < 0) { + return true; + } + if (name_compare > 0) { + return false; + } + int type_compare = type_name.compare(other.type_name); + if (type_compare < 0) { + return true; + } + return false; + } +}; + +} // namespace domain_bridge + +#endif // DOMAIN_BRIDGE__ACTION_BRIDGE_HPP_ diff --git a/include/domain_bridge/action_bridge_impl.inc b/include/domain_bridge/action_bridge_impl.inc new file mode 100644 index 0000000..40a08a7 --- /dev/null +++ b/include/domain_bridge/action_bridge_impl.inc @@ -0,0 +1,246 @@ +// Copyright 2021, 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 +#include +#include + +#include "rclcpp/node.hpp" +#include "rclcpp_action/rclcpp_action.hpp" + +// This is a non-header file meant to be included in `domain_bridge.hpp`. +// It contains the implementation of the DomainBridge::bridge_action() template method. + +// Conditionally include domain_bridge.hpp to help intellisense. +#ifndef DOMAIN_BRIDGE__DOMAIN_BRIDGE_HPP_ +# include "domain_bridge/domain_bridge.hpp" +#endif + +using namespace std::chrono_literals; + +namespace domain_bridge +{ + +namespace detail +{ +// action_name, from_domain_id, to_domain_id +using ActionBridge = std::tuple; + +bool +is_bridging_action( + const DomainBridgeImpl & impl, const ActionBridge & action_bridge); + +void +add_action_bridge( + DomainBridgeImpl & impl, + const rclcpp::Node::SharedPtr & node, + ActionBridge action_bridge, + std::function()> create_action, + std::shared_ptr client); + +rclcpp::Node::SharedPtr +get_node_for_domain(DomainBridgeImpl & impl, std::size_t domain_id); + +const std::string & +get_node_name(const DomainBridgeImpl & impl); +} // namespace detail + +/// Bridge a action from one domain to another. +/** + * \param action_bridge: Struct containing info about the action to bridge. + * \param options: Options for bridging the topic. + */ +template +void +DomainBridge::bridge_action( + const std::string & action_name, + size_t from_domain_id, + size_t to_domain_id, + const ActionBridgeOptions & options) +{ + using ResultT = typename ActionT::Result; + using FeedbackT = typename ActionT::Feedback; + using GoalT = typename ActionT::Goal; + + const auto & node_name = detail::get_node_name(*impl_); + // Validate action name + const std::string & resolved_action_name = rclcpp::expand_topic_or_service_name( + action_name, node_name, "/", true); + + // If a remap name is provided, then validate it + // otherwise "remap" to the same name + std::string action_remapped = resolved_action_name; + if (!options.remap_name().empty()) { + action_remapped = rclcpp::expand_topic_or_service_name( + options.remap_name(), node_name, "/", true); + } + + std::tuple action_bridge = { + resolved_action_name, + from_domain_id, + to_domain_id + }; + + // Check if already bridged + if (detail::is_bridging_action(*impl_, action_bridge)) + { + std::cerr << "Action '" << resolved_action_name << "'" << " already bridged from domain " << + std::to_string(from_domain_id) << " to domain " << std::to_string(to_domain_id) << + ", ignoring" << std::endl; + return; + } + + rclcpp::Node::SharedPtr from_domain_node = detail::get_node_for_domain(*impl_, from_domain_id); + rclcpp::Node::SharedPtr to_domain_node = detail::get_node_for_domain(*impl_, to_domain_id); + + // Create a client for the 'from_domain' + auto client = rclcpp_action::create_client( + from_domain_node, + resolved_action_name, + options.callback_group()); + + auto handle_goal = + [client, to_domain_node]( + const rclcpp_action::GoalUUID & /*uuid*/, + std::shared_ptr /*goal*/) -> rclcpp_action::GoalResponse + { + RCLCPP_INFO(to_domain_node->get_logger(), "Received goal request"); + // Accept all goals + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + }; + auto handle_cancel = + [client, to_domain_node](const std::shared_ptr> /*goal_handle*/) { + RCLCPP_INFO(to_domain_node->get_logger(), "Received request to cancel goal"); + // Accept all cancel requests + return rclcpp_action::CancelResponse::ACCEPT; + }; + auto execute = + [client, to_domain_node](const std::shared_ptr> goal_handle) + { + RCLCPP_INFO(to_domain_node->get_logger(), "Executing goal forwarding"); + + // Wait for the backend action server to be available + if (!client->wait_for_action_server(5s)) { + RCLCPP_ERROR(to_domain_node->get_logger(), "Backend action server not available"); + goal_handle->abort(std::make_shared()); + return; + } + + // Create a goal message for the backend action server + auto goal_msg = GoalT(); + goal_msg = *goal_handle->get_goal(); + + // Set up goal options, including callbacks + using SendGoalOptions = typename rclcpp_action::Client::SendGoalOptions; + auto send_goal_options = SendGoalOptions(); + + // Goal response callback + send_goal_options.goal_response_callback = + [goal_handle, to_domain_node](typename rclcpp_action::ClientGoalHandle::SharedPtr backend_goal_handle) { + if (!backend_goal_handle) { + RCLCPP_ERROR(to_domain_node->get_logger(), "Backend server rejected the goal"); + goal_handle->abort(std::make_shared()); + } else { + RCLCPP_INFO(to_domain_node->get_logger(), "Goal accepted by backend server"); + } + }; + + // Feedback callback + send_goal_options.feedback_callback = + [to_domain_node, goal_handle](typename rclcpp_action::ClientGoalHandle::SharedPtr, + const std::shared_ptr feedback) { + RCLCPP_INFO(to_domain_node->get_logger(), "Forwarding feedback"); + auto feedback_copy = std::make_shared(*feedback); + goal_handle->publish_feedback(feedback_copy); + }; + + // Result callback + send_goal_options.result_callback = + [to_domain_node, goal_handle](const typename rclcpp_action::ClientGoalHandle::WrappedResult & result) { + RCLCPP_INFO(to_domain_node->get_logger(), "Received result from backend server"); + switch (result.code) { + case rclcpp_action::ResultCode::SUCCEEDED: + goal_handle->succeed(result.result); + break; + case rclcpp_action::ResultCode::ABORTED: + RCLCPP_ERROR(to_domain_node->get_logger(), "Goal was aborted by backend server"); + goal_handle->abort(result.result); + break; + case rclcpp_action::ResultCode::CANCELED: + RCLCPP_INFO(to_domain_node->get_logger(), "Goal was canceled by backend server"); + goal_handle->canceled(result.result); + break; + default: + RCLCPP_ERROR(to_domain_node->get_logger(), "Unknown result code from backend server"); + goal_handle->abort(result.result); + break; + } + }; + + // Send the goal to the backend action server + auto future_goal_handle = client->async_send_goal(goal_msg, send_goal_options); + + // Handle cancellation in a loop + while (rclcpp::ok()) { + // Check if the goal has been canceled + if (goal_handle->is_canceling()) { + RCLCPP_INFO(to_domain_node->get_logger(), "Forwarding cancel request to backend server"); + auto cancel_future = client->async_cancel_all_goals(); + + // We don't use spin_until_future_complete, so we just log and return + RCLCPP_INFO(to_domain_node->get_logger(), "Cancel request sent to backend server"); + goal_handle->canceled(std::make_shared()); + return; + } + + // Sleep briefly to prevent busy waiting + std::this_thread::sleep_for(100ms); + + // Check if the goal is done (successfully or not) + if (goal_handle->is_executing() || goal_handle->is_active()) { + continue; // Goal is still executing + } else { + break; // Goal is done + } + } + }; + auto handle_accept = + [client, execute = std::move(execute)](const std::shared_ptr> goal_handle) { + std::thread{std::bind(execute, goal_handle)}.detach(); + }; + + // A action for the 'to_domain' will be created when we detect there is a action running in + // the 'from_domain'. + auto create_action_cb = [ + to_domain_node = std::move(to_domain_node), + action_remapped = std::move(action_remapped), + handle_goal = std::move(handle_goal), + handle_cancel = std::move(handle_cancel), + handle_accept = std::move(handle_accept), + options = std::move(options)]() + { + return rclcpp_action::create_server( + to_domain_node, + action_remapped, + handle_goal, + handle_cancel, + handle_accept); + }; + + detail::add_action_bridge( + *impl_, from_domain_node, action_bridge, create_action_cb, client); +} + +} // namespace domain_bridge diff --git a/include/domain_bridge/action_bridge_options.hpp b/include/domain_bridge/action_bridge_options.hpp new file mode 100644 index 0000000..a02c4a3 --- /dev/null +++ b/include/domain_bridge/action_bridge_options.hpp @@ -0,0 +1,77 @@ +// Copyright 2021, 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. + +#ifndef DOMAIN_BRIDGE__ACTION_BRIDGE_OPTIONS_HPP_ +#define DOMAIN_BRIDGE__ACTION_BRIDGE_OPTIONS_HPP_ + +#include +#include + +#include "rclcpp/callback_group.hpp" + +#include "domain_bridge/visibility_control.hpp" + +namespace domain_bridge +{ + +/// Options for bridged actions. +class ActionBridgeOptions +{ +public: + /// Constructor. + /** + * Default values: + * + * - callback_group = nullptr (node's default) + * - remap_name = "" (no remap) + */ + DOMAIN_BRIDGE_PUBLIC + ActionBridgeOptions() = default; + + /// Assignment operator. + DOMAIN_BRIDGE_PUBLIC + ActionBridgeOptions & + operator=(const ActionBridgeOptions & other) = default; + + /// Get callback group associated with the actions pub/sub pair. + DOMAIN_BRIDGE_PUBLIC + std::shared_ptr + callback_group() const; + + /// Set the callback group associated with the actions pub/sub pair. + DOMAIN_BRIDGE_PUBLIC + ActionBridgeOptions & + callback_group(std::shared_ptr group); + + /// Get remap name. + DOMAIN_BRIDGE_PUBLIC + const std::string & + remap_name() const; + + /// Set remap name. + /** + * An empty string implies no remap. + */ + DOMAIN_BRIDGE_PUBLIC + ActionBridgeOptions & + remap_name(const std::string & remap_name); + +private: + std::shared_ptr callback_group_{nullptr}; + std::string remap_name_; +}; // class ActionBridgeOptions + +} // namespace domain_bridge + +#endif // DOMAIN_BRIDGE__ACTION_BRIDGE_OPTIONS_HPP_ diff --git a/include/domain_bridge/domain_bridge.hpp b/include/domain_bridge/domain_bridge.hpp index ab528d5..7eeef2f 100644 --- a/include/domain_bridge/domain_bridge.hpp +++ b/include/domain_bridge/domain_bridge.hpp @@ -25,6 +25,7 @@ #include "domain_bridge/domain_bridge_options.hpp" #include "domain_bridge/topic_bridge.hpp" #include "domain_bridge/topic_bridge_options.hpp" +#include "domain_bridge/action_bridge_options.hpp" #include "domain_bridge/service_bridge_options.hpp" #include "domain_bridge/visibility_control.hpp" @@ -148,6 +149,21 @@ class DomainBridge size_t to_domain_id, const ServiceBridgeOptions & options = ServiceBridgeOptions()); + /// Bridge a action from one domain to another. + /** + * \param action: Name of the action to be bridged. + * \param from_domain_id: Domain id where there is a action server to be bridged. + * \param to_domain_id: Domain id where we want to make request to the bridged action. + * \param options: Options for bridging the topic. + */ + template + void bridge_action( + const std::string & action, + size_t from_domain_id, + size_t to_domain_id, + const ActionBridgeOptions & options = ActionBridgeOptions()); + + /// Get bridged topics. /** * \return Topic bridges created by `bridge_topic()`. @@ -163,5 +179,6 @@ class DomainBridge // Implementation of the bridge_service() template method. #include "domain_bridge/service_bridge_impl.inc" +#include "domain_bridge/action_bridge_impl.inc" #endif // DOMAIN_BRIDGE__DOMAIN_BRIDGE_HPP_ diff --git a/include/domain_bridge/domain_bridge_config.hpp b/include/domain_bridge/domain_bridge_config.hpp index 73afaa2..155c851 100644 --- a/include/domain_bridge/domain_bridge_config.hpp +++ b/include/domain_bridge/domain_bridge_config.hpp @@ -19,6 +19,10 @@ #include #include "domain_bridge/domain_bridge_options.hpp" +#include "domain_bridge/action_bridge.hpp" +#include "domain_bridge/action_bridge_options.hpp" +#include "domain_bridge/service_bridge.hpp" +#include "domain_bridge/service_bridge_options.hpp" #include "domain_bridge/topic_bridge.hpp" #include "domain_bridge/topic_bridge_options.hpp" @@ -33,6 +37,8 @@ struct DomainBridgeConfig /// Topic bridges with options std::vector> topics; + std::vector> services; + std::vector> actions; }; } // namespace domain_bridge diff --git a/include/domain_bridge/generated_code_interface.hpp b/include/domain_bridge/generated_code_interface.hpp new file mode 100644 index 0000000..b098b46 --- /dev/null +++ b/include/domain_bridge/generated_code_interface.hpp @@ -0,0 +1,24 @@ +#pragma once + +#include +#include "domain_bridge/domain_bridge.hpp" + +namespace domain_bridge { + +bool add_service_bridge_by_name( + DomainBridge & domain_bridge, + const std::string& service_name, + const std::string& service_type, + size_t from_domain_id, + size_t to_domain_id, + const ServiceBridgeOptions & options = ServiceBridgeOptions()); + +bool add_action_bridge_by_name( + DomainBridge & domain_bridge, + const std::string& service_name, + const std::string& service_type, + size_t from_domain_id, + size_t to_domain_id, + const ActionBridgeOptions & options = ActionBridgeOptions()); + +} diff --git a/include/domain_bridge/process_cmd_line_arguments.hpp b/include/domain_bridge/process_cmd_line_arguments.hpp index 9ffdb52..835a04d 100644 --- a/include/domain_bridge/process_cmd_line_arguments.hpp +++ b/include/domain_bridge/process_cmd_line_arguments.hpp @@ -22,6 +22,7 @@ #include #include "domain_bridge/domain_bridge_config.hpp" +#include "domain_bridge/parse_domain_bridge_yaml_config.hpp" namespace domain_bridge { diff --git a/include/domain_bridge/service_bridge.hpp b/include/domain_bridge/service_bridge.hpp new file mode 100644 index 0000000..2336e44 --- /dev/null +++ b/include/domain_bridge/service_bridge.hpp @@ -0,0 +1,63 @@ +#ifndef DOMAIN_BRIDGE__SERVICE_BRIDGE_HPP_ +#define DOMAIN_BRIDGE__SERVICE_BRIDGE_HPP_ + +#include +#include + +namespace domain_bridge +{ + +/// Info and ROS entities related to a service bridge +struct ServiceBridge +{ + /// Name of the bridged service + std::string service_name; + + /// Name of the message type sent on the service + std::string type_name; + + /// Domain ID that the subscription uses + std::size_t from_domain_id; + + /// Domain ID that the publisher uses + std::size_t to_domain_id; + + /// Less-than operator. + /** + * Sort by 'from_domain_id', + * then by 'to_domain_id', + * then by 'service_name', + * then by 'type_name' + */ + bool operator<(const ServiceBridge & other) const + { + if (from_domain_id < other.from_domain_id) { + return true; + } + if (from_domain_id > other.from_domain_id) { + return false; + } + if (to_domain_id < other.to_domain_id) { + return true; + } + if (to_domain_id > other.to_domain_id) { + return false; + } + int name_compare = service_name.compare(other.service_name); + if (name_compare < 0) { + return true; + } + if (name_compare > 0) { + return false; + } + int type_compare = type_name.compare(other.type_name); + if (type_compare < 0) { + return true; + } + return false; + } +}; + +} // namespace domain_bridge + +#endif // DOMAIN_BRIDGE__SERVICE_BRIDGE_HPP_ diff --git a/package.xml b/package.xml index 61921e5..7f131c4 100644 --- a/package.xml +++ b/package.xml @@ -17,6 +17,9 @@ rosidl_typesupport_cpp yaml-cpp zstd_vendor + ament_index_python + ament_cmake_python + python3 rosidl_default_runtime diff --git a/resources/code_template.cpp.em b/resources/code_template.cpp.em new file mode 100644 index 0000000..af7284a --- /dev/null +++ b/resources/code_template.cpp.em @@ -0,0 +1,55 @@ +#include +#include "domain_bridge/domain_bridge.hpp" +#include "domain_bridge/generated_code_interface.hpp" + +@[for srv_type in service_types]@ +@(type_name_to_include(srv_type)) +@[end for]@ + +@[for action_type in action_types]@ +@(type_name_to_include(action_type)) +@[end for]@ + +bool domain_bridge::add_service_bridge_by_name( + domain_bridge::DomainBridge & domain_bridge, + const std::string& service_name, + const std::string& service_type, + size_t from_domain_id, + size_t to_domain_id, + const domain_bridge::ServiceBridgeOptions & options) { + + if (false) { + } +@[for srv_type in service_types]@ + else if (service_type == "@(srv_type)") { + // Handle @(srv_type) + std::cout << "Adding service bridge for @(srv_type) " << service_name << " from domain " << from_domain_id << " to domain " << to_domain_id << std::endl; + + domain_bridge.bridge_service<@(type_name_to_cpp_type(srv_type))>(service_name, from_domain_id, to_domain_id, options); + return true; + } +@[end for]@ + return false; +} + +bool domain_bridge::add_action_bridge_by_name( + domain_bridge::DomainBridge & domain_bridge, + const std::string& action_name, + const std::string& action_type, + size_t from_domain_id, + size_t to_domain_id, + const domain_bridge::ActionBridgeOptions & options) { + + if (false) { + } +@[for action_type in action_types]@ + else if (action_type == "@(action_type)") { + // Handle @(action_type) + std::cout << "Adding action bridge for @(action_type) " << action_name << " from domain " << from_domain_id << " to domain " << to_domain_id << std::endl; + + domain_bridge.bridge_action<@(type_name_to_cpp_type(action_type))>(action_name, from_domain_id, to_domain_id, options); + return true; + } +@[end for]@ + return false; +} diff --git a/src/domain_bridge.cpp b/src/domain_bridge.cpp index 43ab9a2..b07d510 100644 --- a/src/domain_bridge.cpp +++ b/src/domain_bridge.cpp @@ -17,6 +17,7 @@ #include "rclcpp/rclcpp.hpp" #include "rclcpp/executors/single_threaded_executor.hpp" +#include "domain_bridge/generated_code_interface.hpp" #include "domain_bridge/component_manager.hpp" #include "domain_bridge/domain_bridge.hpp" #include "domain_bridge/parse_domain_bridge_yaml_config.hpp" diff --git a/src/domain_bridge/action_bridge_options.cpp b/src/domain_bridge/action_bridge_options.cpp new file mode 100644 index 0000000..71efdc2 --- /dev/null +++ b/src/domain_bridge/action_bridge_options.cpp @@ -0,0 +1,51 @@ +// Copyright 2021, 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 "domain_bridge/action_bridge_options.hpp" + +#include +#include + +#include "rclcpp/callback_group.hpp" + +namespace domain_bridge +{ + +std::shared_ptr +ActionBridgeOptions::callback_group() const +{ + return callback_group_; +} + +ActionBridgeOptions & +ActionBridgeOptions::callback_group(std::shared_ptr group) +{ + callback_group_ = group; + return *this; +} + +const std::string & +ActionBridgeOptions::remap_name() const +{ + return remap_name_; +} + +ActionBridgeOptions & +ActionBridgeOptions::remap_name(const std::string & remap_name) +{ + remap_name_ = remap_name; + return *this; +} + +} // namespace domain_bridge diff --git a/src/domain_bridge/domain_bridge.cpp b/src/domain_bridge/domain_bridge.cpp index 3b51eb5..d4d343f 100644 --- a/src/domain_bridge/domain_bridge.cpp +++ b/src/domain_bridge/domain_bridge.cpp @@ -44,6 +44,7 @@ #include "domain_bridge/topic_bridge.hpp" #include "domain_bridge/topic_bridge_options.hpp" #include "domain_bridge/msg/compressed_msg.hpp" +#include "domain_bridge/generated_code_interface.hpp" #include "wait_for_graph_events.hpp" @@ -108,6 +109,9 @@ class DomainBridgeImpl using ServiceBridgeMap = std::map< detail::ServiceBridge, std::pair, std::shared_ptr>>; + using ActionBridgeMap = std::map< + detail::ActionBridge, + std::pair, std::shared_ptr>>; using TypesupportMap = std::unordered_map< std::string, std::shared_ptr>; @@ -177,6 +181,14 @@ class DomainBridgeImpl return bridged_services_.find(service_bridge) != bridged_services_.end(); } + bool + is_bridging_action(const detail::ActionBridge & action_bridge) const + { + std::lock_guard guard{data_mutex_}; + return bridged_actions_.find(action_bridge) != bridged_actions_.end(); + } + + void add_service_bridge( const rclcpp::Node::SharedPtr & node, @@ -204,6 +216,32 @@ class DomainBridgeImpl ); } + void + add_action_bridge( + const rclcpp::Node::SharedPtr & node, + detail::ActionBridge action_bridge, + std::function()> create_action, + std::shared_ptr client) { + std::pair it_emplaced_pair; + { + std::lock_guard guard{data_mutex_}; + it_emplaced_pair = bridged_actions_.try_emplace( + std::move(action_bridge), nullptr, client); + } + wait_for_graph_events_.register_on_action_server_ready_callback( + std::move(client), + node, + [ + this, + & action = std::get<0>(it_emplaced_pair.first->second), + create_action = std::move(create_action)]() + { + std::lock_guard guard{data_mutex_}; + action = create_action(); + } + ); + } + /// Load typesupport library into a cache. void load_typesupport_library(std::string type) { @@ -537,6 +575,9 @@ class DomainBridgeImpl /// Set of bridged services ServiceBridgeMap bridged_services_; + /// Set of bridged actions + ActionBridgeMap bridged_actions_; + /// Cache of typesupport libraries TypesupportMap loaded_typesupports_; @@ -575,6 +616,24 @@ add_service_bridge( node, std::move(service_bridge), std::move(create_service), std::move(client)); } +bool +is_bridging_action(const DomainBridgeImpl & impl, const detail::ActionBridge & action_bridge) +{ + return impl.is_bridging_action(action_bridge); +} + +void +add_action_bridge( + DomainBridgeImpl & impl, + const rclcpp::Node::SharedPtr & node, + ActionBridge action_bridge, + std::function()> create_action, + std::shared_ptr client) +{ + return impl.add_action_bridge( + node, std::move(action_bridge), std::move(create_action), std::move(client)); +} + const std::string & get_node_name(const DomainBridgeImpl & impl) { @@ -601,6 +660,30 @@ DomainBridge::DomainBridge(const DomainBridgeConfig & config) bridge_topic(reversed_topic_bridge_pair.first, reversed_topic_bridge_pair.second); } } + for (const auto & service_bridge_pair : config.services) { + if (!domain_bridge::add_service_bridge_by_name( + *this, + service_bridge_pair.first.service_name, + service_bridge_pair.first.type_name, + service_bridge_pair.first.from_domain_id, + service_bridge_pair.first.to_domain_id, + service_bridge_pair.second)) { + std::cout << "Failed to add service bridge for service " << + service_bridge_pair.first.service_name << std::endl; + } + } + for (const auto & action_bridge_pair : config.actions) { + if (!domain_bridge::add_action_bridge_by_name( + *this, + action_bridge_pair.first.action_name, + action_bridge_pair.first.type_name, + action_bridge_pair.first.from_domain_id, + action_bridge_pair.first.to_domain_id, + action_bridge_pair.second)) { + std::cout << "Failed to add action bridge for action " << + action_bridge_pair.first.action_name << std::endl; + } + } } DomainBridge::DomainBridge(DomainBridge && other) = default; diff --git a/src/domain_bridge/parse_domain_bridge_yaml_config.cpp b/src/domain_bridge/parse_domain_bridge_yaml_config.cpp index 9e299b6..c7c8f2a 100644 --- a/src/domain_bridge/parse_domain_bridge_yaml_config.cpp +++ b/src/domain_bridge/parse_domain_bridge_yaml_config.cpp @@ -261,6 +261,86 @@ update_domain_bridge_config_from_yaml( // Add topic bridge to config domain_bridge_config.topics.push_back({{topic, type, from_domain_id, to_domain_id}, options}); } + + for (const auto & service_node : config["services"]) { + // Parse keys for a service bridge + const std::string service = service_node.first.as(); + + auto service_info = service_node.second; + if (service_info.Type() != YAML::NodeType::Map) { + throw YamlParsingError(file_path, "expected map value for each service"); + } + + if (!service_info["type"]) { + throw YamlParsingError(file_path, "missing 'type' for service '" + service + "'"); + } + const std::string type = service_info["type"].as(); + + std::size_t from_domain_id = default_from_domain; + if (service_info["from_domain"]) { + from_domain_id = service_info["from_domain"].as(); + } else { + if (!is_default_from_domain) { + throw YamlParsingError(file_path, "missing 'from_domain' for service '" + service + "'"); + } + } + + std::size_t to_domain_id = default_to_domain; + if (service_info["to_domain"]) { + to_domain_id = service_info["to_domain"].as(); + } else { + if (!is_default_to_domain) { + throw YamlParsingError(file_path, "missing 'to_domain' for service '" + service + "'"); + } + } + + // Parse service bridge options + ServiceBridgeOptions options; + if (service_info["remap"]) { + options.remap_name(service_info["remap"].as()); + } + domain_bridge_config.services.push_back({{service, type, from_domain_id, to_domain_id}, options}); + } + + for (const auto & action_node : config["actions"]) { + // Parse keys for a action bridge + const std::string action = action_node.first.as(); + + auto action_info = action_node.second; + if (action_info.Type() != YAML::NodeType::Map) { + throw YamlParsingError(file_path, "expected map value for each action"); + } + + if (!action_info["type"]) { + throw YamlParsingError(file_path, "missing 'type' for action '" + action + "'"); + } + const std::string type = action_info["type"].as(); + + std::size_t from_domain_id = default_from_domain; + if (action_info["from_domain"]) { + from_domain_id = action_info["from_domain"].as(); + } else { + if (!is_default_from_domain) { + throw YamlParsingError(file_path, "missing 'from_domain' for action '" + action + "'"); + } + } + + std::size_t to_domain_id = default_to_domain; + if (action_info["to_domain"]) { + to_domain_id = action_info["to_domain"].as(); + } else { + if (!is_default_to_domain) { + throw YamlParsingError(file_path, "missing 'to_domain' for action '" + action + "'"); + } + } + + // Parse action bridge options + ActionBridgeOptions options; + if (action_info["remap"]) { + options.remap_name(action_info["remap"].as()); + } + domain_bridge_config.actions.push_back({{action, type, from_domain_id, to_domain_id}, options}); + } } } diff --git a/src/domain_bridge/wait_for_graph_events.hpp b/src/domain_bridge/wait_for_graph_events.hpp index 8fdcb56..ef3f6bf 100644 --- a/src/domain_bridge/wait_for_graph_events.hpp +++ b/src/domain_bridge/wait_for_graph_events.hpp @@ -30,6 +30,7 @@ #include #include +#include "rclcpp_action/rclcpp_action.hpp" #include "rclcpp/client.hpp" #include "rclcpp/node.hpp" #include "rclcpp/qos.hpp" @@ -112,6 +113,34 @@ class WaitForGraphEvents t.thread = this->launch_thread(node, t); } + /// Register a callback that is called when a action server is ready. + /** + * \param client: The client waiting for a matching server. + * \param node: The node to use to monitor the topic. + * \param callback: User callback that is triggered when a matching server is found. + */ + void register_on_action_server_ready_callback( + rclcpp_action::ClientBase::SharedPtr client, + const rclcpp::Node::SharedPtr & node, + std::function callback) + { + std::lock_guard lock(mutex_); + auto it_emplaced_pair = waiting_threads_.try_emplace(node); + auto & t = it_emplaced_pair.first->second; + { + std::lock_guard lock(t.mutex); + t.action_clients_callback_vec.push_back({client, callback}); + } + // If we already have a thread for this node, then notify that there is a new callback + if (!it_emplaced_pair.second) { + t.cv.notify_all(); + return; + } + // If we made it this far, there doesn't exist a thread for waiting so we'll create one + t.thread = this->launch_thread(node, t); + } + + /// Register a callback that is called when QoS is ready for one or more publishers. /** * \param topic: The name of the topic to monitor. @@ -192,6 +221,11 @@ class WaitForGraphEvents rclcpp::ClientBase::SharedPtr client; std::function cb; }; + struct ActionClientAndCallback + { + rclcpp_action::ClientBase::SharedPtr client; + std::function cb; + }; struct ThreadMapValue { std::thread thread; @@ -199,6 +233,7 @@ class WaitForGraphEvents std::mutex mutex; std::vector topics_callback_vec; std::vector clients_callback_vec; + std::vector action_clients_callback_vec; bool shutting_down = false; }; using ThreadMap = std::unordered_map< @@ -335,6 +370,26 @@ class WaitForGraphEvents } } } + { + // TODO(tadachs): SharedPtr for action servers and clients are currently broken, so just + // start them all. For more info see https://github.com/ros2/rclcpp/issues/2630 + auto it = t.action_clients_callback_vec.begin(); + while (it != t.action_clients_callback_vec.end()) { + it->cb(); + it = t.action_clients_callback_vec.erase(it); + } + + // Check if a matching action server was found + // auto it = t.action_clients_callback_vec.begin(); + // while (it != t.action_clients_callback_vec.end()) { + // if (it->client->action_server_is_ready()) { + // it->cb(); + // it = t.action_clients_callback_vec.erase(it); + // } else { + // ++it; + // } + // } + } { // Check if QoS is ready for any of the topics auto it = t.topics_callback_vec.begin();