From ef772e92f019c21978fdfa747edbe71c9031adf4 Mon Sep 17 00:00:00 2001 From: Steven Brills Date: Tue, 21 Jun 2022 16:50:14 -0400 Subject: [PATCH 01/12] - Modified findVelocitySignChange method to transform cusp into robot frame before returning distance_to_cusp --- .../src/regulated_pure_pursuit_controller.cpp | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) 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..40510db1b23 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 @@ -738,8 +738,15 @@ double RegulatedPurePursuitController::findVelocitySignChange( 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; + + geometry_msgs::msg::PoseStamped pose_temp; + + transformPose( + tf_, pose.header.frame_id, global_plan_.poses[pose_id], + pose_temp, transform_tolerance_); + + auto x = pose_temp.pose.position.x - pose.pose.position.x; + auto y = pose_temp.pose.position.y - pose.pose.position.y; return hypot(x, y); // returning the distance if there is a cusp } } From 7c38246b598ce5d0feb4c966f19cf8dd1dd98f25 Mon Sep 17 00:00:00 2001 From: Steven Brills Date: Wed, 22 Jun 2022 09:41:38 -0400 Subject: [PATCH 02/12] - 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. --- .../src/regulated_pure_pursuit_controller.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 40510db1b23..6701c9f36f0 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 @@ -742,8 +742,8 @@ double RegulatedPurePursuitController::findVelocitySignChange( geometry_msgs::msg::PoseStamped pose_temp; transformPose( - tf_, pose.header.frame_id, global_plan_.poses[pose_id], - pose_temp, transform_tolerance_); + pose.header.frame_id, global_plan_.poses[pose_id], + pose_temp); auto x = pose_temp.pose.position.x - pose.pose.position.x; auto y = pose_temp.pose.position.y - pose.pose.position.y; From 494c2ad4e06b3fbc7edfef44059521b7fa9018f3 Mon Sep 17 00:00:00 2001 From: Steven Brills Date: Wed, 22 Jun 2022 09:41:38 -0400 Subject: [PATCH 03/12] - 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 --- .../src/regulated_pure_pursuit_controller.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 40510db1b23..6701c9f36f0 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 @@ -742,8 +742,8 @@ double RegulatedPurePursuitController::findVelocitySignChange( geometry_msgs::msg::PoseStamped pose_temp; transformPose( - tf_, pose.header.frame_id, global_plan_.poses[pose_id], - pose_temp, transform_tolerance_); + pose.header.frame_id, global_plan_.poses[pose_id], + pose_temp); auto x = pose_temp.pose.position.x - pose.pose.position.x; auto y = pose_temp.pose.position.y - pose.pose.position.y; From 9555cba57a798b6aaec77b57bf0bfa0c5bc7e81a Mon Sep 17 00:00:00 2001 From: Steven Brills Date: Wed, 22 Jun 2022 11:17:55 -0400 Subject: [PATCH 04/12] Failed lint check due to stray blank line. Removed the blank line --- .../src/regulated_pure_pursuit_controller.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) 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 6701c9f36f0..c2ed1bed5ec 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 @@ -738,12 +738,10 @@ double RegulatedPurePursuitController::findVelocitySignChange( 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) { - + // Converting identified cusp to the robot's frame geometry_msgs::msg::PoseStamped pose_temp; - transformPose( - pose.header.frame_id, global_plan_.poses[pose_id], - pose_temp); + transformPose(pose.header.frame_id, global_plan_.poses[pose_id], pose_temp); auto x = pose_temp.pose.position.x - pose.pose.position.x; auto y = pose_temp.pose.position.y - pose.pose.position.y; From a026e0365c2b4db6301968d6154d0a723519ef26 Mon Sep 17 00:00:00 2001 From: Steven Brills Date: Wed, 22 Jun 2022 11:58:54 -0400 Subject: [PATCH 05/12] - Modified findVelocitySignChange function to take the transformed plan in robot's frame - Removed need to pass pose to the findVelocitySignChange function --- .../regulated_pure_pursuit_controller.hpp | 2 +- .../src/regulated_pure_pursuit_controller.cpp | 37 +++++++++---------- 2 files changed, 18 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 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 c2ed1bed5ec..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,32 +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) { - // Converting identified cusp to the robot's frame - geometry_msgs::msg::PoseStamped pose_temp; - - transformPose(pose.header.frame_id, global_plan_.poses[pose_id], pose_temp); - - auto x = pose_temp.pose.position.x - pose.pose.position.x; - auto y = pose_temp.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); } } From 017cfab9bd06bf4840c70703ae9324079c711f5a Mon Sep 17 00:00:00 2001 From: Steven Brills Date: Wed, 22 Jun 2022 12:19:23 -0400 Subject: [PATCH 06/12] - Modified the test file to reflect change in new parameters that findVelocitySignChange expects. Signed-off-by: Steven Brills --- .../test/test_regulated_pp.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) 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..b35b26693cf 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( @@ -183,13 +183,15 @@ 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); + auto transformed_plan = transformGlobalPlanWrapper(pose); + auto rtn = ctrl->findVelocitySignChangeWrapper(transformed_plan); 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->findVelocitySignChangeWrapper(pose); + auto transformed_plan = transformGlobalPlanWrapper(pose); + rtn = ctrl->findVelocitySignChangeWrapper(transformed_plan); EXPECT_EQ(rtn, std::numeric_limits::max()); } From 523624852e26d181c07684329aface584d60353d Mon Sep 17 00:00:00 2001 From: Steven Brills Date: Wed, 22 Jun 2022 12:33:57 -0400 Subject: [PATCH 07/12] - Corrected call to transformGlobalPoseWrapper method of BasicAPIRPP object. Signed-off-by: Steven Brills --- .../test/test_regulated_pp.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 b35b26693cf..f65a7387b88 100644 --- a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp +++ b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp @@ -183,14 +183,14 @@ TEST(RegulatedPurePursuitTest, findVelocitySignChange) path.poses[2].pose.position.x = -1.0; path.poses[2].pose.position.y = -1.0; ctrl->setPlan(path); - auto transformed_plan = transformGlobalPlanWrapper(pose); + auto transformed_plan = ctrl->transformGlobalPlanWrapper(pose); auto rtn = ctrl->findVelocitySignChangeWrapper(transformed_plan); 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); - auto transformed_plan = transformGlobalPlanWrapper(pose); + auto transformed_plan = ctrl->transformGlobalPlanWrapper(pose); rtn = ctrl->findVelocitySignChangeWrapper(transformed_plan); EXPECT_EQ(rtn, std::numeric_limits::max()); } From b40558971a39fbe33c88994024106d614c11bd62 Mon Sep 17 00:00:00 2001 From: Steven Brills Date: Wed, 22 Jun 2022 12:39:36 -0400 Subject: [PATCH 08/12] - transformGlobalPlanWrapper call bug fix Signed-off-by: Steven Brills --- .../test/test_regulated_pp.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 f65a7387b88..e2eeca1e9af 100644 --- a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp +++ b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp @@ -190,7 +190,7 @@ TEST(RegulatedPurePursuitTest, findVelocitySignChange) path.poses[2].pose.position.x = 3.0; path.poses[2].pose.position.y = 3.0; ctrl->setPlan(path); - auto transformed_plan = ctrl->transformGlobalPlanWrapper(pose); + transformed_plan = ctrl->transformGlobalPlanWrapper(pose); rtn = ctrl->findVelocitySignChangeWrapper(transformed_plan); EXPECT_EQ(rtn, std::numeric_limits::max()); } From 2cf78891e419a916411de04825e81ace964fb55a Mon Sep 17 00:00:00 2001 From: Steven Brills Date: Wed, 22 Jun 2022 16:53:37 -0400 Subject: [PATCH 09/12] - Updated tests now require frame_id and time_stamp for conversion since transformed plan is to be used. Signed-off-by: Steven Brills --- .../test/test_regulated_pp.cpp | 4 ++++ 1 file changed, 4 insertions(+) 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 e2eeca1e9af..31de88f1c1c 100644 --- a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp +++ b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp @@ -171,11 +171,15 @@ TEST(RegulatedPurePursuitTest, findVelocitySignChange) { auto ctrl = std::make_shared(); geometry_msgs::msg::PoseStamped pose; + pose.header.frame_id = "smb"; + pose.header.stamp = rclcpp::Time::now(); 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 = "home" + 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; From 26881d723123ac7138686ca4ec61d5cbe218de6a Mon Sep 17 00:00:00 2001 From: Steven Brills Date: Wed, 22 Jun 2022 17:00:33 -0400 Subject: [PATCH 10/12] - Missing ; in test method Signed-off-by: Steven Brills --- .../test/test_regulated_pp.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 31de88f1c1c..2ac795809c2 100644 --- a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp +++ b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp @@ -178,7 +178,7 @@ TEST(RegulatedPurePursuitTest, findVelocitySignChange) nav_msgs::msg::Path path; path.poses.resize(3); - path.header.frame_id = "home" + path.header.frame_id = "home"; path.header.stamp = pose.header.stamp; path.poses[0].pose.position.x = 1.0; path.poses[0].pose.position.y = 1.0; From f10a7dbccf37e3b4a0b1524f56a8fcbe7b85aa1c Mon Sep 17 00:00:00 2001 From: Steven Brills Date: Thu, 23 Jun 2022 16:06:42 +0000 Subject: [PATCH 11/12] -Modified findVelocitySignChange tests in test_regulated_pp to use transformed_plan instead --- .../test/test_regulated_pp.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) 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 2ac795809c2..9f65a8554fe 100644 --- a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp +++ b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp @@ -170,15 +170,17 @@ 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"; - pose.header.stamp = rclcpp::Time::now(); + 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 = "home"; + 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; @@ -187,15 +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 transformed_plan = ctrl->transformGlobalPlanWrapper(pose); - auto rtn = ctrl->findVelocitySignChangeWrapper(transformed_plan); - 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); - transformed_plan = ctrl->transformGlobalPlanWrapper(pose); - rtn = ctrl->findVelocitySignChangeWrapper(transformed_plan); + rtn = ctrl->findVelocitySignChangeWrapper(path); EXPECT_EQ(rtn, std::numeric_limits::max()); } From 0ac8e36b9abc5e68e7fd2e325878a1cf2e75600f Mon Sep 17 00:00:00 2001 From: Steven Brills Date: Thu, 23 Jun 2022 16:06:42 +0000 Subject: [PATCH 12/12] -Modified findVelocitySignChange tests in test_regulated_pp to use transformed_plan instead Signed-off-by: Steven Brills --- .../test/test_regulated_pp.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) 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 2ac795809c2..9f65a8554fe 100644 --- a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp +++ b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp @@ -170,15 +170,17 @@ 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"; - pose.header.stamp = rclcpp::Time::now(); + 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 = "home"; + 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; @@ -187,15 +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 transformed_plan = ctrl->transformGlobalPlanWrapper(pose); - auto rtn = ctrl->findVelocitySignChangeWrapper(transformed_plan); - 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); - transformed_plan = ctrl->transformGlobalPlanWrapper(pose); - rtn = ctrl->findVelocitySignChangeWrapper(transformed_plan); + rtn = ctrl->findVelocitySignChangeWrapper(path); EXPECT_EQ(rtn, std::numeric_limits::max()); }