diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp index 8265ff4766e..9224921de2f 100644 --- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp +++ b/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. diff --git a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp index 4549db222d1..3428faae854 100644 --- a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp +++ b/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); } } diff --git a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp index 9f8f452195c..9f65a8554fe 100644 --- a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp +++ b/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,12 +170,18 @@ TEST(RegulatedPurePursuitTest, createCarrotMsg) TEST(RegulatedPurePursuitTest, findVelocitySignChange) { auto ctrl = std::make_shared(); + auto node = std::make_shared("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; @@ -183,13 +189,13 @@ TEST(RegulatedPurePursuitTest, findVelocitySignChange) 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::max()); }