Ensure that plugin initialization to be called before updating routines

nav2_costmap_2d/CMakeLists.txt

@@ -180,6 +180,7 @@ if(BUILD_TESTING)
 
   find_package(ament_cmake_gtest REQUIRED)
   add_subdirectory(test)
+  pluginlib_export_plugin_description_file(nav2_costmap_2d test/regression/order_layer.xml)
 endif()
 
 ament_export_include_directories(include)

nav2_costmap_2d/src/costmap_2d_ros.cpp

@@ -435,24 +435,27 @@ Costmap2DROS::mapUpdateLoop(double frequency)
   while (rclcpp::ok() && !map_update_thread_shutdown_) {
     nav2_util::ExecutionTimer timer;
 
-    // Measure the execution time of the updateMap method
-    timer.start();
-    updateMap();
-    timer.end();
-
-    RCLCPP_DEBUG(get_logger(), "Map update time: %.9f", timer.elapsed_time_in_seconds());
-    if (publish_cycle_ > rclcpp::Duration(0s) && layered_costmap_->isInitialized()) {
-      unsigned int x0, y0, xn, yn;
-      layered_costmap_->getBounds(&x0, &xn, &y0, &yn);
-      costmap_publisher_->updateBounds(x0, xn, y0, yn);
-
-      auto current_time = now();
-      if ((last_publish_ + publish_cycle_ < current_time) ||  // publish_cycle_ is due
-        (current_time < last_publish_))      // time has moved backwards, probably due to a switch to sim_time // NOLINT
-      {
-        RCLCPP_DEBUG(get_logger(), "Publish costmap at %s", name_.c_str());
-        costmap_publisher_->publishCostmap();
-        last_publish_ = current_time;
+    // Execute after start() will complete plugins activation
+    if (!stopped_) {
+      // Measure the execution time of the updateMap method
+      timer.start();
+      updateMap();
+      timer.end();
+
+      RCLCPP_DEBUG(get_logger(), "Map update time: %.9f", timer.elapsed_time_in_seconds());
+      if (publish_cycle_ > rclcpp::Duration(0s) && layered_costmap_->isInitialized()) {
+        unsigned int x0, y0, xn, yn;
+        layered_costmap_->getBounds(&x0, &xn, &y0, &yn);
+        costmap_publisher_->updateBounds(x0, xn, y0, yn);
+
+        auto current_time = now();
+        if ((last_publish_ + publish_cycle_ < current_time) ||  // publish_cycle_ is due
+          (current_time < last_publish_))      // time has moved backwards, probably due to a switch to sim_time // NOLINT
+        {
+          RCLCPP_DEBUG(get_logger(), "Publish costmap at %s", name_.c_str());
+          costmap_publisher_->publishCostmap();
+          last_publish_ = current_time;
+        }
       }
     }
 

nav2_costmap_2d/test/regression/CMakeLists.txt

@@ -1,4 +1,24 @@
+# Bresenham2D corner cases test
 ament_add_gtest(costmap_bresenham_2d costmap_bresenham_2d.cpp)
 target_link_libraries(costmap_bresenham_2d
   nav2_costmap_2d_core
 )
+
+# OrderLayer for checking Costmap2D plugins API calling order
+add_library(order_layer SHARED
+            order_layer.cpp)
+ament_target_dependencies(order_layer
+  ${dependencies}
+)
+install(TARGETS
+  order_layer
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+# Costmap2D plugins API calling order test
+ament_add_gtest(plugin_api_order plugin_api_order.cpp)
+target_link_libraries(plugin_api_order
+  nav2_costmap_2d_core
+)

nav2_costmap_2d/test/regression/order_layer.cpp

@@ -0,0 +1,60 @@
+// Copyright (c) 2022 Samsung R&D Institute Russia
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License. Reserved.
+
+#include "order_layer.hpp"
+
+#include <chrono>
+#include <stdexcept>
+
+using namespace std::chrono_literals;
+
+namespace nav2_costmap_2d
+{
+
+OrderLayer::OrderLayer()
+: activated_(false)
+{
+}
+
+void OrderLayer::activate()
+{
+  std::this_thread::sleep_for(100ms);
+  activated_ = true;
+}
+
+void OrderLayer::deactivate()
+{
+  activated_ = false;
+}
+
+void OrderLayer::updateBounds(
+  double, double, double, double *, double *, double *, double *)
+{
+  if (!activated_) {
+    throw std::runtime_error("update before activated");
+  }
+}
+
+void OrderLayer::updateCosts(
+  nav2_costmap_2d::Costmap2D &, int, int, int, int)
+{
+  if (!activated_) {
+    throw std::runtime_error("update before activated");
+  }
+}
+
+}  // namespace nav2_costmap_2d
+
+#include "pluginlib/class_list_macros.hpp"
+PLUGINLIB_EXPORT_CLASS(nav2_costmap_2d::OrderLayer, nav2_costmap_2d::Layer)

nav2_costmap_2d/test/regression/order_layer.hpp

@@ -0,0 +1,46 @@
+// Copyright (c) 2022 Samsung R&D Institute Russia
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License. Reserved.
+
+#ifndef NAV2_COSTMAP_2D__ORDER_LAYER_HPP_
+#define NAV2_COSTMAP_2D__ORDER_LAYER_HPP_
+
+#include "nav2_costmap_2d/layer.hpp"
+
+namespace nav2_costmap_2d
+{
+
+class OrderLayer : public nav2_costmap_2d::Layer
+{
+public:
+  OrderLayer();
+
+  virtual void activate();
+  virtual void deactivate();
+
+  virtual void reset() {}
+  virtual bool isClearable() {return false;}
+
+  virtual void updateBounds(
+    double, double, double, double *, double *, double *, double *);
+
+  virtual void updateCosts(
+    nav2_costmap_2d::Costmap2D &, int, int, int, int);
+
+private:
+  bool activated_;
+};
+
+}  // namespace nav2_costmap_2d
+
+#endif  // NAV2_COSTMAP_2D__ORDER_LAYER_HPP_

nav2_costmap_2d/test/regression/order_layer.xml

@@ -0,0 +1,7 @@
+<class_libraries>
+  <library path="order_layer">
+    <class type="nav2_costmap_2d::OrderLayer" base_class_type="nav2_costmap_2d::Layer">
+      <description>Plugin checking order of activate() and updateBounds()/updateCosts() calls</description>
+    </class>
+  </library>
+</class_libraries>

nav2_costmap_2d/test/regression/plugin_api_order.cpp

@@ -0,0 +1,53 @@
+// Copyright (c) 2022 Samsung R&D Institute Russia
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License. Reserved.
+
+#include <string>
+#include <vector>
+#include <memory>
+
+#include <nav2_costmap_2d/costmap_2d_ros.hpp>
+#include <gtest/gtest.h>
+
+TEST(CostmapPluginsTester, checkPluginAPIOrder)
+{
+  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros =
+    std::make_shared<nav2_costmap_2d::Costmap2DROS>("costmap_ros");
+
+  // Workaround to avoid setting base_link->map transform
+  costmap_ros->set_parameter(rclcpp::Parameter("robot_base_frame", "map"));
+  // Specifying order verification plugin in the parameters
+  std::vector<std::string> plugins_str;
+  plugins_str.push_back("order_layer");
+  costmap_ros->set_parameter(rclcpp::Parameter("plugins", plugins_str));
+  costmap_ros->declare_parameter(
+    "order_layer.plugin",
+    rclcpp::ParameterValue(std::string("nav2_costmap_2d::OrderLayer")));
+
+  // Do actual test: ensure that plugin->updateBounds()/updateCosts()
+  // will be called after plugin->activate()
+  costmap_ros->on_configure(costmap_ros->get_current_state());
+  costmap_ros->on_activate(costmap_ros->get_current_state());
+
+  // Do cleanup
+  costmap_ros->on_deactivate(costmap_ros->get_current_state());
+  costmap_ros->on_cleanup(costmap_ros->get_current_state());
+  costmap_ros->on_shutdown(costmap_ros->get_current_state());
+}
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}