From c28b85919407a214d295a46a5f5cd50e9005871d Mon Sep 17 00:00:00 2001 From: Alexey Merzlyakov Date: Fri, 2 Dec 2022 12:32:32 +0300 Subject: [PATCH] Ensure that plugin initialization to be called before updating routines --- nav2_costmap_2d/CMakeLists.txt | 1 + nav2_costmap_2d/src/costmap_2d_ros.cpp | 39 ++++++------ .../test/regression/CMakeLists.txt | 20 +++++++ .../test/regression/order_layer.cpp | 60 +++++++++++++++++++ .../test/regression/order_layer.hpp | 46 ++++++++++++++ .../test/regression/order_layer.xml | 7 +++ .../test/regression/plugin_api_order.cpp | 53 ++++++++++++++++ 7 files changed, 208 insertions(+), 18 deletions(-) create mode 100644 nav2_costmap_2d/test/regression/order_layer.cpp create mode 100644 nav2_costmap_2d/test/regression/order_layer.hpp create mode 100644 nav2_costmap_2d/test/regression/order_layer.xml create mode 100644 nav2_costmap_2d/test/regression/plugin_api_order.cpp diff --git a/nav2_costmap_2d/CMakeLists.txt b/nav2_costmap_2d/CMakeLists.txt index 96b4f493393..683cb04fdd2 100644 --- a/nav2_costmap_2d/CMakeLists.txt +++ b/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) diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index 50e74c511b2..52b538b6bb8 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/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; + } } } diff --git a/nav2_costmap_2d/test/regression/CMakeLists.txt b/nav2_costmap_2d/test/regression/CMakeLists.txt index c492459d2ff..c9a52aa8af7 100644 --- a/nav2_costmap_2d/test/regression/CMakeLists.txt +++ b/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 +) diff --git a/nav2_costmap_2d/test/regression/order_layer.cpp b/nav2_costmap_2d/test/regression/order_layer.cpp new file mode 100644 index 00000000000..6e69e210a72 --- /dev/null +++ b/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 +#include + +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) diff --git a/nav2_costmap_2d/test/regression/order_layer.hpp b/nav2_costmap_2d/test/regression/order_layer.hpp new file mode 100644 index 00000000000..35bb2aeabb7 --- /dev/null +++ b/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_ diff --git a/nav2_costmap_2d/test/regression/order_layer.xml b/nav2_costmap_2d/test/regression/order_layer.xml new file mode 100644 index 00000000000..ffc553a1522 --- /dev/null +++ b/nav2_costmap_2d/test/regression/order_layer.xml @@ -0,0 +1,7 @@ + + + + Plugin checking order of activate() and updateBounds()/updateCosts() calls + + + diff --git a/nav2_costmap_2d/test/regression/plugin_api_order.cpp b/nav2_costmap_2d/test/regression/plugin_api_order.cpp new file mode 100644 index 00000000000..c7baf34453a --- /dev/null +++ b/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 +#include +#include + +#include +#include + +TEST(CostmapPluginsTester, checkPluginAPIOrder) +{ + std::shared_ptr costmap_ros = + std::make_shared("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 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(); +}