Building an Autonomous Waypoint Navigation System for the myAGV in ROS 2

A technical walkthrough on building a custom waypoint recording and navigation system for the myAGV using ROS 2 and the Nav2 stack.

Transitioning a mobile robot from manual teleoperation to autonomous dispatch requires a robust way to map, store, and recall specific physical locations. In this technical walkthrough, we will explore how to build a custom waypoint recording and navigation system for the myAGV using ROS 2 and the Nav2 stack.

By combining the /amcl_pose localization topic with the nav2_simple_commander API, you can create a streamlined pipeline to visually drive the robot to a point of interest using RViz, save its exact coordinates to a configuration file, and dispatch it back to those locations on demand.

Prerequisites: System Initialization

Before deploying any high-level dispatch scripts, the myAGV platform requires its low-level hardware communication layers to be actively initialized.

The Blueprint: The Waypoint Configuration File

At the heart of this system is a lightweight, human-readable YAML configuration file. Instead of hardcoding coordinates into our navigation scripts, we store them externally.

Located at ~/myagv_ros2/src/myagv_navigation2/config/waypoints.yaml, this file maps semantic names to precise 2D poses (x, y, and yaw).

# Sample waypoints.yaml Configuration
fruit:
  x: 0.916
  y: 0.433
  yaw: 2.306

vegetable:
  x: 0.006
  y: 0.0
  yaw: 0.076

This structure makes it incredibly easy for developers to manually edit, back up, or swap out location data without touching the underlying Python logic.

Part 1: Recording Waypoints on the Fly

To save a waypoint, the AGV needs to know exactly where it is on the map. The waypoint recording script accomplishes this by tapping directly into the robot's localization system.

• AMCL Pose Extraction: The script creates a node (waypoint_recorder) that subscribes to the /amcl_pose topic. This topic broadcasts the robot's estimated position on the 2D map.
• Quaternion to Euler Conversion: ROS 2 handles rotations using quaternions (x, y, z, w), but our YAML file uses a much simpler "yaw" (rotation in radians). The script includes a mathematical helper function, yaw_from_quaternion, to translate this complex 3D rotation data into a flat 2D heading.
• Non-Blocking User Input: Standard Python CLI inputs will freeze a ROS 2 node, preventing it from receiving messages. To solve this, the script utilizes a MultiThreadedExecutor. This allows the script to prompt you to name the location while keeping the background ROS 2 subscription alive.

Part 2: Autonomous Dispatch

Once the waypoints are saved, the dispatch script acts as the bridge between our saved YAML file and the underlying ROS 2 navigation stack.

• Command Line Arguments: The script takes a destination name as a system argument (e.g., sys.argv[1]). It cross-references this name with the keys loaded from the waypoints.yaml file.
• The Nav2 Simple Commander: The script leverages the BasicNavigator class from the nav2_simple_commander package. This API abstracts away the complexity of ROS 2 action servers, allowing us to interact with the Nav2 stack using straightforward Python commands.
• Execution & Feedback: By passing the constructed pose to navigator.goToPose(goal), the AGV begins its autonomous route. The script waits patiently until the chassis reaches its destination before printing a success or failure state based on the TaskResult.

Deployment & Execution Guide

To use this system, you must first launch the underlying hardware communication, the LiDAR, and the pre-saved navigation map. Here is the reference link for 2D mapping.

Step 1: Start the Navigation Stack and LiDAR

Open your terminal layers and boot up the core hardware and Nav2 engines. This will establish the LiDAR connection, load your map, and automatically launch the RViz visual interface.

# Terminal 1: Start the underlying odometry and LiDAR
ros2 launch myagv_odometry myagv_active.launch.py

# Terminal 2: Start Nav2 and load the map (Launches RViz)
ros2 launch myagv_navigation2 navigation2_active.launch.py map:=/path-to-your-map.yaml

Step 2: Navigate and Record a Destination

With RViz open on your screen, you can visually route the robot to the location you want to save.

1. In the RViz top toolbar, click the Nav2 Goal tool.

2. Click anywhere on your map to send the myAGV to that specific place, dragging the arrow to set its final orientation.

3. Once the robot arrives and stops at the designated spot, open a new terminal and run your recording script. Script is available here.

   Bash

   python3 record_waypoint.py

4. The terminal will prompt you to name the location (e.g., "table"). The script will grab the exact coordinates, save them to your YAML file, and exit.

Step 3: Dispatch the Robot to the Next Waypoint

Now that your waypoint is stored, you can command the robot to drive there autonomously from anywhere on the map. In a fresh terminal, run the dispatch script followed by the name of your saved waypoint. Script is available here.

python3 goto_waypoint.py

Visualizing the Path Generation: The moment you execute the goto waypoint code, switch your attention back to the RViz window. You will automatically see the generated path trajectory line laid out on the map in real-time as the myAGV navigates around obstacles to reach its destination!