From b78777968c0b0eb5f0b68ba4825a38adf0a283a1 Mon Sep 17 00:00:00 2001 From: stevenbrills <90438581+stevenbrills@users.noreply.github.com> Date: Thu, 23 Jun 2022 13:37:36 -0400 Subject: [PATCH] =?UTF-8?q?-=20Modified=20findVelocitySignChange=20method?= =?UTF-8?q?=20to=20transform=20cusp=20into=20robot=E2=80=A6=20(#3036)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * - 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 * 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 * - Corrected call to transformGlobalPoseWrapper method of BasicAPIRPP object. Signed-off-by: Steven Brills * - transformGlobalPlanWrapper call bug fix Signed-off-by: Steven Brills * - Updated tests now require frame_id and time_stamp for conversion since transformed plan is to be used. Signed-off-by: Steven Brills * - Missing ; in test method Signed-off-by: Steven Brills * -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 Co-authored-by: Steven Brills (cherry picked from commit 99bec080f460bf2949d433ec1ccce79032c4d4c9) --- .../regulated_pure_pursuit_controller.hpp | 2 +- .../src/regulated_pure_pursuit_controller.cpp | 32 ++++++++++--------- .../test/test_regulated_pp.cpp | 16 +++++++--- 3 files changed, 29 insertions(+), 21 deletions(-) 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 8265ff4766..9224921de2 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 4549db222d..3428faae85 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 9f8f452195..9f65a8554f 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()); }