diff --git a/nav2_integration/README.md b/nav2_integration/README.md index d7e9a0a..4285d29 100644 --- a/nav2_integration/README.md +++ b/nav2_integration/README.md @@ -136,14 +136,9 @@ 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};" ``` -In a separate terminal, spin up the Nav2 traffic node: +In a separate terminal, run the demo launch file containing the path server, plan executor, destination server, path visualizer and Nav2 traffic nodes: ``` -ros2 run rmf_nav2_traffic nav2_traffic --ros-args -p use_sim_time:=true -``` - -Run the demo launch file containing the path server, plan executor, destination server, and path visualizer nodes: -``` -ros2 launch rmf_path_server_demo demo_viz.launch.py robots:="robot0 robot1" +ros2 launch rmf_path_server_demo demo_nav2.launch.py robots:="robot0 robot1" ``` Send action goals to the robots: diff --git a/nav2_integration/rmf_nav2_traffic/src/main.rs b/nav2_integration/rmf_nav2_traffic/src/main.rs index b30f887..59845c9 100644 --- a/nav2_integration/rmf_nav2_traffic/src/main.rs +++ b/nav2_integration/rmf_nav2_traffic/src/main.rs @@ -1,9 +1,9 @@ -use bevy::prelude::*; +use bevy::{log::LogPlugin, prelude::*}; use nav2_traffic::*; fn main() { let mut app = App::new(); - app.add_plugins(DefaultPlugins); + app.add_plugins((MinimalPlugins, LogPlugin::default())); app.add_plugins(Nav2TrafficPlugin::default()); app.run(); } diff --git a/path_server/rmf_path_server_demo/launch/demo_viz.launch.py b/path_server/rmf_path_server_demo/launch/demo_nav2.launch.py similarity index 82% rename from path_server/rmf_path_server_demo/launch/demo_viz.launch.py rename to path_server/rmf_path_server_demo/launch/demo_nav2.launch.py index a732b4e..5abfff4 100644 --- a/path_server/rmf_path_server_demo/launch/demo_viz.launch.py +++ b/path_server/rmf_path_server_demo/launch/demo_nav2.launch.py @@ -55,23 +55,13 @@ def generate_launch_description(): output='both' ) - # 2. Start the visualizer nodes - def generate_visualizers(context, *args, **kwargs): - robots_str = LaunchConfiguration('robots').perform(context) - robot_names = robots_str.split() if robots_str else [] - - visualizers = [] - for robot in robot_names: - visualizers.append(Node( - package='rmf_path_visualizer', - executable='rmf_path_visualizer', - name=f'rmf_path_visualizer_{robot}', - output='both', - arguments=[robot] - )) - return visualizers - - visualizers_launcher = OpaqueFunction(function=generate_visualizers) + # 2. Start the visualizer node + visualizer_node = Node( + package='rmf_path_visualizer', + executable='rmf_path_visualizer', + name='rmf_path_visualizer', + output='both' + ) # 3. Start exactly one selectable destination implementation. simple_destination_server = Node( @@ -102,12 +92,22 @@ def generate_visualizers(context, *args, **kwargs): output='both' ) + # 5. Start nav2_traffic + nav2_traffic = Node( + package='rmf_nav2_traffic', + executable='nav2_traffic', + name='nav2_traffic', + output='both', + parameters=[{'use_sim_time': True}] + ) + return LaunchDescription([ declare_destination_server, declare_config_file, path_server, - visualizers_launcher, + visualizer_node, simple_destination_server, reservation_destination_server, plan_executor, + nav2_traffic, ]) diff --git a/path_server/rmf_path_visualizer/Cargo.toml b/path_server/rmf_path_visualizer/Cargo.toml index d5d21db..fd6da18 100644 --- a/path_server/rmf_path_visualizer/Cargo.toml +++ b/path_server/rmf_path_visualizer/Cargo.toml @@ -6,3 +6,4 @@ edition = "2021" [dependencies] rclrs = "*" ros-env = "0.2.0" +rmf_participant_discovery = { path = "../../rmf_participant_discovery" } diff --git a/path_server/rmf_path_visualizer/src/main.rs b/path_server/rmf_path_visualizer/src/main.rs index f4bc909..9dcfaef 100644 --- a/path_server/rmf_path_visualizer/src/main.rs +++ b/path_server/rmf_path_visualizer/src/main.rs @@ -1,120 +1,198 @@ use rclrs::{Context, CreateBasicExecutor, IntoPrimitiveOptions, SpinOptions}; -use ros_env::geometry_msgs::msg::Point; -use ros_env::rmf_prototype_msgs::msg::{Plan, SafeZone}; -use ros_env::visualization_msgs::msg::{Marker, MarkerArray}; -use std::sync::Arc; +use ros_env::{ + geometry_msgs::msg::Point, + rmf_prototype_msgs::msg::{Plan, SafeZone}, + visualization_msgs::msg::{Marker, MarkerArray}, +}; +use std::collections::HashMap; + +struct RobotConnections { + _plan_subscription: rclrs::WorkerSubscription, + _safe_zone_sub: rclrs::WorkerSubscription, +} -fn main() -> Result<(), Box> { - let context = Context::default_from_env()?; - let mut executor = context.create_basic_executor(); - let node = Arc::new(executor.create_node("rmf_path_visualizer")?); +struct VisualizerServer {} - let args: Vec = std::env::args().collect(); - let mut robot_name = "robot0".to_string(); +struct VisualizerDiscoveryServer { + node: rclrs::Node, + active_robots: HashMap, + visualizer_worker: rclrs::Worker, +} - // Simple arg parsing to get robot_name - for arg in args.iter().skip(1) { - if !arg.starts_with("--ros-args") && !arg.starts_with("-") { - robot_name = arg.clone(); - break; +impl VisualizerDiscoveryServer { + fn new(node: rclrs::Node, visualizer_worker: rclrs::Worker) -> Self { + Self { + node, + active_robots: HashMap::new(), + visualizer_worker, } } +} - let topic_name = format!("{}/plan", robot_name); - let safe_zone_topic = format!("{}/plan/safe_zone", robot_name); - let marker_topic = format!("{}/path_markers", robot_name); - - let publisher = - node.create_publisher::(marker_topic.as_str().transient_local().reliable())?; - - let publisher_plan = publisher.clone(); - let robot_name_clone = robot_name.clone(); - - let _subscription = node.create_subscription::( - topic_name.as_str().transient_local().reliable(), - move |msg: Plan| { - let mut marker_array = MarkerArray { markers: vec![] }; - let mut marker = Marker::default(); - marker.header.frame_id = "map".to_string(); - // Try to use the start_time from the plan - marker.header.stamp = msg.start_time; - marker.ns = robot_name_clone.clone(); - marker.id = 0; - marker.type_ = ros_env::visualization_msgs::msg::Marker::LINE_STRIP as i32; - marker.action = ros_env::visualization_msgs::msg::Marker::ADD as i32; - - // Set color (e.g., green) - marker.color.r = 0.0; - marker.color.g = 1.0; - marker.color.b = 0.0; - marker.color.a = 1.0; - - // Set line width - marker.scale.x = 0.05; - - for wp in &msg.waypoints { - let mut p = Point::default(); - p.x = wp.position[0] as f64; - p.y = wp.position[1] as f64; - p.z = 0.0; - marker.points.push(p); +fn main() -> Result<(), Box> { + let context = Context::default_from_env()?; + let mut executor = context.create_basic_executor(); + let node = executor.create_node("rmf_path_visualizer")?; + + let visualizer_worker = node.create_worker(VisualizerServer {}); + let discovery_worker = node.create_worker(VisualizerDiscoveryServer::new( + node.clone(), + visualizer_worker.clone(), + )); + + let _discovery_subscription = rmf_participant_discovery::create_discovery_subscription( + &discovery_worker, + "/destination/discovery", + |server: &mut VisualizerDiscoveryServer, robot_id: &str| { + if !server.active_robots.contains_key(robot_id) { + rclrs::log!( + server.node.logger(), + "Discovered new participant for visualization: {}", + robot_id + ); + + let robot_id_clone = robot_id.to_string(); + let robot_id_clone2 = robot_id.to_string(); + + let topic_name = format!("{}/plan", robot_id); + let safe_zone_topic = format!("{}/plan/safe_zone", robot_id); + let marker_topic = format!("{}/path_markers", robot_id); + + let publisher = match server.node.create_publisher::( + marker_topic.as_str().transient_local().reliable(), + ) { + Ok(publ) => publ, + Err(err) => { + rclrs::log_error!( + server.node.logger(), + "Failed to create publisher for {}: {:?}", + robot_id, + err + ); + return; + } + }; + + let publisher_plan = publisher.clone(); + let plan_sub = match server.visualizer_worker.create_subscription::( + topic_name.as_str().transient_local().reliable(), + move |_vs: &mut VisualizerServer, msg: Plan| { + let mut marker_array = MarkerArray { markers: vec![] }; + let mut marker = Marker::default(); + marker.header.frame_id = "map".to_string(); + marker.header.stamp = msg.start_time; + marker.ns = robot_id_clone.clone(); + marker.id = 0; + marker.type_ = ros_env::visualization_msgs::msg::Marker::LINE_STRIP as i32; + marker.action = ros_env::visualization_msgs::msg::Marker::ADD as i32; + + marker.color.r = 0.0; + marker.color.g = 1.0; + marker.color.b = 0.0; + marker.color.a = 1.0; + + marker.scale.x = 0.05; + + for wp in &msg.waypoints { + let mut p = Point::default(); + p.x = wp.position[0] as f64; + p.y = wp.position[1] as f64; + p.z = 0.0; + marker.points.push(p); + } + + marker_array.markers.push(marker); + let _ = publisher_plan.publish(&marker_array); + }, + ) { + Ok(sub) => sub, + Err(err) => { + rclrs::log_error!( + server.node.logger(), + "Failed to create plan subscription for {}: {:?}", + robot_id, + err + ); + return; + } + }; + + let publisher_sz = publisher.clone(); + let safe_zone_sub = + match server.visualizer_worker.create_subscription::( + safe_zone_topic.as_str().transient_local().reliable(), + move |_vs: &mut VisualizerServer, msg: SafeZone| { + if msg.incremental_target.regions.is_empty() { + return; + } + let region = &msg.incremental_target.regions[0].region; + if region.points.len() >= 2 { + let x = region.points[0] as f64; + let y = region.points[1] as f64; + + let mut marker_array = MarkerArray { markers: vec![] }; + let mut marker = Marker::default(); + marker.header.frame_id = "map".to_string(); + marker.ns = format!("{}_safe_zone", robot_id_clone2); + marker.id = 1; + marker.type_ = + ros_env::visualization_msgs::msg::Marker::SPHERE as i32; + marker.action = + ros_env::visualization_msgs::msg::Marker::ADD as i32; + + marker.color.r = 1.0; + marker.color.g = 0.0; + marker.color.b = 0.0; + marker.color.a = 1.0; + + marker.scale.x = 0.25; + marker.scale.y = 0.25; + marker.scale.z = 0.25; + + marker.pose.position.x = x; + marker.pose.position.y = y; + marker.pose.position.z = 0.0; + + marker.pose.orientation.w = 1.0; // valid quaternion + + marker_array.markers.push(marker); + let _ = publisher_sz.publish(&marker_array); + } + }, + ) { + Ok(sub) => sub, + Err(err) => { + rclrs::log_error!( + server.node.logger(), + "Failed to create safe_zone subscription for {}: {:?}", + robot_id, + err + ); + return; + } + }; + + server.active_robots.insert( + robot_id.to_string(), + RobotConnections { + _plan_subscription: plan_sub, + _safe_zone_sub: safe_zone_sub, + }, + ); } - - marker_array.markers.push(marker); - let _ = publisher_plan.publish(&marker_array); }, - )?; - - let publisher_sz = publisher.clone(); - let robot_name_clone2 = robot_name.clone(); - - let _safe_zone_sub = node.create_subscription::( - safe_zone_topic.as_str().transient_local().reliable(), - move |msg: SafeZone| { - if msg.incremental_target.regions.is_empty() { - return; - } - let region = &msg.incremental_target.regions[0].region; - if region.points.len() >= 2 { - let x = region.points[0] as f64; - let y = region.points[1] as f64; - - let mut marker_array = MarkerArray { markers: vec![] }; - let mut marker = Marker::default(); - marker.header.frame_id = "map".to_string(); - marker.ns = format!("{}_safe_zone", robot_name_clone2); - marker.id = 1; - marker.type_ = ros_env::visualization_msgs::msg::Marker::SPHERE as i32; - marker.action = ros_env::visualization_msgs::msg::Marker::ADD as i32; - - // Set color (e.g., red dot) - marker.color.r = 1.0; - marker.color.g = 0.0; - marker.color.b = 0.0; - marker.color.a = 1.0; - - // Set scale (size of the dot) - marker.scale.x = 0.25; - marker.scale.y = 0.25; - marker.scale.z = 0.25; - - marker.pose.position.x = x; - marker.pose.position.y = y; - marker.pose.position.z = 0.0; - - marker.pose.orientation.w = 1.0; // valid quaternion - - marker_array.markers.push(marker); - let _ = publisher_sz.publish(&marker_array); + |server: &mut VisualizerDiscoveryServer, robot_id: &str| { + if server.active_robots.remove(robot_id).is_some() { + rclrs::log!( + server.node.logger(), + "Participant left visualization tracking: {}", + robot_id + ); } }, )?; - rclrs::log!( - node.logger(), - "Starting rmf_path_visualizer for robot: {}", - robot_name - ); + rclrs::log!(node.logger(), "Starting rmf_path_visualizer with discovery"); executor.spin(SpinOptions::default()); Ok(()) }