Navigation2 TurtleBot3

This example demonstrates complete TurtleBot3 Navigation2 testing using the Nav2ClientLibrary.

Overview

The Navigation2 TurtleBot3 test covers:

TurtleBot3 Simulation Setup: Launch TurtleBot3 Gazebo simulation
Navigation2 Integration: Start Navigation2 with TurtleBot3
Initial Pose Setting: Set robot initial pose for localization
Navigation Testing: Test navigation to specific poses
Position Verification: Check robot position using transforms
Tolerance Checking: Verify robot arrival within specified tolerance

Test Structure

*** Settings ***
Documentation    Simple Navigation2 test: launch simulator, wait 5s, send vehicle to another place
Library          ros2_client.ROS2ClientLibrary
Library          nav2_client.Nav2ClientLibrary
Library          Collections
Library          OperatingSystem
Library          Process

*** Variables ***
${WAIT_TIME}             5s
${NAVIGATION_TIMEOUT}    30
${GOAL_X}               -1.7
${GOAL_Y}                0.5
${GOAL_THETA}            1.57
${TOLERANCE}             0.5

Key Test Cases

Navigation2 Simple Movement

Test Navigation2 Simple Movement
    [Documentation]    Launch Navigation2 simulation, wait 5 seconds, send vehicle to another place
    [Tags]    nav2    simple    movement
    [Setup]    Setup Navigation2 Simulation
    [Teardown]    Clean Up Navigation2 Simulation

    # Send vehicle to another place
    Log    Sending vehicle to position (${GOAL_X}, ${GOAL_Y}, ${GOAL_THETA})...
    ${nav_success}=    Navigate To Pose Simple    ${GOAL_X}    ${GOAL_Y}    ${GOAL_THETA}    timeout=${NAVIGATION_TIMEOUT}
    Log    Navigation command sent: ${nav_success}

    # Get robot pose
    ${final_pose}=   Get Transform    map    base_link
    Log    Final position: ${final_pose}
    Should Not Be Empty    ${final_pose}

    # Check if robot is within tolerance
    IF    ${final_pose} is not None
        ${arrived}=    Is Within Tolerance    ${final_pose}    tolerance=${TOLERANCE}    target_x=${GOAL_X}    target_y=${GOAL_Y}
        Should Be True    ${arrived}
    ELSE
        Log    Final position is None, skipping tolerance check
    END

Setup and Teardown

The test includes comprehensive setup and teardown procedures:

Setup Navigation2 Simulation
    [Documentation]    Setup Navigation2 simulation
    # Clean up any existing simulation
    Clean Up Navigation2 Simulation

    # Set environment variables for the test
    Set Environment Variable    TURTLEBOT3_MODEL      waffle

    # Launch the TurtleBot3 Gazebo simulation
    Log    Starting TurtleBot3 Gazebo simulation launch...
    ${sim_process}=    Launch Package    turtlebot3_gazebo    turtlebot3_world.launch.py
    Log    Simulation process started with PID: ${sim_process.pid}

    # Wait for simulation to initialize
    Sleep    3s

    # Launch Navigation2
    Log    Starting Navigation2 launch...
    ${nav_process}=    Launch Package    turtlebot3_navigation2    navigation2.launch.py    arguments=use_sim_time:=True

    Sleep    5s

    # Use CLI service call directly (to avoid JSON conversion issues)
    ${result}=    Run Process    ros2    service    call    /set_initial_pose    nav2_msgs/srv/SetInitialPose    {pose: {header: {frame_id: 'map'}, pose: {pose: {position: {x: -2.0, y: -0.5, z: 0.0}, orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}}}}}
    Should Be Equal As Integers    ${result.rc}    0    Service call should succeed
    Log    Service call output: ${result.stdout}
    Log    Service call stderr: ${result.stderr}

    Sleep    2s

    Log    Navigation2 process started with PID: ${nav_process.pid}

    ${ready}=    Wait For Nav2 Ready
    Should Be True    ${ready}

    # Wait for the launch to initialize
    Sleep    ${WAIT_TIME}
    RETURN    ${sim_process}    ${nav_process}

Clean Up Navigation2 Simulation
    [Documentation]    Clean up Navigation2 simulation
    # Shutdown Navigation2 processes
    ${shutdown}=    Shutdown Process    navigation2
    Should Be True    ${shutdown}
    Log    Navigation2 cleanup completed

    # Shutdown Gazebo simulation
    ${shutdown}=    Shutdown Process    gazebo
    Should Be True    ${shutdown}
    Log    Gazebo simulation cleanup completed

Key Features

TurtleBot3 Integration

This example demonstrates complete TurtleBot3 integration:

Gazebo Simulation: Launch TurtleBot3 in Gazebo world
Navigation2 Stack: Start Navigation2 with TurtleBot3 configuration
Initial Pose Setting: Set robot initial pose for proper localization
Navigation Testing: Test navigation to specific poses
Position Verification: Check robot position using transforms

Initial Pose Setting

The example shows how to set the initial pose using direct service calls:

# Use CLI service call directly (to avoid JSON conversion issues)
${result}=    Run Process    ros2    service    call    /set_initial_pose    nav2_msgs/srv/SetInitialPose    {pose: {header: {frame_id: 'map'}, pose: {pose: {position: {x: -2.0, y: -0.5, z: 0.0}, orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}}}}}
Should Be Equal As Integers    ${result.rc}    0    Service call should succeed
Log    Service call output: ${result.stdout}
Log    Service call stderr: ${result.stderr}

Navigation Testing

The example demonstrates navigation testing:

# Send vehicle to another place
Log    Sending vehicle to position (${GOAL_X}, ${GOAL_Y}, ${GOAL_THETA})...
${nav_success}=    Navigate To Pose Simple    ${GOAL_X}    ${GOAL_Y}    ${GOAL_THETA}    timeout=${NAVIGATION_TIMEOUT}
Log    Navigation command sent: ${nav_success}

# Get robot pose
${final_pose}=   Get Transform    map    base_link
Log    Final position: ${final_pose}
Should Not Be Empty    ${final_pose}

# Check if robot is within tolerance
IF    ${final_pose} is not None
    ${arrived}=    Is Within Tolerance    ${final_pose}    tolerance=${TOLERANCE}    target_x=${GOAL_X}    target_y=${GOAL_Y}
    Should Be True    ${arrived}
ELSE
    Log    Final position is None, skipping tolerance check
END

Running the Example

To run this example:

# Run the Navigation2 TurtleBot3 test
robot examples/medium/nav2_turtlebot.robot

# Run with verbose output
robot -v examples/medium/nav2_turtlebot.robot

# Run specific test cases
robot -t "Test Navigation2 Simple Movement" examples/medium/nav2_turtlebot.robot

Expected Output

The test will:

Setup TurtleBot3 Simulation: Launch TurtleBot3 in Gazebo world
Start Navigation2: Launch Navigation2 with TurtleBot3 configuration
Set Initial Pose: Set robot initial pose for proper localization
Test Navigation: Navigate to a specific pose and verify arrival
Verify Position: Check robot position using transforms
Check Tolerance: Verify robot arrival within specified tolerance

Key Learning Points

Troubleshooting

Common Issues

TurtleBot3 not available: Ensure TurtleBot3 packages are installed
Gazebo simulation issues: Check Gazebo installation and configuration
Navigation2 failures: Verify Navigation2 configuration
Initial pose issues: Check initial pose setting

Debug Tips

Check TurtleBot3 environment variables
Verify Gazebo simulation startup
Test Navigation2 configuration
Check initial pose setting

Next Steps

After running this example, you can:

Explore the Navigation2 Simple Monitoring example for advanced monitoring
Check the PyRoboSim example for PyRoboSim integration
Read the ../user_guide/nav2_operations for detailed Navigation2 usage
Review the Navigation2 Client Library for complete API reference