Merge pull request #133 from nobleo/refactor/122-move-test-closestPoi…

…ntOnSegment

Move, refactor and test closestPointOnSegment().

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);

src/calculations.cpp

@@ -1,5 +1,7 @@
 #include "calculations.hpp"
 
+#include <tf2/utils.h>
+
 #include <algorithm>
 #include <limits>
 #include <numeric>
@@ -76,4 +78,41 @@ double distSquared(const tf2::Transform & a, const tf2::Transform & b)
   return a.getOrigin().distance2(b.getOrigin());
 }
 
+tf2::Transform closestPoseOnSegment(
+  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

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 pose between a line segment and a point
+ *
+ * Calculate the closest pose 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 closestPoseOnSegment(
+  const tf2::Transform & point, const tf2::Transform & segment_start,
+  const tf2::Transform & segment_end, bool estimate_pose_angle);
+
 }  // namespace path_tracking_pid

src/controller.cpp

@@ -170,7 +170,7 @@ bool Controller::setPlan(
   for (int idx_path = static_cast<int>(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
@@ -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)
 {
@@ -323,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(

test/unittests/test_calculations.cpp

@@ -1,4 +1,5 @@
 #include <gtest/gtest.h>
+#include <tf2/utils.h>
 
 #include <limits>
 #include <vector>
@@ -8,6 +9,7 @@
 namespace
 {
 
+using path_tracking_pid::closestPoseOnSegment;
 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<tf2::Transform>{};
@@ -198,4 +208,98 @@ TEST(PathTrackingPidCalculations, DistSquared)
   EXPECT_EQ(0, distSquared(create_transform(1, 2, 3), create_transform(1, 2, 3)));
 }
 
+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, closestPoseOnSegment(point, start, end, false));
+}
+
+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, closestPoseOnSegment(point, start, end, false));
+}
+
+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, closestPoseOnSegment(point, start, end, false));
+}
+
+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, closestPoseOnSegment(point, start, end, false));
+}
+
+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, closestPoseOnSegment(point, start, end, false));
+}
+
+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, closestPoseOnSegment(point, start, end, false));
+}
+
+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 = 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
+  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, 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 = 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
+  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