Navigating with Stretch

Use this workflow to create a 2D map and navigate with ROS 2 Nav2.

This guide walks through creating a 2D map and running autonomous navigation on Stretch using ROS 2 Nav2. It covers the basic workflow for mapping and navigation on the robot. If you are new to Nav2, first follow the official Nav2 Getting Started guide to learn the concepts in simulation. You can also explore Navigation University for more advanced topics and tuning.

Phase 1: Generating a Map

Video: Mapping workflow

30MB

mapping.mp4

Open

1. Home the robot: Home the joints before any mobile operation.

stretch_robot_home

2. Stow the robot: Stow the arm so it stays within the base footprint.

stretch_robot_stow

Alternatively, you can use the gamepad to move the wrist and arm, or press runstop and move the arm manually. The images below show the homed position (left) and stowed position (right).

3. Launch the mapping node: In a terminal, start the offline mapping launch file.

ros2 launch stretch_nav2 offline_mapping.launch.py

Optional: Enable the camera view if you want a live feed while driving.

stretch_camera_show --opencv --right

Mapping tips

• Move slowly, especially during turns. This gives the sensors time to capture clean data.
• Watch the laser scan in RViz. If the red scan stops aligning with obstacles, the map is drifting.

4. Start teleoperation: In a second terminal, start the gamepad node and drive the robot through the area you want to map. See the gamepad teleop primer for setup and controls.

stretch_gamepad_teleop

5. Save the map: After you cover the full area, leave the mapping node running. Open a new terminal and save the map to your fleet directory. Replace <map_name> with a name such as hq_upstairs.

ros2 run nav2_map_server map_saver_cli -f ${HELLO_FLEET_PATH}/maps/<map_name>

6. Close the mapping node: Before starting navigation, stop the mapping launch file with Ctrl+C in its terminal.

Phase 2: Navigating the Generated Map

Safety and operational warnings

• Undock before navigating: Avoid initiating navigation while the robot is docked. Always undock the robot completely before sending a navigation goal.
• No navigation safety from line sensors: The navigation stack cannot detect small floor objects or stairs. Keep the area clear and stay away from stairs or ledges.

With the map saved, you can navigate autonomously.

Video: Navigation workflow

25MB

navigation.mp4

Open

1. Launch navigation: Start Nav2 and pass the YAML file for the map you just created.

By default navigation uses filter_type:=sor_ransac. On unlevel or multi-level floors, FloorRansac may latch onto a plane from another part of the room (not the floor under the robot), which distorts /scan_filtered and can confuse MPPI.

Fix: tune floor_detect_z_min, floor_detect_z_max, plane_fitting_threshold, and angle on /pointcloud_to_laserscan (stretch_core/config/dual_lidar_filter.yaml), or relaunch with filter_type:=sor and that uses a fixed-height crop instead. sor_ransac is preferred over sor because it crops less than fixed z_min.

ros2 launch stretch_nav2 navigation_mppi.launch.py map:=${HELLO_FLEET_PATH}/maps/<map_name>.yaml

2. Set the initial pose: Set the robot's initial pose so it matches the robot's actual position and orientation on the map.

You can set the pose interactively in RViz2 using 2D Pose Estimate:

1. Click at the robot's current location on the map.
2. Hold and drag in the direction the robot is facing.
3. Release to set the robot's orientation.

Alternatively, you can configure a fixed initial pose in the AMCL parameters so Nav2 initializes at the same location each time it starts. Add the following to your AMCL configuration file:

set_initial_pose: true

initial_pose:
  x: 0.06   # Example x position in the map frame (meters)
  y: -0.12  # Example y position in the map frame (meters)
  z: 0.0
  yaw: 0.12 # Example heading in radians

The values above are examples only. Replace x, y, and yaw with the robot's actual starting position and orientation in your map.

Video: Setting the initial pose in RViz2

3MB

set_initial_pose.mp4

Open

3. Send a goal: Use Nav2 Goal in RViz2 (as shown in the navigation video), or send a goal from the terminal. Make sure the position is inside the map bounds and clear of obstacles.

ros2 action send_goal /navigate_to_pose nav2_msgs/action/NavigateToPose "{pose: {header: {frame_id: 'map'}, pose: {position: {x: 1.0, y: 1.0, z: 0.0}, orientation: {w: 1.0}}}}"

Tip: Find coordinates in RViz2

To find exact coordinates, open a terminal and run ros2 topic echo /clicked_point. With that running, use the Publish Point tool in RViz2 to click anywhere on the map. The X and Y position for that location will appear in your terminal. Make sure the robot is localized first.

Troubleshooting and Debugging

1. Isolate node errors: If navigation fails at startup, add use_composition:=false to the navigation launch command. This starts nodes outside a shared container, which makes errors easier to spot.
2. Remote operation: If you use a remote desktop session, plug in a dummy HDMI adapter so the display server starts correctly.
3. Navigation does not start after setting the pose: If too much time passes before you set the robot's initial pose, the navigation stack may need to be reset. A common symptom is that the local costmap appears in RViz2, but the global costmap does not.

To recover:

1. In the Navigation panel in RViz2, click Startup.
2. Click Reset.
3. Set the robot's initial pose again.

Once the global costmap appears, the robot is ready to accept navigation goals.

Video: Resetting the Nav2 stack in RViz2

4MB

nav2_reset.mp4

Open