[Nav2] New Python3 Nav2 API To build Applications

Navigation Stack
1

Hi all, your friendly neighborhood navigator here.

As part of the 2021 ROS Developers Day https://www.theconstructsim.com/ros-developers-day-2021/ I prepared some demonstrations of capabilities in Nav2. To make it more approachable, I created a generic python3 BasicNavigator object to do interact with the Nav2 stack to make these demos more straight forward.

I decided along the way this is something that would likely gain a great amount of reuse for other users, so I’ve created the new nav2_python_commander package in Nav2 containing python3 methods to do most everything with Nav2 in order to build basic applications (goToPose(), changeMap(), setInitialPose(), followWaypoints(), etc). The README.md file contains a great deal of context, demos, examples, and the API provided. There are also some example tasks like security, picking, and inspection to show-case how the API makes building basic POCs fast and simple.

I am, however, not primarily a python3 developer. I would appreciate if community members could take a look at the PR below and leave a review to make sure I’m best using the Python3 capabilities / tools / styling so more pythonic users don’t eye-roll me later :slight_smile: . If you have a few minutes and are a python3 developer, please leave me a review.

An example application to go to a pose:


def main():
    rclpy.init()

    navigator = BasicNavigator()

    # Set our demo's initial pose
    initial_pose = PoseStamped()
    initial_pose.header.frame_id = 'map'
    initial_pose.header.stamp = navigator.get_clock().now().to_msg()
    initial_pose.pose.position.x = 3.45
    initial_pose.pose.position.y = 2.15
    initial_pose.pose.orientation.z = 1.0
    initial_pose.pose.orientation.w = 0.0
    navigator.setInitialPose(initial_pose)

    # Wait for navigation to fully activate, since autostarting nav2
    navigator.waitUntilNav2Active()

    # Go to our demos first goal pose
    goal_pose = PoseStamped()
    goal_pose.header.frame_id = 'map'
    goal_pose.header.stamp = navigator.get_clock().now().to_msg()
    goal_pose.pose.position.x = -2.0
    goal_pose.pose.position.y = -0.5
    goal_pose.pose.orientation.w = 1.0

    navigator.goToPose(goal_pose)

    while not navigator.isNavComplete():
        # Do something with the feedback
        feedback = navigator.getFeedback()

        # Some navigation timeout to demo cancellation
        if Duration.from_msg(feedback.navigation_time) > Duration(seconds=600.0):
            navigator.cancelNav()

    # Do something depending on the return code
    result = navigator.getResult()
    if result == NavigationResult.SUCCEEDED:
        print('Goal succeeded!')
    elif result == NavigationResult.CANCELED:
        print('Goal was canceled!')
    elif result == NavigationResult.FAILED:
        print('Goal failed!')

    navigator.lifecycleShutdown()

I hope to have this merged in and ready for use by the end of the month! If there’s any functions missing, please also drop me a note in the PR and I’ll make sure to add it. I think this is a pretty exhaustive look at the available actions / services in Nav2.

Happy pythoning,

Steve

8 Likes
2

Thanks to @Timple and @klintan for spending the time and reviewing it! It has now been merged under the new name nav2_simple_commander! A new section on https://navigation.ros.org/ will show the API description as well shortly!

1 Like