From f70f35f16544926fd7aa64279280cde3728052e6 Mon Sep 17 00:00:00 2001 From: Xiyu Oh Date: Fri, 10 Jul 2026 00:20:39 +0800 Subject: [PATCH 1/3] Discover agents via nav2 nodes Signed-off-by: Xiyu Oh --- .../rmf_nav2_traffic/src/discovery.rs | 50 +++++++++++++++++++ nav2_integration/rmf_nav2_traffic/src/lib.rs | 10 ++-- 2 files changed, 54 insertions(+), 6 deletions(-) create mode 100644 nav2_integration/rmf_nav2_traffic/src/discovery.rs diff --git a/nav2_integration/rmf_nav2_traffic/src/discovery.rs b/nav2_integration/rmf_nav2_traffic/src/discovery.rs new file mode 100644 index 0000000..e373ad2 --- /dev/null +++ b/nav2_integration/rmf_nav2_traffic/src/discovery.rs @@ -0,0 +1,50 @@ +use crate::Nav2Agent; +use bevy::prelude::*; +use bevy_ros2::RclrsNode; +use std::collections::HashSet; + +#[derive(Resource, Default)] +pub struct AgentTracker { + pub known_agents: HashSet, +} + +#[derive(Default)] +pub struct AgentDiscoveryPlugin {} + +impl Plugin for AgentDiscoveryPlugin { + fn build(&self, app: &mut App) { + app.init_resource::() + .add_systems(PreUpdate, discover_agents_via_graph); + } +} + +// Discover agents via existing Nav2 nodes +fn discover_agents_via_graph( + mut commands: Commands, + node: Res, + mut tracker: ResMut, +) { + let Ok(node_names) = node.get_node_names() else { + return; + }; + + for info in node_names { + if info.name == "bt_navigator" { + // Remove /inner suffix + let namespace = info.namespace.trim_start_matches('/'); + let namespace = namespace.strip_suffix("/inner").unwrap_or(namespace); + + if namespace.is_empty() { + continue; + } + + let robot_name = namespace.to_string(); + + if !tracker.known_agents.contains(&robot_name) { + info!("Discovered new Nav2 agent from ROS graph: {}", robot_name); + tracker.known_agents.insert(robot_name.clone()); + commands.spawn(Nav2Agent::new(robot_name)); + } + } + } +} diff --git a/nav2_integration/rmf_nav2_traffic/src/lib.rs b/nav2_integration/rmf_nav2_traffic/src/lib.rs index 469761c..2d673c3 100644 --- a/nav2_integration/rmf_nav2_traffic/src/lib.rs +++ b/nav2_integration/rmf_nav2_traffic/src/lib.rs @@ -25,6 +25,9 @@ pub use agent::*; pub mod destination; pub use destination::*; +pub mod discovery; +pub use discovery::*; + pub mod inner_navigation_client; pub use inner_navigation_client::*; @@ -46,12 +49,7 @@ impl Plugin for Nav2TrafficPlugin { InnerNavigationClientPlugin::default(), NavigationServerPlugin::default(), Nav2AgentPlugin::default(), + AgentDiscoveryPlugin::default(), )); - - // Spawn agents last - let agent_names = vec!["robot0".to_string(), "robot1".to_string()]; - for name in agent_names { - app.world_mut().spawn(Nav2Agent::new(name)); - } } } From a0c27a5321761246e0ab99847d7221ff87442795 Mon Sep 17 00:00:00 2001 From: Xiyu Oh Date: Thu, 16 Jul 2026 10:10:02 +0800 Subject: [PATCH 2/3] Use config file to list agents Signed-off-by: Xiyu Oh --- nav2_integration/README.md | 4 +- nav2_integration/rmf_nav2_traffic/Cargo.toml | 6 ++- .../rmf_nav2_traffic/config/config.yaml | 4 ++ .../rmf_nav2_traffic/src/discovery.rs | 50 ------------------- nav2_integration/rmf_nav2_traffic/src/lib.rs | 31 ++++++++++-- nav2_integration/rmf_nav2_traffic/src/main.rs | 17 +++++++ .../launch/demo_nav2.launch.py | 5 ++ 7 files changed, 59 insertions(+), 58 deletions(-) create mode 100644 nav2_integration/rmf_nav2_traffic/config/config.yaml delete mode 100644 nav2_integration/rmf_nav2_traffic/src/discovery.rs diff --git a/nav2_integration/README.md b/nav2_integration/README.md index 4285d29..ff928fe 100644 --- a/nav2_integration/README.md +++ b/nav2_integration/README.md @@ -133,12 +133,12 @@ With the workspace built and sourced, run the following nodes: Spin up the Nav2 simulation: ``` -ros2 launch sp_demo_nav2_bringup cloned_multi_tb3_simulation_launch.py robots:="robot0={x: 0.0, y: 5.0, yaw: 0.0}; robot1={x: 3.0, y: 5.0, yaw: 0.0};" +ros2 launch sp_demo_nav2_bringup cloned_multi_tb3_simulation_launch.py robots:="robot0={x: 0.0, y: 5.0, yaw: 0.0}; robot1={x: 3.0, y: 5.0, yaw: 0.0};" ``` In a separate terminal, run the demo launch file containing the path server, plan executor, destination server, path visualizer and Nav2 traffic nodes: ``` -ros2 launch rmf_path_server_demo demo_nav2.launch.py robots:="robot0 robot1" +ros2 launch rmf_path_server_demo demo_nav2.launch.py ``` Send action goals to the robots: diff --git a/nav2_integration/rmf_nav2_traffic/Cargo.toml b/nav2_integration/rmf_nav2_traffic/Cargo.toml index c55eb31..d4ffba5 100644 --- a/nav2_integration/rmf_nav2_traffic/Cargo.toml +++ b/nav2_integration/rmf_nav2_traffic/Cargo.toml @@ -13,6 +13,9 @@ name = "nav2_traffic" [package.metadata.ros-env] include = true +[package.metadata.ros] +install_to_share = ["config"] + [dependencies] axum = "0.8" bevy = "0.16" @@ -28,8 +31,9 @@ rclrs = "0.7" reqwest = { version = "0.13", features = ["json", "blocking"] } ros-env = "0.2" rosidl_runtime_rs = "0.6" -serde = "1.0" +serde = { version = "1.0", features = ["derive"] } serde_json = "1.0" +serde_yaml = "0.9" thiserror = "2.0" tokio = { version = "1.52", features = ["full"] } uuid = "1" diff --git a/nav2_integration/rmf_nav2_traffic/config/config.yaml b/nav2_integration/rmf_nav2_traffic/config/config.yaml new file mode 100644 index 0000000..dd4c30d --- /dev/null +++ b/nav2_integration/rmf_nav2_traffic/config/config.yaml @@ -0,0 +1,4 @@ +agents: + robot0: + # TODO(@xiyuoh) add other configurable fields + robot1: diff --git a/nav2_integration/rmf_nav2_traffic/src/discovery.rs b/nav2_integration/rmf_nav2_traffic/src/discovery.rs deleted file mode 100644 index e373ad2..0000000 --- a/nav2_integration/rmf_nav2_traffic/src/discovery.rs +++ /dev/null @@ -1,50 +0,0 @@ -use crate::Nav2Agent; -use bevy::prelude::*; -use bevy_ros2::RclrsNode; -use std::collections::HashSet; - -#[derive(Resource, Default)] -pub struct AgentTracker { - pub known_agents: HashSet, -} - -#[derive(Default)] -pub struct AgentDiscoveryPlugin {} - -impl Plugin for AgentDiscoveryPlugin { - fn build(&self, app: &mut App) { - app.init_resource::() - .add_systems(PreUpdate, discover_agents_via_graph); - } -} - -// Discover agents via existing Nav2 nodes -fn discover_agents_via_graph( - mut commands: Commands, - node: Res, - mut tracker: ResMut, -) { - let Ok(node_names) = node.get_node_names() else { - return; - }; - - for info in node_names { - if info.name == "bt_navigator" { - // Remove /inner suffix - let namespace = info.namespace.trim_start_matches('/'); - let namespace = namespace.strip_suffix("/inner").unwrap_or(namespace); - - if namespace.is_empty() { - continue; - } - - let robot_name = namespace.to_string(); - - if !tracker.known_agents.contains(&robot_name) { - info!("Discovered new Nav2 agent from ROS graph: {}", robot_name); - tracker.known_agents.insert(robot_name.clone()); - commands.spawn(Nav2Agent::new(robot_name)); - } - } - } -} diff --git a/nav2_integration/rmf_nav2_traffic/src/lib.rs b/nav2_integration/rmf_nav2_traffic/src/lib.rs index 2d673c3..5abd7e0 100644 --- a/nav2_integration/rmf_nav2_traffic/src/lib.rs +++ b/nav2_integration/rmf_nav2_traffic/src/lib.rs @@ -18,6 +18,8 @@ use bevy::prelude::*; use bevy_ros2::RclrsPlugin; use crossflow::CrossflowPlugin; +use serde::Deserialize; +use std::collections::HashMap; pub mod agent; pub use agent::*; @@ -25,9 +27,6 @@ pub use agent::*; pub mod destination; pub use destination::*; -pub mod discovery; -pub use discovery::*; - pub mod inner_navigation_client; pub use inner_navigation_client::*; @@ -37,6 +36,21 @@ pub use navigation_server::*; pub mod safe_zone; pub use safe_zone::*; +#[derive(Deserialize, Debug, Default, Clone)] +pub struct AgentConfigEntry { + // TODO(@xiyuoh) add configurable agent-specific fields here when we need it +} + +#[derive(Deserialize, Debug)] +pub struct AgentConfig { + pub agents: HashMap>, +} + +#[derive(Resource, Debug, Clone)] +pub struct ConfiguredAgents { + pub names: Vec, +} + #[derive(Default)] pub struct Nav2TrafficPlugin {} @@ -49,7 +63,14 @@ impl Plugin for Nav2TrafficPlugin { InnerNavigationClientPlugin::default(), NavigationServerPlugin::default(), Nav2AgentPlugin::default(), - AgentDiscoveryPlugin::default(), - )); + )) + .add_systems(Startup, spawn_configured_agents); + } +} + +fn spawn_configured_agents(mut commands: Commands, configured_agents: Res) { + for name in &configured_agents.names { + info!("Spawning configured agent: {}", name); + commands.spawn(Nav2Agent::new(name.clone())); } } diff --git a/nav2_integration/rmf_nav2_traffic/src/main.rs b/nav2_integration/rmf_nav2_traffic/src/main.rs index 59845c9..25c3697 100644 --- a/nav2_integration/rmf_nav2_traffic/src/main.rs +++ b/nav2_integration/rmf_nav2_traffic/src/main.rs @@ -1,8 +1,25 @@ use bevy::{log::LogPlugin, prelude::*}; use nav2_traffic::*; +use std::fs; fn main() { + let args: Vec = std::env::args().collect(); + let config_path = args + .windows(2) + .find(|w| w[0] == "-c" || w[0] == "--config") + .map(|w| w[1].clone()) + .expect("Missing -c or --config argument"); + + let config_contents = fs::read_to_string(&config_path).expect("Failed to read config file"); + let config: AgentConfig = serde_yaml::from_str(&config_contents).expect("Failed to parse config YAML"); + + let mut configured_agents = ConfiguredAgents { names: vec![] }; + for agent_name in config.agents.keys() { + configured_agents.names.push(agent_name.clone()); + } + let mut app = App::new(); + app.insert_resource(configured_agents); app.add_plugins((MinimalPlugins, LogPlugin::default())); app.add_plugins(Nav2TrafficPlugin::default()); app.run(); diff --git a/path_server/rmf_path_server_demo/launch/demo_nav2.launch.py b/path_server/rmf_path_server_demo/launch/demo_nav2.launch.py index 5abfff4..36a9df8 100644 --- a/path_server/rmf_path_server_demo/launch/demo_nav2.launch.py +++ b/path_server/rmf_path_server_demo/launch/demo_nav2.launch.py @@ -98,6 +98,11 @@ def generate_launch_description(): executable='nav2_traffic', name='nav2_traffic', output='both', + arguments=['--config', os.path.join( + get_package_share_directory('rmf_nav2_traffic'), + 'config', + 'config.yaml' + )], parameters=[{'use_sim_time': True}] ) From 0f7f32a4488d90ed0cd4af996efff4a8608ee21e Mon Sep 17 00:00:00 2001 From: Xiyu Oh Date: Sat, 1 Aug 2026 10:48:44 +0800 Subject: [PATCH 3/3] Use clap Signed-off-by: Xiyu Oh --- nav2_integration/rmf_nav2_traffic/Cargo.toml | 1 + nav2_integration/rmf_nav2_traffic/src/main.rs | 25 +++++++++++++------ .../launch/demo_nav2.launch.py | 4 +-- 3 files changed, 20 insertions(+), 10 deletions(-) diff --git a/nav2_integration/rmf_nav2_traffic/Cargo.toml b/nav2_integration/rmf_nav2_traffic/Cargo.toml index d4ffba5..13f8979 100644 --- a/nav2_integration/rmf_nav2_traffic/Cargo.toml +++ b/nav2_integration/rmf_nav2_traffic/Cargo.toml @@ -17,6 +17,7 @@ include = true install_to_share = ["config"] [dependencies] +clap = { version = "4", features = ["derive"] } axum = "0.8" bevy = "0.16" bevy_ecs = "0.16" diff --git a/nav2_integration/rmf_nav2_traffic/src/main.rs b/nav2_integration/rmf_nav2_traffic/src/main.rs index 25c3697..0c366bf 100644 --- a/nav2_integration/rmf_nav2_traffic/src/main.rs +++ b/nav2_integration/rmf_nav2_traffic/src/main.rs @@ -1,17 +1,26 @@ use bevy::{log::LogPlugin, prelude::*}; +use clap::Parser; use nav2_traffic::*; use std::fs; +#[derive(Parser)] +#[command(author, version, about = "RMF Nav2 Traffic manager")] +struct Cli { + /// Path to the YAML config file + #[arg(short = 'c', long)] + config: std::path::PathBuf, + + /// Absorb any trailing arguments appended by ROS 2 launch (e.g. --ros-args) + #[arg(trailing_var_arg = true, allow_hyphen_values = true, hide = true)] + _ros_args: Vec, +} + fn main() { - let args: Vec = std::env::args().collect(); - let config_path = args - .windows(2) - .find(|w| w[0] == "-c" || w[0] == "--config") - .map(|w| w[1].clone()) - .expect("Missing -c or --config argument"); + let cli = Cli::parse(); - let config_contents = fs::read_to_string(&config_path).expect("Failed to read config file"); - let config: AgentConfig = serde_yaml::from_str(&config_contents).expect("Failed to parse config YAML"); + let config_contents = fs::read_to_string(&cli.config).expect("Failed to read config file"); + let config: AgentConfig = + serde_yaml::from_str(&config_contents).expect("Failed to parse config YAML"); let mut configured_agents = ConfiguredAgents { names: vec![] }; for agent_name in config.agents.keys() { diff --git a/path_server/rmf_path_server_demo/launch/demo_nav2.launch.py b/path_server/rmf_path_server_demo/launch/demo_nav2.launch.py index 36a9df8..d541dc0 100644 --- a/path_server/rmf_path_server_demo/launch/demo_nav2.launch.py +++ b/path_server/rmf_path_server_demo/launch/demo_nav2.launch.py @@ -16,7 +16,7 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, OpaqueFunction +from launch.actions import DeclareLaunchArgument from launch.conditions import IfCondition from launch.substitutions import LaunchConfiguration, PythonExpression from launch_ros.actions import Node @@ -101,7 +101,7 @@ def generate_launch_description(): arguments=['--config', os.path.join( get_package_share_directory('rmf_nav2_traffic'), 'config', - 'config.yaml' + 'config.yaml', )], parameters=[{'use_sim_time': True}] )