controller_server node crash at cleanup if no control requested since the start

[BriceRenaudeau]

Bug report

controller_server node crash at cleanup if no control requested since the start.
It is due to the variable current_goal_checker_ not being initialized. It is initialised only in computeControl().

Since #2269

Required Info:

• Operating System:
  Ubuntu 20.04
• ROS2 Version:
  Galactic
• Version or commit hash:
  main
• DDS implementation:
  Cyclone

Steps to reproduce issue

Start and active full navigation stack.
Then deactivate and clean the lifecycle nodes to trigger the controller cleanup method

Expected behavior

The controller node doesn't crash when cleaning up.

Actual behavior

The controller node crashes when cleaning up.

Implementation considerations

Can be solved by initializing current_goal_checker_ in on_configure.
We can use the first element of the list as default value.
https://github.com/ros-planning/navigation2/blob/c88f45ac60a56bfed2c8e170fa89d42d43352c2a/nav2_controller/src/nav2_controller.cpp#L77-L99

current_goal_checker_ = goal_checker_ids_[0];

[doisyg]

Makes me think that some tests should probably be created for testing the stability of the lifecycle transition for each lifecycle nodes

[SteveMacenski]

Good point, it should be replaced with the same logic used for the controller plugins:

  ControllerMap::iterator it;
          for (it = controllers_.begin(); it != controllers_.end(); ++it) {
            it->second->cleanup();
          }
          controllers_.<span class="pl-c1">clear</span>();

From there: https://github.com/ros-planning/navigation2/blob/main/nav2_controller/src/nav2_controller.cpp#L254-L258

All should be reset and then they should all be cleared in the vector, no need to call out the "current". If you can get that in by EOD today, it can be included in the initial release for galactic

[SteveMacenski]

#2354 resolves

[BriceRenaudeau]

The fix #2354 wasn't applied to the Galactic branch.

[SteveMacenski]

That's super odd, OK, I can take care of that, I'm concerned now about what other commits may have been lost, will look into this today.

[SteveMacenski]

ros/rosdistro#29645

New release should contain it in the first Galactic sync