feat(motion_utils): add calcYawDeviation (#3491)

* feat(motion_utils): add calcYawDeviation

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* fix from shimizu-san review

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* fix

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

---------

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

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

@@ -1728,6 +1728,52 @@ T cropPoints(
 
   return cropped_points;
 }
+
+/**
+ * @brief Calculate the angle of the input pose with respect to the nearest trajectory segment.
+ * The function gets the nearest segment index between the points of the trajectory and the given
+ * pose's position, then calculates the azimuth angle of that segment and compares it to the yaw of
+ * the input pose. The segment is a straight path between two continuous points of the trajectory.
+ * @param points Points of the trajectory, path, ...
+ * @param pose Input pose with position and orientation (yaw)
+ * @param throw_exception Flag to enable/disable exception throwing
+ * @return Angle with respect to the trajectory segment (signed) in radians
+ */
+template <class T>
+double calcYawDeviation(
+  const T & points, const geometry_msgs::msg::Pose & pose, const bool throw_exception = false)
+{
+  const auto overlap_removed_points = removeOverlapPoints(points, 0);
+
+  if (throw_exception) {
+    validateNonEmpty(overlap_removed_points);
+  } else {
+    try {
+      validateNonEmpty(overlap_removed_points);
+    } catch (const std::exception & e) {
+      std::cerr << e.what() << std::endl;
+      return 0.0;
+    }
+  }
+
+  if (overlap_removed_points.size() <= 1) {
+    const std::runtime_error e("points size is less than 2");
+    if (throw_exception) {
+      throw e;
+    }
+    std::cerr << e.what() << std::endl;
+    return 0.0;
+  }
+
+  const size_t seg_idx = findNearestSegmentIndex(overlap_removed_points, pose.position);
+
+  const double path_yaw = tier4_autoware_utils::calcAzimuthAngle(
+    tier4_autoware_utils::getPoint(overlap_removed_points.at(seg_idx)),
+    tier4_autoware_utils::getPoint(overlap_removed_points.at(seg_idx + 1)));
+  const double pose_yaw = tf2::getYaw(pose.orientation);
+
+  return tier4_autoware_utils::normalizeRadian(pose_yaw - path_yaw);
+}
 }  // namespace motion_utils
 
 #endif  // MOTION_UTILS__TRAJECTORY__TRAJECTORY_HPP_

common/motion_utils/test/src/trajectory/test_trajectory.cpp

@@ -5238,3 +5238,30 @@ TEST(Trajectory, removeInvalidOrientationPoints)
     },
     1);  // expected size is 1 since only the first point remains
 }
+
+TEST(trajectory, calcYawDeviation)
+{
+  using autoware_auto_planning_msgs::msg::TrajectoryPoint;
+  using motion_utils::calcYawDeviation;
+
+  constexpr double tolerance = 1e-3;
+
+  // Generate test trajectory
+  const auto trajectory = generateTestTrajectory<Trajectory>(4, 10.0, 0.0, 0.0, M_PI / 8);
+
+  // check with fist point
+  {
+    const auto input_pose = createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
+    const double yaw_deviation = calcYawDeviation(trajectory.points, input_pose);
+    constexpr double expected_yaw_deviation = -M_PI / 8;
+    EXPECT_NEAR(yaw_deviation, expected_yaw_deviation, tolerance);
+  }
+
+  // check with middle point
+  {
+    const auto input_pose = createPose(10.0, 10.0, 0.0, 0.0, 0.0, M_PI / 8);
+    const double yaw_deviation = calcYawDeviation(trajectory.points, input_pose);
+    constexpr double expected_yaw_deviation = -0.734;
+    EXPECT_NEAR(yaw_deviation, expected_yaw_deviation, tolerance);
+  }
+}