chore: sync upstream

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

planning/obstacle_cruise_planner/docs/debug.md

@@ -5,41 +5,42 @@
 ### Detection area
 
 Green polygons which is a detection area is visualized by `detection_polygons` in the `~/debug/marker` topic.
+To determine each behavior (cruise, stop, and slow down), if `behavior_determination.*.max_lat_margin` is not zero, the polygons are expanded with the additional width.
 
-![detection_area](../image/detection_area.png)
+![detection_area](../media/detection_area.png)
 
-### Collision point
+### Collision points
 
-Red point which is a collision point with obstacle is visualized by `collision_points` in the `~/debug/marker` topic.
+Red points which are collision points with obstacle are visualized by `*_collision_points` for each behavior in the `~/debug/marker` topic.
 
-![collision_point](../image/collision_point.png)
+![collision_point](../media/collision_point.png)
 
 ### Obstacle for cruise
 
-Yellow sphere which is a obstacle for cruise is visualized by `obstacles_to_cruise` in the `~/debug/marker` topic.
+Orange sphere which is an obstacle for cruise is visualized by `obstacles_to_cruise` in the `~/debug/marker` topic.
 
-![obstacle_to_cruise](../image/obstacle_to_cruise.png)
+Orange wall which means a safe distance to cruise if the ego's front meets the wall is visualized in the `~/debug/cruise/virtual_wall` topic.
 
-### Obstacle for stop
+![cruise_visualization](../media/cruise_visualization.png)
 
-Red sphere which is a obstacle for stop is visualized by `obstacles_to_stop` in the `~/debug/marker` topic.
+### Obstacle for stop
 
-![obstacle_to_stop](../image/obstacle_to_stop.png)
+Red sphere which is an obstacle for stop is visualized by `obstacles_to_stop` in the `~/debug/marker` topic.
 
-<!-- ### Obstacle ignored to cruise or stop intentionally -->
+Red wall which means a safe distance to stop if the ego's front meets the wall is visualized in the `~/virtual_wall` topic.
 
-<!-- Green sphere which is a obstacle ignored intentionally to cruise or stop is visualized by `intentionally_ignored_obstacles` in the `~/debug/marker` topic. -->
+![stop_visualization](../media/stop_visualization.png)
 
-<!-- ![intentionally_ignored_obstacle](../image/intentionally_ignored_obstacle.png) -->
+### Obstacle for slow down
 
-### Obstacle cruise wall
+Yellow sphere which is an obstacle for slow_down is visualized by `obstacles_to_slow_down` in the `~/debug/marker` topic.
 
-Yellow wall which means a safe distance to cruise if the ego's front meets the wall is visualized in the `~/debug/cruise_wall_marker` topic.
+Yellow wall which means a safe distance to slow_down if the ego's front meets the wall is visualized in the `~/debug/slow_down/virtual_wall` topic.
 
-![obstacle_to_cruise](../image/obstacle_to_cruise.png)
+![slow_down_visualization](../media/slow_down_visualization.png)
 
-### Obstacle stop wall
+<!-- ### Obstacle ignored to cruise or stop intentionally -->
 
-Red wall which means a safe distance to stop if the ego's front meets the wall is visualized in the `~/debug/stop_wall_marker` topic.
+<!-- Green sphere which is a obstacle ignored intentionally to cruise or stop is visualized by `intentionally_ignored_obstacles` in the `~/debug/marker` topic. -->
 
-![obstacle_to_stop](../image/obstacle_to_stop.png)
+<!-- ![intentionally_ignored_obstacle](../image/intentionally_ignored_obstacle.png) -->

planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp

@@ -118,6 +118,7 @@ class ObstacleCruisePlannerNode : public rclcpp::Node
   rclcpp::Publisher<MarkerArray>::SharedPtr debug_slow_down_wall_marker_pub_;
   rclcpp::Publisher<Float32MultiArrayStamped>::SharedPtr debug_stop_planning_info_pub_;
   rclcpp::Publisher<Float32MultiArrayStamped>::SharedPtr debug_cruise_planning_info_pub_;
+  rclcpp::Publisher<Float32MultiArrayStamped>::SharedPtr debug_slow_down_planning_info_pub_;
   rclcpp::Publisher<Float32Stamped>::SharedPtr debug_calculation_time_pub_;
 
   // subscriber

planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp

@@ -86,6 +86,11 @@ class PlannerInterface
   {
     return Float32MultiArrayStamped{};
   }
+  Float32MultiArrayStamped getSlowDownPlanningDebugMessage(const rclcpp::Time & current_time)
+  {
+    slow_down_debug_multi_array_.stamp = current_time;
+    return slow_down_debug_multi_array_;
+  }
 
 protected:
   // Parameters
@@ -113,6 +118,7 @@ class PlannerInterface
 
   // debug info
   StopPlanningDebugInfo stop_planning_debug_info_;
+  Float32MultiArrayStamped slow_down_debug_multi_array_;
 
   double calcDistanceToCollisionPoint(
     const PlannerData & planner_data, const geometry_msgs::msg::Point & collision_point);