Ensure that plugin initialization to be called before updating routines

[AlexeyMerzlyakov]

Fixes the situations when Costmap2D plugin is being processed before initialization

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#3299
Primary OS tested on	Ubuntu 20.04 with ROS2 Rolling (built from sources) onboard
Robotic platform tested on	colcon test of nav2_costmap_2d and nav2_system_tests: no regressions

---

Description of contribution in a few bullet points

• Ensure that plugin->updateCosts()/updateBounds() won't be called unless all plugins to be initialized: the Costmap2DROS::updateMap() is being processed in the main update loop only if stopped_ is false.
• Added the regression test for this case. There is a new OrderLayer costmap plugin that sleeps in the activate() stage checking that parallel thread won't call updateCosts()/updateBound() routines before activation will be finished. The testcase creates Costmap2DROS object with this plugin enabled and then calls it though the main Costmap2DROS callback API. If plugin API call sequence is wrong, OrderLayer will throw an exception that will cause testcase to fail. If API call sequence is OK, testcase will be finished normally.

Description of documentation updates required from your changes

• No documentation updates are required

---

Future work that may be required in bullet points

• No future work is required

For Maintainers:

• Check that any new parameters added are updated in navigation.ros.org
• Check that any significant change is added to the migration guide
• Check that any new features OR changes to existing behaviors are reflected in the tuning guide
• Check that any new functions have Doxygen added
• Check that any new features have test coverage
• Check that any new plugins is added to the plugins page
• If BT Node, Additionally: add to BT's XML index of nodes for groot, BT package's readme table, and BT library lists

[mergify]

@AlexeyMerzlyakov, please properly fill in PR template in the future. @SteveMacenski, use this instead.

• Check that any new parameters added are updated in navigation.ros.org
• Check that any significant change is added to the migration guide
• Check that any new features OR changes to existing behaviors are reflected in the tuning guide
• Check that any new functions have Doxygen added
• Check that any new features have test coverage
• Check that any new plugins is added to the plugins page
• If BT Node, Additionally: add to BT's XML index of nodes for groot, BT package's readme table, and BT library lists

[AlexeyMerzlyakov]

In a good way, this change should be checked through std::atomic<bool>, as well as other parallel accessed variables, such as initialized_ (https://github.com/ros-planning/navigation2/blob/main/nav2_costmap_2d/src/costmap_2d_ros.cpp#L494 and https://github.com/ros-planning/navigation2/blob/main/nav2_costmap_2d/src/costmap_2d_ros.cpp#L527). This is rather a subject of separate ticket, I think.