From d196881507103479cd99b4205367b7db236f368e Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Thu, 10 Mar 2022 15:47:43 +0000 Subject: [PATCH 1/2] Move, refactor and test closestPointOnSegment(). --- include/path_tracking_pid/controller.hpp | 15 ---- src/calculations.cpp | 39 +++++++++ src/calculations.hpp | 18 ++++ src/controller.cpp | 33 ------- test/unittests/test_calculations.cpp | 104 +++++++++++++++++++++++ 5 files changed, 161 insertions(+), 48 deletions(-) diff --git a/include/path_tracking_pid/controller.hpp b/include/path_tracking_pid/controller.hpp index df74fa0c..b36312cd 100644 --- a/include/path_tracking_pid/controller.hpp +++ b/include/path_tracking_pid/controller.hpp @@ -193,21 +193,6 @@ class Controller : private boost::noncopyable double getVelMaxObstacle() const; private: - /** - * @brief Closest point between a line segment and a point - * Calculate the closest point between the line segment bounded by PV and the point W. - * - * @param[in] pose_p Start of the line segment - * @param[in] pose_v End of the line segment - * @param[in] pose_w The point - * @param[in] estimate_pose_angle Indicates if the pose angle should be estimated from the line - * segment (true) or if the pose angle from pose_v should be used. - * @return The pose projection of the closest point. - */ - static tf2::Transform closestPointOnSegment( - const tf2::Transform & pose_p, const tf2::Transform & pose_v, const tf2::Transform & pose_w, - bool estimate_pose_angle); - geometry_msgs::Twist computeTricycleModelForwardKinematics(double x_vel, double steering_angle); TricycleSteeringCmdVel computeTricycleModelInverseKinematics( const geometry_msgs::Twist & cmd_vel); diff --git a/src/calculations.cpp b/src/calculations.cpp index b622bc07..037ac0be 100644 --- a/src/calculations.cpp +++ b/src/calculations.cpp @@ -1,5 +1,7 @@ #include "calculations.hpp" +#include + #include #include #include @@ -76,4 +78,41 @@ double distSquared(const tf2::Transform & a, const tf2::Transform & b) return a.getOrigin().distance2(b.getOrigin()); } +tf2::Transform closestPointOnSegment( + const tf2::Transform & point, const tf2::Transform & segment_start, + const tf2::Transform & segment_end, bool estimate_pose_angle) +{ + const double l2 = distSquared(segment_start, segment_end); + if (l2 == 0) { + return segment_end; + } + + tf2::Transform result; + + const double t = std::clamp( + ((point.getOrigin().x() - segment_start.getOrigin().x()) * + (segment_end.getOrigin().x() - segment_start.getOrigin().x()) + + (point.getOrigin().y() - segment_start.getOrigin().y()) * + (segment_end.getOrigin().y() - segment_start.getOrigin().y())) / + l2, + 0.0, 1.0); + result.setOrigin(tf2::Vector3( + segment_start.getOrigin().x() + + t * (segment_end.getOrigin().x() - segment_start.getOrigin().x()), + segment_start.getOrigin().y() + + t * (segment_end.getOrigin().y() - segment_start.getOrigin().y()), + 0.0)); + + const auto yaw = estimate_pose_angle + ? atan2( + segment_end.getOrigin().y() - segment_start.getOrigin().y(), + segment_end.getOrigin().x() - segment_start.getOrigin().x()) + : tf2::getYaw(segment_start.getRotation()); + tf2::Quaternion pose_quaternion; + pose_quaternion.setRPY(0.0, 0.0, yaw); + result.setRotation(pose_quaternion); + + return result; +} + } // namespace path_tracking_pid diff --git a/src/calculations.hpp b/src/calculations.hpp index 0a579c58..8c4c3d55 100644 --- a/src/calculations.hpp +++ b/src/calculations.hpp @@ -55,4 +55,22 @@ bool is_in_direction_of_target( */ double distSquared(const tf2::Transform & a, const tf2::Transform & b); +/** + * @brief Closest point between a line segment and a point + * + * Calculate the closest point between the line segment bounded by `segment_start` and `segment_end` + * and point `point`. + * + * @param[in] point The point + * @param[in] segment_start Start of the line segment + * @param[in] segment_end End of the line segment + * @param[in] estimate_pose_angle Indicates if the pose angle should be estimated from the line + * segment (true) or if the pose angle from segment_start should be + * used. + * @return The pose projection of the closest point. + */ +tf2::Transform closestPointOnSegment( + const tf2::Transform & point, const tf2::Transform & segment_start, + const tf2::Transform & segment_end, bool estimate_pose_angle); + } // namespace path_tracking_pid diff --git a/src/controller.cpp b/src/controller.cpp index 738872dc..c0efcd1f 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -234,39 +234,6 @@ bool Controller::setPlan( return result; } -tf2::Transform Controller::closestPointOnSegment( - const tf2::Transform & pose_p, const tf2::Transform & pose_v, const tf2::Transform & pose_w, - bool estimate_pose_angle) -{ - const double l2 = distSquared(pose_v, pose_w); - if (l2 == 0) { - return pose_w; - } - - tf2::Transform result; - - const double t = std::clamp( - ((pose_p.getOrigin().x() - pose_v.getOrigin().x()) * - (pose_w.getOrigin().x() - pose_v.getOrigin().x()) + - (pose_p.getOrigin().y() - pose_v.getOrigin().y()) * - (pose_w.getOrigin().y() - pose_v.getOrigin().y())) / - l2, - 0.0, 1.0); - result.setOrigin(tf2::Vector3( - pose_v.getOrigin().x() + t * (pose_w.getOrigin().x() - pose_v.getOrigin().x()), - pose_v.getOrigin().y() + t * (pose_w.getOrigin().y() - pose_v.getOrigin().y()), 0.0)); - - const auto yaw = estimate_pose_angle ? atan2( - pose_w.getOrigin().y() - pose_v.getOrigin().y(), - pose_w.getOrigin().x() - pose_v.getOrigin().x()) - : tf2::getYaw(pose_v.getRotation()); - tf2::Quaternion pose_quaternion; - pose_quaternion.setRPY(0.0, 0.0, yaw); - result.setRotation(pose_quaternion); - - return result; -} - tf2::Transform Controller::findPositionOnPlan( const tf2::Transform & current_tf, ControllerState & controller_state, size_t & path_pose_idx) { diff --git a/test/unittests/test_calculations.cpp b/test/unittests/test_calculations.cpp index 441f849b..bf9cd2a3 100644 --- a/test/unittests/test_calculations.cpp +++ b/test/unittests/test_calculations.cpp @@ -1,4 +1,5 @@ #include +#include #include #include @@ -8,6 +9,7 @@ namespace { +using path_tracking_pid::closestPointOnSegment; using path_tracking_pid::deltas_of_plan; using path_tracking_pid::distances_to_goal; using path_tracking_pid::distSquared; @@ -23,6 +25,14 @@ tf2::Transform create_transform(double x, double y, double z) return result; } +// Create a quaternion based on the given roll, pitch and yaw. +tf2::Quaternion create_quaternion(double roll, double pitch, double yaw) +{ + tf2::Quaternion result; + result.setRPY(roll, pitch, yaw); + return result; +} + TEST(PathTrackingPidCalculations, DeltasOfPlan_Empty) { const auto plan = std::vector{}; @@ -198,4 +208,98 @@ TEST(PathTrackingPidCalculations, DistSquared) EXPECT_EQ(0, distSquared(create_transform(1, 2, 3), create_transform(1, 2, 3))); } +TEST(PathTrackingPidCalculations, ClosestPointOnSegment_AtEnd) +{ + const auto start = create_transform(2, 2, 0); + const auto end = create_transform(4, 4, 0); + const auto point = end; + const auto ref = end; + + EXPECT_EQ(ref, closestPointOnSegment(point, start, end, false)); +} + +TEST(PathTrackingPidCalculations, ClosestPointOnSegment_AtStart) +{ + const auto start = create_transform(2, 2, 0); + const auto end = create_transform(4, 4, 0); + const auto point = start; + const auto ref = start; + + EXPECT_EQ(ref, closestPointOnSegment(point, start, end, false)); +} + +TEST(PathTrackingPidCalculations, ClosestPointOnSegment_CloseToEnd) +{ + const auto start = create_transform(2, 2, 0); + const auto end = create_transform(4, 4, 0); + const auto point = create_transform(7, 5, 0); + const auto ref = end; + + EXPECT_EQ(ref, closestPointOnSegment(point, start, end, false)); +} + +TEST(PathTrackingPidCalculations, ClosestPointOnSegment_CloseToStart) +{ + const auto start = create_transform(2, 2, 0); + const auto end = create_transform(4, 4, 0); + const auto point = create_transform(-7, -5, 0); + const auto ref = start; + + EXPECT_EQ(ref, closestPointOnSegment(point, start, end, false)); +} + +TEST(PathTrackingPidCalculations, ClosestPointOnSegment_Halfway) +{ + const auto start = create_transform(2, 2, 0); + const auto end = create_transform(4, 4, 0); + const auto point = create_transform(2, 4, 0); + const auto ref = create_transform(3, 3, 0); + + EXPECT_EQ(ref, closestPointOnSegment(point, start, end, false)); +} + +TEST(PathTrackingPidCalculations, ClosestPointOnSegment_TwoThirds) +{ + const auto start = create_transform(2, 2, 0); + const auto end = create_transform(8, 5, 0); + const auto point = create_transform(2, 12, 0); + const auto ref = create_transform(6, 4, 0); + + EXPECT_EQ(ref, closestPointOnSegment(point, start, end, false)); +} + +TEST(PathTrackingPidCalculations, ClosestPointOnSegment_OtherYaw) +{ + const auto start = tf2::Transform(create_quaternion(1, 1, 1), {2, 2, 0}); + const auto end = create_transform(4, 4, 0); + const auto point = create_transform(2, 4, 0); + const auto ref = tf2::Transform(create_quaternion(0, 0, 1), {3, 3, 0}); + const auto result = closestPointOnSegment(point, start, end, false); + + EXPECT_EQ(ref.getOrigin(), result.getOrigin()); + // allow for small differences in the basis because of rounding errors in the calculations + for (auto r = 0; r < 3; ++r) { + for (auto c = 0; c < 3; ++c) { + EXPECT_NEAR(ref.getBasis()[r][c], result.getBasis()[r][c], 1e-6); + } + } +} + +TEST(PathTrackingPidCalculations, ClosestPointOnSegment_EstimateAngle) +{ + const auto start = create_transform(2, 2, 0); + const auto end = create_transform(4, 4, 0); + const auto point = create_transform(2, 4, 0); + const auto ref = tf2::Transform(create_quaternion(0, 0, M_PI / 4.0), {3, 3, 0}); + const auto result = closestPointOnSegment(point, start, end, true); + + EXPECT_EQ(ref.getOrigin(), result.getOrigin()); + // allow for small differences in the basis because of rounding errors in the calculations + for (auto r = 0; r < 3; ++r) { + for (auto c = 0; c < 3; ++c) { + EXPECT_NEAR(ref.getBasis()[r][c], result.getBasis()[r][c], 1e-6); + } + } +} + } // namespace From 8ab5a256581edc4ef00fd5103a57abbca5be0c37 Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Mon, 14 Mar 2022 08:17:11 +0000 Subject: [PATCH 2/2] Reworked by renaming function. --- src/calculations.cpp | 2 +- src/calculations.hpp | 6 ++--- src/controller.cpp | 29 ++++++++++++------------ test/unittests/test_calculations.cpp | 34 ++++++++++++++-------------- 4 files changed, 35 insertions(+), 36 deletions(-) diff --git a/src/calculations.cpp b/src/calculations.cpp index 037ac0be..9aa369b1 100644 --- a/src/calculations.cpp +++ b/src/calculations.cpp @@ -78,7 +78,7 @@ double distSquared(const tf2::Transform & a, const tf2::Transform & b) return a.getOrigin().distance2(b.getOrigin()); } -tf2::Transform closestPointOnSegment( +tf2::Transform closestPoseOnSegment( const tf2::Transform & point, const tf2::Transform & segment_start, const tf2::Transform & segment_end, bool estimate_pose_angle) { diff --git a/src/calculations.hpp b/src/calculations.hpp index 8c4c3d55..4d1ced22 100644 --- a/src/calculations.hpp +++ b/src/calculations.hpp @@ -56,9 +56,9 @@ bool is_in_direction_of_target( double distSquared(const tf2::Transform & a, const tf2::Transform & b); /** - * @brief Closest point between a line segment and a point + * @brief Closest pose between a line segment and a point * - * Calculate the closest point between the line segment bounded by `segment_start` and `segment_end` + * Calculate the closest pose between the line segment bounded by `segment_start` and `segment_end` * and point `point`. * * @param[in] point The point @@ -69,7 +69,7 @@ double distSquared(const tf2::Transform & a, const tf2::Transform & b); * used. * @return The pose projection of the closest point. */ -tf2::Transform closestPointOnSegment( +tf2::Transform closestPoseOnSegment( const tf2::Transform & point, const tf2::Transform & segment_start, const tf2::Transform & segment_end, bool estimate_pose_angle); diff --git a/src/controller.cpp b/src/controller.cpp index c0efcd1f..f94396a5 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -170,7 +170,7 @@ bool Controller::setPlan( for (int idx_path = static_cast(global_plan_tf_.size() - 2); idx_path >= 0; --idx_path) { /* Get distance to segment to determine if this is the segment to start at */ const auto dist_to_segment = distSquared( - current_tf, closestPointOnSegment( + current_tf, closestPoseOnSegment( current_tf, global_plan_tf_[idx_path], global_plan_tf_[idx_path + 1], estimate_pose_angle_)); // Calculate 3D distance, since current_tf2 might have significant z-offset and roll/pitch values w.r.t. path-pose @@ -290,58 +290,57 @@ tf2::Transform Controller::findPositionOnPlan( // the closest line segment to the current carrot if (controller_state.current_global_plan_index == 0) { - const auto closest_point = closestPointOnSegment( + const auto closest_pose = closestPoseOnSegment( current_tf2, global_plan_tf_[0], global_plan_tf_[1], estimate_pose_angle_); distance_to_goal_ = - distance_to_goal_vector_[1] + sqrt(distSquared(global_plan_tf_[1], closest_point)); + distance_to_goal_vector_[1] + sqrt(distSquared(global_plan_tf_[1], closest_pose)); controller_state.last_visited_pose_index = 0; path_pose_idx = controller_state.current_global_plan_index; - return closest_point; + return closest_pose; } if (controller_state.current_global_plan_index == global_plan_tf_.size() - 1) { - const auto closest_point = closestPointOnSegment( + const auto closest_pose = closestPoseOnSegment( current_tf2, global_plan_tf_[controller_state.current_global_plan_index - 1], global_plan_tf_[controller_state.current_global_plan_index], estimate_pose_angle_); distance_to_goal_ = - sqrt(distSquared(global_plan_tf_[controller_state.current_global_plan_index], closest_point)); + sqrt(distSquared(global_plan_tf_[controller_state.current_global_plan_index], closest_pose)); controller_state.last_visited_pose_index = global_plan_tf_.size() - 2; path_pose_idx = controller_state.current_global_plan_index - 1; - return closest_point; + return closest_pose; } - const auto closest_point_ahead = closestPointOnSegment( + const auto closest_pose_ahead = closestPoseOnSegment( current_tf2, global_plan_tf_[controller_state.current_global_plan_index], global_plan_tf_[controller_state.current_global_plan_index + 1], estimate_pose_angle_); - const auto closest_point_behind = closestPointOnSegment( + const auto closest_pose_behind = closestPoseOnSegment( current_tf2, global_plan_tf_[controller_state.current_global_plan_index - 1], global_plan_tf_[controller_state.current_global_plan_index], estimate_pose_angle_); if ( - distSquared(current_tf2, closest_point_ahead) < - distSquared(current_tf2, closest_point_behind)) { + distSquared(current_tf2, closest_pose_ahead) < distSquared(current_tf2, closest_pose_behind)) { distance_to_goal_ = distance_to_goal_vector_[controller_state.current_global_plan_index + 1] + sqrt(distSquared( - global_plan_tf_[controller_state.current_global_plan_index + 1], closest_point_ahead)); + global_plan_tf_[controller_state.current_global_plan_index + 1], closest_pose_ahead)); controller_state.last_visited_pose_index = controller_state.current_global_plan_index; path_pose_idx = controller_state.current_global_plan_index; - return closest_point_ahead; + return closest_pose_ahead; } distance_to_goal_ = distance_to_goal_vector_[controller_state.current_global_plan_index] + sqrt(distSquared( - global_plan_tf_[controller_state.current_global_plan_index], closest_point_behind)); + global_plan_tf_[controller_state.current_global_plan_index], closest_pose_behind)); controller_state.last_visited_pose_index = controller_state.current_global_plan_index - 1; path_pose_idx = controller_state.current_global_plan_index; - return closest_point_behind; + return closest_pose_behind; } Controller::UpdateResult Controller::update( diff --git a/test/unittests/test_calculations.cpp b/test/unittests/test_calculations.cpp index bf9cd2a3..3850d4c3 100644 --- a/test/unittests/test_calculations.cpp +++ b/test/unittests/test_calculations.cpp @@ -9,7 +9,7 @@ namespace { -using path_tracking_pid::closestPointOnSegment; +using path_tracking_pid::closestPoseOnSegment; using path_tracking_pid::deltas_of_plan; using path_tracking_pid::distances_to_goal; using path_tracking_pid::distSquared; @@ -208,73 +208,73 @@ TEST(PathTrackingPidCalculations, DistSquared) EXPECT_EQ(0, distSquared(create_transform(1, 2, 3), create_transform(1, 2, 3))); } -TEST(PathTrackingPidCalculations, ClosestPointOnSegment_AtEnd) +TEST(PathTrackingPidCalculations, ClosestPoseOnSegment_AtEnd) { const auto start = create_transform(2, 2, 0); const auto end = create_transform(4, 4, 0); const auto point = end; const auto ref = end; - EXPECT_EQ(ref, closestPointOnSegment(point, start, end, false)); + EXPECT_EQ(ref, closestPoseOnSegment(point, start, end, false)); } -TEST(PathTrackingPidCalculations, ClosestPointOnSegment_AtStart) +TEST(PathTrackingPidCalculations, ClosestPoseOnSegment_AtStart) { const auto start = create_transform(2, 2, 0); const auto end = create_transform(4, 4, 0); const auto point = start; const auto ref = start; - EXPECT_EQ(ref, closestPointOnSegment(point, start, end, false)); + EXPECT_EQ(ref, closestPoseOnSegment(point, start, end, false)); } -TEST(PathTrackingPidCalculations, ClosestPointOnSegment_CloseToEnd) +TEST(PathTrackingPidCalculations, ClosestPoseOnSegment_CloseToEnd) { const auto start = create_transform(2, 2, 0); const auto end = create_transform(4, 4, 0); const auto point = create_transform(7, 5, 0); const auto ref = end; - EXPECT_EQ(ref, closestPointOnSegment(point, start, end, false)); + EXPECT_EQ(ref, closestPoseOnSegment(point, start, end, false)); } -TEST(PathTrackingPidCalculations, ClosestPointOnSegment_CloseToStart) +TEST(PathTrackingPidCalculations, ClosestPoseOnSegment_CloseToStart) { const auto start = create_transform(2, 2, 0); const auto end = create_transform(4, 4, 0); const auto point = create_transform(-7, -5, 0); const auto ref = start; - EXPECT_EQ(ref, closestPointOnSegment(point, start, end, false)); + EXPECT_EQ(ref, closestPoseOnSegment(point, start, end, false)); } -TEST(PathTrackingPidCalculations, ClosestPointOnSegment_Halfway) +TEST(PathTrackingPidCalculations, ClosestPoseOnSegment_Halfway) { const auto start = create_transform(2, 2, 0); const auto end = create_transform(4, 4, 0); const auto point = create_transform(2, 4, 0); const auto ref = create_transform(3, 3, 0); - EXPECT_EQ(ref, closestPointOnSegment(point, start, end, false)); + EXPECT_EQ(ref, closestPoseOnSegment(point, start, end, false)); } -TEST(PathTrackingPidCalculations, ClosestPointOnSegment_TwoThirds) +TEST(PathTrackingPidCalculations, ClosestPoseOnSegment_TwoThirds) { const auto start = create_transform(2, 2, 0); const auto end = create_transform(8, 5, 0); const auto point = create_transform(2, 12, 0); const auto ref = create_transform(6, 4, 0); - EXPECT_EQ(ref, closestPointOnSegment(point, start, end, false)); + EXPECT_EQ(ref, closestPoseOnSegment(point, start, end, false)); } -TEST(PathTrackingPidCalculations, ClosestPointOnSegment_OtherYaw) +TEST(PathTrackingPidCalculations, ClosestPoseOnSegment_OtherYaw) { const auto start = tf2::Transform(create_quaternion(1, 1, 1), {2, 2, 0}); const auto end = create_transform(4, 4, 0); const auto point = create_transform(2, 4, 0); const auto ref = tf2::Transform(create_quaternion(0, 0, 1), {3, 3, 0}); - const auto result = closestPointOnSegment(point, start, end, false); + const auto result = closestPoseOnSegment(point, start, end, false); EXPECT_EQ(ref.getOrigin(), result.getOrigin()); // allow for small differences in the basis because of rounding errors in the calculations @@ -285,13 +285,13 @@ TEST(PathTrackingPidCalculations, ClosestPointOnSegment_OtherYaw) } } -TEST(PathTrackingPidCalculations, ClosestPointOnSegment_EstimateAngle) +TEST(PathTrackingPidCalculations, ClosestPoseOnSegment_EstimateAngle) { const auto start = create_transform(2, 2, 0); const auto end = create_transform(4, 4, 0); const auto point = create_transform(2, 4, 0); const auto ref = tf2::Transform(create_quaternion(0, 0, M_PI / 4.0), {3, 3, 0}); - const auto result = closestPointOnSegment(point, start, end, true); + const auto result = closestPoseOnSegment(point, start, end, true); EXPECT_EQ(ref.getOrigin(), result.getOrigin()); // allow for small differences in the basis because of rounding errors in the calculations