AD-R1M ROS2 Examples

This page provides practical examples for using the AD-R1M robot with ROS2, including mapping, localization, and autonomous navigation.

Mapping with SLAM Toolbox

Create a map of your environment using SLAM Toolbox for real-time mapping.

Starting the Mapping Session

On the robot (via SSH), generate the SLAM configuration and start the services:

ad-r1m mkconfig slam
cd slam

# Check if base services are running
docker ps

# If not running, start base services (robot, tof, teleop, zenoh)
docker compose --env-file ad-r1m.env up -d

# Start the SLAM node for mapping
docker compose --env-file ad-r1m.env up slam -d

See Manage AD-R1M runtime configurations for more details on ad-r1m mkconfig.

Active Docker Compose Services:

Base services (started with docker compose --env-file ad-r1m.env up -d):

• zenoh_router - Zenoh middleware router

• robot - motors, IMU, odometry, sensor fusion

• tof - ToF camera and depth-to-laserscan

• teleop_crsf - radio control for driving

Profile service (started with docker compose --env-file ad-r1m.env up slam -d):

• slam - SLAM Toolbox for mapping

Mapping Process

1. Start RViz on your host computer (see AD-R1M ROS 2 Getting Started):

   cd pixiws
   pixi run rviz2
   

2. Change the Fixed Frame in RViz to map

3. Drive the robot around using one of these methods:

   • Remote Control: Arm the robot using the killswitch on the RC handset (switch SA to “ARMED”), then drive using the right stick. See Remote Control for details.

   • Keyboard Teleop: Use keyboard control from your host or via SSH. See Control the robot using a keyboard for setup instructions.

RViz mapping view - change Fixed Frame to “map”

Robot mapping demonstration

Map Legend:

• White areas = Free space (safe to navigate)

• Black areas = Obstacles (walls, objects)

• Gray areas = Unexplored/unknown

Saving the Map

After mapping is complete, save the map:

docker compose --env-file ad-r1m.env run save_map

This creates two files in ros_data/maps/ with a timestamped filename (e.g., map_2024-01-15T10:30:00+00:00):

• map_<timestamp>.png - Grayscale image (white=free, black=occupied, gray=unknown)

• map_<timestamp>.yaml - Map metadata (resolution, origin, thresholds)

Note

To use the saved map with AMCL localization, either:

• Rename the files to map.png and map.yaml and move to ros_data/, or

• Update the map path in the localization_amcl service command in compose.yaml

SLAM Configuration

SLAM Toolbox parameters: /ros_data/mapper_params_online_async.yaml

Key parameters:

slam_toolbox:
  ros__parameters:
    resolution: 0.05            # Map resolution (meters/pixel)
    max_laser_range: 5.0        # Maximum laser range to use
    minimum_travel_distance: 0.3  # Min distance before adding scan

Localization

The robot supports two localization modes: AMCL (using a saved map) or blind/dead-reckoning (no map required).

Starting Localization

On the robot (via SSH), generate the navigation configuration and start the services:

ad-r1m mkconfig nav
cd nav

# Check if base services are running
docker ps

# If not running, start base services
docker compose --env-file ad-r1m.env up -d

Choose one of the localization modes:

Option 1: AMCL Localization (requires saved map)

Warning

AMCL requires a map file at ros_data/map.yaml. You must first create a map using Mapping with SLAM Toolbox and save it before using AMCL localization.

docker compose --env-file ad-r1m.env up -d localization_amcl

Uses AMCL (Adaptive Monte Carlo Localization) with the saved map from ros_data/map.yaml.

Option 2: Blind/Dead-Reckoning Localization (no map required)

docker compose --env-file ad-r1m.env up -d localization_blind map_server

Uses odometry-only localization with an empty map. Useful for testing navigation without a pre-built map.

See Manage AD-R1M runtime configurations for more details.

Setting Initial Pose (AMCL only)

When using AMCL localization, set the robot’s initial pose in RViz:

1. Click 2D Pose Estimate in the toolbar

2. Click on the map where the robot is located

3. Drag to set the orientation (direction robot is facing)

Setting initial pose for AMCL localization

The robot’s estimated position (red arrow) and uncertainty (purple ellipse) will be displayed.

Tips for good localization:

• Set the initial pose as accurately as possible

• Drive the robot around to help particles converge

• Ensure the saved map matches the current environment

AMCL Topics

• Publishes: /amcl_pose (geometry_msgs/PoseWithCovarianceStamped)

• Publishes: TF transform (map → odom)

• Subscribes: /scan (laser scan)

• Subscribes: /odom (odometry)

Autonomous Navigation with Nav2

Use the Nav2 navigation stack for autonomous point-to-point navigation.

Starting Navigation

Navigation requires localization to be running. Start localization first (see above), then add the navigation service:

# Start Nav2 navigation (requires localization to be running)
docker compose --env-file ad-r1m.env up -d nav

Sending Navigation Goals

In RViz:

1. Click 2D Nav Goal in the toolbar

2. Click on the desired destination on the map

3. Drag to set the goal orientation

4. The robot will plan a path and navigate autonomously

RViz navigation view showing global path and costmaps

Robot autonomous navigation demonstration

Navigation displays:

• Green line - Global path plan

• Colored regions - Costmap (inflation around obstacles)

• Robot footprint - Current robot position

Navigation Topics

• Subscribes: /goal_pose (geometry_msgs/PoseStamped)

• Publishes: /cmd_vel_nav (velocity commands to twist_mux)

• Publishes: /plan (global path)

• Publishes: /local_costmap/costmap, /global_costmap/costmap

Nav2 Configuration

Navigation parameters: /ros_data/navigation_params.yaml

Key parameters:

controller_server:
  ros__parameters:
    controller_frequency: 20.0
    FollowPath:
      max_vel_x: 0.5          # Maximum forward velocity
      max_vel_theta: 1.0      # Maximum angular velocity
      xy_goal_tolerance: 0.1  # Position tolerance at goal
      yaw_goal_tolerance: 0.1 # Orientation tolerance at goal

For detailed navigation tuning, see the Nav2 documentation.

Note

If the robot creates a navigation plan but does not move, restarting the robot service may have re-activated the killswitch. See AD-R1M Quick Start Guide for killswitch control instructions.

ROS 2 Topics Reference

Core sensor and control topics used by the AD-R1M:

Sensor Data

• /scan - 2D laser scan (from ToF depth-to-laserscan)

• /cam1/depth_image - Raw depth image from ToF camera

• /imu - IMU data (angular velocities, linear accelerations)

Odometry and Localization

• /diff_drive_controller/odom - Raw odometry from wheel encoders

• /odom - Fused odometry (encoder + IMU via EKF)

• /amcl_pose - AMCL localization pose estimate

Motion Control

• /cmd_vel_joy - Velocity commands from RC handset

• /cmd_vel_nav - Velocity commands from Nav2

• /diff_drive_controller/cmd_vel_unstamped - Final commands to motors

Navigation

• /goal_pose - Navigation goals

• /plan - Global path

• /map - Occupancy grid map

# List all active topics
ros2 topic list

# Echo a topic
ros2 topic echo /imu --once

# Check topic frequency
ros2 topic hz /scan

For detailed architecture information, see AD-R1M ROS2 Architecture.

<root main_tree_to_execute="MainTree">
  <BehaviorTree ID="MainTree">
    <Sequence name="navigate_sequence">
      <ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
      <FollowPath path="{path}" controller_id="FollowPath"/>
    </Sequence>
  </BehaviorTree>
</root>

Load custom behavior trees via the Nav2 parameters:

bt_navigator:
  ros__parameters:
    default_bt_xml_filename: "/path/to/custom_bt.xml"

Waypoint Following

For multi-waypoint missions, use the waypoint follower action.

Python Waypoint Example

from nav2_simple_commander.robot_navigator import BasicNavigator
from geometry_msgs.msg import PoseStamped

navigator = BasicNavigator()

# Define waypoints
waypoints = []

waypoint1 = PoseStamped()
waypoint1.header.frame_id = 'map'
waypoint1.pose.position.x = 1.0
waypoint1.pose.position.y = 0.0
waypoint1.pose.orientation.w = 1.0
waypoints.append(waypoint1)

waypoint2 = PoseStamped()
waypoint2.header.frame_id = 'map'
waypoint2.pose.position.x = 2.0
waypoint2.pose.position.y = 1.0
waypoint2.pose.orientation.w = 1.0
waypoints.append(waypoint2)

# Start waypoint following
navigator.followWaypoints(waypoints)

while not navigator.isTaskComplete():
    feedback = navigator.getFeedback()
    print(f'Current waypoint: {feedback.current_waypoint}')

result = navigator.getResult()
print(f'Navigation result: {result}')

Command Line Waypoint Navigation

Send a single goal via command line:

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

Nav2 Custom Actions

Create custom Nav2 action plugins for specialized behaviors.

Custom Action Plugin Structure

#include "nav2_behavior_tree/bt_action_node.hpp"

class MyCustomAction : public nav2_behavior_tree::BtActionNode<my_msgs::action::MyAction>
{
public:
  MyCustomAction(
    const std::string & xml_tag_name,
    const std::string & action_name,
    const BT::NodeConfiguration & conf)
  : BtActionNode<my_msgs::action::MyAction>(xml_tag_name, action_name, conf)
  {}

  void on_tick() override
  {
    // Set action goal
    goal_.target = getInput<double>("target").value();
  }

  BT::NodeStatus on_success() override
  {
    return BT::NodeStatus::SUCCESS;
  }
};

Register the plugin in your package’s CMakeLists.txt and plugin XML.

Lift Control Integration

The AD-R1M includes a lift mechanism controlled via ROS2 services:

import rclpy
from rclpy.node import Node
from adrd_demo_ros2.srv import LiftGPIO

class LiftController(Node):
    def __init__(self):
        super().__init__('lift_controller')
        self.client = self.create_client(LiftGPIO, '/elevator_to_robot')

    def send_lift_command(self, command):
        """
        Commands:
          0 = Hold
          1 = Lift up
          2 = Lift down
        """
        request = LiftGPIO.Request()
        request.command = command
        future = self.client.call_async(request)
        return future

Multi-Robot Setup

For multi-robot scenarios, use namespaces to separate robot topics and frames.

Launching Multiple Robots

Each robot should have a unique namespace configured. See Manage AD-R1M runtime configurations for details on setting up robot namespaces.

RViz for Multiple Robots

Configure the Zenoh connection to each robot by updating the ZENOH_CONFIG_OVERRIDE environment variable with the appropriate robot hostname.

Each robot’s topics will be prefixed with its namespace (e.g., /ad_r1m_0/cmd_vel, /ad_r1m_1/cmd_vel).

Stopping Services

To stop all services in the current configuration:

docker compose --env-file ad-r1m.env down

To stop specific services:

docker compose --env-file ad-r1m.env stop nav localization_amcl

To stop all running Docker containers (regardless of compose project):

docker stop $(docker ps -q)