diff --git a/path_server/rmf_path_server/src/lib.rs b/path_server/rmf_path_server/src/lib.rs index 81fdf15..abaab44 100644 --- a/path_server/rmf_path_server/src/lib.rs +++ b/path_server/rmf_path_server/src/lib.rs @@ -11,18 +11,23 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. - use mapf_post::{na::Isometry2, MapfResult, SemanticPlan, SemanticWaypoint}; use rclrs::{IntoPrimitiveOptions, Node}; -use ros_env::builtin_interfaces; -use ros_env::nav_msgs::msg::Odometry; -use ros_env::rmf_prototype_msgs; -use ros_env::rmf_prototype_msgs::msg::{Destination, Plan, PlanId, TrafficDependency, Waypoint}; -use std::collections::{hash_map::Entry, HashMap}; -use std::sync::Arc; +use ros_env::{ + builtin_interfaces, + nav_msgs::msg::{OccupancyGrid, Odometry}, + rmf_prototype_msgs::{ + self, + msg::{Destination, Plan, PlanId, TrafficDependency, Waypoint}, + }, +}; +use std::{ + collections::{hash_map::Entry, HashMap}, + sync::Arc, +}; pub mod planner; -pub use planner::{MapfPlanner, MockPlanner, PibtPlanner}; +pub use planner::{Map, MapfPlanner, MockPlanner, PibtPlanner}; pub struct PlanSuccess { pub session_id: u64, @@ -53,6 +58,7 @@ pub struct PlanServer { pub current_planning_session: Option, pub footprints: Arc>>, pub active_plan_ids: HashMap, + pub map: Arc, } impl PlanServer

{ @@ -77,6 +83,7 @@ impl PlanServer

{ current_planning_session: None, footprints, active_plan_ids: HashMap::new(), + map: Arc::new(Map::default()), } } @@ -322,6 +329,7 @@ impl PlanServer

{ let planner_clone = Arc::clone(&self.planner); let footprints_clone = Arc::clone(&self.footprints); let sender_clone = self.plan_sender.clone(); + let map_clone = self.map.clone(); std::thread::spawn(move || { if cancellation.load(std::sync::atomic::Ordering::Relaxed) { @@ -351,6 +359,7 @@ impl PlanServer

{ &goals, &footprints_map, &robot_ids, + map_clone.as_ref(), Arc::clone(&cancellation), ) { Ok(plan) => plan, @@ -489,6 +498,7 @@ pub struct PathServerRunning { rclrs::WorkerSubscription>, pub discovery_subscription: rclrs::WorkerSubscription>, + pub map_subscription: rclrs::WorkerSubscription>, } pub fn start_path_server( @@ -502,6 +512,14 @@ pub fn start_path_server( let destinations_worker = node.create_worker(PlanServer::new(node.clone(), planner, footprints)); + let map_subscription = destinations_worker.create_subscription::( + "/map".transient_local().reliable(), + move |server: &mut PlanServer

, msg: OccupancyGrid| { + rclrs::log!(server.node.logger(), "Received map message"); + server.map = Arc::new(Map { grid: msg }); + }, + )?; + // Create a periodic timer on the Destinations worker to trigger replans asynchronously. let replan_timer = destinations_worker.create_timer_repeating( std::time::Duration::from_millis(100), @@ -611,5 +629,6 @@ pub fn start_path_server( replan_timer: Box::new(replan_timer), list_subscription, discovery_subscription, + map_subscription, }) } diff --git a/path_server/rmf_path_server/src/planner.rs b/path_server/rmf_path_server/src/planner.rs index 23ab433..8b2130a 100644 --- a/path_server/rmf_path_server/src/planner.rs +++ b/path_server/rmf_path_server/src/planner.rs @@ -14,13 +14,18 @@ use hetpibt::external_tracks_pibt::PiBTWithExternalTracks; use mapf_post::na::Isometry2; -use ros_env::nav_msgs::msg::Odometry; +use ros_env::nav_msgs::msg::{OccupancyGrid, Odometry}; use ros_env::rmf_prototype_msgs::msg::Destination; use std::collections::HashMap; use std::sync::atomic::AtomicBool; use std::sync::Arc; +#[derive(Clone, Debug, Default)] +pub struct Map { + pub grid: OccupancyGrid, +} + /// Implement this trait to use your own custom MAPF /// planner. The planner in this scenario will take in /// starts and goals and assign a trajectory to the agents. @@ -31,6 +36,7 @@ pub trait MapfPlanner: Send + Sync + 'static { goals: &HashMap, footprints: &HashMap>, robot_ids: &[String], + map: &Map, cancellation: Arc, ) -> Result>>, Box>; } @@ -45,6 +51,7 @@ impl MapfPlanner for MockPlanner { _goals: &HashMap, _footprints: &HashMap>, robot_ids: &[String], + _map: &Map, _cancellation: Arc, ) -> Result>>, Box> { let mut plan = Vec::new(); @@ -79,74 +86,96 @@ impl MapfPlanner for PibtPlanner { goals: &HashMap, _footprints: &HashMap>, robot_ids: &[String], + map: &Map, _cancellation: Arc, ) -> Result>>, Box> { if robot_ids.is_empty() { return Ok(Vec::new()); } - let mut min_x = f32::MAX; - let mut min_y = f32::MAX; - let mut max_x = f32::MIN; - let mut max_y = f32::MIN; - - for id in robot_ids { - if let Some(odom) = starts.get(id) { - let x = odom.pose.pose.position.x as f32; - let y = odom.pose.pose.position.y as f32; - if x < min_x { - min_x = x; - } - if y < min_y { - min_y = y; - } - if x > max_x { - max_x = x; - } - if y > max_y { - max_y = y; + let use_map = + map.grid.info.width > 0 && map.grid.info.height > 0 && map.grid.info.resolution > 0.0; + + let (width, height, resolution, offset_x, offset_y, grid) = if use_map { + let w = map.grid.info.width as usize; + let h = map.grid.info.height as usize; + let r = map.grid.info.resolution; + let ox = map.grid.info.origin.position.x as f32; + let oy = map.grid.info.origin.position.y as f32; + + let mut g = vec![vec![0; h]; w]; + for x in 0..w { + for y in 0..h { + let ros_val = map.grid.data[y * w + x]; + g[x][y] = if ros_val > 50 || ros_val == -1 { 1 } else { 0 }; } } - if let Some(dest) = goals.get(id) { - if let Some(region) = dest.constraints.regions.first() { - if region.region.points.len() >= 2 { - let x = region.region.points[0]; - let y = region.region.points[1]; - if x < min_x { - min_x = x; - } - if y < min_y { - min_y = y; - } - if x > max_x { - max_x = x; - } - if y > max_y { - max_y = y; + (w, h, r, ox, oy, g) + } else { + let mut min_x = f32::MAX; + let mut min_y = f32::MAX; + let mut max_x = f32::MIN; + let mut max_y = f32::MIN; + + for id in robot_ids { + if let Some(odom) = starts.get(id) { + let x = odom.pose.pose.position.x as f32; + let y = odom.pose.pose.position.y as f32; + if x < min_x { + min_x = x; + } + if y < min_y { + min_y = y; + } + if x > max_x { + max_x = x; + } + if y > max_y { + max_y = y; + } + } + if let Some(dest) = goals.get(id) { + if let Some(region) = dest.constraints.regions.first() { + if region.region.points.len() >= 2 { + let x = region.region.points[0]; + let y = region.region.points[1]; + if x < min_x { + min_x = x; + } + if y < min_y { + min_y = y; + } + if x > max_x { + max_x = x; + } + if y > max_y { + max_y = y; + } } } } } - } - if min_x == f32::MAX { - min_x = 0.0; - min_y = 0.0; - max_x = 0.0; - max_y = 0.0; - } + if min_x == f32::MAX { + min_x = 0.0; + min_y = 0.0; + max_x = 0.0; + max_y = 0.0; + } - let padding = 10.0; - let offset_x = min_x.floor() - padding; - let offset_y = min_y.floor() - padding; + let padding = 10.0; + let ox = min_x.floor() - padding; + let oy = min_y.floor() - padding; - let width = (max_x.ceil() - offset_x + padding) as usize; - let height = (max_y.ceil() - offset_y + padding) as usize; + let w = (max_x.ceil() - ox + padding) as usize; + let h = (max_y.ceil() - oy + padding) as usize; - let width = width.max(1); - let height = height.max(1); + let w = w.max(1); + let h = h.max(1); - let grid = vec![vec![0; height]; width]; + let g = vec![vec![0; h]; w]; + (w, h, 1.0f32, ox, oy, g) + }; let mut grid_starts = Vec::new(); let mut grid_ends = Vec::new(); @@ -165,8 +194,8 @@ impl MapfPlanner for PibtPlanner { )) })?; - let sx = (odom.pose.pose.position.x as f32 - offset_x).round() as usize; - let sy = (odom.pose.pose.position.y as f32 - offset_y).round() as usize; + let sx = ((odom.pose.pose.position.x as f32 - offset_x) / resolution).round() as usize; + let sy = ((odom.pose.pose.position.y as f32 - offset_y) / resolution).round() as usize; let gx_f32; let gy_f32; @@ -188,8 +217,8 @@ impl MapfPlanner for PibtPlanner { ))); } - let gx = (gx_f32 - offset_x).round() as usize; - let gy = (gy_f32 - offset_y).round() as usize; + let gx = ((gx_f32 - offset_x) / resolution).round() as usize; + let gy = ((gy_f32 - offset_y) / resolution).round() as usize; let sx = sx.min(width - 1); let sy = sy.min(height - 1); @@ -211,8 +240,8 @@ impl MapfPlanner for PibtPlanner { let mut trajectories = vec![Vec::new(); robot_ids.len()]; for time_step in solved_paths { for (agent_idx, pos) in time_step.iter().enumerate() { - let world_x = pos.0 as f32 + offset_x; - let world_y = pos.1 as f32 + offset_y; + let world_x = pos.0 as f32 * resolution + offset_x; + let world_y = pos.1 as f32 * resolution + offset_y; trajectories[agent_idx].push(Isometry2::translation(world_x, world_y)); } } diff --git a/path_server/rmf_path_server/tests/test_map_subscription.rs b/path_server/rmf_path_server/tests/test_map_subscription.rs new file mode 100644 index 0000000..accab0b --- /dev/null +++ b/path_server/rmf_path_server/tests/test_map_subscription.rs @@ -0,0 +1,139 @@ +// Copyright 2026 Open Source Robotics Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +use mapf_post::na::Isometry2; +use rclrs::{Context, CreateBasicExecutor, IntoPrimitiveOptions, SpinOptions}; +use rmf_path_server::{start_path_server, Map, MapfPlanner}; +use ros_env::nav_msgs::msg::{OccupancyGrid, Odometry}; +use ros_env::rmf_prototype_msgs::{ + self, + msg::{Destination, Participant, ParticipantList}, +}; +use std::collections::HashMap; +use std::sync::atomic::AtomicBool; +use std::sync::{Arc, Mutex}; + +struct TestPlanner { + received_map: Arc>>, +} + +impl MapfPlanner for TestPlanner { + fn plan( + &self, + _starts: &HashMap, + _goals: &HashMap, + _footprints: &HashMap>, + robot_ids: &[String], + map: &Map, + _cancellation: Arc, + ) -> Result>>, Box> { + if let Ok(mut guard) = self.received_map.lock() { + *guard = Some(map.clone()); + } + let mut plan = Vec::new(); + for _ in 0..robot_ids.len() { + plan.push(vec![Isometry2::identity()]); + } + Ok(plan) + } +} + +#[test] +fn test_map_subscription() -> Result<(), Box> { + let context = Context::default_from_env().unwrap(); + let mut executor = context.create_basic_executor(); + let test_node = Arc::new(executor.create_node("test_node")?); + let server_node = Arc::new(executor.create_node("path_server")?); + + let received_map = Arc::new(Mutex::new(None)); + let planner = TestPlanner { + received_map: Arc::clone(&received_map), + }; + + let _path_server_guard = start_path_server(Arc::clone(&server_node), planner)?; + + // Publishers + let discovery_pub = test_node.create_publisher::( + "/destination/discovery".transient_local().reliable(), + )?; + let map_pub = + test_node.create_publisher::("/map".transient_local().reliable())?; + + let robot_id = "robot1"; + let odom_topic = format!("{}/odom", robot_id); + let odom_pub = test_node.create_publisher::(odom_topic.as_str())?; + let dest_topic = format!("{}/destination", robot_id); + let dest_pub = test_node + .create_publisher::(dest_topic.as_str().transient_local().reliable())?; + + // Publish map + let mut map_msg = OccupancyGrid::default(); + map_msg.info.resolution = 1.0; + map_msg.info.width = 10; + map_msg.info.height = 10; + map_msg.data = vec![0; 100]; + map_pub.publish(&map_msg)?; + println!("Published map"); + + // Publish discovery + let mut discovery_msg = ParticipantList::default(); + discovery_msg.participants.push(Participant { + name: robot_id.to_string(), + components: vec![], + }); + discovery_pub.publish(&discovery_msg)?; + println!("Published discovery"); + + // Publish odom + let mut odom_msg = Odometry::default(); + odom_msg.pose.pose.position.x = 1.0; + odom_msg.pose.pose.position.y = 1.0; + odom_pub.publish(&odom_msg)?; + println!("Published odom"); + + // Publish destination + let mut dest_msg = Destination::default(); + dest_msg + .constraints + .regions + .push(rmf_prototype_msgs::msg::TargetRegion { + region: rmf_prototype_msgs::msg::Region { + points: vec![5.0, 5.0], + ..Default::default() + }, + ..Default::default() + }); + dest_pub.publish(&dest_msg)?; + println!("Published destination"); + + // Wait for the planner to receive the map. + let start = std::time::Instant::now(); + let timeout = std::time::Duration::from_secs(5); + while start.elapsed() < timeout { + odom_pub.publish(&odom_msg)?; + executor.spin(SpinOptions::spin_once().timeout(std::time::Duration::from_millis(100))); + + if let Ok(guard) = received_map.lock() { + if let Some(map) = guard.as_ref() { + assert_eq!(map.grid.info.resolution, 1.0); + assert_eq!(map.grid.info.width, 10); + assert_eq!(map.grid.info.height, 10); + assert_eq!(map.grid.data, vec![0; 100]); + return Ok(()); + } + } + } + + Err("Timeout waiting for map in planner".into()) +} diff --git a/path_server/rmf_path_server/tests/test_pibt_with_map.rs b/path_server/rmf_path_server/tests/test_pibt_with_map.rs new file mode 100644 index 0000000..3d268fa --- /dev/null +++ b/path_server/rmf_path_server/tests/test_pibt_with_map.rs @@ -0,0 +1,109 @@ +// Copyright 2026 Open Source Robotics Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +use rmf_path_server::planner::{Map, MapfPlanner, PibtPlanner}; +use ros_env::nav_msgs::msg::{OccupancyGrid, Odometry}; +use ros_env::rmf_prototype_msgs::msg::Destination; +use std::collections::HashMap; +use std::sync::atomic::AtomicBool; +use std::sync::Arc; + +#[test] +fn test_pibt_planner_with_obstacle() { + let mut occupancy_grid = OccupancyGrid::default(); + occupancy_grid.info.resolution = 1.0; + occupancy_grid.info.width = 10; + occupancy_grid.info.height = 10; + occupancy_grid.info.origin.position.x = 0.0; + occupancy_grid.info.origin.position.y = 0.0; + occupancy_grid.data = vec![0; 100]; + + // Put obstacle at (5,5) -> index = 5 * 10 + 5 = 55 + occupancy_grid.data[55] = 100; // 100 is occupied + + let map = Map { + grid: occupancy_grid, + }; + + let mut starts = HashMap::new(); + let mut odom = Odometry::default(); + odom.pose.pose.position.x = 0.0; + odom.pose.pose.position.y = 5.0; + starts.insert("robot1".to_string(), odom); + + let mut goals = HashMap::new(); + let mut dest = Destination::default(); + dest.constraints + .regions + .push(ros_env::rmf_prototype_msgs::msg::TargetRegion { + region: ros_env::rmf_prototype_msgs::msg::Region { + points: vec![9.0, 5.0], + ..Default::default() + }, + ..Default::default() + }); + goals.insert("robot1".to_string(), dest); + + let footprints = HashMap::new(); + let robot_ids = vec!["robot1".to_string()]; + let planner = PibtPlanner::new(100); + + let result = planner.plan( + &starts, + &goals, + &footprints, + &robot_ids, + &map, + Arc::new(AtomicBool::new(false)), + ); + + assert!(result.is_ok(), "Planning failed: {:?}", result.err()); + let trajectories = result.unwrap(); + assert_eq!(trajectories.len(), 1); + + let path = &trajectories[0]; + assert!(!path.is_empty(), "Path is empty"); + + println!("Generated path:"); + for (i, pose) in path.iter().enumerate() { + let x = pose.translation.x; + let y = pose.translation.y; + println!(" {}: ({}, {})", i, x, y); + + // Verify that the path does not hit the obstacle at (5,5) + // Since resolution is 1.0 and origin is 0,0, grid coords are same as world coords (approx) + let dx = (x - 5.0).abs(); + let dy = (y - 5.0).abs(); + assert!( + dx > 0.1 || dy > 0.1, + "Path hits obstacle at (5,5) at step {}", + i + ); + } + + // Verify that it reached the goal (9,5) + let last_pose = path.last().unwrap(); + let lx = last_pose.translation.x; + let ly = last_pose.translation.y; + assert!( + (lx - 9.0).abs() < 0.1, + "Last pose x is {}, expected 9.0", + lx + ); + assert!( + (ly - 5.0).abs() < 0.1, + "Last pose y is {}, expected 5.0", + ly + ); +} diff --git a/path_server/rmf_path_server_demo/README.md b/path_server/rmf_path_server_demo/README.md index 9f97243..3a48d5b 100644 --- a/path_server/rmf_path_server_demo/README.md +++ b/path_server/rmf_path_server_demo/README.md @@ -44,6 +44,17 @@ full comparison. - **Send**: Click "Send Scenario" to submit goals for resolution and start planning. - The canvas will highlight the chosen goals in solid colors and fade out the unused alternatives. +### 3. Map Server Mode (Grid Map Testing) +In this mode, the launch file also spins up a ROS 2 `map_server` loaded with a custom 20x20 grid map (`demo_grid.png`) and activates it. The dashboard subscribes to `/map` and visualizes it as a background layer. + +Run the launch file: +```bash +ros2 launch rmf_path_server_demo demo_map.launch.py +``` +- Open `http://localhost:8080` in your web browser. +- The canvas will automatically fit to the 20x20 grid map. +- You can add robots and set goals as in the default mode, and see the map obstacles (a box in the center) drawn on the canvas. + ## Reservation Config Visualization When the reservation destination server loads a configuration, it republishes diff --git a/path_server/rmf_path_server_demo/launch/demo_map.launch.py b/path_server/rmf_path_server_demo/launch/demo_map.launch.py new file mode 100644 index 0000000..995b130 --- /dev/null +++ b/path_server/rmf_path_server_demo/launch/demo_map.launch.py @@ -0,0 +1,82 @@ +# Copyright 2026 Open Source Robotics Foundation +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + package_name = 'rmf_path_server_demo' + + # Map file path + map_yaml_file = os.path.join( + get_package_share_directory(package_name), + 'maps', + 'demo_grid.yaml' + ) + + # 1. Start the RMF path server + path_server = Node( + package='rmf_path_server', + executable='rmf_path_server', + name='rmf_path_server', + output='both' + ) + + # 2. Start the web spawner & web dashboard hosting server (REST & SSE Bridge) + robot_spawner = Node( + package='rmf_path_server_demo', + executable='robot_spawner', + name='robot_spawner', + output='both' + ) + + # 3. Start the plan executor + plan_executor = Node( + package='rmf_plan_executor', + executable='rmf_plan_executor', + name='rmf_plan_executor', + output='both' + ) + + # 4. Start the map server + map_server = Node( + package='nav2_map_server', + executable='map_server', + name='map_server', + output='screen', + parameters=[{'yaml_filename': map_yaml_file}] + ) + + # 5. Start the lifecycle manager to activate the map server + lifecycle_manager = Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager_map', + output='screen', + parameters=[{ + 'use_sim_time': False, + 'autostart': True, + 'node_names': ['map_server'] + }] + ) + + return LaunchDescription([ + path_server, + robot_spawner, + plan_executor, + map_server, + lifecycle_manager + ]) diff --git a/path_server/rmf_path_server_demo/maps/demo_grid.png b/path_server/rmf_path_server_demo/maps/demo_grid.png new file mode 100644 index 0000000..4e3e5ab Binary files /dev/null and b/path_server/rmf_path_server_demo/maps/demo_grid.png differ diff --git a/path_server/rmf_path_server_demo/maps/demo_grid.yaml b/path_server/rmf_path_server_demo/maps/demo_grid.yaml new file mode 100644 index 0000000..57d4a8c --- /dev/null +++ b/path_server/rmf_path_server_demo/maps/demo_grid.yaml @@ -0,0 +1,6 @@ +image: demo_grid.png +resolution: 1.0 +origin: [-10.0, -10.0, 0.0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 diff --git a/path_server/rmf_path_server_demo/maps/generate_map.py b/path_server/rmf_path_server_demo/maps/generate_map.py new file mode 100644 index 0000000..038123f --- /dev/null +++ b/path_server/rmf_path_server_demo/maps/generate_map.py @@ -0,0 +1,19 @@ +from PIL import Image, ImageDraw + +def generate_map(): + # 20x20 pixels + size = 20 + # Create a white image (255 is white/free) + img = Image.new('L', (size, size), 255) + draw = ImageDraw.Draw(img) + + # Draw a black box in the center (0 is black/occupied) + # Box from index 8 to 11 (4x4 cells in the center of 20x20) + draw.rectangle([8, 8, 11, 11], fill=0) + + # Save the image + img.save('demo_grid.png') + print("Generated demo_grid.png") + +if __name__ == '__main__': + generate_map() diff --git a/path_server/rmf_path_server_demo/package.xml b/path_server/rmf_path_server_demo/package.xml index 12b7002..c44b8d4 100644 --- a/path_server/rmf_path_server_demo/package.xml +++ b/path_server/rmf_path_server_demo/package.xml @@ -15,6 +15,8 @@ rmf_next_gen_reservation_msgs nav_msgs geometry_msgs + nav2_map_server + nav2_lifecycle_manager rosbridge_server rmf_mock_robot_sim rmf_path_server diff --git a/path_server/rmf_path_server_demo/rmf_path_server_demo/robot_spawner.py b/path_server/rmf_path_server_demo/rmf_path_server_demo/robot_spawner.py index a47acdf..5b5e6eb 100644 --- a/path_server/rmf_path_server_demo/rmf_path_server_demo/robot_spawner.py +++ b/path_server/rmf_path_server_demo/rmf_path_server_demo/robot_spawner.py @@ -24,7 +24,7 @@ from ament_index_python.packages import get_package_share_directory from geometry_msgs.msg import Point -from nav_msgs.msg import Odometry +from nav_msgs.msg import Odometry, OccupancyGrid import rclpy from rclpy.node import Node from rclpy.qos import DurabilityPolicy, HistoryPolicy, QoSProfile, ReliabilityPolicy @@ -113,6 +113,10 @@ def do_GET(self): self.send_ok_response(config or {}) return + elif self.path.startswith('/map'): + map_data = spawner_node.current_map if spawner_node else None + self.send_ok_response(map_data or {}) + return elif self.path.startswith('/config'): use_dest = spawner_node.use_destination_server if spawner_node else False self.send_ok_response({"default_radius": 0.49, "use_destination_server": use_dest}) @@ -177,6 +181,7 @@ def __init__(self): self.declare_parameter('use_destination_server', False) self.use_destination_server = self.get_parameter('use_destination_server').value + self.current_map = None self.active_processes = {} self.active_log_files = {} self.sse_clients = [] @@ -226,6 +231,13 @@ def __init__(self): qos_profile=self.reliable_transient_qos ) + # Subscribe to map topic + self.map_sub = self.create_subscription( + OccupancyGrid, + '/map', + self.map_callback, + qos_profile=self.reliable_transient_qos + ) # Get the package share directory and resolve static files path try: share_dir = get_package_share_directory('rmf_path_server_demo') @@ -307,6 +319,32 @@ def region_to_dict(region): ) self.broadcast(json.dumps(config)) + def map_callback(self, msg): + data_list = list(msg.data) + map_dict = { + "type": "map", + "info": { + "resolution": float(msg.info.resolution), + "width": int(msg.info.width), + "height": int(msg.info.height), + "origin": { + "position": { + "x": float(msg.info.origin.position.x), + "y": float(msg.info.origin.position.y), + "z": float(msg.info.origin.position.z) + }, + "orientation": { + "x": float(msg.info.origin.orientation.x), + "y": float(msg.info.origin.orientation.y), + "z": float(msg.info.origin.orientation.z), + "w": float(msg.info.origin.orientation.w) + } + } + }, + "data": data_list + } + self.current_map = map_dict + self.broadcast(json.dumps(map_dict)) # Spawns a mock simulator node def spawn_robot(self, name, x, y): if name in self.active_processes: diff --git a/path_server/rmf_path_server_demo/setup.py b/path_server/rmf_path_server_demo/setup.py index ae0149a..77eb1a0 100644 --- a/path_server/rmf_path_server_demo/setup.py +++ b/path_server/rmf_path_server_demo/setup.py @@ -31,7 +31,8 @@ (os.path.join('share', package_name, 'config'), glob('config/*.yaml')), (os.path.join('share', package_name, 'maps'), glob('maps/*.building.yaml') - + glob('maps/*.site.json')), + + glob('maps/*.site.json') + + glob('maps/demo_grid.*')), ], install_requires=['setuptools'], zip_safe=True, diff --git a/path_server/rmf_path_server_demo/www/app.js b/path_server/rmf_path_server_demo/www/app.js index 6204738..b7856d1 100644 --- a/path_server/rmf_path_server_demo/www/app.js +++ b/path_server/rmf_path_server_demo/www/app.js @@ -29,6 +29,8 @@ let pendingGoalReset = false; let eventSource = null; let mouseX = 0; let mouseY = 0; +let mapData = null; +let mapFitted = false; // Canvas config const canvas = document.getElementById('grid-canvas'); @@ -87,6 +89,14 @@ function initSSE() { reservationConfig = msg; fitViewToConfig(); console.log(`Received reservation config: ${msg.safe_sets.length} safe set(s), ${msg.parking_spots.length} parking spot(s).`); + } else if (msg.type === 'map') { + const mapChanged = !mapData || mapData.info.width !== msg.info.width || mapData.info.height !== msg.info.height; + mapData = msg; + if (mapChanged || !mapFitted) { + fitViewToConfig(); + mapFitted = true; + } + console.log(`Received map: ${msg.info.width}x${msg.info.height} @ ${msg.info.resolution}m/px`); } else if (msg.type === 'odom') { const r = robots.find(robot => robot.name === msg.name); if (r) { @@ -187,10 +197,25 @@ function reservationConfigBounds() { return Number.isFinite(minX) ? { minX, minY, maxX, maxY } : null; } -// Rescale and recenter the view so the loaded reservation config fits the -// canvas with a margin. Falls back to the default view when there is no config. +// Rescale and recenter the view so the loaded reservation config or map fits the +// canvas with a margin. Falls back to the default view when there is neither. function fitViewToConfig() { - const bounds = reservationConfigBounds(); + let bounds = reservationConfigBounds(); + + if (!bounds && mapData) { + const width = mapData.info.width; + const height = mapData.info.height; + const res = mapData.info.resolution; + const originX = mapData.info.origin.position.x; + const originY = mapData.info.origin.position.y; + bounds = { + minX: originX, + minY: originY, + maxX: originX + width * res, + maxY: originY + height * res + }; + } + if (!bounds) { scale = DEFAULT_SCALE; centerX = canvas.width / 2; @@ -198,7 +223,7 @@ function fitViewToConfig() { return; } - const margin = 1.15; // ~15% padding around the config + const margin = 1.15; // ~15% padding around the config/map const worldW = Math.max(bounds.maxX - bounds.minX, 1e-3); const worldH = Math.max(bounds.maxY - bounds.minY, 1e-3); const fitScale = Math.min( @@ -213,7 +238,6 @@ function fitViewToConfig() { centerX = canvas.width / 2 - worldCenterX * scale; centerY = canvas.height / 2 + worldCenterY * scale; } - // Helper to show instructions function showInstruction(text) { const banner = document.getElementById('instruction-banner'); @@ -691,10 +715,54 @@ function drawReservationConfig() { }); } +function drawMap() { + if (!mapData) return; + + const width = mapData.info.width; + const height = mapData.info.height; + const res = mapData.info.resolution; + const originX = mapData.info.origin.position.x; + const originY = mapData.info.origin.position.y; + + ctx.save(); + ctx.shadowBlur = 0; + + for (let y = 0; y < height; y++) { + for (let x = 0; x < width; x++) { + const idx = x + y * width; + const val = mapData.data[idx]; + + if (val === 0) { + continue; + } + + let color; + if (val === 100) { + color = 'rgba(255, 255, 255, 0.15)'; // Occupied + } else if (val === -1) { + color = 'rgba(128, 128, 128, 0.05)'; // Unknown + } else { + color = `rgba(255, 255, 255, ${val / 100 * 0.15})`; + } + + const cellWorldX = originX + x * res; + const cellWorldY = originY + y * res; + const topLeftPix = toPixel(cellWorldX, cellWorldY + res); + const cellPixelSize = res * scale; + + ctx.fillStyle = color; + ctx.fillRect(topLeftPix.x, topLeftPix.y, cellPixelSize, cellPixelSize); + } + } + + ctx.restore(); +} // Rendering grid background and actors function drawGrid() { ctx.clearRect(0, 0, canvas.width, canvas.height); + // Draw map first + drawMap(); // 1. Draw grid, aligned to the world origin. const worldLeft = (0 - centerX) / scale; const worldRight = (canvas.width - centerX) / scale; @@ -730,6 +798,7 @@ function drawGrid() { // can see every spot a robot can be placed at. if (scale * SNAP_M >= 5) { drawLines(SNAP_M, 'rgba(255, 255, 255, 0.04)'); + } // Major grid: brighter lines on the nice interval. drawLines(majorStep, 'rgba(255, 255, 255, 0.10)'); @@ -1055,9 +1124,22 @@ function fetchReservationConfig() { .catch(err => console.error('Failed to load reservation config:', err)); } +function fetchMap() { + fetch('/map') + .then(res => res.json()) + .then(data => { + if (data && data.info) { + mapData = data; + fitViewToConfig(); + console.log('Loaded map:', mapData); + } + }) + .catch(err => console.error('Failed to load map:', err)); +} // Boot application resizeCanvasDisplay(); fetchConfig(); fetchReservationConfig(); +fetchMap(); initSSE(); animationLoop();