From ebf7405155a1fd6b13dadb54807d78f6dc63d39c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Fri, 24 Apr 2026 06:38:55 +0100 Subject: [PATCH 1/5] Adding timestamp to costmap MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- common/easynav_costmap_common/CMakeLists.txt | 3 + .../easynav_costmap_common/costmap_2d.hpp | 16 +++- common/easynav_costmap_common/package.xml | 1 + .../src/easynav_costmap_common/costmap_2d.cpp | 20 ++++- .../tests/costmap_2d_tests.cpp | 74 +++++++++++++++++++ 5 files changed, 109 insertions(+), 5 deletions(-) diff --git a/common/easynav_costmap_common/CMakeLists.txt b/common/easynav_costmap_common/CMakeLists.txt index fd5578d7..243de87a 100644 --- a/common/easynav_costmap_common/CMakeLists.txt +++ b/common/easynav_costmap_common/CMakeLists.txt @@ -12,6 +12,7 @@ set(CMAKE_CXX_EXTENSIONS OFF) find_package(ament_cmake REQUIRED) find_package(easynav_common REQUIRED) find_package(nav_msgs REQUIRED) +find_package(rclcpp REQUIRED) add_library(${PROJECT_NAME} SHARED @@ -24,6 +25,7 @@ target_include_directories(${PROJECT_NAME} PUBLIC target_link_libraries(${PROJECT_NAME} PUBLIC easynav_common::easynav_common ${nav_msgs_TARGETS} + rclcpp::rclcpp ) install( @@ -56,5 +58,6 @@ ament_export_dependencies( easynav_common # pluginlib nav_msgs + rclcpp ) ament_package() diff --git a/common/easynav_costmap_common/include/easynav_costmap_common/costmap_2d.hpp b/common/easynav_costmap_common/include/easynav_costmap_common/costmap_2d.hpp index 45394e9c..fa57947b 100644 --- a/common/easynav_costmap_common/include/easynav_costmap_common/costmap_2d.hpp +++ b/common/easynav_costmap_common/include/easynav_costmap_common/costmap_2d.hpp @@ -44,6 +44,7 @@ #include #include #include +#include "rclcpp/time.hpp" #include "geometry_msgs/msg/point.hpp" #include "nav_msgs/msg/occupancy_grid.hpp" @@ -126,10 +127,19 @@ class Costmap2D * * The resulting OccupancyGrid message contains metadata such as resolution, size, and origin, * and its data field is populated with cost values from the costmap. Cells with a value of - * NO_INFORMATION are mapped to -1; other values are cast to int8_t directly. + * NO_INFORMATION are mapped to -1; other values are cast to int8_t directly. + * + * The output message header stamp is set to the internal last-modified timestamp. */ void toOccupancyGridMsg(nav_msgs::msg::OccupancyGrid & msg) const; + /** + * @brief Get timestamp of last costmap modification. + * + * Currently only set by constructors and copy/assignment; other updates do not change it. + */ + rclcpp::Time getLastModifiedStamp() const; + /** * @brief Copies the (x0,y0)..(xn,yn) window from source costmap into a current costmap @param source Source costmap where the window will be copied from @@ -580,6 +590,10 @@ class Costmap2D unsigned char * costmap_; unsigned char default_value_; + // Timestamp indicating when the costmap was last modified. + // Currently only set by constructors and copy/assignment; other updates do not change it. + rclcpp::Time last_modified_; + // *INDENT-OFF* Uncrustify doesn't handle indented public/private labels class MarkCell { diff --git a/common/easynav_costmap_common/package.xml b/common/easynav_costmap_common/package.xml index 7f3c271f..d9267fa6 100644 --- a/common/easynav_costmap_common/package.xml +++ b/common/easynav_costmap_common/package.xml @@ -21,6 +21,7 @@ easynav_common nav_msgs + rclcpp ament_index_cpp ament_lint_auto diff --git a/common/easynav_costmap_common/src/easynav_costmap_common/costmap_2d.cpp b/common/easynav_costmap_common/src/easynav_costmap_common/costmap_2d.cpp index 91fd5928..53235782 100644 --- a/common/easynav_costmap_common/src/easynav_costmap_common/costmap_2d.cpp +++ b/common/easynav_costmap_common/src/easynav_costmap_common/costmap_2d.cpp @@ -51,7 +51,7 @@ Costmap2D::Costmap2D( unsigned int cells_size_x, unsigned int cells_size_y, double resolution, double origin_x, double origin_y, unsigned char default_value) : resolution_(resolution), origin_x_(origin_x), - origin_y_(origin_y), costmap_(NULL), default_value_(default_value) + origin_y_(origin_y), costmap_(NULL), default_value_(default_value), last_modified_(0) { access_ = new mutex_t(); @@ -61,7 +61,7 @@ Costmap2D::Costmap2D( } Costmap2D::Costmap2D(const nav_msgs::msg::OccupancyGrid & map) -: default_value_(FREE_SPACE) +: default_value_(FREE_SPACE), last_modified_(map.header.stamp) { access_ = new mutex_t(); @@ -210,6 +210,10 @@ Costmap2D::toOccupancyGridMsg(nav_msgs::msg::OccupancyGrid & msg) const { std::lock_guard lock(*access_); + const int64_t stamp_ns = last_modified_.nanoseconds(); + msg.header.stamp.sec = static_cast(stamp_ns / 1000000000LL); + msg.header.stamp.nanosec = static_cast(stamp_ns % 1000000000LL); + msg.info.width = size_x_; msg.info.height = size_y_; msg.info.resolution = resolution_; @@ -231,6 +235,12 @@ Costmap2D::toOccupancyGridMsg(nav_msgs::msg::OccupancyGrid & msg) const } } +rclcpp::Time Costmap2D::getLastModifiedStamp() const +{ + std::lock_guard lock(*access_); + return last_modified_; +} + Costmap2D & Costmap2D::operator=(const Costmap2D & map) { // check for self assignment @@ -247,6 +257,7 @@ Costmap2D & Costmap2D::operator=(const Costmap2D & map) origin_x_ = map.origin_x_; origin_y_ = map.origin_y_; default_value_ = map.default_value_; + last_modified_ = map.last_modified_; // initialize our various maps initMaps(size_x_, size_y_); @@ -258,7 +269,7 @@ Costmap2D & Costmap2D::operator=(const Costmap2D & map) } Costmap2D::Costmap2D(const Costmap2D & map) -: costmap_(NULL) +: costmap_(NULL), last_modified_(0) { access_ = new mutex_t(); *this = map; @@ -266,7 +277,8 @@ Costmap2D::Costmap2D(const Costmap2D & map) // just initialize everything to NULL by default Costmap2D::Costmap2D() -: size_x_(0), size_y_(0), resolution_(0.0), origin_x_(0.0), origin_y_(0.0), costmap_(NULL) +: size_x_(0), size_y_(0), resolution_(0.0), origin_x_(0.0), origin_y_(0.0), + costmap_(NULL), last_modified_(0) { access_ = new mutex_t(); } diff --git a/common/easynav_costmap_common/tests/costmap_2d_tests.cpp b/common/easynav_costmap_common/tests/costmap_2d_tests.cpp index a9ff17b8..49889c81 100644 --- a/common/easynav_costmap_common/tests/costmap_2d_tests.cpp +++ b/common/easynav_costmap_common/tests/costmap_2d_tests.cpp @@ -16,6 +16,7 @@ #include #include "easynav_costmap_common/costmap_2d.hpp" #include "nav_msgs/msg/occupancy_grid.hpp" +#include "rclcpp/time.hpp" using easynav::Costmap2D; @@ -121,3 +122,76 @@ TEST_F(Costmap2DTest, OccupancyGridConversion) EXPECT_EQ(grid.data[i], expected ? 100 : 0); } } + +TEST_F(Costmap2DTest, TimestampFromOccupancyGridConstructor) +{ + nav_msgs::msg::OccupancyGrid in; + in.header.stamp.sec = 123; + in.header.stamp.nanosec = 456u; + + in.info.width = 2u; + in.info.height = 3u; + in.info.resolution = 0.5; + in.info.origin.position.x = 1.0; + in.info.origin.position.y = -2.0; + in.data.assign(in.info.width * in.info.height, 0); + + Costmap2D map(in); + const int64_t expected_ns = 123LL * 1000000000LL + 456LL; + EXPECT_EQ(map.getLastModifiedStamp().nanoseconds(), expected_ns); + + nav_msgs::msg::OccupancyGrid out; + map.toOccupancyGridMsg(out); + + EXPECT_EQ(out.header.stamp.sec, in.header.stamp.sec); + EXPECT_EQ(out.header.stamp.nanosec, in.header.stamp.nanosec); +} + +TEST_F(Costmap2DTest, TimestampFromCopyConstructor) +{ + nav_msgs::msg::OccupancyGrid in; + in.header.stamp.sec = 10; + in.header.stamp.nanosec = 20u; + in.info.width = 1u; + in.info.height = 1u; + in.info.resolution = 1.0; + in.info.origin.position.x = 0.0; + in.info.origin.position.y = 0.0; + in.data.assign(1u, 0); + + Costmap2D original(in); + Costmap2D copy(original); + + const int64_t expected_ns = 10LL * 1000000000LL + 20LL; + EXPECT_EQ(copy.getLastModifiedStamp().nanoseconds(), expected_ns); + + nav_msgs::msg::OccupancyGrid out; + copy.toOccupancyGridMsg(out); + EXPECT_EQ(out.header.stamp.sec, in.header.stamp.sec); + EXPECT_EQ(out.header.stamp.nanosec, in.header.stamp.nanosec); +} + +TEST_F(Costmap2DTest, TimestampNotUpdatedByChanges) +{ + nav_msgs::msg::OccupancyGrid in; + in.header.stamp.sec = 7; + in.header.stamp.nanosec = 8u; + in.info.width = 3u; + in.info.height = 3u; + in.info.resolution = 1.0; + in.info.origin.position.x = 0.0; + in.info.origin.position.y = 0.0; + in.data.assign(in.info.width * in.info.height, 0); + + Costmap2D map(in); + const int64_t expected_ns = 7LL * 1000000000LL + 8LL; + map.setCost(1, 1, 100); + map.resizeMap(4u, 4u, 1.0, 1.0, 1.0); + + EXPECT_EQ(map.getLastModifiedStamp().nanoseconds(), expected_ns); + + nav_msgs::msg::OccupancyGrid out; + map.toOccupancyGridMsg(out); + EXPECT_EQ(out.header.stamp.sec, in.header.stamp.sec); + EXPECT_EQ(out.header.stamp.nanosec, in.header.stamp.nanosec); +} From d60661c058f4f0ff2bf821585a615aa997d7545c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Fri, 24 Apr 2026 06:56:05 +0100 Subject: [PATCH 2/5] Change map static to base MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../easynav_costmap_localizer/README.md | 2 +- .../AMCLLocalizer.cpp | 6 +-- .../tests/costmap_localizer_tests.cpp | 2 +- .../easynav_costmap_maps_manager/README.md | 10 ++-- .../CostmapMapsManager.hpp | 14 +++--- .../filters/InflationFilter.hpp | 12 ++--- .../CostmapMapsManager.cpp | 36 +++++++-------- .../filters/InflationFilter.cpp | 46 +++++++++---------- .../tests/costmap_mapsmanager_tests.cpp | 24 +++++----- 9 files changed, 76 insertions(+), 76 deletions(-) diff --git a/localizers/easynav_costmap_localizer/README.md b/localizers/easynav_costmap_localizer/README.md index c1590195..998ee961 100644 --- a/localizers/easynav_costmap_localizer/README.md +++ b/localizers/easynav_costmap_localizer/README.md @@ -66,7 +66,7 @@ This package does not create service servers or clients. | Key | Type | Access | Notes | |---|---|---|---| | `points` | `PointPerceptions` | **Read** | Perception point clouds used in correction. | -| `map.static` | `Costmap2D` | **Read** | Static costmap for likelihood evaluation. | +| `map.base` | `Costmap2D` | **Read** | Base costmap for likelihood evaluation. | ## TF Frames diff --git a/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp b/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp index 7c26a2c0..d229c8cb 100644 --- a/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp @@ -572,12 +572,12 @@ AMCLLocalizer::correct(NavState & nav_state) return; } - if (!nav_state.has("map.static")) { - RCLCPP_WARN(get_node()->get_logger(), "There is yet no a map.static map"); + if (!nav_state.has("map.base")) { + RCLCPP_WARN(get_node()->get_logger(), "There is yet no a map.base map"); return; } - const auto & map_static = nav_state.get("map.static"); + const auto & map_static = nav_state.get("map.base"); const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); diff --git a/localizers/easynav_costmap_localizer/tests/costmap_localizer_tests.cpp b/localizers/easynav_costmap_localizer/tests/costmap_localizer_tests.cpp index bc03cbe6..8f7bd0ac 100644 --- a/localizers/easynav_costmap_localizer/tests/costmap_localizer_tests.cpp +++ b/localizers/easynav_costmap_localizer/tests/costmap_localizer_tests.cpp @@ -67,7 +67,7 @@ TEST_F(AMCLLocalizerTest, BasicDynamicUpdate) navstate.set("perceptions", easynav::Perceptions()); navstate.get_mutable("perceptions")->push_back(perception); - navstate.set("map.static", *static_map); + navstate.set("map.base", *static_map); manager->update(navstate); diff --git a/maps_managers/easynav_costmap_maps_manager/README.md b/maps_managers/easynav_costmap_maps_manager/README.md index ebdf66c9..3de9cd38 100644 --- a/maps_managers/easynav_costmap_maps_manager/README.md +++ b/maps_managers/easynav_costmap_maps_manager/README.md @@ -2,7 +2,7 @@ ## Description -Maps Manager that maintains 2D costmaps (static and dynamic), supports filter plugins (such as inflation and obstacle filters), and exposes maps through ROS topics and NavState integration. +Maps Manager that maintains 2D costmaps (base and dynamic), supports filter plugins (such as inflation and obstacle filters), and exposes maps through ROS topics and NavState integration. At the core of this stack lies the Costmap2D data structure. `Costmap2D` extends the binary occupancy grid into a graded cost representation with values in the range [0–255]: @@ -81,7 +81,7 @@ Each entry in `.filters` defines a sub-namespace `.` wit - **Type:** `easynav::InflationFilter` - **Description:** Expands obstacle information in the costmap by assigning graded costs around `LETHAL_OBSTACLE` cells based on distance. Uses a breadth-first wavefront propagation algorithm (distance bins) to efficiently inflate obstacles up to `inflation_radius`. - The filter reads both the static map and the dynamic filtered map, applies inflation to each, and merges results. If `ObstacleBounds` is available in `NavState`, inflation is restricted to the updated region for performance. + The filter reads both the base map and the dynamic filtered map, applies inflation to each, and merges results. If `ObstacleBounds` is available in `NavState`, inflation is restricted to the updated region for performance. **Parameters:** @@ -95,7 +95,7 @@ Each entry in `.filters` defines a sub-namespace `.` wit | Key | Type | Access | Description | |---|---|---|---| -| `map.static` | `Costmap2D` | **Read** | Static costmap to inflate. | +| `map.base` | `Costmap2D` | **Read** | Base costmap to inflate. | | `map.dynamic.filtered` | `Costmap2D` | **Read/Write** | Dynamic costmap input and output after inflation. | | `map.dynamic.obstacle_bounds` | `ObstacleBounds` | **Read** (optional) | Restricts inflation to updated region for performance. | @@ -130,7 +130,7 @@ maps_manager_node: | Direction | Topic | Type | Purpose | QoS | |---|---|---|---|---| | Subscription | `//incoming_map` | `nav_msgs/msg/OccupancyGrid` | Input occupancy map used to update the dynamic map. | `depth=1, transient_local, reliable` | -| Publisher | `//map` | `nav_msgs/msg/OccupancyGrid` | Publishes the static costmap. | `depth=1` | +| Publisher | `//map` | `nav_msgs/msg/OccupancyGrid` | Publishes the base costmap. | `depth=1` | | Publisher | `//dynamic_map` | `nav_msgs/msg/OccupancyGrid` | Publishes the dynamic (live) costmap. | `depth=100` | ### Services @@ -145,7 +145,7 @@ maps_manager_node: | Key | Type | Access | Notes | |---|---|---|---| -| `map.static` | `Costmap2D` | **Write** | Static map loaded from YAML. | +| `map.base` | `Costmap2D` | **Write** | Base map loaded from YAML. | | `map.dynamic` | `Costmap2D` | **Write** | Dynamic map after applying filters. | | `map.dynamic.filtered` | `Costmap2D` | **Read** | Previously filtered map used as input if available. | | `map.dynamic.obstacle_bounds` | `ObstacleBounds` | **Read** | Bounding box of updated obstacles (used to limit inflation region). | 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..cfef0727 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 @@ -78,7 +78,7 @@ class CostmapMapsManager : public easynav::MapsManagerBase * * @param new_map Shared pointer to a new map object. Must be of type SimpleMap. */ - void set_static_map(const Costmap2D & new_map); + void set_base_map(const Costmap2D & new_map); protected: /** @@ -88,9 +88,9 @@ class CostmapMapsManager : public easynav::MapsManagerBase private: /** - * @brief Internal static map. + * @brief Internal base map. */ - Costmap2D static_map_; + Costmap2D map_base_; /** * @brief Internal static map. @@ -98,9 +98,9 @@ class CostmapMapsManager : public easynav::MapsManagerBase std::shared_ptr dynamic_map_; /** - * @brief Publisher for the static occupancy grid. + * @brief Publisher for the base occupancy grid. */ - rclcpp::Publisher::SharedPtr static_occ_pub_; + rclcpp::Publisher::SharedPtr base_occ_pub_; /** * @brief Publisher for the dynamic occupancy grid. @@ -118,9 +118,9 @@ class CostmapMapsManager : public easynav::MapsManagerBase rclcpp::Service::SharedPtr savemap_srv_; /** - * @brief Cached occupancy grid message for the static map. + * @brief Cached occupancy grid message for the base map. */ - nav_msgs::msg::OccupancyGrid static_grid_msg_; + nav_msgs::msg::OccupancyGrid base_grid_msg_; /** * @brief Cached occupancy grid message for the dynamic map. diff --git a/maps_managers/easynav_costmap_maps_manager/include/easynav_costmap_maps_manager/filters/InflationFilter.hpp b/maps_managers/easynav_costmap_maps_manager/include/easynav_costmap_maps_manager/filters/InflationFilter.hpp index 20e9a086..4c2cc037 100644 --- a/maps_managers/easynav_costmap_maps_manager/include/easynav_costmap_maps_manager/filters/InflationFilter.hpp +++ b/maps_managers/easynav_costmap_maps_manager/include/easynav_costmap_maps_manager/filters/InflationFilter.hpp @@ -230,20 +230,20 @@ class InflationFilter : public CostmapFilter bool matchedSize_ {false}; - Costmap2D static_inflated_; - bool has_static_inflated_ {false}; + Costmap2D base_inflated_; + bool has_base_inflated_ {false}; - struct StaticGeomSignature + struct BaseGeomSignature { unsigned int size_x {0}; unsigned int size_y {0}; double resolution {0.0}; double origin_x {0.0}; double origin_y {0.0}; - } static_sig_; + } base_sig_; - bool needs_recompute_static_(const Costmap2D & static_map) const; - void recompute_static_inflation_(const Costmap2D & static_map); + bool needs_recompute_base_(const Costmap2D & base_map) const; + void recompute_base_inflation_(const Costmap2D & base_map); }; } // namespace easynav 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..79fb44b7 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 @@ -98,7 +98,7 @@ CostmapMapsManager::on_initialize() } } - static_occ_pub_ = node->create_publisher( + base_occ_pub_ = node->create_publisher( node->get_fully_qualified_name() + std::string("/") + plugin_name + "/map", rclcpp::QoS(1).transient_local().reliable()); @@ -116,30 +116,30 @@ CostmapMapsManager::on_initialize() throw std::runtime_error("Package " + package_name + " not found. Error: " + ex.what()); } - if (auto ret = loadMapFromYaml(map_path_, static_grid_msg_) != LOAD_MAP_SUCCESS) { + if (auto ret = loadMapFromYaml(map_path_, base_grid_msg_) != LOAD_MAP_SUCCESS) { std::cerr << "loadMapFromYaml returned" << ret << std::endl; throw std::runtime_error("YAML file [" + map_path_ + "] not found or invalid: "); } - static_map_ = Costmap2D(static_grid_msg_); + base_grid_msg_.header.frame_id = tf_info.map_frame; + base_grid_msg_.header.stamp = node->now(); - static_grid_msg_.header.frame_id = tf_info.map_frame; - static_grid_msg_.header.stamp = node->now(); - static_occ_pub_->publish(static_grid_msg_); + map_base_ = Costmap2D(base_grid_msg_); + base_occ_pub_->publish(base_grid_msg_); } incoming_map_sub_ = node->create_subscription( node->get_fully_qualified_name() + std::string("/") + plugin_name + "/incoming_map", rclcpp::QoS(1).transient_local().reliable(), [&](nav_msgs::msg::OccupancyGrid::UniquePtr msg) { - static_grid_msg_ = *msg; + base_grid_msg_ = *msg; - static_map_ = Costmap2D(*msg); + base_grid_msg_.header.frame_id = tf_info.map_frame; + base_grid_msg_.header.stamp = this->get_node()->now(); - static_grid_msg_.header.frame_id = tf_info.map_frame; - static_grid_msg_.header.stamp = this->get_node()->now(); + map_base_ = Costmap2D(base_grid_msg_); - static_occ_pub_->publish(static_grid_msg_); + base_occ_pub_->publish(base_grid_msg_); }); savemap_srv_ = node->create_service( @@ -157,7 +157,7 @@ CostmapMapsManager::on_initialize() params.free_thresh = 0.25; params.occupied_thresh = 0.65; - if (!saveMapToFile(static_grid_msg_, params)) { + if (!saveMapToFile(base_grid_msg_, params)) { response->success = false; response->message = "Failed to save map to: " + map_path_; } else { @@ -168,9 +168,9 @@ CostmapMapsManager::on_initialize() } void -CostmapMapsManager::set_static_map(const Costmap2D & new_map) +CostmapMapsManager::set_base_map(const Costmap2D & new_map) { - static_map_ = new_map; + map_base_ = new_map; } @@ -179,14 +179,14 @@ CostmapMapsManager::update(NavState & nav_state) { EASYNAV_TRACE_EVENT; - if (!nav_state.has("map.static")) { - nav_state.set("map.static", static_map_); + if (!nav_state.has("map.base")) { + nav_state.set("map.base", map_base_); } if (!dynamic_map_) { - dynamic_map_ = std::make_shared(static_map_); + dynamic_map_ = std::make_shared(map_base_); } else { - *dynamic_map_ = static_map_; + *dynamic_map_ = map_base_; } nav_state.set("map.dynamic.filtered", dynamic_map_); diff --git a/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/filters/InflationFilter.cpp b/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/filters/InflationFilter.cpp index 4aeffd9a..6df5384b 100644 --- a/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/filters/InflationFilter.cpp +++ b/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/filters/InflationFilter.cpp @@ -102,10 +102,10 @@ InflationFilter::update(NavState & nav_state) auto dynamic_map_ptr = nav_state.get_ptr("map.dynamic.filtered"); Costmap2D & dynamic_map = *dynamic_map_ptr; - const auto & static_map = nav_state.get("map.static"); + const auto & base_map = nav_state.get("map.base"); - if (needs_recompute_static_(static_map)) { - recompute_static_inflation_(static_map); + if (needs_recompute_base_(base_map)) { + recompute_base_inflation_(base_map); } if (!matchedSize_) { @@ -149,7 +149,7 @@ InflationFilter::update(NavState & nav_state) for (int j = 0; j < dynamic_map.getSizeInCellsY(); j++) { int index = static_cast(dynamic_map.getIndex(i, j)); unsigned char cost = std::max( - dynamic_map.getCost(i, j), static_inflated_.getCost(i, j)); + dynamic_map.getCost(i, j), base_inflated_.getCost(i, j)); dynamic_map.setCost(i, j, cost); } } @@ -443,17 +443,17 @@ InflationFilter::generateIntegerDistances() } bool -InflationFilter::needs_recompute_static_(const Costmap2D & static_map) const +InflationFilter::needs_recompute_base_(const Costmap2D & base_map) const { - if (!has_static_inflated_) { + if (!has_base_inflated_) { return true; } - if (static_map.getSizeInCellsX() != static_sig_.size_x || - static_map.getSizeInCellsY() != static_sig_.size_y || - static_map.getResolution() != static_sig_.resolution || - static_map.getOriginX() != static_sig_.origin_x || - static_map.getOriginY() != static_sig_.origin_y) + if (base_map.getSizeInCellsX() != base_sig_.size_x || + base_map.getSizeInCellsY() != base_sig_.size_y || + base_map.getResolution() != base_sig_.resolution || + base_map.getOriginX() != base_sig_.origin_x || + base_map.getOriginY() != base_sig_.origin_y) { return true; } @@ -462,22 +462,22 @@ InflationFilter::needs_recompute_static_(const Costmap2D & static_map) const } void -InflationFilter::recompute_static_inflation_(const Costmap2D & static_map) +InflationFilter::recompute_base_inflation_(const Costmap2D & base_map) { - static_inflated_ = static_map; + base_inflated_ = base_map; - matchSize(static_inflated_); - const int w = static_inflated_.getSizeInCellsX(); - const int h = static_inflated_.getSizeInCellsY(); - updateCosts(static_inflated_, 0, 0, w, h); + matchSize(base_inflated_); + const int w = base_inflated_.getSizeInCellsX(); + const int h = base_inflated_.getSizeInCellsY(); + updateCosts(base_inflated_, 0, 0, w, h); - static_sig_.size_x = static_map.getSizeInCellsX(); - static_sig_.size_y = static_map.getSizeInCellsY(); - static_sig_.resolution = static_map.getResolution(); - static_sig_.origin_x = static_map.getOriginX(); - static_sig_.origin_y = static_map.getOriginY(); + base_sig_.size_x = base_map.getSizeInCellsX(); + base_sig_.size_y = base_map.getSizeInCellsY(); + base_sig_.resolution = base_map.getResolution(); + base_sig_.origin_x = base_map.getOriginX(); + base_sig_.origin_y = base_map.getOriginY(); - has_static_inflated_ = true; + has_base_inflated_ = true; } } // namespace easynav diff --git a/maps_managers/easynav_costmap_maps_manager/tests/costmap_mapsmanager_tests.cpp b/maps_managers/easynav_costmap_maps_manager/tests/costmap_mapsmanager_tests.cpp index 8440f0c2..2852fd45 100644 --- a/maps_managers/easynav_costmap_maps_manager/tests/costmap_mapsmanager_tests.cpp +++ b/maps_managers/easynav_costmap_maps_manager/tests/costmap_mapsmanager_tests.cpp @@ -43,8 +43,8 @@ class CostmapMapsManagerTest : public ::testing::Test // auto manager = std::make_shared(); // manager->initialize(node, "test"); // -// easynav::Costmap2D static_map(30, 30, 0.1, -1.5, -1.5); -// manager->set_static_map(static_map); +// easynav::Costmap2D base_map(30, 30, 0.1, -1.5, -1.5); +// manager->set_base_map(base_map); // // easynav::NavState navstate; // auto perception = std::make_shared(); @@ -108,8 +108,8 @@ class CostmapMapsManagerTest : public ::testing::Test // easynav::NavState navstate; // manager->update(navstate); // -// ASSERT_TRUE(navstate.has("map.static")); -// const auto & map = navstate.get("map.static"); +// ASSERT_TRUE(navstate.has("map.base")); +// const auto & map = navstate.get("map.base"); // // EXPECT_EQ(map.getCost(5, 5), easynav::LETHAL_OBSTACLE); // EXPECT_EQ(map.getCost(1, 1), easynav::FREE_SPACE); @@ -129,17 +129,17 @@ class CostmapMapsManagerTest : public ::testing::Test // auto manager = std::make_shared(); // manager->initialize(node, "test_savemap"); // -// // Create a 200x200 static costmap with resolution 0.05 and origin at (0, 0) +// // Create a 200x200 base costmap with resolution 0.05 and origin at (0, 0) // const unsigned int width = 200; // const unsigned int height = 200; -// easynav::Costmap2D map_static(width, height, 0.05, 0.0, 0.0); +// easynav::Costmap2D map_base(width, height, 0.05, 0.0, 0.0); // // // Set a vertical line of lethal obstacles at x = 30 // for (unsigned int y = 0; y < height; ++y) { -// map_static.setCost(30, y, easynav::LETHAL_OBSTACLE); +// map_base.setCost(30, y, easynav::LETHAL_OBSTACLE); // } // -// manager->set_static_map(map_static); +// manager->set_base_map(map_base); // // const std::string yaml_path = "/tmp/savemap_test_map"; // const std::string service_name = "/test_savemap_node/test_savemap/savemap"; @@ -169,11 +169,11 @@ class CostmapMapsManagerTest : public ::testing::Test // easynav::Costmap2D loaded_map(loaded_grid); // // // Check map dimensions match -// ASSERT_EQ(loaded_map.getSizeInCellsX(), map_static.getSizeInCellsX()); -// ASSERT_EQ(loaded_map.getSizeInCellsY(), map_static.getSizeInCellsY()); +// ASSERT_EQ(loaded_map.getSizeInCellsX(), map_base.getSizeInCellsX()); +// ASSERT_EQ(loaded_map.getSizeInCellsY(), map_base.getSizeInCellsY()); // -// for (unsigned int y = 0; y < map_static.getSizeInCellsY(); ++y) { -// for (unsigned int x = 0; x < map_static.getSizeInCellsX(); ++x) { +// for (unsigned int y = 0; y < map_base.getSizeInCellsY(); ++y) { +// for (unsigned int x = 0; x < map_base.getSizeInCellsX(); ++x) { // if (x == 30) { // EXPECT_EQ(loaded_map.getCost(x, y), easynav::LETHAL_OBSTACLE) // << "Expected LETHAL_OBSTACLE at (" << x << "," << y << ")"; From 6ac95ae109eba9030fb416866fe05ba22a081481 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Fri, 24 Apr 2026 07:10:23 +0100 Subject: [PATCH 3/5] Costamp base map sincronized with nav_state MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../CostmapMapsManager.cpp | 15 +- .../tests/costmap_mapsmanager_tests.cpp | 161 +++++++++++++++++- 2 files changed, 174 insertions(+), 2 deletions(-) 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 79fb44b7..bc57f90b 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 @@ -179,7 +179,20 @@ CostmapMapsManager::update(NavState & nav_state) { EASYNAV_TRACE_EVENT; - if (!nav_state.has("map.base")) { + // Sync internal base map with NavState, preferring the newest stamp. + // Note: Compare using nanoseconds to avoid rclcpp::Time clock-type mismatches. + if (nav_state.has("map.base")) { + const auto & external_base = nav_state.get("map.base"); + + const int64_t internal_ns = map_base_.getLastModifiedStamp().nanoseconds(); + const int64_t external_ns = external_base.getLastModifiedStamp().nanoseconds(); + + if (external_ns > internal_ns) { + map_base_ = external_base; + } else if (internal_ns > external_ns) { + nav_state.set("map.base", map_base_); + } + } else { nav_state.set("map.base", map_base_); } diff --git a/maps_managers/easynav_costmap_maps_manager/tests/costmap_mapsmanager_tests.cpp b/maps_managers/easynav_costmap_maps_manager/tests/costmap_mapsmanager_tests.cpp index 2852fd45..19cf79db 100644 --- a/maps_managers/easynav_costmap_maps_manager/tests/costmap_mapsmanager_tests.cpp +++ b/maps_managers/easynav_costmap_maps_manager/tests/costmap_mapsmanager_tests.cpp @@ -15,12 +15,22 @@ #include -#include "easynav_sensors/types/PointPerception.hpp" +#include +#include +#include + #include "easynav_common/types/NavState.hpp" +#include "easynav_common/RTTFBuffer.hpp" + +#include "easynav_costmap_common/costmap_2d.hpp" +#include "easynav_costmap_common/cost_values.hpp" +#include "easynav_costmap_maps_manager/CostmapMapsManager.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" + /// \brief Fixture for CostmapMapsManager tests class CostmapMapsManagerTest : public ::testing::Test { @@ -36,6 +46,155 @@ class CostmapMapsManagerTest : public ::testing::Test } }; +static nav_msgs::msg::OccupancyGrid make_grid( + int width, + int height, + int occ_x, + int occ_y, + int64_t stamp_ns) +{ + nav_msgs::msg::OccupancyGrid grid; + grid.header.frame_id = "map"; + grid.header.stamp.sec = static_cast(stamp_ns / 1000000000LL); + grid.header.stamp.nanosec = static_cast(stamp_ns % 1000000000LL); + + grid.info.width = static_cast(width); + grid.info.height = static_cast(height); + grid.info.resolution = 0.2f; + grid.info.origin.position.x = -1.0; + grid.info.origin.position.y = -1.0; + grid.data.assign(static_cast(width * height), 0); + + if (occ_x >= 0 && occ_y >= 0 && occ_x < width && occ_y < height) { + grid.data[static_cast(occ_y * width + occ_x)] = 100; + } + + return grid; +} + +TEST_F(CostmapMapsManagerTest, SyncBaseMapPrefersNewerNavStateMap) +{ + auto node = std::make_shared("test_node_sync_ns"); + + easynav::TFInfo tf_info; + tf_info.map_frame = "map"; + tf_info.odom_frame = "odom"; + tf_info.robot_frame = "base_link"; + tf_info.robot_footprint_frame = "base_footprint"; + easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info); + + auto manager = std::make_shared(); + manager->initialize(node, "test"); + + const int64_t older_ns = 10LL * 1000000000LL; + const int64_t newer_ns = 20LL * 1000000000LL; + + easynav::Costmap2D internal_old(make_grid(10, 10, 1, 1, older_ns)); + easynav::Costmap2D external_new(make_grid(10, 10, 2, 2, newer_ns)); + + manager->set_base_map(internal_old); + + easynav::NavState navstate; + navstate.set("map.base", external_new); + + manager->update(navstate); + + ASSERT_TRUE(navstate.has("map.base")); + const auto & base_after = navstate.get("map.base"); + EXPECT_EQ(base_after.getLastModifiedStamp().nanoseconds(), newer_ns); + + ASSERT_TRUE(navstate.has("map.dynamic.filtered")); + const auto dyn_ptr = navstate.get_ptr("map.dynamic.filtered"); + ASSERT_TRUE(dyn_ptr != nullptr); + EXPECT_EQ(dyn_ptr->getLastModifiedStamp().nanoseconds(), newer_ns); + EXPECT_EQ(dyn_ptr->getCost(2, 2), easynav::LETHAL_OBSTACLE); + EXPECT_EQ(dyn_ptr->getCost(1, 1), easynav::FREE_SPACE); +} + +TEST_F(CostmapMapsManagerTest, SyncBaseMapPrefersNewerInternalMap) +{ + auto node = std::make_shared("test_node_sync_internal"); + + easynav::TFInfo tf_info; + tf_info.map_frame = "map"; + tf_info.odom_frame = "odom"; + tf_info.robot_frame = "base_link"; + tf_info.robot_footprint_frame = "base_footprint"; + easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info); + + auto manager = std::make_shared(); + manager->initialize(node, "test"); + + const int64_t older_ns = 10LL * 1000000000LL; + const int64_t newer_ns = 20LL * 1000000000LL; + + easynav::Costmap2D internal_new(make_grid(10, 10, 3, 3, newer_ns)); + easynav::Costmap2D external_old(make_grid(10, 10, 4, 4, older_ns)); + + manager->set_base_map(internal_new); + + easynav::NavState navstate; + navstate.set("map.base", external_old); + + manager->update(navstate); + + ASSERT_TRUE(navstate.has("map.base")); + const auto & base_after = navstate.get("map.base"); + EXPECT_EQ(base_after.getLastModifiedStamp().nanoseconds(), newer_ns); + + ASSERT_TRUE(navstate.has("map.dynamic.filtered")); + const auto dyn_ptr = navstate.get_ptr("map.dynamic.filtered"); + ASSERT_TRUE(dyn_ptr != nullptr); + EXPECT_EQ(dyn_ptr->getLastModifiedStamp().nanoseconds(), newer_ns); + EXPECT_EQ(dyn_ptr->getCost(3, 3), easynav::LETHAL_OBSTACLE); + EXPECT_EQ(dyn_ptr->getCost(4, 4), easynav::FREE_SPACE); +} + +TEST_F(CostmapMapsManagerTest, IncomingMapTopicUpdatesInternalAndNavState) +{ + auto node = std::make_shared("test_node_incoming"); + + easynav::TFInfo tf_info; + tf_info.map_frame = "map"; + tf_info.odom_frame = "odom"; + tf_info.robot_frame = "base_link"; + tf_info.robot_footprint_frame = "base_footprint"; + easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info); + + auto manager = std::make_shared(); + manager->initialize(node, "test"); + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); + + const std::string topic = node->get_fully_qualified_name() + std::string("/test/incoming_map"); + auto pub = node->create_publisher( + topic, rclcpp::QoS(1).transient_local().reliable()); + pub->on_activate(); + + auto grid = make_grid(10, 10, 5, 5, 123LL * 1000000000LL); + pub->publish(grid); + + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + executor.spin_some(); + + easynav::NavState navstate; + manager->update(navstate); + + ASSERT_TRUE(navstate.has("map.base")); + const auto & base_after = navstate.get("map.base"); + EXPECT_EQ(base_after.getCost(5, 5), easynav::LETHAL_OBSTACLE); + EXPECT_GT(base_after.getLastModifiedStamp().nanoseconds(), 0); + + ASSERT_TRUE(navstate.has("map.dynamic.filtered")); + const auto dyn_ptr = navstate.get_ptr("map.dynamic.filtered"); + ASSERT_TRUE(dyn_ptr != nullptr); + EXPECT_EQ(dyn_ptr->getCost(5, 5), easynav::LETHAL_OBSTACLE); + EXPECT_EQ(dyn_ptr->getLastModifiedStamp().nanoseconds(), + base_after.getLastModifiedStamp().nanoseconds()); +} + ///// \brief Dynamic map update test with point cloud //TEST_F(CostmapMapsManagerTest, BasicDynamicUpdate) //{ From 91c7e1b719beceac4451a1f3deafe9f571763fa1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Fri, 24 Apr 2026 08:25:06 +0100 Subject: [PATCH 4/5] Publish changes in base map and be ready to save updated map MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../CostmapMapsManager.cpp | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) 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 bc57f90b..66be2125 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 @@ -171,6 +171,14 @@ void CostmapMapsManager::set_base_map(const Costmap2D & new_map) { map_base_ = new_map; + + if (base_occ_pub_) { + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + map_base_.toOccupancyGridMsg(base_grid_msg_); + base_grid_msg_.header.frame_id = tf_info.map_frame; + base_grid_msg_.header.stamp = map_base_.getLastModifiedStamp(); + base_occ_pub_->publish(base_grid_msg_); + } } @@ -189,6 +197,14 @@ CostmapMapsManager::update(NavState & nav_state) if (external_ns > internal_ns) { map_base_ = external_base; + + if (base_occ_pub_) { + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + map_base_.toOccupancyGridMsg(base_grid_msg_); + base_grid_msg_.header.frame_id = tf_info.map_frame; + base_grid_msg_.header.stamp = map_base_.getLastModifiedStamp(); + base_occ_pub_->publish(base_grid_msg_); + } } else if (internal_ns > external_ns) { nav_state.set("map.base", map_base_); } From 7ff4876e49c362e0acd2d1421187b5254ebfa052 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Fri, 24 Apr 2026 08:39:03 +0100 Subject: [PATCH 5/5] Any change in the costmap set it as modified MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../easynav_costmap_common/costmap_2d.hpp | 14 ++++++- .../src/easynav_costmap_common/costmap_2d.cpp | 37 ++++++++++++++++++- .../tests/costmap_2d_tests.cpp | 16 +++++--- .../filters/InflationFilter.hpp | 2 + .../filters/InflationFilter.cpp | 6 ++- 5 files changed, 66 insertions(+), 9 deletions(-) diff --git a/common/easynav_costmap_common/include/easynav_costmap_common/costmap_2d.hpp b/common/easynav_costmap_common/include/easynav_costmap_common/costmap_2d.hpp index fa57947b..e0750d4a 100644 --- a/common/easynav_costmap_common/include/easynav_costmap_common/costmap_2d.hpp +++ b/common/easynav_costmap_common/include/easynav_costmap_common/costmap_2d.hpp @@ -136,10 +136,20 @@ class Costmap2D /** * @brief Get timestamp of last costmap modification. * - * Currently only set by constructors and copy/assignment; other updates do not change it. + * Set by constructors and copy/assignment, and may be updated by some mutating operations. + * For high-frequency per-cell updates (e.g., many `setCost()` calls in a loop), prefer calling + * `touch()` once after the batch update. */ rclcpp::Time getLastModifiedStamp() const; + /** + * @brief Mark the costmap as modified. + * + * This bumps the internal last-modified stamp by 1 nanosecond while preserving the clock type. + * It is intended to be called after bulk updates to avoid per-cell overhead. + */ + void touch(); + /** * @brief Copies the (x0,y0)..(xn,yn) window from source costmap into a current costmap @param source Source costmap where the window will be copied from @@ -591,7 +601,7 @@ class Costmap2D unsigned char default_value_; // Timestamp indicating when the costmap was last modified. - // Currently only set by constructors and copy/assignment; other updates do not change it. + // Set by constructors and copy/assignment, and bumped by some mutating operations. rclcpp::Time last_modified_; // *INDENT-OFF* Uncrustify doesn't handle indented public/private labels diff --git a/common/easynav_costmap_common/src/easynav_costmap_common/costmap_2d.cpp b/common/easynav_costmap_common/src/easynav_costmap_common/costmap_2d.cpp index 53235782..71b19648 100644 --- a/common/easynav_costmap_common/src/easynav_costmap_common/costmap_2d.cpp +++ b/common/easynav_costmap_common/src/easynav_costmap_common/costmap_2d.cpp @@ -126,6 +126,9 @@ void Costmap2D::resetMaps() { std::unique_lock lock(*access_); memset(costmap_, default_value_, size_x_ * size_y_ * sizeof(unsigned char)); + + const int64_t new_stamp_ns = last_modified_.nanoseconds() + 1; + last_modified_ = rclcpp::Time(new_stamp_ns, last_modified_.get_clock_type()); } void Costmap2D::resetMap(unsigned int x0, unsigned int y0, unsigned int xn, unsigned int yn) @@ -141,6 +144,9 @@ void Costmap2D::resetMapToValue( for (unsigned int y = y0 * size_x_ + x0; y < yn * size_x_ + x0; y += size_x_) { memset(costmap_ + y, value, len * sizeof(unsigned char)); } + + const int64_t new_stamp_ns = last_modified_.nanoseconds() + 1; + last_modified_ = rclcpp::Time(new_stamp_ns, last_modified_.get_clock_type()); } bool Costmap2D::copyCostmapWindow( @@ -179,6 +185,11 @@ bool Costmap2D::copyCostmapWindow( map.costmap_, lower_left_x, lower_left_y, map.size_x_, costmap_, 0, 0, size_x_, size_x_, size_y_); + + { + std::unique_lock lock(*access_); + last_modified_ = map.getLastModifiedStamp(); + } return true; } @@ -202,6 +213,14 @@ bool Costmap2D::copyWindow( source.costmap_, sx0, sy0, source.size_x_, costmap_, dx0, dy0, size_x_, sz_x, sz_y); + + { + const auto source_stamp = source.getLastModifiedStamp(); + std::unique_lock lock(*access_); + const int64_t new_stamp_ns = std::max(last_modified_.nanoseconds(), + source_stamp.nanoseconds()) + 1; + last_modified_ = rclcpp::Time(new_stamp_ns, last_modified_.get_clock_type()); + } return true; } @@ -241,6 +260,13 @@ rclcpp::Time Costmap2D::getLastModifiedStamp() const return last_modified_; } +void Costmap2D::touch() +{ + std::lock_guard lock(*access_); + const int64_t new_stamp_ns = last_modified_.nanoseconds() + 1; + last_modified_ = rclcpp::Time(new_stamp_ns, last_modified_.get_clock_type()); +} + Costmap2D & Costmap2D::operator=(const Costmap2D & map) { // check for self assignment @@ -317,7 +343,16 @@ unsigned char Costmap2D::getCost(unsigned int undex) const void Costmap2D::setCost(unsigned int mx, unsigned int my, unsigned char cost) { - costmap_[getIndex(mx, my)] = cost; + std::lock_guard lock(*access_); + const unsigned int index = getIndex(mx, my); + + if (costmap_[index] == cost) { + return; + } + + costmap_[index] = cost; + const int64_t new_stamp_ns = last_modified_.nanoseconds() + 1; + last_modified_ = rclcpp::Time(new_stamp_ns, last_modified_.get_clock_type()); } void Costmap2D::mapToWorld(unsigned int mx, unsigned int my, double & wx, double & wy) const diff --git a/common/easynav_costmap_common/tests/costmap_2d_tests.cpp b/common/easynav_costmap_common/tests/costmap_2d_tests.cpp index 49889c81..bfef3f94 100644 --- a/common/easynav_costmap_common/tests/costmap_2d_tests.cpp +++ b/common/easynav_costmap_common/tests/costmap_2d_tests.cpp @@ -171,7 +171,7 @@ TEST_F(Costmap2DTest, TimestampFromCopyConstructor) EXPECT_EQ(out.header.stamp.nanosec, in.header.stamp.nanosec); } -TEST_F(Costmap2DTest, TimestampNotUpdatedByChanges) +TEST_F(Costmap2DTest, TimestampUpdatedBySetCost) { nav_msgs::msg::OccupancyGrid in; in.header.stamp.sec = 7; @@ -185,13 +185,19 @@ TEST_F(Costmap2DTest, TimestampNotUpdatedByChanges) Costmap2D map(in); const int64_t expected_ns = 7LL * 1000000000LL + 8LL; + map.setCost(1, 1, 100); - map.resizeMap(4u, 4u, 1.0, 1.0, 1.0); + EXPECT_EQ(map.getLastModifiedStamp().nanoseconds(), expected_ns + 1); - EXPECT_EQ(map.getLastModifiedStamp().nanoseconds(), expected_ns); + // Setting the same value should not change the stamp. + map.setCost(1, 1, 100); + EXPECT_EQ(map.getLastModifiedStamp().nanoseconds(), expected_ns + 1); nav_msgs::msg::OccupancyGrid out; map.toOccupancyGridMsg(out); - EXPECT_EQ(out.header.stamp.sec, in.header.stamp.sec); - EXPECT_EQ(out.header.stamp.nanosec, in.header.stamp.nanosec); + + const int64_t out_ns = + static_cast(out.header.stamp.sec) * 1000000000LL + + static_cast(out.header.stamp.nanosec); + EXPECT_EQ(out_ns, expected_ns + 1); } diff --git a/maps_managers/easynav_costmap_maps_manager/include/easynav_costmap_maps_manager/filters/InflationFilter.hpp b/maps_managers/easynav_costmap_maps_manager/include/easynav_costmap_maps_manager/filters/InflationFilter.hpp index 4c2cc037..9a6f9b32 100644 --- a/maps_managers/easynav_costmap_maps_manager/include/easynav_costmap_maps_manager/filters/InflationFilter.hpp +++ b/maps_managers/easynav_costmap_maps_manager/include/easynav_costmap_maps_manager/filters/InflationFilter.hpp @@ -42,6 +42,7 @@ #ifndef EASYNAV_PLANNER__FILTERS__IINFLATIONFILTER_HPP_ #define EASYNAV_PLANNER__FILTERS__IINFLATIONFILTER_HPP_ +#include #include #include @@ -240,6 +241,7 @@ class InflationFilter : public CostmapFilter double resolution {0.0}; double origin_x {0.0}; double origin_y {0.0}; + int64_t stamp_ns {0}; } base_sig_; bool needs_recompute_base_(const Costmap2D & base_map) const; diff --git a/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/filters/InflationFilter.cpp b/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/filters/InflationFilter.cpp index 6df5384b..4df61c69 100644 --- a/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/filters/InflationFilter.cpp +++ b/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/filters/InflationFilter.cpp @@ -449,11 +449,14 @@ InflationFilter::needs_recompute_base_(const Costmap2D & base_map) const return true; } + const int64_t stamp_ns = base_map.getLastModifiedStamp().nanoseconds(); + if (base_map.getSizeInCellsX() != base_sig_.size_x || base_map.getSizeInCellsY() != base_sig_.size_y || base_map.getResolution() != base_sig_.resolution || base_map.getOriginX() != base_sig_.origin_x || - base_map.getOriginY() != base_sig_.origin_y) + base_map.getOriginY() != base_sig_.origin_y || + stamp_ns != base_sig_.stamp_ns) { return true; } @@ -476,6 +479,7 @@ InflationFilter::recompute_base_inflation_(const Costmap2D & base_map) base_sig_.resolution = base_map.getResolution(); base_sig_.origin_x = base_map.getOriginX(); base_sig_.origin_y = base_map.getOriginY(); + base_sig_.stamp_ns = base_map.getLastModifiedStamp().nanoseconds(); has_base_inflated_ = true; }