Nav2 and OMPL for autonomous navigation

1. Activity Identity

2. Learning Objectives and Competences

3. Prerequisites

• You can start the FOSSBotEduSim container and the simulator and inspect ROS 2 topics. All the commands you need are in this instruction.

• A pre-built map of the simulated world is provided in your workspace at /fossbot_ros2/ws_fossbot/src/fossbot_educational_navigation/maps/map.yaml (with map.pgm next to it). You navigate on this map; you do not build it.

• Familiarity with the /scan, /odom and /cmd_vel topics.

• For the final, optional step only: access to the lab Wi-Fi network and a physical FOSSBot, provided and powered on by your instructor.

• Ability to capture evidence: screenshots and photos.

4. Required Material and Setup

Tip: The simulation steps (1 to 7) work without the physical robot. If hardware is not available, complete those and read through Step 8.

5. Safety, Ethics and Accessibility Notes

• start_container.sh runs the container with reduced isolation (--network=host, GPU and X-server access). Use it only with the FOSSBotEduSim image you built yourself, and run xhost -local:root afterwards on shared machines. On a machine with integrated graphics, use start_container_no_gpus.sh.

• Step 8 commands real hardware that moves on its own. Clear the floor, keep the speed low, and know the stop command before you send a goal (see Connecting to real robot).

6. Scenario and Problem Statement

The FOSSBot has to drive itself across a depot it has a map of, from one side to goal poses on the far side. A box has been left in the way after the map was made: it is not on the map, so the robot only learns about it from its own sensors as it approaches, and it must still reach the goal without hitting it. From there you look inside the navigation stack - switching the motion controller and the global planner at run time to see how each shapes the robot’s motion and path - then take the wheel yourself under a safety behaviour that will not let you crash, and finally command the robot from code: a repeatable inventory round with docking, and a robot that reacts to goals as they arrive on a topic.

7. Lab Workflow

8. Step-by-Step Instructions

Background. This lab is hands-on. For how Nav2 works (planner, controller, costmaps, localization), read the Nav2 concepts page once. In short: the planner computes a path to the goal, the controller follows it and avoids obstacles, and AMCL tells Nav2 where the robot is on the map.

Before you start. Capture evidence as you go. The full list is in Section 10. Tasks to capture are marked with a 📸 note.

Step 1 - Start the simulator and bring up Nav2

You use the ros2_fossbot_edu Docker image built from the FOSSBotEduSim repository. If you do not have it yet, build it on your workstation first:

git clone https://github.com/LRMPUT/FOSSBotEduSim.git
cd FOSSBotEduSim
bash build_image.sh

Then bring up the whole stack with one command from the FOSSBotEduSim directory:

bash run_nav.sh

This starts the container, the simulator in the depot world, Nav2 on the provided map (/fossbot_ros2/ws_fossbot/src/fossbot_educational_navigation/maps/map.yaml, loaded with the tuned nav2_params.yaml that defines the controllers and planners you switch in Steps 4-5), and RViz with the Nav2 view. Press Ctrl+C in that terminal to stop everything.

The RViz view already shows the map, both costmaps, the planned path, the laser scan and the robot model, and it includes the Nav2 and Selector panels you use in Steps 4-5.

Note: The first run downloads the depot world model from Gazebo Fuel, so it needs an internet connection once; afterwards it is cached and works offline.

Warning: run_nav.sh uses NVIDIA GPU passthrough. On a workstation without an NVIDIA GPU, run bash run_nav_no_gpus.sh instead - it brings up the same stack on integrated graphics.

Expected result: the FOSSBot appears in the depot, already localized - the launch script sets its start pose on the map, so the laser scan lines up with the map walls. (If localization ever drifts, click 2D Pose Estimate and drag an arrow at the robot’s true pose.)

Step 2 - Navigate to a goal

1. Click the Nav2 Goal tool in the RViz toolbar.

2. Click and drag a goal pose across the room.

3. A green path (/plan) appears and the robot drives to the goal on its own.

Expected result: the robot reaches the goal pose and stops.

📸 Capture for submission: screenshot RViz showing the path and the robot at the goal.

Step 3 - Avoid an obstacle

1. Send a fresh Nav2 Goal so the robot is driving across the room.

2. Place a box in its path. In the Gazebo window, click the box icon in the top toolbar, move the cursor into the gap between the robot and its goal, and left-click to drop a box.

3. Watch in RViz: the box appears in the local costmap, the robot steers around it, and the path re-routes.

Expected result: the robot reaches the goal without hitting the box, even though the box was never on the map.

📸 Capture for submission: screenshot RViz with the box in the local costmap and the robot driving around it.

Step 4 - Switch the controller

The controller turns the global path into wheel velocities. Your FOSSBot is set up with two controllers - MPPI (the default) and RPP (Regulated Pure Pursuit) - and you switch between them with the Selector panel in RViz.

Where this is configured. The controllers are defined in nav2_params.yaml (in .../config/) under controller_server. You do not edit it for this lab; to understand or tune the parameters, see the Nav2 controller server docs.

The RViz view already has two displays for this: MPPI Trajectories (topic /trajectories) and RPP Lookahead (topic /lookahead_point). If either is missing, add it with Add -> By topic.

1. Send a Nav2 Goal. MPPI is the default controller, so the robot follows the path with it.

2. In the Selector panel (docked on the left of the RViz window), set the Controller drop-down to RPP and send another goal.

Task 4.1

While each controller drives, watch its display in RViz:

• MPPI: /trajectories is a bundle of candidate trajectories fanning out ahead of the robot, redrawn every cycle.
• RPP: /lookahead_point is a single point sitting on the path ahead of the robot, sliding forward as it drives.

What this means: the two controllers work in fundamentally different ways. MPPI is a sampling-based optimiser - each cycle it rolls out many candidate trajectories, scores them with its internal critics (cost functions) and drives the best one. RPP is a simple geometric tracker - it just steers toward one look-ahead point. So the choice is a trade-off: MPPI is more flexible - it can deviate from the path to optimise its motion - but costs more compute; RPP is lightweight and predictable, but it intentionally does not deviate from the path.

Expected result: same route, but MPPI shows a bundle of trajectories (it weighs many options) and RPP shows a single moving point (it follows one).

📸 Capture for submission: a screenshot of MPPI’s /trajectories bundle, and one of RPP’s /lookahead_point.

Step 5 - Switch the planner

The planner computes the path to the goal. Your FOSSBot has two planners - NavFn (the default) and SmacHybrid - which you switch in the same Selector panel. The point of this step is the difference between a purely geometric path and a kinematically feasible one:

• NavFn searches the costmap grid and returns the shortest route as a chain of cells. It ignores how the robot moves, so the path can have sharp, in-place turns.
• SmacHybrid plans the route only from motions that respect a minimum turning radius, so its path comes out as a smooth, continuous curve. Those motions are Dubins and Reeds-Shepp curves - the shortest paths between two poses for a vehicle with a minimum turning radius (Dubins = forward only, Reeds-Shepp = forward and reverse) - and they come from OMPL (the Open Motion Planning Library).

Why use this on a differential-drive robot? Dubins/Reeds-Shepp assume a car-like robot that cannot turn on the spot. The FOSSBot is differential-drive - its turning radius can be ~0 - so it does not need this constraint, and for a pure diff-drive you would normally pick NavFn or SmacPlanner2D. SmacHybrid is used here for two reasons: it is the Nav2 planner that uses OMPL (this lab’s subject), and with a small minimum_turning_radius it produces smooth, continuous curves the robot follows without stopping to rotate in place. It also checks collisions using the robot’s full shape and orientation. That helps non-circular robots squeeze through tight spaces - but it needs a polygon footprint.

Where this is configured. The planners are defined in the same nav2_params.yaml under planner_server. See the Smac planner docs for the parameters.

1. In the Selector panel, find the Planner drop-down. It lists NavFn and SmacHybrid.

2. Select SmacHybrid.

3. Send the same Nav2 Goal again and watch the green path change shape.

Task 5.1

Compare the resulting /plan paths produced by NavFn and SmacHybrid.

Expected result: NavFn’s path is grid-aligned with sharp corners; SmacHybrid’s is a smooth, continuous curve. For how SmacHybrid builds it, see the Smac planner docs.

📸 Capture for submission: screenshot RViz showing the curved path from the SmacHybrid planner.

Step 6 - Drive it yourself with assisted teleop

Nav2 is not only for autonomous goals - it can also supervise you driving and refuse commands that would crash the robot. The AssistedTeleop behaviour takes your teleop velocity (on the topic /cmd_vel_teleop), checks it against the local costmap, and scales it down - to a full stop - before a collision.

A ready-made script bundles the whole demo. On the host (in the FOSSBotEduSim directory, not inside the container), run:

bash run_assisted_teleop.sh

It starts the simulator, Nav2 and RViz, keeps the AssistedTeleop behaviour armed, and turns that terminal into the keyboard driver. It restarts the simulation in its own container, so you can run it whether or not Step 1 is still up.

Drive with the keyboard: i forward, , back, j / l turn, k stop.

Task 6.1

Drive the robot straight at a wall or shelf at full speed. In open space it follows you, but it stops by itself just short of the obstacle - even while you keep pressing “forward”. Steer away and it follows you again. (If you hold it pressed into the obstacle the behaviour times out; the script re-arms it for you.)

Expected result: the robot drives under your control but will not let you hit anything.

📸 Capture for submission: the robot stopped right in front of an obstacle while you are still commanding “forward”.

Step 7 - Capstone: program an autonomous warehouse round

Until now you sent goals by hand in RViz. Real deployments need missions that run the same way every time with no one clicking. Nav2 ships a Python API for this - nav2_simple_commander - which lets you send goals and read navigation feedback from code.

The case study. The depot runs a nightly inventory round: the robot drives to each station in the storage zone to scan stock and logs which ones it reached. This runs as the depot_inventory node in the fossbot_educational_navigation package; its core is short:

from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult

nav = BasicNavigator()
nav.waitUntilNav2Active()                  # block until AMCL and Nav2 are ready

nav.goToPose(make_pose(8.0, -1.3, -1.57))  # make_pose() builds a pose in the map frame
while not nav.isTaskComplete():
    feedback = nav.getFeedback()           # distance remaining, ETA, recoveries
result = nav.getResult()                   # SUCCEEDED / CANCELED / FAILED

The stations are listed in the STATIONS list at the top of the file. Each leg has a timeout: if the robot cannot reach a station in time - for example the aisle is blocked - it cancels that leg, marks the station MISSED, and carries on, so one bad station never stalls the whole round.

With the simulator and Nav2 running (Step 6 stops them, so start run_nav.sh again if needed), open a new terminal in the container (docker exec -it ros2_fossbot_edu bash) and run the node:

ros2 run fossbot_educational_navigation depot_inventory

The stations appear in RViz as labelled arrows (blue = dock, orange = stations) from the Inventory Stations display. Watch the robot drive the round while the terminal prints the live distance and ETA for each leg, then a summary at the end.

Task 7.1 - Add a station and dock the robot

The node lives at src/fossbot_educational_navigation/fossbot_educational_navigation/depot_inventory.py.

a) Add a fourth station. Add a line to the STATIONS list:

("station D", 17.0, -1.3, -1.57),

b) Make the robot dock. As written, the robot just starts and stops wherever the round ends. Make it behave like a real depot robot: undock from its charging station first, and dock again at the end. The dock is already configured as a docking station (home_dock in the docking_server section of nav2_params.yaml, at the robot’s spawn pose), so you only add two calls from nav2_simple_commander. Right after nav.waitUntilNav2Active():

nav.undockRobot("simple_home_dock")   # pull out of the dock

and after the station loop, before the report:

nav.dockRobotByID("home_dock")        # drive back and dock with a precise approach

See the Nav2 docking docs. Then rebuild and run:

cd /fossbot_ros2/ws_fossbot
colcon build --packages-select fossbot_educational_navigation
ros2 run fossbot_educational_navigation depot_inventory

Expected result: the robot undocks, visits every station (including your station D), and docks back at the charging station; the report lists each station as scanned or MISSED.

📸 Capture for submission: the terminal showing the inventory-round report.

Task 7.2 - React to goals on a topic

A robot often does not know its goals in advance - they arrive at run time (from a fleet manager, an operator, a task queue). Here the goal comes in on a topic and the robot must always chase the latest one: a new goal cancels the one in progress. Because sending a fresh goal to Nav2 preempts the current one, this “always follow the newest goal” behaviour takes only a few lines.

You write the goal follower. Open the template at src/fossbot_educational_navigation/fossbot_educational_navigation/goal_follower.py - the GoalFollower node and the main loop are already written; complete the three TODOs so the node:

1. subscribes to the /mission_goal topic (a PoseStamped) with the callback (TODO 1),
2. stores each incoming goal in the callback (TODO 2),
3. sends the goal to Nav2 so a new goal preempts the one in progress (TODO 3) - the same BasicNavigator call you used for the inventory round above.

The random_station_publisher node is provided for you - it publishes a random station to /mission_goal every few seconds to feed your follower. Rebuild, then run each in its own container terminal (docker exec -it ros2_fossbot_edu bash):

cd /fossbot_ros2/ws_fossbot
colcon build --packages-select fossbot_educational_navigation

ros2 run fossbot_educational_navigation goal_follower

ros2 run fossbot_educational_navigation random_station_publisher

Expected result: the robot heads for each published station; when a new goal arrives before it gets there, it switches immediately and drives to the new one. The follower terminal prints new goal (...) - switching for each.

📸 Capture for submission: RViz showing the robot redirecting to a freshly published goal.

Step 8 - Navigate a real FOSSBot (optional)

The Nav2 commands are the same on real hardware; only the data source and the clock change.

1. Connect to the robot by following Connecting to real robot. Do not launch the simulator.

2. Bring up Nav2 against a map of the real space (build one with SLAM, or use one your instructor provides), using wall-clock time:

ros2 launch fossbot_educational_navigation navigation.launch.py \
  map:=/fossbot_ros2/ws_fossbot/real_map.yaml \
  use_sim_time:=false

3. In RViz, set the Fixed Frame to map, set the 2D Pose Estimate, and send a Nav2 Goal across the cleared space.

Warning: Clear the floor, keep the speed low, and keep the stop command ready (see Connecting to real robot).

Task 8.1

Drive the real FOSSBot to a goal pose, then place a real obstacle in its path and watch it avoid it.

Expected result: the real robot localizes, reaches a goal, and avoids a real obstacle.

📸 Capture for submission: a photo of the real FOSSBot reaching a goal.

9. Analysis Questions

1. When the box appeared in the robot’s path, which costmap did it show up in, and which Nav2 component steered around it: the planner or the controller?

2. MPPI and RPP follow the same path very differently. From their visualisations - MPPI’s bundle of candidate trajectories (/trajectories) versus RPP’s single look-ahead point (/lookahead_point) - describe in one sentence how each one decides its motion, and explain why switching the controller did not change the route.

3. The inflation_radius parameter sets how wide a margin the costmaps keep around obstacles. What would go wrong if you set it very large in a room with a narrow doorway?

4. This lab is titled “Nav2 and OMPL”, yet the default planner (NavFn) is not OMPL-based. How is OMPL actually used in your Nav2 setup, and what does relying on Dubins/Reeds-Shepp curves let SmacHybrid guarantee about its path that NavFn does not?

10. Submission Requirements

• A screenshot of the robot reaching the goal, from Step 2.

• A screenshot of the robot avoiding the unmapped box, from Step 3.

• Screenshots of MPPI’s candidate trajectories (/trajectories) and RPP’s look-ahead point (/lookahead_point), from Task 4.1.

• A screenshot of the curved path from the SmacHybrid planner, from Task 5.1.

• A screenshot of the robot stopped in front of an obstacle while you drive it under assisted teleop, from Task 6.1.

• A screenshot of the inventory-round report, from Task 7.1.

• A screenshot of the robot redirecting to a freshly published goal, from Task 7.2.

• (Optional) A photo of the real FOSSBot reaching a goal, from Task 8.1.

• Short answers (2-3 sentences each) to the four analysis questions.

11. References and Open Licence

• Nav2 concepts: https://docs.nav2.org/concepts/index.html
• Nav2 controller server (DWB, Regulated Pure Pursuit, MPPI): https://docs.nav2.org/configuration/packages/configuring-controller-server.html
• Regulated Pure Pursuit controller: https://docs.nav2.org/configuration/packages/configuring-regulated-pp.html
• Nav2 planner server (NavFn, Smac, Theta*): https://docs.nav2.org/configuration/packages/configuring-planner-server.html
• AMCL localization: https://docs.nav2.org/configuration/packages/configuring-amcl.html
• Nav2 simple commander (Python API): https://docs.nav2.org/commander_api/index.html
• OMPL, the Open Motion Planning Library: https://ompl.kavrakilab.org/
• FOSSBotEduSim repository: https://github.com/LRMPUT/FOSSBotEduSim

The Creative Commons Attribution 4.0 International (CC BY 4.0) license allows users to share, copy, distribute, and adapt the work, even for commercial purposes, as long as proper credit is given to the original creator.

EU funding disclaimer

Funded by the European Union. Views and opinions expressed are however those of the author(s) only and do not necessarily reflect those of the European Union or the European Education and Culture Executive Agency (EACEA). Neither the European Union nor EACEA can be held responsible for them.