feat(obstacle_cruise_planner): approching stop on curve (#4952)

planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml

@@ -20,7 +20,11 @@
       nearest_dist_deviation_threshold: 3.0 # [m] for finding nearest index
       nearest_yaw_deviation_threshold: 1.57 # [rad] for finding nearest index
       min_behavior_stop_margin: 3.0 # [m]
-      suppress_sudden_obstacle_stop: false
+      stop_on_curve:
+        enable_approaching: true # false
+        additional_safe_distance_margin: 0.0 # [m]
+        min_safe_distance_margin: 3.0 # [m]
+      suppress_sudden_obstacle_stop: true
 
       stop_obstacle_type:
         unknown: true
@@ -54,7 +58,7 @@
           pedestrian: false
 
       slow_down_obstacle_type:
-        unknown: true
+        unknown: false
         car: true
         truck: true
         bus: true

planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp

@@ -104,12 +104,15 @@ struct StopObstacle : public TargetObstacleInterface
 {
   StopObstacle(
     const std::string & arg_uuid, const rclcpp::Time & arg_stamp,
-    const geometry_msgs::msg::Pose & arg_pose, const double arg_lon_velocity,
-    const double arg_lat_velocity, const geometry_msgs::msg::Point arg_collision_point)
+    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)
   : TargetObstacleInterface(arg_uuid, arg_stamp, arg_pose, arg_lon_velocity, arg_lat_velocity),
+    shape(arg_shape),
     collision_point(arg_collision_point)
   {
   }
+  Shape shape;
   geometry_msgs::msg::Point collision_point;
 };
 

planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp

@@ -100,6 +100,9 @@ class ObstacleCruisePlannerNode : public rclcpp::Node
   bool enable_debug_info_;
   bool enable_calculation_time_info_;
   double min_behavior_stop_margin_;
+  bool enable_approaching_on_curve_;
+  double additional_safe_distance_margin_on_curve_;
+  double min_safe_distance_margin_on_curve_;
   bool suppress_sudden_obstacle_stop_;
 
   std::vector<int> stop_obstacle_types_;

planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp

@@ -53,11 +53,16 @@ class PlannerInterface
 
   void setParam(
     const bool enable_debug_info, const bool enable_calculation_time_info,
-    const double min_behavior_stop_margin, const bool suppress_sudden_obstacle_stop)
+    const double min_behavior_stop_margin, const double enable_approaching_on_curve,
+    const double additional_safe_distance_margin_on_curve,
+    const double min_safe_distance_margin_on_curve, const bool suppress_sudden_obstacle_stop)
   {
     enable_debug_info_ = enable_debug_info;
     enable_calculation_time_info_ = enable_calculation_time_info;
     min_behavior_stop_margin_ = min_behavior_stop_margin;
+    enable_approaching_on_curve_ = enable_approaching_on_curve;
+    additional_safe_distance_margin_on_curve_ = additional_safe_distance_margin_on_curve;
+    min_safe_distance_margin_on_curve_ = min_safe_distance_margin_on_curve;
     suppress_sudden_obstacle_stop_ = suppress_sudden_obstacle_stop;
   }
 
@@ -102,6 +107,9 @@ class PlannerInterface
   bool enable_calculation_time_info_{false};
   LongitudinalInfo longitudinal_info_;
   double min_behavior_stop_margin_;
+  bool enable_approaching_on_curve_;
+  double additional_safe_distance_margin_on_curve_;
+  double min_safe_distance_margin_on_curve_;
   bool suppress_sudden_obstacle_stop_;
 
   // stop watch
@@ -194,6 +202,8 @@ class PlannerInterface
     std::optional<geometry_msgs::msg::Pose> start_point{std::nullopt};
     std::optional<geometry_msgs::msg::Pose> end_point{std::nullopt};
   };
+  double calculateMarginFromObstacleOnCurve(
+    const PlannerData & planner_data, const StopObstacle & stop_obstacle) const;
   double calculateSlowDownVelocity(
     const SlowDownObstacle & obstacle, const std::optional<SlowDownOutput> & prev_output) const;
   std::optional<std::tuple<double, double, double>> calculateDistanceToSlowDownWithConstraints(

planning/obstacle_cruise_planner/src/node.cpp

@@ -397,11 +397,18 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions &
     }
 
     min_behavior_stop_margin_ = declare_parameter<double>("common.min_behavior_stop_margin");
+    additional_safe_distance_margin_on_curve_ =
+      declare_parameter<double>("common.stop_on_curve.additional_safe_distance_margin");
+    enable_approaching_on_curve_ =
+      declare_parameter<bool>("common.stop_on_curve.enable_approaching");
+    min_safe_distance_margin_on_curve_ =
+      declare_parameter<double>("common.stop_on_curve.min_safe_distance_margin");
     suppress_sudden_obstacle_stop_ =
       declare_parameter<bool>("common.suppress_sudden_obstacle_stop");
     planner_ptr_->setParam(
       enable_debug_info_, enable_calculation_time_info_, min_behavior_stop_margin_,
-      suppress_sudden_obstacle_stop_);
+      enable_approaching_on_curve_, additional_safe_distance_margin_on_curve_,
+      min_safe_distance_margin_on_curve_, suppress_sudden_obstacle_stop_);
   }
 
   {  // stop/cruise/slow down obstacle type
@@ -438,9 +445,20 @@ rcl_interfaces::msg::SetParametersResult ObstacleCruisePlannerNode::onParam(
     parameters, "common.enable_debug_info", enable_debug_info_);
   tier4_autoware_utils::updateParam<bool>(
     parameters, "common.enable_calculation_time_info", enable_calculation_time_info_);
+
+  tier4_autoware_utils::updateParam<bool>(
+    parameters, "common.stop_on_curve.enable_approaching", enable_approaching_on_curve_);
+  tier4_autoware_utils::updateParam<double>(
+    parameters, "common.stop_on_curve.additional_safe_distance_margin",
+    additional_safe_distance_margin_on_curve_);
+  tier4_autoware_utils::updateParam<double>(
+    parameters, "common.stop_on_curve.min_safe_distance_margin",
+    min_safe_distance_margin_on_curve_);
+
   planner_ptr_->setParam(
     enable_debug_info_, enable_calculation_time_info_, min_behavior_stop_margin_,
-    suppress_sudden_obstacle_stop_);
+    enable_approaching_on_curve_, additional_safe_distance_margin_on_curve_,
+    min_safe_distance_margin_on_curve_, suppress_sudden_obstacle_stop_);
 
   tier4_autoware_utils::updateParam<bool>(
     parameters, "common.enable_slow_down_planning", enable_slow_down_planning_);
@@ -911,7 +929,7 @@ std::optional<StopObstacle> ObstacleCruisePlannerNode::createStopObstacle(
   }
 
   const auto [tangent_vel, normal_vel] = projectObstacleVelocityToTrajectory(traj_points, obstacle);
-  return StopObstacle{obstacle.uuid, obstacle.stamp, obstacle.pose,
+  return StopObstacle{obstacle.uuid, obstacle.stamp, obstacle.pose,   obstacle.shape,
                       tangent_vel,   normal_vel,     *collision_point};
 }
 

planning/obstacle_cruise_planner/src/planner_interface.cpp

@@ -15,6 +15,10 @@
 #include "obstacle_cruise_planner/planner_interface.hpp"
 
 #include "motion_utils/distance/distance.hpp"
+#include "motion_utils/marker/marker_helper.hpp"
+#include "motion_utils/resample/resample.hpp"
+#include "motion_utils/trajectory/tmp_conversion.hpp"
+#include "motion_utils/trajectory/trajectory.hpp"
 #include "signal_processing/lowpass_filter_1d.hpp"
 
 namespace
@@ -204,6 +208,19 @@ double calcDecelerationVelocityFromDistanceToTarget(
   }
   return current_velocity;
 }
+
+std::vector<TrajectoryPoint> resampleTrajectoryPoints(
+  const std::vector<TrajectoryPoint> & traj_points, const double interval)
+{
+  const auto traj = motion_utils::convertToTrajectory(traj_points);
+  const auto resampled_traj = motion_utils::resampleTrajectory(traj, interval);
+  return motion_utils::convertToTrajectoryPointArray(resampled_traj);
+}
+
+tier4_autoware_utils::Point2d convertPoint(const geometry_msgs::msg::Point & p)
+{
+  return tier4_autoware_utils::Point2d{p.x, p.y};
+}
 }  // namespace
 
 std::vector<TrajectoryPoint> PlannerInterface::generateStopTrajectory(
@@ -259,16 +276,19 @@ std::vector<TrajectoryPoint> PlannerInterface::generateStopTrajectory(
     planner_data.traj_points, planner_data.ego_pose.position, ego_segment_idx, 0);
   const double dist_to_ego = -negative_dist_to_ego;
 
+  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 = [&]() {
+  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 longitudinal_info_.safe_distance_margin;
+      return margin_from_obstacle;
     }
 
     const double closest_behavior_stop_dist_from_ego = motion_utils::calcSignedArcLength(
@@ -282,29 +302,28 @@ std::vector<TrajectoryPoint> PlannerInterface::generateStopTrajectory(
         abs_ego_offset;
       const double stop_dist_diff =
         closest_behavior_stop_dist_from_ego - closest_obstacle_stop_dist_from_ego;
-      if (stop_dist_diff < longitudinal_info_.safe_distance_margin) {
+      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 -
-                                                         longitudinal_info_.safe_distance_margin -
-                                                         abs_ego_offset;
+      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 (0.0 < stop_dist_diff && stop_dist_diff < longitudinal_info_.safe_distance_margin) {
+      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 longitudinal_info_.safe_distance_margin;
+    return margin_from_obstacle;
   }();
 
   const auto [stop_margin_from_obstacle, will_collide_with_obstacle] = [&]() {
     // Check feasibility to stop
     if (suppress_sudden_obstacle_stop_) {
       const double closest_obstacle_stop_dist =
-        closest_obstacle_dist - margin_from_obstacle - abs_ego_offset;
+        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(
@@ -314,11 +333,12 @@ std::vector<TrajectoryPoint> PlannerInterface::generateStopTrajectory(
 
       if (closest_obstacle_stop_dist < feasible_stop_dist) {
         const auto feasible_margin_from_obstacle =
-          margin_from_obstacle - (feasible_stop_dist - closest_obstacle_stop_dist);
+          margin_from_obstacle_considering_behavior_module -
+          (feasible_stop_dist - closest_obstacle_stop_dist);
         return std::make_pair(feasible_margin_from_obstacle, true);
       }
     }
-    return std::make_pair(margin_from_obstacle, false);
+    return std::make_pair(margin_from_obstacle_considering_behavior_module, false);
   }();
 
   // Generate Output Trajectory
@@ -395,6 +415,91 @@ std::vector<TrajectoryPoint> PlannerInterface::generateStopTrajectory(
   return output_traj_points;
 }
 
+double PlannerInterface::calculateMarginFromObstacleOnCurve(
+  const PlannerData & planner_data, const StopObstacle & stop_obstacle) const
+{
+  if (!enable_approaching_on_curve_) {
+    return longitudinal_info_.safe_distance_margin;
+  }
+
+  const double abs_ego_offset = planner_data.is_driving_forward
+                                  ? std::abs(vehicle_info_.max_longitudinal_offset_m)
+                                  : std::abs(vehicle_info_.min_longitudinal_offset_m);
+
+  // calculate short trajectory points towards obstacle
+  const size_t obj_segment_idx =
+    motion_utils::findNearestSegmentIndex(planner_data.traj_points, stop_obstacle.collision_point);
+  std::vector<TrajectoryPoint> short_traj_points{planner_data.traj_points.at(obj_segment_idx + 1)};
+  double sum_short_traj_length{0.0};
+  for (int i = obj_segment_idx; 0 <= i; --i) {
+    short_traj_points.push_back(planner_data.traj_points.at(i));
+
+    if (
+      1 < short_traj_points.size() &&
+      longitudinal_info_.safe_distance_margin + abs_ego_offset < sum_short_traj_length) {
+      break;
+    }
+    sum_short_traj_length += tier4_autoware_utils::calcDistance2d(
+      planner_data.traj_points.at(i), planner_data.traj_points.at(i + 1));
+  }
+  std::reverse(short_traj_points.begin(), short_traj_points.end());
+  if (short_traj_points.size() < 2) {
+    return longitudinal_info_.safe_distance_margin;
+  }
+
+  // calculate collision index between straight line from ego pose and object
+  const auto calculate_distance_from_straight_ego_path =
+    [&](const auto & ego_pose, const auto & object_polygon) {
+      const auto forward_ego_pose = tier4_autoware_utils::calcOffsetPose(
+        ego_pose, longitudinal_info_.safe_distance_margin + 3.0, 0.0, 0.0);
+      const auto ego_straight_segment = tier4_autoware_utils::Segment2d{
+        convertPoint(ego_pose.position), convertPoint(forward_ego_pose.position)};
+      return boost::geometry::distance(ego_straight_segment, object_polygon);
+    };
+  const auto resampled_short_traj_points = resampleTrajectoryPoints(short_traj_points, 0.5);
+  const auto object_polygon =
+    tier4_autoware_utils::toPolygon2d(stop_obstacle.pose, stop_obstacle.shape);
+  const auto collision_idx = [&]() -> std::optional<size_t> {
+    for (size_t i = 0; i < resampled_short_traj_points.size(); ++i) {
+      const double dist_to_obj = calculate_distance_from_straight_ego_path(
+        resampled_short_traj_points.at(i).pose, object_polygon);
+      if (dist_to_obj < vehicle_info_.vehicle_width_m / 2.0) {
+        return i;
+      }
+    }
+    return std::nullopt;
+  }();
+  if (!collision_idx) {
+    return min_safe_distance_margin_on_curve_;
+  }
+  if (*collision_idx == 0) {
+    return longitudinal_info_.safe_distance_margin;
+  }
+
+  // calculate margin from obstacle
+  const double partial_segment_length = [&]() {
+    const double collision_segment_length = tier4_autoware_utils::calcDistance2d(
+      resampled_short_traj_points.at(*collision_idx - 1),
+      resampled_short_traj_points.at(*collision_idx));
+    const double prev_dist = calculate_distance_from_straight_ego_path(
+      resampled_short_traj_points.at(*collision_idx - 1).pose, object_polygon);
+    const double next_dist = calculate_distance_from_straight_ego_path(
+      resampled_short_traj_points.at(*collision_idx).pose, object_polygon);
+    return (next_dist - vehicle_info_.vehicle_width_m / 2.0) / (next_dist - prev_dist) *
+           collision_segment_length;
+  }();
+
+  const double short_margin_from_obstacle =
+    partial_segment_length +
+    motion_utils::calcSignedArcLength(
+      resampled_short_traj_points, *collision_idx, stop_obstacle.collision_point) -
+    abs_ego_offset + additional_safe_distance_margin_on_curve_;
+
+  return std::min(
+    longitudinal_info_.safe_distance_margin,
+    std::max(min_safe_distance_margin_on_curve_, short_margin_from_obstacle));
+}
+
 double PlannerInterface::calcDistanceToCollisionPoint(
   const PlannerData & planner_data, const geometry_msgs::msg::Point & collision_point)
 {