Solve bug when CostmapInfoServer is reactivated

Signed-off-by: MartiBolet <mboletboixeda@gmail.com>
ros-navigation · Nov 18, 2022 · 5d9f333 · 5d9f333
1 parent 5fa4abc
commit 5d9f333
Show file tree

Hide file tree

Showing 3 changed files with 26 additions and 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
@@ -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
@@ -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
@@ -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();
+}