nav2 action integration: plan gets aborted as soon as the second goal on the path is sent

[matlabbe]

To reproduce, we can launch turtlebot4 demo:

ros2 launch rtabmap_demos turtlebot4_ignition_demo.launch.py

Add a new goal button in rviz with /goal as the topic. Send a couple of goals to extend the graph > 15-20 meters (add MapGraph display to see the graph). When the graph is big enough, send a goal in the opposite direction, so that rtabmap creates some waypoints to reach the goal.

Here is an example of log, the path is planned and first goal sent to nav2:

[rviz2-58] [INFO] [1725944848.846025169] [rviz2]: Setting goal pose: Frame:map, Position(-2.59897, 4.35065, 0), Orientation(0, 0, 0.632893, 0.77424) = Angle: 1.37057
[rtabmap-3] [INFO] [1725944848.846605650] [rtabmap]: Planning: set goal xyz=-2.598968,4.350654,0.000000 rpy=0.000000,-0.000000,1.370567
[rtabmap-3] [INFO] [1725944848.846679644] [rtabmap]: Planning: Time computing path = 0.000091 s
[rtabmap-3] [INFO] [1725944848.846696203] [rtabmap]: Planning: Path successfully created (size=1)
[rtabmap-3] [INFO] [1725944848.846718133] [rtabmap]: Publishing next goal: 15 -> xyz=-2.598968,4.350655,0.000000 rpy=0.000000,-0.000000,1.370567
[rtabmap-3] [INFO] [1725944848.846746807] [rtabmap]: Connecting to navigate_to_pose action server...
[rtabmap-3] [INFO] [1725944848.867948405] [rtabmap]: Global path: [15]
[bt_navigator-54] [INFO] [1725944848.867988322] [bt_navigator]: Begin navigating from current location (-0.33, 0.00) to (-2.60, 4.35)
[rtabmap-3] [INFO] [1725944848.869537792] [rtabmap]: Goal accepted by server, waiting for result
[controller_server-50] [INFO] [1725944848.888918417] [controller_server]: Received a goal, begin computing control effort.
[controller_server-50] [WARN] [1725944848.888979628] [controller_server]: No goal checker was specified in parameter 'current_goal_checker'. Server will use only plugin loaded general_goal_checker . This warning will appear once.
[controller_server-50] [INFO] [1725944849.939150100] [controller_server]: Passing new path to controller.

After each rtabmap update, rtabmap publishes the same or new goal on the path:

[rtabmap-3] [INFO] [1725944851.115946610] [rtabmap]: Publishing next goal: 15 -> xyz=-2.599640,4.350494,0.000000 rpy=0.000000,-0.000000,1.370578
[bt_navigator-54] [INFO] [1725944851.118231400] [bt_navigator]: Received goal preemption request
[bt_navigator-54] [INFO] [1725944851.118592778] [bt_navigator]: Begin navigating from current location (-0.67, 0.29) to (-2.60, 4.35)
[rtabmap-3] [INFO] [1725944851.121408676] [rtabmap]: Goal accepted by server, waiting for result
[rtabmap-3] [ERROR] [1725944851.121992618] [rtabmap]: Planning: nav2 failed for some reason: Aborted. Aborting the plan...

rtabmap receives an Aborted state and thinks the plan failed, so it fails consecutively, not sending anymore goals on the path. Note that nav2 still continues towards the last goal received and eventually reaches it:

[controller_server-50] [INFO] [1725944871.142519833] [controller_server]: Reached the goal!
[bt_navigator-54] [INFO] [1725944871.178164447] [bt_navigator]: Goal succeeded

The answer could be related to this post: ros2/design#284, not sure in the recent years there was a workaround implemented on nav2 side. The expected behavior is the same than on ros1, move_base won't send back an aborted state if it receives a new goal while navigating to previous one, so rtabmap was happy and kept sending goals on the path.

A workaround is to connect goal_out topic from rtabmap (with use_action_for_goal:=false to not use nav2 action server) to nav2 input goal topic. That way, rtabmap won't receive any status from nav2 and keep sending goals. The downside is that if nav2 really fails to reach a goal, rtabmap will never know (it will eventually abort the plan if RGBD/PlanStuckIterations is set).

[matlabbe]

Reading ClientGoalHandle API, it seems each goal sent has it own unique ID. We could ignore "Aborted" results if they are not from the last goal sent.