feat:Add a very rudimentary way of loading maps (#22)

Related to #12

### Implemented feature

Adds a very rudimentary way of loading environment maps. This is by no means a final approach to loading maps, but it provides a high level way of representing them for the time being.

### Implementation description

We subscribe to the `/map` topic to listen for OccupancyGrids. It gets converted internally to a `Map` struct

### Test it

Use the launch file provided by `demo_map.launch.py` . It 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.

### GenAI Use
We follow [OSRA's policy on GenAI tools](https://github.com/openrobotics/osrf-policies-and-procedures/blob/main/OSRF%20Policy%20on%20the%20Use%20of%20Generative%20Tools%20(%E2%80%9CGenerative%20AI%E2%80%9D)%20in%20Contributions.md)

- [x] I used a GenAI tool in this PR.
- [ ] I did not use GenAI

Generated-by: Gemini Pro 3.1

---------

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>
Signed-off-by: Arjo Chakravarty <arjo129@gmail.com>
Co-authored-by: Xiyu <ohxiyu@gmail.com>

Author: arjo129

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<P: MapfPlanner> {
     pub current_planning_session: Option<u64>,
     pub footprints: Arc<std::sync::Mutex<HashMap<String, f32>>>,
     pub active_plan_ids: HashMap<String, PlanId>,
+    pub map: Arc<Map>,
 }
 
 impl<P: MapfPlanner> PlanServer<P> {
@@ -77,6 +83,7 @@ impl<P: MapfPlanner> PlanServer<P> {
             current_planning_session: None,
             footprints,
             active_plan_ids: HashMap::new(),
+            map: Arc::new(Map::default()),
         }
     }
 
@@ -322,6 +329,7 @@ impl<P: MapfPlanner> PlanServer<P> {
         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<P: MapfPlanner> PlanServer<P> {
                 &goals,
                 &footprints_map,
                 &robot_ids,
+                map_clone.as_ref(),
                 Arc::clone(&cancellation),
             ) {
                 Ok(plan) => plan,
@@ -489,6 +498,7 @@ pub struct PathServerRunning<P: MapfPlanner> {
         rclrs::WorkerSubscription<rmf_prototype_msgs::msg::ParticipantList, DiscoveryServer<P>>,
     pub discovery_subscription:
         rclrs::WorkerSubscription<rmf_prototype_msgs::msg::ParticipantList, DiscoveryServer<P>>,
+    pub map_subscription: rclrs::WorkerSubscription<OccupancyGrid, PlanServer<P>>,
 }
 
 pub fn start_path_server<P: MapfPlanner + 'static>(
@@ -502,6 +512,14 @@ pub fn start_path_server<P: MapfPlanner + 'static>(
     let destinations_worker =
         node.create_worker(PlanServer::new(node.clone(), planner, footprints));
 
+    let map_subscription = destinations_worker.create_subscription::<OccupancyGrid, _>(
+        "/map".transient_local().reliable(),
+        move |server: &mut PlanServer<P>, 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<P: MapfPlanner + 'static>(
         replan_timer: Box::new(replan_timer),
         list_subscription,
         discovery_subscription,
+        map_subscription,
     })
 }

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<String, Destination>,
         footprints: &HashMap<String, Arc<dyn mapf_post::shape::Shape>>,
         robot_ids: &[String],
+        map: &Map,
         cancellation: Arc<AtomicBool>,
     ) -> Result<Vec<Vec<Isometry2<f32>>>, Box<dyn std::error::Error>>;
 }
@@ -45,6 +51,7 @@ impl MapfPlanner for MockPlanner {
         _goals: &HashMap<String, Destination>,
         _footprints: &HashMap<String, Arc<dyn mapf_post::shape::Shape>>,
         robot_ids: &[String],
+        _map: &Map,
         _cancellation: Arc<AtomicBool>,
     ) -> Result<Vec<Vec<Isometry2<f32>>>, Box<dyn std::error::Error>> {
         let mut plan = Vec::new();
@@ -79,74 +86,96 @@ impl MapfPlanner for PibtPlanner {
         goals: &HashMap<String, Destination>,
         _footprints: &HashMap<String, Arc<dyn mapf_post::shape::Shape>>,
         robot_ids: &[String],
+        map: &Map,
         _cancellation: Arc<AtomicBool>,
     ) -> Result<Vec<Vec<Isometry2<f32>>>, Box<dyn std::error::Error>> {
         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));
             }
         }