Nav2: How to determine whether a goal is finished?

Navigation Stack
, ,
1

I’m not an expert on ROS2. I’m trying to achieve the following from a Java application (using ros2-java). Mind that actions are not (yet) supported in ros2-java.

  1. Send navigation goal to a navigation goal
  2. Determine whether the robot arrived to the navigation goal.

I achieved the first point by using a publisher to post a message on move_base_simple/goal, which gets picked up by an action client (/bt_navigator_rclpp_node).
When I post to move_base_simple/goal, the robot calculates a route and drives to the given coordinates, which is great.

However, I cannot manage to determine whether the robot is finished with its navigation action.

In the console, messages such as ‘Navigation succeeded’ are thrown by /tmp/binarydeb/ros-dashing-nav2-bt-navigator-0.2.6/src/bt_navigator.cpp:navigateToPose:206.

But these messages are not thrown in the topic /NavigateToPose/_action/status, where gives the following data:

/niels@laptop:~$ ros2 topic echo /NavigateToPose/_action/status
status_list:
- goal_info:
    goal_id:
      uuid: [252, 126, 236, 51, 95, 235, 87, 156, 252, 98, 102, 225, 45, 118, 160, 13]
    stamp:
      sec: 1586252361
      nanosec: 703727920
  status: 6
- goal_info:
    goal_id:
      uuid: [41, 55, 100, 99, 191, 18, 167, 153, 57, 243, 1, 90, 68, 37, 116, 3]
    stamp:
      sec: 1586252365
      nanosec: 466342743
  status: 4
- goal_info:
    goal_id:
      uuid: [236, 4, 47, 230, 142, 173, 162, 72, 214, 90, 203, 227, 250, 192, 60, 180]
    stamp:
      sec: 1586253246
      nanosec: 14198261
  status: 4
- goal_info:
    goal_id:
      uuid: [15, 142, 123, 28, 148, 183, 150, 200, 227, 38, 255, 190, 197, 21, 148, 231]
    stamp:
      sec: 1586253278
      nanosec: 662794261
  status: 4
---

I get similar (but more frequent) data on topic /FollowPath/_action/status. Can I assume that the status codes correspond with the list that is visible on this page for ROS1? This would mean that status ‘4’ equals SUCCEEDED. However, status code 4 gets thrown multiple times, which I find remarkable.

My question, how do I know (for sure) if a navigation goal is succeeded? Is there a certain topic I missed? Or is there a certain service I need to call?

1 Like
2

@niels

i guess that you can refer to ros2 action design.
there is a service provided by action server to get result, i believe that this is exactly what you need.

hope this helps,

3

@tomoyafujita
Thank you for your response. I figured it out.
Here follows information for people who run into the same problem:

I can now confirm that the ROS 2 goal_info status codes are equal to the (ROS 1) actionlib status codes (see here)

Also, the messages in /NavigateToPose/_action/status mean a list containing all navigation goals, include old, already completed, navigation goals. The list is sort from newest to oldest entries. In my case, I’m only interested in the ‘current’ (newest) navigation goal, so I always take the first entry in the list. Using the status of the first entry, I can determine whether my navigation goal has finished.

4

@niels

Just one comment,

The list is sort from newest to oldest entries.

i do not think this is guaranteed. It just happens to do in that way, cz i do not see any sorting code in the implementation in rcl action.