diff --git a/zenoh-bridge-ros2dds/src/bridge_args.rs b/zenoh-bridge-ros2dds/src/bridge_args.rs index 6381b4d..b7603d9 100644 --- a/zenoh-bridge-ros2dds/src/bridge_args.rs +++ b/zenoh-bridge-ros2dds/src/bridge_args.rs @@ -74,6 +74,16 @@ pub struct BridgeArgs { /// reports as error log any stalled status during the specified period [default: 1.0 second] #[arg(short, long, value_name = "FLOAT", default_missing_value = "1.0")] pub watchdog: Option>, + + /// ROS command line arguments as specified in https://design.ros2.org/articles/ros_command_line_arguments.html + /// Supported capabilities: + /// -r, --remap : remapping is supported only for '__ns' and '__node' + #[arg( + long, + value_name = " list of ROS args until '--' ", + verbatim_doc_comment + )] + pub ros_args: (), } impl From for Config { @@ -119,7 +129,7 @@ impl From<&BridgeArgs> for Config { } } -fn insert_json5(config: &mut Config, key: &str, value: &T) +pub(crate) fn insert_json5(config: &mut Config, key: &str, value: &T) where T: Sized + serde::Serialize, { @@ -128,7 +138,7 @@ where .unwrap(); } -fn insert_json5_option(config: &mut Config, key: &str, value: &Option) +pub(crate) fn insert_json5_option(config: &mut Config, key: &str, value: &Option) where T: Sized + serde::Serialize, { @@ -139,7 +149,7 @@ where } } -fn insert_json5_list(config: &mut Config, key: &str, values: &Vec) +pub(crate) fn insert_json5_list(config: &mut Config, key: &str, values: &Vec) where T: Sized + serde::Serialize, { diff --git a/zenoh-bridge-ros2dds/src/main.rs b/zenoh-bridge-ros2dds/src/main.rs index a53a0ce..4cb2e32 100644 --- a/zenoh-bridge-ros2dds/src/main.rs +++ b/zenoh-bridge-ros2dds/src/main.rs @@ -14,15 +14,42 @@ use async_liveliness_monitor::LivelinessMonitor; use bridge_args::BridgeArgs; use clap::Parser; +use ros_args::RosArgs; use std::time::{Duration, SystemTime}; use zenoh::config::Config; mod bridge_args; +mod ros_args; mod zenoh_args; +const ROS_ARG_START_FLAG: &str = "--ros-args"; +const ROS_ARG_END_FLAG: &str = "--"; + fn parse_args() -> (Option, Config) { - let args = BridgeArgs::parse(); - (args.watchdog.flatten(), args.into()) + // Split arguments between "ROS-defined" ones and the "user-defined" ones + // (as per https://design.ros2.org/articles/ros_command_line_arguments.html) + let mut ros_args = vec!["ros-args".to_string()]; + let mut user_args = Vec::new(); + let mut in_ros_args_section = false; + for arg in std::env::args() { + match arg.as_str() { + ROS_ARG_START_FLAG => in_ros_args_section = true, + ROS_ARG_END_FLAG => in_ros_args_section = false, + _ if in_ros_args_section => ros_args.push(arg), + _ => user_args.push(arg), + } + } + + // Create config parsing user-defined args + let bridge_args = BridgeArgs::parse_from(user_args); + let watchdog_opt = bridge_args.watchdog.flatten(); + let mut config = bridge_args.into(); + + // Amend config with "ROS-define" args + let ros_args = RosArgs::parse_from(ros_args); + ros_args.update_config(&mut config); + + (watchdog_opt, config) } #[async_std::main] diff --git a/zenoh-bridge-ros2dds/src/ros_args.rs b/zenoh-bridge-ros2dds/src/ros_args.rs new file mode 100644 index 0000000..33d7d87 --- /dev/null +++ b/zenoh-bridge-ros2dds/src/ros_args.rs @@ -0,0 +1,80 @@ +// +// Copyright (c) 2022 ZettaScale Technology +// +// This program and the accompanying materials are made available under the +// terms of the Eclipse Public License 2.0 which is available at +// http://www.eclipse.org/legal/epl-2.0, or the Apache License, Version 2.0 +// which is available at https://www.apache.org/licenses/LICENSE-2.0. +// +// SPDX-License-Identifier: EPL-2.0 OR Apache-2.0 +// +// Contributors: +// ZettaScale Zenoh Team, +// + +use zenoh::config::Config; + +use crate::bridge_args::insert_json5; + +#[derive(clap::Parser, Clone, PartialEq, Eq, Hash, Debug)] +pub struct RosArgs { + /// Name remapping + #[arg(short, long, value_name = "FROM:=TO", value_parser = parse_remap)] + pub remap: Vec, +} + +#[derive(Clone, PartialEq, Eq, Hash, Debug)] +enum RemapFrom { + Namespace, + Node, + _Name(String), +} + +#[derive(Clone, PartialEq, Eq, Hash, Debug)] +pub struct Remap { + from: RemapFrom, + to: String, +} + +fn parse_remap(s: &str) -> Result { + match s.split_once(":=") { + Some(("__ns", to)) if !to.is_empty() => Ok(Remap { + from: RemapFrom::Namespace, + to: to.into(), + }), + Some(("__node", to)) if !to.is_empty() => Ok(Remap { + from: RemapFrom::Node, + to: to.into(), + }), + Some((from, to)) if !from.is_empty() && !to.is_empty() => { + Err("only remapping for '__ns' and '__node' are currently supported".into()) + } + _ => Err("valid value must have format 'fromp:=to'".into()), + } +} + +impl RosArgs { + pub fn update_config(&self, config: &mut Config) { + for r in &self.remap { + match r.from { + RemapFrom::Namespace => { + log::info!( + "Remapping namespace to '{}' as per ROS command line argument", + r.to + ); + insert_json5(config, "plugins/ros2dds/namespace", &r.to); + } + RemapFrom::Node => { + log::info!( + "Remapping node name to '{}' as per ROS command line argument", + r.to + ); + insert_json5(config, "plugins/ros2dds/nodename", &r.to); + } + RemapFrom::_Name(_) => { + panic!("Unsupported remapping... only '__node' and '__ns' are supported") + } + } + } + } +}