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..13f8979 100644 --- a/nav2_integration/rmf_nav2_traffic/Cargo.toml +++ b/nav2_integration/rmf_nav2_traffic/Cargo.toml @@ -13,7 +13,11 @@ name = "nav2_traffic" [package.metadata.ros-env] include = true +[package.metadata.ros] +install_to_share = ["config"] + [dependencies] +clap = { version = "4", features = ["derive"] } axum = "0.8" bevy = "0.16" bevy_ecs = "0.16" @@ -28,8 +32,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/lib.rs b/nav2_integration/rmf_nav2_traffic/src/lib.rs index 469761c..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::*; @@ -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>, +} + +#[derive(Resource, Debug, Clone)] +pub struct ConfiguredAgents { + pub names: Vec, +} + #[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) { + 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..0c366bf 100644 --- a/nav2_integration/rmf_nav2_traffic/src/main.rs +++ b/nav2_integration/rmf_nav2_traffic/src/main.rs @@ -1,8 +1,34 @@ 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 cli = Cli::parse(); + + 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() { + 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..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 @@ -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}] )