[ros2-master] controller_server on_cleanup crash with TEB

[doisyg]

Hi, I just started playing with the lifecycle manager functionality of ROS2. It looks like there is an error in the TEB implementation. If I try to desactivate a controller_server using TEB:

ros2 service call /controller_server/change_state lifecycle_msgs/srv/ChangeState "transition: id: 4 label: ''"

I get the following crash:

[controller_server-51] terminate called after throwing an instance of 'std::system_error' [controller_server-51] what(): Invalid argument [ERROR] [controller_server-51]: process has died [pid 508395, exit code -6, cmd '/home/gd/elodie2_ws/install/nav2_controller/lib/nav2_controller/controller_server --ros-args --params-file *****

It looks like it crashes at this line: https://github.com/ros-planning/navigation2/blob/6096221d513a2d66ffdf96adcb87b39ca20c2ddd/nav2_controller/src/nav2_controller.cpp#L236

No issue when using another controller. Propably caused by the current lifecycle implementation of TEB: https://github.com/rst-tu-dortmund/teb_local_planner/blob/8e4be1bc2fa9789db564a10c1312382a574089fb/teb_local_planner/src/teb_local_planner_ros.cpp#L1248-L1263

[SteveMacenski]

The destructor for TEB is empty, which I would have expected to have something deleted that was already deleted, but that did not appear to be the case

[doisyg]

The destructor for TEB is empty, which I would have expected to have something deleted that was already deleted, but that did not appear to be the case

Almost! This is actually related to the costmap_converter (what would we do without gdb backtrace...). When desactivating the controller, BaseCostmapToPolygons::stopWorker() is called twice:

1. In teb cleanup() : https://github.com/rst-tu-dortmund/teb_local_planner/blob/8e4be1bc2fa9789db564a10c1312382a574089fb/teb_local_planner/src/teb_local_planner_ros.cpp#L1258-L1263 AND
2. In BaseCostmapToPolygons destructor:

virtual ~BaseCostmapToPolygons() 
{
  stopWorker();
}

https://github.com/rst-tu-dortmund/costmap_converter/blob/9565858432664e9cfe0b3556e1809336c4ab06d4/costmap_converter/include/costmap_converter/costmap_converter_interface.h#L94-L97

Hence the second time spin_thread_->join(); crashes as it is not joinable anymore.

Should we remove the first call or is there something more fundamentally wrong? (I am slowly learning the lifecycles ways)

[SteveMacenski]

Stopping in cleanup seems pretty proper to me, but in case people don't use lifecycle states, I suppose its right to be in the destructor. But looking at the code the stopWorker checks if worker_timer_ and spin_thread_ are non-null before doing anything, this should be OK to call twice?

[doisyg]

But looking at the code the stopWorker checks if worker_timer_ and spin_thread_ are non-null before doing anything, this should be OK to call twice?

I checked and spin_thread_ is not null after a delete. I propose the following fix : https://github.com/rst-tu-dortmund/costmap_converter/pull/27

[SteveMacenski]

Seems reasonable, though I don't have push access to that

[SteveMacenski]

@doisyg should we break the lifecycle encapsulation a bit and just let the costmap converter handle itself if we can't get things merged into it?

[doisyg]

I am not sure I understand what you mean " let the costmap converter handle itself". Though as the issue is due to the costmap converter, we can treat it as independent from TEB (if that's what you mean). Moreover, using the costmap converter is optional in TEB

[SteveMacenski]

yeah that's what I mean, don't mess with it in the lifecycle state transitions, let it destroy itself the way it wants to internally