From 79e8b6eda597da619a2e062ec7589550c62e8d0f Mon Sep 17 00:00:00 2001 From: silanus23 Date: Mon, 13 Oct 2025 16:50:08 +0300 Subject: [PATCH 01/10] Library addition Signed-off-by: silanus23 --- .../nav2_costmap_2d/tracking_error_layer.hpp | 92 +++++++++++++++++++ 1 file changed, 92 insertions(+) create mode 100644 nav2_costmap_2d/include/nav2_costmap_2d/tracking_error_layer.hpp 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..0864668d08c --- /dev/null +++ b/nav2_costmap_2d/include/nav2_costmap_2d/tracking_error_layer.hpp @@ -0,0 +1,92 @@ +// 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 +#include +#include + +#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 +#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> 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 parameters); + +private: + nav2::Subscription::SharedPtr path_sub_; + nav2::Subscription::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 tf_buffer_; + std::shared_ptr 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_ From 1decf953488db30e421244e18d56ff0f2834d649 Mon Sep 17 00:00:00 2001 From: silanus23 Date: Mon, 13 Oct 2025 16:50:38 +0300 Subject: [PATCH 02/10] source file addition Signed-off-by: silanus23 --- .../plugins/tracking_error_layer.cpp | 360 ++++++++++++++++++ 1 file changed, 360 insertions(+) create mode 100644 nav2_costmap_2d/plugins/tracking_error_layer.cpp 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..b583a0ce184 --- /dev/null +++ b/nav2_costmap_2d/plugins/tracking_error_layer.cpp @@ -0,0 +1,360 @@ +// 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(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( + "/plan", + std::bind(&TrackingErrorLayer::pathCallback, this, std::placeholders::_1), + rclcpp::QoS(10).reliable() + ); + tracking_feedback_sub_ = node->create_subscription( + "/tracking_feedback", + std::bind(&TrackingErrorLayer::trackingCallback, this, std::placeholders::_1), + rclcpp::QoS(10).reliable() + ); + tf_buffer_ = std::make_shared(node->get_clock()); + tf_buffer_->setUsingDedicatedThread(true); + tf_listener_ = std::make_shared(*tf_buffer_); +} + +void TrackingErrorLayer::pathCallback(const nav_msgs::msg::Path::SharedPtr msg) +{ + std::lock_guard lock(path_mutex_); + last_path_ = *msg; +} + +void TrackingErrorLayer::trackingCallback( + const nav2_msgs::msg::TrackingFeedback::SharedPtr msg) +{ + std::lock_guard 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 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> TrackingErrorLayer::getWallPoints( + const nav_msgs::msg::Path & segment) +{ + std::vector> 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 path_lock(path_mutex_); + std::lock_guard 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 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> 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(map_x0), static_cast(map_y0), + static_cast(map_x1), static_cast(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(map_x0), static_cast(map_y0), + static_cast(map_x1), static_cast(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 parameters) +{ + std::lock_guard 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(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 From e38e8a2fa8f57b2382903ffebe7b9ad5a2e3c4c2 Mon Sep 17 00:00:00 2001 From: silanus23 Date: Mon, 13 Oct 2025 16:50:59 +0300 Subject: [PATCH 03/10] test file added Signed-off-by: silanus23 --- .../test/unit/tracking_error_layer_test.cpp | 416 ++++++++++++++++++ 1 file changed, 416 insertions(+) create mode 100644 nav2_costmap_2d/test/unit/tracking_error_layer_test.cpp 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..324722e0621 --- /dev/null +++ b/nav2_costmap_2d/test/unit/tracking_error_layer_test.cpp @@ -0,0 +1,416 @@ +// Copyright (c) 2025 Berkan Tali +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include +#include +#include + +#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" +// 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("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(100, 100, 0.05, -2.5, -2.5); + layered_costmap_ = std::make_shared("map", false, false); + layered_costmap_->resizeMap(100, 100, 0.05, -2.5, -2.5); + + layer_ = std::make_shared(); + layer_->initialize(layered_costmap_.get(), "test_layer", nullptr, node_, nullptr); + layer_->last_tracking_feedback_.current_path_index = static_cast(-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 costmap_; + std::shared_ptr layered_costmap_; + std::shared_ptr 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(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(path)); + layer_->trackingCallback(std::make_shared(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(path)); + layer_->trackingCallback(std::make_shared(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(path)); + layer_->trackingCallback(std::make_shared( + 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(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(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 test_indices = {10, 25, 50, 75, 90}; + std::vector segment_sizes; + + layer_->pathCallback(std::make_shared(path)); + + for (auto index : test_indices) { + auto tracking_error = createTrackingFeedback(index); + layer_->trackingCallback(std::make_shared(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(path)); + layer_->trackingCallback(std::make_shared(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(path)); + layer_->trackingCallback(std::make_shared(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(path)); + layer_->trackingCallback(std::make_shared(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::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(path)); + layer_->trackingCallback(std::make_shared(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) +{ + rclcpp::init(argc, argv); + testing::InitGoogleTest(&argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} From 4f717618676932f18a2720ddfd7c42c30fc988f3 Mon Sep 17 00:00:00 2001 From: silanus23 Date: Mon, 13 Oct 2025 16:53:00 +0300 Subject: [PATCH 04/10] bresenhamLineaddition Signed-off-by: silanus23 --- .../include/nav2_util/geometry_utils.hpp | 27 +++++++++++++++++++ 1 file changed, 27 insertions(+) diff --git a/nav2_util/include/nav2_util/geometry_utils.hpp b/nav2_util/include/nav2_util/geometry_utils.hpp index 3869fbb9c49..12c929c2095 100644 --- a/nav2_util/include/nav2_util/geometry_utils.hpp +++ b/nav2_util/include/nav2_util/geometry_utils.hpp @@ -304,6 +304,33 @@ inline double cross_product_2d( return (path_vec_x * robot_vec_y) - (path_vec_y * robot_vec_x); } +inline std::vector> bresenhamLine(int x0, int y0, int x1, int y1) +{ + std::vector> 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 From 40987951c6f6a8d825114e9ebb8dc6b6832867fd Mon Sep 17 00:00:00 2001 From: silanus23 Date: Mon, 13 Oct 2025 18:20:23 +0300 Subject: [PATCH 05/10] Integration and last additions Signed-off-by: silanus23 --- nav2_costmap_2d/CMakeLists.txt | 1 + nav2_costmap_2d/costmap_plugins.xml | 3 +++ .../include/nav2_costmap_2d/tracking_error_layer.hpp | 1 - nav2_costmap_2d/plugins/tracking_error_layer.cpp | 11 ++++++----- nav2_costmap_2d/test/unit/CMakeLists.txt | 6 ++++++ .../test/unit/tracking_error_layer_test.cpp | 6 ++++++ nav2_util/include/nav2_util/geometry_utils.hpp | 1 + 7 files changed, 23 insertions(+), 6 deletions(-) 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 @@ Similar to obstacle costmap, but uses 3D voxel grid to store data. + + Creates a corridor around the path. + A range-sensor (sonar, IR) based obstacle layer for costmap_2d diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/tracking_error_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/tracking_error_layer.hpp index 0864668d08c..2a286217a8f 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/tracking_error_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/tracking_error_layer.hpp @@ -61,7 +61,6 @@ class TrackingErrorLayer : public nav2_costmap_2d::CostmapLayer std::vector> 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); diff --git a/nav2_costmap_2d/plugins/tracking_error_layer.cpp b/nav2_costmap_2d/plugins/tracking_error_layer.cpp index b583a0ce184..dab13229e7a 100644 --- a/nav2_costmap_2d/plugins/tracking_error_layer.cpp +++ b/nav2_costmap_2d/plugins/tracking_error_layer.cpp @@ -51,16 +51,17 @@ void TrackingErrorLayer::onInitialize() 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)); + dyn_params_handler_ = node->add_on_set_parameters_callback( + std::bind( + &TrackingErrorLayer::dynamicParametersCallback, + this, std::placeholders::_1)); path_sub_ = node->create_subscription( "/plan", std::bind(&TrackingErrorLayer::pathCallback, this, std::placeholders::_1), rclcpp::QoS(10).reliable() ); + tracking_feedback_sub_ = node->create_subscription( "/tracking_feedback", std::bind(&TrackingErrorLayer::trackingCallback, this, std::placeholders::_1), @@ -296,7 +297,7 @@ rcl_interfaces::msg::SetParametersResult TrackingErrorLayer::dynamicParametersCa 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; diff --git a/nav2_costmap_2d/test/unit/CMakeLists.txt b/nav2_costmap_2d/test/unit/CMakeLists.txt index c22366a2a49..a14d3408406 100644 --- a/nav2_costmap_2d/test/unit/CMakeLists.txt +++ b/nav2_costmap_2d/test/unit/CMakeLists.txt @@ -71,3 +71,9 @@ nav2_add_gtest(observation_buffer_test observation_buffer_test.cpp) target_link_libraries(observation_buffer_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 index 324722e0621..167f4fbfa38 100644 --- a/nav2_costmap_2d/test/unit/tracking_error_layer_test.cpp +++ b/nav2_costmap_2d/test/unit/tracking_error_layer_test.cpp @@ -27,6 +27,7 @@ #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; @@ -408,6 +409,11 @@ TEST_F(TrackingErrorLayerTestFixture, test_frame_transformation) 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(); diff --git a/nav2_util/include/nav2_util/geometry_utils.hpp b/nav2_util/include/nav2_util/geometry_utils.hpp index 12c929c2095..fc85bf64d69 100644 --- a/nav2_util/include/nav2_util/geometry_utils.hpp +++ b/nav2_util/include/nav2_util/geometry_utils.hpp @@ -17,6 +17,7 @@ #include #include +#include #include "geometry_msgs/msg/pose.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" From 12aa72fcf3b14bc46c1de6a3ed2b56dbeb5afd83 Mon Sep 17 00:00:00 2001 From: silanus23 Date: Fri, 24 Oct 2025 16:33:45 +0300 Subject: [PATCH 06/10] first styling changes Signed-off-by: silanus23 --- .../nav2_costmap_2d/tracking_error_layer.hpp | 5 +- .../plugins/tracking_error_layer.cpp | 82 +++++++++++++------ 2 files changed, 58 insertions(+), 29 deletions(-) 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 index 2a286217a8f..3ebca172c33 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/tracking_error_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/tracking_error_layer.hpp @@ -51,13 +51,11 @@ class TrackingErrorLayer : public nav2_costmap_2d::CostmapLayer 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> getWallPoints(const nav_msgs::msg::Path & segment); nav_msgs::msg::Path getPathSegment(); @@ -71,8 +69,7 @@ class TrackingErrorLayer : public nav2_costmap_2d::CostmapLayer private: nav2::Subscription::SharedPtr path_sub_; nav2::Subscription::SharedPtr tracking_feedback_sub_; - std::mutex path_mutex_; - std::mutex tracking_error_mutex_; + std::mutex data_mutex_; nav_msgs::msg::Path last_path_; rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; diff --git a/nav2_costmap_2d/plugins/tracking_error_layer.cpp b/nav2_costmap_2d/plugins/tracking_error_layer.cpp index dab13229e7a..43a975fc801 100644 --- a/nav2_costmap_2d/plugins/tracking_error_layer.cpp +++ b/nav2_costmap_2d/plugins/tracking_error_layer.cpp @@ -22,6 +22,15 @@ namespace nav2_costmap_2d TrackingErrorLayer::TrackingErrorLayer() {} +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::onInitialize() { auto node = node_.lock(); @@ -55,18 +64,6 @@ void TrackingErrorLayer::onInitialize() std::bind( &TrackingErrorLayer::dynamicParametersCallback, this, std::placeholders::_1)); - - path_sub_ = node->create_subscription( - "/plan", - std::bind(&TrackingErrorLayer::pathCallback, this, std::placeholders::_1), - rclcpp::QoS(10).reliable() - ); - - tracking_feedback_sub_ = node->create_subscription( - "/tracking_feedback", - std::bind(&TrackingErrorLayer::trackingCallback, this, std::placeholders::_1), - rclcpp::QoS(10).reliable() - ); tf_buffer_ = std::make_shared(node->get_clock()); tf_buffer_->setUsingDedicatedThread(true); tf_listener_ = std::make_shared(*tf_buffer_); @@ -74,14 +71,14 @@ void TrackingErrorLayer::onInitialize() void TrackingErrorLayer::pathCallback(const nav_msgs::msg::Path::SharedPtr msg) { - std::lock_guard lock(path_mutex_); + std::lock_guard lock(data_mutex_); last_path_ = *msg; } void TrackingErrorLayer::trackingCallback( const nav2_msgs::msg::TrackingFeedback::SharedPtr msg) { - std::lock_guard lock(tracking_error_mutex_); + std::lock_guard lock(data_mutex_); last_tracking_feedback_ = *msg; } @@ -167,8 +164,7 @@ std::vector> TrackingErrorLayer::getWallPoints( nav_msgs::msg::Path TrackingErrorLayer::getPathSegment() { - std::lock_guard path_lock(path_mutex_); - std::lock_guard error_lock(tracking_error_mutex_); + std::lock_guard path_lock(data_mutex_); nav_msgs::msg::Path segment; if (last_path_.poses.empty() || @@ -342,20 +338,56 @@ rcl_interfaces::msg::SetParametersResult TrackingErrorLayer::dynamicParametersCa return result; } -TrackingErrorLayer::~TrackingErrorLayer() +void TrackingErrorLayer::activate() { auto node = node_.lock(); - if (dyn_params_handler_ && node) { - node->remove_on_set_parameters_callback(dyn_params_handler_.get()); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; } - dyn_params_handler_.reset(); + + // Create subscriptions when layer is activated + path_sub_ = node->create_subscription( + "plan", + std::bind(&TrackingErrorLayer::pathCallback, this, std::placeholders::_1), + nav2::qos::StandardTopicQoS() + ); + + tracking_feedback_sub_ = node->create_subscription( + "tracking_feedback", + std::bind(&TrackingErrorLayer::trackingCallback, this, std::placeholders::_1), + nav2::qos::StandardTopicQoS() + ); + + enabled_ = true; + current_ = true; } -void TrackingErrorLayer::reset() {} -void TrackingErrorLayer::activate() {enabled_ = true;} -void TrackingErrorLayer::deactivate() {enabled_ = false;} +void TrackingErrorLayer::deactivate() +{ + // Destroy subscriptions when layer is deactivated + path_sub_.reset(); + tracking_feedback_sub_.reset(); + + // Clear cached data + { + std::lock_guard lock(data_mutex_); + last_path_.poses.clear(); + last_path_.header = std_msgs::msg::Header(); + last_tracking_feedback_ = nav2_msgs::msg::TrackingFeedback(); + } + + enabled_ = false; +} -void TrackingErrorLayer::onFootprintChanged() {} -void TrackingErrorLayer::cleanup() {} +void TrackingErrorLayer::reset() +{ + std::lock_guard guard(*getMutex()); + + // Clear the costmap data + resetMaps(); + + // Mark as current + current_ = true; +} } // namespace nav2_costmap_2d From 4a80b1347dce1232126556d0b64e2627816c8314 Mon Sep 17 00:00:00 2001 From: silanus23 Date: Tue, 28 Oct 2025 08:43:24 +0300 Subject: [PATCH 07/10] fixed tf usage made it layer test fix Signed-off-by: silanus23 --- .../nav2_costmap_2d/tracking_error_layer.hpp | 5 +--- .../plugins/tracking_error_layer.cpp | 16 +------------ .../test/unit/tracking_error_layer_test.cpp | 24 ------------------- 3 files changed, 2 insertions(+), 43 deletions(-) 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 index 3ebca172c33..7d881d4a5fc 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/tracking_error_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/tracking_error_layer.hpp @@ -37,7 +37,7 @@ namespace nav2_costmap_2d { -class TrackingErrorLayer : public nav2_costmap_2d::CostmapLayer +class TrackingErrorLayer : public nav2_costmap_2d::Layer { public: TrackingErrorLayer(); @@ -73,9 +73,6 @@ class TrackingErrorLayer : public nav2_costmap_2d::CostmapLayer nav_msgs::msg::Path last_path_; rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; - std::shared_ptr tf_buffer_; - std::shared_ptr tf_listener_; - size_t temp_step_; int step_; double width_; diff --git a/nav2_costmap_2d/plugins/tracking_error_layer.cpp b/nav2_costmap_2d/plugins/tracking_error_layer.cpp index 43a975fc801..aa6472d789c 100644 --- a/nav2_costmap_2d/plugins/tracking_error_layer.cpp +++ b/nav2_costmap_2d/plugins/tracking_error_layer.cpp @@ -64,9 +64,6 @@ void TrackingErrorLayer::onInitialize() std::bind( &TrackingErrorLayer::dynamicParametersCallback, this, std::placeholders::_1)); - tf_buffer_ = std::make_shared(node->get_clock()); - tf_buffer_->setUsingDedicatedThread(true); - tf_listener_ = std::make_shared(*tf_buffer_); } void TrackingErrorLayer::pathCallback(const nav_msgs::msg::Path::SharedPtr msg) @@ -86,11 +83,9 @@ 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 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_); @@ -206,8 +201,6 @@ void TrackingErrorLayer::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; } @@ -224,7 +217,7 @@ void TrackingErrorLayer::updateCosts( 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)) + pose, transformed_pose, *tf_, costmap_frame, 0.1)) { transformed_segment.poses.push_back(transformed_pose); } @@ -286,7 +279,6 @@ void TrackingErrorLayer::updateCosts( rcl_interfaces::msg::SetParametersResult TrackingErrorLayer::dynamicParametersCallback( std::vector parameters) { - std::lock_guard guard(*getMutex()); rcl_interfaces::msg::SetParametersResult result; result.successful = true; @@ -381,12 +373,6 @@ void TrackingErrorLayer::deactivate() void TrackingErrorLayer::reset() { - std::lock_guard guard(*getMutex()); - - // Clear the costmap data - resetMaps(); - - // Mark as current current_ = true; } diff --git a/nav2_costmap_2d/test/unit/tracking_error_layer_test.cpp b/nav2_costmap_2d/test/unit/tracking_error_layer_test.cpp index 167f4fbfa38..fe27960ec60 100644 --- a/nav2_costmap_2d/test/unit/tracking_error_layer_test.cpp +++ b/nav2_costmap_2d/test/unit/tracking_error_layer_test.cpp @@ -164,15 +164,6 @@ TEST_F(TrackingErrorLayerTestFixture, test_initialization_and_parameters) 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) { @@ -392,21 +383,6 @@ TEST_F(TrackingErrorLayerTestFixture, test_malformed_data) 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(path)); - layer_->trackingCallback(std::make_shared(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", From f728e2175123fc782626f0e1ffc584a89ab1feaa Mon Sep 17 00:00:00 2001 From: silanus23 Date: Tue, 28 Oct 2025 08:50:03 +0300 Subject: [PATCH 08/10] less complicated line creation Signed-off-by: silanus23 --- .../plugins/tracking_error_layer.cpp | 65 ++++++------------- .../include/nav2_util/geometry_utils.hpp | 27 -------- 2 files changed, 19 insertions(+), 73 deletions(-) diff --git a/nav2_costmap_2d/plugins/tracking_error_layer.cpp b/nav2_costmap_2d/plugins/tracking_error_layer.cpp index aa6472d789c..40ae49b85b8 100644 --- a/nav2_costmap_2d/plugins/tracking_error_layer.cpp +++ b/nav2_costmap_2d/plugins/tracking_error_layer.cpp @@ -225,52 +225,25 @@ void TrackingErrorLayer::updateCosts( auto wall_points = getWallPoints(transformed_segment); - // Separate left and right wall points - std::vector> 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(map_x0), static_cast(map_y0), - static_cast(map_x1), static_cast(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(map_x0), static_cast(map_y0), - static_cast(map_x1), static_cast(map_y1)); - - for (const auto & cell : cells) { - master_grid.setCost(cell.first, cell.second, nav2_costmap_2d::LETHAL_OBSTACLE); + // Create 3x3 blocks around each wall point + for (const auto & point : wall_points) { + double wx = point[0]; + double wy = point[1]; + + unsigned int center_x, center_y; + if (master_grid.worldToMap(wx, wy, center_x, center_y)) { + // Create 3x3 block centered on the point + for (int dx = -1; dx <= 1; ++dx) { + for (int dy = -1; dy <= 1; ++dy) { + unsigned int map_x = center_x + dx; + unsigned int map_y = center_y + dy; + + // Check bounds before setting cost + if (map_x < master_grid.getSizeInCellsX() && + map_y < master_grid.getSizeInCellsY()) { + master_grid.setCost(map_x, map_y, nav2_costmap_2d::LETHAL_OBSTACLE); + } + } } } } diff --git a/nav2_util/include/nav2_util/geometry_utils.hpp b/nav2_util/include/nav2_util/geometry_utils.hpp index fc85bf64d69..da2857c2503 100644 --- a/nav2_util/include/nav2_util/geometry_utils.hpp +++ b/nav2_util/include/nav2_util/geometry_utils.hpp @@ -305,33 +305,6 @@ inline double cross_product_2d( return (path_vec_x * robot_vec_y) - (path_vec_y * robot_vec_x); } -inline std::vector> bresenhamLine(int x0, int y0, int x1, int y1) -{ - std::vector> 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 From c65dffe3c9c95d0589cff6606d9482bf3709693b Mon Sep 17 00:00:00 2001 From: silanus23 Date: Fri, 5 Dec 2025 15:55:48 +0300 Subject: [PATCH 09/10] styling Signed-off-by: silanus23 --- nav2_costmap_2d/plugins/tracking_error_layer.cpp | 9 +++++---- nav2_costmap_2d/test/unit/tracking_error_layer_test.cpp | 4 ++-- 2 files changed, 7 insertions(+), 6 deletions(-) diff --git a/nav2_costmap_2d/plugins/tracking_error_layer.cpp b/nav2_costmap_2d/plugins/tracking_error_layer.cpp index 40ae49b85b8..9cd912bb876 100644 --- a/nav2_costmap_2d/plugins/tracking_error_layer.cpp +++ b/nav2_costmap_2d/plugins/tracking_error_layer.cpp @@ -229,7 +229,7 @@ void TrackingErrorLayer::updateCosts( for (const auto & point : wall_points) { double wx = point[0]; double wy = point[1]; - + unsigned int center_x, center_y; if (master_grid.worldToMap(wx, wy, center_x, center_y)) { // Create 3x3 block centered on the point @@ -237,10 +237,11 @@ void TrackingErrorLayer::updateCosts( for (int dy = -1; dy <= 1; ++dy) { unsigned int map_x = center_x + dx; unsigned int map_y = center_y + dy; - + // Check bounds before setting cost - if (map_x < master_grid.getSizeInCellsX() && - map_y < master_grid.getSizeInCellsY()) { + if (map_x < master_grid.getSizeInCellsX() && + map_y < master_grid.getSizeInCellsY()) + { master_grid.setCost(map_x, map_y, nav2_costmap_2d::LETHAL_OBSTACLE); } } diff --git a/nav2_costmap_2d/test/unit/tracking_error_layer_test.cpp b/nav2_costmap_2d/test/unit/tracking_error_layer_test.cpp index fe27960ec60..06da04b22f3 100644 --- a/nav2_costmap_2d/test/unit/tracking_error_layer_test.cpp +++ b/nav2_costmap_2d/test/unit/tracking_error_layer_test.cpp @@ -387,8 +387,8 @@ 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; + { + std::cerr << "Failed to set logger level" << std::endl; } rclcpp::init(argc, argv); testing::InitGoogleTest(&argc, argv); From 97ed4c8175746408164b2af163cb3fcec80be235 Mon Sep 17 00:00:00 2001 From: silanus23 Date: Fri, 5 Dec 2025 16:19:36 +0300 Subject: [PATCH 10/10] topic naming issue Signed-off-by: silanus23 --- nav2_bringup/params/nav2_params.yaml | 8 +++++++- nav2_costmap_2d/plugins/tracking_error_layer.cpp | 4 ++-- 2 files changed, 9 insertions(+), 3 deletions(-) diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index b9c75a413de..7401e894ff1 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -231,8 +231,14 @@ local_costmap: height: 3 resolution: 0.05 robot_radius: 0.22 - plugins: ["voxel_layer", "inflation_layer"] + plugins: ["voxel_layer", "inflation_layer","tracking_error_layer"] filters: ["keepout_filter"] + tracking_error_layer: + plugin: "nav2_costmap_2d::TrackingErrorLayer" + enabled: True + look_ahead: 5.0 + step: 5 + corridor_width: 2.0 keepout_filter: plugin: "nav2_costmap_2d::KeepoutFilter" enabled: KEEPOUT_ZONE_ENABLED diff --git a/nav2_costmap_2d/plugins/tracking_error_layer.cpp b/nav2_costmap_2d/plugins/tracking_error_layer.cpp index 9cd912bb876..05d4a0ee9cf 100644 --- a/nav2_costmap_2d/plugins/tracking_error_layer.cpp +++ b/nav2_costmap_2d/plugins/tracking_error_layer.cpp @@ -313,13 +313,13 @@ void TrackingErrorLayer::activate() // Create subscriptions when layer is activated path_sub_ = node->create_subscription( - "plan", + "/plan", std::bind(&TrackingErrorLayer::pathCallback, this, std::placeholders::_1), nav2::qos::StandardTopicQoS() ); tracking_feedback_sub_ = node->create_subscription( - "tracking_feedback", + "/tracking_feedback", std::bind(&TrackingErrorLayer::trackingCallback, this, std::placeholders::_1), nav2::qos::StandardTopicQoS() );