diff --git a/discourse/3-layered-global-occupancy-map.md b/discourse/3-layered-global-occupancy-map.md index 2985994..3dbec68 100644 --- a/discourse/3-layered-global-occupancy-map.md +++ b/discourse/3-layered-global-occupancy-map.md @@ -2,8 +2,9 @@ # Layered Global Map Observations -This post describes the observation interface for contributing temporary map -information to the next generation Open-RMF prototype. +This post describes the implemented observation interface and demos for +contributing temporary map information to the next generation Open-RMF +prototype. The goal is to let robots and perception systems publish what they currently observe without tying them to a specific central map implementation. The first @@ -19,15 +20,20 @@ observations, can be added later. same sensor snapshot * Clear-space patches have TTLs, just like obstacle patches * A source can reset its previous observations without using a TTL -* Robot-mounted sources can include the robot pose at observation time +* Robot-mounted sources include the observation-frame pose at observation time +* The Rust map server composes a static occupancy grid and active observations + into `/map` +* The Nav2 demo converts scans from three robots into clear ray sectors and occupied endpoint regions, then displays the source contributions and map * The observation messages live in `rmf_layered_map_msgs`, leaving `rmf_prototype_msgs` unchanged -# Observation Topic +# Observation Topics -Observation sources publish dynamic map observations on: +The layered map server uses these topics: +* `/map/static` - static `nav_msgs/OccupancyGrid` * `/map/region_updates` - [`MapRegionUpdate.msg`](../rmf_layered_map_msgs/msg/MapRegionUpdate.msg) +* `/map` - composed `nav_msgs/OccupancyGrid` The topic is an event stream. Updates are not latched because expired observations should not be replayed to a restarted map service as if they were @@ -105,6 +111,28 @@ sensor-local regions with their sensor pose, while synthetic sources whose regions are already global can use the identity pose. The first implementation accepts point and axis-aligned rectangle regions. +# Layered Map Server + +`rmf_layered_map_server` keeps the static occupancy grid separate from dynamic +observations and publishes their composition on `/map`. It validates source +timestamps and frames, ignores updates that are older than the latest accepted +update from the same source, supports source resets, and removes observations +after their TTL expires. + +The server applies clear patches before obstacle patches from the same update. +During composition, obstacle observations win over clear observations so +occupied space is not accidentally erased by another active source. + +# Three-Robot Nav2 Demo + +The launch file starts three robots in different free corners of the warehouse, cycles them through fixed Nav2 goals, and spawns one deterministic Gazebo box near each robot. Each observation node subscribes to its robot's local `sensor_msgs/LaserScan`, filters invalid or out-of-range returns, and publishes free-space ray sectors as convex polygons and occupied endpoints as point regions. + +Each scan adds temporary clear and obstacle patches to a rolling observation history. The map server rasterizes clear sectors before obstacle endpoints so a measured hit remains occupied. Active obstacle evidence still wins over clear evidence until its TTL expires. An update may set `reset_source` so the new scan replaces all active observations from that source instead of being added to its history. The observation-frame pose is recorded in the shared `map` frame so the map server can transform the scan-local regions before rasterizing them. + +The demo launches Nav2 localization, planning, and control for the fixed goal loops, but does not launch RMF planning. Robot-local RViz windows show Nav2 state, while another RViz window displays the combined global `/map`. The combined view overlays incoming region updates as colored markers grouped by source, with the same retention behavior as the map contributions. + +The scan-to-region conversion uses one convex sector per sampled beam instead of expanding a Bresenham line into many point regions. Scan sampling reduces the number of sectors, publication throttling limits the snapshot rate, and the observation range limits represented returns. The TTL controls how quickly stale observations expire. Each publisher logs its input beams and clear and obstacle regions so message density, update rate, and visual fidelity can be compared. The current demo remains a 2D occupancy approximation; it does not fuse probabilistic confidence or compress adjacent sectors into larger regions. + # Example Flow A local costmap or LiDAR observation node can publish a replacement snapshot by: @@ -118,22 +146,27 @@ A local costmap or LiDAR observation node can publish a replacement snapshot by: 5. Giving each patch a TTL long enough to survive normal publication jitter but short enough to decay when the observation is no longer refreshed. -The map service should ignore snapshots from a source if their timestamp is -older than a newer snapshot that has already been accepted. This prevents a late -clear/reset message from removing obstacle information that came from a newer +The map server ignores snapshots from a source if their timestamp is older than +a newer snapshot that has already been accepted. This prevents a late clear or +reset message from removing obstacle information that came from a newer observation. # Implemented Test Coverage -The first server tests cover: +The committed server and demo tests cover: * composing obstacle regions over a static planning grid * point regions with non-zero map origins and non-1.0 resolutions * transforming robot-local regions into the global map frame * out-of-bounds regions, malformed point arrays, and unsupported region types -* rejecting updates without a timestamp +* rejecting updates without a timestamp or in a different global frame * pruning expired observations by TTL * clear and obstacle patches in the same update * late older snapshots being ignored * reset updates removing observations from the same source and map * multiple robot sources being stitched into one composed grid +* converting laser beams into clear sectors and occupied endpoints +* filtering invalid and out-of-range laser returns +* preserving original beam angles when scan points are sampled +* converting point and rectangle updates into source-colored markers +* retaining markers until TTL expiry and replacing them on source reset diff --git a/map_server/README.md b/map_server/README.md index 883cd14..ee71057 100644 --- a/map_server/README.md +++ b/map_server/README.md @@ -11,3 +11,5 @@ ros2 launch rmf_layered_map_server_demo demo.launch.py ``` Use `use_rviz:=False` for a headless run. + +See [`rmf_layered_map_server_demo/README.md`](rmf_layered_map_server_demo/README.md) for the three-robot Nav2 observation demo. diff --git a/map_server/rmf_layered_map_server/src/lib.rs b/map_server/rmf_layered_map_server/src/lib.rs index f5529ce..50214d6 100644 --- a/map_server/rmf_layered_map_server/src/lib.rs +++ b/map_server/rmf_layered_map_server/src/lib.rs @@ -494,9 +494,14 @@ fn region_validation_error(region: &Region) -> Option { Region::HINT_AXIS_ALIGNED_RECTANGLE if region.points.len() < 4 => { Some("axis-aligned rectangle must contain at least two x/y pairs".to_string()) } - Region::HINT_POINT | Region::HINT_AXIS_ALIGNED_RECTANGLE => None, + Region::HINT_CONVEX_POLYGON if region.points.len() < 6 => { + Some("convex polygon must contain at least three x/y pairs".to_string()) + } + Region::HINT_POINT + | Region::HINT_AXIS_ALIGNED_RECTANGLE + | Region::HINT_CONVEX_POLYGON => None, hint => Some(format!( - "unsupported region hint {}; expected a point or axis-aligned rectangle", + "unsupported region hint {}; expected a point, axis-aligned rectangle, or convex polygon", hint )), } @@ -833,6 +838,13 @@ mod tests { } } + fn convex_polygon(points: Vec) -> Region { + Region { + hint: Region::HINT_CONVEX_POLYGON, + points, + } + } + fn patch(update_type: u8, regions: Vec) -> MapRegionPatch { MapRegionPatch { update_type, @@ -896,6 +908,24 @@ mod tests { assert_eq!(composed.data[0], 0); } + #[test] + fn convex_clear_regions_are_composed_over_static_map() { + let mut map = LayeredMap::default(); + map.set_static_map(static_grid(5, 5, 100)); + + assert!(map.ingest_region_update( + update( + MapRegionPatch::UPDATE_CLEAR, + vec![convex_polygon(vec![0.0, 0.0, 4.0, 0.0, 4.0, 4.0])] + ), + 0, + )); + + let composed = map.compose().unwrap(); + assert_eq!(composed.data[1 * 5 + 3], 0); + assert_eq!(composed.data[3 * 5 + 1], 100); + } + #[test] fn robot_local_regions_are_transformed_into_the_map_frame() { let mut map = LayeredMap::default(); diff --git a/map_server/rmf_layered_map_server_demo/README.md b/map_server/rmf_layered_map_server_demo/README.md new file mode 100644 index 0000000..136dac7 --- /dev/null +++ b/map_server/rmf_layered_map_server_demo/README.md @@ -0,0 +1,33 @@ +# Layered Map Server Demos + +This package contains two demonstrations of the layered map server. + +## TTL Smoke Test + +This demo publishes a synthetic static grid and a temporary rectangle with a five-second TTL: + +```bash +ros2 launch rmf_layered_map_server_demo demo.launch.py +``` + +Use `use_rviz:=False` for a headless run. + +## Three-Robot Nav2 Observation Demo + +This demo combines laser observations from three moving Nav2 robots in the global `/map`: + +```bash +ros2 launch rmf_layered_map_server_demo nav2_observations.launch.py +``` + +![Three robot laser observations in the combined global map](docs/images/nav2_observations.png) + +By default, the demo opens three robot-local RViz windows and one combined-map view, moves the robots between fixed goals, spawns demo obstacles, and retains scans for ten seconds. + +* Set `use_nav2_rviz:=False` or `use_global_rviz:=False` to disable either RViz view. +* Set `move_robots:=False` to disable robot movement. +* Set `spawn_clutter:=False` to use the unmodified warehouse world. +* Set `reset_source:=True` to show only the latest scan from each robot. +* Use `map` and `params_file` to override the warehouse map and shared Nav2 parameters. +* `beam_stride:=1` and `publish_period_sec:=0.5` control scan sampling. +* `max_observation_range:=2.5` and `ttl_sec:=10.0` control range and retention. diff --git a/map_server/rmf_layered_map_server_demo/docs/images/nav2_observations.png b/map_server/rmf_layered_map_server_demo/docs/images/nav2_observations.png new file mode 100644 index 0000000..0d5aad7 Binary files /dev/null and b/map_server/rmf_layered_map_server_demo/docs/images/nav2_observations.png differ diff --git a/map_server/rmf_layered_map_server_demo/launch/nav2_observations.launch.py b/map_server/rmf_layered_map_server_demo/launch/nav2_observations.launch.py new file mode 100644 index 0000000..5f18d63 --- /dev/null +++ b/map_server/rmf_layered_map_server_demo/launch/nav2_observations.launch.py @@ -0,0 +1,251 @@ +# 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.actions import ( + DeclareLaunchArgument, + EmitEvent, + ExecuteProcess, + RegisterEventHandler, + TimerAction, +) +from launch.conditions import IfCondition +from launch.event_handlers import OnProcessExit +from launch.events import Shutdown +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from launch_ros.parameter_descriptions import ParameterValue + + +ROBOTS = ( + ('robot0', -12.0, -21.0, 0.7854), + ('robot1', 10.0, -21.0, 2.3562), + ('robot2', -12.0, 21.0, -0.7854), +) + +ROBOT_GOALS = { + 'robot0': ((-12.0, -16.0, 1.5708), (-12.0, -21.0, -1.5708)), + 'robot1': ((10.0, -16.0, 1.5708), (10.0, -21.0, -1.5708)), + 'robot2': ((-12.0, 16.0, -1.5708), (-12.0, 21.0, 1.5708)), +} + + +def robots_argument(): + """Format the three warehouse-corner robot poses for Nav2 bringup.""" + return '; '.join( + f'{name}={{x: {x}, y: {y}, yaw: {yaw}}}' + for name, x, y, yaw in ROBOTS + ) + + +def generate_launch_description(): + demo_share = get_package_share_directory('rmf_layered_map_server_demo') + nav2_share = get_package_share_directory('sp_demo_nav2_bringup') + + map_file = LaunchConfiguration('map') + params_file = LaunchConfiguration('params_file') + use_nav2_rviz = LaunchConfiguration('use_nav2_rviz') + use_global_rviz = LaunchConfiguration('use_global_rviz') + move_robots = LaunchConfiguration('move_robots') + spawn_clutter = LaunchConfiguration('spawn_clutter') + beam_stride = LaunchConfiguration('beam_stride') + publish_period_sec = LaunchConfiguration('publish_period_sec') + ttl_sec = LaunchConfiguration('ttl_sec') + reset_source = LaunchConfiguration('reset_source') + max_observation_range = LaunchConfiguration('max_observation_range') + + declarations = [ + DeclareLaunchArgument( + 'map', + default_value=os.path.join(nav2_share, 'maps', 'warehouse.yaml'), + description='Static map used by each Nav2 robot.', + ), + DeclareLaunchArgument( + 'params_file', + default_value=os.path.join( + nav2_share, 'params', 'nav2_multirobot_params_all.yaml' + ), + description='Nav2 parameters shared by the three robots.', + ), + DeclareLaunchArgument( + 'use_nav2_rviz', + default_value='True', + description='Whether to open one local Nav2 RViz window per robot.', + ), + DeclareLaunchArgument( + 'use_global_rviz', + default_value='True', + description='Whether to open the combined global-map RViz window.', + ), + DeclareLaunchArgument( + 'move_robots', + default_value='True', + description='Whether to cycle the robots through example Nav2 goals.', + ), + DeclareLaunchArgument( + 'spawn_clutter', + default_value='True', + description='Whether to spawn one deterministic obstacle near each robot.', + ), + DeclareLaunchArgument( + 'beam_stride', + default_value='1', + description='Convert every Nth laser beam into map regions.', + ), + DeclareLaunchArgument( + 'publish_period_sec', + default_value='0.5', + description='Minimum period between region snapshots from each robot.', + ), + DeclareLaunchArgument( + 'ttl_sec', + default_value='10.0', + description='Lifetime of each robot observation snapshot.', + ), + DeclareLaunchArgument( + 'reset_source', + default_value='False', + description='Whether each snapshot replaces the source history.', + ), + DeclareLaunchArgument( + 'max_observation_range', + default_value='2.5', + description='Maximum laser return distance represented globally.', + ), + ] + + # ParseMultiRobotPose in the Nav2 demo reads sys.argv directly. + # Launch a managed child process so the robot list reaches that parser. + nav2_simulation = ExecuteProcess( + cmd=[ + 'ros2', + 'launch', + 'sp_demo_nav2_bringup', + 'cloned_multi_tb3_simulation_launch.py', + f'robots:={robots_argument()}', + ['map:=', map_file], + ['params_file:=', params_file], + ['use_rviz:=', use_nav2_rviz], + ['use_navigation:=', move_robots], + ], + output='screen', + ) + + layered_map_server = Node( + package='rmf_layered_map_server', + executable='rmf_layered_map_server', + output='screen', + remappings=[('/map/static', '/robot0/inner/map')], + ) + + region_visualizer = Node( + condition=IfCondition(use_global_rviz), + package='rmf_layered_map_server_demo', + executable='region_update_visualizer', + output='screen', + ) + + observation_nodes = [] + goal_nodes = [] + for robot_name, _, _, _ in ROBOTS: + observation_nodes.append( + Node( + package='rmf_layered_map_server_demo', + executable='scan_region_publisher', + namespace=f'{robot_name}/inner', + output='screen', + parameters=[{ + 'robot_name': robot_name, + 'map_frame': 'map', + 'map_name': 'warehouse', + 'scan_topic': f'/{robot_name}/inner/scan', + 'beam_stride': ParameterValue(beam_stride, value_type=int), + 'publish_period_sec': ParameterValue( + publish_period_sec, value_type=float + ), + 'ttl_sec': ParameterValue(ttl_sec, value_type=float), + 'reset_source': ParameterValue( + reset_source, value_type=bool + ), + 'max_observation_range': ParameterValue( + max_observation_range, value_type=float + ), + }], + remappings=[('/tf', 'tf'), ('/tf_static', 'tf_static')], + ) + ) + goal_nodes.append( + Node( + condition=IfCondition(move_robots), + package='rmf_layered_map_server_demo', + executable='nav2_goal_publisher', + namespace=f'{robot_name}/inner', + output='screen', + parameters=[{ + 'use_sim_time': True, + 'waypoints': [ + value + for waypoint in ROBOT_GOALS[robot_name] + for value in waypoint + ], + }], + ) + ) + + clutter_spawner = TimerAction( + period=5.0, + actions=[ + Node( + condition=IfCondition(spawn_clutter), + package='rmf_layered_map_server_demo', + executable='layered_map_demo_clutter_spawner', + output='screen', + ), + ], + ) + + global_rviz = Node( + condition=IfCondition(use_global_rviz), + package='rviz2', + executable='rviz2', + name='layered_global_map_rviz', + arguments=[ + '-d', + os.path.join(demo_share, 'rviz', 'nav2_observations.rviz'), + ], + parameters=[{'use_sim_time': False}], + output='screen', + ) + global_rviz_exit_handler = RegisterEventHandler( + condition=IfCondition(use_global_rviz), + event_handler=OnProcessExit( + target_action=global_rviz, + on_exit=EmitEvent(event=Shutdown(reason='global RViz exited')), + ), + ) + + return LaunchDescription([ + *declarations, + nav2_simulation, + layered_map_server, + region_visualizer, + *observation_nodes, + *goal_nodes, + clutter_spawner, + global_rviz, + global_rviz_exit_handler, + ]) diff --git a/map_server/rmf_layered_map_server_demo/package.xml b/map_server/rmf_layered_map_server_demo/package.xml index 6ad8738..0ec46c5 100644 --- a/map_server/rmf_layered_map_server_demo/package.xml +++ b/map_server/rmf_layered_map_server_demo/package.xml @@ -9,14 +9,27 @@ ament_python + action_msgs rclpy ament_index_python geometry_msgs + gz_tools_vendor + launch + launch_ros nav_msgs + nav2_msgs rmf_layered_map_msgs rmf_layered_map_server rmf_prototype_msgs rviz2 + sensor_msgs + sp_demo_nav2_bringup + tf2_ros_py + visualization_msgs + + ament_copyright + ament_pep257 + python3-pytest ament_python diff --git a/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/demo_clutter_spawner.py b/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/demo_clutter_spawner.py new file mode 100644 index 0000000..42fa6b9 --- /dev/null +++ b/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/demo_clutter_spawner.py @@ -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. + +import subprocess + +import rclpy +from rclpy.node import Node + + +DEMO_OBSTACLES = ( + ('layered_map_obstacle_0', -11.0, -20.0), + ('layered_map_obstacle_1', 9.0, -20.0), + ('layered_map_obstacle_2', -11.0, 20.0), +) + + +def box_sdf(name, x, y): + """Return a small static box model for a deterministic laser target.""" + return ( + "" + f"" + f'{x} {y} 0.5 0 0 0' + 'true' + "" + "" + '0.3 0.3 1.0' + '' + "" + '0.3 0.3 1.0' + '' + '' + '' + '' + ) + + +class DemoClutterSpawner(Node): + """Retry deterministic Gazebo box creation until the world is ready.""" + + def __init__(self): + super().__init__('layered_map_demo_clutter_spawner') + self.world_name = self.declare_parameter('world_name', 'warehouse').value + self.pending = list(DEMO_OBSTACLES) + self.timer = self.create_timer(2.0, self.spawn_pending) + + def spawn_pending(self): + for obstacle in list(self.pending): + name, x, y = obstacle + request = f'sdf: "{box_sdf(name, x, y)}"' + try: + result = subprocess.run( + [ + 'gz', + 'service', + '-s', + f'/world/{self.world_name}/create', + '--reqtype', + 'gz.msgs.EntityFactory', + '--reptype', + 'gz.msgs.Boolean', + '--timeout', + '1000', + '--req', + request, + ], + capture_output=True, + check=False, + text=True, + timeout=3.0, + ) + except (OSError, subprocess.TimeoutExpired) as error: + self.get_logger().warning(f'Waiting for Gazebo: {error}') + return + + if result.returncode != 0: + detail = result.stderr.strip() or result.stdout.strip() + self.get_logger().warning(f'Waiting for Gazebo: {detail}') + return + + self.pending.remove(obstacle) + self.get_logger().info(f'Spawned {name} at ({x}, {y})') + + if not self.pending: + self.get_logger().info('All layered-map demo obstacles are ready') + self.timer.cancel() + rclpy.shutdown() + + +def main(): + rclpy.init() + node = DemoClutterSpawner() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.try_shutdown() diff --git a/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/nav2_goal_publisher.py b/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/nav2_goal_publisher.py new file mode 100644 index 0000000..649cb98 --- /dev/null +++ b/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/nav2_goal_publisher.py @@ -0,0 +1,147 @@ +# 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. + +from math import cos, sin + +from action_msgs.msg import GoalStatus +from geometry_msgs.msg import PoseStamped, PoseWithCovarianceStamped +from nav2_msgs.action import NavigateToPose +import rclpy +from rclpy.action import ActionClient +from rclpy.node import Node +from rclpy.qos import DurabilityPolicy, QoSProfile, ReliabilityPolicy + + +def parse_waypoints(values): + """Parse a flat sequence of x, y, yaw waypoint triples.""" + if not values or len(values) % 3 != 0: + raise ValueError('waypoints must contain one or more x, y, yaw triples') + + return [tuple(values[index:index + 3]) for index in range(0, len(values), 3)] + + +def goal_pose(x, y, yaw, stamp): + """Create a map-frame pose for a Nav2 goal.""" + pose = PoseStamped() + pose.header.frame_id = 'map' + pose.header.stamp = stamp + pose.pose.position.x = x + pose.pose.position.y = y + pose.pose.orientation.z = sin(yaw / 2.0) + pose.pose.orientation.w = cos(yaw / 2.0) + return pose + + +def advance_waypoint(current, waypoint_count, status): + """Advance after a successful goal and otherwise retry the same waypoint.""" + if status == GoalStatus.STATUS_SUCCEEDED: + return (current + 1) % waypoint_count + return current + + +class Nav2GoalPublisher(Node): + """Cycle through demo waypoints whenever Nav2 completes a goal.""" + + def __init__(self): + super().__init__('nav2_goal_publisher') + self.waypoints = parse_waypoints( + self.declare_parameter('waypoints', [0.0, 0.0, 0.0]).value + ) + self.next_waypoint = 0 + self.goal_in_progress = False + self.waiting_logged = False + self.localized = False + self.action_client = ActionClient(self, NavigateToPose, 'navigate_to_pose') + pose_qos = QoSProfile( + depth=1, + durability=DurabilityPolicy.TRANSIENT_LOCAL, + reliability=ReliabilityPolicy.RELIABLE, + ) + self.pose_subscription = self.create_subscription( + PoseWithCovarianceStamped, + 'amcl_pose', + self.receive_pose, + pose_qos, + ) + self.timer = self.create_timer(1.0, self.send_next_goal) + + def receive_pose(self, _): + self.localized = True + + def send_next_goal(self): + if self.goal_in_progress: + return + + if not self.localized or not self.action_client.server_is_ready(): + if not self.waiting_logged: + self.get_logger().info('Waiting for Nav2 localization and navigation') + self.waiting_logged = True + return + + self.waiting_logged = False + x, y, yaw = self.waypoints[self.next_waypoint] + goal = NavigateToPose.Goal() + goal.pose = goal_pose(x, y, yaw, self.get_clock().now().to_msg()) + self.goal_in_progress = True + future = self.action_client.send_goal_async(goal) + future.add_done_callback(self.goal_response) + self.get_logger().info(f'Sending demo goal ({x:.1f}, {y:.1f})') + + def goal_response(self, future): + try: + goal_handle = future.result() + except Exception as error: + self.get_logger().error(f'Failed to send demo goal: {error}') + self.goal_in_progress = False + return + + if not goal_handle.accepted: + self.get_logger().warning('Nav2 rejected the demo goal; retrying') + self.goal_in_progress = False + return + + result = goal_handle.get_result_async() + result.add_done_callback(self.goal_finished) + + def goal_finished(self, future): + try: + status = future.result().status + except Exception as error: + self.get_logger().error(f'Demo goal failed: {error}') + else: + if status == GoalStatus.STATUS_SUCCEEDED: + self.get_logger().info('Demo goal reached') + else: + self.get_logger().warning( + f'Demo goal finished with status {status}; retrying' + ) + self.next_waypoint = advance_waypoint( + self.next_waypoint, + len(self.waypoints), + status, + ) + + self.goal_in_progress = False + + +def main(): + rclpy.init() + node = Nav2GoalPublisher() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.try_shutdown() diff --git a/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/region_update_visualizer.py b/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/region_update_visualizer.py new file mode 100644 index 0000000..bae3500 --- /dev/null +++ b/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/region_update_visualizer.py @@ -0,0 +1,298 @@ +# 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. + +from math import isfinite + +from geometry_msgs.msg import Point +import rclpy +from rclpy.duration import Duration +from rclpy.node import Node +from rclpy.qos import QoSProfile, ReliabilityPolicy +from rmf_layered_map_msgs.msg import MapRegionPatch, MapRegionUpdate +from rmf_prototype_msgs.msg import Region +from visualization_msgs.msg import Marker, MarkerArray + + +SOURCE_COLORS = ( + (0.96, 0.26, 0.21), + (0.18, 0.80, 0.44), + (0.20, 0.60, 0.98), + (1.00, 0.65, 0.15), + (0.61, 0.35, 0.71), + (0.10, 0.74, 0.80), +) +CLEAR_COLOR_BLEND = 0.5 +CLEAR_MARKER_Z = 0.04 +OBSTACLE_MARKER_Z = 0.08 + + +def _point(x, y, z): + point = Point() + point.x = float(x) + point.y = float(y) + point.z = z + return point + + +def _marker_lifetime(patch, update, default_ttl_sec): + for ttl_sec in ( + patch.ttl_sec, + update.source.default_ttl_sec, + default_ttl_sec, + ): + if isfinite(ttl_sec) and ttl_sec > 0.0: + return Duration(seconds=ttl_sec).to_msg() + return Duration().to_msg() + + +def _patch_color(color, update_type): + if update_type == MapRegionPatch.UPDATE_OBSTACLE: + return color + return tuple( + channel + (1.0 - channel) * CLEAR_COLOR_BLEND + for channel in color + ) + + +def _new_marker(update, patch, marker_id, marker_type, color, default_ttl_sec): + color = _patch_color(color, patch.update_type) + marker = Marker() + marker.header.frame_id = update.source.header.frame_id + marker.ns = update.source.source_id + marker.id = marker_id + marker.type = marker_type + marker.action = Marker.ADD + marker.pose = update.source.robot_pose + marker.frame_locked = False + marker.lifetime = _marker_lifetime(patch, update, default_ttl_sec) + marker.color.r = color[0] + marker.color.g = color[1] + marker.color.b = color[2] + marker.color.a = ( + 0.85 if patch.update_type == MapRegionPatch.UPDATE_OBSTACLE else 0.35 + ) + return marker + + +def _markers_from_patch(update, patch, first_id, color, default_ttl_sec): + point_regions = [] + region_lines = [] + z = ( + OBSTACLE_MARKER_Z + if patch.update_type == MapRegionPatch.UPDATE_OBSTACLE + else CLEAR_MARKER_Z + ) + + for region in patch.regions: + if region.hint == Region.HINT_POINT and len(region.points) == 2: + point_regions.append(_point(region.points[0], region.points[1], z)) + elif ( + region.hint == Region.HINT_AXIS_ALIGNED_RECTANGLE + and len(region.points) >= 4 + and len(region.points) % 2 == 0 + ): + xs = region.points[0::2] + ys = region.points[1::2] + min_x, max_x = min(xs), max(xs) + min_y, max_y = min(ys), max(ys) + corners = ( + _point(min_x, min_y, z), + _point(max_x, min_y, z), + _point(max_x, max_y, z), + _point(min_x, max_y, z), + ) + for index, corner in enumerate(corners): + region_lines.extend((corner, corners[(index + 1) % len(corners)])) + elif ( + region.hint == Region.HINT_CONVEX_POLYGON + and len(region.points) >= 6 + and len(region.points) % 2 == 0 + ): + corners = tuple( + _point(x, y, z) + for x, y in zip(region.points[0::2], region.points[1::2]) + ) + for index, corner in enumerate(corners): + region_lines.extend((corner, corners[(index + 1) % len(corners)])) + + markers = [] + marker_id = first_id + if point_regions: + marker = _new_marker( + update, + patch, + marker_id, + Marker.POINTS, + color, + default_ttl_sec, + ) + marker.scale.x = 0.12 + marker.scale.y = 0.12 + marker.points = point_regions + markers.append(marker) + marker_id += 1 + + if region_lines: + marker = _new_marker( + update, + patch, + marker_id, + Marker.LINE_LIST, + color, + default_ttl_sec, + ) + marker.scale.x = 0.08 + marker.points = region_lines + markers.append(marker) + + return markers + + +class RegionMarkerState: + """Convert region updates into persistent per-source RViz marker actions.""" + + def __init__(self, default_ttl_sec=30.0): + self.default_ttl_sec = default_ttl_sec + self.markers_by_source = {} + self.next_ids = {} + self.latest_stamps = {} + self.source_colors = {} + + def apply_update(self, update): + """Return marker actions that apply one region update to the RViz state.""" + stamp_nsec = ( + update.source.header.stamp.sec * 1_000_000_000 + + update.source.header.stamp.nanosec + ) + if stamp_nsec == 0 or not update.source.header.frame_id: + return MarkerArray() + + key = (update.source.source_id, update.source.map_name) + latest_stamp = self.latest_stamps.get(key) + if latest_stamp is not None and stamp_nsec < latest_stamp: + return MarkerArray() + + color = self.source_colors.setdefault( + key, + SOURCE_COLORS[len(self.source_colors) % len(SOURCE_COLORS)], + ) + marker_array = MarkerArray() + self.markers_by_source[key] = [ + marker + for marker in self.markers_by_source.get(key, ()) + if marker[3] is None or marker[3] > stamp_nsec + ] + + if update.reset_source: + for namespace, marker_id, frame_id, _ in self.markers_by_source[key]: + marker = Marker() + marker.header.frame_id = frame_id + marker.ns = namespace + marker.id = marker_id + marker.action = Marker.DELETE + marker_array.markers.append(marker) + self.markers_by_source[key] = [] + self.next_ids[key] = 0 + + next_id = self.next_ids.get(key, 0) + new_markers = [] + for patch in update.patches: + if patch.update_type not in ( + MapRegionPatch.UPDATE_CLEAR, + MapRegionPatch.UPDATE_OBSTACLE, + ): + continue + patch_markers = _markers_from_patch( + update, + patch, + next_id, + color, + self.default_ttl_sec, + ) + new_markers.extend(patch_markers) + next_id += len(patch_markers) + + marker_array.markers.extend(new_markers) + self.next_ids[key] = next_id + self.markers_by_source[key].extend( + ( + marker.ns, + marker.id, + marker.header.frame_id, + stamp_nsec + marker.lifetime.sec * 1_000_000_000 + + marker.lifetime.nanosec + if marker.lifetime.sec > 0 or marker.lifetime.nanosec > 0 + else None, + ) + for marker in new_markers + ) + + if new_markers or update.reset_source: + self.latest_stamps[key] = stamp_nsec + + return marker_array + + +class RegionUpdateVisualizer(Node): + """Visualize active map-region contributions as colored RViz markers.""" + + def __init__(self): + super().__init__('region_update_visualizer') + input_topic = self.declare_parameter( + 'input_topic', '/map/region_updates' + ).value + output_topic = self.declare_parameter( + 'output_topic', '/map/region_markers' + ).value + default_ttl_sec = self.declare_parameter( + 'default_ttl_sec', 30.0 + ).value + + reliable_qos = QoSProfile( + depth=10, + reliability=ReliabilityPolicy.RELIABLE, + ) + self.state = RegionMarkerState(default_ttl_sec) + self.publisher = self.create_publisher( + MarkerArray, + output_topic, + reliable_qos, + ) + self.subscription = self.create_subscription( + MapRegionUpdate, + input_topic, + self.visualize_update, + reliable_qos, + ) + self.get_logger().info( + f'Visualizing {input_topic} as {output_topic}' + ) + + def visualize_update(self, update): + """Publish the marker actions for an incoming region update.""" + marker_array = self.state.apply_update(update) + if marker_array.markers: + self.publisher.publish(marker_array) + + +def main(): + rclpy.init() + node = RegionUpdateVisualizer() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.try_shutdown() diff --git a/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/scan_conversion.py b/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/scan_conversion.py new file mode 100644 index 0000000..c069235 --- /dev/null +++ b/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/scan_conversion.py @@ -0,0 +1,72 @@ +# 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. + +from math import cos, isfinite, sin + + +def scan_regions( + ranges, + angle_min, + angle_increment, + range_min, + range_max, + max_observation_range, + beam_stride, +): + """Convert laser beams into clear sectors and occupied endpoints.""" + if beam_stride < 1: + raise ValueError('beam_stride must be at least one') + if not isfinite(angle_min) or not isfinite(angle_increment): + raise ValueError('scan angles must be finite') + if angle_increment == 0.0: + raise ValueError('angle_increment must not be zero') + + effective_max = range_max + if max_observation_range > 0.0: + effective_max = min(effective_max, max_observation_range) + + clear_polygons = [] + obstacle_points = [] + half_width = abs(angle_increment) * beam_stride / 2.0 + for index in range(0, len(ranges), beam_stride): + distance = ranges[index] + has_obstacle = False + if isfinite(distance): + if distance < range_min or distance > range_max: + continue + clear_distance = min(distance, effective_max) + has_obstacle = distance <= effective_max + elif distance > 0.0 and isfinite(effective_max): + clear_distance = effective_max + else: + continue + if not isfinite(clear_distance) or clear_distance <= 0.0: + continue + + angle = angle_min + index * angle_increment + clear_polygons.append(( + 0.0, + 0.0, + clear_distance * cos(angle - half_width), + clear_distance * sin(angle - half_width), + clear_distance * cos(angle + half_width), + clear_distance * sin(angle + half_width), + )) + if has_obstacle: + obstacle_points.append(( + distance * cos(angle), + distance * sin(angle), + )) + + return clear_polygons, obstacle_points diff --git a/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/scan_region_publisher.py b/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/scan_region_publisher.py new file mode 100644 index 0000000..96a0571 --- /dev/null +++ b/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/scan_region_publisher.py @@ -0,0 +1,224 @@ +# 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 rclpy +from rclpy.node import Node +from rclpy.qos import ( + qos_profile_sensor_data, + QoSProfile, + ReliabilityPolicy, +) +from rclpy.time import Time +from rmf_layered_map_msgs.msg import ( + MapObservationSource, + MapRegionPatch, + MapRegionUpdate, +) +from rmf_prototype_msgs.msg import Region +from sensor_msgs.msg import LaserScan +from tf2_ros import Buffer, TransformException, TransformListener + +from .scan_conversion import scan_regions + + +def make_scan_patches(clear_polygons, obstacle_points, ttl_sec): + """Build clear and obstacle patches for one converted laser scan.""" + patches = [] + if clear_polygons: + clear_patch = MapRegionPatch() + clear_patch.update_type = MapRegionPatch.UPDATE_CLEAR + clear_patch.occupancy_value = 0 + clear_patch.ttl_sec = ttl_sec + for polygon in clear_polygons: + region = Region() + region.hint = Region.HINT_CONVEX_POLYGON + region.points = list(polygon) + clear_patch.regions.append(region) + patches.append(clear_patch) + + if obstacle_points: + obstacle_patch = MapRegionPatch() + obstacle_patch.update_type = MapRegionPatch.UPDATE_OBSTACLE + obstacle_patch.occupancy_value = 100 + obstacle_patch.ttl_sec = ttl_sec + for x, y in obstacle_points: + region = Region() + region.hint = Region.HINT_POINT + region.points = [x, y] + obstacle_patch.regions.append(region) + patches.append(obstacle_patch) + + return patches + + +class ScanRegionPublisher(Node): + """Publish one robot's laser scan as clear and obstacle regions.""" + + def __init__(self): + super().__init__('scan_region_publisher') + + self.robot_name = self.declare_parameter('robot_name', '').value + self.map_frame = self.declare_parameter('map_frame', 'map').value + self.map_name = self.declare_parameter('map_name', 'warehouse').value + self.scan_topic = self.declare_parameter('scan_topic', 'scan').value + self.ttl_sec = self.declare_parameter('ttl_sec', 10.0).value + self.reset_source = self.declare_parameter( + 'reset_source', False + ).value + self.publish_period_sec = self.declare_parameter( + 'publish_period_sec', 0.5 + ).value + self.max_observation_range = self.declare_parameter( + 'max_observation_range', 2.5 + ).value + self.beam_stride = self.declare_parameter('beam_stride', 1).value + + if not self.robot_name: + raise ValueError('robot_name must not be empty') + if self.ttl_sec <= 0.0: + raise ValueError('ttl_sec must be positive') + if self.publish_period_sec < 0.0: + raise ValueError('publish_period_sec must not be negative') + if self.beam_stride < 1: + raise ValueError('beam_stride must be at least one') + + self.source_id = f'{self.robot_name}/scan' + self.last_publish_stamp_sec = None + self.pending_scan = None + self.publish_count = 0 + self.transform_failure_count = 0 + self.tf_buffer = Buffer() + self.tf_listener = TransformListener(self.tf_buffer, self) + + reliable_qos = QoSProfile( + depth=10, + reliability=ReliabilityPolicy.RELIABLE, + ) + self.region_update_publisher = self.create_publisher( + MapRegionUpdate, + '/map/region_updates', + reliable_qos, + ) + self.scan_subscription = self.create_subscription( + LaserScan, + self.scan_topic, + self.publish_scan, + qos_profile_sensor_data, + ) + self.scan_retry_timer = self.create_timer(0.05, self.publish_pending_scan) + + self.get_logger().info( + f'Converting {self.scan_topic} into region updates for ' + f'{self.robot_name} (stride={self.beam_stride}, ' + f'period={self.publish_period_sec:.2f}s)' + ) + + def publish_scan(self, scan): + self.pending_scan = scan + self.publish_pending_scan() + + def publish_pending_scan(self): + scan = self.pending_scan + if scan is None: + return + + stamp_sec = scan.header.stamp.sec + scan.header.stamp.nanosec / 1e9 + if scan.header.stamp.sec == 0 and scan.header.stamp.nanosec == 0: + self.get_logger().warning('Ignoring scan with a zero timestamp') + self.pending_scan = None + return + + if self.last_publish_stamp_sec is not None: + elapsed = stamp_sec - self.last_publish_stamp_sec + if 0.0 <= elapsed < self.publish_period_sec: + self.pending_scan = None + return + + try: + transform = self.tf_buffer.lookup_transform( + self.map_frame, + scan.header.frame_id, + Time.from_msg(scan.header.stamp), + ) + except TransformException as error: + self.transform_failure_count += 1 + if self.transform_failure_count == 1 or ( + self.transform_failure_count % 20 == 0 + ): + self.get_logger().warning( + f'Cannot transform {scan.header.frame_id} to ' + f'{self.map_frame} at the scan timestamp: {error}' + ) + return + + self.pending_scan = None + clear_polygons, obstacle_points = scan_regions( + scan.ranges, + scan.angle_min, + scan.angle_increment, + scan.range_min, + scan.range_max, + self.max_observation_range, + self.beam_stride, + ) + update = self.make_update(transform, clear_polygons, obstacle_points) + self.region_update_publisher.publish(update) + self.last_publish_stamp_sec = stamp_sec + self.publish_count += 1 + + if self.publish_count == 1 or self.publish_count % 20 == 0: + self.get_logger().info( + f'Published {len(clear_polygons)} clear regions and ' + f'{len(obstacle_points)} obstacle regions from ' + f'{len(scan.ranges)} laser beams' + ) + + def make_update(self, transform, clear_polygons, obstacle_points): + update = MapRegionUpdate() + update.reset_source = self.reset_source + update.source = MapObservationSource() + # The Rust map server currently uses a system-time clock. + # Keep TTL and update ordering in that clock while using the scan stamp for TF. + update.source.header.stamp = self.get_clock().now().to_msg() + update.source.header.frame_id = self.map_frame + update.source.source_id = self.source_id + update.source.robot_name = self.robot_name + update.source.map_name = self.map_name + update.source.default_ttl_sec = self.ttl_sec + + translation = transform.transform.translation + rotation = transform.transform.rotation + update.source.robot_pose.position.x = translation.x + update.source.robot_pose.position.y = translation.y + update.source.robot_pose.position.z = translation.z + update.source.robot_pose.orientation = rotation + + update.patches = make_scan_patches( + clear_polygons, + obstacle_points, + self.ttl_sec, + ) + return update + + +def main(): + rclpy.init() + node = ScanRegionPublisher() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.try_shutdown() diff --git a/map_server/rmf_layered_map_server_demo/rviz/nav2_observations.rviz b/map_server/rmf_layered_map_server_demo/rviz/nav2_observations.rviz new file mode 100644 index 0000000..3babd04 --- /dev/null +++ b/map_server/rmf_layered_map_server_demo/rviz/nav2_observations.rviz @@ -0,0 +1,117 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Combined Global Map1 + - /Region Contributions1 + Splitter Ratio: 0.5 + Tree Height: 595 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.03 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 60 + Reference Frame: + Value: true + - Alpha: 1 + Binary representation: false + Binary threshold: 100 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: true + Enabled: true + Name: Combined Global Map + Topic: + Depth: 1 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map_updates + Use Timestamp: false + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: Region Contributions + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map/region_markers + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/TopDownOrtho + Enable Stereo Rendering: + Stereo Eye Separation: 0.06 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.01 + Scale: 55 + Target Frame: + Value: TopDownOrtho (rviz_default_plugins) + X: 0 + Y: 0 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 900 + Hide Left Dock: false + Hide Right Dock: false + Views: + collapsed: false + Width: 1200 + X: 180 + Y: 90 diff --git a/map_server/rmf_layered_map_server_demo/setup.py b/map_server/rmf_layered_map_server_demo/setup.py index 28bb275..39c6bc3 100644 --- a/map_server/rmf_layered_map_server_demo/setup.py +++ b/map_server/rmf_layered_map_server_demo/setup.py @@ -39,7 +39,15 @@ tests_require=['pytest'], entry_points={ 'console_scripts': [ + 'layered_map_demo_clutter_spawner = ' + 'rmf_layered_map_server_demo.demo_clutter_spawner:main', 'layered_map_demo_publisher = rmf_layered_map_server_demo.demo_publisher:main', + 'nav2_goal_publisher = ' + 'rmf_layered_map_server_demo.nav2_goal_publisher:main', + 'region_update_visualizer = ' + 'rmf_layered_map_server_demo.region_update_visualizer:main', + 'scan_region_publisher = ' + 'rmf_layered_map_server_demo.scan_region_publisher:main', ], }, ) diff --git a/map_server/rmf_layered_map_server_demo/test/test_copyright.py b/map_server/rmf_layered_map_server_demo/test/test_copyright.py new file mode 100644 index 0000000..f87f331 --- /dev/null +++ b/map_server/rmf_layered_map_server_demo/test/test_copyright.py @@ -0,0 +1,23 @@ +# 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. + +from ament_copyright.main import main +import pytest + + +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0 diff --git a/map_server/rmf_layered_map_server_demo/test/test_nav2_goal_publisher.py b/map_server/rmf_layered_map_server_demo/test/test_nav2_goal_publisher.py new file mode 100644 index 0000000..e3173e8 --- /dev/null +++ b/map_server/rmf_layered_map_server_demo/test/test_nav2_goal_publisher.py @@ -0,0 +1,58 @@ +# 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. + +from math import pi + +from action_msgs.msg import GoalStatus +from geometry_msgs.msg import PoseStamped +import pytest + +from rmf_layered_map_server_demo.nav2_goal_publisher import advance_waypoint +from rmf_layered_map_server_demo.nav2_goal_publisher import goal_pose +from rmf_layered_map_server_demo.nav2_goal_publisher import parse_waypoints + + +def test_parse_waypoints_groups_xyz_triples(): + assert parse_waypoints([1.0, 2.0, 3.0, 4.0, 5.0, 6.0]) == [ + (1.0, 2.0, 3.0), + (4.0, 5.0, 6.0), + ] + + +@pytest.mark.parametrize('values', [[], [1.0], [1.0, 2.0]]) +def test_parse_waypoints_rejects_incomplete_triples(values): + with pytest.raises(ValueError, match='x, y, yaw'): + parse_waypoints(values) + + +def test_goal_pose_uses_map_frame_and_yaw_orientation(): + stamp = PoseStamped().header.stamp + stamp.sec = 1 + stamp.nanosec = 2 + + pose = goal_pose(3.0, 4.0, pi, stamp) + + assert pose.header.frame_id == 'map' + assert pose.header.stamp == stamp + assert pose.pose.position.x == 3.0 + assert pose.pose.position.y == 4.0 + assert pose.pose.orientation.z == pytest.approx(1.0) + assert pose.pose.orientation.w == pytest.approx(0.0, abs=1e-10) + + +def test_waypoint_advances_only_after_success(): + assert advance_waypoint(0, 2, GoalStatus.STATUS_SUCCEEDED) == 1 + assert advance_waypoint(1, 2, GoalStatus.STATUS_SUCCEEDED) == 0 + assert advance_waypoint(0, 2, GoalStatus.STATUS_ABORTED) == 0 + assert advance_waypoint(1, 2, GoalStatus.STATUS_CANCELED) == 1 diff --git a/map_server/rmf_layered_map_server_demo/test/test_pep257.py b/map_server/rmf_layered_map_server_demo/test/test_pep257.py new file mode 100644 index 0000000..1b15a96 --- /dev/null +++ b/map_server/rmf_layered_map_server_demo/test/test_pep257.py @@ -0,0 +1,23 @@ +# 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. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0 diff --git a/map_server/rmf_layered_map_server_demo/test/test_region_update_visualizer.py b/map_server/rmf_layered_map_server_demo/test/test_region_update_visualizer.py new file mode 100644 index 0000000..fe8df1f --- /dev/null +++ b/map_server/rmf_layered_map_server_demo/test/test_region_update_visualizer.py @@ -0,0 +1,172 @@ +# 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 pytest + +from rmf_layered_map_msgs.msg import MapRegionPatch, MapRegionUpdate +from rmf_layered_map_server_demo.region_update_visualizer import ( + RegionMarkerState, + SOURCE_COLORS, +) +from rmf_prototype_msgs.msg import Region +from visualization_msgs.msg import Marker + + +def _update(stamp_sec=1, reset_source=True): + update = MapRegionUpdate() + update.source.header.stamp.sec = stamp_sec + update.source.header.frame_id = 'map' + update.source.source_id = 'robot0/scan' + update.source.robot_name = 'robot0' + update.source.map_name = 'warehouse' + update.source.robot_pose.position.x = 2.0 + update.source.robot_pose.position.y = 3.0 + update.source.robot_pose.orientation.w = 1.0 + update.source.default_ttl_sec = 4.0 + update.reset_source = reset_source + return update + + +def _region(hint, points): + region = Region() + region.hint = hint + region.points = points + return region + + +def test_visualizes_point_and_rectangle_regions_in_the_source_pose(): + update = _update() + patch = MapRegionPatch() + patch.update_type = MapRegionPatch.UPDATE_OBSTACLE + patch.regions = [ + _region(Region.HINT_POINT, [1.0, 2.0]), + _region(Region.HINT_AXIS_ALIGNED_RECTANGLE, [-1.0, -2.0, 1.0, 2.0]), + ] + update.patches = [patch] + + markers = RegionMarkerState().apply_update(update).markers + + assert len(markers) == 2 + assert markers[0].type == Marker.POINTS + assert markers[0].pose.position.x == 2.0 + assert markers[0].pose.position.y == 3.0 + assert [(point.x, point.y) for point in markers[0].points] == [(1.0, 2.0)] + assert markers[0].lifetime.sec == 4 + assert markers[1].type == Marker.LINE_LIST + assert len(markers[1].points) == 8 + + +def test_visualizes_clear_ray_sectors_as_polygon_outlines(): + update = _update() + patch = MapRegionPatch() + patch.update_type = MapRegionPatch.UPDATE_CLEAR + patch.regions = [ + _region(Region.HINT_CONVEX_POLYGON, [0.0, 0.0, 1.0, -0.1, 1.0, 0.1]), + ] + update.patches = [patch] + + markers = RegionMarkerState().apply_update(update).markers + + assert len(markers) == 1 + assert markers[0].type == Marker.LINE_LIST + assert markers[0].color.a == pytest.approx(0.35) + assert len(markers[0].points) == 6 + + +def test_visualizes_clear_regions_with_a_lighter_source_color(): + update = _update() + clear_patch = MapRegionPatch() + clear_patch.update_type = MapRegionPatch.UPDATE_CLEAR + clear_patch.regions = [_region(Region.HINT_POINT, [1.0, 2.0])] + obstacle_patch = MapRegionPatch() + obstacle_patch.update_type = MapRegionPatch.UPDATE_OBSTACLE + obstacle_patch.regions = [_region(Region.HINT_POINT, [1.0, 2.0])] + update.patches = [clear_patch, obstacle_patch] + + clear_marker, obstacle_marker = RegionMarkerState().apply_update( + update + ).markers + clear_color = ( + clear_marker.color.r, + clear_marker.color.g, + clear_marker.color.b, + ) + obstacle_color = ( + obstacle_marker.color.r, + obstacle_marker.color.g, + obstacle_marker.color.b, + ) + + assert obstacle_color == pytest.approx(SOURCE_COLORS[0]) + assert clear_color == pytest.approx( + tuple((channel + 1.0) / 2.0 for channel in SOURCE_COLORS[0]) + ) + assert clear_marker.points[0].z < obstacle_marker.points[0].z + + +def test_reset_deletes_the_previous_source_markers_before_replacing_them(): + state = RegionMarkerState() + first = _update(stamp_sec=1) + first_patch = MapRegionPatch() + first_patch.update_type = MapRegionPatch.UPDATE_OBSTACLE + first_patch.regions = [_region(Region.HINT_POINT, [1.0, 2.0])] + first.patches = [first_patch] + state.apply_update(first) + + replacement = _update(stamp_sec=2) + replacement_patch = MapRegionPatch() + replacement_patch.update_type = MapRegionPatch.UPDATE_OBSTACLE + replacement_patch.regions = [_region(Region.HINT_POINT, [3.0, 4.0])] + replacement.patches = [replacement_patch] + + markers = state.apply_update(replacement).markers + + assert [marker.action for marker in markers] == [Marker.DELETE, Marker.ADD] + assert markers[0].ns == markers[1].ns == 'robot0/scan' + assert markers[0].id == markers[1].id == 0 + assert [(point.x, point.y) for point in markers[1].points] == [(3.0, 4.0)] + + +def test_older_update_does_not_replace_newer_visualization(): + state = RegionMarkerState() + state.apply_update(_update(stamp_sec=2)) + + markers = state.apply_update(_update(stamp_sec=1)).markers + + assert markers == [] + + +def test_non_reset_updates_retain_only_unexpired_marker_bookkeeping(): + state = RegionMarkerState() + first = _update(stamp_sec=1, reset_source=False) + first_patch = MapRegionPatch() + first_patch.update_type = MapRegionPatch.UPDATE_OBSTACLE + first_patch.regions = [_region(Region.HINT_POINT, [1.0, 2.0])] + first.patches = [first_patch] + state.apply_update(first) + + second = _update(stamp_sec=2, reset_source=False) + second_patch = MapRegionPatch() + second_patch.update_type = MapRegionPatch.UPDATE_OBSTACLE + second_patch.regions = [_region(Region.HINT_POINT, [3.0, 4.0])] + second.patches = [second_patch] + markers = state.apply_update(second).markers + + assert [marker.action for marker in markers] == [Marker.ADD] + assert markers[0].id == 1 + assert len(state.markers_by_source[('robot0/scan', 'warehouse')]) == 2 + + state.apply_update(_update(stamp_sec=6, reset_source=False)) + + assert state.markers_by_source[('robot0/scan', 'warehouse')] == [] diff --git a/map_server/rmf_layered_map_server_demo/test/test_scan_conversion.py b/map_server/rmf_layered_map_server_demo/test/test_scan_conversion.py new file mode 100644 index 0000000..f2c631a --- /dev/null +++ b/map_server/rmf_layered_map_server_demo/test/test_scan_conversion.py @@ -0,0 +1,102 @@ +# 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. + +from math import cos, inf, nan, pi, sin + +import pytest + +from rmf_layered_map_server_demo.scan_conversion import scan_regions + + +def test_scan_filters_invalid_and_out_of_range_returns(): + clear_polygons, obstacle_points = scan_regions( + [nan, inf, 0.05, 1.0, 3.0], + angle_min=0.0, + angle_increment=pi / 2.0, + range_min=0.1, + range_max=10.0, + max_observation_range=2.5, + beam_stride=1, + ) + + assert len(clear_polygons) == 3 + assert len(obstacle_points) == 1 + assert obstacle_points[0][0] == pytest.approx(0.0, abs=1e-6) + assert obstacle_points[0][1] == pytest.approx(-1.0) + + +def test_scan_creates_a_clear_sector_around_each_sampled_beam(): + clear_polygons, obstacle_points = scan_regions( + [1.0], + angle_min=0.0, + angle_increment=0.2, + range_min=0.1, + range_max=5.0, + max_observation_range=2.5, + beam_stride=1, + ) + + assert obstacle_points == [(1.0, 0.0)] + assert clear_polygons[0] == pytest.approx(( + 0.0, + 0.0, + cos(0.1), + -sin(0.1), + cos(0.1), + sin(0.1), + )) + + +def test_scan_without_a_return_clears_to_the_observation_limit(): + clear_polygons, obstacle_points = scan_regions( + [inf], + angle_min=0.0, + angle_increment=0.2, + range_min=0.1, + range_max=10.0, + max_observation_range=2.5, + beam_stride=1, + ) + + assert obstacle_points == [] + assert max(clear_polygons[0]) == pytest.approx(2.5 * cos(0.1)) + + +def test_scan_stride_preserves_original_beam_angles(): + _, obstacle_points = scan_regions( + [1.0, 1.0, 1.0, 1.0], + angle_min=0.0, + angle_increment=pi / 2.0, + range_min=0.1, + range_max=5.0, + max_observation_range=0.0, + beam_stride=2, + ) + + assert len(obstacle_points) == 2 + assert obstacle_points[0] == pytest.approx((1.0, 0.0), abs=1e-6) + assert obstacle_points[1] == pytest.approx((-1.0, 0.0), abs=1e-6) + + +def test_scan_rejects_invalid_stride(): + with pytest.raises(ValueError, match='beam_stride'): + scan_regions( + [], + angle_min=0.0, + angle_increment=0.1, + range_min=0.1, + range_max=5.0, + max_observation_range=2.5, + beam_stride=0, + ) diff --git a/map_server/rmf_layered_map_server_demo/test/test_scan_region_publisher.py b/map_server/rmf_layered_map_server_demo/test/test_scan_region_publisher.py new file mode 100644 index 0000000..b6394d0 --- /dev/null +++ b/map_server/rmf_layered_map_server_demo/test/test_scan_region_publisher.py @@ -0,0 +1,34 @@ +# 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. + +from rmf_layered_map_msgs.msg import MapRegionPatch +from rmf_layered_map_server_demo.scan_region_publisher import make_scan_patches +from rmf_prototype_msgs.msg import Region + + +def test_scan_patches_clear_rays_before_marking_obstacle_endpoints(): + patches = make_scan_patches( + [(0.0, 0.0, 1.0, -0.1, 1.0, 0.1)], + [(1.0, 0.0)], + ttl_sec=5.0, + ) + + assert [patch.update_type for patch in patches] == [ + MapRegionPatch.UPDATE_CLEAR, + MapRegionPatch.UPDATE_OBSTACLE, + ] + assert patches[0].occupancy_value == 0 + assert patches[0].regions[0].hint == Region.HINT_CONVEX_POLYGON + assert patches[1].occupancy_value == 100 + assert patches[1].regions[0].hint == Region.HINT_POINT diff --git a/nav2_integration/sp_demo_nav2_bringup/launch/bringup_launch.py b/nav2_integration/sp_demo_nav2_bringup/launch/bringup_launch.py index eee19f6..352987a 100644 --- a/nav2_integration/sp_demo_nav2_bringup/launch/bringup_launch.py +++ b/nav2_integration/sp_demo_nav2_bringup/launch/bringup_launch.py @@ -49,6 +49,7 @@ def generate_launch_description(): use_respawn = LaunchConfiguration('use_respawn') log_level = LaunchConfiguration('log_level') use_localization = LaunchConfiguration('use_localization') + use_navigation = LaunchConfiguration('use_navigation') # Map fully qualified names to relative ones so the node's namespace can be prepended. # In case of the transforms (tf), currently, there doesn't seem to be a better alternative @@ -105,6 +106,11 @@ def generate_launch_description(): description='Whether to enable localization or not' ) + declare_use_navigation_cmd = DeclareLaunchArgument( + 'use_navigation', default_value='True', + description='Whether to enable the Nav2 planning and control stack' + ) + declare_use_sim_time_cmd = DeclareLaunchArgument( 'use_sim_time', default_value='false', @@ -186,6 +192,7 @@ def generate_launch_description(): PythonLaunchDescriptionSource( os.path.join(launch_dir, 'navigation_launch.py') ), + condition=IfCondition(use_navigation), launch_arguments={ 'namespace': namespace, 'use_sim_time': use_sim_time, @@ -217,6 +224,7 @@ def generate_launch_description(): ld.add_action(declare_use_respawn_cmd) ld.add_action(declare_log_level_cmd) ld.add_action(declare_use_localization_cmd) + ld.add_action(declare_use_navigation_cmd) # Add the actions to launch all of the navigation nodes ld.add_action(bringup_cmd_group) diff --git a/nav2_integration/sp_demo_nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py b/nav2_integration/sp_demo_nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py index 55b7b3b..0b06c61 100644 --- a/nav2_integration/sp_demo_nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py +++ b/nav2_integration/sp_demo_nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py @@ -63,6 +63,7 @@ def generate_launch_description(): rviz_config_file = LaunchConfiguration('rviz_config') use_robot_state_pub = LaunchConfiguration('use_robot_state_pub') use_rviz = LaunchConfiguration('use_rviz') + use_navigation = LaunchConfiguration('use_navigation') log_settings = LaunchConfiguration('log_settings', default='true') # Declare the launch arguments @@ -108,6 +109,12 @@ def generate_launch_description(): 'use_rviz', default_value='True', description='Whether to start RVIZ' ) + declare_use_navigation_cmd = DeclareLaunchArgument( + 'use_navigation', + default_value='True', + description='Whether to enable the Nav2 planning and control stack', + ) + # Start Gazebo with plugin providing the robot spawning service world_sdf = tempfile.mktemp(prefix='nav2_', suffix='.sdf') world_sdf_xacro = ExecuteProcess( @@ -126,7 +133,7 @@ def generate_launch_description(): # Define commands for launching the navigation instances bringup_cmd_group = [] - for robot_name in robots_list: + for robot_index, robot_name in enumerate(robots_list): init_pose = robots_list[robot_name] namespace = robot_name + '/inner' group = GroupAction( @@ -166,6 +173,14 @@ def generate_launch_description(): 'use_simulator': 'False', 'headless': 'False', 'use_robot_state_pub': use_robot_state_pub, + 'use_navigation': use_navigation, + 'clock_topic': TextSubstitution( + text=( + '/clock' + if robot_index == 0 + else f'/{namespace}/clock' + ) + ), 'x_pose': TextSubstitution(text=str(init_pose['x'])), 'y_pose': TextSubstitution(text=str(init_pose['y'])), 'z_pose': TextSubstitution(text=str(init_pose['z'])), @@ -199,6 +214,7 @@ def generate_launch_description(): ld.add_action(declare_autostart_cmd) ld.add_action(declare_rviz_config_file_cmd) ld.add_action(declare_use_robot_state_pub_cmd) + ld.add_action(declare_use_navigation_cmd) # initial localization node for robot_name in robots_list: diff --git a/nav2_integration/sp_demo_nav2_bringup/launch/tb3_simulation_launch.py b/nav2_integration/sp_demo_nav2_bringup/launch/tb3_simulation_launch.py index db1964f..fb32f2f 100644 --- a/nav2_integration/sp_demo_nav2_bringup/launch/tb3_simulation_launch.py +++ b/nav2_integration/sp_demo_nav2_bringup/launch/tb3_simulation_launch.py @@ -23,22 +23,24 @@ from launch.actions import ( DeclareLaunchArgument, ExecuteProcess, + GroupAction, IncludeLaunchDescription, OpaqueFunction, RegisterEventHandler, ) -from launch.conditions import IfCondition +from launch.conditions import IfCondition, UnlessCondition from launch.event_handlers import OnShutdown from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, PythonExpression -from launch_ros.actions import Node +from launch_ros.actions import Node, SetRemap def generate_launch_description(): # Get the launch directory bringup_dir = get_package_share_directory('nav2_bringup') launch_dir = os.path.join(bringup_dir, 'launch') + prototype_bringup_dir = get_package_share_directory('sp_demo_nav2_bringup') sim_dir = get_package_share_directory('nav2_minimal_tb3_sim') # Create the launch configuration variables @@ -51,6 +53,8 @@ def generate_launch_description(): autostart = LaunchConfiguration('autostart') use_composition = LaunchConfiguration('use_composition') use_respawn = LaunchConfiguration('use_respawn') + use_navigation = LaunchConfiguration('use_navigation') + clock_topic = LaunchConfiguration('clock_topic') # Launch configuration variables specific to simulation rviz_config_file = LaunchConfiguration('rviz_config_file') @@ -122,6 +126,18 @@ def generate_launch_description(): description='Whether to respawn if a node crashes. Applied when composition is disabled.', ) + declare_use_navigation_cmd = DeclareLaunchArgument( + 'use_navigation', + default_value='True', + description='Whether to enable the Nav2 planning and control stack', + ) + + declare_clock_topic_cmd = DeclareLaunchArgument( + 'clock_topic', + default_value='/clock', + description='ROS topic for the Gazebo clock bridge', + ) + declare_rviz_config_file_cmd = DeclareLaunchArgument( 'rviz_config_file', default_value=os.path.join(bringup_dir, 'rviz', 'nav2_default_view.rviz'), @@ -194,6 +210,29 @@ def generate_launch_description(): bringup_cmd = IncludeLaunchDescription( PythonLaunchDescriptionSource(os.path.join(launch_dir, 'bringup_launch.py')), + condition=IfCondition(use_navigation), + launch_arguments={ + 'namespace': namespace, + 'use_namespace': use_namespace, + 'slam': slam, + 'map': map_yaml_file, + 'use_sim_time': use_sim_time, + 'params_file': params_file, + 'autostart': autostart, + 'use_composition': use_composition, + 'use_respawn': use_respawn, + 'use_navigation': use_navigation, + }.items(), + ) + localization_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join( + prototype_bringup_dir, + 'launch', + 'bringup_launch.py', + ) + ), + condition=UnlessCondition(use_navigation), launch_arguments={ 'namespace': namespace, 'use_namespace': use_namespace, @@ -204,6 +243,7 @@ def generate_launch_description(): 'autostart': autostart, 'use_composition': use_composition, 'use_respawn': use_respawn, + 'use_navigation': 'False', }.items(), ) # The SDF file for the world is a xacro file because we wanted to @@ -236,19 +276,22 @@ def generate_launch_description(): launch_arguments={'gz_args': ['-v4 -g ']}.items(), ) - gz_robot = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(sim_dir, 'launch', 'spawn_tb3.launch.py')), - launch_arguments={'namespace': namespace, - 'use_sim_time': use_sim_time, - 'robot_name': robot_name, - 'robot_sdf': robot_sdf, - 'x_pose': pose['x'], - 'y_pose': pose['y'], - 'z_pose': pose['z'], - 'roll': pose['R'], - 'pitch': pose['P'], - 'yaw': pose['Y']}.items()) + gz_robot = GroupAction([ + SetRemap(src='/clock', dst=clock_topic), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(sim_dir, 'launch', 'spawn_tb3.launch.py')), + launch_arguments={'namespace': namespace, + 'use_sim_time': use_sim_time, + 'robot_name': robot_name, + 'robot_sdf': robot_sdf, + 'x_pose': pose['x'], + 'y_pose': pose['y'], + 'z_pose': pose['z'], + 'roll': pose['R'], + 'pitch': pose['P'], + 'yaw': pose['Y']}.items()) + ]) # Create the launch description and populate ld = LaunchDescription() @@ -272,6 +315,8 @@ def generate_launch_description(): ld.add_action(declare_robot_name_cmd) ld.add_action(declare_robot_sdf_cmd) ld.add_action(declare_use_respawn_cmd) + ld.add_action(declare_use_navigation_cmd) + ld.add_action(declare_clock_topic_cmd) ld.add_action(world_sdf_xacro) ld.add_action(remove_temp_sdf_file) @@ -283,5 +328,6 @@ def generate_launch_description(): ld.add_action(start_robot_state_publisher_cmd) ld.add_action(rviz_cmd) ld.add_action(bringup_cmd) + ld.add_action(localization_cmd) return ld diff --git a/nav2_integration/sp_demo_nav2_bringup/package.xml b/nav2_integration/sp_demo_nav2_bringup/package.xml index ec55ca5..ec7ac03 100644 --- a/nav2_integration/sp_demo_nav2_bringup/package.xml +++ b/nav2_integration/sp_demo_nav2_bringup/package.xml @@ -29,6 +29,7 @@ ros_gz_sim rviz2 slam_toolbox + spatio_temporal_partition_layer xacro nav2_minimal_tb4_sim nav2_minimal_tb3_sim diff --git a/nav2_integration/sp_demo_nav2_bringup/params/nav2_multirobot_params_all.yaml b/nav2_integration/sp_demo_nav2_bringup/params/nav2_multirobot_params_all.yaml index cbece86..31861b8 100644 --- a/nav2_integration/sp_demo_nav2_bringup/params/nav2_multirobot_params_all.yaml +++ b/nav2_integration/sp_demo_nav2_bringup/params/nav2_multirobot_params_all.yaml @@ -320,7 +320,7 @@ collision_monitor: FootprintApproach: type: "polygon" action_type: "approach" - footprint_topic: "/local_costmap/published_footprint" + footprint_topic: "local_costmap/published_footprint" time_before_collision: 2.0 simulation_time_step: 0.1 min_points: 6 diff --git a/rmf_layered_map_msgs/msg/MapRegionPatch.msg b/rmf_layered_map_msgs/msg/MapRegionPatch.msg index 14dc00b..d735041 100644 --- a/rmf_layered_map_msgs/msg/MapRegionPatch.msg +++ b/rmf_layered_map_msgs/msg/MapRegionPatch.msg @@ -24,6 +24,6 @@ float64 ttl_sec # 2D geometric observation patches in the robot-local observation frame. The # map service transforms them into source.header.frame_id using -# source.robot_pose. The first prototype supports point and axis-aligned -# rectangle regions. +# source.robot_pose. The first prototype supports point, axis-aligned +# rectangle, and convex polygon regions. rmf_prototype_msgs/Region[] regions