diff --git a/nav2_costmap_2d/CMakeLists.txt b/nav2_costmap_2d/CMakeLists.txt
index 00cb720aa15..5bfaa70bc90 100644
--- a/nav2_costmap_2d/CMakeLists.txt
+++ b/nav2_costmap_2d/CMakeLists.txt
@@ -73,6 +73,7 @@ add_library(layers SHARED
   plugins/range_sensor_layer.cpp
   plugins/denoise_layer.cpp
   plugins/plugin_container_layer.cpp
+  plugins/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 6d2bde4a1c3..5647cfb91ee 100644
--- a/nav2_costmap_2d/costmap_plugins.xml
+++ b/nav2_costmap_2d/costmap_plugins.xml
@@ -12,6 +12,9 @@
     <class type="nav2_costmap_2d::VoxelLayer"     base_class_type="nav2_costmap_2d::Layer">
       <description>Similar to obstacle costmap, but uses 3D voxel grid to store data.</description>
     </class>
+    <class type="nav2_costmap_2d::TrackingErrorLayer"    base_class_type="nav2_costmap_2d::Layer">
+      <description>Creates a corridor around the path.</description>
+    </class>
     <class type="nav2_costmap_2d::RangeSensorLayer" base_class_type="nav2_costmap_2d::Layer">
       <description>A range-sensor (sonar, IR) based obstacle layer for costmap_2d</description>
     </class>
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/tracking_error_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/tracking_error_layer.hpp
new file mode 100644
index 00000000000..2a286217a8f
--- /dev/null
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/tracking_error_layer.hpp
@@ -0,0 +1,91 @@
+// 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__TRACKING_ERROR_LAYER_HPP_
+#define NAV2_COSTMAP_2D__TRACKING_ERROR_LAYER_HPP_
+
+#include <vector>
+#include <memory>
+#include <mutex>
+
+#include "nav2_costmap_2d/layered_costmap.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "nav2_costmap_2d/layer.hpp"
+#include "nav_msgs/msg/path.hpp"
+#include "nav2_msgs/msg/tracking_feedback.hpp"
+#include "nav2_costmap_2d/costmap_layer.hpp"
+#include "nav2_util/path_utils.hpp"
+#include "nav2_util/geometry_utils.hpp"
+#include "nav2_util/robot_utils.hpp"
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#include "tf2_ros/buffer.hpp"
+#include "tf2_ros/transform_listener.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+
+
+namespace nav2_costmap_2d
+{
+
+class TrackingErrorLayer : public nav2_costmap_2d::CostmapLayer
+{
+public:
+  TrackingErrorLayer();
+
+  ~TrackingErrorLayer();
+  virtual void onInitialize();
+  virtual void updateBounds(
+    double robot_x, double robot_y, double robot_yaw, double * min_x,
+    double * min_y, double * max_x, double * max_y);
+  virtual void updateCosts(
+    nav2_costmap_2d::Costmap2D & master_grid,
+    int min_i, int min_j, int max_i, int max_j);
+  virtual void reset();
+  virtual void onFootprintChanged();
+  virtual bool isClearable() {return false;}
+
+  // Lifecycle methods
+  virtual void activate();
+  virtual void deactivate();
+  virtual void cleanup();
+  std::vector<std::vector<double>> getWallPoints(const nav_msgs::msg::Path & segment);
+  nav_msgs::msg::Path getPathSegment();
+
+protected:
+  void pathCallback(const nav_msgs::msg::Path::SharedPtr msg);
+  void trackingCallback(const nav2_msgs::msg::TrackingFeedback::SharedPtr msg);
+  nav2_msgs::msg::TrackingFeedback last_tracking_feedback_;
+  rcl_interfaces::msg::SetParametersResult
+  dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters);
+
+private:
+  nav2::Subscription<nav_msgs::msg::Path>::SharedPtr path_sub_;
+  nav2::Subscription<nav2_msgs::msg::TrackingFeedback>::SharedPtr tracking_feedback_sub_;
+  std::mutex path_mutex_;
+  std::mutex tracking_error_mutex_;
+  nav_msgs::msg::Path last_path_;
+  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
+
+  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
+  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
+
+  size_t temp_step_;
+  int step_;
+  double width_;
+  double look_ahead_;
+  bool enabled_;
+};
+
+}  // namespace nav2_costmap_2d
+
+#endif  // NAV2_COSTMAP_2D__TRACKING_ERROR_LAYER_HPP_
diff --git a/nav2_costmap_2d/plugins/tracking_error_layer.cpp b/nav2_costmap_2d/plugins/tracking_error_layer.cpp
new file mode 100644
index 00000000000..dab13229e7a
--- /dev/null
+++ b/nav2_costmap_2d/plugins/tracking_error_layer.cpp
@@ -0,0 +1,361 @@
+// 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/tracking_error_layer.hpp"
+#include "pluginlib/class_list_macros.hpp"
+
+PLUGINLIB_EXPORT_CLASS(nav2_costmap_2d::TrackingErrorLayer, nav2_costmap_2d::Layer)
+
+namespace nav2_costmap_2d
+{
+
+TrackingErrorLayer::TrackingErrorLayer() {}
+
+void TrackingErrorLayer::onInitialize()
+{
+  auto node = node_.lock();
+  if (!node) {
+    throw std::runtime_error{"Failed to lock node"};
+  }
+
+  // Declare and get parameters
+  declareParameter("look_ahead", rclcpp::ParameterValue(5.0));
+  node->get_parameter(name_ + "." + "look_ahead", look_ahead_);
+
+  declareParameter("step", rclcpp::ParameterValue(5));
+  node->get_parameter(name_ + "." + "step", step_);
+  temp_step_ = static_cast<size_t>(step_);
+
+  declareParameter("corridor_width", rclcpp::ParameterValue(2.0));
+  node->get_parameter(name_ + "." + "corridor_width", width_);
+
+  declareParameter("enabled", rclcpp::ParameterValue(true));
+  node->get_parameter(name_ + "." + "enabled", enabled_);
+  if (look_ahead_ <= 0.0) {
+    throw std::runtime_error{"look_ahead must be positive"};
+  }
+  if (width_ <= 0.0) {
+    throw std::runtime_error{"corridor_width must be positive"};
+  }
+  if (temp_step_ == 0) {
+    throw std::runtime_error{"step must be greater than zero"};
+  }
+  dyn_params_handler_ = node->add_on_set_parameters_callback(
+    std::bind(
+      &TrackingErrorLayer::dynamicParametersCallback,
+      this, std::placeholders::_1));
+
+  path_sub_ = node->create_subscription<nav_msgs::msg::Path>(
+    "/plan",
+    std::bind(&TrackingErrorLayer::pathCallback, this, std::placeholders::_1),
+    rclcpp::QoS(10).reliable()
+  );
+
+  tracking_feedback_sub_ = node->create_subscription<nav2_msgs::msg::TrackingFeedback>(
+    "/tracking_feedback",
+    std::bind(&TrackingErrorLayer::trackingCallback, this, std::placeholders::_1),
+    rclcpp::QoS(10).reliable()
+  );
+  tf_buffer_ = std::make_shared<tf2_ros::Buffer>(node->get_clock());
+  tf_buffer_->setUsingDedicatedThread(true);
+  tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
+}
+
+void TrackingErrorLayer::pathCallback(const nav_msgs::msg::Path::SharedPtr msg)
+{
+  std::lock_guard<std::mutex> lock(path_mutex_);
+  last_path_ = *msg;
+}
+
+void TrackingErrorLayer::trackingCallback(
+  const nav2_msgs::msg::TrackingFeedback::SharedPtr msg)
+{
+  std::lock_guard<std::mutex> lock(tracking_error_mutex_);
+  last_tracking_feedback_ = *msg;
+}
+
+void TrackingErrorLayer::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<Costmap2D::mutex_t> guard(*getMutex());
+  if (!enabled_) {
+    return;
+  }
+  useExtraBounds(min_x, min_y, max_x, max_y);
+  *min_x = std::min(*min_x, robot_x - look_ahead_);
+  *max_x = std::max(*max_x, robot_x + look_ahead_);
+  *min_y = std::min(*min_y, robot_y - look_ahead_);
+  *max_y = std::max(*max_y, robot_y + look_ahead_);
+}
+
+std::vector<std::vector<double>> TrackingErrorLayer::getWallPoints(
+  const nav_msgs::msg::Path & segment)
+{
+  std::vector<std::vector<double>> point_list;
+
+  // Early return checks
+  if (segment.poses.empty() || temp_step_ == 0) {
+    return point_list;
+  }
+
+  // Reserve space for better performance (estimate 2 points per step)
+  size_t estimated_points = (segment.poses.size() / temp_step_) * 2;
+  point_list.reserve(estimated_points);
+
+  // Process path points with step size
+  for (size_t current_index = 0; current_index < segment.poses.size();
+    current_index += temp_step_)
+  {
+    const auto & current_pose = segment.poses[current_index];
+    double px = current_pose.pose.position.x;
+    double py = current_pose.pose.position.y;
+
+    // Calculate direction vector to next point
+    double dx = 0.0, dy = 0.0;
+
+    if (current_index + temp_step_ < segment.poses.size()) {
+      // Use next point at step distance
+      const auto & next_pose = segment.poses[current_index + temp_step_];
+      dx = next_pose.pose.position.x - px;
+      dy = next_pose.pose.position.y - py;
+    } else if (current_index + 1 < segment.poses.size()) {
+      // Use immediate next point if step would go out of bounds
+      const auto & next_pose = segment.poses[current_index + 1];
+      dx = next_pose.pose.position.x - px;
+      dy = next_pose.pose.position.y - py;
+    } else {
+      // Last point - use previous direction if available
+      if (current_index > 0) {
+        const auto & prev_pose = segment.poses[current_index - 1];
+        dx = px - prev_pose.pose.position.x;
+        dy = py - prev_pose.pose.position.y;
+      } else {
+        continue;
+      }
+    }
+
+    // Calculate perpendicular direction for wall points
+    double norm = std::hypot(dx, dy);
+    if (norm < 1e-6) {
+      continue;
+    }
+
+    double perp_x = -dy / norm;
+    double perp_y = dx / norm;
+
+    double half_width = width_ * 0.5;
+
+    point_list.push_back({px + perp_x * half_width, py + perp_y * half_width});
+    point_list.push_back({px - perp_x * half_width, py - perp_y * half_width});
+  }
+
+  return point_list;
+}
+
+
+nav_msgs::msg::Path TrackingErrorLayer::getPathSegment()
+{
+  std::lock_guard<std::mutex> path_lock(path_mutex_);
+  std::lock_guard<std::mutex> error_lock(tracking_error_mutex_);
+
+  nav_msgs::msg::Path segment;
+  if (last_path_.poses.empty() ||
+    last_tracking_feedback_.current_path_index >= last_path_.poses.size())
+  {
+    return segment;
+  }
+
+  size_t start_index = last_tracking_feedback_.current_path_index;
+  size_t end_index = start_index;
+  double dist_traversed = 0.0;
+
+  // Limit the corridor with look_ahead_
+  for (size_t i = start_index; i < last_path_.poses.size() - 1; ++i) {
+    dist_traversed += nav2_util::geometry_utils::euclidean_distance(
+      last_path_.poses[i], last_path_.poses[i + 1]);
+    end_index = i + 1;
+    if (dist_traversed >= look_ahead_) {
+      break;
+    }
+  }
+
+  // Create the forward corridor segment
+  if (start_index < end_index && end_index <= last_path_.poses.size()) {
+    segment.header = last_path_.header;
+    segment.poses.assign(
+      last_path_.poses.begin() + start_index,
+      last_path_.poses.begin() + end_index
+    );
+  } else if (start_index == last_path_.poses.size() - 1) {
+    segment.header = last_path_.header;
+    segment.poses.push_back(last_path_.poses.back());
+  }
+
+  return segment;
+}
+
+void TrackingErrorLayer::updateCosts(
+  nav2_costmap_2d::Costmap2D & master_grid,
+  int /*min_i*/, int /*min_j*/, int /*max_i*/, int /*max_j*/)
+{
+  std::lock_guard<Costmap2D::mutex_t> guard(*getMutex());
+
+  if (!enabled_) {
+    return;
+  }
+
+  nav_msgs::msg::Path segment = getPathSegment();
+  if (segment.poses.size() < 2) {
+    return;
+  }
+
+  std::string costmap_frame = layered_costmap_->getGlobalFrameID();
+  nav_msgs::msg::Path transformed_segment;
+  transformed_segment.header.frame_id = costmap_frame;
+
+  for (const auto & pose : segment.poses) {
+    geometry_msgs::msg::PoseStamped transformed_pose;
+    if (nav2_util::transformPoseInTargetFrame(
+          pose, transformed_pose, *tf_buffer_, costmap_frame, 0.1))
+    {
+      transformed_segment.poses.push_back(transformed_pose);
+    }
+  }
+
+  auto wall_points = getWallPoints(transformed_segment);
+
+  // Separate left and right wall points
+  std::vector<std::vector<double>> left_wall_points, right_wall_points;
+  for (size_t i = 0; i < wall_points.size(); i += 2) {
+    if (i + 1 < wall_points.size()) {
+      left_wall_points.push_back(wall_points[i]);  // Even indices = left
+      right_wall_points.push_back(wall_points[i + 1]);  // Odd indices = right
+    }
+  }
+
+  // Use Bresenham to get consistent lines
+  for (size_t i = 1; i < left_wall_points.size(); ++i) {
+    double x0 = left_wall_points[i - 1][0];
+    double y0 = left_wall_points[i - 1][1];
+    double x1 = left_wall_points[i][0];
+    double y1 = left_wall_points[i][1];
+
+    unsigned int map_x0, map_y0, map_x1, map_y1;
+    if (master_grid.worldToMap(x0, y0, map_x0, map_y0) &&
+      master_grid.worldToMap(x1, y1, map_x1, map_y1))
+    {
+      auto cells = nav2_util::geometry_utils::bresenhamLine(
+        static_cast<int>(map_x0), static_cast<int>(map_y0),
+        static_cast<int>(map_x1), static_cast<int>(map_y1));
+
+      for (const auto & cell : cells) {
+        master_grid.setCost(cell.first, cell.second, nav2_costmap_2d::LETHAL_OBSTACLE);
+      }
+    }
+  }
+
+  for (size_t i = 1; i < right_wall_points.size(); ++i) {
+    double x0 = right_wall_points[i - 1][0];
+    double y0 = right_wall_points[i - 1][1];
+    double x1 = right_wall_points[i][0];
+    double y1 = right_wall_points[i][1];
+
+    unsigned int map_x0, map_y0, map_x1, map_y1;
+    if (master_grid.worldToMap(x0, y0, map_x0, map_y0) &&
+      master_grid.worldToMap(x1, y1, map_x1, map_y1))
+    {
+      auto cells = nav2_util::geometry_utils::bresenhamLine(
+        static_cast<int>(map_x0), static_cast<int>(map_y0),
+        static_cast<int>(map_x1), static_cast<int>(map_y1));
+
+      for (const auto & cell : cells) {
+        master_grid.setCost(cell.first, cell.second, nav2_costmap_2d::LETHAL_OBSTACLE);
+      }
+    }
+  }
+}
+
+rcl_interfaces::msg::SetParametersResult TrackingErrorLayer::dynamicParametersCallback(
+  std::vector<rclcpp::Parameter> parameters)
+{
+  std::lock_guard<Costmap2D::mutex_t> guard(*getMutex());
+  rcl_interfaces::msg::SetParametersResult result;
+  result.successful = true;
+
+  for (auto parameter : parameters) {
+    const auto & param_type = parameter.get_type();
+    const auto & param_name = parameter.get_name();
+
+    // Only process parameters for this layer
+    if (param_name.find(name_ + ".") != 0) {
+      continue;
+    }
+
+    if (param_type == rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE) {
+      if (param_name == name_ + "." + "look_ahead") {
+        double new_value = parameter.as_double();
+        if (new_value <= 0.0) {
+          result.successful = false;
+          result.reason = "look_ahead must be positive";
+          return result;
+        }
+        look_ahead_ = new_value;
+      } else if (param_name == name_ + "." + "corridor_width") {
+        double new_value = parameter.as_double();
+        if (new_value <= 0.0) {
+          result.successful = false;
+          result.reason = "corridor_width must be positive";
+          return result;
+        }
+        width_ = new_value;
+      }
+    } else if (param_type == rcl_interfaces::msg::ParameterType::PARAMETER_BOOL) {
+      if (param_name == name_ + "." + "enabled") {
+        enabled_ = parameter.as_bool();
+      }
+    } else if (param_type == rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER) {
+      if (param_name == name_ + "." + "step") {
+        int new_value = parameter.as_int();
+        if (new_value <= 0) {
+          result.successful = false;
+          result.reason = "step must be greater than zero";
+          return result;
+        }
+        step_ = new_value;
+        temp_step_ = static_cast<size_t>(step_);
+      }
+    }
+  }
+
+  return result;
+}
+
+TrackingErrorLayer::~TrackingErrorLayer()
+{
+  auto node = node_.lock();
+  if (dyn_params_handler_ && node) {
+    node->remove_on_set_parameters_callback(dyn_params_handler_.get());
+  }
+  dyn_params_handler_.reset();
+}
+
+void TrackingErrorLayer::reset() {}
+void TrackingErrorLayer::activate() {enabled_ = true;}
+void TrackingErrorLayer::deactivate() {enabled_ = false;}
+
+void TrackingErrorLayer::onFootprintChanged() {}
+void TrackingErrorLayer::cleanup() {}
+
+}  // namespace nav2_costmap_2d
diff --git a/nav2_costmap_2d/test/unit/CMakeLists.txt b/nav2_costmap_2d/test/unit/CMakeLists.txt
index 559ef4ae4e2..9d9f3e9f959 100644
--- a/nav2_costmap_2d/test/unit/CMakeLists.txt
+++ b/nav2_costmap_2d/test/unit/CMakeLists.txt
@@ -67,3 +67,9 @@ ament_add_gtest(coordinate_transform_test coordinate_transform_test.cpp)
 target_link_libraries(coordinate_transform_test
   nav2_costmap_2d_core
 )
+
+ament_add_gtest(tracking_error_layer_test tracking_error_layer_test.cpp)
+target_link_libraries(tracking_error_layer_test
+  nav2_costmap_2d_core
+  layers
+)
diff --git a/nav2_costmap_2d/test/unit/tracking_error_layer_test.cpp b/nav2_costmap_2d/test/unit/tracking_error_layer_test.cpp
new file mode 100644
index 00000000000..167f4fbfa38
--- /dev/null
+++ b/nav2_costmap_2d/test/unit/tracking_error_layer_test.cpp
@@ -0,0 +1,422 @@
+// 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 <gtest/gtest.h>
+#include <memory>
+#include <string>
+#include <vector>
+#include <thread>
+#include <atomic>
+#include <cmath>
+
+#include "nav2_ros_common/lifecycle_node.hpp"
+#include "nav2_costmap_2d/tracking_error_layer.hpp"
+#include "nav2_costmap_2d/layered_costmap.hpp"
+#include "nav2_costmap_2d/costmap_2d.hpp"
+#include "nav_msgs/msg/path.hpp"
+#include "nav2_msgs/msg/tracking_feedback.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "rcutils/logging.h"
+// Test constants
+static constexpr double TEST_CORRIDOR_WIDTH = 2.0;
+static constexpr int TEST_PATH_POINTS = 50;
+static constexpr double TEST_PATH_LENGTH = 5.0;
+
+class TrackingErrorLayerWrapper : public nav2_costmap_2d::TrackingErrorLayer
+{
+public:
+  using TrackingErrorLayer::last_tracking_feedback_;
+  using TrackingErrorLayer::pathCallback;
+  using TrackingErrorLayer::trackingCallback;
+  using TrackingErrorLayer::getPathSegment;
+  using TrackingErrorLayer::getWallPoints;
+};
+
+class TrackingErrorLayerTestFixture : public ::testing::Test
+{
+public:
+  TrackingErrorLayerTestFixture()
+  {
+    node_ = std::make_shared<nav2::LifecycleNode>("test_tracking_error_layer");
+    node_->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE);
+    node_->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE);
+
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+    rclcpp::executors::SingleThreadedExecutor exec;
+    exec.spin_some();
+    costmap_ = std::make_shared<nav2_costmap_2d::Costmap2D>(100, 100, 0.05, -2.5, -2.5);
+    layered_costmap_ = std::make_shared<nav2_costmap_2d::LayeredCostmap>("map", false, false);
+    layered_costmap_->resizeMap(100, 100, 0.05, -2.5, -2.5);
+
+    layer_ = std::make_shared<TrackingErrorLayerWrapper>();
+    layer_->initialize(layered_costmap_.get(), "test_layer", nullptr, node_, nullptr);
+    layer_->last_tracking_feedback_.current_path_index = static_cast<uint32_t>(-1);
+  }
+
+  void SetUp() override
+  {
+    layer_->activate();
+  }
+
+  void TearDown() override
+  {
+    layer_->deactivate();
+    if (node_->get_current_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) {
+      node_->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE);
+      node_->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CLEANUP);
+      node_->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_UNCONFIGURED_SHUTDOWN);
+    }
+  }
+
+protected:
+  nav2::LifecycleNode::SharedPtr node_;
+  std::shared_ptr<nav2_costmap_2d::Costmap2D> costmap_;
+  std::shared_ptr<nav2_costmap_2d::LayeredCostmap> layered_costmap_;
+  std::shared_ptr<TrackingErrorLayerWrapper> layer_;
+
+  nav_msgs::msg::Path createStraightPath(
+    double start_x = 0.0, double start_y = 0.0,
+    double length = TEST_PATH_LENGTH, int num_points = TEST_PATH_POINTS)
+  {
+    nav_msgs::msg::Path path;
+    path.header.frame_id = "map";
+    path.header.stamp = node_->now();
+
+    for (int i = 0; i < num_points; ++i) {
+      geometry_msgs::msg::PoseStamped pose;
+      pose.header = path.header;
+      pose.pose.position.x = start_x + (i * length / num_points);
+      pose.pose.position.y = start_y;
+      pose.pose.position.z = 0.0;
+      pose.pose.orientation.w = 1.0;
+      path.poses.push_back(pose);
+    }
+    return path;
+  }
+
+  nav_msgs::msg::Path createCurvedPath()
+  {
+    nav_msgs::msg::Path path;
+    path.header.frame_id = "map";
+    path.header.stamp = node_->now();
+
+    for (int i = 0; i <= 50; ++i) {
+      geometry_msgs::msg::PoseStamped pose;
+      pose.header = path.header;
+      double angle = M_PI * i / 50.0;
+      pose.pose.position.x = 2.0 * cos(angle);
+      pose.pose.position.y = 2.0 * sin(angle);
+      pose.pose.position.z = 0.0;
+      pose.pose.orientation.w = 1.0;
+      path.poses.push_back(pose);
+    }
+    return path;
+  }
+
+  nav2_msgs::msg::TrackingFeedback createTrackingFeedback(
+    uint32_t path_index = 0,
+    double error = 0.1)
+  {
+    nav2_msgs::msg::TrackingFeedback feedback;
+    feedback.header.frame_id = "map";
+    feedback.header.stamp = node_->now();
+    feedback.current_path_index = path_index;
+    feedback.tracking_error = error;
+
+    feedback.robot_pose.header = feedback.header;
+    feedback.robot_pose.pose.position.x = path_index * 0.1;
+    feedback.robot_pose.pose.position.y = 0.0;
+    feedback.robot_pose.pose.orientation.w = 1.0;
+
+    return feedback;
+  }
+
+  bool hasObstacles(const nav2_costmap_2d::Costmap2D & costmap)
+  {
+    for (unsigned int i = 0; i < costmap.getSizeInCellsX(); ++i) {
+      for (unsigned int j = 0; j < costmap.getSizeInCellsY(); ++j) {
+        if (costmap.getCost(i, j) == nav2_costmap_2d::LETHAL_OBSTACLE) {
+          return true;
+        }
+      }
+    }
+    return false;
+  }
+};
+
+// Basic initialization and lifecycle tests
+TEST_F(TrackingErrorLayerTestFixture, test_initialization_and_parameters)
+{
+  EXPECT_TRUE(layer_ != nullptr);
+  EXPECT_FALSE(layer_->isClearable());
+  EXPECT_NO_THROW(layer_->activate());
+  EXPECT_NO_THROW(layer_->deactivate());
+}
+
+TEST_F(TrackingErrorLayerTestFixture, test_lifecycle_methods)
+{
+  EXPECT_NO_THROW(layer_->activate());
+  EXPECT_NO_THROW(layer_->deactivate());
+  EXPECT_NO_THROW(layer_->reset());
+  EXPECT_NO_THROW(layer_->onFootprintChanged());
+  EXPECT_NO_THROW(layer_->cleanup());
+}
+
+// Empty data handling tests
+TEST_F(TrackingErrorLayerTestFixture, test_empty_data_handling)
+{
+  nav2_costmap_2d::Costmap2D master_grid(100, 100, 0.05, -2.5, -2.5);
+  EXPECT_NO_THROW(layer_->updateCosts(master_grid, 0, 0, 100, 100));
+  EXPECT_FALSE(hasObstacles(master_grid));
+}
+
+TEST_F(TrackingErrorLayerTestFixture, test_path_segment_no_data)
+{
+  // Test segment extraction with no path
+  auto segment_no_path = layer_->getPathSegment();
+  EXPECT_TRUE(segment_no_path.poses.empty());
+
+  // Test with path but no tracking feedback
+  auto path = createStraightPath();
+  layer_->pathCallback(std::make_shared<nav_msgs::msg::Path>(path));
+  auto segment_no_tracking = layer_->getPathSegment();
+  EXPECT_TRUE(segment_no_tracking.poses.empty());
+}
+
+// Path segment extraction tests
+TEST_F(TrackingErrorLayerTestFixture, test_path_segment_extraction)
+{
+  auto path = createStraightPath();
+  auto tracking_error = createTrackingFeedback(10);
+  layer_->pathCallback(std::make_shared<nav_msgs::msg::Path>(path));
+  layer_->trackingCallback(std::make_shared<nav2_msgs::msg::TrackingFeedback>(tracking_error));
+
+  auto segment = layer_->getPathSegment();
+  EXPECT_FALSE(segment.poses.empty());
+  EXPECT_EQ(segment.header.frame_id, "map");
+  EXPECT_GT(segment.poses.size(), 0u);
+  EXPECT_GE(segment.poses[0].pose.position.x, tracking_error.robot_pose.pose.position.x);
+}
+
+TEST_F(TrackingErrorLayerTestFixture, test_path_segment_bounds)
+{
+  auto path = createStraightPath(0.0, 0.0, 5.0, 50);  // 50 points, 5m long
+  auto tracking_error = createTrackingFeedback(25);
+
+  layer_->pathCallback(std::make_shared<nav_msgs::msg::Path>(path));
+  layer_->trackingCallback(std::make_shared<nav2_msgs::msg::TrackingFeedback>(tracking_error));
+
+  auto segment = layer_->getPathSegment();
+
+  // Should extract segment around index 25
+  EXPECT_FALSE(segment.poses.empty());
+  EXPECT_LT(segment.poses.size(), path.poses.size());
+
+  // Check segment boundaries make sense
+  double first_x = segment.poses.front().pose.position.x;
+  double last_x = segment.poses.back().pose.position.x;
+  double robot_x = tracking_error.robot_pose.pose.position.x;
+
+  EXPECT_LE(first_x, robot_x);
+  EXPECT_GE(last_x, robot_x);
+}
+
+TEST_F(TrackingErrorLayerTestFixture, test_path_segment_edge_cases)
+{
+  auto path = createStraightPath(0.0, 0.0, 5.0, 50);
+
+  // Test near start of path
+  auto tracking_error_start = createTrackingFeedback(2);
+  layer_->pathCallback(std::make_shared<nav_msgs::msg::Path>(path));
+  layer_->trackingCallback(std::make_shared<nav2_msgs::msg::TrackingFeedback>(
+    tracking_error_start));
+  auto segment_start = layer_->getPathSegment();
+  EXPECT_FALSE(segment_start.poses.empty());
+
+  // Test near end of path
+  auto tracking_error_end = createTrackingFeedback(47);
+  layer_->trackingCallback(std::make_shared<nav2_msgs::msg::TrackingFeedback>(tracking_error_end));
+  auto segment_end = layer_->getPathSegment();
+  EXPECT_FALSE(segment_end.poses.empty());
+
+  // Test exactly at end
+  auto tracking_error_last = createTrackingFeedback(49);
+  layer_->trackingCallback(std::make_shared<nav2_msgs::msg::TrackingFeedback>(tracking_error_last));
+  auto segment_last = layer_->getPathSegment();
+  EXPECT_FALSE(segment_last.poses.empty());
+}
+
+TEST_F(TrackingErrorLayerTestFixture, test_path_segment_length_consistency)
+{
+  auto path = createStraightPath(0.0, 0.0, 10.0, 100);
+
+  // Test different positions, segment length should be consistent
+  std::vector<size_t> test_indices = {10, 25, 50, 75, 90};
+  std::vector<size_t> segment_sizes;
+
+  layer_->pathCallback(std::make_shared<nav_msgs::msg::Path>(path));
+
+  for (auto index : test_indices) {
+    auto tracking_error = createTrackingFeedback(index);
+    layer_->trackingCallback(std::make_shared<nav2_msgs::msg::TrackingFeedback>(tracking_error));
+    auto segment = layer_->getPathSegment();
+    segment_sizes.push_back(segment.poses.size());
+  }
+
+  // Segments should have similar sizes
+  for (size_t i = 1; i < segment_sizes.size() - 1; ++i) {
+    EXPECT_GT(segment_sizes[i], 0u);
+  }
+}
+
+TEST_F(TrackingErrorLayerTestFixture, test_invalid_path_index)
+{
+  auto path = createStraightPath();
+  auto tracking_error = createTrackingFeedback(1000);  // Invalid index
+  layer_->pathCallback(std::make_shared<nav_msgs::msg::Path>(path));
+  layer_->trackingCallback(std::make_shared<nav2_msgs::msg::TrackingFeedback>(tracking_error));
+  auto segment = layer_->getPathSegment();
+  EXPECT_TRUE(segment.poses.empty());
+}
+
+// Wall points generation tests
+TEST_F(TrackingErrorLayerTestFixture, test_wall_points_straight_path)
+{
+  auto path = createStraightPath();
+  auto wall_points = layer_->getWallPoints(path);
+  EXPECT_FALSE(wall_points.empty());
+  EXPECT_EQ(wall_points.size() % 2, 0u);  // Should be pairs (left/right)
+
+  for (const auto & point : wall_points) {
+    EXPECT_EQ(point.size(), 2u);  // [x, y]
+    EXPECT_TRUE(std::isfinite(point[0]));
+    EXPECT_TRUE(std::isfinite(point[1]));
+  }
+
+  if (wall_points.size() >= 2) {
+    double left_y = wall_points[0][1];
+    double right_y = wall_points[1][1];
+    EXPECT_TRUE((left_y > 0 && right_y < 0) || (left_y < 0 && right_y > 0));
+  }
+}
+
+TEST_F(TrackingErrorLayerTestFixture, test_wall_points_curved_path)
+{
+  auto curved_path = createCurvedPath();
+  auto wall_points = layer_->getWallPoints(curved_path);
+  EXPECT_FALSE(wall_points.empty());
+  EXPECT_EQ(wall_points.size() % 2, 0u);
+
+  if (wall_points.size() >= 4) {
+    double first_left_x = wall_points[0][0];
+    double second_left_x = wall_points[2][0];
+    EXPECT_NE(first_left_x, second_left_x);
+  }
+}
+
+TEST_F(TrackingErrorLayerTestFixture, test_single_point_path)
+{
+  nav_msgs::msg::Path single_point_path;
+  single_point_path.header.frame_id = "map";
+  single_point_path.header.stamp = node_->now();
+
+  geometry_msgs::msg::PoseStamped single_pose;
+  single_pose.header = single_point_path.header;
+  single_pose.pose.position.x = 1.0;
+  single_pose.pose.position.y = 1.0;
+  single_pose.pose.orientation.w = 1.0;
+  single_point_path.poses.push_back(single_pose);
+
+  auto wall_points = layer_->getWallPoints(single_point_path);
+  EXPECT_TRUE(wall_points.empty());
+}
+
+TEST_F(TrackingErrorLayerTestFixture, test_update_costs_same_frame)
+{
+  auto path = createStraightPath();
+  auto tracking_error = createTrackingFeedback(5);
+  layer_->pathCallback(std::make_shared<nav_msgs::msg::Path>(path));
+  layer_->trackingCallback(std::make_shared<nav2_msgs::msg::TrackingFeedback>(tracking_error));
+
+  nav2_costmap_2d::Costmap2D master_grid(100, 100, 0.05, -2.5, -2.5);
+  EXPECT_NO_THROW(layer_->updateCosts(master_grid, 0, 0, 100, 100));
+}
+
+TEST_F(TrackingErrorLayerTestFixture, test_obstacles_created_in_corridor)
+{
+  auto path = createStraightPath();
+  auto tracking_error = createTrackingFeedback(5);
+  layer_->pathCallback(std::make_shared<nav_msgs::msg::Path>(path));
+  layer_->trackingCallback(std::make_shared<nav2_msgs::msg::TrackingFeedback>(tracking_error));
+
+  nav2_costmap_2d::Costmap2D master_grid(100, 100, 0.05, -2.5, -2.5);
+  layer_->updateCosts(master_grid, 0, 0, 100, 100);
+
+  // Check that some obstacles were actually created
+  EXPECT_TRUE(hasObstacles(master_grid));
+}
+
+// Edge cases and robustness tests
+TEST_F(TrackingErrorLayerTestFixture, test_step_size_edge_cases)
+{
+  auto path = createStraightPath(0.0, 0.0, 1.0, 10);
+  auto wall_points = layer_->getWallPoints(path);
+  EXPECT_TRUE(wall_points.size() <= path.poses.size() * 2);
+}
+
+TEST_F(TrackingErrorLayerTestFixture, test_malformed_data)
+{
+  // Test with NaN coordinates
+  nav_msgs::msg::Path nan_path;
+  nan_path.header.frame_id = "map";
+  nan_path.header.stamp = node_->now();
+
+  geometry_msgs::msg::PoseStamped nan_pose;
+  nan_pose.header = nan_path.header;
+  nan_pose.pose.position.x = std::numeric_limits<double>::quiet_NaN();
+  nan_pose.pose.position.y = 0.0;
+  nan_pose.pose.orientation.w = 1.0;
+  nan_path.poses.push_back(nan_pose);
+
+  EXPECT_NO_THROW(layer_->getWallPoints(nan_path));
+}
+
+TEST_F(TrackingErrorLayerTestFixture, test_frame_transformation)
+{
+  // Create path in different frame
+  auto path = createStraightPath();
+  path.header.frame_id = "base_link";
+
+  auto tracking_error = createTrackingFeedback(5);
+  layer_->pathCallback(std::make_shared<nav_msgs::msg::Path>(path));
+  layer_->trackingCallback(std::make_shared<nav2_msgs::msg::TrackingFeedback>(tracking_error));
+
+  nav2_costmap_2d::Costmap2D master_grid(100, 100, 0.05, -2.5, -2.5);
+  // Should handle frame mismatch
+  EXPECT_NO_THROW(layer_->updateCosts(master_grid, 0, 0, 100, 100));
+}
+
+int main(int argc, char ** argv)
+{
+  if (rcutils_logging_set_logger_level("test_tracking_error_layer",
+    RCUTILS_LOG_SEVERITY_ERROR) != RCUTILS_RET_OK)
+    {
+      std::cerr << "Failed to set logger level" << std::endl;
+  }
+  rclcpp::init(argc, argv);
+  testing::InitGoogleTest(&argc, argv);
+  int result = RUN_ALL_TESTS();
+  rclcpp::shutdown();
+  return result;
+}
diff --git a/nav2_util/include/nav2_util/geometry_utils.hpp b/nav2_util/include/nav2_util/geometry_utils.hpp
index f4cf4078042..d12cb06cc49 100644
--- a/nav2_util/include/nav2_util/geometry_utils.hpp
+++ b/nav2_util/include/nav2_util/geometry_utils.hpp
@@ -17,6 +17,7 @@
 
 #include <cmath>
 #include <vector>
+#include <utility>
 
 #include "geometry_msgs/msg/pose.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
@@ -303,6 +304,33 @@ inline double cross_product_2d(
   return (path_vec_x * robot_vec_y) - (path_vec_y * robot_vec_x);
 }
 
+inline std::vector<std::pair<int, int>> bresenhamLine(int x0, int y0, int x1, int y1)
+{
+  std::vector<std::pair<int, int>> cells;
+  int dx = std::abs(x1 - x0);
+  int dy = std::abs(y1 - y0);
+  int sx = (x0 < x1) ? 1 : -1;
+  int sy = (y0 < y1) ? 1 : -1;
+  int err = dx - dy;
+
+  while (true) {
+    cells.push_back({x0, y0});
+    if (x0 == x1 && y0 == y1) {
+      break;
+    }
+    int e2 = 2 * err;
+    if (e2 > -dy) {
+      err -= dy;
+      x0 += sx;
+    }
+    if (e2 < dx) {
+      err += dx;
+      y0 += sy;
+    }
+  }
+  return cells;
+}
+
 }  // namespace geometry_utils
 }  // namespace nav2_util