-
Notifications
You must be signed in to change notification settings - Fork 7
Nav2 PR follow up #30
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
Merged
Merged
Changes from all commits
Commits
Show all changes
3 commits
Select commit
Hold shift + click to select a range
File filter
Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
There are no files selected for viewing
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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(); | ||
| } |
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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<Plan, VisualizerServer>, | ||
| _safe_zone_sub: rclrs::WorkerSubscription<SafeZone, VisualizerServer>, | ||
| } | ||
|
|
||
| fn main() -> Result<(), Box<dyn std::error::Error>> { | ||
| 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<String> = std::env::args().collect(); | ||
| let mut robot_name = "robot0".to_string(); | ||
| struct VisualizerDiscoveryServer { | ||
| node: rclrs::Node, | ||
| active_robots: HashMap<String, RobotConnections>, | ||
| visualizer_worker: rclrs::Worker<VisualizerServer>, | ||
| } | ||
|
|
||
| // 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<VisualizerServer>) -> 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::<MarkerArray>(marker_topic.as_str().transient_local().reliable())?; | ||
|
|
||
| let publisher_plan = publisher.clone(); | ||
| let robot_name_clone = robot_name.clone(); | ||
|
|
||
| let _subscription = node.create_subscription::<Plan, _>( | ||
| 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<dyn std::error::Error>> { | ||
| 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::<MarkerArray>( | ||
| 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::<Plan, _>( | ||
| 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::<SafeZone, _>( | ||
| 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::<SafeZone, _>( | ||
| 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(()) | ||
| } |
Oops, something went wrong.
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
knit: Lets rename this to demo_nav2.launch.py
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
0372bd8, thanks! Will merge after CI passes