From 070fb226b0751c10e502bec4c97560360c21e31b Mon Sep 17 00:00:00 2001
From: MartiBolet <43337758+MartiBolet@users.noreply.github.com>
Date: Sun, 20 Nov 2022 09:05:06 +0100
Subject: [PATCH] Solve bug when CostmapInfoServer is reactivated (#3292)

* Solve bug when CostmapInfoServer is reactivated
---
 .../costmap_filter_info_server.hpp            |  2 +-
 .../costmap_filter_info_server.cpp            | 17 +++++-----
 .../unit/test_costmap_filter_info_server.cpp  | 31 +++++++++++++++++++
 3 files changed, 41 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<nav2_msgs::msg::CostmapFilterInfo>::SharedPtr publisher_;
 
-  std::unique_ptr<nav2_msgs::msg::CostmapFilterInfo> 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<nav2_msgs::msg::CostmapFilterInfo>(
     filter_info_topic, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable());
 
-  msg_ = std::make_unique<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<float>(get_parameter("base").as_double());
-  msg_->multiplier = static_cast<float>(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<float>(get_parameter("base").as_double());
+  msg_.multiplier = static_cast<float>(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<nav2_msgs::msg::CostmapFilterInfo>(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..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
@@ -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,24 @@ TEST_F(InfoServerTester, testCostmapFilterInfoPublish)
   EXPECT_NEAR(info_->base, BASE, EPSILON);
   EXPECT_NEAR(info_->multiplier, MULTIPLIER, EPSILON);
 }
+
+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);
+}