From 49e0b84c053d16e01f5b937e19f1d2244798f796 Mon Sep 17 00:00:00 2001 From: jagutic Date: Fri, 13 Mar 2026 20:21:21 +0100 Subject: [PATCH] Internal static map <-> navstate static map --- .../CostmapMapsManager.hpp | 7 ++++++- .../CostmapMapsManager.cpp | 21 ++++++++++++++++--- 2 files changed, 24 insertions(+), 4 deletions(-) diff --git a/maps_managers/easynav_costmap_maps_manager/include/easynav_costmap_maps_manager/CostmapMapsManager.hpp b/maps_managers/easynav_costmap_maps_manager/include/easynav_costmap_maps_manager/CostmapMapsManager.hpp index b36ee8ff..f447be97 100644 --- a/maps_managers/easynav_costmap_maps_manager/include/easynav_costmap_maps_manager/CostmapMapsManager.hpp +++ b/maps_managers/easynav_costmap_maps_manager/include/easynav_costmap_maps_manager/CostmapMapsManager.hpp @@ -93,10 +93,15 @@ class CostmapMapsManager : public easynav::MapsManagerBase Costmap2D static_map_; /** - * @brief Internal static map. + * @brief Internal dynamic map. */ std::shared_ptr dynamic_map_; + /** + * @brief Flag to update navstate + */ + bool update_map_static_ = false; + /** * @brief Publisher for the static occupancy grid. */ diff --git a/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/CostmapMapsManager.cpp b/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/CostmapMapsManager.cpp index ea4e4f14..3235b0d2 100644 --- a/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/CostmapMapsManager.cpp +++ b/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/CostmapMapsManager.cpp @@ -135,6 +135,7 @@ CostmapMapsManager::on_initialize() static_grid_msg_ = *msg; static_map_ = Costmap2D(*msg); + update_map_static_ = true; static_grid_msg_.header.frame_id = tf_info.map_frame; static_grid_msg_.header.stamp = this->get_node()->now(); @@ -178,9 +179,25 @@ void CostmapMapsManager::update(NavState & nav_state) { EASYNAV_TRACE_EVENT; + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); - if (!nav_state.has("map.static")) { + // Update navstate static map with internal map + if (update_map_static_ || !nav_state.has("map.static")) { nav_state.set("map.static", static_map_); + update_map_static_ = false; + } + + // Update internal map with navstate static map + if (nav_state.has("map.static.update") && nav_state.get("map.static.update")) { + static_map_ = nav_state.get("map.static"); + nav_state.set("map.static.update", false); + + // Publish to static map topic + static_map_.toOccupancyGridMsg(static_grid_msg_); + + static_grid_msg_.header.frame_id = tf_info.map_frame; + static_grid_msg_.header.stamp = this->get_node()->now(); + static_occ_pub_->publish(static_grid_msg_); } if (!dynamic_map_) { @@ -201,8 +218,6 @@ CostmapMapsManager::update(NavState & nav_state) nav_state.set("map.dynamic", dynamic_map_); - const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); - rclcpp::Time map_stamp = nav_state.get("map_time"); dynamic_map_->toOccupancyGridMsg(dynamic_grid_msg_);