diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index e6aed2ef..ab683cba 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -16,4 +16,3 @@ jobs: package-name: ros1_bridge target-ros1-distro: noetic target-ros2-distro: rolling - diff --git a/CMakeLists.txt b/CMakeLists.txt index 5f878af4..0f7855a4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -221,6 +221,7 @@ add_library(${PROJECT_NAME} SHARED "src/builtin_interfaces_factories.cpp" "src/convert_builtin_interfaces.cpp" "src/bridge.cpp" + "src/command_parser_utils.cpp" ${generated_files}) ament_target_dependencies(${PROJECT_NAME} ${prefixed_ros1_message_packages} diff --git a/README.md b/README.md index 59d9509f..99fc110b 100644 --- a/README.md +++ b/README.md @@ -46,7 +46,7 @@ To run the following examples you will also need these ROS 1 packages: ### Prerequisites for the examples in this file -In order to make the examples below portable between versions of ROS, we define two environment variables, `ROS1_INSTALL_PATH` and `ROS2_INSTALL_PATH`. +In order to make the examples below portable between versions of ROS, we define two environment variables, `ROS1_INSTALL_PATH` and `ROS2_INSTALL_PATH`. These are defined as the paths to the installation location of their respective ROS versions. If you installed Noetic in the default location, then the definition of `ROS1_INSTALL_PATH` will be `/opt/ros/noetic`. @@ -125,6 +125,31 @@ colcon build --symlink-install --packages-select ros1_bridge --cmake-force-confi *Note:* If you are building on a memory constrained system you might want to limit the number of parallel jobs by setting e.g. the environment variable `MAKEFLAGS=-j1`. +## Running the bridge + +There are two types of bridges that are available for general use: + +* The dynamic bridge which will watch the available ROS 1 and ROS 2 topics. + Once a *matching* topic has been detected it starts to bridge the messages on this topic. +* The parameter bridge which will just bridge specific topics with specific QoS profiles depending on a configuration provided by the user. For more details see the [Example 4](#example-4-bridge-only-selected-topics-and-services) section. + +Too see the available command line options for each type of bridge run: + +```bash +ros2 run ros1_bridge [dynamic_bridge|parameter_bridge] --help +``` + +To pass arguments to the ROS 1 node of the bridge, use the `--ros1-args` option. For example, to run the dynamic bridge changing the ROS 1 node namespace to `ros1_bridge`: + +```bash +ros2 run ros1_bridge dynamic_bridge --ros1-args __ns:=ros1_bridge +``` + +To pass arguments to the ROS 2 node of the bridge, use the `--ros2-args` option. For example, to run the dynamic bridge changing the ROS 2 node namespace to `ros2_bridge`: + +```bash +ros2 run ros1_bridge dynamic_bridge --ros2-args __ns:=ros2_bridge +``` ## Example 1: run the bridge and the example talker and listener diff --git a/include/ros1_bridge/command_parser_utils.hpp b/include/ros1_bridge/command_parser_utils.hpp new file mode 100644 index 00000000..92f0c2d3 --- /dev/null +++ b/include/ros1_bridge/command_parser_utils.hpp @@ -0,0 +1,51 @@ +// 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. + +#ifndef ROS1_BRIDGE__COMMAND_PARSER_UTILS_HPP_ +#define ROS1_BRIDGE__COMMAND_PARSER_UTILS_HPP_ + +#include +#include + +namespace ros1_bridge +{ + +bool find_command_option( + const std::vector & args, + const std::string & option); + +bool get_option_value( + std::vector & args, + const std::string & option, + const char * & value, + bool remove = false); + +bool get_option_values( + std::vector & args, const std::string & option, + std::vector & available_options, + std::vector & values, bool remove = false); + +bool get_option_flag( + std::vector & args, + const std::string & option, + bool remove = false); + +void split_ros1_ros2_args( + const std::vector & args, + std::vector & ros1_args, + std::vector & ros2_args); + +} // namespace ros1_bridge + +#endif // ROS1_BRIDGE__COMMAND_PARSER_UTILS_HPP_ diff --git a/src/command_parser_utils.cpp b/src/command_parser_utils.cpp new file mode 100644 index 00000000..d3fe0d96 --- /dev/null +++ b/src/command_parser_utils.cpp @@ -0,0 +1,128 @@ +// 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 "ros1_bridge/command_parser_utils.hpp" + +#include +#include + +namespace ros1_bridge +{ + +bool find_command_option(const std::vector & args, const std::string & option) +{ + auto it = std::find_if(args.begin(), args.end(), [&option] (const char * const & element) { + return strcmp(element, option.c_str()) == 0; + }); + + return it != args.end(); +} + +bool get_option_value(std::vector & args, const std::string & option, const char * & value, bool remove) +{ + auto it = std::find_if(args.begin(), args.end(), [&option] (const char * const & element) { + return strcmp(element, option.c_str()) == 0; + }); + + if (it != args.end()) { + auto value_it = std::next(it); + + if (value_it != args.end()) { + value = *value_it; + + if (remove) { + args.erase(it); // Remove option + args.erase(it); // Remove value + } + + return true; + } + } + + return false; +} + +bool get_option_values( + std::vector & args, const std::string & option, + std::vector & available_options, + std::vector & values, bool remove) +{ + auto it = std::find_if(args.begin(), args.end(), [&option] (const char * const & element) { + return strcmp(element, option.c_str()) == 0; + }); + + if (it != args.end()) { + auto value_it = std::next(it); + + while (value_it != args.end() and + std::none_of(available_options.begin(), available_options.end(), + [value_it](const char * available_option) { + return strcmp(*value_it, available_option) == 0; + })) { + values.push_back(*value_it); + + if (remove) { + args.erase(value_it); + } else { + ++value_it; + } + } + + if (remove) { + args.erase(it); // Remove option + } + + return true; + } + + return false; +} + +bool get_option_flag(std::vector & args, const std::string & option, bool remove) +{ + auto it = std::find_if(args.begin(), args.end(), [&option] (const char * const & element) { + return strcmp(element, option.c_str()) == 0; + }); + + if (it != args.end()) { + if (remove) { + args.erase(it); + } + return true; + } + + return false; +} + +void split_ros1_ros2_args( + const std::vector & args, std::vector & ros1_args, + std::vector & ros2_args) +{ + // Start iterating from the second argument, since the first argument is the executable name + auto it = std::find_if(args.begin() + 1, args.end(), [] (const char * const & element) { + return strcmp(element, "--ros-args") == 0; + }); + + if (it != args.end()) { + ros1_args = std::vector(args.begin(), it); + ros2_args = std::vector(it, args.end()); + } else { + ros1_args = args; + ros2_args = {}; + } + + ros2_args.insert(ros2_args.begin(), args.at(0)); +} + +} // namespace ros1_bridge diff --git a/src/dynamic_bridge.cpp b/src/dynamic_bridge.cpp index 1e1a5522..b87d1bac 100644 --- a/src/dynamic_bridge.cpp +++ b/src/dynamic_bridge.cpp @@ -39,6 +39,7 @@ #include "rcpputils/scope_exit.hpp" #include "ros1_bridge/bridge.hpp" +#include "ros1_bridge/command_parser_utils.hpp" std::mutex g_bridge_mutex; @@ -57,26 +58,31 @@ struct Bridge2to1HandlesAndMessageTypes std::string ros2_type_name; }; -bool find_command_option(const std::vector & args, const std::string & option) -{ - return std::find(args.begin(), args.end(), option) != args.end(); -} - -bool get_flag_option(const std::vector & args, const std::string & option) -{ - auto it = std::find(args.begin(), args.end(), option); - return it != args.end(); -} - bool parse_command_options( - int argc, char ** argv, bool & output_topic_introspection, + int argc, char ** argv, std::vector & ros1_args, + std::vector & ros2_args, bool & output_topic_introspection, bool & bridge_all_1to2_topics, bool & bridge_all_2to1_topics) { - std::vector args(argv, argv + argc); - - if (find_command_option(args, "-h") || find_command_option(args, "--help")) { + std::vector args(argv, argv + argc); + + std::vector available_options = { + "-h", "--help", + "--show-introspection", + "--print-pairs", + "--bridge-all-topics", + "--bridge-all-1to2-topics", + "--bridge-all-2to1-topics", + "--ros1-args", + "--ros2-args", + }; + + if (ros1_bridge::find_command_option(args, "-h") || ros1_bridge::find_command_option(args, "--help")) { std::stringstream ss; ss << "Usage:" << std::endl; + ss << "ros2 run ros1_bridge dynamic_bridge [Bridge specific options] \\" << std::endl; + ss << " [--ros1-args [ROS1 arguments]] [--ros2-args [ROS2 arguments]]" << std::endl; + ss << std::endl; + ss << "Options:" << std::endl; ss << " -h, --help: This message." << std::endl; ss << " --show-introspection: Print output of introspection of both sides of the bridge."; ss << std::endl; @@ -88,11 +94,13 @@ bool parse_command_options( ss << "a matching subscriber." << std::endl; ss << " --bridge-all-2to1-topics: Bridge all ROS 2 topics to ROS 1, whether or not there is "; ss << "a matching subscriber." << std::endl; + ss << " --ros1-args: Arguments to pass to the ROS 1 bridge node." << std::endl; + ss << " --ros2-args: Arguments to pass to the ROS 2 bridge node." << std::endl; std::cout << ss.str(); return false; } - if (get_flag_option(args, "--print-pairs")) { + if (ros1_bridge::get_option_flag(args, "--print-pairs")) { auto mappings_2to1 = ros1_bridge::get_all_message_mappings_2to1(); if (mappings_2to1.size() > 0) { printf("Supported ROS 2 <=> ROS 1 message type conversion pairs:\n"); @@ -114,11 +122,41 @@ bool parse_command_options( return false; } - output_topic_introspection = get_flag_option(args, "--show-introspection"); + output_topic_introspection = ros1_bridge::get_option_flag(args, "--show-introspection", true); + + bool bridge_all_topics = ros1_bridge::get_option_flag(args, "--bridge-all-topics", true); + bridge_all_1to2_topics = bridge_all_topics || ros1_bridge::get_option_flag(args, "--bridge-all-1to2-topics", true); + bridge_all_2to1_topics = bridge_all_topics || ros1_bridge::get_option_flag(args, "--bridge-all-2to1-topics", true); + + auto logger = rclcpp::get_logger("ros1_bridge"); + + // Get ROS1 arguments + if (ros1_bridge::get_option_values(args, "--ros1-args", available_options, ros1_args, true)) { + if (ros1_args.size() == 0) { + RCLCPP_ERROR(logger, "Error: --ros1-args specified but no arguments provided."); + return false; + } + } - bool bridge_all_topics = get_flag_option(args, "--bridge-all-topics"); - bridge_all_1to2_topics = bridge_all_topics || get_flag_option(args, "--bridge-all-1to2-topics"); - bridge_all_2to1_topics = bridge_all_topics || get_flag_option(args, "--bridge-all-2to1-topics"); + ros1_args.insert(ros1_args.begin(), args.at(0)); + + // Get ROS2 arguments + if (ros1_bridge::get_option_values(args, "--ros2-args", available_options, ros2_args, true)) { + if (ros2_args.size() == 0) { + RCLCPP_ERROR(logger, "Error: --ros2-args specified but no arguments provided."); + return false; + } + + ros2_args.insert(ros2_args.begin(), "--ros-args"); + } + + ros2_args.insert(ros2_args.begin(), args.at(0)); + + if (ros1_bridge::find_command_option(args, "--ros-args") or args.size() > 1) { + RCLCPP_WARN(logger, "Warning: passing the ROS node arguments directly to the node is deprecated, use --ros1-args and --ros2-args instead."); + + ros1_bridge::split_ros1_ros2_args(args, ros1_args, ros2_args); + } return true; } @@ -192,14 +230,14 @@ void update_bridge( bridge.ros1_type_name, topic_name, 10, bridge.ros2_type_name, topic_name, ros2_publisher_qos); } catch (std::runtime_error & e) { - fprintf( - stderr, - "failed to create 1to2 bridge for topic '%s' " - "with ROS 1 type '%s' and ROS 2 type '%s': %s\n", - topic_name.c_str(), bridge.ros1_type_name.c_str(), bridge.ros2_type_name.c_str(), e.what()); - if (std::string(e.what()).find("No template specialization") != std::string::npos) { - fprintf(stderr, "check the list of supported pairs with the `--print-pairs` option\n"); - } + // fprintf( + // stderr, + // "failed to create 1to2 bridge for topic '%s' " + // "with ROS 1 type '%s' and ROS 2 type '%s': %s\n", + // topic_name.c_str(), bridge.ros1_type_name.c_str(), bridge.ros2_type_name.c_str(), e.what()); + // if (std::string(e.what()).find("No template specialization") != std::string::npos) { + // fprintf(stderr, "check the list of supported pairs with the `--print-pairs` option\n"); + // } continue; } @@ -258,14 +296,14 @@ void update_bridge( bridge.ros2_type_name, topic_name, 10, bridge.ros1_type_name, topic_name, 10); } catch (std::runtime_error & e) { - fprintf( - stderr, - "failed to create 2to1 bridge for topic '%s' " - "with ROS 2 type '%s' and ROS 1 type '%s': %s\n", - topic_name.c_str(), bridge.ros2_type_name.c_str(), bridge.ros1_type_name.c_str(), e.what()); - if (std::string(e.what()).find("No template specialization") != std::string::npos) { - fprintf(stderr, "check the list of supported pairs with the `--print-pairs` option\n"); - } + // fprintf( + // stderr, + // "failed to create 2to1 bridge for topic '%s' " + // "with ROS 2 type '%s' and ROS 1 type '%s': %s\n", + // topic_name.c_str(), bridge.ros2_type_name.c_str(), bridge.ros1_type_name.c_str(), e.what()); + // if (std::string(e.what()).find("No template specialization") != std::string::npos) { + // fprintf(stderr, "check the list of supported pairs with the `--print-pairs` option\n"); + // } continue; } @@ -462,19 +500,25 @@ int main(int argc, char * argv[]) bool output_topic_introspection; bool bridge_all_1to2_topics; bool bridge_all_2to1_topics; + + std::vector ros1_args; + std::vector ros2_args; + if (!parse_command_options( - argc, argv, output_topic_introspection, bridge_all_1to2_topics, bridge_all_2to1_topics)) + argc, argv, ros1_args, ros2_args, output_topic_introspection, + bridge_all_1to2_topics, bridge_all_2to1_topics)) { return 0; } // ROS 2 node - rclcpp::init(argc, argv); + rclcpp::init(ros2_args.size(), ros2_args.data()); auto ros2_node = rclcpp::Node::make_shared("ros_bridge"); // ROS 1 node - ros::init(argc, argv, "ros_bridge"); + int argc_ros1 = ros1_args.size(); + ros::init(argc_ros1, const_cast(ros1_args.data()), "ros_bridge"); ros::NodeHandle ros1_node; // mapping of available topic names to type names diff --git a/src/parameter_bridge.cpp b/src/parameter_bridge.cpp index a95d49a4..22626a8f 100644 --- a/src/parameter_bridge.cpp +++ b/src/parameter_bridge.cpp @@ -31,6 +31,7 @@ #include "rclcpp/rclcpp.hpp" #include "ros1_bridge/bridge.hpp" +#include "ros1_bridge/command_parser_utils.hpp" rclcpp::QoS qos_from_params(XmlRpc::XmlRpcValue qos_params) { @@ -229,19 +230,119 @@ rclcpp::QoS qos_from_params(XmlRpc::XmlRpcValue qos_params) return ros2_publisher_qos; } -int main(int argc, char * argv[]) +bool parse_command_options( + int argc, char ** argv, std::vector & ros1_args, + std::vector & ros2_args, const char * & topics_parameter_name, + const char * & services_1_to_2_parameter_name, const char * & services_2_to_1_parameter_name) { - // ROS 1 node - ros::init(argc, argv, "ros_bridge"); - ros::NodeHandle ros1_node; + topics_parameter_name = "topics"; + services_1_to_2_parameter_name = "services_1_to_2"; + services_2_to_1_parameter_name = "services_2_to_1"; + + std::vector args(argv, argv + argc); + + std::vector available_options = { + "-h", "--help", + "--topics", + "--services-1-to-2", + "--services-2-to-1", + "--ros1-args", + "--ros2-args", + }; + + if (ros1_bridge::find_command_option(args, "-h") || ros1_bridge::find_command_option(args, "--help")) { + std::stringstream ss; + ss << "Usage:" << std::endl; + ss << "ros2 run ros1_bridge parameter_bridge [Bridge specific options] \\" << std::endl; + ss << " [--ros1-args [ROS1 arguments]] [--ros2-args [ROS2 arguments]]" << std::endl; + ss << std::endl; + ss << "Options:" << std::endl; + ss << " -h, --help: This message." << std::endl; + ss << " --print-pairs: Print a list of the supported ROS 2 <=> ROS 1 conversion pairs."; + ss << std::endl; + ss << " --topics: Name of the parameter that contains the list of topics to bridge."; + ss << std::endl; + ss << " --services-1-to-2: Name of the parameter that contains the list of services to bridge from ROS 1 to ROS 2."; + ss << std::endl; + ss << " --services-2-to-1: Name of the parameter that contains the list of services to bridge from ROS 2 to ROS 1."; + ss << std::endl; + ss << " --ros1-args: Arguments to pass to the ROS 1 bridge node." << std::endl; + ss << " --ros2-args: Arguments to pass to the ROS 2 bridge node." << std::endl; + std::cout << ss.str(); + return false; + } - // ROS 2 node - rclcpp::init(argc, argv); - auto ros2_node = rclcpp::Node::make_shared("ros_bridge"); + if (ros1_bridge::get_option_flag(args, "--print-pairs")) { + auto mappings_2to1 = ros1_bridge::get_all_message_mappings_2to1(); + if (mappings_2to1.size() > 0) { + printf("Supported ROS 2 <=> ROS 1 message 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 message type conversion pairs supported.\n"); + } + mappings_2to1 = ros1_bridge::get_all_service_mappings_2to1(); + if (mappings_2to1.size() > 0) { + printf("Supported ROS 2 <=> ROS 1 service 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 service type conversion pairs supported.\n"); + } + return false; + } - std::list all_handles; - std::list service_bridges_1_to_2; - std::list service_bridges_2_to_1; + if (!ros1_bridge::get_option_value(args, "--topics", topics_parameter_name, true)) { + printf("Using default topics parameter name: %s\n", topics_parameter_name); + } + + if (!ros1_bridge::get_option_value(args, "--services-1-to-2", services_1_to_2_parameter_name, true)) { + printf("Using default services 1 to 2 parameter name: %s\n", services_1_to_2_parameter_name); + } + + if (!ros1_bridge::get_option_value(args, "--services-2-to-1", services_2_to_1_parameter_name, true)) { + printf("Using default services 2 to 1 parameter name: %s\n", services_2_to_1_parameter_name); + } + + auto logger = rclcpp::get_logger("ros1_bridge"); + + // Get ROS1 arguments + if (ros1_bridge::get_option_values(args, "--ros1-args", available_options, ros1_args, true)) { + if (ros1_args.size() == 0) { + RCLCPP_ERROR(logger, "Error: --ros1-args specified but no arguments provided."); + return false; + } + } + + ros1_args.insert(ros1_args.begin(), args.at(0)); + + // Get ROS2 arguments + if (ros1_bridge::get_option_values(args, "--ros2-args", available_options, ros2_args, true)) { + if (ros2_args.size() == 0) { + RCLCPP_ERROR(logger, "Error: --ros2-args specified but no arguments provided."); + return false; + } + + ros2_args.insert(ros2_args.begin(), "--ros-args"); + } + + ros2_args.insert(ros2_args.begin(), args.at(0)); + + if (ros1_bridge::find_command_option(args, "--ros-args") or args.size() > 1) { + RCLCPP_WARN(logger, "Warning: passing the ROS node arguments directly to the node is deprecated, use --ros1-args and --ros2-args instead."); + + ros1_bridge::split_ros1_ros2_args(args, ros1_args, ros2_args); + } + + return true; +} + +int main(int argc, char * argv[]) +{ + std::vector ros1_args; + std::vector ros2_args; // bridge all topics listed in a ROS 1 parameter // the topics parameter needs to be an array @@ -249,25 +350,35 @@ int main(int argc, char * argv[]) // topic: the name of the topic to bridge (e.g. '/topic_name') // type: the type of the topic to bridge (e.g. 'pkgname/msg/MsgName') // queue_size: the queue size to use (default: 100) - const char * topics_parameter_name = "topics"; + const char * topics_parameter_name; // the services parameters need to be arrays // and each item needs to be a dictionary with the following keys; // service: the name of the service to bridge (e.g. '/service_name') // type: the type of the service to bridge (e.g. 'pkgname/srv/SrvName') - const char * services_1_to_2_parameter_name = "services_1_to_2"; - const char * services_2_to_1_parameter_name = "services_2_to_1"; + const char * services_1_to_2_parameter_name; + const char * services_2_to_1_parameter_name; const char * service_execution_timeout_parameter_name = "ros1_bridge/parameter_bridge/service_execution_timeout"; - if (argc > 1) { - topics_parameter_name = argv[1]; - } - if (argc > 2) { - services_1_to_2_parameter_name = argv[2]; - } - if (argc > 3) { - services_2_to_1_parameter_name = argv[3]; + + if (!parse_command_options( + argc, argv, ros1_args, ros2_args, topics_parameter_name, + services_1_to_2_parameter_name, services_2_to_1_parameter_name)) { + return 0; } + // ROS 2 node + rclcpp::init(ros2_args.size(), ros2_args.data()); + auto ros2_node = rclcpp::Node::make_shared("ros_bridge"); + + // ROS 1 node + int argc_ros1 = ros1_args.size(); + ros::init(argc_ros1, const_cast(ros1_args.data()), "ros_bridge"); + ros::NodeHandle ros1_node; + + std::list all_handles; + std::list service_bridges_1_to_2; + std::list service_bridges_2_to_1; + // Topics XmlRpc::XmlRpcValue topics; if (