diff --git a/nav2_costmap_2d/CMakeLists.txt b/nav2_costmap_2d/CMakeLists.txt
index f7282456bea..90e5eb5c08e 100644
--- a/nav2_costmap_2d/CMakeLists.txt
+++ b/nav2_costmap_2d/CMakeLists.txt
@@ -74,6 +74,7 @@ target_link_libraries(nav2_costmap_2d_core PUBLIC
add_library(layers SHARED
plugins/inflation_layer.cpp
plugins/legacy_inflation_layer.cpp
+ plugins/asymmetric_inflation_layer.cpp
plugins/static_layer.cpp
plugins/obstacle_layer.cpp
src/observation_buffer.cpp
diff --git a/nav2_costmap_2d/costmap_plugins.xml b/nav2_costmap_2d/costmap_plugins.xml
index 1ef0408c158..d0e27dc48c5 100644
--- a/nav2_costmap_2d/costmap_plugins.xml
+++ b/nav2_costmap_2d/costmap_plugins.xml
@@ -6,6 +6,9 @@
Legacy inflation layer using traditional CellData-based algorithm (original Navigation2 implementation).
+
+ OpenMP-accelerated inflation layer creating an asymmetric costmap based on the global path.
+
Listens to laser scan and point cloud messages and marks and clears grid cells.
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/asymmetric_inflation_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/asymmetric_inflation_layer.hpp
new file mode 100644
index 00000000000..bfea6657b4f
--- /dev/null
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/asymmetric_inflation_layer.hpp
@@ -0,0 +1,250 @@
+// Copyright (c) 2026, Marc Blöchlinger
+//
+// 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__ASYMMETRIC_INFLATION_LAYER_HPP_
+#define NAV2_COSTMAP_2D__ASYMMETRIC_INFLATION_LAYER_HPP_
+
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+#include "rclcpp/rclcpp.hpp"
+#include "nav2_costmap_2d/inflation_layer.hpp"
+#include "nav_msgs/msg/path.hpp"
+
+namespace nav2_costmap_2d
+{
+
+/**
+ * @class AsymmetricInflationLayer
+ * @brief Costmap layer that inflates obstacles asymmetrically relative to the
+ * global path, biasing the navigable corridor toward one side.
+ *
+ * Inherits the distance-transform based InflationLayer, which writes the
+ * symmetric baseline first. This layer then classifies each lethal obstacle as
+ * left (+1), right (-1), or neutral (0) relative to the path and runs a second
+ * distance transform seeded from only the disfavored-side boundary cells.
+ * Falls back to the inherited symmetric inflation when no path is available,
+ * the goal is nearby, or the per-side scaling factors are equal.
+ */
+class AsymmetricInflationLayer : public nav2_costmap_2d::InflationLayer
+{
+public:
+ AsymmetricInflationLayer();
+ ~AsymmetricInflationLayer() override = default;
+
+ /**
+ * @brief Initialization process of layer on startup
+ */
+ void onInitialize() override;
+
+ /**
+ * @brief Activate the layer; registers the parameter validation/update callbacks.
+ */
+ void activate() override;
+
+ /**
+ * @brief Deactivate the layer; removes the parameter validation/update callbacks.
+ */
+ void deactivate() override;
+
+ /**
+ * @brief Update the bounds of the master costmap by this layer's update dimensions.
+ *
+ * Stores the robot position for goal-proximity checks in updateCosts(), then
+ * delegates the actual inflation bound expansion to InflationLayer.
+ * @param robot_x X pose of robot
+ * @param robot_y Y pose of robot
+ * @param robot_yaw Robot orientation
+ * @param min_x X min map coord of the window to update
+ * @param min_y Y min map coord of the window to update
+ * @param max_x X max map coord of the window to update
+ * @param max_y Y max map coord of the window 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 Update the costs in the master costmap.
+ *
+ * First writes the inherited symmetric inflation baseline. Runs a second
+ * distance-transform pass seeded from disfavored-side boundary cells when a
+ * valid path is available and the two per-side decay rates differ.
+ * Otherwise leaves the symmetric baseline unchanged.
+ * @param master_grid The master costmap grid to update
+ * @param min_i X min cell index of the window to update
+ * @param min_j Y min cell index of the window to update
+ * @param max_i X max cell index of the window to update
+ * @param max_j Y max cell index of the window to update
+ */
+ void updateCosts(
+ nav2_costmap_2d::Costmap2D & master_grid,
+ int min_i, int min_j, int max_i, int max_j) override;
+
+ /**
+ * @brief Match the size of the master costmap
+ */
+ void matchSize() override;
+
+protected:
+ /**
+ * @brief Process updates on footprint changes to the inflation layer
+ */
+ void onFootprintChanged() override;
+
+ /**
+ * @brief Callback for incoming global path messages
+ */
+ void globalPathCallback(const nav_msgs::msg::Path::SharedPtr msg);
+
+ /**
+ * @brief Extract the global path points that fall inside the local costmap window.
+ *
+ * Transforms the path into the costmap frame via TF2. Returns an empty
+ * vector when the robot is within goal_distance_threshold_ of the goal,
+ * which disables asymmetry near the goal to prevent docking oscillations.
+ * @param master_grid Reference costmap used to filter points to the current window.
+ * @return Path points in costmap-frame world coordinates; empty disables asymmetry.
+ */
+ std::vector> extractLocalPath(
+ nav2_costmap_2d::Costmap2D & master_grid);
+
+ /**
+ * @brief Classify an obstacle cell as left (+1), right (-1), or neutral (0)
+ * relative to the closest path segment.
+ *
+ * For each candidate segment, performs an AABB rejection pass followed by an
+ * exact perpendicular-distance and cross-product computation to determine side.
+ * Obstacles whose nearest perpendicular distance exceeds the inflation radius
+ * are returned as 0 (neutral -> symmetric inflation).
+ *
+ * @param cx World x-coordinate of the obstacle cell (metres).
+ * @param cy World y-coordinate of the obstacle cell (metres).
+ * @param candidates Segment indices whose padded AABB may contain (cx, cy).
+ * @param local_path_pts Path points in costmap-frame order; consecutive pairs form segments.
+ * @return +1 (left of path), -1 (right of path), or 0 (neutral / beyond inflation radius).
+ */
+ int8_t computeObstacleSide(
+ double cx, double cy,
+ const std::vector & candidates,
+ const std::vector> & local_path_pts);
+
+ /**
+ * @brief Build a spatial hash mapping 2D bucket keys to path segment indices.
+ *
+ * Uses inflation_radius_ as the bucket size so each bucket spans exactly one
+ * inflation radius. Each segment is inserted into every bucket whose padded
+ * AABB it overlaps, enabling O(1) nearest-segment queries during the disfavored-cell seeding phase.
+ * @param local_path_pts Path waypoints in costmap-frame world coordinates.
+ * @return Hash map from packed (bucket_x, bucket_y) key to segment index list.
+ */
+ std::unordered_map>
+ buildPathSpatialHash(
+ const std::vector> & local_path_pts);
+
+ /**
+ * @brief Build a distance map seeded from disfavored-side obstacle boundary cells.
+ *
+ * Initialises a MatrixXfRM (roi_height × roi_width) to DT_INF, then sets
+ * disfavored boundary cells to 0.0f. The caller passes the result to
+ * DistanceTransform::distanceTransform2D() then applyInflation().
+ * @param master_grid Costmap used for coordinate and cost lookups.
+ * @param roi_min_i Left edge of the padded ROI (cells).
+ * @param roi_min_j Bottom edge of the padded ROI (cells).
+ * @param roi_width Width of the padded ROI (cells).
+ * @param roi_height Height of the padded ROI (cells).
+ * @param spatial_hash Segment lookup structure from buildPathSpatialHash().
+ * @param local_path_pts Path waypoints in costmap-frame world coordinates.
+ * @return Distance map with 0.0f at disfavored seeds and DT_INF elsewhere.
+ */
+ MatrixXfRM seedDistanceMap(
+ nav2_costmap_2d::Costmap2D & master_grid,
+ int roi_min_i, int roi_min_j, int roi_width, int roi_height,
+ const std::unordered_map> & spatial_hash,
+ const std::vector> & local_path_pts);
+
+ /**
+ * @brief Apply disfavored-side costs from a distance map with max(old, new) semantics.
+ *
+ * Mirrors InflationLayer::applyInflation() but uses cost_lut_disfavored_
+ * (built with c_side) instead of the parent's cost_lut_ (built with c_max).
+ * Costs are applied only within the originally requested update window.
+ * @param master_array Raw costmap data pointer.
+ * @param distance_map Result of distanceTransform2D() on the disfavored seed map.
+ * @param min_i Left edge of the write window.
+ * @param min_j Bottom edge of the write window.
+ * @param max_i Right edge of the write window.
+ * @param max_j Top edge of the write window.
+ * @param roi_min_i Left edge of the padded ROI used to build distance_map.
+ * @param roi_min_j Bottom edge of the padded ROI used to build distance_map.
+ * @param size_x Full costmap width (cells), for index arithmetic.
+ */
+ void applyInflation(
+ unsigned char * master_array,
+ const MatrixXfRM & distance_map,
+ int min_i, int min_j, int max_i, int max_j,
+ int roi_min_i, int roi_min_j,
+ unsigned int size_x);
+
+ /**
+ * @brief Pre-compute cost_lut_disfavored_ using c_side (the smaller per-side scaling factor)
+ */
+ void computeAsymmetricCaches();
+
+ /**
+ * @brief Validate parameter updates (pre-set callback). Returns success/failure
+ * without mutating any state.
+ */
+ rcl_interfaces::msg::SetParametersResult validateParameterUpdatesCallback(
+ const std::vector & parameters);
+
+ /**
+ * @brief Apply parameter updates (post-set callback) after they have been validated.
+ * Recomputes caches when geometry parameters change.
+ */
+ void updateParametersCallback(
+ const std::vector & parameters);
+
+ // --- Parameters ---
+ /// Exponential decay rate for cells on the LEFT side of the path
+ double cost_scaling_factor_left_;
+ /// Exponential decay rate for cells on the RIGHT side of the path
+ double cost_scaling_factor_right_;
+ /// Distance to goal where asymmetry disables to prevent docking oscillations
+ double goal_distance_threshold_;
+ std::string plan_topic_;
+
+ // --- State ---
+ double current_robot_x_{0.0};
+ double current_robot_y_{0.0};
+
+ // --- Asymmetric Lookup Table ---
+ /// Cost LUT for the disfavored side, built with c_side (the smaller per-side scaling factor)
+ std::vector cost_lut_disfavored_;
+
+ // --- Path subscription ---
+ nav2::Subscription::SharedPtr path_sub_;
+ nav_msgs::msg::Path::SharedPtr latest_global_path_;
+ std::mutex path_mutex_;
+};
+
+} // namespace nav2_costmap_2d
+
+#endif // NAV2_COSTMAP_2D__ASYMMETRIC_INFLATION_LAYER_HPP_
diff --git a/nav2_costmap_2d/plugins/asymmetric_inflation_layer.cpp b/nav2_costmap_2d/plugins/asymmetric_inflation_layer.cpp
new file mode 100644
index 00000000000..aad2d0f75f2
--- /dev/null
+++ b/nav2_costmap_2d/plugins/asymmetric_inflation_layer.cpp
@@ -0,0 +1,771 @@
+// Copyright (c) 2026, Marc Blöchlinger
+//
+// 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/asymmetric_inflation_layer.hpp"
+
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+#ifdef _OPENMP
+#include
+#endif
+
+#include "nav2_ros_common/node_utils.hpp"
+#include "pluginlib/class_list_macros.hpp"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "geometry_msgs/msg/transform_stamped.hpp"
+
+PLUGINLIB_EXPORT_CLASS(nav2_costmap_2d::AsymmetricInflationLayer, nav2_costmap_2d::Layer)
+
+using nav2_costmap_2d::LETHAL_OBSTACLE;
+using nav2_costmap_2d::NO_INFORMATION;
+using rcl_interfaces::msg::ParameterType;
+
+namespace nav2_costmap_2d
+{
+
+AsymmetricInflationLayer::AsymmetricInflationLayer()
+: cost_scaling_factor_left_(0),
+ cost_scaling_factor_right_(0),
+ goal_distance_threshold_(0)
+{
+}
+
+void
+AsymmetricInflationLayer::onInitialize()
+{
+ {
+ auto node = node_.lock();
+ if (!node) {
+ throw std::runtime_error{"Failed to lock node"};
+ }
+
+ enabled_ = node->declare_or_get_parameter(name_ + "." + "enabled", true);
+ inflation_radius_ = node->declare_or_get_parameter(
+ name_ + "." + "inflation_radius", 2.0);
+ inflate_unknown_ = node->declare_or_get_parameter(name_ + "." + "inflate_unknown", false);
+ inflate_around_unknown_ = node->declare_or_get_parameter(
+ name_ + "." + "inflate_around_unknown", false);
+ num_threads_ = node->declare_or_get_parameter(
+ name_ + "." + "num_threads", -1);
+ cost_scaling_factor_left_ = node->declare_or_get_parameter(
+ name_ + "." + "cost_scaling_factor_left", 4.0);
+ cost_scaling_factor_right_ = node->declare_or_get_parameter(
+ name_ + "." + "cost_scaling_factor_right", 4.0);
+ plan_topic_ = node->declare_or_get_parameter(
+ name_ + "." + "plan_topic", "plan");
+ goal_distance_threshold_ = node->declare_or_get_parameter(
+ name_ + "." + "goal_distance_threshold", 1.5);
+
+ // Apply the same bound checks as dynamic reconfigure, so bad YAML values fail
+ // loudly at startup instead of silently producing bad costmaps.
+ if (inflation_radius_ <= 0.0) {
+ throw std::runtime_error(
+ "AsymmetricInflationLayer: inflation_radius must be > 0");
+ }
+ if (cost_scaling_factor_left_ <= 0.0) {
+ throw std::runtime_error(
+ "AsymmetricInflationLayer: cost_scaling_factor_left must be > 0");
+ }
+ if (cost_scaling_factor_right_ <= 0.0) {
+ throw std::runtime_error(
+ "AsymmetricInflationLayer: cost_scaling_factor_right must be > 0");
+ }
+ if (goal_distance_threshold_ < 0.0) {
+ throw std::runtime_error(
+ "AsymmetricInflationLayer: goal_distance_threshold must be >= 0");
+ }
+ if (num_threads_ < -1) {
+ throw std::runtime_error(
+ "AsymmetricInflationLayer: num_threads must be -1 (auto) or > 0");
+ }
+
+ cost_scaling_factor_ =
+ std::max(cost_scaling_factor_left_, cost_scaling_factor_right_);
+
+ plan_topic_ = joinWithParentNamespace(plan_topic_);
+ path_sub_ = node->create_subscription(
+ plan_topic_,
+ std::bind(
+ &AsymmetricInflationLayer::globalPathCallback,
+ this, std::placeholders::_1),
+ rclcpp::QoS(1).durability_volatile());
+ }
+
+ setCurrent(true);
+ need_reinflation_ = false;
+ cell_inflation_radius_ = cellDistance(inflation_radius_);
+ matchSize();
+}
+
+void
+AsymmetricInflationLayer::activate()
+{
+ auto node = node_.lock();
+ if (!node) {
+ throw std::runtime_error{"Failed to lock node"};
+ }
+ on_set_params_handler_ = node->add_on_set_parameters_callback(
+ std::bind(
+ &AsymmetricInflationLayer::validateParameterUpdatesCallback,
+ this, std::placeholders::_1));
+ post_set_params_handler_ = node->add_post_set_parameters_callback(
+ std::bind(
+ &AsymmetricInflationLayer::updateParametersCallback,
+ this, std::placeholders::_1));
+}
+
+void
+AsymmetricInflationLayer::deactivate()
+{
+ auto node = node_.lock();
+ if (on_set_params_handler_ && node) {
+ node->remove_on_set_parameters_callback(on_set_params_handler_.get());
+ }
+ on_set_params_handler_.reset();
+ if (post_set_params_handler_ && node) {
+ node->remove_post_set_parameters_callback(post_set_params_handler_.get());
+ }
+ post_set_params_handler_.reset();
+}
+
+void
+AsymmetricInflationLayer::globalPathCallback(const nav_msgs::msg::Path::SharedPtr msg)
+{
+ std::lock_guard guard(*getMutex());
+ {
+ std::lock_guard lock(path_mutex_);
+ latest_global_path_ = msg;
+ }
+ // Path change invalidates all asymmetric costs in the costmap.
+ // Force a full-map reinflation on the next update cycle.
+ need_reinflation_ = true;
+ setCurrent(false);
+}
+
+void
+AsymmetricInflationLayer::matchSize()
+{
+ std::lock_guard guard(*getMutex());
+ InflationLayer::matchSize();
+
+ computeAsymmetricCaches();
+}
+
+void
+AsymmetricInflationLayer::updateBounds(
+ double robot_x, double robot_y, double robot_yaw, double * min_x,
+ double * min_y, double * max_x, double * max_y)
+{
+ std::lock_guard guard(*getMutex());
+
+ // Track robot pose for the goal-proximity fallback check
+ current_robot_x_ = robot_x;
+ current_robot_y_ = robot_y;
+
+ InflationLayer::updateBounds(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y);
+}
+
+void
+AsymmetricInflationLayer::onFootprintChanged()
+{
+ std::lock_guard guard(*getMutex());
+ InflationLayer::onFootprintChanged();
+ computeAsymmetricCaches();
+}
+
+std::vector>
+AsymmetricInflationLayer::extractLocalPath(
+ nav2_costmap_2d::Costmap2D & master_grid)
+{
+ std::vector> local_path_pts;
+ nav_msgs::msg::Path current_path;
+ {
+ std::lock_guard lock(path_mutex_);
+ if (!latest_global_path_ || latest_global_path_->poses.empty()) {
+ return local_path_pts;
+ }
+ current_path = *latest_global_path_;
+ }
+
+ // Check if the path is already in costmap frame
+ std::string global_frame = layered_costmap_->getGlobalFrameID();
+ std::string path_frame = current_path.header.frame_id;
+ geometry_msgs::msg::TransformStamped transform;
+ bool need_transform = (global_frame != path_frame && !path_frame.empty());
+
+ // Find the transform from path frame to costmap frame (e.g., map -> odom)
+ if (need_transform) {
+ try {
+ transform = tf_->lookupTransform(global_frame, path_frame, tf2::TimePointZero);
+ } catch (const tf2::TransformException & ex) {
+ RCLCPP_WARN(
+ logger_,
+ "AsymmetricInflationLayer: TF lookup failed (%s -> %s): %s. "
+ "Falling back to symmetric inflation.",
+ path_frame.c_str(), global_frame.c_str(), ex.what());
+ return local_path_pts;
+ }
+ }
+
+ // Disable asymmetry near the goal to prevent target oscillations
+ geometry_msgs::msg::PoseStamped goal_pose = current_path.poses.back();
+ if (need_transform) {
+ tf2::doTransform(goal_pose, goal_pose, transform);
+ }
+
+ double dist_to_goal = std::hypot(
+ goal_pose.pose.position.x - current_robot_x_,
+ goal_pose.pose.position.y - current_robot_y_);
+
+ if (dist_to_goal <= goal_distance_threshold_) {
+ // Empty vector causes algorithm to use standard symmetry
+ return local_path_pts;
+ }
+
+ // Extract local path
+ for (const auto & pose : current_path.poses) {
+ geometry_msgs::msg::PoseStamped transformed_pose = pose;
+ if (need_transform) {
+ tf2::doTransform(pose, transformed_pose, transform);
+ }
+
+ double px = transformed_pose.pose.position.x;
+ double py = transformed_pose.pose.position.y;
+ unsigned int mx, my;
+
+ // Only process points inside our current local costmap window
+ if (master_grid.worldToMap(px, py, mx, my)) {
+ local_path_pts.push_back({px, py});
+ }
+ }
+ return local_path_pts;
+}
+
+int8_t
+AsymmetricInflationLayer::computeObstacleSide(
+ double cx, double cy,
+ const std::vector & candidates,
+ const std::vector> & local_path_pts)
+{
+ const double inflation_radius_sq = inflation_radius_ * inflation_radius_;
+
+ double min_dist_sq = std::numeric_limits::max();
+ double best_cross = 0.0;
+
+ // Evaluate candidate segments provided by the spatial hash.
+ for (size_t p : candidates) {
+ // Define segment endpoints A (start) and B (end).
+ double ax = local_path_pts[p].first;
+ double ay = local_path_pts[p].second;
+ double bx = local_path_pts[p + 1].first;
+ double by = local_path_pts[p + 1].second;
+
+ // Skip cells outside of the segment bounding box expanded by the inflation radius.
+ double min_x = std::min(ax, bx) - inflation_radius_;
+ double max_x = std::max(ax, bx) + inflation_radius_;
+ if (cx < min_x || cx > max_x) {continue;}
+
+ double min_y = std::min(ay, by) - inflation_radius_;
+ double max_y = std::max(ay, by) + inflation_radius_;
+ if (cy < min_y || cy > max_y) {continue;}
+
+ // Calculate distance to path segment and orientation via cross product.
+ // Vectors: AB (path segment) and AC (path to cell)
+ double abx = bx - ax;
+ double aby = by - ay;
+ double len_sq = abx * abx + aby * aby;
+
+ double acx = cx - ax;
+ double acy = cy - ay;
+
+ double dist_sq;
+ double cross;
+
+ // Prevent division by zero for zero-length segments
+ if (len_sq < 1e-10) {
+ dist_sq = acx * acx + acy * acy;
+ cross = 0.0;
+ } else {
+ // 't': Scalar projection of C onto AB, clamped to segment bounds [0, 1]
+ double t = std::clamp((acx * abx + acy * aby) / len_sq, 0.0, 1.0);
+
+ // Vector from the projected point on AB to C
+ double proj_dx = acx - t * abx;
+ double proj_dy = acy - t * aby;
+ dist_sq = proj_dx * proj_dx + proj_dy * proj_dy;
+
+ // 2D cross product for orientation (Positive = Left, Negative = Right)
+ cross = abx * acy - aby * acx;
+ }
+
+ // Update if shortest distance so far
+ if (dist_sq < min_dist_sq) {
+ min_dist_sq = dist_sq;
+ best_cross = cross;
+ }
+ }
+
+ // Check if the cell is outside the inflation radius.
+ if (min_dist_sq > inflation_radius_sq) {
+ return 0;
+ }
+
+ // Return the orientation based on the cross product of the closest segment.
+ if (best_cross > 0.0) {return 1;} // Left
+ if (best_cross < 0.0) {return -1;} // Right
+
+ return 0; // Neutral/On the line
+}
+
+void
+AsymmetricInflationLayer::updateCosts(
+ nav2_costmap_2d::Costmap2D & master_grid, int min_i, int min_j,
+ int max_i, int max_j)
+{
+ std::lock_guard guard(*getMutex());
+
+ if (!enabled_) {
+ return;
+ }
+
+ // Pass 1: symmetric baseline via inherited distance-transform inflation
+ InflationLayer::updateCosts(master_grid, min_i, min_j, max_i, max_j);
+
+ std::vector> local_path_pts = extractLocalPath(master_grid);
+
+ // Abort if we don't have a valid path or if the scaling rates are equal (no asymmetry).
+ if (!((local_path_pts.size() >= 2) &&
+ (cost_scaling_factor_left_ != cost_scaling_factor_right_)))
+ {
+ setCurrent(true);
+ return;
+ }
+
+ // Pass 2: disfavored-side asymmetric overlay via distance transform
+ unsigned char * master_array = master_grid.getCharMap();
+ const unsigned int size_x = master_grid.getSizeInCellsX();
+ const unsigned int size_y = master_grid.getSizeInCellsY();
+
+ // Clamp update window (mirrors InflationLayer::updateCosts)
+ const int cmin_i = std::max(0, min_i);
+ const int cmin_j = std::max(0, min_j);
+ const int cmax_i = std::min(static_cast(size_x), max_i);
+ const int cmax_j = std::min(static_cast(size_y), max_j);
+
+ // Padded ROI — same formula as InflationLayer::updateCosts
+ const int padding = static_cast(cell_inflation_radius_);
+ const int roi_min_i = std::max(0, cmin_i - padding);
+ const int roi_min_j = std::max(0, cmin_j - padding);
+ const int roi_max_i = std::min(static_cast(size_x), cmax_i + padding);
+ const int roi_max_j = std::min(static_cast(size_y), cmax_j + padding);
+ const int roi_width = roi_max_i - roi_min_i;
+ const int roi_height = roi_max_j - roi_min_j;
+
+ auto spatial_hash = buildPathSpatialHash(local_path_pts);
+
+ MatrixXfRM dist_map = seedDistanceMap(
+ master_grid, roi_min_i, roi_min_j, roi_width, roi_height,
+ spatial_hash, local_path_pts);
+
+ DistanceTransform::distanceTransform2D(dist_map, roi_height, roi_width);
+
+ applyInflation(
+ master_array, dist_map,
+ cmin_i, cmin_j, cmax_i, cmax_j,
+ roi_min_i, roi_min_j, size_x);
+
+ setCurrent(true);
+}
+
+std::unordered_map>
+AsymmetricInflationLayer::buildPathSpatialHash(
+ const std::vector> & local_path_pts)
+{
+ std::unordered_map> spatial_hash;
+
+ for (size_t p = 0; p < local_path_pts.size() - 1; ++p) {
+ // Create segment AB from consecutive path points
+ double ax = local_path_pts[p].first;
+ double ay = local_path_pts[p].second;
+ double bx = local_path_pts[p + 1].first;
+ double by = local_path_pts[p + 1].second;
+
+ // Pad the segment's bounding box by the inflation radius.
+ double min_x = std::min(ax, bx) - inflation_radius_;
+ double max_x = std::max(ax, bx) + inflation_radius_;
+ double min_y = std::min(ay, by) - inflation_radius_;
+ double max_y = std::max(ay, by) + inflation_radius_;
+
+ // Find which buckets this padded segment touches
+ int64_t min_bx = static_cast(std::floor(min_x / inflation_radius_));
+ int64_t max_bx = static_cast(std::floor(max_x / inflation_radius_));
+ int64_t min_by = static_cast(std::floor(min_y / inflation_radius_));
+ int64_t max_by = static_cast(std::floor(max_y / inflation_radius_));
+
+ for (int64_t b_x = min_bx; b_x <= max_bx; ++b_x) {
+ for (int64_t b_y = min_by; b_y <= max_by; ++b_y) {
+ // Bitwise magic to safely map 2D signed coordinates into a 1D unsigned 64-bit key
+ uint64_t key = (static_cast(static_cast(b_x)) << 32) |
+ (static_cast(b_y));
+
+ spatial_hash[key].push_back(p);
+ }
+ }
+ }
+
+ return spatial_hash;
+}
+
+MatrixXfRM
+AsymmetricInflationLayer::seedDistanceMap(
+ nav2_costmap_2d::Costmap2D & master_grid,
+ int roi_min_i, int roi_min_j, int roi_width, int roi_height,
+ const std::unordered_map> & spatial_hash,
+ const std::vector> & local_path_pts)
+{
+ unsigned char * master_array = master_grid.getCharMap();
+ const unsigned int size_x = master_grid.getSizeInCellsX();
+ const unsigned int size_y = master_grid.getSizeInCellsY();
+
+ MatrixXfRM dist_map(roi_height, roi_width);
+ dist_map.setConstant(DistanceTransform::DT_INF);
+
+ int8_t disfavored_side = (cost_scaling_factor_left_ < cost_scaling_factor_right_) ? 1 : -1;
+ const int roi_max_i = roi_min_i + roi_width;
+ const int roi_max_j = roi_min_j + roi_height;
+
+ // Helper function to check if a neighbor is "traversable" (i.e., open space)
+ auto is_traversable = [&](int nx, int ny) {
+ unsigned char c = master_array[master_grid.getIndex(nx, ny)];
+ return inflate_around_unknown_ ?
+ (c != LETHAL_OBSTACLE && c != NO_INFORMATION) : (c != LETHAL_OBSTACLE);
+ };
+
+ // Seed all obstacle boundary cells, that are nearby a path segment
+ for (int j = roi_min_j; j < roi_max_j; ++j) {
+ for (int i = roi_min_i; i < roi_max_i; ++i) {
+ unsigned char cost = master_array[master_grid.getIndex(i, j)];
+
+ // Early exit 1: Skip cells that aren't lethal/unknown obstacles
+ if (cost != LETHAL_OBSTACLE && !(inflate_around_unknown_ && cost == NO_INFORMATION)) {
+ continue;
+ }
+
+ // Check if the cell touches the absolute edges of the costmap
+ bool is_on_map_edge = (i == 0 || i == static_cast(size_x) - 1 ||
+ j == 0 || j == static_cast(size_y) - 1);
+
+ // An obstacle cell is a boundary if it's on the map edge OR touches free space.
+ bool is_boundary = is_on_map_edge ||
+ is_traversable(i - 1, j) || is_traversable(i + 1, j) ||
+ is_traversable(i, j - 1) || is_traversable(i, j + 1);
+
+ // Early exit 2: Skip interior obstacle cells
+ if (!is_boundary) {
+ continue;
+ }
+
+ // Find segments that are nearby this cell using the spatial hash
+ double cx, cy;
+ master_grid.mapToWorld(i, j, cx, cy);
+ int64_t b_x = static_cast(std::floor(cx / inflation_radius_));
+ int64_t b_y = static_cast(std::floor(cy / inflation_radius_));
+ uint64_t key = (static_cast(static_cast(b_x)) << 32) |
+ static_cast(b_y);
+
+ // Only enqueue boundary cells on the disfavored side of the path.
+ // Cells on favored side already got correctly inflated during the symmetric inflation pass.
+ auto it = spatial_hash.find(key);
+ if (it == spatial_hash.end()) {
+ continue;
+ }
+
+ // Determine which side of the path this cell is on
+ int8_t side = computeObstacleSide(cx, cy, it->second, local_path_pts);
+ if (side != 0 && side == disfavored_side) {
+ dist_map(j - roi_min_j, i - roi_min_i) = 0.0f;
+ }
+ }
+ }
+
+ return dist_map;
+}
+
+void
+AsymmetricInflationLayer::applyInflation(
+ unsigned char * master_array,
+ const MatrixXfRM & distance_map,
+ int min_i, int min_j, int max_i, int max_j,
+ int roi_min_i, int roi_min_j,
+ unsigned int size_x)
+{
+ if (cost_lut_disfavored_.empty()) {
+ return;
+ }
+
+ const float cell_inflation_radius_f = static_cast(cell_inflation_radius_);
+ const int lut_max = static_cast(cost_lut_disfavored_.size() - 1);
+ const unsigned char * lut_data = cost_lut_disfavored_.data();
+ const int lut_precision = COST_LUT_PRECISION;
+
+#ifdef _OPENMP
+ const int num_threads = getOptimalThreadCount();
+ #pragma omp parallel for num_threads(num_threads) schedule(dynamic, 16)
+#endif
+ for (int j = min_j; j < max_j; ++j) {
+ const int row_offset = j * static_cast(size_x);
+ const int dist_row = j - roi_min_j;
+
+ for (int i = min_i; i < max_i; ++i) {
+ const float distance_cells = distance_map(dist_row, i - roi_min_i);
+ if (distance_cells > cell_inflation_radius_f) {
+ continue;
+ }
+
+ const unsigned int index = row_offset + i;
+ const unsigned char old_cost = master_array[index];
+ const unsigned int d_scaled = std::min(
+ static_cast(lut_max),
+ static_cast(distance_cells * lut_precision + 0.5f));
+ const unsigned char new_cost = lut_data[d_scaled];
+
+ if (new_cost > old_cost) {
+ master_array[index] = new_cost;
+ }
+ }
+ }
+}
+
+void
+AsymmetricInflationLayer::computeAsymmetricCaches()
+{
+ std::lock_guard guard(*getMutex());
+
+ if (cell_inflation_radius_ == 0) {
+ return;
+ }
+
+ // Build cost LUT for the disfavored side using c_side (the smaller scaling factor).
+ // computeCost() always uses cost_scaling_factor_ (c_max), so we inline the formula with c_side.
+ const double c_side = std::min(cost_scaling_factor_left_, cost_scaling_factor_right_);
+ const unsigned int max_dist_scaled = cell_inflation_radius_ * COST_LUT_PRECISION + 1;
+
+ cost_lut_disfavored_.resize(max_dist_scaled + 1);
+ for (unsigned int d_scaled = 0; d_scaled <= max_dist_scaled; ++d_scaled) {
+ const double distance = static_cast(d_scaled) / COST_LUT_PRECISION;
+ unsigned char cost = 0;
+ if (distance == 0.0) {
+ cost = LETHAL_OBSTACLE;
+ } else if (distance * resolution_ <= inscribed_radius_) {
+ cost = INSCRIBED_INFLATED_OBSTACLE;
+ } else {
+ double factor = exp(-c_side * (distance * resolution_ - inscribed_radius_));
+ cost = static_cast((INSCRIBED_INFLATED_OBSTACLE - 1) * factor);
+ }
+ cost_lut_disfavored_[d_scaled] = cost;
+ }
+}
+
+rcl_interfaces::msg::SetParametersResult
+AsymmetricInflationLayer::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 &&
+ param_name == name_ + ".inflation_radius")
+ {
+ if (parameter.as_double() <= 0.0) {
+ RCLCPP_WARN(
+ logger_, "inflation_radius must be > 0. Rejecting parameter update.");
+ result.successful = false;
+ result.reason = "inflation_radius must be > 0";
+ return result;
+ }
+ continue;
+ }
+
+ if (param_type == ParameterType::PARAMETER_DOUBLE &&
+ param_name == name_ + ".cost_scaling_factor_left")
+ {
+ if (parameter.as_double() <= 0.0) {
+ RCLCPP_WARN(
+ logger_, "cost_scaling_factor_left must be > 0. Rejecting parameter update.");
+ result.successful = false;
+ result.reason = "cost_scaling_factor_left must be > 0";
+ return result;
+ }
+ continue;
+ }
+
+ if (param_type == ParameterType::PARAMETER_DOUBLE &&
+ param_name == name_ + ".cost_scaling_factor_right")
+ {
+ if (parameter.as_double() <= 0.0) {
+ RCLCPP_WARN(
+ logger_, "cost_scaling_factor_right must be > 0. Rejecting parameter update.");
+ result.successful = false;
+ result.reason = "cost_scaling_factor_right must be > 0";
+ return result;
+ }
+ continue;
+ }
+
+ if (param_type == ParameterType::PARAMETER_DOUBLE &&
+ param_name == name_ + ".goal_distance_threshold")
+ {
+ if (parameter.as_double() < 0.0) {
+ RCLCPP_WARN(
+ logger_, "goal_distance_threshold must be >= 0. Rejecting parameter update.");
+ result.successful = false;
+ result.reason = "goal_distance_threshold must be >= 0";
+ return result;
+ }
+ continue;
+ }
+
+ if (param_type == ParameterType::PARAMETER_INTEGER &&
+ param_name == name_ + ".num_threads")
+ {
+ if (parameter.as_int() < -1) {
+ RCLCPP_WARN(
+ logger_, "num_threads must be -1 (auto) or > 0. Rejecting parameter update.");
+ result.successful = false;
+ result.reason = "num_threads must be -1 (auto) or > 0";
+ return result;
+ }
+ }
+ }
+
+ return result;
+}
+
+void
+AsymmetricInflationLayer::updateParametersCallback(
+ const std::vector & parameters)
+{
+ std::lock_guard guard(*getMutex());
+ bool need_cache_recompute = false;
+ bool side_scaling_changed = false;
+
+ 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_ + ".inflation_radius" &&
+ inflation_radius_ != parameter.as_double())
+ {
+ inflation_radius_ = parameter.as_double();
+ need_reinflation_ = true;
+ need_cache_recompute = true;
+ setCurrent(false);
+ } else if (param_name == name_ + ".cost_scaling_factor_left" && // NOLINT
+ cost_scaling_factor_left_ != parameter.as_double())
+ {
+ cost_scaling_factor_left_ = parameter.as_double();
+ side_scaling_changed = true;
+ } else if (param_name == name_ + ".cost_scaling_factor_right" && // NOLINT
+ cost_scaling_factor_right_ != parameter.as_double())
+ {
+ cost_scaling_factor_right_ = parameter.as_double();
+ side_scaling_changed = true;
+ } else if (param_name == name_ + ".goal_distance_threshold" && // NOLINT
+ goal_distance_threshold_ != parameter.as_double())
+ {
+ goal_distance_threshold_ = parameter.as_double();
+ need_reinflation_ = true;
+ setCurrent(false);
+ }
+ } else if (param_type == ParameterType::PARAMETER_BOOL) {
+ if (param_name == name_ + ".enabled" && enabled_ != parameter.as_bool()) {
+ enabled_ = parameter.as_bool();
+ need_reinflation_ = true;
+ setCurrent(false);
+ } else if (param_name == name_ + ".inflate_around_unknown" && // NOLINT
+ inflate_around_unknown_ != parameter.as_bool())
+ {
+ inflate_around_unknown_ = parameter.as_bool();
+ need_reinflation_ = true;
+ setCurrent(false);
+ } else if (param_name == name_ + ".inflate_unknown" && // NOLINT
+ inflate_unknown_ != parameter.as_bool())
+ {
+ inflate_unknown_ = parameter.as_bool();
+ need_reinflation_ = true;
+ setCurrent(false);
+ }
+ } else if (param_type == ParameterType::PARAMETER_INTEGER) {
+ if (param_name == name_ + ".num_threads" && // NOLINT
+ num_threads_ != parameter.as_int())
+ {
+ int new_value = parameter.as_int();
+#ifdef _OPENMP
+ int available_cores = omp_get_max_threads();
+ if (new_value > available_cores) {
+ RCLCPP_WARN(
+ logger_,
+ "num_threads=%d exceeds available cores (%d). Ignoring.",
+ new_value, available_cores);
+ } else {
+ num_threads_ = new_value;
+ RCLCPP_INFO(
+ logger_,
+ "Updated num_threads to %d %s",
+ num_threads_,
+ num_threads_ == -1 ? "(auto)" : "");
+ }
+#else
+ RCLCPP_WARN(
+ logger_,
+ "num_threads parameter ignored - OpenMP support not available. "
+ "Inflation layer will use single thread.");
+ num_threads_ = new_value;
+#endif
+ }
+ }
+ }
+
+ if (side_scaling_changed) {
+ cost_scaling_factor_ =
+ std::max(cost_scaling_factor_left_, cost_scaling_factor_right_);
+ need_reinflation_ = true;
+ need_cache_recompute = true;
+ setCurrent(false);
+ }
+
+ if (need_cache_recompute) {
+ matchSize();
+ }
+}
+
+} // namespace nav2_costmap_2d
diff --git a/nav2_costmap_2d/test/benchmark/CMakeLists.txt b/nav2_costmap_2d/test/benchmark/CMakeLists.txt
index 96991cb9a8a..b5ad6af0dc9 100644
--- a/nav2_costmap_2d/test/benchmark/CMakeLists.txt
+++ b/nav2_costmap_2d/test/benchmark/CMakeLists.txt
@@ -16,3 +16,10 @@ if(ENABLE_OPENMP)
target_compile_options(inflation_layer_updatecosts_benchmark PRIVATE -fopenmp)
target_link_libraries(inflation_layer_updatecosts_benchmark OpenMP::OpenMP_CXX)
endif()
+
+ament_add_google_benchmark(asymmetric_layer_updatecosts_benchmark
+ asymmetric_layer_updatecosts_benchmark.cpp)
+target_link_libraries(asymmetric_layer_updatecosts_benchmark
+ nav2_costmap_2d_core
+ layers
+)
diff --git a/nav2_costmap_2d/test/benchmark/asymmetric_layer_updatecosts_benchmark.cpp b/nav2_costmap_2d/test/benchmark/asymmetric_layer_updatecosts_benchmark.cpp
new file mode 100644
index 00000000000..c809a0296cb
--- /dev/null
+++ b/nav2_costmap_2d/test/benchmark/asymmetric_layer_updatecosts_benchmark.cpp
@@ -0,0 +1,898 @@
+// Copyright (c) 2026 Marc Blöchlinger
+//
+// 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.
+
+/**
+ * @file asymmetric_inflation_overhead_benchmark.cpp
+ * @brief Measures the per-update overhead of AsymmetricInflationLayer in a
+ * standalone path-aware inflation pipeline.
+ *
+ * Three fixtures are compared across the registered costmap sizes at
+ * 0.05 m/cell, 50 iterations each:
+ *
+ * InflationFixture — InflationLayer::updateCosts() alone
+ * (the distance-transform symmetric baseline)
+ *
+ * LegacyInflationFixture
+ * — LegacyInflationLayer::updateCosts() alone
+ * (the pre-DT symmetric baseline)
+ *
+ * AsymmetricFixture — AsymmetricInflationLayer::updateCosts() alone
+ * (inherited distance-transform baseline plus
+ * asymmetric overlay)
+ *
+ * Comparing AsymmetricFixture against InflationFixture shows the overlay
+ * overhead on top of the DT baseline; LegacyInflationFixture keeps the old
+ * symmetric algorithm visible as a separate reference point.
+ */
+
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+#include "benchmark/benchmark.h"
+#include "rcutils/logging.h"
+#include "rclcpp/rclcpp.hpp"
+#include "nav2_ros_common/lifecycle_node.hpp"
+#include "nav2_costmap_2d/layered_costmap.hpp"
+#include "nav2_costmap_2d/costmap_2d.hpp"
+#include "nav2_costmap_2d/inflation_layer.hpp"
+#include "nav2_costmap_2d/legacy_inflation_layer.hpp"
+#include "nav2_costmap_2d/asymmetric_inflation_layer.hpp"
+#include "nav_msgs/msg/path.hpp"
+#include "geometry_msgs/msg/point.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+
+namespace
+{
+
+static constexpr const char * kGlobalFrame = "map";
+static constexpr double kResolution = 0.05; ///< metres per cell
+static constexpr double kInflationRadius = 2.0; ///< metres
+/// Asymmetric per-side decay rates so the overlay work is active.
+static constexpr double kCostScalingFactorLeft = 4.0;
+static constexpr double kCostScalingFactorRight = 1.0;
+static constexpr double kCostScalingFactor =
+ (kCostScalingFactorLeft > kCostScalingFactorRight) ?
+ kCostScalingFactorLeft : kCostScalingFactorRight;
+static constexpr double kOccupancy = 0.50; ///< lethal-obstacle fraction
+static constexpr unsigned int kObstacleSeed = 42;
+
+/**
+ * @brief Exposes the protected globalPathCallback for path injection in benchmarks.
+ */
+class BenchmarkAsymmetricInflationLayer : public nav2_costmap_2d::AsymmetricInflationLayer
+{
+public:
+ /** @brief Deliver @p msg as if it arrived on the plan topic. */
+ void injectPath(nav_msgs::msg::Path::SharedPtr msg) {globalPathCallback(msg);}
+};
+
+/**
+ * @brief Seed lethal rectangular obstacles covering ~@p occupancy_pct of the costmap.
+ *
+ * Clears the entire map first, then places random-sized rectangles until the
+ * target cell count is reached.
+ */
+void generateRectangularObstacles(
+ nav2_costmap_2d::Costmap2D & costmap,
+ double occupancy_pct,
+ unsigned int seed = kObstacleSeed)
+{
+ const unsigned int size_x = costmap.getSizeInCellsX();
+ const unsigned int size_y = costmap.getSizeInCellsY();
+ const std::size_t total = static_cast(size_x) * static_cast(size_y);
+ const std::size_t target = static_cast(
+ static_cast(total) * occupancy_pct);
+
+ const unsigned int min_w = std::max(1u, static_cast(size_x * 0.05));
+ const unsigned int max_w = std::max(min_w + 1u, static_cast(size_x * 0.15));
+ const unsigned int min_h = std::max(1u, static_cast(size_y * 0.05));
+ const unsigned int max_h = std::max(min_h + 1u, static_cast(size_y * 0.15));
+
+ std::mt19937 gen(seed);
+ std::uniform_int_distribution dist_x(0, size_x - 1);
+ std::uniform_int_distribution dist_y(0, size_y - 1);
+ std::uniform_int_distribution dist_w(min_w, max_w);
+ std::uniform_int_distribution dist_h(min_h, max_h);
+
+ std::memset(costmap.getCharMap(), nav2_costmap_2d::FREE_SPACE, total);
+
+ std::size_t occupied = 0;
+ for (unsigned int attempts = 0; occupied < target && attempts < 10000u; ++attempts) {
+ unsigned int rx = dist_x(gen);
+ unsigned int ry = dist_y(gen);
+ unsigned int ex = std::min(rx + dist_w(gen), size_x);
+ unsigned int ey = std::min(ry + dist_h(gen), size_y);
+ for (unsigned int cy = ry; cy < ey && occupied < target; ++cy) {
+ for (unsigned int cx = rx; cx < ex && occupied < target; ++cx) {
+ if (costmap.getCost(cx, cy) != nav2_costmap_2d::LETHAL_OBSTACLE) {
+ costmap.setCost(cx, cy, nav2_costmap_2d::LETHAL_OBSTACLE);
+ ++occupied;
+ }
+ }
+ }
+ }
+}
+
+/** @brief Build a 0.6 m × 0.4 m rectangular robot footprint. */
+std::vector makeFootprint()
+{
+ std::vector fp(4);
+ fp[0].x = 0.3; fp[0].y = 0.2;
+ fp[1].x = 0.3; fp[1].y = -0.2;
+ fp[2].x = -0.3; fp[2].y = -0.2;
+ fp[3].x = -0.3; fp[3].y = 0.2;
+ return fp;
+}
+
+/**
+ * @brief Clear and re-seed lethal obstacles within a square sub-region.
+ *
+ * Simulates one cycle of incremental sensor data: the patch is cleared then
+ * re-filled with random rectangular obstacles at @p kOccupancy density.
+ * Called during paused timing so only the resulting updateCosts() is measured.
+ *
+ * @param costmap The costmap to modify in-place.
+ * @param ox Left cell coordinate of the patch.
+ * @param oy Bottom cell coordinate of the patch.
+ * @param side Side length of the square patch in cells.
+ * @param seed RNG seed; increment each call to vary the obstacle layout.
+ */
+void modifyPatch(
+ nav2_costmap_2d::Costmap2D & costmap,
+ unsigned int ox, unsigned int oy, unsigned int side, unsigned int seed)
+{
+ const unsigned int ex = ox + side;
+ const unsigned int ey = oy + side;
+
+ for (unsigned int cy = oy; cy < ey; ++cy) {
+ for (unsigned int cx = ox; cx < ex; ++cx) {
+ costmap.setCost(cx, cy, nav2_costmap_2d::FREE_SPACE);
+ }
+ }
+
+ const std::size_t target = static_cast(
+ static_cast(side) * static_cast(side) * kOccupancy);
+ const unsigned int min_d = std::max(1u, side / 10u);
+ const unsigned int max_d = std::max(min_d + 1u, side / 5u);
+
+ std::mt19937 gen(seed);
+ std::uniform_int_distribution dist_x(ox, ex - 1u);
+ std::uniform_int_distribution dist_y(oy, ey - 1u);
+ std::uniform_int_distribution dist_w(min_d, max_d);
+ std::uniform_int_distribution dist_h(min_d, max_d);
+
+ std::size_t occupied = 0;
+ for (unsigned int a = 0; occupied < target && a < 10000u; ++a) {
+ unsigned int rx = dist_x(gen);
+ unsigned int ry = dist_y(gen);
+ unsigned int bx = std::min(rx + dist_w(gen), ex);
+ unsigned int by = std::min(ry + dist_h(gen), ey);
+ for (unsigned int cy = ry; cy < by && occupied < target; ++cy) {
+ for (unsigned int cx = rx; cx < bx && occupied < target; ++cx) {
+ if (costmap.getCost(cx, cy) != nav2_costmap_2d::LETHAL_OBSTACLE) {
+ costmap.setCost(cx, cy, nav2_costmap_2d::LETHAL_OBSTACLE);
+ ++occupied;
+ }
+ }
+ }
+ }
+}
+
+} // namespace
+
+// ---------------------------------------------------------------------------
+// Baseline: InflationLayer alone
+// ---------------------------------------------------------------------------
+
+/**
+ * @brief Fixture for the baseline pipeline: InflationLayer only.
+ *
+ * Benchmark arguments: {width_cells, height_cells}.
+ */
+class InflationFixture : public benchmark::Fixture
+{
+public:
+ void SetUp(benchmark::State & state) override
+ {
+ width_ = static_cast(state.range(0));
+ height_ = static_cast(state.range(1));
+
+ auto opts = rclcpp::NodeOptions().parameter_overrides(
+ {
+ rclcpp::Parameter("inflation.inflation_radius", kInflationRadius),
+ rclcpp::Parameter("inflation.cost_scaling_factor", kCostScalingFactor),
+ rclcpp::Parameter("inflation.inflate_unknown", false),
+ rclcpp::Parameter("inflation.inflate_around_unknown", false),
+ });
+ node_ = std::make_shared("inflation_benchmark", "", opts);
+
+ layers_ = std::make_unique(kGlobalFrame, false, false);
+ layers_->resizeMap(width_, height_, kResolution, 0.0, 0.0);
+
+ layer_ = std::make_shared();
+ layer_->initialize(layers_.get(), "inflation", nullptr, node_, nullptr);
+ layers_->addPlugin(std::static_pointer_cast(layer_));
+ layers_->setFootprint(makeFootprint());
+
+ generateRectangularObstacles(*layers_->getCostmap(), kOccupancy);
+ }
+
+ void TearDown(benchmark::State &) override
+ {
+ layer_.reset();
+ layers_.reset();
+ node_.reset();
+ }
+
+ unsigned int width_{0};
+ unsigned int height_{0};
+ nav2::LifecycleNode::SharedPtr node_;
+ std::unique_ptr layers_;
+ std::shared_ptr layer_;
+};
+
+BENCHMARK_DEFINE_F(InflationFixture, UpdateCosts)(benchmark::State & state)
+{
+ auto * costmap = layers_->getCostmap();
+ const int w = static_cast(width_);
+ const int h = static_cast(height_);
+
+ for (auto _ : state) {
+ state.PauseTiming();
+ generateRectangularObstacles(*costmap, kOccupancy);
+ state.ResumeTiming();
+
+ layer_->updateCosts(*costmap, 0, 0, w, h);
+ }
+
+ state.counters["cells"] = static_cast(width_) * static_cast(height_);
+ state.counters["map_side_m"] = static_cast(width_) * kResolution;
+}
+
+BENCHMARK_REGISTER_F(InflationFixture, UpdateCosts)
+->Args({60, 60}) // 3x3 m
+->Args({100, 100}) // 5x5 m
+->Args({200, 200}) // 10x10 m
+->Args({400, 400}) // 20x20 m
+->Args({800, 800}) // 40x40 m
+->Args({1600, 1600}) // 80x80 m
+->Args({3200, 3200}) // 160x160 m
+->Args({6400, 6400}) // 320x320 m
+->Args({12800, 12800}) // 640x640 m
+->Iterations(50)
+->Unit(benchmark::kMillisecond);
+
+// ---------------------------------------------------------------------------
+// Baseline: LegacyInflationLayer alone
+// ---------------------------------------------------------------------------
+
+/**
+ * @brief Fixture for the legacy baseline pipeline: LegacyInflationLayer only.
+ *
+ * Benchmark arguments: {width_cells, height_cells}.
+ */
+class LegacyInflationFixture : public benchmark::Fixture
+{
+public:
+ void SetUp(benchmark::State & state) override
+ {
+ width_ = static_cast(state.range(0));
+ height_ = static_cast(state.range(1));
+
+ auto opts = rclcpp::NodeOptions().parameter_overrides(
+ {
+ rclcpp::Parameter("legacy_inflation.inflation_radius", kInflationRadius),
+ rclcpp::Parameter("legacy_inflation.cost_scaling_factor", kCostScalingFactor),
+ rclcpp::Parameter("legacy_inflation.inflate_unknown", false),
+ rclcpp::Parameter("legacy_inflation.inflate_around_unknown", false),
+ });
+ node_ = std::make_shared("legacy_inflation_benchmark", "", opts);
+
+ layers_ = std::make_unique(kGlobalFrame, false, false);
+ layers_->resizeMap(width_, height_, kResolution, 0.0, 0.0);
+
+ layer_ = std::make_shared();
+ layer_->initialize(layers_.get(), "legacy_inflation", nullptr, node_, nullptr);
+ layers_->addPlugin(std::static_pointer_cast(layer_));
+ layers_->setFootprint(makeFootprint());
+
+ generateRectangularObstacles(*layers_->getCostmap(), kOccupancy);
+ }
+
+ void TearDown(benchmark::State &) override
+ {
+ layer_.reset();
+ layers_.reset();
+ node_.reset();
+ }
+
+ unsigned int width_{0};
+ unsigned int height_{0};
+ nav2::LifecycleNode::SharedPtr node_;
+ std::unique_ptr layers_;
+ std::shared_ptr layer_;
+};
+
+BENCHMARK_DEFINE_F(LegacyInflationFixture, UpdateCosts)(benchmark::State & state)
+{
+ auto * costmap = layers_->getCostmap();
+ const int w = static_cast(width_);
+ const int h = static_cast(height_);
+
+ for (auto _ : state) {
+ state.PauseTiming();
+ generateRectangularObstacles(*costmap, kOccupancy);
+ state.ResumeTiming();
+
+ layer_->updateCosts(*costmap, 0, 0, w, h);
+ }
+
+ state.counters["cells"] = static_cast(width_) * static_cast(height_);
+ state.counters["map_side_m"] = static_cast(width_) * kResolution;
+}
+
+BENCHMARK_REGISTER_F(LegacyInflationFixture, UpdateCosts)
+->Args({60, 60}) // 3x3 m
+->Args({100, 100}) // 5x5 m
+->Args({200, 200}) // 10x10 m
+->Args({400, 400}) // 20x20 m
+->Args({800, 800}) // 40x40 m
+->Args({1600, 1600}) // 80x80 m
+->Args({3200, 3200}) // 160x160 m
+->Args({6400, 6400}) // 320x320 m
+->Args({12800, 12800}) // 640x640 m
+->Iterations(50)
+->Unit(benchmark::kMillisecond);
+
+// ---------------------------------------------------------------------------
+// Full map: AsymmetricInflationLayer standalone
+// ---------------------------------------------------------------------------
+
+/**
+ * @brief Fixture for the standalone asymmetric pipeline.
+ *
+ * AsymmetricInflationLayer writes the inherited distance-transform symmetric
+ * baseline and then raises costs where the path-aware overlay exceeds it.
+ *
+ * Benchmark arguments: {width_cells, height_cells}.
+ */
+class AsymmetricFixture : public benchmark::Fixture
+{
+public:
+ void SetUp(benchmark::State & state) override
+ {
+ width_ = static_cast(state.range(0));
+ height_ = static_cast(state.range(1));
+
+ auto opts = rclcpp::NodeOptions().parameter_overrides(
+ {
+ rclcpp::Parameter("asymmetric_inflation_layer.enabled", true),
+ rclcpp::Parameter("asymmetric_inflation_layer.inflation_radius", kInflationRadius),
+ rclcpp::Parameter("asymmetric_inflation_layer.inflate_unknown", false),
+ rclcpp::Parameter(
+ "asymmetric_inflation_layer.cost_scaling_factor_left", kCostScalingFactorLeft),
+ rclcpp::Parameter(
+ "asymmetric_inflation_layer.cost_scaling_factor_right", kCostScalingFactorRight),
+ rclcpp::Parameter("asymmetric_inflation_layer.inflate_around_unknown", false),
+ rclcpp::Parameter("asymmetric_inflation_layer.goal_distance_threshold", 1.5),
+ rclcpp::Parameter(
+ "asymmetric_inflation_layer.plan_topic", std::string("/benchmark_asymmetric_plan")),
+ });
+ node_ = std::make_shared("asymmetric_benchmark", "", opts);
+
+ layers_ = std::make_unique(kGlobalFrame, false, false);
+ layers_->resizeMap(width_, height_, kResolution, 0.0, 0.0);
+
+ asym_layer_ = std::make_shared();
+ asym_layer_->initialize(
+ layers_.get(), "asymmetric_inflation_layer", nullptr, node_, nullptr);
+ layers_->addPlugin(std::static_pointer_cast(asym_layer_));
+
+ layers_->setFootprint(makeFootprint());
+
+ injectDiagonalPath();
+ generateRectangularObstacles(*layers_->getCostmap(), kOccupancy);
+ }
+
+ void TearDown(benchmark::State &) override
+ {
+ asym_layer_.reset();
+ layers_.reset();
+ node_.reset();
+ }
+
+ unsigned int width_{0};
+ unsigned int height_{0};
+ nav2::LifecycleNode::SharedPtr node_;
+ std::unique_ptr layers_;
+ std::shared_ptr asym_layer_;
+
+private:
+ void injectDiagonalPath()
+ {
+ const double width_m = static_cast(width_) * kResolution;
+ const double height_m = static_cast(height_) * kResolution;
+ const double diag = std::hypot(width_m, height_m);
+ const double step = 0.5;
+ const int n = std::max(2, static_cast(std::ceil(diag / step)) + 1);
+
+ auto path = std::make_shared();
+ path->header.frame_id = kGlobalFrame;
+
+ for (int i = 0; i < n; ++i) {
+ double t = static_cast(i) / static_cast(n - 1);
+ geometry_msgs::msg::PoseStamped ps;
+ ps.header.frame_id = kGlobalFrame;
+ ps.pose.position.x = t * width_m;
+ ps.pose.position.y = t * height_m;
+ ps.pose.orientation.w = 1.0;
+ path->poses.push_back(ps);
+ }
+
+ asym_layer_->injectPath(path);
+ }
+};
+
+BENCHMARK_DEFINE_F(AsymmetricFixture, UpdateCosts)(benchmark::State & state)
+{
+ auto * costmap = layers_->getCostmap();
+ const int w = static_cast(width_);
+ const int h = static_cast(height_);
+
+ for (auto _ : state) {
+ state.PauseTiming();
+ generateRectangularObstacles(*costmap, kOccupancy);
+ state.ResumeTiming();
+
+ asym_layer_->updateCosts(*costmap, 0, 0, w, h);
+ }
+
+ state.counters["cells"] = static_cast(width_) * static_cast(height_);
+ state.counters["map_side_m"] = static_cast(width_) * kResolution;
+}
+
+BENCHMARK_REGISTER_F(AsymmetricFixture, UpdateCosts)
+->Args({60, 60}) // 3x3 m
+->Args({100, 100}) // 5x5 m
+->Args({200, 200}) // 10x10 m
+->Args({400, 400}) // 20x20 m
+->Args({800, 800}) // 40x40 m
+->Args({1600, 1600}) // 80x80 m
+->Args({3200, 3200}) // 160x160 m
+->Args({6400, 6400}) // 320x320 m
+->Args({12800, 12800}) // 640x640 m
+->Iterations(50)
+->Unit(benchmark::kMillisecond);
+
+// ---------------------------------------------------------------------------
+// Incremental: InflationLayer with a small dirty window
+// ---------------------------------------------------------------------------
+
+/**
+ * @brief Fixture for incremental updates: InflationLayer only.
+ *
+ * Pre-warms the costmap with a full updateCosts() call, then benchmarks
+ * updateCosts() on a small moving patch — simulating an upstream obstacle
+ * layer that dirtied only a local window.
+ *
+ * The patch_side_cells argument represents the window after updateBounds()
+ * has expanded the raw dirty region by inflation_radius_ on each side.
+ *
+ * Benchmark arguments: {width_cells, height_cells, patch_side_cells}.
+ */
+class IncrementalInflationFixture : public benchmark::Fixture
+{
+public:
+ void SetUp(benchmark::State & state) override
+ {
+ width_ = static_cast(state.range(0));
+ height_ = static_cast(state.range(1));
+ patch_ = static_cast(state.range(2));
+
+ auto opts = rclcpp::NodeOptions().parameter_overrides(
+ {
+ rclcpp::Parameter("inflation.inflation_radius", kInflationRadius),
+ rclcpp::Parameter("inflation.cost_scaling_factor", kCostScalingFactor),
+ rclcpp::Parameter("inflation.inflate_unknown", false),
+ rclcpp::Parameter("inflation.inflate_around_unknown", false),
+ });
+ node_ = std::make_shared("incremental_inflation_benchmark", "", opts);
+
+ layers_ = std::make_unique(kGlobalFrame, false, false);
+ layers_->resizeMap(width_, height_, kResolution, 0.0, 0.0);
+
+ layer_ = std::make_shared();
+ layer_->initialize(layers_.get(), "inflation", nullptr, node_, nullptr);
+ layers_->addPlugin(std::static_pointer_cast(layer_));
+ layers_->setFootprint(makeFootprint());
+
+ // Pre-warm: full update to put the layer in a valid post-update state.
+ auto * costmap = layers_->getCostmap();
+ generateRectangularObstacles(*costmap, kOccupancy);
+ layer_->updateCosts(*costmap, 0, 0, static_cast(width_), static_cast(height_));
+ }
+
+ void TearDown(benchmark::State &) override
+ {
+ layer_.reset();
+ layers_.reset();
+ node_.reset();
+ }
+
+ unsigned int width_{0};
+ unsigned int height_{0};
+ unsigned int patch_{0};
+ nav2::LifecycleNode::SharedPtr node_;
+ std::unique_ptr layers_;
+ std::shared_ptr layer_;
+};
+
+BENCHMARK_DEFINE_F(IncrementalInflationFixture, UpdateCosts)(benchmark::State & state)
+{
+ auto * costmap = layers_->getCostmap();
+ const unsigned int max_ox = (width_ > patch_) ? width_ - patch_ : 0u;
+ const unsigned int max_oy = (height_ > patch_) ? height_ - patch_ : 0u;
+ unsigned int step = 0;
+
+ for (auto _ : state) {
+ state.PauseTiming();
+ // Cycle patch position with co-prime strides to avoid hot-cache repetition.
+ const unsigned int ox = max_ox ? (step * 37u) % max_ox : 0u;
+ const unsigned int oy = max_oy ? (step * 53u) % max_oy : 0u;
+ modifyPatch(*costmap, ox, oy, patch_, kObstacleSeed + step);
+ ++step;
+ state.ResumeTiming();
+
+ layer_->updateCosts(
+ *costmap,
+ static_cast(ox), static_cast(oy),
+ static_cast(ox + patch_), static_cast(oy + patch_));
+ }
+
+ state.counters["cells"] = static_cast(width_) * static_cast(height_);
+ state.counters["patch_side_m"] = static_cast(patch_) * kResolution;
+ state.counters["map_side_m"] = static_cast(width_) * kResolution;
+}
+
+BENCHMARK_REGISTER_F(IncrementalInflationFixture, UpdateCosts)
+->Args({800, 800, 100}) // 40×40 m map, ~5 m patch
+->Args({800, 800, 200}) // 40×40 m map, ~10 m patch
+->Args({1600, 1600, 100}) // 80×80 m map, ~5 m patch
+->Args({1600, 1600, 200}) // 80×80 m map, ~10 m patch
+->Args({3200, 3200, 100}) // 160×160 m map, ~5 m patch
+->Args({3200, 3200, 200}) // 160×160 m map, ~10 m patch
+->Args({6400, 6400, 100}) // 320×320 m map, ~5 m patch
+->Args({6400, 6400, 200}) // 320×320 m map, ~10 m patch
+->Args({12800, 12800, 100}) // 640×640 m map, ~5 m patch
+->Args({12800, 12800, 200}) // 640×640 m map, ~10 m patch
+->Args({25600, 25600, 100}) // 1280×1280 m map, ~5 m patch
+->Args({25600, 25600, 200}) // 1280×1280 m map, ~10 m patch
+->Iterations(50)
+->Unit(benchmark::kMillisecond);
+
+// ---------------------------------------------------------------------------
+// Incremental: LegacyInflationLayer with a small dirty window
+// ---------------------------------------------------------------------------
+
+/**
+ * @brief Fixture for incremental updates: LegacyInflationLayer only.
+ *
+ * Benchmark arguments: {width_cells, height_cells, patch_side_cells}.
+ */
+class IncrementalLegacyInflationFixture : public benchmark::Fixture
+{
+public:
+ void SetUp(benchmark::State & state) override
+ {
+ width_ = static_cast(state.range(0));
+ height_ = static_cast(state.range(1));
+ patch_ = static_cast(state.range(2));
+
+ auto opts = rclcpp::NodeOptions().parameter_overrides(
+ {
+ rclcpp::Parameter("legacy_inflation.inflation_radius", kInflationRadius),
+ rclcpp::Parameter("legacy_inflation.cost_scaling_factor", kCostScalingFactor),
+ rclcpp::Parameter("legacy_inflation.inflate_unknown", false),
+ rclcpp::Parameter("legacy_inflation.inflate_around_unknown", false),
+ });
+ node_ =
+ std::make_shared("incremental_legacy_inflation_benchmark", "", opts);
+
+ layers_ = std::make_unique(kGlobalFrame, false, false);
+ layers_->resizeMap(width_, height_, kResolution, 0.0, 0.0);
+
+ layer_ = std::make_shared();
+ layer_->initialize(layers_.get(), "legacy_inflation", nullptr, node_, nullptr);
+ layers_->addPlugin(std::static_pointer_cast(layer_));
+ layers_->setFootprint(makeFootprint());
+
+ // Pre-warm: full update to put the layer in a valid post-update state.
+ auto * costmap = layers_->getCostmap();
+ generateRectangularObstacles(*costmap, kOccupancy);
+ layer_->updateCosts(*costmap, 0, 0, static_cast(width_), static_cast(height_));
+ }
+
+ void TearDown(benchmark::State &) override
+ {
+ layer_.reset();
+ layers_.reset();
+ node_.reset();
+ }
+
+ unsigned int width_{0};
+ unsigned int height_{0};
+ unsigned int patch_{0};
+ nav2::LifecycleNode::SharedPtr node_;
+ std::unique_ptr layers_;
+ std::shared_ptr layer_;
+};
+
+BENCHMARK_DEFINE_F(IncrementalLegacyInflationFixture, UpdateCosts)(benchmark::State & state)
+{
+ auto * costmap = layers_->getCostmap();
+ const unsigned int max_ox = (width_ > patch_) ? width_ - patch_ : 0u;
+ const unsigned int max_oy = (height_ > patch_) ? height_ - patch_ : 0u;
+ unsigned int step = 0;
+
+ for (auto _ : state) {
+ state.PauseTiming();
+ const unsigned int ox = max_ox ? (step * 37u) % max_ox : 0u;
+ const unsigned int oy = max_oy ? (step * 53u) % max_oy : 0u;
+ modifyPatch(*costmap, ox, oy, patch_, kObstacleSeed + step);
+ ++step;
+ state.ResumeTiming();
+
+ layer_->updateCosts(
+ *costmap,
+ static_cast(ox), static_cast(oy),
+ static_cast(ox + patch_), static_cast(oy + patch_));
+ }
+
+ state.counters["cells"] = static_cast(width_) * static_cast(height_);
+ state.counters["patch_side_m"] = static_cast(patch_) * kResolution;
+ state.counters["map_side_m"] = static_cast(width_) * kResolution;
+}
+
+BENCHMARK_REGISTER_F(IncrementalLegacyInflationFixture, UpdateCosts)
+->Args({800, 800, 100}) // 40×40 m map, ~5 m patch
+->Args({800, 800, 200}) // 40×40 m map, ~10 m patch
+->Args({1600, 1600, 100}) // 80×80 m map, ~5 m patch
+->Args({1600, 1600, 200}) // 80×80 m map, ~10 m patch
+->Args({3200, 3200, 100}) // 160×160 m map, ~5 m patch
+->Args({3200, 3200, 200}) // 160×160 m map, ~10 m patch
+->Args({6400, 6400, 100}) // 320×320 m map, ~5 m patch
+->Args({6400, 6400, 200}) // 320×320 m map, ~10 m patch
+->Args({12800, 12800, 100}) // 640×640 m map, ~5 m patch
+->Args({12800, 12800, 200}) // 640×640 m map, ~10 m patch
+->Args({25600, 25600, 100}) // 1280×1280 m map, ~5 m patch
+->Args({25600, 25600, 200}) // 1280×1280 m map, ~10 m patch
+->Iterations(50)
+->Unit(benchmark::kMillisecond);
+
+// ---------------------------------------------------------------------------
+// Incremental: AsymmetricInflationLayer standalone
+// ---------------------------------------------------------------------------
+
+/**
+ * @brief Fixture for incremental updates: standalone asymmetric pipeline.
+ *
+ * Same structure as IncrementalInflationFixture but benchmarks
+ * AsymmetricInflationLayer, which internally runs the inherited symmetric
+ * baseline before the asymmetric overlay. A diagonal path is injected so the
+ * overlay asymmetric layer is active.
+ *
+ * Benchmark arguments:
+ * {width_cells, height_cells, patch_side_cells, fixed_path_length_m}
+ *
+ * fixed_path_length_m == 0 keeps the historical full-map diagonal path, so path
+ * length grows with map size. Non-zero values keep the path length fixed and
+ * center the dirty patch on that path, isolating global-map-size effects from
+ * path-length effects.
+ */
+class IncrementalAsymmetricFixture : public benchmark::Fixture
+{
+public:
+ void SetUp(benchmark::State & state) override
+ {
+ width_ = static_cast(state.range(0));
+ height_ = static_cast(state.range(1));
+ patch_ = static_cast(state.range(2));
+ fixed_path_length_m_ = static_cast(state.range(3));
+
+ auto opts = rclcpp::NodeOptions().parameter_overrides(
+ {
+ rclcpp::Parameter("asymmetric_inflation_layer.enabled", true),
+ rclcpp::Parameter("asymmetric_inflation_layer.inflation_radius", kInflationRadius),
+ rclcpp::Parameter("asymmetric_inflation_layer.inflate_unknown", false),
+ rclcpp::Parameter(
+ "asymmetric_inflation_layer.cost_scaling_factor_left", kCostScalingFactorLeft),
+ rclcpp::Parameter(
+ "asymmetric_inflation_layer.cost_scaling_factor_right", kCostScalingFactorRight),
+ rclcpp::Parameter("asymmetric_inflation_layer.inflate_around_unknown", false),
+ rclcpp::Parameter("asymmetric_inflation_layer.goal_distance_threshold", 1.5),
+ rclcpp::Parameter(
+ "asymmetric_inflation_layer.plan_topic", std::string("/benchmark_incr_asymmetric_plan")),
+ });
+ node_ = std::make_shared("incremental_asymmetric_benchmark", "", opts);
+
+ layers_ = std::make_unique(kGlobalFrame, false, false);
+ layers_->resizeMap(width_, height_, kResolution, 0.0, 0.0);
+
+ asym_layer_ = std::make_shared();
+ asym_layer_->initialize(
+ layers_.get(), "asymmetric_inflation_layer", nullptr, node_, nullptr);
+ layers_->addPlugin(std::static_pointer_cast(asym_layer_));
+
+ layers_->setFootprint(makeFootprint());
+ injectDiagonalPath();
+
+ // Pre-warm: full update so the layer starts in a valid post-update state.
+ auto * costmap = layers_->getCostmap();
+ generateRectangularObstacles(*costmap, kOccupancy);
+ asym_layer_->updateCosts(
+ *costmap, 0, 0, static_cast(width_), static_cast(height_));
+ }
+
+ void TearDown(benchmark::State &) override
+ {
+ asym_layer_.reset();
+ layers_.reset();
+ node_.reset();
+ }
+
+ unsigned int width_{0};
+ unsigned int height_{0};
+ unsigned int patch_{0};
+ double fixed_path_length_m_{0.0};
+ double actual_path_length_m_{0.0};
+ std::size_t path_pose_count_{0};
+ nav2::LifecycleNode::SharedPtr node_;
+ std::unique_ptr layers_;
+ std::shared_ptr asym_layer_;
+
+private:
+ void injectDiagonalPath()
+ {
+ const double width_m = static_cast(width_) * kResolution;
+ const double height_m = static_cast(height_) * kResolution;
+ const double diag = std::hypot(width_m, height_m);
+ double start_x = 0.0;
+ double start_y = 0.0;
+ double end_x = width_m;
+ double end_y = height_m;
+
+ if (fixed_path_length_m_ > 0.0) {
+ const double path_length = std::min(fixed_path_length_m_, diag * 0.95);
+ const double axis_delta = path_length / std::sqrt(2.0);
+ const double center_x = width_m * 0.5;
+ const double center_y = height_m * 0.5;
+ start_x = std::clamp(center_x - axis_delta * 0.5, 0.0, width_m);
+ start_y = std::clamp(center_y - axis_delta * 0.5, 0.0, height_m);
+ end_x = std::clamp(center_x + axis_delta * 0.5, 0.0, width_m);
+ end_y = std::clamp(center_y + axis_delta * 0.5, 0.0, height_m);
+ }
+
+ actual_path_length_m_ = std::hypot(end_x - start_x, end_y - start_y);
+ const double step = 0.5;
+ const int n = std::max(2, static_cast(std::ceil(actual_path_length_m_ / step)) + 1);
+
+ auto path = std::make_shared();
+ path->header.frame_id = kGlobalFrame;
+ for (int i = 0; i < n; ++i) {
+ double t = static_cast(i) / static_cast(n - 1);
+ geometry_msgs::msg::PoseStamped ps;
+ ps.header.frame_id = kGlobalFrame;
+ ps.pose.position.x = start_x + t * (end_x - start_x);
+ ps.pose.position.y = start_y + t * (end_y - start_y);
+ ps.pose.orientation.w = 1.0;
+ path->poses.push_back(ps);
+ }
+ path_pose_count_ = path->poses.size();
+ asym_layer_->injectPath(path);
+ }
+};
+
+BENCHMARK_DEFINE_F(IncrementalAsymmetricFixture, UpdateCosts)(benchmark::State & state)
+{
+ auto * costmap = layers_->getCostmap();
+ const unsigned int max_ox = (width_ > patch_) ? width_ - patch_ : 0u;
+ const unsigned int max_oy = (height_ > patch_) ? height_ - patch_ : 0u;
+ unsigned int step = 0;
+
+ for (auto _ : state) {
+ state.PauseTiming();
+ const bool fixed_path = fixed_path_length_m_ > 0.0;
+ const unsigned int ox = fixed_path ?
+ ((width_ > patch_) ? (width_ - patch_) / 2u : 0u) :
+ (max_ox ? (step * 37u) % max_ox : 0u);
+ const unsigned int oy = fixed_path ?
+ ((height_ > patch_) ? (height_ - patch_) / 2u : 0u) :
+ (max_oy ? (step * 53u) % max_oy : 0u);
+ modifyPatch(*costmap, ox, oy, patch_, kObstacleSeed + step);
+ ++step;
+ state.ResumeTiming();
+
+ asym_layer_->updateCosts(
+ *costmap,
+ static_cast(ox), static_cast(oy),
+ static_cast(ox + patch_), static_cast(oy + patch_));
+ }
+
+ state.counters["cells"] = static_cast(width_) * static_cast(height_);
+ state.counters["patch_side_m"] = static_cast(patch_) * kResolution;
+ state.counters["map_side_m"] = static_cast(width_) * kResolution;
+ state.counters["fixed_path_length_m"] = fixed_path_length_m_;
+ state.counters["actual_path_length_m"] = actual_path_length_m_;
+ state.counters["path_poses"] = static_cast(path_pose_count_);
+}
+
+BENCHMARK_REGISTER_F(IncrementalAsymmetricFixture, UpdateCosts)
+->Args({800, 800, 100, 0}) // 40×40 m map, ~5 m patch, full diagonal path
+->Args({800, 800, 200, 0}) // 40×40 m map, ~10 m patch, full diagonal path
+->Args({1600, 1600, 100, 0}) // 80×80 m map, ~5 m patch, full diagonal path
+->Args({1600, 1600, 200, 0}) // 80×80 m map, ~10 m patch, full diagonal path
+->Args({3200, 3200, 100, 0}) // 160×160 m map, ~5 m patch, full diagonal path
+->Args({3200, 3200, 200, 0}) // 160×160 m map, ~10 m patch, full diagonal path
+->Args({6400, 6400, 100, 0}) // 320×320 m map, ~5 m patch, full diagonal path
+->Args({6400, 6400, 200, 0}) // 320×320 m map, ~10 m patch, full diagonal path
+->Args({12800, 12800, 100, 0}) // 640×640 m map, ~5 m patch, full diagonal path
+->Args({12800, 12800, 200, 0}) // 640×640 m map, ~10 m patch, full diagonal path
+->Args({25600, 25600, 100, 0}) // 1280×1280 m map, ~5 m patch, full diagonal path
+->Args({25600, 25600, 200, 0}) // 1280×1280 m map, ~10 m patch, full diagonal path
+->Args({800, 800, 100, 20}) // 40×40 m map, ~5 m patch, fixed 20 m path
+->Args({1600, 1600, 100, 20}) // 80×80 m map, ~5 m patch, fixed 20 m path
+->Args({3200, 3200, 100, 20}) // 160×160 m map, ~5 m patch, fixed 20 m path
+->Args({6400, 6400, 100, 20}) // 320×320 m map, ~5 m patch, fixed 20 m path
+->Args({12800, 12800, 100, 20}) // 640×640 m map, ~5 m patch, fixed 20 m path
+->Args({25600, 25600, 100, 20}) // 1280×1280 m map, ~5 m patch, fixed 20 m path
+->Iterations(50)
+->Unit(benchmark::kMillisecond);
+
+// ---------------------------------------------------------------------------
+// main
+// ---------------------------------------------------------------------------
+
+int main(int argc, char ** argv)
+{
+ rclcpp::init(argc, argv);
+
+ auto ret =
+ rcutils_logging_set_logger_level("inflation_benchmark", RCUTILS_LOG_SEVERITY_ERROR);
+ ret = rcutils_logging_set_logger_level("legacy_inflation_benchmark",
+ RCUTILS_LOG_SEVERITY_ERROR);
+ ret = rcutils_logging_set_logger_level("asymmetric_benchmark", RCUTILS_LOG_SEVERITY_ERROR);
+ ret = rcutils_logging_set_logger_level("incremental_inflation_benchmark",
+ RCUTILS_LOG_SEVERITY_ERROR);
+ ret = rcutils_logging_set_logger_level("incremental_legacy_inflation_benchmark",
+ RCUTILS_LOG_SEVERITY_ERROR);
+ ret = rcutils_logging_set_logger_level("incremental_asymmetric_benchmark",
+ RCUTILS_LOG_SEVERITY_ERROR);
+ ret = rcutils_logging_set_logger_level("rclcpp_lifecycle", RCUTILS_LOG_SEVERITY_ERROR);
+ (void)ret;
+
+ benchmark::Initialize(&argc, argv);
+ if (benchmark::ReportUnrecognizedArguments(argc, argv)) {
+ rclcpp::shutdown();
+ return 1;
+ }
+ benchmark::RunSpecifiedBenchmarks();
+
+ rclcpp::shutdown();
+ return 0;
+}
diff --git a/nav2_costmap_2d/test/integration/CMakeLists.txt b/nav2_costmap_2d/test/integration/CMakeLists.txt
index b541f6c9c37..9d8de6ba6fd 100644
--- a/nav2_costmap_2d/test/integration/CMakeLists.txt
+++ b/nav2_costmap_2d/test/integration/CMakeLists.txt
@@ -31,6 +31,14 @@ target_link_libraries(legacy_inflation_tests_exec
layers
)
+ament_add_gtest_executable(asymmetric_inflation_tests_exec
+ asymmetric_inflation_tests.cpp
+)
+target_link_libraries(asymmetric_inflation_tests_exec
+ nav2_costmap_2d_core
+ layers
+)
+
nav2_add_gtest(obstacle_tests
obstacle_tests.cpp
)
@@ -100,6 +108,16 @@ nav2_add_test(inflation_tests
TEST_EXECUTABLE=$
)
+nav2_add_test(asymmetric_inflation_tests
+ GENERATE_RESULT_FOR_RETURN_CODE_ZERO
+ COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/costmap_tests_launch.py"
+ WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
+ ENV
+ TEST_MAP=${TEST_MAP_DIR}/TenByTen.yaml
+ TEST_LAUNCH_DIR=${TEST_LAUNCH_DIR}
+ TEST_EXECUTABLE=$
+)
+
nav2_add_test(plugin_container_tests
GENERATE_RESULT_FOR_RETURN_CODE_ZERO
COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/costmap_tests_launch.py"
diff --git a/nav2_costmap_2d/test/integration/asymmetric_inflation_tests.cpp b/nav2_costmap_2d/test/integration/asymmetric_inflation_tests.cpp
new file mode 100644
index 00000000000..17b46fa6c50
--- /dev/null
+++ b/nav2_costmap_2d/test/integration/asymmetric_inflation_tests.cpp
@@ -0,0 +1,228 @@
+// Copyright (c) 2026 Marc Blöchlinger
+//
+// 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.
+
+// Integration tests for AsymmetricInflationLayer.
+//
+// These tests stand up a minimal LayeredCostmap with the asymmetric layer
+// attached and exercise updateCosts() on synthetic obstacle layouts.
+
+#include
+
+#include
+#include
+#include
+#include
+#include
+
+#include "rclcpp/rclcpp.hpp"
+#include "tf2_ros/buffer.hpp"
+#include "nav2_ros_common/lifecycle_node.hpp"
+#include "nav2_costmap_2d/costmap_2d.hpp"
+#include "nav2_costmap_2d/cost_values.hpp"
+#include "nav2_costmap_2d/inflation_layer_interface.hpp"
+#include "nav2_costmap_2d/layered_costmap.hpp"
+#include "nav2_costmap_2d/asymmetric_inflation_layer.hpp"
+#include "geometry_msgs/msg/point.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "nav_msgs/msg/path.hpp"
+
+using geometry_msgs::msg::Point;
+
+class TestableAsymmetricInflationLayer : public nav2_costmap_2d::AsymmetricInflationLayer
+{
+public:
+ void injectPath(const nav_msgs::msg::Path::SharedPtr msg)
+ {
+ globalPathCallback(msg);
+ }
+
+ void setSideFactors(double left, double right)
+ {
+ cost_scaling_factor_left_ = left;
+ cost_scaling_factor_right_ = right;
+ cost_scaling_factor_ = std::max(left, right);
+ need_reinflation_ = true;
+ setCurrent(false);
+ matchSize();
+ }
+};
+
+class RclCppFixture
+{
+public:
+ RclCppFixture() {rclcpp::init(0, nullptr);}
+ ~RclCppFixture() {rclcpp::shutdown();}
+};
+RclCppFixture g_rclcpp_fixture;
+
+class AsymmetricInflationIntegrationTest : public ::testing::Test
+{
+protected:
+ void SetUp() override
+ {
+ // Lifecycle node with the parameters the layer expects.
+ auto options = rclcpp::NodeOptions();
+ options.parameter_overrides(
+ {
+ {"asymmetric_inflation_layer.enabled", true},
+ {"asymmetric_inflation_layer.inflation_radius", 0.55},
+ {"asymmetric_inflation_layer.cost_scaling_factor_left", 10.0},
+ {"asymmetric_inflation_layer.cost_scaling_factor_right", 10.0},
+ {"asymmetric_inflation_layer.inflate_unknown", false},
+ {"asymmetric_inflation_layer.inflate_around_unknown", false},
+ {"asymmetric_inflation_layer.plan_topic", std::string("plan")},
+ {"asymmetric_inflation_layer.goal_distance_threshold", 1.5},
+ {"map_topic", std::string("map")},
+ {"track_unknown_space", false},
+ {"use_maximum", false},
+ {"lethal_cost_threshold", 100},
+ {"unknown_cost_value", static_cast(0xff)},
+ {"trinary_costmap", true},
+ {"transform_tolerance", 0.3},
+ {"observation_sources", std::string("")},
+ });
+
+ node_ = std::make_shared(
+ "asymmetric_inflation_test_node", "", options);
+
+ tf_ = std::make_shared(node_->get_clock());
+
+ // 40x40 cells, 0.1m resolution, origin at (0,0) — world range [0, 4]m.
+ layers_ = std::make_shared("map", false, false);
+ layers_->resizeMap(40, 40, 0.1, 0.0, 0.0);
+
+ // Give the layered costmap a small square footprint — inscribed ≈ 0.1m.
+ std::vector footprint;
+ Point p;
+ p.x = 0.1; p.y = 0.1; footprint.push_back(p);
+ p.x = 0.1; p.y = -0.1; footprint.push_back(p);
+ p.x = -0.1; p.y = -0.1; footprint.push_back(p);
+ p.x = -0.1; p.y = 0.1; footprint.push_back(p);
+ layers_->setFootprint(footprint);
+
+ layer_ = std::make_shared();
+ layer_->initialize(
+ layers_.get(), "asymmetric_inflation_layer", tf_.get(), node_, nullptr);
+ layers_->addPlugin(std::static_pointer_cast(layer_));
+ }
+
+ void seedLethalObstacle(unsigned int mx, unsigned int my)
+ {
+ layers_->getCostmap()->setCost(mx, my, nav2_costmap_2d::LETHAL_OBSTACLE);
+ }
+
+ void runInflation()
+ {
+ // Run the standalone layer on the full map.
+ auto * costmap = layers_->getCostmap();
+ unsigned int w = costmap->getSizeInCellsX();
+ unsigned int h = costmap->getSizeInCellsY();
+ layer_->updateCosts(*costmap, 0, 0, w, h);
+ }
+
+ void injectStraightPath()
+ {
+ auto path = std::make_shared();
+ path->header.frame_id = "map";
+
+ for (double x : {0.5, 1.5, 2.5, 3.5}) {
+ geometry_msgs::msg::PoseStamped pose;
+ pose.header.frame_id = "map";
+ pose.pose.position.x = x;
+ pose.pose.position.y = 2.0;
+ path->poses.push_back(pose);
+ }
+
+ layer_->injectPath(path);
+ }
+
+ nav2::LifecycleNode::SharedPtr node_;
+ std::shared_ptr tf_;
+ std::shared_ptr layers_;
+ std::shared_ptr layer_;
+};
+
+// Test: without a published path the layer must behave symmetrically
+// (the no-asymmetry short-circuit in updateCosts skips the BFS because
+// extractLocalPath returns empty).
+TEST_F(AsymmetricInflationIntegrationTest, no_path_produces_symmetric_output)
+{
+ seedLethalObstacle(20, 20);
+
+ // Kick the layer — no path has been published.
+ runInflation();
+
+ auto * costmap = layers_->getCostmap();
+ // With no path, the two horizontally-mirrored cells around the obstacle must
+ // receive identical costs (Pass 1 only, which is symmetric).
+ unsigned char left = costmap->getCost(15, 20);
+ unsigned char right = costmap->getCost(25, 20);
+ EXPECT_EQ(left, right) << "Without a path, costs must be symmetric";
+ EXPECT_GT(left, nav2_costmap_2d::FREE_SPACE)
+ << "Standalone asymmetric layer must still write the symmetric baseline";
+}
+
+// Test: with cost_scaling_factor_left == cost_scaling_factor_right the layer
+// is a no-op overlay even when a path is available, so the costmap stays symmetric.
+TEST_F(AsymmetricInflationIntegrationTest, equal_sides_produce_symmetric_output)
+{
+ // Both sides set to 10.0 in SetUp() — equal decay rates short-circuit the BFS.
+ injectStraightPath();
+ seedLethalObstacle(20, 20);
+ seedLethalObstacle(20, 30); // second obstacle so we can also check vertical symmetry
+
+ runInflation();
+
+ auto * costmap = layers_->getCostmap();
+ // Horizontal symmetry across x = 20
+ for (int dy = -5; dy <= 5; ++dy) {
+ unsigned char left = costmap->getCost(15, 20 + dy);
+ unsigned char right = costmap->getCost(25, 20 + dy);
+ EXPECT_EQ(left, right) << "Asymmetry with equal sides (y_off=" << dy << ")";
+ }
+}
+
+TEST_F(AsymmetricInflationIntegrationTest, unequal_sides_raise_costs_from_standalone_layer)
+{
+ layer_->setSideFactors(1.0, 10.0);
+ injectStraightPath();
+
+ seedLethalObstacle(20, 24); // left/north of the path, within inflation radius
+ seedLethalObstacle(20, 16); // right/south of the path, within inflation radius
+
+ runInflation();
+
+ auto * costmap = layers_->getCostmap();
+ unsigned char left_side_cost = costmap->getCost(20, 28);
+ unsigned char right_side_cost = costmap->getCost(20, 12);
+ EXPECT_GT(left_side_cost, right_side_cost)
+ << "The smaller left-side decay factor should raise costs above the symmetric baseline";
+}
+
+TEST_F(AsymmetricInflationIntegrationTest, implements_inflation_layer_interface)
+{
+ std::shared_ptr base_layer = layer_;
+ auto inflation_layer =
+ std::dynamic_pointer_cast(base_layer);
+
+ ASSERT_NE(inflation_layer, nullptr);
+ EXPECT_DOUBLE_EQ(inflation_layer->getInflationRadius(), 0.55);
+ EXPECT_DOUBLE_EQ(inflation_layer->getCostScalingFactor(), 10.0);
+}
+
+int main(int argc, char ** argv)
+{
+ ::testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/nav2_costmap_2d/test/unit/CMakeLists.txt b/nav2_costmap_2d/test/unit/CMakeLists.txt
index 621b4e8b119..15078f170a2 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(asymmetric_inflation_layer_test asymmetric_inflation_layer_test.cpp)
+target_link_libraries(asymmetric_inflation_layer_test
+ nav2_costmap_2d_core
+ layers
+)
diff --git a/nav2_costmap_2d/test/unit/asymmetric_inflation_layer_test.cpp b/nav2_costmap_2d/test/unit/asymmetric_inflation_layer_test.cpp
new file mode 100644
index 00000000000..a87bb5fd6d9
--- /dev/null
+++ b/nav2_costmap_2d/test/unit/asymmetric_inflation_layer_test.cpp
@@ -0,0 +1,274 @@
+// Copyright (c) 2026 Marc Blöchlinger
+//
+// 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.
+
+// Unit tests for AsymmetricInflationLayer — pure-algorithm coverage.
+//
+// Tests exercise cost_lut_disfavored_ and computeObstacleSide via a test-subclass
+// that exposes protected members and methods without requiring a full
+// LayeredCostmap / LifecycleNode stack.
+
+#include
+
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+#include "nav2_costmap_2d/costmap_2d.hpp"
+#include "nav2_costmap_2d/asymmetric_inflation_layer.hpp"
+
+namespace nav2_costmap_2d
+{
+
+class TestableAsymmetricInflationLayer : public AsymmetricInflationLayer
+{
+public:
+ // Expose protected methods and members for testing
+ using AsymmetricInflationLayer::computeObstacleSide;
+ using AsymmetricInflationLayer::cost_lut_disfavored_;
+
+ // Plain setters so tests can configure the math without going through ROS init
+ void setResolution(double r) {resolution_ = r;}
+ void setInscribedRadius(double r) {inscribed_radius_ = r;}
+ void setInflationRadius(double r) {inflation_radius_ = r;}
+ void setCostScalingFactorLeft(double c)
+ {
+ cost_scaling_factor_left_ = c;
+ updateCostScalingFactor();
+ }
+ void setCostScalingFactorRight(double c)
+ {
+ cost_scaling_factor_right_ = c;
+ updateCostScalingFactor();
+ }
+ void setCellInflationRadius(unsigned int r) {cell_inflation_radius_ = r;}
+ void rebuildCaches() {computeAsymmetricCaches();}
+
+private:
+ void updateCostScalingFactor()
+ {
+ cost_scaling_factor_ = std::max(cost_scaling_factor_left_, cost_scaling_factor_right_);
+ }
+};
+
+} // namespace nav2_costmap_2d
+
+using nav2_costmap_2d::TestableAsymmetricInflationLayer;
+
+// Test 1: cost_lut_disfavored_ is built with c_side (the smaller scaling factor).
+// Verifies the mathematical equivalence: using c_max on effective distance equals
+// using c_side on physical distance, so the LUT must reflect c_side.
+TEST(DisfavoredLutTest, lut_uses_c_side_scaling_factor)
+{
+ auto layer = std::make_unique();
+ // resolution=0.1m, inscribed_radius=0.3m (3 cells), cell_inflation_radius=20 cells
+ layer->setResolution(0.1);
+ layer->setInscribedRadius(0.3);
+ layer->setInflationRadius(2.0);
+ layer->setCellInflationRadius(20);
+
+ // c_left=1, c_right=7 → c_side=1 (left is disfavored), c_max=7
+ layer->setCostScalingFactorLeft(1.0);
+ layer->setCostScalingFactorRight(7.0);
+ layer->rebuildCaches();
+
+ ASSERT_FALSE(layer->cost_lut_disfavored_.empty());
+
+ const double resolution = 0.1;
+ const double inscribed_radius = 0.3;
+ const double c_side = 1.0;
+ const int lut_precision = 100; // COST_LUT_PRECISION
+
+ // Spot-check several distances beyond the inscribed radius
+ for (int d_scaled = lut_precision * 4; d_scaled <= lut_precision * 19;
+ d_scaled += lut_precision)
+ {
+ double distance = static_cast(d_scaled) / lut_precision; // cells
+ double factor = exp(-c_side * (distance * resolution - inscribed_radius));
+ unsigned char expected =
+ static_cast((nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE - 1) * factor);
+
+ EXPECT_EQ(layer->cost_lut_disfavored_[d_scaled], expected)
+ << "LUT mismatch at d_scaled=" << d_scaled;
+ }
+
+ // Inside the inscribed radius: costs must equal INSCRIBED_INFLATED_OBSTACLE
+ for (int d_scaled = 1; d_scaled < lut_precision * 3; ++d_scaled) {
+ EXPECT_EQ(
+ layer->cost_lut_disfavored_[d_scaled],
+ nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
+ << "Expected INSCRIBED at d_scaled=" << d_scaled;
+ }
+
+ // At distance=0: must be LETHAL_OBSTACLE
+ EXPECT_EQ(layer->cost_lut_disfavored_[0], nav2_costmap_2d::LETHAL_OBSTACLE);
+}
+
+// Test 2: swapping side factors rebuilds the LUT with the new c_side.
+TEST(DisfavoredLutTest, lut_reflects_updated_c_side)
+{
+ auto layer = std::make_unique();
+ layer->setResolution(0.1);
+ layer->setInscribedRadius(0.3);
+ layer->setCellInflationRadius(10);
+ layer->setCostScalingFactorLeft(1.0);
+ layer->setCostScalingFactorRight(7.0);
+ layer->rebuildCaches();
+
+ std::vector lut_c1 = layer->cost_lut_disfavored_;
+
+ // Swap sides: now c_right=1 is disfavored, c_left=7 is favored
+ layer->setCostScalingFactorLeft(7.0);
+ layer->setCostScalingFactorRight(1.0);
+ layer->rebuildCaches();
+
+ // With both cases having c_side=1, LUTs should be identical
+ EXPECT_EQ(lut_c1, layer->cost_lut_disfavored_);
+}
+
+class ObstacleSideTest : public ::testing::Test
+{
+protected:
+ void SetUp() override
+ {
+ // 40x40 cells, 0.1m resolution, origin at (0,0) — world range [0, 4]m.
+ costmap_ = std::make_unique(40, 40, 0.1, 0.0, 0.0);
+ layer_ = std::make_unique();
+ layer_->setResolution(0.1);
+ layer_->setInscribedRadius(0.3);
+ layer_->setInflationRadius(1.0);
+ // Side classification doesn't depend on the decay rates; pick any unequal pair.
+ layer_->setCostScalingFactorLeft(2.0);
+ layer_->setCostScalingFactorRight(8.0);
+ }
+
+ // Helper method to simulate the spatial hash construction from updateCosts
+ std::unordered_map> buildSpatialHash(
+ const std::vector> & path,
+ double bucket_size)
+ {
+ std::unordered_map