copilot reviews

Signed-off-by: silanus23 <[email protected]>

Author: silanus23

nav2_controller/include/nav2_controller/controller_server.hpp

@@ -233,7 +233,6 @@ class ControllerServer : public nav2::LifecycleNode
   // Dynamic parameters handler
   rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
   std::mutex dynamic_params_lock_;
-  std::mutex path_mutex_;
 
   // The controller needs a costmap node
   std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;

nav2_msgs/msg/TrackingError.msg

@@ -5,4 +5,4 @@ float32 tracking_error
 uint32 last_index
 float32 cross_product
 geometry_msgs/PoseStamped robot_pose
-float32 distance_to_goal
+float32 distance_to_goal

nav2_util/include/nav2_util/geometry_utils.hpp

@@ -254,7 +254,7 @@ inline double cross_product_2d(
   const double robot_vec_x = p.x - a.x;
   const double robot_vec_y = p.y - a.y;
 
-  return  (path_vec_x * robot_vec_y) - (path_vec_y * robot_vec_x);
+  return (path_vec_x * robot_vec_y) - (path_vec_y * robot_vec_x);
 }
 
 }  // namespace geometry_utils

nav2_util/src/path_utils.cpp

@@ -43,7 +43,7 @@ PathSearchResult distance_from_path(
     return result;
   }
 
-  if (start_index >= path.poses.size()) {
+  if (start_index >= path.poses.size() - 1) {
     throw std::invalid_argument(
       "Invalid operation: requested start index (" + std::to_string(start_index) +
       ") is greater than or equal to path size (" + std::to_string(path.poses.size()) +

nav2_util/test/test_path_utils.cpp

@@ -177,8 +177,12 @@ class RetracingPathTest : public ::testing::Test
 protected:
   void SetUp() override
   {
-    for (int i = 0; i <= 10; ++i) {target_path.poses.push_back(createPoseStamped(i, 0.0));}
-    for (int i = 9; i >= 0; --i) {target_path.poses.push_back(createPoseStamped(i, 0.0));}
+    for (int i = 0; i <= 10; ++i) {
+      target_path.poses.push_back(createPoseStamped(i, 0.0));
+    }
+    for (int i = 9; i >= 0; --i) {
+      target_path.poses.push_back(createPoseStamped(i, 0.0));
+    }
     for (int i = 0; i <= 10; ++i) {
       robot_trajectory.push_back(createPose(i, 0.5));
     }