From af6d57f2f40607f00b68760a9e8cfee708498be5 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Tue, 17 Dec 2024 23:52:24 +0100 Subject: [PATCH] Forward --ros-args to BridgeROS2 --- mola_bridge_ros2/src/BridgeROS2.cpp | 22 ++++++++---- mola_launcher/apps/mola-cli.cpp | 53 +++++++++++++++++++++++------ 2 files changed, 59 insertions(+), 16 deletions(-) diff --git a/mola_bridge_ros2/src/BridgeROS2.cpp b/mola_bridge_ros2/src/BridgeROS2.cpp index 0ab193e4..1fde668b 100644 --- a/mola_bridge_ros2/src/BridgeROS2.cpp +++ b/mola_bridge_ros2/src/BridgeROS2.cpp @@ -77,10 +77,22 @@ void BridgeROS2::ros_node_thread_main(Yaml cfg) try { - const int argc = 1; - char const* const argv[2] = {NODE_NAME, nullptr}; + // build argc/argv for ROS: + std::vector rosArgs = {NODE_NAME}; + if (cfg.has("ros_args")) + { + ASSERT_(cfg["ros_args"].isSequence()); + for (const auto& p : cfg["ros_args"].asSequenceRange()) + rosArgs.push_back(p.as()); + } + + // Convert to good old C (argc,argv): + std::vector rosArgsC; + for (const auto& s : rosArgs) rosArgsC.push_back(s.c_str()); + + const int argc = static_cast(rosArgs.size()); + const char** argv = rosArgsC.data(); - // Initialize ROS: // Initialize ROS (only once): if (!rclcpp::ok()) { rclcpp::init(argc, argv); } @@ -110,9 +122,7 @@ void BridgeROS2::ros_node_thread_main(Yaml cfg) auto ds_subscribe = cfg["subscribe"]; if (!ds_subscribe.isSequence() || ds_subscribe.asSequence().empty()) { - MRPT_LOG_INFO( - "No ROS2 topic found for subscription under YAML entry " - "`subscribe`."); + MRPT_LOG_INFO("No ROS2 topic found for subscription under YAML entry `subscribe`."); } else { internalAnalyzeTopicsToSubscribe(ds_subscribe); } diff --git a/mola_launcher/apps/mola-cli.cpp b/mola_launcher/apps/mola-cli.cpp index c3ab38ea..9b4c6807 100644 --- a/mola_launcher/apps/mola-cli.cpp +++ b/mola_launcher/apps/mola-cli.cpp @@ -92,12 +92,6 @@ struct Cli "Paths " "can be added with the environment variable MOLA_MODULES_SHARED_PATH.", cmd}; - - TCLAP::SwitchArg arg_ros_args{ - "", "ros-args", - "Dummy flag, defined just to allow the program invocation from ROS 2 " - "launch files.", - cmd}; }; namespace @@ -124,8 +118,11 @@ void mola_install_signal_handler() // Default task for mola-cli: launching a SLAM system // ----------------------------------------------------- -int mola_cli_launch_slam(Cli& cli) +int mola_cli_launch_slam( + Cli& cli, const std::optional>& rosArgs) { + using namespace std::string_literals; + // Load YAML config file: if (!cli.arg_yaml_cfg.isSet()) { @@ -136,7 +133,19 @@ int mola_cli_launch_slam(Cli& cli) } const auto file_yml = cli.arg_yaml_cfg.getValue(); - auto cfg = mola::load_yaml_file(file_yml); + // replace a special variable for ROS args: + mola::YAMLParseOptions po; + + if (rosArgs) + { + std::string strRosArgs = "["; + for (const auto& s : *rosArgs) strRosArgs += " '"s + s + "', "s; + strRosArgs += "]"; + po.variables["ROS_ARGS"] = strRosArgs; + } + + // Load YAML from file: + auto cfg = mola::load_yaml_file(file_yml, po); mola::MolaLauncherApp app; theApp = &app; // for the signal handler @@ -255,8 +264,32 @@ int main(int argc, char** argv) { Cli cli; + // Handle special ROS arguments (if mola-cli is launched as a ROS node) + // before handling (argc,argv) to tclap: + std::optional> rosArgs; + std::vector otherArgs; + for (int i = 0; i < argc; i++) + { + const auto sArg = std::string(argv[i]); + if (sArg == "--ros-args" && !rosArgs) + { + rosArgs.emplace(); + continue; + } + if (rosArgs) + rosArgs->push_back(sArg); + else + otherArgs.push_back(sArg); + } + // handle the case "--ros-args\n" + if (rosArgs && rosArgs->empty()) rosArgs.reset(); + + std::vector argvBis; + for (const auto& s : otherArgs) argvBis.push_back(s.c_str()); + const int argcBis = static_cast(argvBis.size()); + // Parse arguments: - if (!cli.cmd.parse(argc, argv)) return 1; // should exit. + if (!cli.cmd.parse(argcBis, argvBis.data())) return 1; // should exit. mola_install_signal_handler(); @@ -274,7 +307,7 @@ int main(int argc, char** argv) return mola_cli_list_module_shared_dirs(); // Default task: - return mola_cli_launch_slam(cli); + return mola_cli_launch_slam(cli, rosArgs); return 0; }