fix(out_of_lane): correct calculations of the stop pose (#9209)

Signed-off-by: Maxime CLEMENT <[email protected]>

planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.cpp

@@ -127,4 +127,21 @@ std::optional<geometry_msgs::msg::Pose> calculate_slowdown_point(
   }
   return slowdown_point;
 }
+
+void calculate_min_stop_and_slowdown_distances(
+  out_of_lane::EgoData & ego_data, const PlannerData & planner_data,
+  const std::optional<geometry_msgs::msg::Pose> & previous_slowdown_pose)
+{
+  ego_data.min_stop_distance = planner_data.calculate_min_deceleration_distance(0.0).value_or(0.0);
+  if (previous_slowdown_pose) {
+    // Ensure we do not remove the previous slowdown point due to the min distance limit
+    const auto previous_slowdown_pose_arc_length = motion_utils::calcSignedArcLength(
+      ego_data.trajectory_points, 0UL, previous_slowdown_pose->position);
+    ego_data.min_stop_distance =
+      std::min(previous_slowdown_pose_arc_length, ego_data.min_stop_distance);
+  }
+  ego_data.min_stop_arc_length =
+    ego_data.longitudinal_offset_to_first_trajectory_index + ego_data.min_stop_distance;
+}
+
 }  // namespace autoware::motion_velocity_planner::out_of_lane

planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp

@@ -17,6 +17,7 @@
 
 #include "types.hpp"
 
+#include <autoware/motion_velocity_planner_common/planner_data.hpp>
 #include <autoware/universe_utils/geometry/geometry.hpp>
 
 #include <geometry_msgs/msg/pose.hpp>
@@ -53,5 +54,13 @@ std::optional<geometry_msgs::msg::Pose> calculate_pose_ahead_of_collision(
 /// @return optional slowdown point to insert in the trajectory
 std::optional<geometry_msgs::msg::Pose> calculate_slowdown_point(
   const EgoData & ego_data, const OutOfLaneData & out_of_lane_data, PlannerParam params);
+
+/// @brief calculate the minimum stop and slowdown distances of ego
+/// @param [inout] ego_data ego data where minimum stop and slowdown distances are set
+/// @param [in] planner_data data with vehicle related information
+/// @param [in] previous_slowdown_pose previous slowdown pose
+void calculate_min_stop_and_slowdown_distances(
+  out_of_lane::EgoData & ego_data, const PlannerData & planner_data,
+  const std::optional<geometry_msgs::msg::Pose> & previous_slowdown_pose);
 }  // namespace autoware::motion_velocity_planner::out_of_lane
 #endif  // CALCULATE_SLOWDOWN_POINTS_HPP_

planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp

@@ -116,7 +116,7 @@ autoware_perception_msgs::msg::PredictedObjects filter_predicted_objects(
 
     const auto is_coming_from_behind =
       motion_utils::calcSignedArcLength(
-        ego_data.trajectory_points, ego_data.first_trajectory_idx,
+        ego_data.trajectory_points, 0UL,
         object.kinematics.initial_pose_with_covariance.pose.position) < 0.0;
     if (params.objects_ignore_behind_ego && is_coming_from_behind) {
       continue;

planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/footprint.cpp

@@ -60,7 +60,7 @@ std::vector<lanelet::BasicPolygon2d> calculate_trajectory_footprints(
   const auto base_footprint = make_base_footprint(params);
   std::vector<lanelet::BasicPolygon2d> trajectory_footprints;
   trajectory_footprints.reserve(ego_data.trajectory_points.size());
-  for (auto i = ego_data.first_trajectory_idx; i < ego_data.trajectory_points.size(); ++i) {
+  for (auto i = 0UL; i < ego_data.trajectory_points.size(); ++i) {
     const auto & trajectory_pose = ego_data.trajectory_points[i].pose;
     const auto angle = tf2::getYaw(trajectory_pose.orientation);
     const auto rotated_footprint = universe_utils::rotatePolygon(base_footprint, angle);

planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_collisions.cpp

@@ -159,7 +159,7 @@ std::vector<OutOfLanePoint> calculate_out_of_lane_points(const EgoData & ego_dat
     const auto & footprint = ego_data.trajectory_footprints[i];
     OutOfLanePoint p =
       calculate_out_of_lane_point(footprint, ego_data.out_lanelets, ego_data.out_lanelets_rtree);
-    p.trajectory_index = ego_data.first_trajectory_idx + i;
+    p.trajectory_index = i;
     if (!p.overlapped_lanelets.empty()) {
       out_of_lane_points.push_back(p);
     }

planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp

@@ -163,28 +163,6 @@ void OutOfLaneModule::limit_trajectory_size(
   }
 }
 
-void OutOfLaneModule::calculate_min_stop_and_slowdown_distances(
-  out_of_lane::EgoData & ego_data, const PlannerData & planner_data,
-  std::optional<geometry_msgs::msg::Pose> & previous_slowdown_pose_, const double slow_velocity)
-{
-  ego_data.min_stop_distance = planner_data.calculate_min_deceleration_distance(0.0).value_or(0.0);
-  ego_data.min_slowdown_distance =
-    planner_data.calculate_min_deceleration_distance(slow_velocity).value_or(0.0);
-  if (previous_slowdown_pose_) {
-    // Ensure we do not remove the previous slowdown point due to the min distance limit
-    const auto previous_slowdown_pose_arc_length = motion_utils::calcSignedArcLength(
-      ego_data.trajectory_points, ego_data.first_trajectory_idx, previous_slowdown_pose_->position);
-    ego_data.min_stop_distance =
-      std::min(previous_slowdown_pose_arc_length, ego_data.min_stop_distance);
-    ego_data.min_slowdown_distance =
-      std::min(previous_slowdown_pose_arc_length, ego_data.min_slowdown_distance);
-  }
-  ego_data.min_stop_arc_length = motion_utils::calcSignedArcLength(
-                                   ego_data.trajectory_points, 0UL, ego_data.first_trajectory_idx) +
-                                 ego_data.longitudinal_offset_to_first_trajectory_index +
-                                 ego_data.min_stop_distance;
-}
-
 void prepare_stop_lines_rtree(
   out_of_lane::EgoData & ego_data, const PlannerData & planner_data, const double search_distance)
 {
@@ -241,8 +219,8 @@ VelocityPlanningResult OutOfLaneModule::plan(
   out_of_lane::EgoData ego_data;
   ego_data.pose = planner_data->current_odometry.pose.pose;
   limit_trajectory_size(ego_data, ego_trajectory_points, params_.max_arc_length);
-  calculate_min_stop_and_slowdown_distances(
-    ego_data, *planner_data, previous_slowdown_pose_, params_.slow_velocity);
+  out_of_lane::calculate_min_stop_and_slowdown_distances(
+    ego_data, *planner_data, previous_slowdown_pose_);
   prepare_stop_lines_rtree(ego_data, *planner_data, params_.max_arc_length);
   const auto preprocessing_us = stopwatch.toc("preprocessing");
 
@@ -295,32 +273,30 @@ VelocityPlanningResult OutOfLaneModule::plan(
   auto slowdown_pose = out_of_lane::calculate_slowdown_point(ego_data, out_of_lane_data, params_);
   const auto calculate_slowdown_point_us = stopwatch.toc("calculate_slowdown_point");
 
-  if (  // reset the timer if there is no previous inserted point
-    slowdown_pose && (!previous_slowdown_pose_)) {
-    previous_slowdown_time_ = clock_->now();
-  }
   // reuse previous stop pose if there is no new one or if its velocity is not higher than the new
   // one and its arc length is lower
+  if (slowdown_pose) {  // reset the clock when we could calculate a valid slowdown pose
+    previous_slowdown_time_ = clock_->now();
+  }
   const auto should_use_previous_pose = [&]() {
     if (slowdown_pose && previous_slowdown_pose_) {
       const auto arc_length =
-        motion_utils::calcSignedArcLength(ego_trajectory_points, 0LU, slowdown_pose->position);
+        motion_utils::calcSignedArcLength(ego_data.trajectory_points, 0LU, slowdown_pose->position);
       const auto prev_arc_length = motion_utils::calcSignedArcLength(
-        ego_trajectory_points, 0LU, previous_slowdown_pose_->position);
+        ego_data.trajectory_points, 0LU, previous_slowdown_pose_->position);
       return prev_arc_length < arc_length;
     }
-    return !slowdown_pose && previous_slowdown_pose_;
+    return slowdown_pose && previous_slowdown_pose_;
   }();
   if (should_use_previous_pose) {
     // if the trajectory changed the prev point is no longer on the trajectory so we project it
     const auto new_arc_length = motion_utils::calcSignedArcLength(
-      ego_trajectory_points, 0LU, previous_slowdown_pose_->position);
-    slowdown_pose = motion_utils::calcInterpolatedPose(ego_trajectory_points, new_arc_length);
+      ego_data.trajectory_points, 0UL, previous_slowdown_pose_->position);
+    slowdown_pose = motion_utils::calcInterpolatedPose(ego_data.trajectory_points, new_arc_length);
   }
   if (slowdown_pose) {
     const auto arc_length =
-      motion_utils::calcSignedArcLength(
-        ego_trajectory_points, ego_data.first_trajectory_idx, slowdown_pose->position) -
+      motion_utils::calcSignedArcLength(ego_data.trajectory_points, 0UL, slowdown_pose->position) -
       ego_data.longitudinal_offset_to_first_trajectory_index;
     const auto slowdown_velocity =
       arc_length <= params_.stop_dist_threshold ? 0.0 : params_.slow_velocity;

planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.hpp

@@ -54,10 +54,6 @@ class OutOfLaneModule : public PluginModuleInterface
     out_of_lane::EgoData & ego_data,
     const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & ego_trajectory_points,
     const double max_arc_length);
-  /// @brief calculate the minimum stop and slowdown distances of ego
-  static void calculate_min_stop_and_slowdown_distances(
-    out_of_lane::EgoData & ego_data, const PlannerData & planner_data,
-    std::optional<geometry_msgs::msg::Pose> & previous_slowdown_pose_, const double slow_velocity);
 
   out_of_lane::PlannerParam params_{};
 

planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/types.hpp

@@ -94,19 +94,23 @@ using OutLaneletRtree = bgi::rtree<LaneletNode, bgi::rstar<16>>;
 /// @brief data related to the ego vehicle
 struct EgoData
 {
-  std::vector<autoware_planning_msgs::msg::TrajectoryPoint> trajectory_points;
+  std::vector<autoware_planning_msgs::msg::TrajectoryPoint>
+    trajectory_points;  // filtered trajectory starting from the 1st point behind ego
   geometry_msgs::msg::Pose pose;
-  size_t first_trajectory_idx{};
-  double longitudinal_offset_to_first_trajectory_index{};
+  size_t first_trajectory_idx{};  // segment index closest to ego on the original trajectory
+  double
+    longitudinal_offset_to_first_trajectory_index{};  // longitudinal offset of ego along the
+                                                      // closest segment on the original trajectory
   double min_stop_distance{};
-  double min_slowdown_distance{};
-  double min_stop_arc_length{};
+  double min_stop_arc_length{};  // [m] minimum arc length along the filtered trajectory where ego
+                                 // can stop
 
-  lanelet::ConstLanelets out_lanelets;
+  lanelet::ConstLanelets out_lanelets;  // lanelets where ego would be considered "out of lane"
   OutLaneletRtree out_lanelets_rtree;
 
   lanelet::BasicPolygon2d current_footprint;
-  std::vector<lanelet::BasicPolygon2d> trajectory_footprints;
+  std::vector<lanelet::BasicPolygon2d>
+    trajectory_footprints;  // ego footprints along the filtered trajectory
 
   StopLinesRtree stop_lines_rtree;
 };