Multi Floor Mapping

name: multi-floor-mapping description: 'Handle multi-floor mapping. Use when managing separate maps per floor, floor transitions, or multi-level navigation.'

Multi-Floor Navigation

The Challenge

Each floor of a building requires its own 2D occupancy grid map. The navigation stack must switch maps, re-localize the robot, and update costmaps when the robot changes floors. There is no standard ROS 2 package for this—it requires custom orchestration.

Coordinate System Considerations

Each floor's map has its own origin. The map frame exists on a single 2D plane, so when switching floors:

• The map → odom transform becomes invalid.
• AMCL's particle distribution is meaningless for the new floor.
• The global costmap contains stale data from the previous floor.

You must explicitly handle the transition: load the new map, reinitialize AMCL, and clear the costmaps.

Approach 1: Single Map Server with LoadMap Service

The simplest approach. One map server instance, and you call /map_server/load_map when the floor changes.

Map Server Config

map_server:
  ros__parameters:
    yaml_filename: /home/robot/maps/floor1.yaml   # Default floor at startup.
    topic_name: map
    frame_id: map

Floor Transition Sequence

1. Robot arrives at elevator/stairway transition point.
2. Detect floor change (sensor, button, or action completion).
3. Call /map_server/load_map with the new floor's YAML.
4. Publish new /initialpose for AMCL on the new floor.
5. Clear costmaps via /global_costmap/clear_entirely_global_costmap.
6. Resume navigation.

Approach 2: Multiple Map Servers

Run one map server per floor, each on a namespaced topic. A multiplexer node relays the active floor's map to /map.

# Launch:
Node(package='nav2_map_server', executable='map_server', name='map_server_floor1',
     parameters=[{'yaml_filename': '/home/robot/maps/floor1.yaml'}],
     remappings=[('map', '/floor1/map')]),
Node(package='nav2_map_server', executable='map_server', name='map_server_floor2',
     parameters=[{'yaml_filename': '/home/robot/maps/floor2.yaml'}],
     remappings=[('map', '/floor2/map')]),

Advantage: maps are preloaded, so switching is faster. Disadvantage: more memory usage, more complexity.

Floor-Specific Navigation Goals

Maintain a per-floor goal database:

floors:
  floor1:
    map_yaml: /home/robot/maps/floor1.yaml
    initial_pose: {x: 1.0, y: 2.0, yaw: 0.0}
    goals:
      kitchen: {x: 5.2, y: 3.1, yaw: 1.57}
      living_room: {x: 8.0, y: 6.5, yaw: 0.0}
      elevator_entrance: {x: 12.0, y: 1.5, yaw: 3.14}
  floor2:
    map_yaml: /home/robot/maps/floor2.yaml
    initial_pose: {x: 12.0, y: 1.5, yaw: 0.0}
    goals:
      bedroom: {x: 3.0, y: 7.0, yaw: 0.0}
      bathroom: {x: 6.5, y: 2.0, yaw: 1.57}
      elevator_entrance: {x: 12.0, y: 1.5, yaw: 3.14}

Floor Change Detection

For a home patrol robot, a deterministic trigger (e.g., complete the "ride elevator" action → switch floor) is most reliable.

Floor Transition Manager (Python)

import rclpy
from rclpy.node import Node
from nav2_msgs.srv import LoadMap
from nav2_msgs.srv import ClearEntireCostmap
from geometry_msgs.msg import PoseWithCovarianceStamped
from std_msgs.msg import String
import yaml


class FloorTransitionManager(Node):
    def __init__(self):
        super().__init__('floor_transition_manager')

        # Load floor config
        self.declare_parameter('floor_config', '/home/robot/config/floors.yaml')
        config_path = self.get_parameter('floor_config').value
        with open(config_path, 'r') as f:
            self.config = yaml.safe_load(f)

        self.current_floor = 'floor1'

        # Clients
        self.load_map_client = self.create_client(LoadMap, '/map_server/load_map')
        self.clear_global_costmap = self.create_client(
            ClearEntireCostmap, '/global_costmap/clear_entirely_global_costmap')
        self.clear_local_costmap = self.create_client(
            ClearEntireCostmap, '/local_costmap/clear_entirely_local_costmap')

        # Publisher for initial pose
        self.initial_pose_pub = self.create_publisher(
            PoseWithCovarianceStamped, '/initialpose', 10)

        # Subscribe to floor change commands
        self.create_subscription(String, '/floor_change', self.on_floor_change, 10)

        self.get_logger().info(f'Floor manager ready. Current floor: {self.current_floor}')

    def on_floor_change(self, msg: String):
        target_floor = msg.data
        if target_floor == self.current_floor:
            self.get_logger().info(f'Already on {target_floor}')
            return
        if target_floor not in self.config['floors']:
            self.get_logger().error(f'Unknown floor: {target_floor}')
            return

        self.get_logger().info(f'Switching from {self.current_floor} to {target_floor}')
        floor_cfg = self.config['floors'][target_floor]

        # Step 1: Load new map
        self._load_map(floor_cfg['map_yaml'])

        # Step 2: Clear costmaps
        self._clear_costmaps()

        # Step 3: Set initial pose on new floor
        self._set_initial_pose(floor_cfg['initial_pose'])

        self.current_floor = target_floor
        self.get_logger().info(f'Floor transition to {target_floor} complete')

    def _load_map(self, map_yaml: str):
        if not self.load_map_client.wait_for_service(timeout_sec=5.0):
            self.get_logger().error('load_map service not available')
            return
        req = LoadMap.Request()
        req.map_url = map_yaml
        future = self.load_map_client.call_async(req)
        rclpy.spin_until_future_complete(self, future, timeout_sec=10.0)
        if future.result() is not None:
            self.get_logger().info(f'Map loaded: {map_yaml}')
        else:
            self.get_logger().error('Failed to load map')

    def _clear_costmaps(self):
        for client in [self.clear_global_costmap, self.clear_local_costmap]:
            if client.wait_for_service(timeout_sec=2.0):
                req = ClearEntireCostmap.Request()
                client.call_async(req)

    def _set_initial_pose(self, pose_cfg: dict):
        import math
        msg = PoseWithCovarianceStamped()
        msg.header.frame_id = 'map'
        msg.header.stamp = self.get_clock().now().to_msg()
        msg.pose.pose.position.x = pose_cfg['x']
        msg.pose.pose.position.y = pose_cfg['y']
        yaw = pose_cfg['yaw']
        msg.pose.pose.orientation.z = math.sin(yaw / 2.0)
        msg.pose.pose.orientation.w = math.cos(yaw / 2.0)
        # Covariance: moderate uncertainty for AMCL to converge.
        msg.pose.covariance[0] = 0.25   # x variance
        msg.pose.covariance[7] = 0.25   # y variance
        msg.pose.covariance[35] = 0.07  # yaw variance
        self.initial_pose_pub.publish(msg)
        self.get_logger().info(f'Initial pose set: x={pose_cfg["x"]}, y={pose_cfg["y"]}')


def main():
    rclpy.init()
    node = FloorTransitionManager()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

Timing Considerations

After calling load_map, wait briefly before publishing initialpose. The map publication is asynchronous—AMCL needs to receive and process the new map before it can use the initial pose. A 0.5-1.0 second delay (or better, a confirmation callback) ensures correct sequencing.

AMCL Re-Initialization Details

When AMCL receives a new map and a new initial pose:

1. It rebuilds its internal likelihood field from the new OccupancyGrid.
2. It distributes particles around the initial pose using the provided covariance.
3. Over the next few scan cycles, particles converge to the correct pose.

Set the initial pose covariance large enough that AMCL can converge even if the pose estimate is slightly off (e.g., elevator drift). Typical values: 0.25 m² for x/y, 0.07 rad² for yaw.

Testing Multi-Floor

1. Create maps for each floor separately using SLAM.
2. Define initial_pose for each floor at a known landmark (e.g., elevator exit).
3. Test the transition sequence with the robot stationary at the landmark first.
4. Then test with actual floor changes.