- Modified findVelocitySignChange method to transform cusp into robot… (

#3036) (#3041)

* - Modified findVelocitySignChange method to transform cusp into robot frame before returning distance_to_cusp

* - Previous commit had incorrect usage of method transformPose based on different nav2 branch.
- Now a placeholder pose, input pose and desired frame id is passed.

* - Previous commit had incorrect usage of method transformPose based on different nav2 branch.
- Now a placeholder pose, input pose and desired frame id is passed.

Signed-off-by: Steven Brills <sbrills@hotmail.com>

* Failed lint check due to stray blank line.  Removed the blank line

* - Modified findVelocitySignChange function to take the transformed plan in robot's frame
- Removed need to pass pose to the findVelocitySignChange function

* - Modified the test file to reflect change in new parameters that findVelocitySignChange expects.

Signed-off-by: Steven Brills <stevenbrills@hotmail.com>

* - Corrected call to transformGlobalPoseWrapper method of BasicAPIRPP object.

Signed-off-by: Steven Brills <stevenbrills@hotmail.com>

* - transformGlobalPlanWrapper call bug fix

Signed-off-by: Steven Brills <stevenbrills@hotmail.com>

* - Updated tests now require frame_id and time_stamp for conversion since transformed plan is to be used.

Signed-off-by: Steven Brills <stevenbrills@hotmail.com>

* - Missing ; in test method

Signed-off-by: Steven Brills <stevenbrills@hotmail.com>

* -Modified findVelocitySignChange tests in test_regulated_pp to use transformed_plan instead

* -Modified findVelocitySignChange tests in test_regulated_pp to use transformed_plan instead

Signed-off-by: Steven Brills <stevenbrills@hotmail.com>

Co-authored-by: Steven Brills <sbrills@aethon.com>
(cherry picked from commit 99bec08)

Co-authored-by: stevenbrills <90438581+stevenbrills@users.noreply.github.com>

nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp

@@ -249,7 +249,7 @@ class RegulatedPurePursuitController : public nav2_core::Controller
    * @param pose Pose input to determine the cusp position
    * @return robot distance from the cusp
    */
-  double findVelocitySignChange(const geometry_msgs::msg::PoseStamped & pose);
+  double findVelocitySignChange(const nav_msgs::msg::Path & transformed_plan);
 
   /**
    * Get the greatest extent of the costmap in meters from the center.

nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp

@@ -283,7 +283,7 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity
   // Check for reverse driving
   if (allow_reversing_) {
     // Cusp check
-    double dist_to_cusp = findVelocitySignChange(pose);
+    double dist_to_cusp = findVelocitySignChange(transformed_plan);
 
     // if the lookahead distance is further than the cusp, use the cusp distance instead
     if (dist_to_cusp < lookahead_dist) {
@@ -720,27 +720,29 @@ nav_msgs::msg::Path RegulatedPurePursuitController::transformGlobalPlan(
 }
 
 double RegulatedPurePursuitController::findVelocitySignChange(
-  const geometry_msgs::msg::PoseStamped & pose)
+  const nav_msgs::msg::Path & transformed_plan)
 {
-  // Iterating through the global path to determine the position of the cusp
-  for (unsigned int pose_id = 1; pose_id < global_plan_.poses.size() - 1; ++pose_id) {
+  // Iterating through the transformed global path to determine the position of the cusp
+  for (unsigned int pose_id = 1; pose_id < transformed_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;
-    double oa_y = global_plan_.poses[pose_id].pose.position.y -
-      global_plan_.poses[pose_id - 1].pose.position.y;
-    double ab_x = global_plan_.poses[pose_id + 1].pose.position.x -
-      global_plan_.poses[pose_id].pose.position.x;
-    double ab_y = global_plan_.poses[pose_id + 1].pose.position.y -
-      global_plan_.poses[pose_id].pose.position.y;
+    double oa_x = transformed_plan.poses[pose_id].pose.position.x -
+      transformed_plan.poses[pose_id - 1].pose.position.x;
+    double oa_y = transformed_plan.poses[pose_id].pose.position.y -
+      transformed_plan.poses[pose_id - 1].pose.position.y;
+    double ab_x = transformed_plan.poses[pose_id + 1].pose.position.x -
+      transformed_plan.poses[pose_id].pose.position.x;
+    double ab_y = transformed_plan.poses[pose_id + 1].pose.position.y -
+      transformed_plan.poses[pose_id].pose.position.y;
 
     /* Checking for the existance of cusp, in the path, using the dot product
     and determine it's distance from the robot. If there is no cusp in the path,
     then just determine the distance to the goal location. */
     if ( (oa_x * ab_x) + (oa_y * ab_y) < 0.0) {
-      auto x = global_plan_.poses[pose_id].pose.position.x - pose.pose.position.x;
-      auto y = global_plan_.poses[pose_id].pose.position.y - pose.pose.position.y;
-      return hypot(x, y);  // returning the distance if there is a cusp
+      // returning the distance if there is a cusp
+      // The transformed path is in the robots frame, so robot is at the origin
+      return hypot(
+        transformed_plan.poses[pose_id].pose.position.x,
+        transformed_plan.poses[pose_id].pose.position.y);
     }
   }
 

nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp

@@ -103,9 +103,9 @@ class BasicAPIRPP : public nav2_regulated_pure_pursuit_controller::RegulatedPure
   }
 
   double findVelocitySignChangeWrapper(
-    const geometry_msgs::msg::PoseStamped & pose)
+    const nav_msgs::msg::Path & transformed_plan)
   {
-    return findVelocitySignChange(pose);
+    return findVelocitySignChange(transformed_plan);
   }
 
   nav_msgs::msg::Path transformGlobalPlanWrapper(
@@ -170,26 +170,32 @@ TEST(RegulatedPurePursuitTest, createCarrotMsg)
 TEST(RegulatedPurePursuitTest, findVelocitySignChange)
 {
   auto ctrl = std::make_shared<BasicAPIRPP>();
+  auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("testRPPfindVelocitySignChange");
   geometry_msgs::msg::PoseStamped pose;
+  pose.header.frame_id = "smb";
+  auto time = node->get_clock()->now();
+  pose.header.stamp = time;
   pose.pose.position.x = 1.0;
   pose.pose.position.y = 0.0;
 
   nav_msgs::msg::Path path;
   path.poses.resize(3);
+  path.header.frame_id = "smb";
+  path.header.stamp = pose.header.stamp;
   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->findVelocitySignChangeWrapper(pose);
-  EXPECT_EQ(rtn, sqrt(5.0));
+  auto rtn = ctrl->findVelocitySignChangeWrapper(path);
+  EXPECT_EQ(rtn, sqrt(8.0));
 
   path.poses[2].pose.position.x = 3.0;
   path.poses[2].pose.position.y = 3.0;
   ctrl->setPlan(path);
-  rtn = ctrl->findVelocitySignChangeWrapper(pose);
+  rtn = ctrl->findVelocitySignChangeWrapper(path);
   EXPECT_EQ(rtn, std::numeric_limits<double>::max());
 }