Navigation2 Simple Monitoring

This example demonstrates Navigation2 monitoring and control operations using the Nav2ClientLibrary.

Overview

The Navigation2 simple monitoring test covers:

• Navigation2 Simulation Setup: Launch and manage Navigation2 simulation

• Simple Movement: Navigate to a specific pose and verify arrival

• Navigation Cancellation: Test navigation cancellation and status monitoring

• 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

Navigation Cancellation

Test Navigation2 Cancel Navigation
    [Documentation]    Test Navigation2 cancel navigation
    [Tags]    nav2    cancel    navigation
    [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})...
    Async Navigate To Pose Simple    ${GOAL_X}    ${GOAL_Y}    ${GOAL_THETA}    timeout=${NAVIGATION_TIMEOUT}

    Sleep    1s
    # Call Service    /navigate_to_pose/_action/cancel_goal    action_msgs/srv/CancelGoal   {}
    Cancel Navigation
    # Check if navigation is active
    ${active}=    Is Navigation Active
    Should Be Equal    ${active}    ${False}

    # Get navigation status
    ${status}=    Get Navigation Status
    Should Not Be Empty    ${status}
    Should Contain    ${status}    navigation_active
    Should Contain    ${status}    current_pose
    Should Contain    ${status}    goal_pose

    ${final_pose}=    Get Transform    map    base_link
    Should Not Be Empty    ${final_pose}

    IF    ${final_pose} is not None
        ${arrived}=    Is Within Tolerance    ${final_pose}    tolerance=${TOLERANCE}    target_x=${GOAL_X}    target_y=${GOAL_Y}
        Should Be Equal    ${arrived}    ${False}
    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
    ${running}=    Has Running Nodes
    # Clean up any existing simulation
    Clean Up Navigation2 Simulation

    Should Be Equal    ${running}    ${False}
    # Set environment variables for the test
    Set Environment Variable    TURTLEBOT3_MODEL      waffle

    # Launch the Navigation2 simulation
    Log    Starting Navigation2 simulation launch...
    ${process}=    Launch Package    turtlebot3    simulation.launch.py
    Should Not Be Equal    ${process}    ${None}
    Log    Launch process started with PID: ${process.pid}

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

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

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

    ${shutdown}=    Shutdown Process    ros_gz_bridge
    Should Be True    ${shutdown}

    ${shutdown}=    Shutdown Process    rviz2
    Should Be True    ${shutdown}

    ${shutdown}=    Kill Process By Name    ros
    Should Be True    ${shutdown}

    ${running}=    Has Running Nodes
    Should Be Equal    ${running}    ${False}

Running the Example

To run this example:

# Run the Navigation2 simple monitoring test
robot examples/medium/nav2_simple_monitoring_test.robot

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

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

Expected Output

The test will:

1. Setup Navigation2 Simulation: Launch TurtleBot3 simulation with Navigation2

2. Test Simple Movement: Navigate to a specific pose and verify arrival

3. Test Navigation Cancellation: Start navigation, cancel it, and verify status

4. Verify Position: Check robot position using transforms

5. Check Tolerance: Verify robot arrival within specified tolerance

Key Learning Points

Troubleshooting

Common Issues

1. Simulation not available: Ensure TurtleBot3 simulation is installed

2. Navigation failures: Check if robot is properly localized

3. Transform errors: Verify transform availability between frames

4. Timeout errors: Increase timeout values for slow systems

Debug Tips

1. Check Navigation2 status before navigation

2. Verify robot localization

3. Test individual operations manually

4. Check simulation environment setup

Next Steps

After running this example, you can:

• Explore the Navigation2 TurtleBot3 example for complete TurtleBot3 testing

• 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