goal that is aborted needs to be cancelled before another NavigateToPose request is processed by controller_server

[Jconn]

Bug report

Required Info:

• Operating System:
  Ubuntu 18.04
• Version or commit hash:
  fd2955a
• DDS implementation:
  Fast-RTPS

Steps to reproduce issue

Set a navigation goal through rviz, wait for the goal to fail due to Failed to make progress

Set another navigation goal through rviz.

Expected behavior

Actual behavior

After the first goal is aborted, the second goal doesn't appear to be picked up by the controller server.
If I cancel the first goal (which I do through rviz), and then send the second goal again, everything works okay.

Additional information

Here is the log output from this behavior. The relevant part is after that first Aborting Handle I can only get the controller server to start spitting log info after I cancel the goal. The Received goal preemption request are from me sending NavigateToPose requests through the rviz button

[bt_navigator-14] [INFO] [1591369067.853691380] [bt_navigator]: Begin navigating from current location to (2.24, -0.34)
[controller_server-11] [INFO] [1591369067.858775641] [controller_server]: Received a goal, begin computing control effort.
[controller_server-11] [INFO] [1591369067.859614374] [controller_server]: Passing new path to controller.
[controller_server-11] [INFO] [1591369067.909798072] [controller_server]: Passing new path to controller.
[controller_server-11] [INFO] [1591369068.909685192] [controller_server]: Passing new path to controller.
[controller_server-11] [INFO] [1591369069.959686889] [controller_server]: Passing new path to controller.
[controller_server-11] [INFO] [1591369070.959683377] [controller_server]: Passing new path to controller.
[controller_server-11] [INFO] [1591369072.009687667] [controller_server]: Passing new path to controller.
[controller_server-11] [INFO] [1591369073.059711623] [controller_server]: Passing new path to controller.
[controller_server-11] [INFO] [1591369074.059691576] [controller_server]: Passing new path to controller.
[controller_server-11] [INFO] [1591369075.109682494] [controller_server]: Passing new path to controller.
[controller_server-11] [INFO] [1591369076.109683507] [controller_server]: Passing new path to controller.
[controller_server-11] [ERROR] [1591369076.159813487] [tf_help]: Transform data too old when converting from odom to map
[controller_server-11] [ERROR] [1591369076.159853557] [tf_help]: Data time: 1591369076s 146044447ns, Transform time: 1591369075s 722273255ns
[controller_server-11] [ERROR] [1591369076.159997115] [controller_server]: Unable to transform robot pose into global plan's frame
[controller_server-11] [WARN] [1591369076.160043059] [controller_server_rclcpp_node]: [follow_path] [ActionServer] Aborting handle.
[bt_navigator-14] [INFO] [1591369088.545809046] [bt_navigator]: Received goal preemption request
[bt_navigator-14] [INFO] [1591369088.545923338] [bt_navigator]: Begin navigating from current location to (1.93, 0.86)
 [bt_navigator-14] [INFO] [1591369208.345823751] [bt_navigator]: Received goal preemption request
[bt_navigator-14] [INFO] [1591369208.345940658] [bt_navigator]: Begin navigating from current location to (1.66, -0.57)
[bt_navigator-14] [INFO] [1591369229.435885793] [bt_navigator]: Navigation canceled
[bt_navigator-14] [WARN] [1591369229.435957371] [bt_navigator]: [navigate_to_pose] [ActionServer] Client requested to cancel the goal. Cancelling.
[bt_navigator-14] [INFO] [1591369234.146704563] [bt_navigator]: Begin navigating from current location to (2.99, 0.71)
[controller_server-11] [INFO] [1591369234.151464547] [controller_server]: Received a goal, begin computing control effort.
[controller_server-11] [INFO] [1591369234.151858704] [controller_server]: Passing new path to controller.
[controller_server-11] [INFO] [1591369234.201926882] [controller_server]: Passing new path to controller.
[controller_server-11] [INFO] [1591369235.201928081] [controller_server]: Passing new path to controller.
[controller_server-11] [INFO] [1591369236.251935052] [controller_server]: Passing new path to controller.
[controller_server-11] [INFO] [1591369237.301928957] [controller_server]: Passing new path to controller.
[controller_server-11] [INFO] [1591369238.302994169] [controller_server]: Passing new path to controller.
[controller_server-11] [INFO] [1591369239.351935908] [controller_server]: Passing new path to controller.
[controller_server-11] [INFO] [1591369240.351930099] [controller_server]: Passing new path to controller.
[controller_server-11] [INFO] [1591369241.401947726] [controller_server]: Passing new path to controller.
[controller_server-11] [INFO] [1591369242.451976608] [controller_server]: Passing new path to controller.
[controller_server-11] [INFO] [1591369243.501903378] [controller_server]: Passing new path to controller.

[SteveMacenski]

@Jconn does this describe this issue? #1652 I think they're one in the same, but just want you to verify.

Work is in progress to resolve, but its going to take a little bit of time because we have to update ROS2 design docs, rcl, and rclcpp to support. @naiveHobo is the one looking over this at the moment but I'm also open to more actively taking a role here as well to accelerate this getting in.

[Jconn]

Yeah at first glance that looks like the issue, sorry the for the double post.

We are just starting to switch to foxy and master branch of nav, eloquent-devel has been working great for us (no news is good news).

It would be good to get this fixed, before that happens I will have to stay on eloquent unless there is a workaround

[SteveMacenski]

Does eloquent not show that issue? Maybe we can look at reverting something if that works. The process of getting the new updates will probably be at least another 4 weeks before its in master because we have to go through a bunch of stuff in rcl/rclcpp which are slower moving ships.

Can you post a summary of your issue in that ticket as a follow up to make sure we have all of it in 1 place?

no news is good news

I like good news :-)

[Jconn]

Does eloquent not show that issue

Eloquent does show this issue! This is my mistake, there is something else going on in my robot that is causing more nav goals to abort with foxy than eloquent.

I will post to the other ticket

[SteveMacenski]

ah ok, thanks!