From 5d9f333d7f5b1ba3b12ee114bd395975e5844cd8 Mon Sep 17 00:00:00 2001 From: MartiBolet Date: Fri, 18 Nov 2022 13:27:36 +0100 Subject: [PATCH 1/2] Solve bug when CostmapInfoServer is reactivated Signed-off-by: MartiBolet --- .../costmap_filter_info_server.hpp | 2 +- .../costmap_filter_info_server.cpp | 17 +++++++++-------- .../unit/test_costmap_filter_info_server.cpp | 16 ++++++++++++++++ 3 files changed, 26 insertions(+), 9 deletions(-) diff --git a/nav2_map_server/include/nav2_map_server/costmap_filter_info_server.hpp b/nav2_map_server/include/nav2_map_server/costmap_filter_info_server.hpp index 99311deff7..14a69ad229 100644 --- a/nav2_map_server/include/nav2_map_server/costmap_filter_info_server.hpp +++ b/nav2_map_server/include/nav2_map_server/costmap_filter_info_server.hpp @@ -71,7 +71,7 @@ class CostmapFilterInfoServer : public nav2_util::LifecycleNode private: rclcpp_lifecycle::LifecyclePublisher::SharedPtr publisher_; - std::unique_ptr msg_; + nav2_msgs::msg::CostmapFilterInfo msg_; }; // CostmapFilterInfoServer } // namespace nav2_map_server diff --git a/nav2_map_server/src/costmap_filter_info/costmap_filter_info_server.cpp b/nav2_map_server/src/costmap_filter_info/costmap_filter_info_server.cpp index 7b1c618315..f6671b0291 100644 --- a/nav2_map_server/src/costmap_filter_info/costmap_filter_info_server.cpp +++ b/nav2_map_server/src/costmap_filter_info/costmap_filter_info_server.cpp @@ -48,13 +48,13 @@ CostmapFilterInfoServer::on_configure(const rclcpp_lifecycle::State & /*state*/) publisher_ = this->create_publisher( filter_info_topic, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); - msg_ = std::make_unique(); - msg_->header.frame_id = ""; - msg_->header.stamp = now(); - msg_->type = get_parameter("type").as_int(); - msg_->filter_mask_topic = get_parameter("mask_topic").as_string(); - msg_->base = static_cast(get_parameter("base").as_double()); - msg_->multiplier = static_cast(get_parameter("multiplier").as_double()); + msg_ = nav2_msgs::msg::CostmapFilterInfo(); + msg_.header.frame_id = ""; + msg_.header.stamp = now(); + msg_.type = get_parameter("type").as_int(); + msg_.filter_mask_topic = get_parameter("mask_topic").as_string(); + msg_.base = static_cast(get_parameter("base").as_double()); + msg_.multiplier = static_cast(get_parameter("multiplier").as_double()); return nav2_util::CallbackReturn::SUCCESS; } @@ -65,7 +65,8 @@ CostmapFilterInfoServer::on_activate(const rclcpp_lifecycle::State & /*state*/) RCLCPP_INFO(get_logger(), "Activating"); publisher_->on_activate(); - publisher_->publish(std::move(msg_)); + auto costmap_filter_info = std::make_unique(msg_); + publisher_->publish(std::move(costmap_filter_info)); // create bond connection createBond(); diff --git a/nav2_map_server/test/unit/test_costmap_filter_info_server.cpp b/nav2_map_server/test/unit/test_costmap_filter_info_server.cpp index 168fbc9f7b..66e9831b44 100644 --- a/nav2_map_server/test/unit/test_costmap_filter_info_server.cpp +++ b/nav2_map_server/test/unit/test_costmap_filter_info_server.cpp @@ -59,6 +59,16 @@ class InfoServerWrapper : public nav2_map_server::CostmapFilterInfoServer on_cleanup(get_current_state()); on_shutdown(get_current_state()); } + + void deactivate() + { + on_deactivate(get_current_state()); + } + + void activate() + { + on_activate(get_current_state()); + } }; class InfoServerTester : public ::testing::Test @@ -144,3 +154,9 @@ TEST_F(InfoServerTester, testCostmapFilterInfoPublish) EXPECT_NEAR(info_->base, BASE, EPSILON); EXPECT_NEAR(info_->multiplier, MULTIPLIER, EPSILON); } + +TEST_F(InfoServerTester, testCostmapFilterInfoDeactivateActivate) +{ + info_server_->deactivate(); + info_server_->activate(); +} From 25125d1924c72c699e616976603ea2dcbdcf82ee Mon Sep 17 00:00:00 2001 From: Alexey Merzlyakov <60094858+AlexeyMerzlyakov@users.noreply.github.com> Date: Sun, 20 Nov 2022 11:01:17 +0300 Subject: [PATCH 2/2] Update test_costmap_filter_info_server.cpp --- .../test/unit/test_costmap_filter_info_server.cpp | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/nav2_map_server/test/unit/test_costmap_filter_info_server.cpp b/nav2_map_server/test/unit/test_costmap_filter_info_server.cpp index 66e9831b44..072aaf8d1b 100644 --- a/nav2_map_server/test/unit/test_costmap_filter_info_server.cpp +++ b/nav2_map_server/test/unit/test_costmap_filter_info_server.cpp @@ -158,5 +158,20 @@ TEST_F(InfoServerTester, testCostmapFilterInfoPublish) TEST_F(InfoServerTester, testCostmapFilterInfoDeactivateActivate) { info_server_->deactivate(); + info_ = nullptr; info_server_->activate(); + + rclcpp::Time start_time = info_server_->now(); + while (!isReceived()) { + rclcpp::spin_some(info_server_->get_node_base_interface()); + std::this_thread::sleep_for(100ms); + // Waiting no more than 5 seconds + ASSERT_TRUE((info_server_->now() - start_time) <= rclcpp::Duration(5000ms)); + } + + // Checking received CostmapFilterInfo for consistency + EXPECT_EQ(info_->type, TYPE); + EXPECT_EQ(info_->filter_mask_topic, MASK_TOPIC); + EXPECT_NEAR(info_->base, BASE, EPSILON); + EXPECT_NEAR(info_->multiplier, MULTIPLIER, EPSILON); }