Discover agents via Nav2 nodes

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:

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"

nav2_integration/rmf_nav2_traffic/config/config.yaml

@@ -0,0 +1,4 @@
+agents:
+  robot0:
+    # TODO(@xiyuoh) add other configurable fields
+  robot1:

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::*;
@@ -34,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<String, Option<AgentConfigEntry>>,
+}
+
+#[derive(Resource, Debug, Clone)]
+pub struct ConfiguredAgents {
+    pub names: Vec<String>,
+}
+
 #[derive(Default)]
 pub struct Nav2TrafficPlugin {}
 
@@ -46,12 +63,14 @@ impl Plugin for Nav2TrafficPlugin {
                 InnerNavigationClientPlugin::default(),
                 NavigationServerPlugin::default(),
                 Nav2AgentPlugin::default(),
-            ));
+            ))
+            .add_systems(Startup, spawn_configured_agents);
+    }
+}
 
-        // 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));
-        }
+fn spawn_configured_agents(mut commands: Commands, configured_agents: Res<ConfiguredAgents>) {
+    for name in &configured_agents.names {
+        info!("Spawning configured agent: {}", name);
+        commands.spawn(Nav2Agent::new(name.clone()));
     }
 }

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<String> = 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();

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}]
     )