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();
+}