Improve bridge command parser

.github/workflows/main.yml

@@ -16,4 +16,3 @@ jobs:
           package-name: ros1_bridge
           target-ros1-distro: noetic
           target-ros2-distro: rolling
-

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}

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
 

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 <string>
+#include <vector>
+
+namespace ros1_bridge
+{
+
+bool find_command_option(
+    const std::vector<const char *> & args,
+    const std::string & option);
+
+bool get_option_value(
+    std::vector<const char *> & args,
+    const std::string & option,
+    const char * & value,
+    bool remove = false);
+
+bool get_option_values(
+  std::vector<const char *> & args, const std::string & option,
+  std::vector<const char *> & available_options,
+  std::vector<const char *> & values, bool remove = false);
+
+bool get_option_flag(
+    std::vector<const char *> & args,
+    const std::string & option,
+    bool remove = false);
+
+void split_ros1_ros2_args(
+  const std::vector<const char *> & args,
+  std::vector<const char *> & ros1_args,
+  std::vector<const char *> & ros2_args);
+
+}  // namespace ros1_bridge
+
+#endif  // ROS1_BRIDGE__COMMAND_PARSER_UTILS_HPP_

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 <algorithm>
+#include <cstring>
+
+namespace ros1_bridge
+{
+
+bool find_command_option(const std::vector<const char *> & 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<const char *> & 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<const char *> & args, const std::string & option,
+  std::vector<const char *> & available_options,
+  std::vector<const char *> & 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<const char *> & 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<const char *> & args, std::vector<const char *> & ros1_args,
+  std::vector<const char *> & 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<const char *>(args.begin(), it);
+    ros2_args = std::vector<const char *>(it, args.end());
+  } else {
+    ros1_args = args;
+    ros2_args = {};
+  }
+
+  ros2_args.insert(ros2_args.begin(), args.at(0));
+}
+
+}  // namespace ros1_bridge

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<std::string> & args, const std::string & option)
-{
-  return std::find(args.begin(), args.end(), option) != args.end();
-}
-
-bool get_flag_option(const std::vector<std::string> & 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<const char *> & ros1_args,
+  std::vector<const char *> & ros2_args, bool & output_topic_introspection,
   bool & bridge_all_1to2_topics, bool & bridge_all_2to1_topics)
 {
-  std::vector<std::string> args(argv, argv + argc);
-
-  if (find_command_option(args, "-h") || find_command_option(args, "--help")) {
+  std::vector<const char *> args(argv, argv + argc);
+
+  std::vector<const char *> 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);
 
-  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");
+  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;
 }
@@ -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<const char *> ros1_args;
+  std::vector<const char *> 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<char **>(ros1_args.data()), "ros_bridge");
   ros::NodeHandle ros1_node;
 
   // mapping of available topic names to type names