Navigation2 Basic Test

This example demonstrates Navigation2 operations using the Nav2ClientLibrary.

Overview

The Navigation2 basic test covers:

• Navigation2 Library Import: Verify library initialization

• Basic Navigation Operations: Distance and angle calculations

• Pose Management: Initial pose setting and localization

• Navigation Status: Real-time navigation status monitoring

• Costmap Operations: Global and local costmap management

• Path Planning: Path computation and validation

• Navigation Through Poses: Multi-waypoint navigation

• Navigation Cancellation: Navigation cancellation and status verification

Test Structure

*** Settings ***
Documentation    Basic Navigation2 test using the Nav2ClientLibrary
Library          ros2_client.ROS2ClientLibrary
Library          nav2_client.Nav2ClientLibrary
Library          Collections
Library          OperatingSystem
Library          Process

*** Variables ***
${NAV2_TIMEOUT}    60.0
${POSE_TIMEOUT}    30.0
${WAIT_TIME}       5s
${TOLERANCE}       0.5

Key Test Cases

Navigation2 Library Import

Test Navigation2 Library Import
    [Documentation]    Verify that the Navigation2 library can be imported and initialized
    [Tags]    smoke
    [Setup]    Setup Navigation2 Simulation
    [Teardown]    Clean Up Navigation2 Simulation
    Log    Navigation2 library imported successfully

Basic Navigation Operations

Test Basic Navigation Operations
    [Documentation]    Test basic navigation operations with simulation
    [Tags]    navigation
    [Setup]    Setup Navigation2 Simulation
    [Teardown]    Clean Up Navigation2 Simulation

    # Test utility functions
    ${distance}=    Calculate Distance    0.0    0.0    3.0    4.0
    Should Be Equal As Numbers    ${distance}    5.0    0.1

    ${angle}=    Calculate Angle    0.0    0.0    1.0    1.0
    Should Be Equal As Numbers    ${angle}    0.785398    0.001

    ${degrees}=    Radians To Degrees    1.570796
    Should Be Equal As Numbers    ${degrees}    90.0    precision=0

    ${radians}=    Degrees To Radians    90.0
    Should Be Equal As Numbers    ${radians}    1.570796    0.1

Pose Management

Test Pose Management
    [Documentation]    Test pose management and transformations
    [Tags]    pose
    [Setup]    Setup Navigation2 Simulation
    [Teardown]    Clean Up Navigation2 Simulation

    # Test setting initial pose
    ${success}=    Set Initial Pose    0.0    0.0    0.0
    Should Be True    ${success}

    # Test getting current pose (may return None if no robot)
    ${current_pose}=    Get Current Pose    timeout=${POSE_TIMEOUT}
    Log    Current pose: ${current_pose}

    # Test waiting for localization
    ${localized}=    Wait For Localization    timeout=10.0
    Log    Localization status: ${localized}

Navigation Status

Test Navigation Status
    [Documentation]    Test navigation status operations
    [Tags]    status
    [Setup]    Setup Navigation2 Simulation
    [Teardown]    Clean Up Navigation2 Simulation

    # 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

Costmap Operations

Test Costmap Operations
    [Documentation]    Test costmap information retrieval
    [Tags]    costmap
    [Setup]    Setup Navigation2 Simulation
    [Teardown]    Clean Up Navigation2 Simulation

    # Get global costmap info
    ${global_info}=    Get Costmap Info    global
    Log    Global costmap info: ${global_info}

    # Get local costmap info
    ${local_info}=    Get Costmap Info    local
    Log    Local costmap info: ${local_info}

    # Test clearing costmaps (may fail if no robot running)
    ${cleared_global}=    Clear Costmap    global
    Log    Global costmap cleared: ${cleared_global}

    ${cleared_local}=    Clear Costmap    local
    Log    Local costmap cleared: ${cleared_local}

Path Planning

Test Path Planning
    [Documentation]    Test path planning operations
    [Tags]    path_planning
    [Setup]    Setup Navigation2 Simulation
    [Teardown]    Clean Up Navigation2 Simulation

    # Test path computation (may fail if no robot/map)
    ${path}=    Compute Path    0.0    0.0    0.0    2.0    1.0    1.57
    Log    Computed path: ${path}

    # If path was computed, test path length calculation
    IF    ${path} is not None
        ${path_length}=    Get Path Length    ${path}
        Should Be True    ${path_length} > 0.0
        Log    Path length: ${path_length} meters
    END

Navigation Through Poses

Test Navigation Through Poses
    [Documentation]    Test navigation through multiple poses
    [Tags]    navigation    poses
    [Setup]    Setup Navigation2 Simulation
    [Teardown]    Clean Up Navigation2 Simulation

    # Create a list of poses
    @{poses}=    Create List

    # Add poses to the list
    ${pose1}=    Create Dictionary    x=-2.0    y=0.0    theta=0.0
    ${pose2}=    Create Dictionary    x=-2.0    y=1.5    theta=1.57
    ${pose3}=    Create Dictionary    x=-2.0    y=1.0    theta=3.14

    Append To List    ${poses}    ${pose1}
    Append To List    ${poses}    ${pose2}
    Append To List    ${poses}    ${pose3}

    Log    Created ${poses} poses for navigation

    # Test navigation through poses (may fail if no robot)
    ${result}=    Navigate Through Poses    ${poses}    timeout=${NAV2_TIMEOUT}
    Log    Navigation result: ${result}

    # Check if navigation was successful
    IF    $result is not None
        Should Be True    $result.success
        Log    Navigation completed successfully
    ELSE
        Log    Navigation result is None
    END

Single Pose Navigation

Test Single Pose Navigation
    [Documentation]    Test navigation to a single pose
    [Tags]    navigation    single_pose
    [Setup]    Setup Navigation2 Simulation
    [Teardown]    Clean Up Navigation2 Simulation

    # Test navigation to a single pose (may fail if no robot)
    ${result}=    Navigate To Pose Simple    1.0    1.0    0.785    timeout=${NAV2_TIMEOUT}
    Log    Navigation result: ${result}

    # Check if navigation was successful
    IF    ${result} is not None
        Should Be True    ${result}
        Log    Navigation completed successfully
    ELSE
        Log    Navigation result is None
    END

Navigation Cancellation

Test Navigation Cancellation
    [Documentation]    Test navigation cancellation
    [Tags]    navigation    cancellation
    [Setup]    Setup Navigation2 Simulation
    [Teardown]    Clean Up Navigation2 Simulation

    # Test cancelling navigation
    ${cancelled}=    Cancel Navigation
    Log    Navigation cancelled: ${cancelled}

    # Verify navigation is not active after cancellation
    ${active}=    Is Navigation Active
    Should Be Equal    ${active}    ${False}

Setup and Teardown

The test includes comprehensive setup and teardown procedures:

Setup Navigation2 Simulation
    [Documentation]    Setup Navigation2 simulation
    ${running}=    Has Running Nodes
    Should Be Equal    ${running}    ${False}
    # Set environment variables for the test
    Set Environment Variable    TURTLEBOT3_MODEL      waffle

    # Clean up any existing simulation
    Clean Up Navigation2 Simulation

    # 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 basic test
robot examples/basics/nav2_basic_test.robot

# Run with verbose output
robot -v examples/basics/nav2_basic_test.robot

# Run specific test cases
robot -t "Test Basic Navigation Operations" examples/basics/nav2_basic_test.robot

Expected Output

The test will:

1. Verify Library Import: Ensure Navigation2 library is properly imported

2. Test Basic Operations: Verify distance and angle calculations

3. Test Pose Management: Set initial pose and check localization

4. Test Navigation Status: Monitor navigation status and activity

5. Test Costmap Operations: Retrieve and clear costmap information

6. Test Path Planning: Compute paths and calculate path lengths

7. Test Navigation: Navigate through multiple poses and single poses

8. Test Cancellation: Cancel navigation and verify status

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. Costmap errors: Verify costmap configuration

4. Path planning failures: Check map and robot configuration

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 Simple Monitoring example for advanced monitoring

• Check the Navigation2 TurtleBot3 example for complete TurtleBot3 testing

• Read the ../user_guide/nav2_operations for detailed Navigation2 usage

• Review the Navigation2 Client Library for complete API reference