|
| 1 | +// Copyright 2026 Open Source Robotics Foundation |
| 2 | +// |
| 3 | +// Licensed under the Apache License, Version 2.0 (the "License"); |
| 4 | +// you may not use this file except in compliance with the License. |
| 5 | +// You may obtain a copy of the License at |
| 6 | +// |
| 7 | +// http://www.apache.org/licenses/LICENSE-2.0 |
| 8 | +// |
| 9 | +// Unless required by applicable law or agreed to in writing, software |
| 10 | +// distributed under the License is distributed on an "AS IS" BASIS, |
| 11 | +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
| 12 | +// See the License for the specific language governing permissions and |
| 13 | +// limitations under the License. |
| 14 | + |
| 15 | +use mapf_post::na::Isometry2; |
| 16 | +use rclrs::{Context, CreateBasicExecutor, IntoPrimitiveOptions, SpinOptions}; |
| 17 | +use rmf_path_server::{start_path_server, Map, MapfPlanner}; |
| 18 | +use ros_env::nav_msgs::msg::{OccupancyGrid, Odometry}; |
| 19 | +use ros_env::rmf_prototype_msgs::{ |
| 20 | + self, |
| 21 | + msg::{Destination, Participant, ParticipantList}, |
| 22 | +}; |
| 23 | +use std::collections::HashMap; |
| 24 | +use std::sync::atomic::AtomicBool; |
| 25 | +use std::sync::{Arc, Mutex}; |
| 26 | + |
| 27 | +struct TestPlanner { |
| 28 | + received_map: Arc<Mutex<Option<Map>>>, |
| 29 | +} |
| 30 | + |
| 31 | +impl MapfPlanner for TestPlanner { |
| 32 | + fn plan( |
| 33 | + &self, |
| 34 | + _starts: &HashMap<String, Odometry>, |
| 35 | + _goals: &HashMap<String, Destination>, |
| 36 | + _footprints: &HashMap<String, Arc<dyn mapf_post::shape::Shape>>, |
| 37 | + robot_ids: &[String], |
| 38 | + map: &Map, |
| 39 | + _cancellation: Arc<AtomicBool>, |
| 40 | + ) -> Result<Vec<Vec<Isometry2<f32>>>, Box<dyn std::error::Error>> { |
| 41 | + if let Ok(mut guard) = self.received_map.lock() { |
| 42 | + *guard = Some(map.clone()); |
| 43 | + } |
| 44 | + let mut plan = Vec::new(); |
| 45 | + for _ in 0..robot_ids.len() { |
| 46 | + plan.push(vec![Isometry2::identity()]); |
| 47 | + } |
| 48 | + Ok(plan) |
| 49 | + } |
| 50 | +} |
| 51 | + |
| 52 | +#[test] |
| 53 | +fn test_map_subscription() -> Result<(), Box<dyn std::error::Error>> { |
| 54 | + let context = Context::default_from_env().unwrap(); |
| 55 | + let mut executor = context.create_basic_executor(); |
| 56 | + let test_node = Arc::new(executor.create_node("test_node")?); |
| 57 | + let server_node = Arc::new(executor.create_node("path_server")?); |
| 58 | + |
| 59 | + let received_map = Arc::new(Mutex::new(None)); |
| 60 | + let planner = TestPlanner { |
| 61 | + received_map: Arc::clone(&received_map), |
| 62 | + }; |
| 63 | + |
| 64 | + let _path_server_guard = start_path_server(Arc::clone(&server_node), planner)?; |
| 65 | + |
| 66 | + // Publishers |
| 67 | + let discovery_pub = test_node.create_publisher::<ParticipantList>( |
| 68 | + "/destination/discovery".transient_local().reliable(), |
| 69 | + )?; |
| 70 | + let map_pub = |
| 71 | + test_node.create_publisher::<OccupancyGrid>("/map".transient_local().reliable())?; |
| 72 | + |
| 73 | + let robot_id = "robot1"; |
| 74 | + let odom_topic = format!("{}/odom", robot_id); |
| 75 | + let odom_pub = test_node.create_publisher::<Odometry>(odom_topic.as_str())?; |
| 76 | + let dest_topic = format!("{}/destination", robot_id); |
| 77 | + let dest_pub = test_node |
| 78 | + .create_publisher::<Destination>(dest_topic.as_str().transient_local().reliable())?; |
| 79 | + |
| 80 | + // Publish map |
| 81 | + let mut map_msg = OccupancyGrid::default(); |
| 82 | + map_msg.info.resolution = 1.0; |
| 83 | + map_msg.info.width = 10; |
| 84 | + map_msg.info.height = 10; |
| 85 | + map_msg.data = vec![0; 100]; |
| 86 | + map_pub.publish(&map_msg)?; |
| 87 | + println!("Published map"); |
| 88 | + |
| 89 | + // Publish discovery |
| 90 | + let mut discovery_msg = ParticipantList::default(); |
| 91 | + discovery_msg.participants.push(Participant { |
| 92 | + name: robot_id.to_string(), |
| 93 | + components: vec![], |
| 94 | + }); |
| 95 | + discovery_pub.publish(&discovery_msg)?; |
| 96 | + println!("Published discovery"); |
| 97 | + |
| 98 | + // Publish odom |
| 99 | + let mut odom_msg = Odometry::default(); |
| 100 | + odom_msg.pose.pose.position.x = 1.0; |
| 101 | + odom_msg.pose.pose.position.y = 1.0; |
| 102 | + odom_pub.publish(&odom_msg)?; |
| 103 | + println!("Published odom"); |
| 104 | + |
| 105 | + // Publish destination |
| 106 | + let mut dest_msg = Destination::default(); |
| 107 | + dest_msg |
| 108 | + .constraints |
| 109 | + .regions |
| 110 | + .push(rmf_prototype_msgs::msg::TargetRegion { |
| 111 | + region: rmf_prototype_msgs::msg::Region { |
| 112 | + points: vec![5.0, 5.0], |
| 113 | + ..Default::default() |
| 114 | + }, |
| 115 | + ..Default::default() |
| 116 | + }); |
| 117 | + dest_pub.publish(&dest_msg)?; |
| 118 | + println!("Published destination"); |
| 119 | + |
| 120 | + // Wait for the planner to receive the map. |
| 121 | + let start = std::time::Instant::now(); |
| 122 | + let timeout = std::time::Duration::from_secs(5); |
| 123 | + while start.elapsed() < timeout { |
| 124 | + odom_pub.publish(&odom_msg)?; |
| 125 | + executor.spin(SpinOptions::spin_once().timeout(std::time::Duration::from_millis(100))); |
| 126 | + |
| 127 | + if let Ok(guard) = received_map.lock() { |
| 128 | + if let Some(map) = guard.as_ref() { |
| 129 | + assert_eq!(map.grid.info.resolution, 1.0); |
| 130 | + assert_eq!(map.grid.info.width, 10); |
| 131 | + assert_eq!(map.grid.info.height, 10); |
| 132 | + assert_eq!(map.grid.data, vec![0; 100]); |
| 133 | + return Ok(()); |
| 134 | + } |
| 135 | + } |
| 136 | + } |
| 137 | + |
| 138 | + Err("Timeout waiting for map in planner".into()) |
| 139 | +} |
0 commit comments