Add new test for nav2_regulated_pure_pursuit (#2542)

* unit test for findDirChg

* lint fix

* add test for unchanged direction

* fix  for loop ending

nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp

@@ -624,7 +624,7 @@ double RegulatedPurePursuitController::findDirectionChange(
   const geometry_msgs::msg::PoseStamped & pose)
 {
   // Iterating through the global path to determine the position of the cusp
-  for (unsigned int pose_id = 1; pose_id < global_plan_.poses.size(); ++pose_id) {
+  for (unsigned int pose_id = 1; pose_id < global_plan_.poses.size() - 1; ++pose_id) {
     // We have two vectors for the dot product OA and AB. Determining the vectors.
     double oa_x = global_plan_.poses[pose_id].pose.position.x -
       global_plan_.poses[pose_id - 1].pose.position.x;

nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp

@@ -16,6 +16,7 @@
 #include <memory>
 #include <string>
 #include <vector>
+#include <limits>
 
 #include "gtest/gtest.h"
 #include "rclcpp/rclcpp.hpp"
@@ -91,6 +92,12 @@ class BasicAPIRPP : public nav2_regulated_pure_pursuit_controller::RegulatedPure
       dist_error, lookahead_dist, curvature, curr_speed, pose_cost,
       linear_vel, sign);
   }
+
+  double findDirectionChangeWrapper(
+    const geometry_msgs::msg::PoseStamped & pose)
+  {
+    return findDirectionChange(pose);
+  }
 };
 
 TEST(RegulatedPurePursuitTest, basicAPI)
@@ -144,6 +151,32 @@ TEST(RegulatedPurePursuitTest, createCarrotMsg)
   EXPECT_EQ(rtn->point.z, 0.01);
 }
 
+TEST(RegulatedPurePursuitTest, findDirectionChange)
+{
+  auto ctrl = std::make_shared<BasicAPIRPP>();
+  geometry_msgs::msg::PoseStamped pose;
+  pose.pose.position.x = 1.0;
+  pose.pose.position.y = 0.0;
+
+  nav_msgs::msg::Path path;
+  path.poses.resize(3);
+  path.poses[0].pose.position.x = 1.0;
+  path.poses[0].pose.position.y = 1.0;
+  path.poses[1].pose.position.x = 2.0;
+  path.poses[1].pose.position.y = 2.0;
+  path.poses[2].pose.position.x = -1.0;
+  path.poses[2].pose.position.y = -1.0;
+  ctrl->setPlan(path);
+  auto rtn = ctrl->findDirectionChangeWrapper(pose);
+  EXPECT_EQ(rtn, sqrt(5.0));
+
+  path.poses[2].pose.position.x = 3.0;
+  path.poses[2].pose.position.y = 3.0;
+  ctrl->setPlan(path);
+  rtn = ctrl->findDirectionChangeWrapper(pose);
+  EXPECT_EQ(rtn, std::numeric_limits<double>::max());
+}
+
 TEST(RegulatedPurePursuitTest, lookaheadAPI)
 {
   auto ctrl = std::make_shared<BasicAPIRPP>();