Costmap activation blocks for ever if required tf transform is not available

[kfabian]

Bug report

Required Info:

• Operating System:
  • Ubuntu 20.04
• ROS2 Version:
  • Galactic
• Version or commit hash:
  • 1.0.12-1focal.20221208.082109
• DDS implementation:
  • Cyclone DDS

Steps to reproduce issue

Activate the controller server without providing the transformation from robot_base_frame to global_frame

Expected behavior

The transition from configured to active fails

Actual behavior

The transitions blocks indefinitely

Additional information

In the costmaps on_activate method it is waited indefinitely for the transform from robot_base_frame to global_frame to become available. In my opinion a behavior where nav2_util::CallbackReturn::FAILURE is returned after some timeout would make more sense.

[SteveMacenski]

A timeout parameter could be made that could fail a transition after N seconds of trying. Should be a pretty trivial change in https://github.com/ros-planning/navigation2/blob/main/nav2_costmap_2d/src/costmap_2d_ros.cpp#L276. Would that be something you'd be open to contributing?

The only additional thing is to check that output in the planner / controller servers to make sure that the return type is sent back to the main server to fail its transition

[SteveMacenski]

Any update? 😄

[kfabian]

I think the change is not so trivial, because the costmap lifecycle node will then transition to the error state and you would need to check its current lifecycle state in the controller_server on_activate method and return an error if the costmap node is not in the active state after triggering the transition. You would also need to implement the on_error method for both nodes, if I got this right. Also I am afraid that contributing per se is still an issues for me, but I will check this again.

[SteveMacenski]

I think that you might be overthinking it, but I also could be missing an important detail :-)

The planner/controller servers call activate on the costmap node. Configure/Activate, according to the API returns the state which you would immediately know in the planner/controller server if there was an issue for itself to fail its own lifecycle transition. There might be some glue to pass around that false or throw/catch an exception to get to the right method, but I think that should be straight forward - no?

The costmap transitions mirror the main server's transitions, so as part of planner/controller configuring/activating, we can just pay attention to what the costmap is telling us and relay that as a failure to the main server's transitions if the costmap is unable to do so.

[SteveMacenski]

Merged!