feat(obstacle_cruise_planner): prior generation of collision distance (…

…autowarefoundation#5619)

* set goal extension param as unused
* change to use collision point
* fill out z coordinate value
* handle with the backward paths

Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>

planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp

@@ -110,14 +110,20 @@ struct StopObstacle : public TargetObstacleInterface
     const std::string & arg_uuid, const rclcpp::Time & arg_stamp,
     const geometry_msgs::msg::Pose & arg_pose, const Shape & arg_shape,
     const double arg_lon_velocity, const double arg_lat_velocity,
-    const geometry_msgs::msg::Point arg_collision_point)
+    const geometry_msgs::msg::Point arg_collision_point,
+    const double arg_dist_to_collide_on_decimated_traj)
   : TargetObstacleInterface(arg_uuid, arg_stamp, arg_pose, arg_lon_velocity, arg_lat_velocity),
     shape(arg_shape),
-    collision_point(arg_collision_point)
+    collision_point(arg_collision_point),
+    dist_to_collide_on_decimated_traj(arg_dist_to_collide_on_decimated_traj)
   {
   }
   Shape shape;
-  geometry_msgs::msg::Point collision_point;
+  geometry_msgs::msg::Point
+    collision_point;  // TODO(yuki_takagi): this member variable still used in
+                      // calculateMarginFromObstacleOnCurve() and  should be removed as it can be
+                      // replaced by ”dist_to_collide_on_decimated_traj”
+  double dist_to_collide_on_decimated_traj;
 };
 
 struct CruiseObstacle : public TargetObstacleInterface

planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp

@@ -91,7 +91,7 @@ class ObstacleCruisePlannerNode : public rclcpp::Node
 
   void checkConsistency(
     const rclcpp::Time & current_time, const PredictedObjects & predicted_objects,
-    const std::vector<TrajectoryPoint> & traj_points, std::vector<StopObstacle> & stop_obstacles);
+    std::vector<StopObstacle> & stop_obstacles);
   void publishVelocityLimit(
     const std::optional<VelocityLimit> & vel_limit, const std::string & module_name);
   void publishDebugMarker() const;

planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp

@@ -107,6 +107,7 @@ class PlannerInterface
     slow_down_debug_multi_array_.stamp = current_time;
     return slow_down_debug_multi_array_;
   }
+  double getSafeDistanceMargin() const { return longitudinal_info_.safe_distance_margin; }
 
 protected:
   // Parameters

planning/obstacle_cruise_planner/include/obstacle_cruise_planner/polygon_utils.hpp

@@ -38,9 +38,10 @@ Polygon2d createOneStepPolygon(
   const std::vector<geometry_msgs::msg::Pose> & current_poses,
   const vehicle_info_util::VehicleInfo & vehicle_info, const double lat_margin);
 
-std::optional<geometry_msgs::msg::Point> getCollisionPoint(
+std::optional<std::pair<geometry_msgs::msg::Point, double>> getCollisionPoint(
   const std::vector<TrajectoryPoint> & traj_points, const std::vector<Polygon2d> & traj_polygons,
-  const Obstacle & obstacle, const bool is_driving_forward);
+  const Obstacle & obstacle, const bool is_driving_forward,
+  const vehicle_info_util::VehicleInfo & vehicle_info);
 
 std::vector<PointWithStamp> getCollisionPoints(
   const std::vector<TrajectoryPoint> & traj_points, const std::vector<Polygon2d> & traj_polygons,

planning/obstacle_cruise_planner/include/obstacle_cruise_planner/utils.hpp

@@ -37,7 +37,6 @@ PoseWithStamp getCurrentObjectPose(
   const rclcpp::Time & current_time, const bool use_prediction);
 
 std::optional<StopObstacle> getClosestStopObstacle(
-  const std::vector<TrajectoryPoint> & traj_points,
   const std::vector<StopObstacle> & stop_obstacles);
 
 template <class T>

planning/obstacle_cruise_planner/src/node.cpp

@@ -761,7 +761,7 @@ ObstacleCruisePlannerNode::determineEgoBehaviorAgainstObstacles(
   slow_down_condition_counter_.removeCounterUnlessUpdated();
 
   // Check target obstacles' consistency
-  checkConsistency(objects_ptr_->header.stamp, *objects_ptr_, traj_points, stop_obstacles);
+  checkConsistency(objects_ptr_->header.stamp, *objects_ptr_, stop_obstacles);
 
   // update previous obstacles
   prev_stop_obstacles_ = stop_obstacles;
@@ -794,7 +794,8 @@ std::vector<TrajectoryPoint> ObstacleCruisePlannerNode::decimateTrajectoryPoints
 
   // extend trajectory
   const auto extended_traj_points = extendTrajectoryPoints(
-    decimated_traj_points, p.goal_extension_length, p.goal_extension_interval);
+    decimated_traj_points, planner_ptr_->getSafeDistanceMargin(),
+    p.decimate_trajectory_step_length);
   if (extended_traj_points.size() < 2) {
     return traj_points;
   }
@@ -974,6 +975,7 @@ std::optional<StopObstacle> ObstacleCruisePlannerNode::createStopObstacle(
   const Obstacle & obstacle, const double precise_lat_dist) const
 {
   const auto & p = behavior_determination_param_;
+  const auto & object_id = obstacle.uuid.substr(0, 4);
 
   // NOTE: consider all target obstacles when driving backward
   if (!isStopObstacle(obstacle.classification.label)) {
@@ -994,25 +996,6 @@ std::optional<StopObstacle> ObstacleCruisePlannerNode::createStopObstacle(
     }
   }
 
-  const auto collision_point =
-    createCollisionPointForStopObstacle(traj_points, traj_polys, obstacle);
-  if (!collision_point) {
-    return std::nullopt;
-  }
-
-  const auto [tangent_vel, normal_vel] = projectObstacleVelocityToTrajectory(traj_points, obstacle);
-  return StopObstacle{obstacle.uuid, obstacle.stamp, obstacle.pose,   obstacle.shape,
-                      tangent_vel,   normal_vel,     *collision_point};
-}
-
-std::optional<geometry_msgs::msg::Point>
-ObstacleCruisePlannerNode::createCollisionPointForStopObstacle(
-  const std::vector<TrajectoryPoint> & traj_points, const std::vector<Polygon2d> & traj_polys,
-  const Obstacle & obstacle) const
-{
-  const auto & p = behavior_determination_param_;
-  const auto & object_id = obstacle.uuid.substr(0, 4);
-
   // NOTE: Dynamic obstacles for stop are crossing ego's trajectory with high speed,
   //       and the collision between ego and obstacles are within the margin threshold.
   const bool is_obstacle_crossing = isObstacleCrossing(traj_points, obstacle);
@@ -1054,9 +1037,17 @@ ObstacleCruisePlannerNode::createCollisionPointForStopObstacle(
   // calculate collision points with trajectory with lateral stop margin
   const auto traj_polys_with_lat_margin = createOneStepPolygons(
     traj_points, vehicle_info_, ego_odom_ptr_->pose.pose, p.max_lat_margin_for_stop);
+
   const auto collision_point = polygon_utils::getCollisionPoint(
-    traj_points, traj_polys_with_lat_margin, obstacle, is_driving_forward_);
-  return collision_point;
+    traj_points, traj_polys_with_lat_margin, obstacle, is_driving_forward_, vehicle_info_);
+  if (!collision_point) {
+    return std::nullopt;
+  }
+
+  const auto [tangent_vel, normal_vel] = projectObstacleVelocityToTrajectory(traj_points, obstacle);
+  return StopObstacle{
+    obstacle.uuid, obstacle.stamp, obstacle.pose,          obstacle.shape,
+    tangent_vel,   normal_vel,     collision_point->first, collision_point->second};
 }
 
 std::optional<SlowDownObstacle> ObstacleCruisePlannerNode::createSlowDownObstacle(
@@ -1185,10 +1176,10 @@ std::optional<SlowDownObstacle> ObstacleCruisePlannerNode::createSlowDownObstacl
 
 void ObstacleCruisePlannerNode::checkConsistency(
   const rclcpp::Time & current_time, const PredictedObjects & predicted_objects,
-  const std::vector<TrajectoryPoint> & traj_points, std::vector<StopObstacle> & stop_obstacles)
+  std::vector<StopObstacle> & stop_obstacles)
 {
   const auto current_closest_stop_obstacle =
-    obstacle_cruise_utils::getClosestStopObstacle(traj_points, stop_obstacles);
+    obstacle_cruise_utils::getClosestStopObstacle(stop_obstacles);
 
   // If previous closest obstacle ptr is not set
   if (!prev_closest_stop_obstacle_ptr_) {

planning/obstacle_cruise_planner/src/planner_interface.cpp

@@ -24,6 +24,7 @@
 
 #include <boost/geometry/algorithms/distance.hpp>
 #include <boost/geometry/strategies/strategies.hpp>
+
 namespace
 {
 StopSpeedExceeded createStopSpeedExceededMsg(
@@ -257,8 +258,7 @@ std::vector<TrajectoryPoint> PlannerInterface::generateStopTrajectory(
     "stop planning");
 
   // Get Closest Stop Obstacle
-  const auto closest_stop_obstacle =
-    obstacle_cruise_utils::getClosestStopObstacle(planner_data.traj_points, stop_obstacles);
+  const auto closest_stop_obstacle = obstacle_cruise_utils::getClosestStopObstacle(stop_obstacles);
   if (!closest_stop_obstacle) {
     // delete marker
     const auto markers =
@@ -269,85 +269,58 @@ std::vector<TrajectoryPoint> PlannerInterface::generateStopTrajectory(
     return planner_data.traj_points;
   }
 
-  // Get Closest Obstacle Stop Distance
-  const double closest_obstacle_dist = motion_utils::calcSignedArcLength(
-    planner_data.traj_points, 0, closest_stop_obstacle->collision_point);
-
   const auto ego_segment_idx =
     ego_nearest_param_.findSegmentIndex(planner_data.traj_points, planner_data.ego_pose);
-  const auto negative_dist_to_ego = motion_utils::calcSignedArcLength(
-    planner_data.traj_points, planner_data.ego_pose.position, ego_segment_idx, 0);
-  const double dist_to_ego = -negative_dist_to_ego;
+  const double dist_to_collide_on_ref_traj =
+    motion_utils::calcSignedArcLength(planner_data.traj_points, 0, ego_segment_idx) +
+    closest_stop_obstacle->dist_to_collide_on_decimated_traj;
 
-  const double margin_from_obstacle =
-    calculateMarginFromObstacleOnCurve(planner_data, *closest_stop_obstacle);
-
-  // If behavior stop point is ahead of the closest_obstacle_stop point within a certain margin
-  // we set closest_obstacle_stop_distance to closest_behavior_stop_distance
   const double margin_from_obstacle_considering_behavior_module = [&]() {
-    const size_t nearest_segment_idx =
-      findEgoSegmentIndex(planner_data.traj_points, planner_data.ego_pose);
-    const auto closest_behavior_stop_idx =
-      motion_utils::searchZeroVelocityIndex(planner_data.traj_points, nearest_segment_idx + 1);
-
-    if (!closest_behavior_stop_idx) {
-      return margin_from_obstacle;
+    const double margin_from_obstacle =
+      calculateMarginFromObstacleOnCurve(planner_data, *closest_stop_obstacle);
+    // Use terminal margin (terminal_safe_distance_margin) for obstacle stop
+    const auto ref_traj_length = motion_utils::calcSignedArcLength(
+      planner_data.traj_points, 0, planner_data.traj_points.size() - 1);
+    if (dist_to_collide_on_ref_traj > ref_traj_length) {
+      return longitudinal_info_.terminal_safe_distance_margin;
     }
 
-    const double closest_behavior_stop_dist_from_ego = motion_utils::calcSignedArcLength(
-      planner_data.traj_points, planner_data.ego_pose.position, nearest_segment_idx,
-      *closest_behavior_stop_idx);
-
-    if (*closest_behavior_stop_idx == planner_data.traj_points.size() - 1) {
-      // Closest behavior stop point is the end point
-      const double closest_obstacle_stop_dist_from_ego =
-        closest_obstacle_dist - dist_to_ego - longitudinal_info_.terminal_safe_distance_margin -
-        abs_ego_offset;
-      const double stop_dist_diff =
-        closest_behavior_stop_dist_from_ego - closest_obstacle_stop_dist_from_ego;
-      if (stop_dist_diff < margin_from_obstacle) {
-        // Use terminal margin (terminal_safe_distance_margin) for obstacle stop
-        return longitudinal_info_.terminal_safe_distance_margin;
-      }
-    } else {
-      const double closest_obstacle_stop_dist_from_ego =
-        closest_obstacle_dist - dist_to_ego - margin_from_obstacle - abs_ego_offset;
-      const double stop_dist_diff =
-        closest_behavior_stop_dist_from_ego - closest_obstacle_stop_dist_from_ego;
+    // If behavior stop point is ahead of the closest_obstacle_stop point within a certain margin
+    // we set closest_obstacle_stop_distance to closest_behavior_stop_distance
+    const auto closest_behavior_stop_idx =
+      motion_utils::searchZeroVelocityIndex(planner_data.traj_points, ego_segment_idx + 1);
+    if (closest_behavior_stop_idx) {
+      const double closest_behavior_stop_dist_on_ref_traj =
+        motion_utils::calcSignedArcLength(planner_data.traj_points, 0, *closest_behavior_stop_idx);
+      const double stop_dist_diff = closest_behavior_stop_dist_on_ref_traj -
+                                    (dist_to_collide_on_ref_traj - margin_from_obstacle);
       if (0.0 < stop_dist_diff && stop_dist_diff < margin_from_obstacle) {
-        // Use shorter margin (min_behavior_stop_margin) for obstacle stop
         return min_behavior_stop_margin_;
       }
     }
     return margin_from_obstacle;
   }();
 
-  const auto [stop_margin_from_obstacle, will_collide_with_obstacle] = [&]() {
+  // Generate Output Trajectory
+  const auto [zero_vel_dist, will_collide_with_obstacle] = [&]() {
+    double candidate_zero_vel_dist =
+      std::max(0.0, dist_to_collide_on_ref_traj - margin_from_obstacle_considering_behavior_module);
+
     // Check feasibility to stop
     if (suppress_sudden_obstacle_stop_) {
-      const double closest_obstacle_stop_dist =
-        closest_obstacle_dist - margin_from_obstacle_considering_behavior_module - abs_ego_offset;
-
       // Calculate feasible stop margin (Check the feasibility)
-      const double feasible_stop_dist = calcMinimumDistanceToStop(
-                                          planner_data.ego_vel, longitudinal_info_.limit_max_accel,
-                                          longitudinal_info_.limit_min_accel) +
-                                        dist_to_ego;
-
-      if (closest_obstacle_stop_dist < feasible_stop_dist) {
-        const auto feasible_margin_from_obstacle =
-          margin_from_obstacle_considering_behavior_module -
-          (feasible_stop_dist - closest_obstacle_stop_dist);
-        return std::make_pair(feasible_margin_from_obstacle, true);
+      const double feasible_stop_dist =
+        calcMinimumDistanceToStop(
+          planner_data.ego_vel, longitudinal_info_.limit_max_accel,
+          longitudinal_info_.limit_min_accel) +
+        motion_utils::calcSignedArcLength(
+          planner_data.traj_points, 0, planner_data.ego_pose.position);
+
+      if (candidate_zero_vel_dist < feasible_stop_dist) {
+        candidate_zero_vel_dist = feasible_stop_dist;
+        return std::make_pair(candidate_zero_vel_dist, true);
       }
     }
-    return std::make_pair(margin_from_obstacle_considering_behavior_module, false);
-  }();
-
-  // Generate Output Trajectory
-  const double zero_vel_dist = [&]() {
-    const double current_zero_vel_dist =
-      std::max(0.0, closest_obstacle_dist - abs_ego_offset - stop_margin_from_obstacle);
 
     // Hold previous stop distance if necessary
     if (
@@ -364,13 +337,12 @@ std::vector<TrajectoryPoint> PlannerInterface::generateStopTrajectory(
 
       const double prev_zero_vel_dist = prev_stop_distance_info_->second - diff_dist_front_points;
       if (
-        std::abs(prev_zero_vel_dist - current_zero_vel_dist) <
+        std::abs(prev_zero_vel_dist - candidate_zero_vel_dist) <
         longitudinal_info_.hold_stop_distance_threshold) {
-        return prev_zero_vel_dist;
+        candidate_zero_vel_dist = prev_zero_vel_dist;
       }
     }
-
-    return current_zero_vel_dist;
+    return std::make_pair(candidate_zero_vel_dist, false);
   }();
 
   // Insert stop point
@@ -401,12 +373,13 @@ std::vector<TrajectoryPoint> PlannerInterface::generateStopTrajectory(
 
   stop_planning_debug_info_.set(
     StopPlanningDebugInfo::TYPE::STOP_CURRENT_OBSTACLE_DISTANCE,
-    closest_obstacle_dist - abs_ego_offset);  // TODO(murooka)
+    closest_stop_obstacle->dist_to_collide_on_decimated_traj);
   stop_planning_debug_info_.set(
     StopPlanningDebugInfo::TYPE::STOP_CURRENT_OBSTACLE_VELOCITY, closest_stop_obstacle->velocity);
 
   stop_planning_debug_info_.set(
-    StopPlanningDebugInfo::TYPE::STOP_TARGET_OBSTACLE_DISTANCE, stop_margin_from_obstacle);
+    StopPlanningDebugInfo::TYPE::STOP_TARGET_OBSTACLE_DISTANCE,
+    margin_from_obstacle_considering_behavior_module);
   stop_planning_debug_info_.set(StopPlanningDebugInfo::TYPE::STOP_TARGET_VELOCITY, 0.0);
   stop_planning_debug_info_.set(StopPlanningDebugInfo::TYPE::STOP_TARGET_ACCELERATION, 0.0);
 
@@ -721,8 +694,8 @@ PlannerInterface::calculateDistanceToSlowDownWithConstraints(
     }
 
     // NOTE: This min_relative_vel forces the relative velocity positive if the ego velocity is
-    // lower than the obstacle velocity. Without this, the slow down feature will flicker where the
-    // ego velocity is very close to the obstacle velocity.
+    // lower than the obstacle velocity. Without this, the slow down feature will flicker where
+    // the ego velocity is very close to the obstacle velocity.
     constexpr double min_relative_vel = 1.0;
     const double time_to_collision = (dist_to_front_collision - dist_to_ego - abs_ego_offset) /
                                      std::max(min_relative_vel, relative_vel);

planning/obstacle_cruise_planner/src/polygon_utils.cpp

@@ -16,6 +16,7 @@
 
 #include "motion_utils/trajectory/trajectory.hpp"
 #include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp"
+#include "tier4_autoware_utils/geometry/geometry.hpp"
 
 namespace
 {
@@ -84,6 +85,7 @@ std::optional<std::pair<size_t, std::vector<PointWithStamp>>> getCollisionIndex(
           collision_geom_point.stamp = object_time;
           collision_geom_point.point.x = collision_point.x();
           collision_geom_point.point.y = collision_point.y();
+          collision_geom_point.point.z = traj_points.at(i).pose.position.z;
           collision_geom_points.push_back(collision_geom_point);
         }
       }
@@ -134,19 +136,36 @@ Polygon2d createOneStepPolygon(
   return hull_polygon;
 }
 
-std::optional<geometry_msgs::msg::Point> getCollisionPoint(
+std::optional<std::pair<geometry_msgs::msg::Point, double>> getCollisionPoint(
   const std::vector<TrajectoryPoint> & traj_points, const std::vector<Polygon2d> & traj_polygons,
-  const Obstacle & obstacle, const bool is_driving_forward)
+  const Obstacle & obstacle, const bool is_driving_forward,
+  const vehicle_info_util::VehicleInfo & vehicle_info)
 {
   const auto collision_info =
     getCollisionIndex(traj_points, traj_polygons, obstacle.pose, obstacle.stamp, obstacle.shape);
-  if (collision_info) {
-    const auto nearest_collision_point = calcNearestCollisionPoint(
-      collision_info->first, collision_info->second, traj_points, is_driving_forward);
-    return nearest_collision_point.point;
+  if (!collision_info) {
+    return std::nullopt;
   }
 
-  return std::nullopt;
+  const double x_diff_to_bumper = is_driving_forward ? vehicle_info.max_longitudinal_offset_m
+                                                     : vehicle_info.min_longitudinal_offset_m;
+  const auto bumper_pose = tier4_autoware_utils::calcOffsetPose(
+    traj_points.at(collision_info->first).pose, x_diff_to_bumper, 0.0, 0.0);
+
+  std::optional<double> max_collision_length = std::nullopt;
+  std::optional<geometry_msgs::msg::Point> max_collision_point = std::nullopt;
+  for (const auto & poly_vertex : collision_info->second) {
+    const double dist_from_bumper =
+      std::abs(tier4_autoware_utils::inverseTransformPoint(poly_vertex.point, bumper_pose).x);
+
+    if (!max_collision_length.has_value() || dist_from_bumper > *max_collision_length) {
+      max_collision_length = dist_from_bumper;
+      max_collision_point = poly_vertex.point;
+    }
+  }
+  return std::make_pair(
+    *max_collision_point, motion_utils::calcSignedArcLength(traj_points, 0, collision_info->first) -
+                            *max_collision_length);
 }
 
 // NOTE: max_lat_dist is used for efficient calculation to suppress boost::geometry's polygon