diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml
index 590b30b91c1..1265b908e8d 100644
--- a/nav2_bringup/params/nav2_params.yaml
+++ b/nav2_bringup/params/nav2_params.yaml
@@ -298,7 +298,7 @@ global_costmap:
robot_radius: 0.22
resolution: 0.05
track_unknown_space: true
- plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
+ plugins: ["static_layer", "obstacle_layer","bounded_tracking_error_layer", "inflation_layer"]
filters: ["keepout_filter", "speed_filter"]
keepout_filter:
plugin: "nav2_costmap_2d::KeepoutFilter"
@@ -331,6 +331,16 @@ global_costmap:
raytrace_min_range: 0.0
obstacle_max_range: 2.5
obstacle_min_range: 0.0
+ bounded_tracking_error_layer:
+ plugin: "nav2_costmap_2d::BoundedTrackingErrorLayer"
+ enabled: true
+ path_topic: "/plan"
+ look_ahead: 2.0
+ step: 40
+ corridor_width: 2.0
+ corridor_cost: 190
+ wall_thickness: 4
+ cost_write_mode: 2
static_layer:
plugin: "nav2_costmap_2d::StaticLayer"
map_subscribe_transient_local: true
diff --git a/nav2_costmap_2d/CMakeLists.txt b/nav2_costmap_2d/CMakeLists.txt
index f7282456bea..3af5410cc1d 100644
--- a/nav2_costmap_2d/CMakeLists.txt
+++ b/nav2_costmap_2d/CMakeLists.txt
@@ -81,6 +81,7 @@ add_library(layers SHARED
plugins/range_sensor_layer.cpp
plugins/denoise_layer.cpp
plugins/plugin_container_layer.cpp
+ plugins/bounded_tracking_error_layer.cpp
)
target_include_directories(layers
PUBLIC
diff --git a/nav2_costmap_2d/costmap_plugins.xml b/nav2_costmap_2d/costmap_plugins.xml
index 1ef0408c158..2a08b713ee7 100644
--- a/nav2_costmap_2d/costmap_plugins.xml
+++ b/nav2_costmap_2d/costmap_plugins.xml
@@ -15,6 +15,9 @@
Similar to obstacle costmap, but uses 3D voxel grid to store data.
+
+ Creates a corridor around the path.
+
A range-sensor (sonar, IR) based obstacle layer for costmap_2d
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/bounded_tracking_error_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/bounded_tracking_error_layer.hpp
new file mode 100644
index 00000000000..8854552c249
--- /dev/null
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/bounded_tracking_error_layer.hpp
@@ -0,0 +1,402 @@
+// Copyright (c) 2025 Berkan Tali
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef NAV2_COSTMAP_2D__BOUNDED_TRACKING_ERROR_LAYER_HPP_
+#define NAV2_COSTMAP_2D__BOUNDED_TRACKING_ERROR_LAYER_HPP_
+
+#include
+#include
+#include
+#include
+#include
+#include
+
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "nav_msgs/msg/path.hpp"
+#include "tf2/time.hpp"
+#include "tf2_ros/buffer.hpp"
+
+#include "nav2_costmap_2d/layer.hpp"
+#include "nav2_costmap_2d/layered_costmap.hpp"
+#include "nav2_util/geometry_utils.hpp"
+#include "nav2_util/line_iterator.hpp"
+#include "nav2_util/path_utils.hpp"
+#include "nav2_util/robot_utils.hpp"
+
+
+namespace nav2_costmap_2d
+{
+
+/**
+ * @class nav2_costmap_2d::BoundedTrackingErrorLayer
+ * @brief A costmap layer that imposes a penalty corridor around the planned path.
+ *
+ * On every costmap update cycle this layer finds the closest point on the current
+ * path, extracts a look-ahead segment, and rasterizes high-cost walls along both
+ * sides of that segment. The walls penalise the local trajectory controller when
+ * the robot deviates beyond the configured corridor width without blocking the
+ * global planner. Optionally, all free-space cells outside the corridor within
+ * the update bounds can be elevated to corridor cost. All parameters are
+ * dynamically reconfigurable at runtime.
+ */
+class BoundedTrackingErrorLayer : public nav2_costmap_2d::Layer
+{
+public:
+ /**
+ * @brief Constructor for BoundedTrackingErrorLayer.
+ */
+ BoundedTrackingErrorLayer() = default;
+
+ /**
+ * @brief Destructor for BoundedTrackingErrorLayer.
+ */
+ ~BoundedTrackingErrorLayer() = default;
+
+ /**
+ * @brief Initializes the layer, declares parameters and creates the path subscription.
+ */
+ void onInitialize() override;
+
+ /**
+ * @brief Expands the update bounds to cover the corridor region around the robot.
+ * @param robot_x Robot X position in world coordinates.
+ * @param robot_y Robot Y position in world coordinates.
+ * @param robot_yaw Robot yaw (unused).
+ * @param min_x Minimum X bound to update.
+ * @param min_y Minimum Y bound to update.
+ * @param max_x Maximum X bound to update.
+ * @param max_y Maximum Y bound to update.
+ */
+ void updateBounds(
+ double robot_x, double robot_y, double robot_yaw, double * min_x,
+ double * min_y, double * max_x, double * max_y) override;
+
+ /**
+ * @brief Writes corridor wall or fill costs into the master costmap.
+ * @param master_grid Costmap to write into.
+ * @param min_i Minimum X cell index of the update window.
+ * @param min_j Minimum Y cell index of the update window.
+ * @param max_i Maximum X cell index of the update window.
+ * @param max_j Maximum Y cell index of the update window.
+ */
+ void updateCosts(
+ nav2_costmap_2d::Costmap2D & master_grid,
+ int min_i, int min_j, int max_i, int max_j) override;
+
+ /**
+ * @brief Deactivates the layer and removes parameter update callbacks.
+ */
+ void deactivate() override;
+
+ /**
+ * @brief Activates the layer and registers parameter update callbacks.
+ */
+ void activate() override;
+
+ /**
+ * @brief Resets the layer, clearing path state and marking as not current.
+ */
+ void reset() override;
+
+ /**
+ * @brief Returns whether this layer can be cleared by the costmap clearing service.
+ * @return Always true; clearing is safe since costs are recomputed every cycle,
+ * and allows recovery behaviors to give the robot room to maneuver out.
+ */
+ bool isClearable() override {return true;}
+
+ /**
+ * @brief Updates resolution and frame ID from the layered costmap on resize.
+ */
+ void matchSize() override;
+
+ /**
+ * @brief Validate parameter updates before they are applied.
+ * @param parameters Incoming parameter update list.
+ * @return Result indicating success or failure with reason.
+ */
+ rcl_interfaces::msg::SetParametersResult
+ validateParameterUpdatesCallback(const std::vector & parameters);
+
+ /**
+ * @brief Apply validated parameter updates to layer state.
+ * @param parameters Validated parameter update list.
+ */
+ void updateParametersCallback(const std::vector & parameters);
+
+protected:
+ /** @brief Wall polygon components for left and right corridor boundaries. */
+ struct WallPolygons
+ {
+ std::vector> left_inner;
+ std::vector> left_outer;
+ std::vector> right_inner;
+ std::vector> right_outer;
+
+ /**
+ * @brief Clear all boundary vectors and reserve capacity for reuse.
+ * @param capacity Expected number of points per boundary.
+ */
+ void clearAndReserve(size_t capacity)
+ {
+ left_inner.clear();
+ left_outer.clear();
+ right_inner.clear();
+ right_outer.clear();
+ left_inner.reserve(capacity);
+ left_outer.reserve(capacity);
+ right_inner.reserve(capacity);
+ right_outer.reserve(capacity);
+ }
+ };
+
+ /**
+ * @brief Subscription callback that stores the latest global plan.
+ * @param msg Incoming path message.
+ */
+ void pathCallback(const nav_msgs::msg::Path::ConstSharedPtr msg);
+
+ /**
+ * @brief Extract a look-ahead sub-path starting from a given index.
+ * @param path Full path in costmap frame.
+ * @param path_index Index of the closest pose on the path.
+ * @param segment Output sub-path segment.
+ */
+ void getPathSegment(
+ const nav_msgs::msg::Path & path,
+ size_t path_index,
+ nav_msgs::msg::Path & segment);
+
+ /**
+ * @brief Compute wall polygon boundary points from a path segment.
+ *
+ * step_size_ is the pose-index stride used to estimate the local path direction
+ * at each polygon vertex. A stride of 1 gives noisy normals on dense paths;
+ * a larger value averages over more poses for smoother corridor boundaries.
+ * @param segment Path segment in costmap frame.
+ * @param walls Output wall polygons.
+ */
+ void getWallPolygons(const nav_msgs::msg::Path & segment, WallPolygons & walls);
+
+ /**
+ * @brief Apply the fill-outside-corridor mode: trace interior quads, add end-cap
+ * circles, guarantee the robot cell is interior, then flood-fill outside.
+ * @param master_grid Costmap to write into.
+ * @param robot_pose Current robot pose in costmap frame.
+ * @param full_path Full transformed path in costmap frame.
+ */
+ void applyFillOutsideCorridor(
+ nav2_costmap_2d::Costmap2D & master_grid,
+ const geometry_msgs::msg::PoseStamped & robot_pose,
+ const nav_msgs::msg::Path & full_path);
+
+ /**
+ * @brief Rasterize corridor wall cells between inner and outer boundary polylines.
+ * @param master_grid Costmap to write into.
+ * @param inner_points Inner boundary polyline in world coordinates.
+ * @param outer_points Outer boundary polyline in world coordinates.
+ */
+ void drawCorridorWalls(
+ nav2_costmap_2d::Costmap2D & master_grid,
+ const std::vector> & inner_points,
+ const std::vector> & outer_points);
+
+ /**
+ * @brief Record all cell indices inside the corridor interior into the index set.
+ *
+ * Always accumulates into the existing set. Callers are responsible for
+ * resetting the relevant region of corridor_interior_mask_ before the first
+ * call in each update cycle.
+ * @param master_grid Costmap used for coordinate conversion.
+ * @param walls Wall polygons defining the corridor interior.
+ */
+ void saveCorridorInterior(
+ nav2_costmap_2d::Costmap2D & master_grid,
+ const WallPolygons & walls);
+
+ /**
+ * @brief Mark all cells within a circle radius as corridor interior. That robot needs
+ * for turning around
+ *
+ * @param master_grid Costmap used for coordinate conversion.
+ * @param cx Circle center X in cell coordinates.
+ * @param cy Circle center Y in cell coordinates.
+ * @param r_sq Squared radius in cells.
+ */
+ void markCircleAsInterior(
+ nav2_costmap_2d::Costmap2D & master_grid,
+ int cx, int cy, int r_sq);
+
+ /**
+ * @brief Elevate all cells outside the corridor index set to corridor cost.
+ * @param master_grid Costmap to write into.
+ * @param min_i Fill area minimum X cell index.
+ * @param min_j Fill area minimum Y cell index.
+ * @param max_i Fill area maximum X cell index.
+ * @param max_j Fill area maximum Y cell index.
+ */
+ void fillOutsideCorridor(
+ nav2_costmap_2d::Costmap2D & master_grid,
+ int min_i, int min_j, int max_i, int max_j);
+
+ /**
+ * @brief Reset path state and index tracker.
+ */
+ void resetState();
+
+ size_t step_size_;
+ double look_ahead_;
+ double corridor_width_;
+ int wall_thickness_;
+ unsigned char corridor_cost_;
+ int cost_write_mode_{0};
+
+ double resolution_{0.0};
+ std::string costmap_frame_;
+ std::string robot_base_frame_;
+
+ std::atomic current_path_index_{0};
+ std::vector corridor_interior_mask_;
+
+ // Previous fill bounding box in cell indices; -1 = no fill performed yet.
+ int prev_fill_min_i_{-1};
+ int prev_fill_min_j_{-1};
+ int prev_fill_max_i_{-1};
+ int prev_fill_max_j_{-1};
+
+private:
+ /** @brief A 2D costmap cell coordinate. */
+ struct CellPoint
+ {
+ int x;
+ int y;
+ };
+
+ /**
+ * @brief Declare and load all layer parameters from the node.
+ */
+ void getParameters();
+
+ /**
+ * @brief Convert world coordinates to a CellPoint, clamping to map bounds if outside.
+ * @param master_grid Costmap used for coordinate conversion.
+ * @param wx World X coordinate.
+ * @param wy World Y coordinate.
+ * @param out Output cell coordinate.
+ * @return True if the point is within map bounds, false if it was clamped.
+ */
+ bool worldToCell(
+ const nav2_costmap_2d::Costmap2D & master_grid,
+ double wx, double wy,
+ CellPoint & out) const;
+
+ /**
+ * @brief Trace all 4 edges of a quad into the span buffers (p0→p1→p2→p3→p0).
+ * @param p0 First vertex.
+ * @param p1 Second vertex.
+ * @param p2 Third vertex.
+ * @param p3 Fourth vertex.
+ * @param clamped_y_min Minimum Y index of the span buffer.
+ * @param height Height of the span buffer.
+ */
+ void traceQuad(
+ CellPoint p0, CellPoint p1, CellPoint p2, CellPoint p3,
+ int clamped_y_min, int height);
+
+ /**
+ * @brief Fill a convex quadrilateral corridor quad using a span buffer approach.
+ *
+ * See https://en.wikipedia.org/wiki/Scanline_rendering
+ * @param master_grid Costmap to write into.
+ * @param inner0 First inner boundary cell.
+ * @param inner1 Second inner boundary cell.
+ * @param outer0 First outer boundary cell.
+ * @param outer1 Second outer boundary cell.
+ */
+ void fillCorridorQuad(
+ nav2_costmap_2d::Costmap2D & master_grid,
+ CellPoint inner0,
+ CellPoint inner1,
+ CellPoint outer0,
+ CellPoint outer1);
+
+ /**
+ * @brief Reset the corridor interior mask over the union of current and previous fill bounding boxes.
+ * @param size_x Costmap width in cells.
+ * @param reset_min_i Union bbox minimum X cell index.
+ * @param reset_min_j Union bbox minimum Y cell index.
+ * @param reset_max_i Union bbox maximum X cell index.
+ * @param reset_max_j Union bbox maximum Y cell index.
+ */
+ void resetCorridorMask(
+ unsigned int size_x,
+ int reset_min_i, int reset_min_j,
+ int reset_max_i, int reset_max_j);
+
+ /**
+ * @brief Iterate the full path, split into bbox-clipped sub-segments, and
+ * build the corridor interior mask for each.
+ * @param master_grid Costmap used for coordinate conversion.
+ * @param full_path Full transformed path in costmap frame.
+ * @param fill_min_i Fill bbox minimum X cell index.
+ * @param fill_min_j Fill bbox minimum Y cell index.
+ * @param fill_max_i Fill bbox maximum X cell index.
+ * @param fill_max_j Fill bbox maximum Y cell index.
+ * @param extra_poses Extra poses beyond bbox boundary for geometric coverage.
+ */
+ void buildCorridorMask(
+ nav2_costmap_2d::Costmap2D & master_grid,
+ const nav_msgs::msg::Path & full_path,
+ int fill_min_i, int fill_min_j,
+ int fill_max_i, int fill_max_j,
+ size_t extra_poses, int r_cells_sq);
+
+ /**
+ * @brief Process a sub-segment of the path, filtering to the fill bbox margin
+ * and saving the corridor interior mask for each contiguous chunk.
+ * @param master_grid Costmap used for coordinate conversion.
+ * @param sub_segment Path sub-segment to process.
+ * @param fill_min_i Fill bbox minimum X cell index.
+ * @param fill_min_j Fill bbox minimum Y cell index.
+ * @param fill_max_i Fill bbox maximum X cell index.
+ * @param fill_max_j Fill bbox maximum Y cell index.
+ * @param extra_poses Extra poses beyond bbox boundary for geometric coverage.
+ */
+ void flushSubSegment(
+ nav2_costmap_2d::Costmap2D & master_grid,
+ nav_msgs::msg::Path & sub_segment,
+ int fill_min_i, int fill_min_j,
+ int fill_max_i, int fill_max_j,
+ size_t extra_poses);
+
+ std::string path_topic_;
+ tf2::Duration transform_tolerance_;
+
+ nav2::Subscription::SharedPtr path_sub_;
+ rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr on_set_params_handler_;
+ rclcpp::node_interfaces::PostSetParametersCallbackHandle::SharedPtr post_set_params_handler_;
+
+ std::mutex data_mutex_;
+ nav_msgs::msg::Path::ConstSharedPtr last_path_ptr_;
+
+ nav_msgs::msg::Path segment_buffer_;
+ nav_msgs::msg::Path full_transformed_path_buffer_;
+ WallPolygons walls_buffer_;
+ std::vector span_x_min_buffer_;
+ std::vector span_x_max_buffer_;
+};
+
+} // namespace nav2_costmap_2d
+
+#endif // NAV2_COSTMAP_2D__BOUNDED_TRACKING_ERROR_LAYER_HPP_
diff --git a/nav2_costmap_2d/plugins/bounded_tracking_error_layer.cpp b/nav2_costmap_2d/plugins/bounded_tracking_error_layer.cpp
new file mode 100644
index 00000000000..0e2e9090ddc
--- /dev/null
+++ b/nav2_costmap_2d/plugins/bounded_tracking_error_layer.cpp
@@ -0,0 +1,995 @@
+// Copyright (c) 2025 Berkan Tali
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "nav2_costmap_2d/bounded_tracking_error_layer.hpp"
+
+#include
+#include
+#include
+#include
+#include
+#include
+
+#include "pluginlib/class_list_macros.hpp"
+
+PLUGINLIB_EXPORT_CLASS(nav2_costmap_2d::BoundedTrackingErrorLayer, nav2_costmap_2d::Layer)
+
+using rcl_interfaces::msg::ParameterType;
+
+namespace nav2_costmap_2d
+{
+
+void
+BoundedTrackingErrorLayer::onInitialize()
+{
+ getParameters();
+
+ auto node = node_.lock();
+ if (!node) {
+ throw std::runtime_error{"Failed to lock node"};
+ }
+
+ path_sub_ = node->create_subscription(
+ joinWithParentNamespace(path_topic_),
+ std::bind(&BoundedTrackingErrorLayer::pathCallback, this, std::placeholders::_1),
+ nav2::qos::StandardTopicQoS());
+
+ // Seed resolution and frame eagerly; matchSize() will keep them current on subsequent resizes.
+ if (layered_costmap_) {
+ resolution_ = layered_costmap_->getCostmap()->getResolution();
+ costmap_frame_ = layered_costmap_->getGlobalFrameID();
+ }
+
+ current_ = true;
+ RCLCPP_INFO(
+ logger_,
+ "BoundedTrackingErrorLayer initialized in %s mode",
+ cost_write_mode_ == 0 ? "corridor" : cost_write_mode_ ==
+ 1 ? "fill (safe)" : "fill (overwrite)");
+}
+
+void
+BoundedTrackingErrorLayer::activate()
+{
+ auto node = node_.lock();
+ if (!node) {
+ throw std::runtime_error{"Failed to lock node"};
+ }
+ post_set_params_handler_ = node->add_post_set_parameters_callback(
+ std::bind(
+ &BoundedTrackingErrorLayer::updateParametersCallback,
+ this, std::placeholders::_1));
+ on_set_params_handler_ = node->add_on_set_parameters_callback(
+ std::bind(
+ &BoundedTrackingErrorLayer::validateParameterUpdatesCallback,
+ this, std::placeholders::_1));
+}
+
+void
+BoundedTrackingErrorLayer::deactivate()
+{
+ auto node = node_.lock();
+ if (post_set_params_handler_ && node) {
+ node->remove_post_set_parameters_callback(post_set_params_handler_.get());
+ }
+ post_set_params_handler_.reset();
+ if (on_set_params_handler_ && node) {
+ node->remove_on_set_parameters_callback(on_set_params_handler_.get());
+ }
+ on_set_params_handler_.reset();
+ path_sub_.reset();
+}
+
+void
+BoundedTrackingErrorLayer::reset()
+{
+ resetState();
+ current_ = false;
+}
+
+void
+BoundedTrackingErrorLayer::matchSize()
+{
+ resolution_ = layered_costmap_->getCostmap()->getResolution();
+ costmap_frame_ = layered_costmap_->getGlobalFrameID();
+ const auto * costmap = layered_costmap_->getCostmap();
+ corridor_interior_mask_.assign(
+ costmap->getSizeInCellsX() * costmap->getSizeInCellsY(), 0);
+ prev_fill_min_i_ = -1;
+ prev_fill_min_j_ = -1;
+ prev_fill_max_i_ = -1;
+ prev_fill_max_j_ = -1;
+}
+
+void
+BoundedTrackingErrorLayer::pathCallback(const nav_msgs::msg::Path::ConstSharedPtr msg)
+{
+ std::lock_guard lock(data_mutex_);
+ if (!last_path_ptr_ || msg->header.stamp != last_path_ptr_->header.stamp) {
+ current_path_index_.store(0);
+ }
+ last_path_ptr_ = msg;
+}
+
+void
+BoundedTrackingErrorLayer::updateBounds(
+ double robot_x, double robot_y, double /*robot_yaw*/,
+ double * min_x, double * min_y, double * max_x, double * max_y)
+{
+ if (!enabled_) {
+ return;
+ }
+
+ const double margin = look_ahead_ + corridor_width_ * 0.5 + wall_thickness_ * resolution_;
+
+ *min_x = std::min(*min_x, robot_x - margin);
+ *max_x = std::max(*max_x, robot_x + margin);
+ *min_y = std::min(*min_y, robot_y - margin);
+ *max_y = std::max(*max_y, robot_y + margin);
+}
+
+void
+BoundedTrackingErrorLayer::updateCosts(
+ nav2_costmap_2d::Costmap2D & master_grid,
+ [[maybe_unused]] int min_i, [[maybe_unused]] int min_j,
+ [[maybe_unused]] int max_i, [[maybe_unused]] int max_j)
+{
+ if (!enabled_) {
+ return;
+ }
+
+ if (resolution_ <= 0.0) {
+ RCLCPP_ERROR_THROTTLE(
+ logger_, *clock_, 1000,
+ "Resolution is not initialized (%.4f), skipping corridor update", resolution_);
+ return;
+ }
+
+ nav_msgs::msg::Path::ConstSharedPtr path_ptr;
+ size_t path_index;
+ {
+ std::lock_guard data_lock(data_mutex_);
+ path_ptr = last_path_ptr_;
+ path_index = current_path_index_.load();
+
+ if (path_ptr && !path_ptr->poses.empty()) {
+ const auto age = (clock_->now() - rclcpp::Time(path_ptr->header.stamp)).seconds();
+ if (age > 5.0) {
+ RCLCPP_WARN_THROTTLE(
+ logger_, *clock_, 5000,
+ "Path is %.2f seconds old, clearing corridor state — waiting for new plan", age);
+ last_path_ptr_.reset();
+ current_path_index_.store(0);
+ prev_fill_min_i_ = -1;
+ prev_fill_min_j_ = -1;
+ prev_fill_max_i_ = -1;
+ prev_fill_max_j_ = -1;
+ return;
+ }
+ }
+ }
+
+ if (!path_ptr || path_ptr->poses.empty()) {
+ return;
+ }
+
+ geometry_msgs::msg::PoseStamped robot_pose;
+
+ const double tf_tol = tf2::durationToSec(transform_tolerance_);
+
+ if (!nav2_util::getCurrentPose(
+ robot_pose, *tf_, costmap_frame_, robot_base_frame_, tf_tol))
+ {
+ RCLCPP_WARN_THROTTLE(
+ logger_,
+ *clock_,
+ 1000,
+ "Failed to get robot pose in %s, skipping corridor update",
+ costmap_frame_.c_str());
+ return;
+ }
+
+ const nav_msgs::msg::Path * full_transformed_ptr = nullptr;
+ if (path_ptr->header.frame_id == costmap_frame_) {
+ full_transformed_ptr = path_ptr.get();
+ } else {
+ if (!nav2_util::transformPathInTargetFrame(
+ *path_ptr, full_transformed_path_buffer_, *tf_, costmap_frame_, tf_tol))
+ {
+ RCLCPP_WARN_THROTTLE(
+ logger_, *clock_, 1000,
+ "Failed to transform path from '%s' to '%s', skipping corridor update",
+ path_ptr->header.frame_id.c_str(), costmap_frame_.c_str());
+ return;
+ }
+ full_transformed_ptr = &full_transformed_path_buffer_;
+ }
+
+ if (path_index >= full_transformed_ptr->poses.size()) {
+ path_index = 0;
+ current_path_index_.store(0);
+ }
+
+ const auto search_result = nav2_util::distance_from_path(
+ *full_transformed_ptr,
+ robot_pose.pose,
+ path_index,
+ look_ahead_);
+ current_path_index_.store(search_result.closest_segment_index);
+
+ getPathSegment(*full_transformed_ptr, search_result.closest_segment_index, segment_buffer_);
+
+ const size_t min_poses = (step_size_ * 2) + 1;
+ if (segment_buffer_.poses.size() < min_poses) {
+ RCLCPP_DEBUG_THROTTLE(
+ logger_, *clock_, 2000,
+ "Segment too small (%zu poses), need at least %zu — expected near end of path",
+ segment_buffer_.poses.size(), min_poses);
+ return;
+ }
+
+ if (cost_write_mode_ >= 1) {
+ if (corridor_interior_mask_.size() !=
+ master_grid.getSizeInCellsX() * master_grid.getSizeInCellsY())
+ {
+ RCLCPP_WARN_THROTTLE(
+ logger_, *clock_, 5000,
+ "Corridor interior mask size mismatch, skipping fill update — call matchSize()");
+ return;
+ }
+ applyFillOutsideCorridor(master_grid, robot_pose, *full_transformed_ptr);
+ } else {
+ getWallPolygons(segment_buffer_, walls_buffer_);
+ drawCorridorWalls(master_grid, walls_buffer_.left_inner, walls_buffer_.left_outer);
+ drawCorridorWalls(master_grid, walls_buffer_.right_inner, walls_buffer_.right_outer);
+ }
+}
+
+void
+BoundedTrackingErrorLayer::getPathSegment(
+ const nav_msgs::msg::Path & path,
+ const size_t path_index,
+ nav_msgs::msg::Path & segment)
+{
+ segment.poses.clear();
+
+ if (path.poses.empty() || path_index >= path.poses.size()) {
+ return;
+ }
+
+ size_t end_index = path_index;
+ double dist_traversed = 0.0;
+
+ for (size_t i = path_index; i < path.poses.size() - 1; i++) {
+ dist_traversed += nav2_util::geometry_utils::euclidean_distance(
+ path.poses[i], path.poses[i + 1]);
+
+ end_index = i + 1;
+ if (dist_traversed >= look_ahead_) {
+ break;
+ }
+ }
+
+ if (path_index < end_index && end_index < path.poses.size()) {
+ segment.header = path.header;
+ segment.poses.assign(
+ path.poses.begin() + path_index,
+ path.poses.begin() + end_index + 1);
+ } else if (path_index == path.poses.size() - 1) {
+ segment.header = path.header;
+ segment.poses.push_back(path.poses[path_index]);
+ }
+}
+
+void
+BoundedTrackingErrorLayer::getWallPolygons(
+ const nav_msgs::msg::Path & segment,
+ WallPolygons & walls)
+{
+ if (segment.poses.empty() || step_size_ == 0) {
+ walls.clearAndReserve(0);
+ return;
+ }
+
+ const double inner_offset = corridor_width_ * 0.5;
+ const double outer_offset = inner_offset + (wall_thickness_ * resolution_);
+
+ const size_t estimated_points = (segment.poses.size() / step_size_) + 1;
+ walls.clearAndReserve(estimated_points);
+
+ const size_t last_index = segment.poses.size() - 1;
+
+ for (size_t current_index = 0; current_index < segment.poses.size(); ) {
+ const auto & current_pose = segment.poses[current_index];
+ const double px = current_pose.pose.position.x;
+ const double py = current_pose.pose.position.y;
+
+ size_t next_index = current_index + step_size_;
+ if (next_index >= segment.poses.size()) {
+ next_index = last_index;
+ if (next_index == current_index) {
+ break;
+ }
+ }
+
+ const auto & next_pose = segment.poses[next_index];
+ const double dx = next_pose.pose.position.x - px;
+ const double dy = next_pose.pose.position.y - py;
+
+ const double norm = std::hypot(dx, dy);
+ if (norm < resolution_) {
+ RCLCPP_DEBUG_THROTTLE(
+ logger_, *clock_, 5000,
+ "Skipping zero-length path segment at index %zu: norm %.4f < resolution %.4f",
+ current_index, norm, resolution_);
+ if (current_index >= last_index) {
+ break;
+ }
+ current_index = std::min(current_index + step_size_, last_index);
+ continue;
+ }
+
+ // Unit perpendicular to the path direction (rotate 90 CCW): (-dy, dx).
+ // In an X-forward path frame this points to the left side of travel.
+ const double inv_norm = 1.0 / norm;
+ const double perp_x = -dy * inv_norm;
+ const double perp_y = dx * inv_norm;
+
+ walls.left_inner.push_back(
+ {px + perp_x * inner_offset, py + perp_y * inner_offset});
+ walls.right_inner.push_back(
+ {px - perp_x * inner_offset, py - perp_y * inner_offset});
+ walls.left_outer.push_back(
+ {px + perp_x * outer_offset, py + perp_y * outer_offset});
+ walls.right_outer.push_back(
+ {px - perp_x * outer_offset, py - perp_y * outer_offset});
+
+ if (current_index >= last_index) {
+ break;
+ }
+ current_index = std::min(current_index + step_size_, last_index);
+ }
+}
+
+void
+BoundedTrackingErrorLayer::applyFillOutsideCorridor(
+ nav2_costmap_2d::Costmap2D & master_grid,
+ const geometry_msgs::msg::PoseStamped & robot_pose,
+ const nav_msgs::msg::Path & full_path)
+{
+ const double fill_radius = look_ahead_ + corridor_width_ * 0.5 + wall_thickness_ * resolution_;
+ const double rx = robot_pose.pose.position.x;
+ const double ry = robot_pose.pose.position.y;
+
+ int cell_min_x, cell_min_y, cell_max_x, cell_max_y;
+ master_grid.worldToMapEnforceBounds(rx - fill_radius, ry - fill_radius, cell_min_x, cell_min_y);
+ master_grid.worldToMapEnforceBounds(rx + fill_radius, ry + fill_radius, cell_max_x, cell_max_y);
+
+ const unsigned int fill_size_x = master_grid.getSizeInCellsX();
+ const unsigned int fill_size_y = master_grid.getSizeInCellsY();
+
+ int fill_min_i = std::max(cell_min_x, 0);
+ int fill_min_j = std::max(cell_min_y, 0);
+ int fill_max_i = std::min(cell_max_x, static_cast(fill_size_x) - 1);
+ int fill_max_j = std::min(cell_max_y, static_cast(fill_size_y) - 1);
+
+ const int reset_min_i = (prev_fill_min_i_ < 0) ? fill_min_i : std::min(fill_min_i,
+ prev_fill_min_i_);
+ const int reset_min_j = (prev_fill_min_j_ < 0) ? fill_min_j : std::min(fill_min_j,
+ prev_fill_min_j_);
+ const int reset_max_i = (prev_fill_max_i_ < 0) ? fill_max_i : std::max(fill_max_i,
+ prev_fill_max_i_);
+ const int reset_max_j = (prev_fill_max_j_ < 0) ? fill_max_j : std::max(fill_max_j,
+ prev_fill_max_j_);
+
+ resetCorridorMask(
+ master_grid.getSizeInCellsX(),
+ reset_min_i, reset_min_j,
+ reset_max_i, reset_max_j);
+
+ prev_fill_min_i_ = fill_min_i;
+ prev_fill_min_j_ = fill_min_j;
+ prev_fill_max_i_ = fill_max_i;
+ prev_fill_max_j_ = fill_max_j;
+
+ const double r_cells = (corridor_width_ * 0.5) / resolution_;
+ const int r_cells_sq = static_cast(std::llround(r_cells * r_cells));
+
+ // extra_poses extends wall polygon generation beyond the bbox boundary to cover
+ // geometric gaps at sub-segment exit points on diagonal paths.
+ const size_t extra_poses = static_cast(
+ std::ceil(corridor_width_ / resolution_)) + step_size_;
+
+ buildCorridorMask(
+ master_grid, full_path,
+ fill_min_i, fill_min_j,
+ fill_max_i, fill_max_j,
+ extra_poses, r_cells_sq);
+
+ RCLCPP_DEBUG_THROTTLE(
+ logger_, *clock_, 2000,
+ "Fill bbox: [%d,%d] to [%d,%d], extra_poses=%zu",
+ fill_min_i, fill_min_j, fill_max_i, fill_max_j,
+ extra_poses);
+
+ fillOutsideCorridor(master_grid, fill_min_i, fill_min_j, fill_max_i, fill_max_j);
+}
+
+void
+BoundedTrackingErrorLayer::drawCorridorWalls(
+ nav2_costmap_2d::Costmap2D & master_grid,
+ const std::vector> & inner_points,
+ const std::vector> & outer_points)
+{
+ if (inner_points.size() < 2 || outer_points.size() < 2) {
+ RCLCPP_DEBUG(
+ logger_,
+ "Skipping corridor wall draw: inner=%zu, outer=%zu points (need >= 2 each)",
+ inner_points.size(), outer_points.size());
+ return;
+ }
+
+ const size_t num_segments = std::min(inner_points.size(), outer_points.size()) - 1;
+
+ for (size_t i = 0; i < num_segments; ++i) {
+ CellPoint inner0, inner1, outer0, outer1;
+
+ const bool inner0_valid = worldToCell(
+ master_grid, inner_points[i][0], inner_points[i][1], inner0);
+ const bool inner1_valid = worldToCell(
+ master_grid, inner_points[i + 1][0], inner_points[i + 1][1], inner1);
+ if (!inner0_valid && !inner1_valid) {
+ continue;
+ }
+
+ worldToCell(master_grid, outer_points[i][0], outer_points[i][1], outer0);
+ worldToCell(master_grid, outer_points[i + 1][0], outer_points[i + 1][1], outer1);
+
+ fillCorridorQuad(master_grid, inner0, inner1, outer0, outer1);
+ }
+}
+
+void
+BoundedTrackingErrorLayer::saveCorridorInterior(
+ nav2_costmap_2d::Costmap2D & master_grid,
+ const WallPolygons & walls)
+{
+ if (walls.left_inner.size() < 2 || walls.right_inner.size() < 2) {
+ RCLCPP_DEBUG(
+ logger_,
+ "Skipping corridor interior save: left_inner=%zu, right_inner=%zu points (need >= 2 each)",
+ walls.left_inner.size(), walls.right_inner.size());
+ return;
+ }
+
+ const unsigned int size_x = master_grid.getSizeInCellsX();
+ const unsigned int size_y = master_grid.getSizeInCellsY();
+
+ const size_t num_segments = std::min(walls.left_inner.size(), walls.right_inner.size()) - 1;
+
+ for (size_t i = 0; i < num_segments; ++i) {
+ CellPoint left0, left1, right0, right1;
+
+ const bool l0 = worldToCell(
+ master_grid, walls.left_inner[i][0], walls.left_inner[i][1], left0);
+ const bool l1 = worldToCell(
+ master_grid, walls.left_inner[i + 1][0], walls.left_inner[i + 1][1], left1);
+ const bool r0 = worldToCell(
+ master_grid, walls.right_inner[i][0], walls.right_inner[i][1], right0);
+ const bool r1 = worldToCell(
+ master_grid, walls.right_inner[i + 1][0], walls.right_inner[i + 1][1], right1);
+
+ if (!l0 && !l1 && !r0 && !r1) {
+ continue;
+ }
+
+ const int y_min = std::min({left0.y, left1.y, right0.y, right1.y});
+ const int y_max = std::max({left0.y, left1.y, right0.y, right1.y});
+ const int clamped_y_min = std::max(y_min, 0);
+ const int clamped_y_max = std::min(y_max, static_cast(size_y) - 1);
+
+ if (clamped_y_min > clamped_y_max) {
+ continue;
+ }
+
+ const int height = clamped_y_max - clamped_y_min + 1;
+ span_x_min_buffer_.assign(height, std::numeric_limits::max());
+ span_x_max_buffer_.assign(height, std::numeric_limits::min());
+ traceQuad(left0, left1, right1, right0, clamped_y_min, height);
+
+ for (int buffer_idx = 0; buffer_idx < height; ++buffer_idx) {
+ const int y = clamped_y_min + buffer_idx;
+ const int x_min = span_x_min_buffer_[buffer_idx];
+ const int x_max = span_x_max_buffer_[buffer_idx];
+
+ if (x_min > x_max) {
+ continue;
+ }
+
+ const int x_start = std::max(x_min, 0);
+ const int x_end = std::min(x_max, static_cast(size_x) - 1);
+
+ for (int x = x_start; x <= x_end; ++x) {
+ const unsigned int flat_idx =
+ static_cast(y) * size_x + static_cast(x);
+ corridor_interior_mask_[flat_idx] = 1;
+ }
+ }
+ }
+}
+
+void
+BoundedTrackingErrorLayer::markCircleAsInterior(
+ nav2_costmap_2d::Costmap2D & master_grid,
+ int cx, int cy, int r_sq)
+{
+ const unsigned int size_x = master_grid.getSizeInCellsX();
+ const unsigned int size_y = master_grid.getSizeInCellsY();
+ const int r = static_cast(std::ceil(std::sqrt(static_cast(r_sq))));
+
+ const int x_start = std::max(cx - r, 0);
+ const int x_end = std::min(cx + r, static_cast(size_x) - 1);
+ const int y_start = std::max(cy - r, 0);
+ const int y_end = std::min(cy + r, static_cast(size_y) - 1);
+
+ for (int y = y_start; y <= y_end; ++y) {
+ const int dy = y - cy;
+ for (int x = x_start; x <= x_end; ++x) {
+ const int dx = x - cx;
+ if (dx * dx + dy * dy <= r_sq) {
+ const unsigned int flat_idx =
+ static_cast(y) * size_x + static_cast(x);
+ corridor_interior_mask_[flat_idx] = 1;
+ }
+ }
+ }
+}
+
+void
+BoundedTrackingErrorLayer::fillOutsideCorridor(
+ nav2_costmap_2d::Costmap2D & master_grid,
+ int min_i, int min_j, int max_i, int max_j)
+{
+ unsigned char * costmap = master_grid.getCharMap();
+ const unsigned int size_x = master_grid.getSizeInCellsX();
+ const unsigned int size_y = master_grid.getSizeInCellsY();
+
+ const int x_start = std::max(min_i, 0);
+ const int x_end = std::min(max_i, static_cast(size_x) - 1);
+ const int y_start = std::max(min_j, 0);
+ const int y_end = std::min(max_j, static_cast(size_y) - 1);
+
+ for (int y = y_start; y <= y_end; ++y) {
+ for (int x = x_start; x <= x_end; ++x) {
+ const unsigned int flat_idx = static_cast(y) * size_x +
+ static_cast(x);
+ if (corridor_interior_mask_[flat_idx]) {
+ continue;
+ }
+ if (cost_write_mode_ == 2) {
+ costmap[flat_idx] = corridor_cost_;
+ } else {
+ costmap[flat_idx] = std::max(costmap[flat_idx], corridor_cost_);
+ }
+ }
+ }
+}
+
+void
+BoundedTrackingErrorLayer::resetState()
+{
+ std::lock_guard lock(data_mutex_);
+ last_path_ptr_.reset();
+ current_path_index_.store(0);
+ prev_fill_min_i_ = -1;
+ prev_fill_min_j_ = -1;
+ prev_fill_max_i_ = -1;
+ prev_fill_max_j_ = -1;
+}
+
+void
+BoundedTrackingErrorLayer::getParameters()
+{
+ auto node = node_.lock();
+ if (!node) {
+ throw std::runtime_error{"Failed to lock node"};
+ }
+
+ path_topic_ = node->declare_or_get_parameter(
+ name_ + "." + "path_topic", std::string("plan"));
+
+ const int step_param = node->declare_or_get_parameter(name_ + "." + "step", 10);
+ if (step_param <= 0) {
+ throw std::runtime_error{"step must be greater than zero"};
+ }
+ step_size_ = static_cast(step_param);
+
+ look_ahead_ = node->declare_or_get_parameter(name_ + "." + "look_ahead", 2.5);
+ if (look_ahead_ <= 0.0) {
+ throw std::runtime_error{"look_ahead must be positive"};
+ }
+
+ corridor_width_ = node->declare_or_get_parameter(name_ + "." + "corridor_width", 2.0);
+ if (corridor_width_ <= 0.0) {
+ throw std::runtime_error{"corridor_width must be positive"};
+ }
+
+ wall_thickness_ = node->declare_or_get_parameter(name_ + "." + "wall_thickness", 1);
+ if (wall_thickness_ <= 0) {
+ throw std::runtime_error{"wall_thickness must be greater than zero"};
+ }
+
+ int corridor_cost_param = node->declare_or_get_parameter(name_ + "." + "corridor_cost", 190);
+ if (corridor_cost_param <= 0 || corridor_cost_param > 254) {
+ throw std::runtime_error{"corridor_cost must be between 1 and 254"};
+ }
+ corridor_cost_ = static_cast(corridor_cost_param);
+
+ int cost_write_mode_param = node->declare_or_get_parameter(
+ name_ + "." + "cost_write_mode", 0);
+ if (cost_write_mode_param < 0 || cost_write_mode_param > 2) {
+ throw std::runtime_error{
+ "cost_write_mode must be 0 (max), 1 (overwrite walls), or 2 (overwrite all)"};
+ }
+ cost_write_mode_ = cost_write_mode_param;
+
+ enabled_ = node->declare_or_get_parameter(name_ + "." + "enabled", true);
+
+ double temp_tf_tol = 0.1;
+ node->get_parameter("transform_tolerance", temp_tf_tol);
+ transform_tolerance_ = tf2::durationFromSec(temp_tf_tol);
+
+ node->get_parameter("robot_base_frame", robot_base_frame_);
+}
+
+bool
+BoundedTrackingErrorLayer::worldToCell(
+ const nav2_costmap_2d::Costmap2D & master_grid,
+ double wx, double wy,
+ CellPoint & out) const
+{
+ unsigned int ux, uy;
+ if (master_grid.worldToMap(wx, wy, ux, uy)) {
+ out = {static_cast(ux), static_cast(uy)};
+ return true;
+ }
+ // Clamp to costmap boundary so quads straddling the edge are still drawn.
+ int cx, cy;
+ master_grid.worldToMapNoBounds(wx, wy, cx, cy);
+ out = {
+ std::clamp(cx, 0, static_cast(master_grid.getSizeInCellsX()) - 1),
+ std::clamp(cy, 0, static_cast(master_grid.getSizeInCellsY()) - 1)
+ };
+ return false;
+}
+
+void
+BoundedTrackingErrorLayer::traceQuad(
+ CellPoint p0, CellPoint p1, CellPoint p2, CellPoint p3,
+ int clamped_y_min, int height)
+{
+ auto trace = [&](CellPoint a, CellPoint b) {
+ nav2_util::LineIterator line(a.x, a.y, b.x, b.y);
+ for (; line.isValid(); line.advance()) {
+ const int x = line.getX();
+ const int y = line.getY();
+ const int buffer_idx = y - clamped_y_min;
+ if (buffer_idx >= 0 && buffer_idx < height) {
+ span_x_min_buffer_[buffer_idx] = std::min(span_x_min_buffer_[buffer_idx], x);
+ span_x_max_buffer_[buffer_idx] = std::max(span_x_max_buffer_[buffer_idx], x);
+ }
+ }
+ };
+
+ trace(p0, p1);
+ trace(p1, p2);
+ trace(p2, p3);
+ trace(p3, p0);
+}
+
+void
+BoundedTrackingErrorLayer::fillCorridorQuad(
+ nav2_costmap_2d::Costmap2D & master_grid,
+ CellPoint inner0,
+ CellPoint inner1,
+ CellPoint outer0,
+ CellPoint outer1)
+{
+ unsigned char * costmap = master_grid.getCharMap();
+ const unsigned int size_x = master_grid.getSizeInCellsX();
+ const unsigned int size_y = master_grid.getSizeInCellsY();
+
+ const int y_min = std::min({inner0.y, inner1.y, outer0.y, outer1.y});
+ const int y_max = std::max({inner0.y, inner1.y, outer0.y, outer1.y});
+
+ const int clamped_y_min = std::max(y_min, 0);
+ const int clamped_y_max = std::min(y_max, static_cast(size_y) - 1);
+
+ if (clamped_y_min > clamped_y_max) {
+ return;
+ }
+
+ const int height = clamped_y_max - clamped_y_min + 1;
+
+ span_x_min_buffer_.assign(height, std::numeric_limits::max());
+ span_x_max_buffer_.assign(height, std::numeric_limits::min());
+
+ traceQuad(inner0, inner1, outer1, outer0, clamped_y_min, height);
+
+ for (int buffer_idx = 0; buffer_idx < height; ++buffer_idx) {
+ const int y = clamped_y_min + buffer_idx;
+ const int x_min = span_x_min_buffer_[buffer_idx];
+ const int x_max = span_x_max_buffer_[buffer_idx];
+
+ if (x_min > x_max) {
+ continue;
+ }
+
+ // Clamp to costmap bounds before writing.
+ const int x_start = std::max(x_min, 0);
+ const int x_end = std::min(x_max, static_cast(size_x) - 1);
+
+ for (int x = x_start; x <= x_end; ++x) {
+ costmap[y * size_x + x] = std::max(costmap[y * size_x + x], corridor_cost_);
+ }
+ }
+}
+
+void
+BoundedTrackingErrorLayer::resetCorridorMask(
+ unsigned int size_x,
+ int reset_min_i, int reset_min_j,
+ int reset_max_i, int reset_max_j)
+{
+ for (int y = reset_min_j; y <= reset_max_j; ++y) {
+ std::fill(
+ corridor_interior_mask_.begin() + y * size_x + reset_min_i,
+ corridor_interior_mask_.begin() + y * size_x + reset_max_i + 1,
+ 0);
+ }
+}
+
+void
+BoundedTrackingErrorLayer::buildCorridorMask(
+ nav2_costmap_2d::Costmap2D & master_grid,
+ const nav_msgs::msg::Path & full_path,
+ int fill_min_i, int fill_min_j,
+ int fill_max_i, int fill_max_j,
+ size_t extra_poses, int r_cells_sq)
+{
+ nav_msgs::msg::Path sub_segment;
+ sub_segment.header = full_path.header;
+ bool first_hit_marked = false;
+
+ for (size_t i = 0; i < full_path.poses.size(); ++i) {
+ const auto & pose = full_path.poses[i];
+ unsigned int mx, my;
+ const bool in_area = master_grid.worldToMap(
+ pose.pose.position.x, pose.pose.position.y, mx, my) &&
+ static_cast(mx) >= fill_min_i &&
+ static_cast(mx) <= fill_max_i &&
+ static_cast(my) >= fill_min_j &&
+ static_cast(my) <= fill_max_j;
+
+ if (in_area) {
+ if (!first_hit_marked) {
+ markCircleAsInterior(master_grid, static_cast(mx), static_cast(my), r_cells_sq);
+ first_hit_marked = true;
+ }
+ sub_segment.poses.push_back(pose);
+ } else {
+ for (size_t j = i; j < std::min(i + extra_poses, full_path.poses.size()); ++j) {
+ sub_segment.poses.push_back(full_path.poses[j]);
+ }
+ flushSubSegment(
+ master_grid, sub_segment,
+ fill_min_i, fill_min_j,
+ fill_max_i, fill_max_j,
+ extra_poses);
+ }
+ }
+ flushSubSegment(
+ master_grid, sub_segment,
+ fill_min_i, fill_min_j,
+ fill_max_i, fill_max_j,
+ extra_poses);
+}
+
+void
+BoundedTrackingErrorLayer::flushSubSegment(
+ nav2_costmap_2d::Costmap2D & master_grid,
+ nav_msgs::msg::Path & sub_segment,
+ int fill_min_i, int fill_min_j,
+ int fill_max_i, int fill_max_j,
+ size_t extra_poses)
+{
+ if (sub_segment.poses.size() < 2) {
+ sub_segment.poses.clear();
+ return;
+ }
+
+ nav_msgs::msg::Path bbox_segment;
+ bbox_segment.header = sub_segment.header;
+
+ auto flush_bbox_segment = [&]() {
+ if (bbox_segment.poses.size() >= 2) {
+ WallPolygons fill_walls;
+ getWallPolygons(bbox_segment, fill_walls);
+ saveCorridorInterior(master_grid, fill_walls);
+ }
+ bbox_segment.poses.clear();
+ };
+
+ for (const auto & p : sub_segment.poses) {
+ unsigned int mx, my;
+ const bool in_margin = master_grid.worldToMap(
+ p.pose.position.x, p.pose.position.y, mx, my) &&
+ static_cast(mx) >= fill_min_i - static_cast(extra_poses) &&
+ static_cast(mx) <= fill_max_i + static_cast(extra_poses) &&
+ static_cast(my) >= fill_min_j - static_cast(extra_poses) &&
+ static_cast(my) <= fill_max_j + static_cast(extra_poses);
+
+ if (in_margin) {
+ bbox_segment.poses.push_back(p);
+ } else {
+ flush_bbox_segment();
+ }
+ }
+ flush_bbox_segment();
+
+ sub_segment.poses.clear();
+}
+
+rcl_interfaces::msg::SetParametersResult
+BoundedTrackingErrorLayer::validateParameterUpdatesCallback(
+ const std::vector & parameters)
+{
+ rcl_interfaces::msg::SetParametersResult result;
+ result.successful = true;
+
+ for (const auto & parameter : parameters) {
+ const auto & param_type = parameter.get_type();
+ const auto & param_name = parameter.get_name();
+
+ if (param_name.find(name_ + ".") != 0) {
+ continue;
+ }
+
+ if (param_type == ParameterType::PARAMETER_DOUBLE) {
+ if (param_name == name_ + "." + "look_ahead") {
+ const double new_value = parameter.as_double();
+ if (new_value <= 0.0) {
+ RCLCPP_WARN(
+ logger_, "The value of parameter '%s' is incorrectly set to %f, "
+ "it should be > 0. Rejecting parameter update.",
+ param_name.c_str(), new_value);
+ result.successful = false;
+ result.reason = "look_ahead must be positive";
+ }
+ } else if (param_name == name_ + "." + "corridor_width") {
+ const double new_value = parameter.as_double();
+ if (new_value <= 0.0) {
+ RCLCPP_WARN(
+ logger_, "The value of parameter '%s' is incorrectly set to %f, "
+ "it should be > 0. Rejecting parameter update.",
+ param_name.c_str(), new_value);
+ result.successful = false;
+ result.reason = "corridor_width must be positive";
+ }
+ }
+ } else if (param_type == ParameterType::PARAMETER_INTEGER) {
+ if (param_name == name_ + "." + "step") {
+ const int new_value = parameter.as_int();
+ if (new_value <= 0) {
+ RCLCPP_WARN(
+ logger_, "The value of parameter '%s' is incorrectly set to %d, "
+ "it should be > 0. Rejecting parameter update.",
+ param_name.c_str(), new_value);
+ result.successful = false;
+ result.reason = "step must be greater than zero";
+ }
+ } else if (param_name == name_ + "." + "corridor_cost") {
+ const int new_value = parameter.as_int();
+ if (new_value <= 0 || new_value > 254) {
+ RCLCPP_WARN(
+ logger_, "The value of parameter '%s' is incorrectly set to %d, "
+ "it should be between 1 and 254. Rejecting parameter update.",
+ param_name.c_str(), new_value);
+ result.successful = false;
+ result.reason = "corridor_cost must be between 1 and 254";
+ }
+ } else if (param_name == name_ + "." + "wall_thickness") {
+ const int new_value = parameter.as_int();
+ if (new_value <= 0) {
+ RCLCPP_WARN(
+ logger_, "The value of parameter '%s' is incorrectly set to %d, "
+ "it should be > 0. Rejecting parameter update.",
+ param_name.c_str(), new_value);
+ result.successful = false;
+ result.reason = "wall_thickness must be greater than zero";
+ }
+ } else if (param_name == name_ + "." + "cost_write_mode") {
+ const int new_value = parameter.as_int();
+ if (new_value < 0 || new_value > 2) {
+ RCLCPP_WARN(
+ logger_, "The value of parameter '%s' is incorrectly set to %d, "
+ "it should be 0, 1, or 2. Rejecting parameter update.",
+ param_name.c_str(), new_value);
+ result.successful = false;
+ result.reason =
+ "cost_write_mode must be 0 (corridor), 1 (fill max), or 2 (fill overwrite)";
+ }
+ }
+ }
+ }
+
+ return result;
+}
+
+void
+BoundedTrackingErrorLayer::updateParametersCallback(
+ const std::vector & parameters)
+{
+ std::lock_guard guard(data_mutex_);
+
+ for (const auto & parameter : parameters) {
+ const auto & param_type = parameter.get_type();
+ const auto & param_name = parameter.get_name();
+
+ if (param_name.find(name_ + ".") != 0) {
+ continue;
+ }
+
+ if (param_type == ParameterType::PARAMETER_DOUBLE) {
+ const bool is_look_ahead = param_name == name_ + "." + "look_ahead";
+ const bool is_corridor_width = param_name == name_ + "." + "corridor_width";
+ if (is_look_ahead && look_ahead_ != parameter.as_double()) {
+ look_ahead_ = parameter.as_double();
+ current_ = false;
+ } else if (is_corridor_width && corridor_width_ != parameter.as_double()) {
+ corridor_width_ = parameter.as_double();
+ current_ = false;
+ }
+ } else if (param_type == ParameterType::PARAMETER_BOOL) {
+ const bool is_enabled = param_name == name_ + "." + "enabled";
+ if (is_enabled && enabled_ != parameter.as_bool()) {
+ enabled_ = parameter.as_bool();
+ current_ = false;
+ }
+ } else if (param_type == ParameterType::PARAMETER_INTEGER) {
+ const bool is_step = param_name == name_ + "." + "step";
+ const bool is_cost = param_name == name_ + "." + "corridor_cost";
+ const bool is_thickness = param_name == name_ + "." + "wall_thickness";
+ const bool is_write_mode = param_name == name_ + "." + "cost_write_mode";
+ if (is_step) {
+ const int new_step = parameter.as_int();
+ if (static_cast(new_step) != step_size_) {
+ step_size_ = static_cast(new_step);
+ current_ = false;
+ }
+ } else if (is_cost && corridor_cost_ != static_cast(parameter.as_int())) {
+ corridor_cost_ = static_cast(parameter.as_int());
+ current_ = false;
+ } else if (is_thickness && wall_thickness_ != parameter.as_int()) {
+ wall_thickness_ = parameter.as_int();
+ current_ = false;
+ } else if (is_write_mode && cost_write_mode_ != parameter.as_int()) {
+ cost_write_mode_ = parameter.as_int();
+ current_ = false;
+ }
+ }
+ }
+}
+
+} // namespace nav2_costmap_2d
diff --git a/nav2_costmap_2d/test/unit/CMakeLists.txt b/nav2_costmap_2d/test/unit/CMakeLists.txt
index 621b4e8b119..c0ef1b295cf 100644
--- a/nav2_costmap_2d/test/unit/CMakeLists.txt
+++ b/nav2_costmap_2d/test/unit/CMakeLists.txt
@@ -77,3 +77,9 @@ target_link_libraries(obstacle_layer_test
nav2_costmap_2d_core
layers
)
+
+nav2_add_gtest(bounded_tracking_error_layer_test bounded_tracking_error_layer_test.cpp)
+target_link_libraries(bounded_tracking_error_layer_test
+ nav2_costmap_2d_core
+ layers
+)
diff --git a/nav2_costmap_2d/test/unit/bounded_tracking_error_layer_test.cpp b/nav2_costmap_2d/test/unit/bounded_tracking_error_layer_test.cpp
new file mode 100644
index 00000000000..0ebcecf54e1
--- /dev/null
+++ b/nav2_costmap_2d/test/unit/bounded_tracking_error_layer_test.cpp
@@ -0,0 +1,1754 @@
+// Copyright (c) 2025 Berkan Tali
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include
+
+#include
+#include
+#include
+#include
+#include
+
+#include "nav2_costmap_2d/bounded_tracking_error_layer.hpp"
+#include "nav2_costmap_2d/cost_values.hpp"
+#include "nav2_costmap_2d/costmap_2d.hpp"
+#include "nav2_costmap_2d/layered_costmap.hpp"
+#include "nav2_ros_common/lifecycle_node.hpp"
+#include "tf2_ros/buffer.hpp"
+#include "tf2_ros/transform_listener.hpp"
+#include "tf2_ros/static_transform_broadcaster.hpp"
+#include "nav_msgs/msg/path.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "geometry_msgs/msg/transform_stamped.hpp"
+
+class RclCppFixture
+{
+public:
+ RclCppFixture() {rclcpp::init(0, nullptr);}
+ ~RclCppFixture() {rclcpp::shutdown();}
+};
+RclCppFixture g_rclcppfixture;
+
+class TestLifecycleNode : public nav2::LifecycleNode
+{
+public:
+ explicit TestLifecycleNode(const std::string & name)
+ : nav2::LifecycleNode(name) {}
+
+ nav2::CallbackReturn on_configure(const rclcpp_lifecycle::State &)
+ {return nav2::CallbackReturn::SUCCESS;}
+ nav2::CallbackReturn on_activate(const rclcpp_lifecycle::State &)
+ {return nav2::CallbackReturn::SUCCESS;}
+ nav2::CallbackReturn on_deactivate(const rclcpp_lifecycle::State &)
+ {return nav2::CallbackReturn::SUCCESS;}
+ nav2::CallbackReturn on_cleanup(const rclcpp_lifecycle::State &)
+ {return nav2::CallbackReturn::SUCCESS;}
+ nav2::CallbackReturn onShutdown(const rclcpp_lifecycle::State &)
+ {return nav2::CallbackReturn::SUCCESS;}
+ nav2::CallbackReturn onError(const rclcpp_lifecycle::State &)
+ {return nav2::CallbackReturn::SUCCESS;}
+};
+
+class TestableBoundedTrackingErrorLayer
+ : public nav2_costmap_2d::BoundedTrackingErrorLayer
+{
+public:
+ using WallPolygons = BoundedTrackingErrorLayer::WallPolygons;
+
+ void testGetPathSegment(
+ const nav_msgs::msg::Path & path, size_t idx,
+ nav_msgs::msg::Path & segment)
+ {
+ getPathSegment(path, idx, segment);
+ }
+
+ void testGetWallPolygons(
+ const nav_msgs::msg::Path & segment,
+ WallPolygons & walls)
+ {
+ getWallPolygons(segment, walls);
+ }
+
+ void testDrawCorridorWalls(
+ nav2_costmap_2d::Costmap2D & master_grid,
+ const std::vector> & inner_points,
+ const std::vector> & outer_points)
+ {
+ drawCorridorWalls(master_grid, inner_points, outer_points);
+ }
+
+ void testPathCallback(const nav_msgs::msg::Path::ConstSharedPtr msg)
+ {
+ pathCallback(msg);
+ }
+
+ void testResetState() {resetState();}
+
+ rcl_interfaces::msg::SetParametersResult testValidateParams(
+ const std::vector & params)
+ {
+ return validateParameterUpdatesCallback(params);
+ }
+
+ void testUpdateParams(const std::vector & params)
+ {
+ updateParametersCallback(params);
+ }
+
+ bool & enabledRef() {return enabled_;}
+ std::atomic_bool & currentRef() {return current_;}
+ std::atomic & pathIndexRef() {return current_path_index_;}
+
+ void setStepSize(size_t s) {step_size_ = s;}
+ void setLookAhead(double v) {look_ahead_ = v;}
+ void setCorridorWidth(double v) {corridor_width_ = v;}
+ void setWallThickness(int v) {wall_thickness_ = v;}
+ void setCorridorCost(unsigned char v) {corridor_cost_ = v;}
+ void setResolution(double v) {resolution_ = v;}
+ void setCostmapFrame(const std::string & f) {costmap_frame_ = f;}
+ void setCostWriteMode(int v) {cost_write_mode_ = v;}
+ void setRobotBaseFrame(const std::string & f) {robot_base_frame_ = f;}
+
+ void testSaveCorridorInterior(
+ nav2_costmap_2d::Costmap2D & master_grid,
+ const WallPolygons & walls)
+ {
+ saveCorridorInterior(master_grid, walls);
+ }
+
+ void resetCorridorInteriorMask()
+ {
+ std::fill(corridor_interior_mask_.begin(), corridor_interior_mask_.end(), false);
+ }
+
+ void testMarkCircleAsInterior(
+ nav2_costmap_2d::Costmap2D & master_grid,
+ int cx, int cy, int r_sq)
+ {
+ markCircleAsInterior(master_grid, cx, cy, r_sq);
+ }
+
+ void testFillOutsideCorridor(
+ nav2_costmap_2d::Costmap2D & master_grid,
+ int min_i, int min_j, int max_i, int max_j)
+ {
+ fillOutsideCorridor(master_grid, min_i, min_j, max_i, max_j);
+ }
+
+ bool isInterior(unsigned int flat_idx) const
+ {
+ return flat_idx < corridor_interior_mask_.size() && corridor_interior_mask_[flat_idx];
+ }
+
+ size_t getStepSize() const {return step_size_;}
+ double getLookAhead() const {return look_ahead_;}
+ double getCorridorWidth() const {return corridor_width_;}
+ int getWallThickness() const {return wall_thickness_;}
+ unsigned char getCorridorCost() const {return corridor_cost_;}
+ int getCostWriteMode() const {return cost_write_mode_;}
+ double getResolution() const {return resolution_;}
+ const std::string & getCostmapFrame() const {return costmap_frame_;}
+};
+
+static constexpr const char * kRobotBaseFrame = "base_link";
+
+// Shared preamble for updateCosts integration tests: enables the layer, sets the
+// robot frame, calls matchSize, and resets the costmap to FREE_SPACE.
+static void prepareForUpdateCosts(
+ TestableBoundedTrackingErrorLayer * layer,
+ nav2_costmap_2d::Costmap2D * costmap)
+{
+ layer->matchSize();
+ layer->enabledRef() = true;
+ layer->setRobotBaseFrame(kRobotBaseFrame);
+ costmap->resetMapToValue(
+ 0, 0, costmap->getSizeInCellsX(), costmap->getSizeInCellsY(),
+ nav2_costmap_2d::FREE_SPACE);
+}
+
+static geometry_msgs::msg::PoseStamped makePose(
+ double x, double y,
+ const std::string & frame = "map")
+{
+ geometry_msgs::msg::PoseStamped ps;
+ ps.header.frame_id = frame;
+ ps.pose.position.x = x;
+ ps.pose.position.y = y;
+ ps.pose.position.z = 0.0;
+ ps.pose.orientation.w = 1.0;
+ return ps;
+}
+
+// dir_x / dir_y form a direction vector (need not be unit length — normalised internally).
+static nav_msgs::msg::Path makeStraightPath(
+ double x0, double y0,
+ double dir_x, double dir_y,
+ size_t num_poses, double spacing,
+ const std::string & frame = "map")
+{
+ nav_msgs::msg::Path path;
+ path.header.frame_id = frame;
+ double len = std::hypot(dir_x, dir_y);
+ double ux = dir_x / len;
+ double uy = dir_y / len;
+ for (size_t i = 0; i < num_poses; ++i) {
+ path.poses.push_back(
+ makePose(x0 + ux * spacing * i, y0 + uy * spacing * i, frame));
+ }
+ return path;
+}
+
+// num_poses must be >= 2 (start and end point at minimum).
+static nav_msgs::msg::Path makeCircularPath(
+ double cx, double cy, double radius,
+ double start_angle, double end_angle,
+ size_t num_poses,
+ const std::string & frame = "map")
+{
+ assert(num_poses >= 2 && "makeCircularPath requires at least 2 poses");
+ nav_msgs::msg::Path path;
+ path.header.frame_id = frame;
+ for (size_t i = 0; i < num_poses; ++i) {
+ double t = static_cast(i) / static_cast(num_poses - 1);
+ double angle = start_angle + t * (end_angle - start_angle);
+ path.poses.push_back(
+ makePose(cx + radius * std::cos(angle), cy + radius * std::sin(angle), frame));
+ }
+ return path;
+}
+
+// Fixed-seed 32-bit LCG (a=1664525, c=1013904223, Numerical Recipes) — platform-independent
+// sequence without touching global RNG state.
+static nav_msgs::msg::Path makeRandomPath(
+ double x0, double y0, size_t num_poses,
+ double step = 0.15,
+ const std::string & frame = "map")
+{
+ nav_msgs::msg::Path path;
+ path.header.frame_id = frame;
+ uint32_t seed = 0xdeadbeef;
+ // Numerical Recipes 32-bit LCG: multiplier=1664525, increment=1013904223
+ auto lcg = [&]() -> double {
+ seed = seed * 1664525u + 1013904223u;
+ return (static_cast(seed & 0xFFFF) / 65535.0) * 2.0 - 1.0;
+ };
+ double x = x0, y = y0;
+ for (size_t i = 0; i < num_poses; ++i) {
+ path.poses.push_back(makePose(x, y, frame));
+ // x advances by step ± 0.04, so the path is not strictly monotone in x.
+ x += step + lcg() * 0.04;
+ y += lcg() * 0.04;
+ }
+ return path;
+}
+
+class BoundedTrackingErrorLayerTest : public ::testing::Test
+{
+protected:
+ void SetUp() override
+ {
+ node_ = std::make_shared("bte_test_node");
+ node_->declare_parameter("transform_tolerance", rclcpp::ParameterValue(0.3));
+
+ layers_ = std::make_unique("map", false, false);
+ layers_->resizeMap(100, 100, 0.05, 0.0, 0.0);
+
+ tf_buffer_ = std::make_shared(node_->get_clock());
+ tf_listener_ = std::make_shared(*tf_buffer_);
+
+ tf_broadcaster_ = std::make_shared(node_);
+ geometry_msgs::msg::TransformStamped t;
+ t.header.stamp = node_->now();
+ t.header.frame_id = "map";
+ t.child_frame_id = kRobotBaseFrame;
+ t.transform.rotation.w = 1.0;
+ tf_broadcaster_->sendTransform(t);
+
+ layer_ = std::make_shared();
+ layer_->initialize(layers_.get(), "bte_layer", tf_buffer_.get(), node_, nullptr);
+ }
+
+ void TearDown() override
+ {
+ layer_->deactivate();
+ layer_.reset();
+ layers_.reset();
+ }
+
+ std::shared_ptr node_;
+ std::unique_ptr layers_;
+ std::shared_ptr tf_buffer_;
+ std::shared_ptr tf_listener_;
+ std::shared_ptr tf_broadcaster_;
+ std::shared_ptr layer_;
+};
+
+TEST(WallPolygonsTest, testClearAndReserve)
+{
+ TestableBoundedTrackingErrorLayer::WallPolygons walls;
+ walls.left_inner.push_back({1.0, 2.0});
+ walls.left_outer.push_back({3.0, 4.0});
+ walls.right_inner.push_back({5.0, 6.0});
+ walls.right_outer.push_back({7.0, 8.0});
+
+ walls.clearAndReserve(10);
+ EXPECT_TRUE(walls.left_inner.empty());
+ EXPECT_TRUE(walls.left_outer.empty());
+ EXPECT_TRUE(walls.right_inner.empty());
+ EXPECT_TRUE(walls.right_outer.empty());
+ EXPECT_GE(walls.left_inner.capacity(), 10u);
+ EXPECT_GE(walls.left_outer.capacity(), 10u);
+ EXPECT_GE(walls.right_inner.capacity(), 10u);
+ EXPECT_GE(walls.right_outer.capacity(), 10u);
+}
+
+TEST_F(BoundedTrackingErrorLayerTest, testDefaultParameterValues)
+{
+ EXPECT_EQ(layer_->getStepSize(), 10u);
+ EXPECT_DOUBLE_EQ(layer_->getLookAhead(), 2.5);
+ EXPECT_DOUBLE_EQ(layer_->getCorridorWidth(), 2.0);
+ EXPECT_EQ(layer_->getWallThickness(), 1);
+ EXPECT_EQ(layer_->getCorridorCost(), 190);
+ EXPECT_TRUE(layer_->enabledRef());
+}
+
+TEST_F(BoundedTrackingErrorLayerTest, testIsClearable)
+{
+ EXPECT_TRUE(layer_->isClearable());
+}
+
+TEST_F(BoundedTrackingErrorLayerTest, testGetPathSegmentEmptyPath)
+{
+ nav_msgs::msg::Path empty_path;
+ nav_msgs::msg::Path segment;
+ layer_->testGetPathSegment(empty_path, 0, segment);
+ EXPECT_TRUE(segment.poses.empty());
+}
+
+TEST_F(BoundedTrackingErrorLayerTest, testGetPathSegmentIndexBeyondPath)
+{
+ auto path = makeStraightPath(0, 0, 1, 0, 5, 0.1);
+ nav_msgs::msg::Path segment;
+ layer_->testGetPathSegment(path, 100, segment);
+ EXPECT_TRUE(segment.poses.empty());
+}
+
+TEST_F(BoundedTrackingErrorLayerTest, testGetPathSegmentSinglePoseAtEnd)
+{
+ auto path = makeStraightPath(0, 0, 1, 0, 5, 0.1);
+ nav_msgs::msg::Path segment;
+ layer_->testGetPathSegment(path, 4, segment);
+ EXPECT_EQ(segment.poses.size(), 1u);
+ EXPECT_DOUBLE_EQ(segment.poses[0].pose.position.x, path.poses[4].pose.position.x);
+}
+
+TEST_F(BoundedTrackingErrorLayerTest, testGetPathSegmentLookAheadExceedsPath)
+{
+ // 25 poses at 0.1 m spacing = 2.4 m total, just under look_ahead=2.5 m.
+ const size_t num_poses = 25;
+ const double spacing = 0.1;
+ auto path = makeStraightPath(0, 0, 1, 0, num_poses, spacing);
+ nav_msgs::msg::Path segment;
+ layer_->testGetPathSegment(path, 0, segment);
+ EXPECT_EQ(segment.poses.size(), num_poses);
+}
+
+TEST_F(BoundedTrackingErrorLayerTest, testGetPathSegmentIncludesBoundaryPose)
+{
+ // loop crosses threshold at step 3 (dist=0.3 >= look_ahead=0.3); the boundary
+ // pose is included so getWallPolygons receives a complete segment.
+ layer_->setLookAhead(0.3);
+ auto path = makeStraightPath(0, 0, 1, 0, 10, 0.1);
+ nav_msgs::msg::Path segment;
+ layer_->testGetPathSegment(path, 0, segment);
+ ASSERT_EQ(segment.poses.size(), 4u);
+ EXPECT_NEAR(segment.poses.front().pose.position.x, 0.0, 1e-6);
+ EXPECT_NEAR(segment.poses.back().pose.position.x, 0.3, 1e-6);
+}
+
+TEST_F(BoundedTrackingErrorLayerTest, testGetPathSegmentRandomPathRespectLookAhead)
+{
+ auto path = makeRandomPath(1.0, 1.0, 60);
+ nav_msgs::msg::Path segment;
+ layer_->testGetPathSegment(path, 0, segment);
+ ASSERT_GT(segment.poses.size(), 0u);
+ EXPECT_DOUBLE_EQ(segment.poses.front().pose.position.x, path.poses[0].pose.position.x);
+ EXPECT_DOUBLE_EQ(segment.poses.front().pose.position.y, path.poses[0].pose.position.y);
+ double dist = 0.0;
+ for (size_t i = 0; i + 1 < segment.poses.size(); ++i) {
+ double dx = segment.poses[i + 1].pose.position.x - segment.poses[i].pose.position.x;
+ double dy = segment.poses[i + 1].pose.position.y - segment.poses[i].pose.position.y;
+ dist += std::hypot(dx, dy);
+ }
+ // Max step in makeRandomPath is base_step + 0.04 = 0.15 + 0.04 = 0.19 m.
+ // The segment may overshoot by at most one step beyond look_ahead.
+ const double max_step = 0.15 + 0.04;
+ EXPECT_LE(dist, layer_->getLookAhead() + max_step);
+}
+
+TEST_F(BoundedTrackingErrorLayerTest, testGetPathSegmentRandomPathStartFromMiddle)
+{
+ auto path = makeRandomPath(0.5, 0.5, 80);
+ nav_msgs::msg::Path segment;
+ const size_t start = 15;
+ layer_->testGetPathSegment(path, start, segment);
+ ASSERT_GT(segment.poses.size(), 0u);
+ EXPECT_DOUBLE_EQ(
+ segment.poses.front().pose.position.x, path.poses[start].pose.position.x);
+ EXPECT_DOUBLE_EQ(
+ segment.poses.front().pose.position.y, path.poses[start].pose.position.y);
+}
+
+TEST_F(BoundedTrackingErrorLayerTest, testGetPathSegmentNonUniformSpacing)
+{
+ // Build a path with alternating large (0.3 m) and small (0.02 m) steps.
+ // The accumulated distance must not overshoot look_ahead by more than
+ // the largest single step in the path.
+ const double large_step = 0.3;
+ const double small_step = 0.02;
+ const double max_single_step = large_step;
+
+ nav_msgs::msg::Path path;
+ path.header.frame_id = "map";
+ double x = 0.0;
+ for (int i = 0; i < 30; ++i) {
+ path.poses.push_back(makePose(x, 0.0));
+ x += (i % 2 == 0) ? large_step : small_step;
+ }
+
+ nav_msgs::msg::Path segment;
+ layer_->testGetPathSegment(path, 0, segment);
+
+ ASSERT_GT(segment.poses.size(), 0u);
+
+ double dist = 0.0;
+ for (size_t i = 0; i + 1 < segment.poses.size(); ++i) {
+ double dx = segment.poses[i + 1].pose.position.x - segment.poses[i].pose.position.x;
+ double dy = segment.poses[i + 1].pose.position.y - segment.poses[i].pose.position.y;
+ dist += std::hypot(dx, dy);
+ }
+ EXPECT_LE(dist, layer_->getLookAhead() + max_single_step);
+}
+
+TEST_F(BoundedTrackingErrorLayerTest, testGetWallPolygonsEmptySegment)
+{
+ nav_msgs::msg::Path empty;
+ TestableBoundedTrackingErrorLayer::WallPolygons walls;
+ layer_->setResolution(0.05);
+ layer_->testGetWallPolygons(empty, walls);
+ EXPECT_TRUE(walls.left_inner.empty());
+}
+
+TEST_F(BoundedTrackingErrorLayerTest, testGetWallPolygonsSinglePoseSegment)
+{
+ layer_->setStepSize(1);
+ layer_->setCorridorWidth(2.0);
+ layer_->setWallThickness(1);
+ layer_->setResolution(0.05);
+ nav_msgs::msg::Path segment;
+ segment.header.frame_id = "map";
+ segment.poses.push_back(makePose(1.0, 1.0));
+ TestableBoundedTrackingErrorLayer::WallPolygons walls;
+ layer_->testGetWallPolygons(segment, walls);
+ EXPECT_TRUE(walls.left_inner.empty());
+}
+
+TEST_F(BoundedTrackingErrorLayerTest, testGetWallPolygonsDegenerateSegmentSkipped)
+{
+ layer_->setStepSize(1);
+ layer_->setCorridorWidth(2.0);
+ layer_->setWallThickness(1);
+ layer_->setResolution(0.05);
+ nav_msgs::msg::Path segment;
+ segment.header.frame_id = "map";
+ segment.poses.push_back(makePose(1.0, 1.0));
+ segment.poses.push_back(makePose(1.0, 1.0));
+ TestableBoundedTrackingErrorLayer::WallPolygons walls;
+ layer_->testGetWallPolygons(segment, walls);
+ EXPECT_TRUE(walls.left_inner.empty());
+}
+
+TEST_F(BoundedTrackingErrorLayerTest, testGetWallPolygonsDiagonalPath)
+{
+ layer_->setStepSize(1);
+ layer_->setCorridorWidth(2.0);
+ layer_->setWallThickness(1);
+ layer_->setResolution(0.05);
+
+ auto segment = makeStraightPath(0, 0, 1, 1, 11, 0.1);
+ TestableBoundedTrackingErrorLayer::WallPolygons walls;
+ layer_->testGetWallPolygons(segment, walls);
+
+ EXPECT_GT(walls.left_inner.size(), 0u);
+
+ const double inner_offset = 1.0;
+ const double inv_sqrt2 = 1.0 / std::sqrt(2.0);
+ EXPECT_NEAR(walls.left_inner[0][0], -inv_sqrt2 * inner_offset, 1e-6);
+ EXPECT_NEAR(walls.left_inner[0][1], inv_sqrt2 * inner_offset, 1e-6);
+ EXPECT_NEAR(walls.right_inner[0][0], inv_sqrt2 * inner_offset, 1e-6);
+ EXPECT_NEAR(walls.right_inner[0][1], -inv_sqrt2 * inner_offset, 1e-6);
+}
+
+TEST_F(BoundedTrackingErrorLayerTest, testGetWallPolygonsStepSizeSkipsPoses)
+{
+ layer_->setStepSize(5);
+ layer_->setCorridorWidth(2.0);
+ layer_->setWallThickness(1);
+ layer_->setResolution(0.05);
+
+ // With num_poses=21 and step=5, current_index advances as 0,5,10,15 and
+ // next_index=20 on the last iteration — producing 4 wall points total.
+ // Expected count is derived directly from the setup variables.
+ const size_t num_poses = 21;
+ const size_t step_size = 5;
+ const size_t expected_points = (num_poses - 1) / step_size;
+ auto segment = makeStraightPath(0, 0, 1, 0, num_poses, 0.1);
+ TestableBoundedTrackingErrorLayer::WallPolygons walls;
+ layer_->testGetWallPolygons(segment, walls);
+ EXPECT_EQ(walls.left_inner.size(), expected_points);
+}
+
+TEST_F(BoundedTrackingErrorLayerTest, testGetWallPolygonsThicknessAddedToOutside)
+{
+ layer_->setStepSize(1);
+ layer_->setCorridorWidth(2.0);
+ layer_->setWallThickness(3);
+ layer_->setResolution(0.05);
+
+ auto segment = makeStraightPath(0.0, 0.0, 1, 0, 11, 0.1);
+ TestableBoundedTrackingErrorLayer::WallPolygons walls;
+ layer_->testGetWallPolygons(segment, walls);
+
+ ASSERT_GT(walls.left_inner.size(), 0u);
+
+ const double inner_offset = 1.0;
+ const double outer_offset = 1.0 + 3 * 0.05;
+
+ for (size_t i = 0; i < walls.left_inner.size(); ++i) {
+ EXPECT_NEAR(walls.left_inner[i][1], inner_offset, 1e-6);
+ EXPECT_NEAR(walls.right_inner[i][1], -inner_offset, 1e-6);
+ EXPECT_NEAR(walls.left_outer[i][1], outer_offset, 1e-6);
+ EXPECT_NEAR(walls.right_outer[i][1], -outer_offset, 1e-6);
+ }
+}
+
+TEST_F(BoundedTrackingErrorLayerTest, testGetWallPolygonsCurvedPathOffsetMagnitudes)
+{
+ layer_->setStepSize(1);
+ layer_->setCorridorWidth(0.6);
+ layer_->setWallThickness(2);
+ layer_->setResolution(0.05);
+
+ auto segment = makeCircularPath(0.0, 0.0, 2.0, 0.0, M_PI / 2.0, 21);
+ TestableBoundedTrackingErrorLayer::WallPolygons walls;
+ layer_->testGetWallPolygons(segment, walls);
+
+ ASSERT_GT(walls.left_inner.size(), 0u);
+ EXPECT_EQ(walls.left_inner.size(), walls.left_outer.size());
+ EXPECT_EQ(walls.left_inner.size(), walls.right_inner.size());
+
+ const double inner_offset = 0.3;
+ const double outer_offset = 0.3 + 2 * 0.05;
+
+ for (size_t i = 0; i < walls.left_inner.size(); ++i) {
+ size_t pose_idx = i * layer_->getStepSize();
+ double px = segment.poses[pose_idx].pose.position.x;
+ double py = segment.poses[pose_idx].pose.position.y;
+
+ double li_dist = std::hypot(walls.left_inner[i][0] - px, walls.left_inner[i][1] - py);
+ double lo_dist = std::hypot(walls.left_outer[i][0] - px, walls.left_outer[i][1] - py);
+ double ri_dist = std::hypot(walls.right_inner[i][0] - px, walls.right_inner[i][1] - py);
+ double ro_dist = std::hypot(walls.right_outer[i][0] - px, walls.right_outer[i][1] - py);
+
+ EXPECT_NEAR(li_dist, inner_offset, 1e-5) << "left inner offset at " << i;
+ EXPECT_NEAR(lo_dist, outer_offset, 1e-5) << "left outer offset at " << i;
+ EXPECT_NEAR(ri_dist, inner_offset, 1e-5) << "right inner offset at " << i;
+ EXPECT_NEAR(ro_dist, outer_offset, 1e-5) << "right outer offset at " << i;
+
+ const double expected_gap = layer_->getWallThickness() * layer_->getResolution();
+ EXPECT_NEAR(
+ lo_dist - li_dist, expected_gap,
+ 1e-5) << "left outer-inner gap must equal wall_thickness * resolution at " << i;
+ EXPECT_NEAR(
+ ro_dist - ri_dist, expected_gap,
+ 1e-5) << "right outer-inner gap must equal wall_thickness * resolution at " << i;
+
+ double lx = walls.left_inner[i][0] - px;
+ double ly = walls.left_inner[i][1] - py;
+ double rx = walls.right_inner[i][0] - px;
+ double ry = walls.right_inner[i][1] - py;
+ EXPECT_LT(lx * rx + ly * ry, 0.0) << "left and right must be on opposite sides";
+ }
+}
+
+TEST_F(BoundedTrackingErrorLayerTest, testGetWallPolygonsNegativeXDirection)
+{
+ // Path travels right-to-left (dx=-1, dy=0). The perpendicular "left" of travel
+ // is now (0,-1), so left_inner must be at -inner_offset in Y and right_inner
+ // at +inner_offset — opposite of the positive-X case.
+ layer_->setStepSize(1);
+ layer_->setCorridorWidth(2.0);
+ layer_->setWallThickness(1);
+ layer_->setResolution(0.05);
+
+ auto segment = makeStraightPath(1.0, 0.0, -1, 0, 11, 0.1);
+ TestableBoundedTrackingErrorLayer::WallPolygons walls;
+ layer_->testGetWallPolygons(segment, walls);
+
+ ASSERT_GT(walls.left_inner.size(), 0u);
+
+ const double inner_offset = 1.0;
+ for (size_t i = 0; i < walls.left_inner.size(); ++i) {
+ EXPECT_NEAR(walls.left_inner[i][1], -inner_offset, 1e-6)
+ << "left wall must be on negative-Y side when travelling in -X at " << i;
+ EXPECT_NEAR(walls.right_inner[i][1], inner_offset, 1e-6)
+ << "right wall must be on positive-Y side when travelling in -X at " << i;
+ }
+}
+
+TEST_F(BoundedTrackingErrorLayerTest, testGetWallPolygonsSharpTurn)
+{
+ // Path makes a near-180 degree turn: goes right then sharply back left.
+ // Verifies no crash and that wall offsets remain correct at every sampled point.
+ layer_->setStepSize(1);
+ layer_->setCorridorWidth(0.6);
+ layer_->setWallThickness(1);
+ layer_->setResolution(0.05);
+
+ nav_msgs::msg::Path segment;
+ segment.header.frame_id = "map";
+ // Rightward leg
+ for (int i = 0; i <= 5; ++i) {
+ segment.poses.push_back(makePose(i * 0.1, 0.0));
+ }
+ // Sharp turn — step sideways then go back left
+ segment.poses.push_back(makePose(0.5, 0.05));
+ for (int i = 5; i >= 0; --i) {
+ segment.poses.push_back(makePose(i * 0.1, 0.1));
+ }
+
+ TestableBoundedTrackingErrorLayer::WallPolygons walls;
+ ASSERT_NO_THROW(layer_->testGetWallPolygons(segment, walls));
+ ASSERT_GT(walls.left_inner.size(), 0u);
+
+ const double inner_offset = 0.3;
+ for (size_t i = 0; i < walls.left_inner.size(); ++i) {
+ size_t pose_idx = i * layer_->getStepSize();
+ double px = segment.poses[pose_idx].pose.position.x;
+ double py = segment.poses[pose_idx].pose.position.y;
+ double li_dist = std::hypot(walls.left_inner[i][0] - px, walls.left_inner[i][1] - py);
+ double ri_dist = std::hypot(walls.right_inner[i][0] - px, walls.right_inner[i][1] - py);
+ EXPECT_NEAR(li_dist, inner_offset, 1e-5) << "left inner offset at " << i;
+ EXPECT_NEAR(ri_dist, inner_offset, 1e-5) << "right inner offset at " << i;
+ double lo_dist = std::hypot(walls.left_outer[i][0] - px, walls.left_outer[i][1] - py);
+ double ro_dist = std::hypot(walls.right_outer[i][0] - px, walls.right_outer[i][1] - py);
+ const double expected_gap = layer_->getWallThickness() * layer_->getResolution();
+ EXPECT_NEAR(lo_dist - li_dist, expected_gap, 1e-5) << "left gap at " << i;
+ EXPECT_NEAR(ro_dist - ri_dist, expected_gap, 1e-5) << "right gap at " << i;
+ }
+}
+
+TEST_F(BoundedTrackingErrorLayerTest, testGetWallPolygonsRandomPathWallsInCostmapBounds)
+{
+ layer_->matchSize();
+ layer_->setStepSize(2);
+ layer_->setCorridorWidth(0.4);
+ layer_->setWallThickness(1);
+ layer_->setResolution(0.05);
+
+ auto segment = makeRandomPath(1.0, 1.0, 30, 0.1);
+ TestableBoundedTrackingErrorLayer::WallPolygons walls;
+ layer_->testGetWallPolygons(segment, walls);
+
+ ASSERT_GT(walls.left_inner.size(), 0u);
+
+ auto * costmap = layers_->getCostmap();
+ for (size_t i = 0; i < walls.left_inner.size(); ++i) {
+ unsigned int mx, my;
+ EXPECT_TRUE(costmap->worldToMap(walls.left_inner[i][0], walls.left_inner[i][1], mx, my))
+ << "left_inner[" << i << "] out of bounds";
+ EXPECT_TRUE(costmap->worldToMap(walls.left_outer[i][0], walls.left_outer[i][1], mx, my))
+ << "left_outer[" << i << "] out of bounds";
+ EXPECT_TRUE(costmap->worldToMap(walls.right_inner[i][0], walls.right_inner[i][1], mx, my))
+ << "right_inner[" << i << "] out of bounds";
+ EXPECT_TRUE(costmap->worldToMap(walls.right_outer[i][0], walls.right_outer[i][1], mx, my))
+ << "right_outer[" << i << "] out of bounds";
+ }
+}
+
+TEST_F(BoundedTrackingErrorLayerTest, testUpdateBoundsExpandsBounds)
+{
+ layer_->matchSize();
+ layer_->enabledRef() = true;
+
+ double min_x = 1e10, min_y = 1e10, max_x = -1e10, max_y = -1e10;
+ double rx = 2.5, ry = 2.5;
+ layer_->updateBounds(rx, ry, 0.0, &min_x, &min_y, &max_x, &max_y);
+
+ double margin = layer_->getLookAhead() +
+ layer_->getCorridorWidth() * 0.5 +
+ layer_->getWallThickness() * layer_->getResolution();
+
+ EXPECT_DOUBLE_EQ(min_x, rx - margin);
+ EXPECT_DOUBLE_EQ(max_x, rx + margin);
+ EXPECT_DOUBLE_EQ(min_y, ry - margin);
+ EXPECT_DOUBLE_EQ(max_y, ry + margin);
+}
+
+TEST_F(BoundedTrackingErrorLayerTest, testUpdateBoundsDisabledDoesNotExpand)
+{
+ layer_->enabledRef() = false;
+
+ double min_x = 0.0, min_y = 0.0, max_x = 1.0, max_y = 1.0;
+ layer_->updateBounds(0.5, 0.5, 0.0, &min_x, &min_y, &max_x, &max_y);
+
+ EXPECT_DOUBLE_EQ(min_x, 0.0);
+ EXPECT_DOUBLE_EQ(max_x, 1.0);
+ EXPECT_DOUBLE_EQ(min_y, 0.0);
+ EXPECT_DOUBLE_EQ(max_y, 1.0);
+}
+
+TEST_F(BoundedTrackingErrorLayerTest, testMatchSizeCachesResolutionAndFrame)
+{
+ layer_->matchSize();
+ EXPECT_DOUBLE_EQ(layer_->getResolution(), 0.05);
+ EXPECT_EQ(layer_->getCostmapFrame(), "map");
+}
+
+TEST_F(BoundedTrackingErrorLayerTest, testPathCallbackNewStampResetsPathIndex)
+{
+ // Store a non-zero path index, then deliver a path with a different stamp.
+ // The callback must reset current_path_index_ to 0 because it's a new path.
+ layer_->pathIndexRef().store(42);
+
+ auto msg = std::make_shared();
+ msg->header.frame_id = "map";
+ msg->header.stamp = node_->now();
+ msg->poses.push_back(makePose(0.0, 0.0));
+ layer_->testPathCallback(msg);
+
+ EXPECT_EQ(layer_->pathIndexRef().load(), 0u)
+ << "New stamp must reset path index to 0";
+}
+
+TEST_F(BoundedTrackingErrorLayerTest, testPathCallbackSameStampPreservesPathIndex)
+{
+ // Deliver a first path to initialise last_path_ptr_, then advance the index.
+ // A second callback with the identical stamp must NOT reset the index —
+ // it is a duplicate delivery of the same plan, not a new one.
+ auto stamp = node_->now();
+
+ auto msg1 = std::make_shared();
+ msg1->header.frame_id = "map";
+ msg1->header.stamp = stamp;
+ msg1->poses.push_back(makePose(0.0, 0.0));
+ layer_->testPathCallback(msg1);
+
+ layer_->pathIndexRef().store(15);
+
+ auto msg2 = std::make_shared();
+ msg2->header.frame_id = "map";
+ msg2->header.stamp = stamp; // identical stamp
+ msg2->poses.push_back(makePose(0.0, 0.0));
+ layer_->testPathCallback(msg2);
+
+ EXPECT_EQ(layer_->pathIndexRef().load(), 15u)
+ << "Same stamp must preserve existing path index";
+}
+
+TEST_F(BoundedTrackingErrorLayerTest, testResetClearsState)
+{
+ auto msg = std::make_shared();
+ msg->header.frame_id = "map";
+ msg->poses.push_back(makePose(0.0, 0.0));
+ layer_->testPathCallback(msg);
+ layer_->pathIndexRef().store(42);
+
+ layer_->reset();
+
+ EXPECT_EQ(layer_->pathIndexRef().load(), 0u);
+ EXPECT_FALSE(layer_->currentRef());
+}
+
+TEST_F(BoundedTrackingErrorLayerTest, testResetStateDoesNotTouchCurrent)
+{
+ // resetState() clears path data and path index but must NOT touch current_.
+ // reset() is the only caller that should modify current_.
+ // This pins the contract so a future merge of the two functions is caught.
+ layer_->currentRef() = true;
+ layer_->pathIndexRef().store(42);
+
+ layer_->testResetState();
+
+ EXPECT_EQ(layer_->pathIndexRef().load(), 0u);
+ EXPECT_TRUE(layer_->currentRef()) << "resetState() must not modify current_";
+}
+
+TEST_F(BoundedTrackingErrorLayerTest, testDeactivateAndReactivate)
+{
+ layer_->activate();
+ layer_->deactivate();
+ EXPECT_NO_THROW(layer_->activate());
+}
+
+TEST_F(BoundedTrackingErrorLayerTest, testValidateParamsRejectNegativeLookAhead)
+{
+ layer_->activate();
+ auto result_negative = layer_->testValidateParams(
+ {rclcpp::Parameter("bte_layer.look_ahead", -1.0)});
+ EXPECT_FALSE(result_negative.successful);
+ EXPECT_EQ(result_negative.reason, "look_ahead must be positive");
+
+ auto result_zero = layer_->testValidateParams(
+ {rclcpp::Parameter("bte_layer.look_ahead", 0.0)});
+ EXPECT_FALSE(result_zero.successful);
+ EXPECT_EQ(result_zero.reason, "look_ahead must be positive");
+}
+
+TEST_F(BoundedTrackingErrorLayerTest, testValidateParamsRejectNegativeCorridorWidth)
+{
+ layer_->activate();
+ auto result_negative = layer_->testValidateParams(
+ {rclcpp::Parameter("bte_layer.corridor_width", -0.5)});
+ EXPECT_FALSE(result_negative.successful);
+ EXPECT_EQ(result_negative.reason, "corridor_width must be positive");
+
+ auto result_zero = layer_->testValidateParams(
+ {rclcpp::Parameter("bte_layer.corridor_width", 0.0)});
+ EXPECT_FALSE(result_zero.successful);
+ EXPECT_EQ(result_zero.reason, "corridor_width must be positive");
+}
+
+TEST_F(BoundedTrackingErrorLayerTest, testValidateParamsRejectZeroStep)
+{
+ layer_->activate();
+ std::vector params = {
+ rclcpp::Parameter("bte_layer.step", 0),
+ };
+ auto result = layer_->testValidateParams(params);
+ EXPECT_FALSE(result.successful);
+ EXPECT_EQ(result.reason, "step must be greater than zero");
+}
+
+TEST_F(BoundedTrackingErrorLayerTest, testValidateParamsRejectCorridorCostZero)
+{
+ layer_->activate();
+ std::vector params = {
+ rclcpp::Parameter("bte_layer.corridor_cost", 0),
+ };
+ auto result = layer_->testValidateParams(params);
+ EXPECT_FALSE(result.successful);
+ EXPECT_EQ(result.reason, "corridor_cost must be between 1 and 254");
+}
+
+TEST_F(BoundedTrackingErrorLayerTest, testValidateParamsRejectCorridorCostAbove254)
+{
+ layer_->activate();
+ std::vector params = {
+ rclcpp::Parameter("bte_layer.corridor_cost", 255),
+ };
+ auto result = layer_->testValidateParams(params);
+ EXPECT_FALSE(result.successful);
+ EXPECT_EQ(result.reason, "corridor_cost must be between 1 and 254");
+}
+
+TEST_F(BoundedTrackingErrorLayerTest, testValidateParamsCorridorCostBoundariesAccepted)
+{
+ layer_->activate();
+ auto result1 = layer_->testValidateParams(
+ {rclcpp::Parameter("bte_layer.corridor_cost", 1)});
+ EXPECT_TRUE(result1.successful);
+ auto result254 = layer_->testValidateParams(
+ {rclcpp::Parameter("bte_layer.corridor_cost", 254)});
+ EXPECT_TRUE(result254.successful);
+}
+
+TEST_F(BoundedTrackingErrorLayerTest, testValidateParamsRejectZeroWallThickness)
+{
+ layer_->activate();
+ std::vector params = {
+ rclcpp::Parameter("bte_layer.wall_thickness", 0),
+ };
+ auto result = layer_->testValidateParams(params);
+ EXPECT_FALSE(result.successful);
+ EXPECT_EQ(result.reason, "wall_thickness must be greater than zero");
+}
+
+TEST_F(BoundedTrackingErrorLayerTest, testValidateParamsIgnoreUnrelatedParameter)
+{
+ layer_->activate();
+ std::vector params = {
+ rclcpp::Parameter("some_other_layer.look_ahead", -99.0),
+ };
+ auto result = layer_->testValidateParams(params);
+ EXPECT_TRUE(result.successful);
+}
+
+TEST_F(BoundedTrackingErrorLayerTest, testValidateParamsRejectInvalidCostWriteMode)
+{
+ layer_->activate();
+ auto result = layer_->testValidateParams(
+ {rclcpp::Parameter("bte_layer.cost_write_mode", 3)});
+ EXPECT_FALSE(result.successful);
+ EXPECT_EQ(result.reason,
+ "cost_write_mode must be 0 (corridor), 1 (fill max), or 2 (fill overwrite)");
+
+ auto result_neg = layer_->testValidateParams(
+ {rclcpp::Parameter("bte_layer.cost_write_mode", -1)});
+ EXPECT_FALSE(result_neg.successful);
+}
+
+TEST_F(BoundedTrackingErrorLayerTest, testValidateParamsCostWriteModeValidValuesAccepted)
+{
+ layer_->activate();
+ for (int v : {0, 1, 2}) {
+ auto result = layer_->testValidateParams(
+ {rclcpp::Parameter("bte_layer.cost_write_mode", v)});
+ EXPECT_TRUE(result.successful) << "Mode " << v << " must be accepted";
+ }
+}
+
+TEST_F(BoundedTrackingErrorLayerTest, testUpdateParamsLookAhead)
+{
+ layer_->activate();
+ layer_->currentRef() = true;
+ layer_->testUpdateParams({rclcpp::Parameter("bte_layer.look_ahead", 5.0)});
+ EXPECT_DOUBLE_EQ(layer_->getLookAhead(), 5.0);
+ EXPECT_FALSE(layer_->currentRef());
+}
+
+TEST_F(BoundedTrackingErrorLayerTest, testUpdateParamsCorridorWidth)
+{
+ layer_->activate();
+ layer_->currentRef() = true;
+ layer_->testUpdateParams({rclcpp::Parameter("bte_layer.corridor_width", 3.0)});
+ EXPECT_DOUBLE_EQ(layer_->getCorridorWidth(), 3.0);
+ EXPECT_FALSE(layer_->currentRef());
+}
+
+TEST_F(BoundedTrackingErrorLayerTest, testUpdateParamsEnabled)
+{
+ layer_->activate();
+ layer_->currentRef() = true;
+ layer_->enabledRef() = true;
+ layer_->testUpdateParams({rclcpp::Parameter("bte_layer.enabled", false)});
+ EXPECT_FALSE(layer_->enabledRef());
+ EXPECT_FALSE(layer_->currentRef());
+}
+
+TEST_F(BoundedTrackingErrorLayerTest, testUpdateParamsStep)
+{
+ layer_->activate();
+ layer_->currentRef() = true;
+ layer_->testUpdateParams({rclcpp::Parameter("bte_layer.step", 20)});
+ EXPECT_EQ(layer_->getStepSize(), 20u);
+ EXPECT_FALSE(layer_->currentRef());
+}
+
+TEST_F(BoundedTrackingErrorLayerTest, testUpdateParamsCorridorCost)
+{
+ layer_->activate();
+ layer_->currentRef() = true;
+ layer_->testUpdateParams({rclcpp::Parameter("bte_layer.corridor_cost", 250)});
+ EXPECT_EQ(layer_->getCorridorCost(), 250);
+ EXPECT_FALSE(layer_->currentRef());
+}
+
+TEST_F(BoundedTrackingErrorLayerTest, testUpdateParamsWallThickness)
+{
+ layer_->activate();
+ layer_->currentRef() = true;
+ layer_->testUpdateParams({rclcpp::Parameter("bte_layer.wall_thickness", 3)});
+ EXPECT_EQ(layer_->getWallThickness(), 3);
+ EXPECT_FALSE(layer_->currentRef());
+}
+
+TEST_F(BoundedTrackingErrorLayerTest, testUpdateParamsSameValueNoCurrentReset)
+{
+ layer_->activate();
+ layer_->currentRef() = true;
+ layer_->testUpdateParams({rclcpp::Parameter("bte_layer.look_ahead", layer_->getLookAhead())});
+ EXPECT_TRUE(layer_->currentRef());
+}
+
+TEST_F(BoundedTrackingErrorLayerTest, testUpdateParamsIgnoreUnrelated)
+{
+ layer_->activate();
+ layer_->currentRef() = true;
+ layer_->testUpdateParams({rclcpp::Parameter("other_layer.look_ahead", 99.0)});
+ EXPECT_TRUE(layer_->currentRef());
+ EXPECT_DOUBLE_EQ(layer_->getLookAhead(), 2.5);
+}
+
+TEST_F(BoundedTrackingErrorLayerTest, testDrawCorridorWallsHorizontalPathCellsMarked)
+{
+ layer_->matchSize();
+ layer_->setStepSize(1);
+ layer_->setCorridorWidth(0.5);
+ layer_->setWallThickness(2);
+ layer_->setCorridorCost(190);
+ layer_->setResolution(0.05);
+
+ auto * costmap = layers_->getCostmap();
+ costmap->resetMapToValue(
+ 0, 0, costmap->getSizeInCellsX(), costmap->getSizeInCellsY(),
+ nav2_costmap_2d::FREE_SPACE);
+
+ auto segment = makeStraightPath(1.0, 2.5, 1, 0, 21, 0.1);
+ TestableBoundedTrackingErrorLayer::WallPolygons walls;
+ layer_->testGetWallPolygons(segment, walls);
+ layer_->testDrawCorridorWalls(*costmap, walls.left_inner, walls.left_outer);
+ layer_->testDrawCorridorWalls(*costmap, walls.right_inner, walls.right_outer);
+
+ unsigned int mx, my;
+ // Derived from test parameters: corridor_width=0.5, wall_thickness=2, resolution=0.05.
+ const double path_x = 2.0; // sample x along the path
+ const double path_y = 2.5;
+ const double inner_off = 0.5 * 0.5; // corridor_width / 2 = 0.25
+ const double outer_off = inner_off + 2 * 0.05; // + wall_thickness * resolution = 0.35
+ const double wall_mid = (inner_off + outer_off) * 0.5; // midpoint of wall band = 0.30
+
+ ASSERT_TRUE(costmap->worldToMap(path_x, path_y + wall_mid, mx, my));
+ EXPECT_EQ(costmap->getCost(mx, my), 190) << "Left wall cell must be marked";
+
+ ASSERT_TRUE(costmap->worldToMap(path_x, path_y - wall_mid, mx, my));
+ EXPECT_EQ(costmap->getCost(mx, my), 190) << "Right wall cell must be marked";
+
+ ASSERT_TRUE(costmap->worldToMap(path_x, path_y, mx, my));
+ EXPECT_EQ(costmap->getCost(mx, my), nav2_costmap_2d::FREE_SPACE)
+ << "Centre of corridor must remain free";
+}
+
+TEST_F(BoundedTrackingErrorLayerTest, testDrawCorridorWallsVerticalPathCellsMarked)
+{
+ // Path moves in Y — wall quads are nearly horizontal, meaning all four quad
+ // vertices share very few Y rows. This exercises the span-buffer edge case
+ // where a thin horizontal quad could collapse and mark nothing.
+ layer_->matchSize();
+ layer_->setStepSize(1);
+ layer_->setCorridorWidth(0.5);
+ layer_->setWallThickness(2);
+ layer_->setCorridorCost(190);
+ layer_->setResolution(0.05);
+
+ auto * costmap = layers_->getCostmap();
+ costmap->resetMapToValue(
+ 0, 0, costmap->getSizeInCellsX(), costmap->getSizeInCellsY(),
+ nav2_costmap_2d::FREE_SPACE);
+
+ // Path along Y axis at x=2.5; left wall at x=2.25, right wall at x=2.75
+ auto segment = makeStraightPath(2.5, 1.0, 0, 1, 21, 0.1);
+ TestableBoundedTrackingErrorLayer::WallPolygons walls;
+ layer_->testGetWallPolygons(segment, walls);
+ layer_->testDrawCorridorWalls(*costmap, walls.left_inner, walls.left_outer);
+ layer_->testDrawCorridorWalls(*costmap, walls.right_inner, walls.right_outer);
+
+ unsigned int mx, my;
+ // Derived from test parameters: corridor_width=0.5, wall_thickness=2, resolution=0.05.
+ const double path_x = 2.5;
+ const double sample_y = 1.5;
+ const double inner_off = 0.5 * 0.5;
+ const double outer_off = inner_off + 2 * 0.05;
+ const double wall_mid = (inner_off + outer_off) * 0.5;
+
+ ASSERT_TRUE(costmap->worldToMap(path_x - wall_mid, sample_y, mx, my));
+ EXPECT_EQ(costmap->getCost(mx, my), 190) << "Left wall of vertical path must be marked";
+
+ ASSERT_TRUE(costmap->worldToMap(path_x + wall_mid, sample_y, mx, my));
+ EXPECT_EQ(costmap->getCost(mx, my), 190) << "Right wall of vertical path must be marked";
+
+ ASSERT_TRUE(costmap->worldToMap(path_x, sample_y, mx, my));
+ EXPECT_EQ(costmap->getCost(mx, my), nav2_costmap_2d::FREE_SPACE)
+ << "Centre of vertical corridor must remain free";
+}
+
+TEST_F(BoundedTrackingErrorLayerTest, testDrawCorridorWallsPreservesHigherCost)
+{
+ layer_->matchSize();
+ layer_->setStepSize(1);
+ layer_->setCorridorWidth(0.5);
+ layer_->setWallThickness(2);
+ layer_->setCorridorCost(190);
+ layer_->setResolution(0.05);
+
+ auto * costmap = layers_->getCostmap();
+ costmap->resetMapToValue(
+ 0, 0, costmap->getSizeInCellsX(), costmap->getSizeInCellsY(),
+ nav2_costmap_2d::FREE_SPACE);
+
+ unsigned int mx, my;
+ ASSERT_TRUE(costmap->worldToMap(2.0, 2.80, mx, my));
+ costmap->setCost(mx, my, nav2_costmap_2d::LETHAL_OBSTACLE);
+
+ auto segment = makeStraightPath(1.0, 2.5, 1, 0, 21, 0.1);
+ TestableBoundedTrackingErrorLayer::WallPolygons walls;
+ layer_->testGetWallPolygons(segment, walls);
+ layer_->testDrawCorridorWalls(*costmap, walls.left_inner, walls.left_outer);
+ layer_->testDrawCorridorWalls(*costmap, walls.right_inner, walls.right_outer);
+
+ EXPECT_EQ(costmap->getCost(mx, my), nav2_costmap_2d::LETHAL_OBSTACLE)
+ << "LETHAL_OBSTACLE must not be overwritten by corridor_cost";
+}
+
+TEST_F(BoundedTrackingErrorLayerTest, testDrawCorridorWallsOverwritesLowerCost)
+{
+ layer_->matchSize();
+ layer_->setStepSize(1);
+ layer_->setCorridorWidth(0.5);
+ layer_->setWallThickness(2);
+ layer_->setCorridorCost(190);
+ layer_->setResolution(0.05);
+
+ auto * costmap = layers_->getCostmap();
+ costmap->resetMapToValue(
+ 0, 0, costmap->getSizeInCellsX(), costmap->getSizeInCellsY(),
+ nav2_costmap_2d::FREE_SPACE);
+
+ unsigned int mx, my;
+ ASSERT_TRUE(costmap->worldToMap(2.0, 2.80, mx, my));
+ costmap->setCost(mx, my, 50);
+
+ auto segment = makeStraightPath(1.0, 2.5, 1, 0, 21, 0.1);
+ TestableBoundedTrackingErrorLayer::WallPolygons walls;
+ layer_->testGetWallPolygons(segment, walls);
+ layer_->testDrawCorridorWalls(*costmap, walls.left_inner, walls.left_outer);
+
+ EXPECT_EQ(costmap->getCost(mx, my), 190) << "Lower cost must be overwritten";
+}
+
+TEST_F(BoundedTrackingErrorLayerTest, testDrawCorridorWallsTooFewPointsNoOp)
+{
+ layer_->matchSize();
+ layer_->setCorridorCost(190);
+
+ auto * costmap = layers_->getCostmap();
+ costmap->resetMapToValue(
+ 0, 0, costmap->getSizeInCellsX(), costmap->getSizeInCellsY(),
+ nav2_costmap_2d::FREE_SPACE);
+
+ std::vector> inner = {{1.0, 1.0}};
+ std::vector> outer = {{1.0, 1.1}};
+ layer_->testDrawCorridorWalls(*costmap, inner, outer);
+
+ bool found_marked = false;
+ for (unsigned int y = 0; y < costmap->getSizeInCellsY() && !found_marked; ++y) {
+ for (unsigned int x = 0; x < costmap->getSizeInCellsX(); ++x) {
+ if (costmap->getCost(x, y) != nav2_costmap_2d::FREE_SPACE) {
+ found_marked = true; break;
+ }
+ }
+ }
+ EXPECT_FALSE(found_marked) << "Single-point walls should not mark any cells";
+}
+
+TEST_F(BoundedTrackingErrorLayerTest, testDrawCorridorWallsOutOfBoundsQuadSkipped)
+{
+ layer_->matchSize();
+ layer_->setCorridorCost(190);
+
+ auto * costmap = layers_->getCostmap();
+ costmap->resetMapToValue(
+ 0, 0, costmap->getSizeInCellsX(), costmap->getSizeInCellsY(),
+ nav2_costmap_2d::FREE_SPACE);
+
+ std::vector> inner = {{10.0, 10.0}, {11.0, 10.0}};
+ std::vector> outer = {{10.0, 10.5}, {11.0, 10.5}};
+ layer_->testDrawCorridorWalls(*costmap, inner, outer);
+
+ bool found_marked = false;
+ for (unsigned int y = 0; y < costmap->getSizeInCellsY() && !found_marked; ++y) {
+ for (unsigned int x = 0; x < costmap->getSizeInCellsX(); ++x) {
+ if (costmap->getCost(x, y) != nav2_costmap_2d::FREE_SPACE) {
+ found_marked = true; break;
+ }
+ }
+ }
+ EXPECT_FALSE(found_marked) << "Out-of-bounds quads should not mark any cells";
+}
+
+TEST_F(BoundedTrackingErrorLayerTest, testDrawCorridorWallsQuadGrazingBottomEdgeCellsMarked)
+{
+ layer_->matchSize();
+ layer_->setCorridorCost(200);
+ layer_->setResolution(0.05);
+
+ auto * costmap = layers_->getCostmap();
+ costmap->resetMapToValue(
+ 0, 0, costmap->getSizeInCellsX(), costmap->getSizeInCellsY(),
+ nav2_costmap_2d::FREE_SPACE);
+
+ std::vector> inner = {{1.0, 0.05}, {2.0, 0.05}};
+ std::vector> outer = {{1.0, 0.20}, {2.0, 0.20}};
+ layer_->testDrawCorridorWalls(*costmap, inner, outer);
+
+ unsigned int mx, my;
+ ASSERT_TRUE(costmap->worldToMap(1.5, 0.10, mx, my));
+ EXPECT_EQ(costmap->getCost(mx, my), 200) << "Cell near bottom edge must be marked";
+ ASSERT_TRUE(costmap->worldToMap(1.5, 0.15, mx, my));
+ EXPECT_EQ(costmap->getCost(mx, my), 200) << "Cell near bottom edge must be marked";
+}
+
+TEST_F(BoundedTrackingErrorLayerTest, testUpdateCostsDisabledNoCostmapChanges)
+{
+ auto * costmap = layers_->getCostmap();
+ prepareForUpdateCosts(layer_.get(), costmap);
+ layer_->enabledRef() = false; // override: this test verifies the disabled-layer early return
+
+ auto msg = std::make_shared();
+ msg->header.frame_id = "map";
+ msg->header.stamp = node_->now();
+ for (int i = 0; i < 30; ++i) {
+ msg->poses.push_back(makePose(i * 0.1, 0.0));
+ }
+ layer_->testPathCallback(msg);
+
+ layer_->updateCosts(*costmap, 0, 0, 100, 100);
+
+ unsigned int mx, my;
+ ASSERT_TRUE(costmap->worldToMap(1.5, 1.5, mx, my));
+ EXPECT_EQ(costmap->getCost(mx, my), nav2_costmap_2d::FREE_SPACE)
+ << "Disabled layer must not modify costmap";
+}
+
+TEST_F(BoundedTrackingErrorLayerTest, testUpdateCostsNoPathNoCostmapChanges)
+{
+ auto * costmap = layers_->getCostmap();
+ prepareForUpdateCosts(layer_.get(), costmap);
+
+ layer_->updateCosts(*costmap, 0, 0, 100, 100);
+
+ unsigned int mx, my;
+ ASSERT_TRUE(costmap->worldToMap(2.5, 2.5, mx, my));
+ EXPECT_EQ(costmap->getCost(mx, my), nav2_costmap_2d::FREE_SPACE)
+ << "No-path layer must not modify costmap";
+}
+
+TEST_F(BoundedTrackingErrorLayerTest, testUpdateCostsStalePathNoCostmapChanges)
+{
+ auto * costmap = layers_->getCostmap();
+ prepareForUpdateCosts(layer_.get(), costmap);
+
+ auto msg = std::make_shared();
+ msg->header.frame_id = "map";
+ // Use 5.1 s — just over the 5.0 s staleness threshold — to test the boundary
+ // precisely rather than being trivially far from it.
+ msg->header.stamp = node_->now() - rclcpp::Duration::from_seconds(5.1);
+ for (int i = 0; i < 30; ++i) {
+ msg->poses.push_back(makePose(i * 0.1, 0.0));
+ }
+ layer_->testPathCallback(msg);
+ layer_->pathIndexRef().store(7);
+
+ layer_->updateCosts(*costmap, 0, 0, 100, 100);
+
+ // Costmap must be untouched.
+ unsigned int mx, my;
+ ASSERT_TRUE(costmap->worldToMap(1.5, 0.0, mx, my));
+ EXPECT_EQ(costmap->getCost(mx, my), nav2_costmap_2d::FREE_SPACE)
+ << "Stale-path layer must not modify costmap";
+
+ // resetState() must have been called: path index zeroed and path cleared so
+ // a second updateCosts also returns early without touching the costmap.
+ EXPECT_EQ(layer_->pathIndexRef().load(), 0u)
+ << "Stale path must reset path index to zero";
+
+ layer_->updateCosts(*costmap, 0, 0, 100, 100);
+ ASSERT_TRUE(costmap->worldToMap(1.5, 0.0, mx, my));
+ EXPECT_EQ(costmap->getCost(mx, my), nav2_costmap_2d::FREE_SPACE)
+ << "After stale reset, second updateCosts must also be a no-op";
+}
+
+TEST_F(BoundedTrackingErrorLayerTest, testSaveCorridorInteriorMarksInteriorCells)
+{
+ layer_->matchSize();
+ layer_->setStepSize(1);
+ layer_->setCorridorWidth(0.6);
+ layer_->setWallThickness(1);
+ layer_->setResolution(0.05);
+
+ auto * costmap = layers_->getCostmap();
+ auto segment = makeStraightPath(1.0, 2.5, 1, 0, 11, 0.1);
+ TestableBoundedTrackingErrorLayer::WallPolygons walls;
+ layer_->testGetWallPolygons(segment, walls);
+
+ layer_->resetCorridorInteriorMask();
+ layer_->testSaveCorridorInterior(*costmap, walls);
+
+ unsigned int cx, cy;
+ ASSERT_TRUE(costmap->worldToMap(1.5, 2.5, cx, cy));
+ const unsigned int flat = cy * costmap->getSizeInCellsX() + cx;
+ EXPECT_TRUE(layer_->isInterior(flat))
+ << "Centre cell must be recorded as interior";
+
+ unsigned int ox, oy;
+ ASSERT_TRUE(costmap->worldToMap(1.5, 4.0, ox, oy));
+ const unsigned int flat_out = oy * costmap->getSizeInCellsX() + ox;
+ EXPECT_FALSE(layer_->isInterior(flat_out))
+ << "Cell far outside corridor must not be interior";
+}
+
+TEST_F(BoundedTrackingErrorLayerTest, testSaveCorridorInteriorAccumulateAddsToExisting)
+{
+ layer_->matchSize();
+ layer_->setStepSize(1);
+ layer_->setCorridorWidth(0.6);
+ layer_->setWallThickness(1);
+ layer_->setResolution(0.05);
+
+ auto * costmap = layers_->getCostmap();
+
+ auto seg1 = makeStraightPath(1.0, 2.5, 1, 0, 11, 0.1);
+ TestableBoundedTrackingErrorLayer::WallPolygons walls1;
+ layer_->testGetWallPolygons(seg1, walls1);
+ layer_->resetCorridorInteriorMask();
+ layer_->testSaveCorridorInterior(*costmap, walls1);
+ unsigned int cx1, cy1;
+ ASSERT_TRUE(costmap->worldToMap(1.5, 2.5, cx1, cy1));
+ const unsigned int flat1 = cy1 * costmap->getSizeInCellsX() + cx1;
+ ASSERT_TRUE(layer_->isInterior(flat1));
+
+ // Second segment is disjoint (y=3.5); both must be interior after accumulate=true.
+ auto seg2 = makeStraightPath(1.0, 3.5, 1, 0, 11, 0.1);
+ TestableBoundedTrackingErrorLayer::WallPolygons walls2;
+ layer_->testGetWallPolygons(seg2, walls2);
+ layer_->testSaveCorridorInterior(*costmap, walls2);
+
+ unsigned int cx2, cy2;
+ ASSERT_TRUE(costmap->worldToMap(1.5, 3.5, cx2, cy2));
+ const unsigned int flat2 = cy2 * costmap->getSizeInCellsX() + cx2;
+ EXPECT_TRUE(layer_->isInterior(flat2))
+ << "Accumulate=true must add new interior cells to the existing set";
+ EXPECT_TRUE(layer_->isInterior(flat1))
+ << "Accumulate=true must preserve previously recorded interior cells";
+}
+
+TEST_F(BoundedTrackingErrorLayerTest, testMarkCircleAsInteriorCellsWithinRadiusAdded)
+{
+ layer_->matchSize();
+
+ auto * costmap = layers_->getCostmap();
+
+ const int cx = 50, cy = 50;
+ const int r = 3;
+ layer_->testMarkCircleAsInterior(*costmap, cx, cy, r * r);
+
+ for (int dy = -r; dy <= r; ++dy) {
+ for (int dx = -r; dx <= r; ++dx) {
+ if (dx * dx + dy * dy <= r * r) {
+ unsigned int flat = static_cast(cy + dy) *
+ costmap->getSizeInCellsX() + static_cast(cx + dx);
+ EXPECT_TRUE(layer_->isInterior(flat))
+ << "Cell (" << cx + dx << "," << cy + dy << ") must be interior";
+ }
+ }
+ }
+
+ unsigned int flat_out = static_cast(cy + r + 1) *
+ costmap->getSizeInCellsX() + static_cast(cx);
+ EXPECT_FALSE(layer_->isInterior(flat_out))
+ << "Cell just outside radius must not be interior";
+}
+
+TEST_F(BoundedTrackingErrorLayerTest, testFillOutsideCorridorMarksNonInteriorCells)
+{
+ layer_->matchSize();
+ layer_->setCorridorCost(190);
+
+ auto * costmap = layers_->getCostmap();
+ costmap->resetMapToValue(
+ 0, 0, costmap->getSizeInCellsX(), costmap->getSizeInCellsY(),
+ nav2_costmap_2d::FREE_SPACE);
+
+ // Trivial case: r_sq=0 marks only the centre cell interior.
+ layer_->testMarkCircleAsInterior(*costmap, 50, 50, 0);
+ layer_->testFillOutsideCorridor(*costmap, 48, 48, 52, 52);
+
+ EXPECT_EQ(costmap->getCost(50, 50), nav2_costmap_2d::FREE_SPACE)
+ << "Interior cell must not be marked";
+ EXPECT_EQ(costmap->getCost(48, 48), 190)
+ << "Non-interior cell in bbox must be marked with corridor_cost";
+
+ // Non-trivial case: r_sq=4 (radius 2) — all cells within radius stay free,
+ // first cell outside the circle is filled.
+ costmap->resetMapToValue(
+ 0, 0, costmap->getSizeInCellsX(), costmap->getSizeInCellsY(),
+ nav2_costmap_2d::FREE_SPACE);
+ layer_->resetCorridorInteriorMask();
+ const int cx = 50, cy = 50, r_sq = 4;
+ layer_->testMarkCircleAsInterior(*costmap, cx, cy, r_sq);
+ layer_->testFillOutsideCorridor(*costmap, cx - 3, cy - 3, cx + 3, cy + 3);
+
+ for (int dy = -2; dy <= 2; ++dy) {
+ for (int dx = -2; dx <= 2; ++dx) {
+ if (dx * dx + dy * dy <= r_sq) {
+ EXPECT_EQ(costmap->getCost(cx + dx, cy + dy), nav2_costmap_2d::FREE_SPACE)
+ << "Cell inside radius (" << cx + dx << "," << cy + dy << ") must stay free";
+ }
+ }
+ }
+ // Cell at distance 3 (dx=3,dy=0) is outside r_sq=4, must be filled.
+ EXPECT_EQ(costmap->getCost(cx + 3, cy), 190)
+ << "Cell outside radius must be filled with corridor_cost";
+}
+
+TEST_F(BoundedTrackingErrorLayerTest, testFillOutsideCorridorPreservesHigherCost)
+{
+ layer_->matchSize();
+ layer_->setCorridorCost(190);
+
+ auto * costmap = layers_->getCostmap();
+ costmap->resetMapToValue(
+ 0, 0, costmap->getSizeInCellsX(), costmap->getSizeInCellsY(),
+ nav2_costmap_2d::FREE_SPACE);
+
+ costmap->setCost(48, 48, nav2_costmap_2d::LETHAL_OBSTACLE);
+ layer_->testFillOutsideCorridor(*costmap, 47, 47, 52, 52);
+
+ EXPECT_EQ(costmap->getCost(48, 48), nav2_costmap_2d::LETHAL_OBSTACLE)
+ << "LETHAL_OBSTACLE must not be overwritten by fillOutsideCorridor";
+}
+
+TEST_F(BoundedTrackingErrorLayerTest, testUpdateCostsStaleIndexRecoveredSilently)
+{
+ auto * costmap = layers_->getCostmap();
+ prepareForUpdateCosts(layer_.get(), costmap);
+
+ auto msg = std::make_shared();
+ msg->header.frame_id = "map";
+ msg->header.stamp = node_->now();
+ for (int i = 0; i < 10; ++i) {
+ msg->poses.push_back(makePose(i * 0.1, 0.0));
+ }
+ layer_->testPathCallback(msg);
+ layer_->pathIndexRef().store(999);
+
+ ASSERT_NO_THROW(layer_->updateCosts(*costmap, 0, 0, 100, 100));
+ EXPECT_EQ(layer_->pathIndexRef().load(), 0u)
+ << "Out-of-range path index must be silently reset to 0";
+}
+
+TEST_F(BoundedTrackingErrorLayerTest, testUpdateCostsSegmentTooSmallNoCostmapChanges)
+{
+ auto * costmap = layers_->getCostmap();
+ prepareForUpdateCosts(layer_.get(), costmap);
+ // step_size=5 => min_poses = (5*2)+1 = 11. A 5-pose path is too small.
+ layer_->setStepSize(5);
+
+ auto msg = std::make_shared();
+ msg->header.frame_id = "map";
+ msg->header.stamp = node_->now();
+ for (int i = 0; i < 5; ++i) {
+ msg->poses.push_back(makePose(i * 0.1, 0.0));
+ }
+ layer_->testPathCallback(msg);
+
+ layer_->updateCosts(*costmap, 0, 0, 100, 100);
+
+ // Costmap must be untouched — segment was too small to form a wall
+ bool found_marked = false;
+ for (unsigned int y = 0; y < costmap->getSizeInCellsY() && !found_marked; ++y) {
+ for (unsigned int x = 0; x < costmap->getSizeInCellsX(); ++x) {
+ if (costmap->getCost(x, y) != nav2_costmap_2d::FREE_SPACE) {
+ found_marked = true; break;
+ }
+ }
+ }
+ EXPECT_FALSE(found_marked) << "Too-small segment must not mark any cells";
+}
+
+TEST_F(BoundedTrackingErrorLayerTest, testUpdateCostsFreshPathMarksWalls)
+{
+ // Path at 4.9 s old — just under the 5.0 s staleness threshold.
+ // updateCosts must proceed and mark corridor walls.
+ auto * costmap = layers_->getCostmap();
+ prepareForUpdateCosts(layer_.get(), costmap);
+ layer_->setStepSize(1);
+ layer_->setCorridorWidth(0.5);
+ layer_->setWallThickness(2);
+ layer_->setCorridorCost(190);
+
+ auto msg = std::make_shared();
+ msg->header.frame_id = "map";
+ msg->header.stamp = node_->now() - rclcpp::Duration::from_seconds(4.9);
+ for (int i = 0; i < 30; ++i) {
+ msg->poses.push_back(makePose(i * 0.1, 2.5));
+ }
+ layer_->testPathCallback(msg);
+
+ layer_->updateCosts(*costmap, 0, 0, 100, 100);
+
+ // Left wall should be marked at y = 2.5 + corridor_width/2 = 2.75
+ unsigned int mx, my;
+ ASSERT_TRUE(costmap->worldToMap(1.5, 2.80, mx, my));
+ EXPECT_EQ(costmap->getCost(mx, my), 190)
+ << "Path just under staleness threshold must draw corridor walls";
+}
+
+TEST_F(BoundedTrackingErrorLayerTest, testUpdateCostsFillOutsideCorridorFirstPathPointIsInterior)
+{
+ // markCircleAsInterior must fire at the first in-area path point, not the robot pose.
+ // Place the path well away from the robot (robot is at origin, path starts at x=1.0, y=0.0)
+ // so the robot cell is clearly outside the corridor. The first path point and its
+ // surrounding cells (within corridor_width/2 radius) must be marked as interior.
+ auto * costmap = layers_->getCostmap();
+ prepareForUpdateCosts(layer_.get(), costmap);
+ layer_->setCostWriteMode(1);
+ layer_->setStepSize(1);
+ layer_->setCorridorWidth(1.0);
+ layer_->setWallThickness(1);
+ layer_->setCorridorCost(190);
+
+ auto msg = std::make_shared();
+ msg->header.frame_id = "map";
+ msg->header.stamp = node_->now();
+ // Path runs along y=0 starting at x=1.0, well within the fill bbox around the robot.
+ for (int i = 0; i < 40; ++i) {
+ msg->poses.push_back(makePose(1.0 + i * 0.1, 0.0));
+ }
+ layer_->testPathCallback(msg);
+
+ layer_->updateCosts(*costmap, 0, 0, 100, 100);
+
+ // The first path point (1.0, 0.0) must be interior — markCircleAsInterior fires there.
+ unsigned int fx, fy;
+ ASSERT_TRUE(costmap->worldToMap(1.0, 0.0, fx, fy));
+ const unsigned int flat_fx = fy * costmap->getSizeInCellsX() + fx;
+ EXPECT_TRUE(layer_->isInterior(flat_fx))
+ << "First in-area path point must be marked as interior by markCircleAsInterior";
+
+ // The robot cell (origin) is off-path and must NOT have been given a free circle —
+ // the old robot-pose circle logic is gone.
+ unsigned int rx, ry;
+ ASSERT_TRUE(costmap->worldToMap(0.0, 0.0, rx, ry));
+ const unsigned int flat_rx = ry * costmap->getSizeInCellsX() + rx;
+ EXPECT_FALSE(layer_->isInterior(flat_rx))
+ << "Robot cell must not be marked interior — circle is at path point, not robot pose";
+}
+
+TEST_F(BoundedTrackingErrorLayerTest, testUpdateCostsZeroResolutionEarlyReturn)
+{
+ // prepareForUpdateCosts calls matchSize(); override resolution_ to 0 afterwards.
+ auto * costmap = layers_->getCostmap();
+ prepareForUpdateCosts(layer_.get(), costmap);
+ layer_->setResolution(0.0);
+
+ auto msg = std::make_shared();
+ msg->header.frame_id = "map";
+ msg->header.stamp = node_->now();
+ for (int i = 0; i < 30; ++i) {
+ msg->poses.push_back(makePose(i * 0.1, 2.5));
+ }
+ layer_->testPathCallback(msg);
+
+ ASSERT_NO_THROW(layer_->updateCosts(*costmap, 0, 0, 100, 100));
+
+ // Nothing should be marked — early return before any work is done
+ bool found_marked = false;
+ for (unsigned int y = 0; y < costmap->getSizeInCellsY() && !found_marked; ++y) {
+ for (unsigned int x = 0; x < costmap->getSizeInCellsX(); ++x) {
+ if (costmap->getCost(x, y) != nav2_costmap_2d::FREE_SPACE) {
+ found_marked = true;
+ break;
+ }
+ }
+ }
+ EXPECT_FALSE(found_marked) << "Zero resolution must prevent any costmap writes";
+}
+
+TEST_F(BoundedTrackingErrorLayerTest, testUpdateCostsFillBranchFlushSegmentFires)
+{
+ auto * costmap = layers_->getCostmap();
+ prepareForUpdateCosts(layer_.get(), costmap);
+ layer_->setCostWriteMode(1);
+ layer_->setStepSize(1);
+ layer_->setCorridorWidth(0.5);
+ layer_->setWallThickness(1);
+ layer_->setCorridorCost(190);
+ layer_->setLookAhead(2.5);
+
+ // Path runs through the middle of the costmap (y=2.5) so both corridor sides
+ // (y=2.5±0.25) are well within map bounds. Robot is at origin via TF fixture.
+ auto msg = std::make_shared();
+ msg->header.frame_id = "map";
+ msg->header.stamp = node_->now();
+ for (int i = 0; i < 60; ++i) {
+ msg->poses.push_back(makePose(i * 0.05, 2.5));
+ }
+ layer_->testPathCallback(msg);
+
+ ASSERT_NO_THROW(layer_->updateCosts(*costmap, 0, 0, 100, 100));
+
+ unsigned int mx, my;
+ ASSERT_TRUE(costmap->worldToMap(0.5, 0.5, mx, my));
+ EXPECT_EQ(costmap->getCost(mx, my), 190)
+ << "Cells outside corridor but inside fill bbox must be elevated to corridor_cost";
+
+ ASSERT_TRUE(costmap->worldToMap(0.5, 2.5, mx, my));
+ EXPECT_EQ(costmap->getCost(mx, my), nav2_costmap_2d::FREE_SPACE)
+ << "Cell inside corridor interior must not be penalised by fill";
+}
+
+TEST_F(BoundedTrackingErrorLayerTest, testGetWallPolygonsMixedDegenerateAndValidSteps)
+{
+ layer_->matchSize();
+ layer_->setStepSize(1);
+ layer_->setCorridorWidth(0.5);
+ layer_->setWallThickness(1);
+ // resolution = 0.05 from matchSize; identical poses produce norm=0 < resolution → skipped
+ nav_msgs::msg::Path segment;
+ segment.header.frame_id = "map";
+ // Step 0→1: identical poses → norm = 0, must be skipped
+ segment.poses.push_back(makePose(1.0, 1.0));
+ segment.poses.push_back(makePose(1.0, 1.0));
+ // Step 1→2 (current=1, next clamped to 2): valid, norm > resolution, must be emitted
+ segment.poses.push_back(makePose(1.5, 1.0));
+
+ TestableBoundedTrackingErrorLayer::WallPolygons walls;
+ layer_->testGetWallPolygons(segment, walls);
+
+ // The degenerate first step is skipped; the valid step must still produce a point
+ EXPECT_GE(walls.left_inner.size(), 1u)
+ << "Valid steps must produce wall points even when earlier steps are degenerate";
+ EXPECT_EQ(walls.left_inner.size(), walls.right_inner.size());
+ EXPECT_EQ(walls.left_outer.size(), walls.right_outer.size());
+}
+
+TEST_F(BoundedTrackingErrorLayerTest, testUpdateParamsCostWriteMode)
+{
+ layer_->activate();
+ layer_->currentRef() = true;
+ layer_->testUpdateParams({rclcpp::Parameter("bte_layer.cost_write_mode", 1)});
+ EXPECT_EQ(layer_->getCostWriteMode(), 1);
+ EXPECT_FALSE(layer_->currentRef());
+}
+
+TEST_F(BoundedTrackingErrorLayerTest, testUpdateParamsCostWriteModeSameValueNoReset)
+{
+ layer_->activate();
+ layer_->currentRef() = true;
+ layer_->testUpdateParams({rclcpp::Parameter("bte_layer.cost_write_mode", 0)});
+ EXPECT_TRUE(layer_->currentRef())
+ << "Same cost_write_mode must not reset current_";
+}
+
+TEST_F(BoundedTrackingErrorLayerTest, testUpdateParamsSameValueIntegersNoCurrentReset)
+{
+ layer_->activate();
+
+ // corridor_cost same value
+ layer_->currentRef() = true;
+ layer_->testUpdateParams(
+ {rclcpp::Parameter("bte_layer.corridor_cost",
+ static_cast(layer_->getCorridorCost()))});
+ EXPECT_TRUE(layer_->currentRef())
+ << "Same corridor_cost must not reset current_";
+
+ // wall_thickness same value
+ layer_->currentRef() = true;
+ layer_->testUpdateParams(
+ {rclcpp::Parameter("bte_layer.wall_thickness", layer_->getWallThickness())});
+ EXPECT_TRUE(layer_->currentRef())
+ << "Same wall_thickness must not reset current_";
+
+ // step same value
+ layer_->currentRef() = true;
+ layer_->testUpdateParams(
+ {rclcpp::Parameter("bte_layer.step", static_cast(layer_->getStepSize()))});
+ EXPECT_TRUE(layer_->currentRef())
+ << "Same step must not reset current_";
+}
+
+TEST_F(BoundedTrackingErrorLayerTest, testCostWriteModeOverwriteAllIgnoresHigherCostOutsideCorridor)
+{
+ layer_->matchSize();
+ layer_->setCorridorCost(190);
+ layer_->setCostWriteMode(2);
+
+ auto * costmap = layers_->getCostmap();
+ costmap->resetMapToValue(
+ 0, 0, costmap->getSizeInCellsX(), costmap->getSizeInCellsY(),
+ nav2_costmap_2d::FREE_SPACE);
+
+ costmap->setCost(48, 48, nav2_costmap_2d::LETHAL_OBSTACLE);
+ layer_->testFillOutsideCorridor(*costmap, 47, 47, 52, 52);
+
+ EXPECT_EQ(costmap->getCost(48, 48), 190)
+ << "Mode 2 must overwrite LETHAL_OBSTACLE outside corridor";
+}
+
+// Path starts inside the fill bbox, exits beyond the radius, then re-enters from
+// a different Y. flush_segment must fire at both the exit and re-entry boundaries,
+// producing two separate interior sub-segments. Cells on the spine of both chunks
+// must remain interior; a cell inside the bbox but off both spines must be penalised.
+//
+// Geometry (robot at origin, fill_radius = 1.0 + 0.2 + 0.05 = 1.25 m):
+// chunk A: x = 0.0..1.0, y = 0.3 — inside radius, flushed at x=1.0
+// gap: x = 1.3..2.0, y = 0.3 — outside radius, no interior recorded
+// chunk B: x = 1.0..0.0, y = 0.7 — re-enters radius travelling in -X
+//
+// Both y=0.3 and y=0.7 are well inside the costmap (origin 0,0 size 100x100 res 0.05).
+TEST_F(BoundedTrackingErrorLayerTest, testFillBranchPathExitsAndReentersBbox)
+{
+ auto * costmap = layers_->getCostmap();
+ prepareForUpdateCosts(layer_.get(), costmap);
+ layer_->setCostWriteMode(1);
+ layer_->setStepSize(1);
+ layer_->setCorridorWidth(0.4);
+ layer_->setWallThickness(1);
+ layer_->setCorridorCost(190);
+ layer_->setLookAhead(1.0);
+
+ auto msg = std::make_shared();
+ msg->header.frame_id = "map";
+ msg->header.stamp = node_->now();
+
+ // Chunk A: travels +X at y=0.3, stays inside fill radius (x <= 1.0 < 1.25)
+ for (int i = 0; i <= 10; ++i) {
+ msg->poses.push_back(makePose(i * 0.1, 0.3));
+ }
+ // Gap: x = 1.3..2.0 — outside fill radius, flush_segment fires at boundary
+ for (int i = 13; i <= 20; ++i) {
+ msg->poses.push_back(makePose(i * 0.1, 0.3));
+ }
+ // Chunk B: travels -X at y=0.7, re-enters fill radius (x <= 1.0 < 1.25)
+ for (int i = 10; i >= 0; --i) {
+ msg->poses.push_back(makePose(i * 0.1, 0.7));
+ }
+
+ layer_->testPathCallback(msg);
+ ASSERT_NO_THROW(layer_->updateCosts(*costmap, 0, 0, 100, 100));
+
+ unsigned int mx, my;
+
+ // Mid-spine of chunk A must be interior (not penalised)
+ ASSERT_TRUE(costmap->worldToMap(0.5, 0.3, mx, my));
+ EXPECT_EQ(costmap->getCost(mx, my), nav2_costmap_2d::FREE_SPACE)
+ << "Spine of first in-bbox chunk must be interior";
+
+ // Mid-spine of chunk B must be interior (not penalised)
+ ASSERT_TRUE(costmap->worldToMap(0.5, 0.7, mx, my));
+ EXPECT_EQ(costmap->getCost(mx, my), nav2_costmap_2d::FREE_SPACE)
+ << "Spine of second in-bbox chunk must be interior after re-entry";
+
+ // A cell inside the fill bbox but between and off both spines must be penalised
+ ASSERT_TRUE(costmap->worldToMap(0.5, 1.1, mx, my));
+ EXPECT_EQ(costmap->getCost(mx, my), 190)
+ << "Cell inside fill bbox but off both path spines must be penalised";
+}
+
+int main(int argc, char ** argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}