diff --git a/path_server/rmf_plan_executor/src/lib.rs b/path_server/rmf_plan_executor/src/lib.rs index ba487d5..535223f 100644 --- a/path_server/rmf_plan_executor/src/lib.rs +++ b/path_server/rmf_plan_executor/src/lib.rs @@ -90,6 +90,54 @@ impl BlockageMonitor { } } +#[derive(Clone, Debug, PartialEq)] +pub struct PlanExecutorConfig { + pub grid_width: u32, + pub grid_height: u32, + pub grid_resolution: f32, + pub grid_origin: Pose, +} + +impl Default for PlanExecutorConfig { + fn default() -> Self { + let mut origin = Pose::default(); + origin.orientation.w = 1.0; + Self { + grid_width: 20, + grid_height: 20, + grid_resolution: 1.0, + grid_origin: origin, + } + } +} + +impl PlanExecutorConfig { + pub fn new(grid_width: u32, grid_height: u32, grid_resolution: f32, grid_origin: Pose) -> Self { + Self { + grid_width, + grid_height, + grid_resolution, + grid_origin, + } + } + + pub fn with_grid_size(mut self, width: u32, height: u32) -> Self { + self.grid_width = width; + self.grid_height = height; + self + } + + pub fn with_resolution(mut self, resolution: f32) -> Self { + self.grid_resolution = resolution; + self + } + + pub fn with_origin(mut self, origin: Pose) -> Self { + self.grid_origin = origin; + self + } +} + pub struct PlanExecutor { pub node: Node, // TODO(arjoc) The use of BTreeMap here is to make sure robts are ordered by their names @@ -134,19 +182,29 @@ fn target_yaw(plan: &Plan, target_idx: usize) -> f32 { impl PlanExecutor { pub fn new(node: Node) -> Self { - let mut origin = Pose::default(); - origin.orientation.w = 1.0; + Self::with_config(node, PlanExecutorConfig::default()) + } + + pub fn new_with_config(node: Node, config: PlanExecutorConfig) -> Self { + Self::with_config(node, config) + } + + pub fn with_config(node: Node, config: PlanExecutorConfig) -> Self { + let grid = Arc::new(Grid2D::new( + vec![vec![0; config.grid_height as usize]; config.grid_width as usize], + config.grid_resolution, + )); Self { node, active_robots: BTreeMap::new(), plan_release_publishers: HashMap::new(), safezone_publishers: HashMap::new(), plan_error_publishers: HashMap::new(), - grid: Arc::new(Grid2D::new(vec![vec![0; 20]; 20], 1.0)), - grid_width: 20, - grid_height: 20, - grid_resolution: 1.0, - grid_origin: origin, + grid, + grid_width: config.grid_width, + grid_height: config.grid_height, + grid_resolution: config.grid_resolution, + grid_origin: config.grid_origin, latest_map: None, } } @@ -250,6 +308,26 @@ impl PlanExecutor { } pub fn handle_map(&mut self, msg: OccupancyGrid) { + if msg.info.width > 0 && msg.info.height > 0 && msg.info.resolution > 0.0 { + rclrs::log!( + self.node.logger(), + "PlanExecutor reconfiguring grid from map: width={}, height={}, resolution={}, origin=({}, {})", + msg.info.width, + msg.info.height, + msg.info.resolution, + msg.info.origin.position.x, + msg.info.origin.position.y + ); + self.grid_width = msg.info.width; + self.grid_height = msg.info.height; + self.grid_resolution = msg.info.resolution; + self.grid_origin = msg.info.origin.clone(); + self.grid = Arc::new(Grid2D::new( + vec![vec![0; self.grid_height as usize]; self.grid_width as usize], + self.grid_resolution, + )); + } + self.latest_map = Some(msg); let robot_ids: Vec<_> = self.active_robots.keys().cloned().collect(); for robot_id in robot_ids { @@ -933,4 +1011,69 @@ mod tests { follower.get_semantic_waypoint().trajectory_index ); } + + #[test] + fn test_plan_executor_config_default_and_builders() { + use crate::PlanExecutorConfig; + use ros_env::geometry_msgs::msg::Pose; + + let config = PlanExecutorConfig::default(); + assert_eq!(config.grid_width, 20); + assert_eq!(config.grid_height, 20); + assert_eq!(config.grid_resolution, 1.0); + assert_eq!(config.grid_origin.orientation.w, 1.0); + + let mut custom_origin = Pose::default(); + custom_origin.position.x = -10.0; + custom_origin.position.y = -5.0; + custom_origin.orientation.w = 1.0; + + let custom_config = PlanExecutorConfig::new(50, 60, 0.5, custom_origin.clone()); + assert_eq!(custom_config.grid_width, 50); + assert_eq!(custom_config.grid_height, 60); + assert_eq!(custom_config.grid_resolution, 0.5); + assert_eq!(custom_config.grid_origin.position.x, -10.0); + + let builder_config = PlanExecutorConfig::default() + .with_grid_size(100, 200) + .with_resolution(0.2) + .with_origin(custom_origin.clone()); + assert_eq!(builder_config.grid_width, 100); + assert_eq!(builder_config.grid_height, 200); + assert_eq!(builder_config.grid_resolution, 0.2); + assert_eq!(builder_config.grid_origin, custom_origin); + } + + #[test] + fn test_handle_map() { + use crate::PlanExecutor; + use rclrs::{Context, CreateBasicExecutor}; + use ros_env::nav_msgs::msg::OccupancyGrid; + + if let Ok(context) = Context::default_from_env() { + let executor = context.create_basic_executor(); + if let Ok(node) = executor.create_node("test_handle_map_node") { + let mut plan_executor = PlanExecutor::new(node); + assert_eq!(plan_executor.grid_width, 20); + assert_eq!(plan_executor.grid_height, 20); + assert_eq!(plan_executor.grid_resolution, 1.0); + + let mut map_msg = OccupancyGrid::default(); + map_msg.info.width = 150; + map_msg.info.height = 200; + map_msg.info.resolution = 0.05; + map_msg.info.origin.position.x = -15.0; + map_msg.info.origin.position.y = -10.0; + map_msg.info.origin.orientation.w = 1.0; + + plan_executor.handle_map(map_msg); + + assert_eq!(plan_executor.grid_width, 150); + assert_eq!(plan_executor.grid_height, 200); + assert_eq!(plan_executor.grid_resolution, 0.05); + assert_eq!(plan_executor.grid_origin.position.x, -15.0); + assert_eq!(plan_executor.grid_origin.position.y, -10.0); + } + } + } } diff --git a/path_server/rmf_plan_executor/src/main.rs b/path_server/rmf_plan_executor/src/main.rs index 8ecd582..06907c1 100644 --- a/path_server/rmf_plan_executor/src/main.rs +++ b/path_server/rmf_plan_executor/src/main.rs @@ -13,10 +13,12 @@ // limitations under the License. use rclrs::{Context, CreateBasicExecutor, IntoPrimitiveOptions, SpinOptions}; -use rmf_plan_executor::PlanExecutor; +use rmf_plan_executor::{PlanExecutor, PlanExecutorConfig}; +use ros_env::geometry_msgs::msg::Pose; use ros_env::nav_msgs::msg::{OccupancyGrid, Odometry}; use ros_env::rmf_prototype_msgs::msg::{ParticipantList, Plan}; use std::collections::HashMap; +use std::env; struct RobotConnections { _odom_subscription: rclrs::WorkerSubscription, @@ -44,8 +46,119 @@ fn main() -> Result<(), Box> { let mut executor = context.create_basic_executor(); let node = executor.create_node("plan_executor")?; + let param_grid_width = node + .declare_parameter("grid_width") + .default(20i64) + .mandatory() + .map(|p| p.get() as u32) + .unwrap_or(20); + + let param_grid_height = node + .declare_parameter("grid_height") + .default(20i64) + .mandatory() + .map(|p| p.get() as u32) + .unwrap_or(20); + + let param_grid_resolution = node + .declare_parameter("grid_resolution") + .default(1.0f64) + .mandatory() + .map(|p| p.get() as f32) + .unwrap_or(1.0); + + let param_grid_size = node + .declare_parameter("grid_size") + .default(1.0f64) + .mandatory() + .map(|p| p.get() as f32) + .unwrap_or(1.0); + + let param_grid_origin_x = node + .declare_parameter("grid_origin_x") + .default(0.0f64) + .mandatory() + .map(|p| p.get()) + .unwrap_or(0.0); + + let param_grid_origin_y = node + .declare_parameter("grid_origin_y") + .default(0.0f64) + .mandatory() + .map(|p| p.get()) + .unwrap_or(0.0); + + let args: Vec = env::args().collect(); + let get_cli_arg = |flag: &str| -> Option { + args.iter() + .position(|arg| arg == flag) + .and_then(|i| args.get(i + 1).cloned()) + .or_else(|| { + let prefix = format!("{}=", flag); + args.iter() + .find_map(|arg| arg.strip_prefix(&prefix).map(String::from)) + }) + }; + + let grid_width = get_cli_arg("--grid-width") + .and_then(|s| s.parse::().ok()) + .unwrap_or(param_grid_width); + + let grid_height = get_cli_arg("--grid-height") + .and_then(|s| s.parse::().ok()) + .unwrap_or(param_grid_height); + + let grid_resolution = get_cli_arg("--grid-resolution") + .or_else(|| get_cli_arg("--grid-size")) + .and_then(|s| s.parse::().ok()) + .unwrap_or_else(|| { + if param_grid_size != 1.0 && param_grid_resolution == 1.0 { + param_grid_size + } else { + param_grid_resolution + } + }); + + let grid_origin_x = get_cli_arg("--grid-origin-x") + .and_then(|s| s.parse::().ok()) + .unwrap_or(param_grid_origin_x); + + let grid_origin_y = get_cli_arg("--grid-origin-y") + .and_then(|s| s.parse::().ok()) + .unwrap_or(param_grid_origin_y); + + let mut origin = Pose::default(); + origin.position.x = grid_origin_x; + origin.position.y = grid_origin_y; + origin.orientation.w = 1.0; + + let config = PlanExecutorConfig { + grid_width, + grid_height, + grid_resolution, + grid_origin: origin, + }; + + rclrs::log!( + node.logger(), + "PlanExecutor initialized with grid: width={}, height={}, resolution={}, origin=({}, {})", + config.grid_width, + config.grid_height, + config.grid_resolution, + config.grid_origin.position.x, + config.grid_origin.position.y + ); + // Create the executor worker - let executor_worker = node.create_worker(PlanExecutor::new(node.clone())); + let executor_worker = node.create_worker(PlanExecutor::with_config(node.clone(), config)); + + // Subscribe to map to auto-reconfigure grid size, resolution, and origin + let _map_subscription = executor_worker.create_subscription::( + "/map".transient_local().reliable(), + move |executor: &mut PlanExecutor, msg: OccupancyGrid| { + executor.handle_map(msg); + }, + )?; // Create the discovery worker let discovery_worker = node.create_worker(ExecutorDiscoveryServer::new( @@ -73,13 +186,6 @@ fn main() -> Result<(), Box> { }, )?; - let _map_subscription = executor_worker.create_subscription::( - "/map".transient_local().reliable(), - move |executor: &mut PlanExecutor, msg: OccupancyGrid| { - executor.handle_map(msg); - }, - )?; - // 2. Subscribe to discovery on the discovery worker to manage odom/plan subscriptions let _discovery_subscription = rmf_participant_discovery::create_discovery_subscription( &discovery_worker,