Skip to content

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

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.

Sign up for GitHub

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
arjo129 merged 7 commits into from
Jul 6, 2026
Merged

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

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
3139e25
Add a very rudimentary way of loading maps
arjo129 Jun 25, 2026
51be638
feat(path_server): Use Map/OccupancyGrid in PIBT planning
arjo129 Jun 25, 2026
568ffb7
Merge remote-tracking branch 'upstream/epic/next-gen-base' into arjoc…
arjo129 Jul 3, 2026
278a931
feat(path_server): Add map server mode to demo with 20x20 grid
arjo129 Jul 3, 2026
1bdf95f
Update path_server/rmf_path_server/src/lib.rs
arjo129 Jul 6, 2026
c6784fc
Update path_server/rmf_path_server/src/lib.rs
arjo129 Jul 6, 2026
db2bddc
Add accidentally deleted imports for mapf_post and rclrs
arjo129 Jul 6, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
35 changes: 27 additions & 8 deletions path_server/rmf_path_server/src/lib.rs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down Expand Up @@ -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>,

@xiyuoh xiyuoh Jul 6, 2026

Copy link
Copy Markdown
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I wonder if we can explore other ways of storing/manipulating Map (and other PlanServer members). Understood that it's expensive to clone so wrapping it with Arc is a good strategy here. Perhaps if we integrate with Bevy in the future we could make use of Resource or something. But not a blocker for this PR.

@arjo129 arjo129 Jul 6, 2026

Copy link
Copy Markdown
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Id be open to moving this to bevy_ros2 if it provides a better Developer Experience. We could put this as a future roadmap item. For now Im using the bare minimum to get an MVP out.

}

impl<P: MapfPlanner> PlanServer<P> {
Expand All @@ -77,6 +83,7 @@ impl<P: MapfPlanner> PlanServer<P> {
current_planning_session: None,
footprints,
active_plan_ids: HashMap::new(),
map: Arc::new(Map::default()),
}
}

Expand Down Expand Up @@ -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) {
Expand Down Expand Up @@ -351,6 +359,7 @@ impl<P: MapfPlanner> PlanServer<P> {
&goals,
&footprints_map,
&robot_ids,
map_clone.as_ref(),
Arc::clone(&cancellation),
) {
Ok(plan) => plan,
Expand Down Expand Up @@ -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>(
Expand All @@ -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),
Expand Down Expand Up @@ -611,5 +629,6 @@ pub fn start_path_server<P: MapfPlanner + 'static>(
replan_timer: Box::new(replan_timer),
list_subscription,
discovery_subscription,
map_subscription,
})
}
145 changes: 87 additions & 58 deletions path_server/rmf_path_server/src/planner.rs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand All @@ -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>>;
}
Expand All @@ -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();
Expand Down Expand Up @@ -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();
Expand All @@ -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;
Expand All @@ -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);
Expand All @@ -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));
}
}
Expand Down
Loading
Loading