feat(motion_utils): fix orientaion (#1397)

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

@@ -89,6 +89,45 @@ void validateNonSharpAngle(
   }
 }
 
+template <class T>
+bool isDrivingForward(const T points)
+{
+  if (points.size() < 2) {
+    return true;
+  }
+
+  // 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;
+  }
+
+  return false;
+}
+
+template <class T>
+bool isDrivingForwardWithTwist(const T points_with_twist)
+{
+  if (points_with_twist.empty()) {
+    return true;
+  }
+  if (points_with_twist.size() == 1) {
+    if (0.0 <= tier4_autoware_utils::getLongitudinalVelocity(points_with_twist.front())) {
+      return true;
+    }
+    return false;
+  }
+
+  return isDrivingForward(points_with_twist);
+}
+
 template <class T>
 boost::optional<size_t> searchZeroVelocityIndex(
   const T & points_with_twist, const size_t src_idx, const size_t dst_idx)
@@ -805,10 +844,13 @@ inline boost::optional<size_t> insertTargetPoint(
   const auto overlap_with_back =
     tier4_autoware_utils::calcDistance2d(p_target, p_back) < overlap_threshold;
 
+  const bool is_driving_forward = isDrivingForward(points);
+
   geometry_msgs::msg::Pose target_pose;
   {
-    const auto pitch = tier4_autoware_utils::calcElevationAngle(p_target, p_back);
-    const auto yaw = tier4_autoware_utils::calcAzimuthAngle(p_target, p_back);
+    const auto p_base = is_driving_forward ? p_back : p_front;
+    const auto pitch = tier4_autoware_utils::calcElevationAngle(p_target, p_base);
+    const auto yaw = tier4_autoware_utils::calcAzimuthAngle(p_target, p_base);
 
     target_pose.position = p_target;
     target_pose.orientation = tier4_autoware_utils::createQuaternionFromRPY(0.0, pitch, yaw);
@@ -817,17 +859,22 @@ inline boost::optional<size_t> insertTargetPoint(
   auto p_insert = points.at(seg_idx);
   tier4_autoware_utils::setPose(target_pose, p_insert);
 
-  geometry_msgs::msg::Pose front_pose;
+  geometry_msgs::msg::Pose base_pose;
   {
-    const auto pitch = tier4_autoware_utils::calcElevationAngle(p_front, p_target);
-    const auto yaw = tier4_autoware_utils::calcAzimuthAngle(p_front, p_target);
+    const auto p_base = is_driving_forward ? p_front : p_back;
+    const auto pitch = tier4_autoware_utils::calcElevationAngle(p_base, p_target);
+    const auto yaw = tier4_autoware_utils::calcAzimuthAngle(p_base, p_target);
 
-    front_pose.position = tier4_autoware_utils::getPoint(points.at(seg_idx));
-    front_pose.orientation = tier4_autoware_utils::createQuaternionFromRPY(0.0, pitch, yaw);
+    base_pose.position = tier4_autoware_utils::getPoint(p_base);
+    base_pose.orientation = tier4_autoware_utils::createQuaternionFromRPY(0.0, pitch, yaw);
   }
 
   if (!overlap_with_front && !overlap_with_back) {
-    tier4_autoware_utils::setPose(front_pose, points.at(seg_idx));
+    if (is_driving_forward) {
+      tier4_autoware_utils::setPose(base_pose, points.at(seg_idx));
+    } else {
+      tier4_autoware_utils::setPose(base_pose, points.at(seg_idx + 1));
+    }
     points.insert(points.begin() + seg_idx + 1, p_insert);
     return seg_idx + 1;
   }
@@ -1012,36 +1059,6 @@ inline boost::optional<size_t> insertStopPoint(
 
   return stop_idx;
 }
-
-template <class T>
-inline bool isDrivingForward(const T points_with_twist)
-{
-  // if points size is smaller than 2
-  if (points_with_twist.empty()) {
-    return true;
-  }
-  if (points_with_twist.size() == 1) {
-    if (0.0 <= tier4_autoware_utils::getLongitudinalVelocity(points_with_twist.front())) {
-      return true;
-    }
-    return false;
-  }
-
-  // check the first point direction
-  const auto & first_point_pose = tier4_autoware_utils::getPose(points_with_twist.at(0));
-  const auto & second_point_pose = tier4_autoware_utils::getPose(points_with_twist.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)) <
-    M_PI_2) {
-    return true;
-  }
-
-  return false;
-}
 }  // namespace motion_utils
 
 #endif  // MOTION_UTILS__TRAJECTORY__TRAJECTORY_HPP_

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

@@ -1691,6 +1691,64 @@ TEST(trajectory, insertTargetPoint)
   }
 }
 
+TEST(trajectory, insertTargetPoint_Reverse)
+{
+  using motion_utils::calcArcLength;
+  using motion_utils::findNearestSegmentIndex;
+  using motion_utils::insertTargetPoint;
+  using tier4_autoware_utils::calcDistance2d;
+  using tier4_autoware_utils::createPoint;
+  using tier4_autoware_utils::createQuaternionFromYaw;
+  using tier4_autoware_utils::deg2rad;
+  using tier4_autoware_utils::getPose;
+
+  constexpr double overlap_threshold = 1e-4;
+  auto traj = generateTestTrajectory<Trajectory>(10, 1.0);
+  for (size_t i = 0; i < traj.points.size(); ++i) {
+    traj.points.at(i).pose.orientation = createQuaternionFromYaw(tier4_autoware_utils::pi);
+  }
+  const auto total_length = calcArcLength(traj.points);
+
+  for (double x_start = 0.0; x_start < total_length; x_start += 1.0) {
+    auto traj_out = traj;
+
+    const auto p_target = createPoint(x_start + 1.1e-4, 0.0, 0.0);
+    const size_t base_idx = findNearestSegmentIndex(traj.points, p_target);
+    const auto insert_idx =
+      insertTargetPoint(base_idx, p_target, traj_out.points, overlap_threshold);
+
+    EXPECT_NE(insert_idx, boost::none);
+    EXPECT_EQ(insert_idx.get(), base_idx + 1);
+    EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1);
+
+    for (size_t i = 0; i < traj_out.points.size() - 1; ++i) {
+      EXPECT_TRUE(
+        calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > overlap_threshold);
+    }
+
+    {
+      const auto p_insert = getPose(traj_out.points.at(insert_idx.get()));
+      const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(180));
+      EXPECT_EQ(p_insert.position.x, p_target.x);
+      EXPECT_EQ(p_insert.position.y, p_target.y);
+      EXPECT_EQ(p_insert.position.z, p_target.z);
+      EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon);
+      EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon);
+      EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon);
+      EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon);
+    }
+
+    {
+      const auto p_base = getPose(traj_out.points.at(base_idx));
+      const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(180));
+      EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon);
+      EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon);
+      EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon);
+      EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon);
+    }
+  }
+}
+
 TEST(trajectory, insertTargetPoint_OverlapThreshold)
 {
   using motion_utils::calcArcLength;

planning/obstacle_cruise_planner/src/node.cpp

@@ -516,6 +516,7 @@ void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr ms
 
   // Get Target Obstacles
   DebugData debug_data;
+  const bool is_driving_forward = motion_utils::isDrivingForwardWithTwist(msg->points);
   const auto target_obstacles = getTargetObstacles(
     *msg, current_pose_ptr->pose, current_twist_ptr_->twist.linear.x, debug_data);