feat(motion_utils): fix pose in interpolated pose function (tier4#1402)

* feat(motion_utils): fix pose in interpolated pose function

Signed-off-by: yutaka <purewater0901@gmail.com>

* replace the isDriving funciton with tier4_autoware_utils

Signed-off-by: yutaka <purewater0901@gmail.com>

common/motion_utils/include/motion_utils/trajectory/trajectory.hpp

@@ -97,19 +97,10 @@ bool isDrivingForward(const T points)
   }
 
   // check the first point direction
-  const auto & first_point_pose = tier4_autoware_utils::getPose(points.at(0));
-  const auto & second_point_pose = tier4_autoware_utils::getPose(points.at(1));
-
-  const double first_point_yaw = tf2::getYaw(first_point_pose.orientation);
-  const double driving_direction_yaw =
-    tier4_autoware_utils::calcAzimuthAngle(first_point_pose.position, second_point_pose.position);
-  if (
-    std::abs(tier4_autoware_utils::normalizeRadian(first_point_yaw - driving_direction_yaw)) <
-    tier4_autoware_utils::pi / 2.0) {
-    return true;
-  }
+  const auto & first_pose = tier4_autoware_utils::getPose(points.at(0));
+  const auto & second_pose = tier4_autoware_utils::getPose(points.at(1));
 
-  return false;
+  return tier4_autoware_utils::isDrivingForward(first_pose, second_pose);
 }
 
 template <class T>

common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp

@@ -21,6 +21,8 @@
 
 #define EIGEN_MPL2_ONLY
 #include "tier4_autoware_utils/geometry/boost_geometry.hpp"
+#include "tier4_autoware_utils/math/constants.hpp"
+#include "tier4_autoware_utils/math/normalization.hpp"
 
 #include <Eigen/Core>
 #include <Eigen/Geometry>
@@ -32,6 +34,8 @@
 #include <geometry_msgs/msg/quaternion.hpp>
 #include <geometry_msgs/msg/transform_stamped.hpp>
 
+#include <tf2/utils.h>
+
 #ifdef ROS_DISTRO_GALACTIC
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 #else
@@ -581,6 +585,19 @@ inline double calcCurvature(
   return 2.0 * ((p2.x - p1.x) * (p3.y - p1.y) - (p2.y - p1.y) * (p3.x - p1.x)) / denominator;
 }
 
+template <class Pose1, class Pose2>
+bool isDrivingForward(const Pose1 & src_pose, const Pose2 & dst_pose)
+{
+  // check the first point direction
+  const double src_yaw = tf2::getYaw(getPose(src_pose).orientation);
+  const double pose_direction_yaw = calcAzimuthAngle(getPoint(src_pose), getPoint(dst_pose));
+  if (std::fabs(normalizeRadian(src_yaw - pose_direction_yaw)) < pi / 2.0) {
+    return true;
+  }
+
+  return false;
+}
+
 /**
  * @brief Calculate offset pose. The offset values are defined in the local coordinate of the input
  * pose.
@@ -638,7 +655,7 @@ geometry_msgs::msg::Point calcInterpolatedPoint(
 
 /**
  * @brief Calculate a pose by linear interpolation.
- * Note that if ratio>=1.0 or dist(src_pose, dst_pose)<=0.01
+ * Note that if dist(src_pose, dst_pose)<=0.01
  * the orientation of the output pose is same as the orientation
  * of the dst_pose
  * @param src source point
@@ -653,20 +670,25 @@ geometry_msgs::msg::Pose calcInterpolatedPose(
   const bool set_orientation_from_position_direction = true)
 {
   const double clamped_ratio = std::clamp(ratio, 0.0, 1.0);
+
   geometry_msgs::msg::Pose output_pose;
   output_pose.position =
     calcInterpolatedPoint(getPoint(src_pose), getPoint(dst_pose), clamped_ratio);
 
   if (set_orientation_from_position_direction) {
     const double input_poses_dist = calcDistance2d(getPoint(src_pose), getPoint(dst_pose));
+    const bool is_driving_forward = isDrivingForward(src_pose, dst_pose);
 
     // Get orientation from interpolated point and src_pose
-    if (clamped_ratio < 1.0 && input_poses_dist > 1e-3) {
-      const double pitch = calcElevationAngle(getPoint(output_pose), getPoint(dst_pose));
-      const double yaw = calcAzimuthAngle(output_pose.position, getPoint(dst_pose));
-      output_pose.orientation = createQuaternionFromRPY(0.0, pitch, yaw);
-    } else {
+    if ((is_driving_forward && clamped_ratio > 1.0 - (1e-6)) || input_poses_dist < 1e-3) {
       output_pose.orientation = getPose(dst_pose).orientation;
+    } else if (!is_driving_forward && clamped_ratio < 1e-6) {
+      output_pose.orientation = getPose(src_pose).orientation;
+    } else {
+      const auto & base_pose = is_driving_forward ? dst_pose : src_pose;
+      const double pitch = calcElevationAngle(getPoint(output_pose), getPoint(base_pose));
+      const double yaw = calcAzimuthAngle(getPoint(output_pose), getPoint(base_pose));
+      output_pose.orientation = createQuaternionFromRPY(0.0, pitch, yaw);
     }
   } else {
     // Get orientation by spherical linear interpolation

common/tier4_autoware_utils/test/src/geometry/test_geometry.cpp

@@ -1083,6 +1083,68 @@ TEST(geometry, calcOffsetPose)
   }
 }
 
+TEST(geometry, isDrivingForward)
+{
+  using tier4_autoware_utils::calcInterpolatedPoint;
+  using tier4_autoware_utils::createPoint;
+  using tier4_autoware_utils::createQuaternion;
+  using tier4_autoware_utils::createQuaternionFromRPY;
+  using tier4_autoware_utils::deg2rad;
+  using tier4_autoware_utils::isDrivingForward;
+
+  const double epsilon = 1e-3;
+
+  {
+    geometry_msgs::msg::Pose src_pose;
+    src_pose.position = createPoint(0.0, 0.0, 0.0);
+    src_pose.orientation = createQuaternion(0.0, 0.0, 0.0, 1.0);
+
+    geometry_msgs::msg::Pose dst_pose;
+    dst_pose.position = createPoint(3.0, 0.0, 0.0);
+    dst_pose.orientation = createQuaternion(0.0, 0.0, 0.0, 1.0);
+
+    EXPECT_TRUE(isDrivingForward(src_pose, dst_pose));
+  }
+
+  {
+    geometry_msgs::msg::Pose src_pose;
+    src_pose.position = createPoint(0.0, 0.0, 0.0);
+    src_pose.orientation = createQuaternionFromRPY(0.0, 0.0, deg2rad(180));
+
+    geometry_msgs::msg::Pose dst_pose;
+    dst_pose.position = createPoint(3.0, 0.0, 0.0);
+    dst_pose.orientation = createQuaternionFromRPY(0.0, 0.0, deg2rad(180));
+
+    EXPECT_FALSE(isDrivingForward(src_pose, dst_pose));
+  }
+
+  // Boundary Condition
+  {
+    geometry_msgs::msg::Pose src_pose;
+    src_pose.position = createPoint(0.0, 0.0, 0.0);
+    src_pose.orientation = createQuaternionFromRPY(0.0, 0.0, deg2rad(90));
+
+    geometry_msgs::msg::Pose dst_pose;
+    dst_pose.position = createPoint(3.0, 0.0, 0.0);
+    dst_pose.orientation = createQuaternionFromRPY(0.0, 0.0, deg2rad(90));
+
+    EXPECT_TRUE(isDrivingForward(src_pose, dst_pose));
+  }
+
+  // Boundary Condition
+  {
+    geometry_msgs::msg::Pose src_pose;
+    src_pose.position = createPoint(0.0, 0.0, 0.0);
+    src_pose.orientation = createQuaternionFromRPY(0.0, 0.0, deg2rad(90 + epsilon));
+
+    geometry_msgs::msg::Pose dst_pose;
+    dst_pose.position = createPoint(3.0, 0.0, 0.0);
+    dst_pose.orientation = createQuaternionFromRPY(0.0, 0.0, deg2rad(90 + epsilon));
+
+    EXPECT_FALSE(isDrivingForward(src_pose, dst_pose));
+  }
+}
+
 TEST(geometry, calcInterpolatedPoint)
 {
   using tier4_autoware_utils::calcInterpolatedPoint;
@@ -1226,7 +1288,7 @@ TEST(geometry, calcInterpolatedPose)
     dst_pose.position = createPoint(1.0, 1.0, 0.0);
     dst_pose.orientation = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(60));
 
-    for (double ratio = 0.0; ratio < 1.0; ratio += 0.1) {
+    for (double ratio = 0.0; ratio < 1.0 - epsilon; ratio += 0.1) {
       const auto p_out = calcInterpolatedPose(src_pose, dst_pose, ratio);
 
       const auto ans_quat = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(45));
@@ -1298,6 +1360,30 @@ TEST(geometry, calcInterpolatedPose)
       EXPECT_DOUBLE_EQ(p_out.orientation.w, ans_quat.w);
     }
   }
+
+  // Driving Backward
+  {
+    geometry_msgs::msg::Pose src_pose;
+    src_pose.position = createPoint(0.0, 0.0, 0.0);
+    src_pose.orientation = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(180));
+
+    geometry_msgs::msg::Pose dst_pose;
+    dst_pose.position = createPoint(5.0, 0.0, 0.0);
+    dst_pose.orientation = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(180));
+
+    for (double ratio = 0.0; ratio < 1.0 + epsilon; ratio += 0.1) {
+      const auto p_out = calcInterpolatedPose(src_pose, dst_pose, ratio);
+
+      const auto ans_quat = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(180));
+      EXPECT_DOUBLE_EQ(p_out.position.x, 5.0 * ratio);
+      EXPECT_DOUBLE_EQ(p_out.position.y, 0.0);
+      EXPECT_DOUBLE_EQ(p_out.position.z, 0.0);
+      EXPECT_DOUBLE_EQ(p_out.orientation.x, ans_quat.x);
+      EXPECT_DOUBLE_EQ(p_out.orientation.y, ans_quat.y);
+      EXPECT_DOUBLE_EQ(p_out.orientation.z, ans_quat.z);
+      EXPECT_DOUBLE_EQ(p_out.orientation.w, ans_quat.w);
+    }
+  }
 }
 
 TEST(geometry, calcInterpolatedPose_with_Spherical_Interpolation)