add abandon function

checkConsistency() is not supported yet

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

Author: yuki-takagi-66

planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp

@@ -113,14 +113,15 @@ struct StopObstacle : public TargetObstacleInterface
 {
   StopObstacle(
     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 ObjectClassification & object_classification, 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 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),
-    dist_to_collide_on_decimated_traj(arg_dist_to_collide_on_decimated_traj)
+    dist_to_collide_on_decimated_traj(arg_dist_to_collide_on_decimated_traj),
+    classification(object_classification)
   {
   }
   Shape shape;
@@ -129,6 +130,7 @@ struct StopObstacle : public TargetObstacleInterface
                       // calculateMarginFromObstacleOnCurve() and  should be removed as it can be
                       // replaced by ”dist_to_collide_on_decimated_traj”
   double dist_to_collide_on_decimated_traj;
+  ObjectClassification classification;
 };
 
 struct CruiseObstacle : public TargetObstacleInterface

planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp

@@ -42,7 +42,8 @@ class PlannerInterface
     vehicle_info_(vehicle_info),
     ego_nearest_param_(ego_nearest_param),
     debug_data_ptr_(debug_data_ptr),
-    slow_down_param_(SlowDownParam(node))
+    slow_down_param_(SlowDownParam(node)),
+    stop_param_(StopParam(node, longitudinal_info))
   {
     stop_reasons_pub_ = node.create_publisher<StopReasonArray>("~/output/stop_reasons", 1);
     velocity_factors_pub_ =
@@ -333,6 +334,44 @@ class PlannerInterface
     double lpf_gain_dist_to_slow_down;
   };
   SlowDownParam slow_down_param_;
+  struct StopParam
+  {
+    struct ObstacleSpecificParams
+    {
+      double limit_min_acc;
+      bool abandon_to_stop;
+    };
+    const std::unordered_map<uint8_t, std::string> types_maps = {
+      {ObjectClassification::UNKNOWN, "unknown"}, {ObjectClassification::CAR, "car"},
+      {ObjectClassification::TRUCK, "truck"},     {ObjectClassification::BUS, "bus"},
+      {ObjectClassification::TRAILER, "trailer"}, {ObjectClassification::MOTORCYCLE, "motorcycle"},
+      {ObjectClassification::BICYCLE, "bicycle"}, {ObjectClassification::PEDESTRIAN, "pedestrian"}};
+    std::unordered_map<std::string, ObstacleSpecificParams> type_specified_param_list;
+    explicit StopParam(rclcpp::Node & node, const LongitudinalInfo & longitudinal_info)
+    {
+      const std::string param_prefix = "stop.type_specified_params.";
+      const ObstacleSpecificParams default_param{longitudinal_info.limit_min_accel, false};
+      type_specified_param_list.emplace("default", default_param);
+      for (const auto & type_map : types_maps) {
+        if (node.declare_parameter<bool>(
+              param_prefix + "use_type_specified_params." + type_map.second)) {
+          const ObstacleSpecificParams param{
+            node.declare_parameter<double>(param_prefix + "limit_min_acc." + type_map.second),
+            node.declare_parameter<bool>(param_prefix + "abandon_to_stop." + type_map.second)};
+          type_specified_param_list.emplace(type_map.second, param);
+        }
+      }
+    }
+    std::string getParamType(ObjectClassification label)
+    {
+      const auto type_str = types_maps.at(label.label);
+      if (type_specified_param_list.count(type_str) != 0) {
+        return type_str;
+      }
+      return "default";
+    }
+  };
+  StopParam stop_param_;
   double moving_object_speed_threshold;
   double moving_object_hysteresis_range;
   std::vector<SlowDownOutput> prev_slow_down_output_;

planning/obstacle_cruise_planner/src/node.cpp

@@ -815,6 +815,12 @@ ObstacleCruisePlannerNode::determineEgoBehaviorAgainstObstacles(
   }
   slow_down_condition_counter_.removeCounterUnlessUpdated();
 
+  std::sort(
+    stop_obstacles.begin(), stop_obstacles.end(),
+    [](const StopObstacle & o1, const StopObstacle & o2) {
+      return o1.dist_to_collide_on_decimated_traj < o2.dist_to_collide_on_decimated_traj;
+    });
+    
   // Check target obstacles' consistency
   checkConsistency(objects.header.stamp, objects, stop_obstacles);
 
@@ -1242,9 +1248,9 @@ std::optional<StopObstacle> ObstacleCruisePlannerNode::createStopObstacle(
   }
 
   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};
+  return StopObstacle{obstacle.uuid, obstacle.stamp,         obstacle.classification,
+                      obstacle.pose, obstacle.shape,         tangent_vel,
+                      normal_vel,    collision_point->first, collision_point->second};
 }
 
 std::optional<SlowDownObstacle> ObstacleCruisePlannerNode::createSlowDownObstacle(

planning/obstacle_cruise_planner/src/planner_interface.cpp

@@ -259,9 +259,78 @@ std::vector<TrajectoryPoint> PlannerInterface::generateStopTrajectory(
     rclcpp::get_logger("ObstacleCruisePlanner::PIDBasedPlanner"), enable_debug_info_,
     "stop planning");
 
-  // Get Closest Stop Obstacle
-  const auto closest_stop_obstacle = obstacle_cruise_utils::getClosestStopObstacle(stop_obstacles);
-  if (!closest_stop_obstacle) {
+  std::optional<StopObstacle> determined_stop_obstacle{};
+  std::optional<double> determined_zero_vel_dist{};
+  std::optional<double> determined_desired_margin{};
+  // std::optional<bool> will_collide_with_obstacle{};
+  for (const auto & type_specified_param : stop_param_.type_specified_param_list) {
+    // stop_obstacles had been sorted by dist_to_collide_on_decimated_traj
+    for (const auto & stop_obstacle : stop_obstacles) {
+      if (stop_param_.getParamType(stop_obstacle.classification) != type_specified_param.first) {
+        continue;
+      }
+      const auto ego_segment_idx =
+        ego_nearest_param_.findSegmentIndex(planner_data.traj_points, planner_data.ego_pose);
+      const double dist_to_collide_on_ref_traj =
+        motion_utils::calcSignedArcLength(planner_data.traj_points, 0, ego_segment_idx) +
+        stop_obstacle.dist_to_collide_on_decimated_traj;
+
+      const double desired_margin = [&]() {
+        const double margin_from_obstacle =
+          calculateMarginFromObstacleOnCurve(planner_data, 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;
+        }
+
+        // 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) {
+            return min_behavior_stop_margin_;
+          }
+        }
+        return margin_from_obstacle;
+      }();
+
+      // calc stop point against the obstacle
+      double candidate_zero_vel_dist = std::max(0.0, dist_to_collide_on_ref_traj - desired_margin);
+      if (suppress_sudden_obstacle_stop_) {
+        const double acceptable_stop_dist =
+          calcMinimumDistanceToStop(
+            planner_data.ego_vel, longitudinal_info_.limit_max_accel,
+            type_specified_param.second.limit_min_acc) +
+          motion_utils::calcSignedArcLength(
+            planner_data.traj_points, 0, planner_data.ego_pose.position);
+
+        if (acceptable_stop_dist > candidate_zero_vel_dist) {
+          if (type_specified_param.second.abandon_to_stop && acceptable_stop_dist > dist_to_collide_on_ref_traj) {
+            RCLCPP_WARN(
+              rclcpp::get_logger("ObstacleCruisePlanner::StopPlanner"),
+              "[Cruise] abandon to stop against %s object", type_specified_param.first.c_str());
+            continue;
+          }
+          candidate_zero_vel_dist = acceptable_stop_dist;
+        }
+      }
+      if (!determined_zero_vel_dist || candidate_zero_vel_dist < determined_zero_vel_dist) {
+        determined_zero_vel_dist = candidate_zero_vel_dist;
+        determined_stop_obstacle = stop_obstacle;
+        determined_desired_margin = desired_margin;
+        break;
+      }
+    }
+  }
+
+  if (!determined_zero_vel_dist) {
     // delete marker
     const auto markers =
       motion_utils::createDeletedStopVirtualWallMarker(planner_data.current_time, 0);
@@ -271,117 +340,75 @@ std::vector<TrajectoryPoint> PlannerInterface::generateStopTrajectory(
     return planner_data.traj_points;
   }
 
-  const auto ego_segment_idx =
-    ego_nearest_param_.findSegmentIndex(planner_data.traj_points, planner_data.ego_pose);
-  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_considering_behavior_module = [&]() {
-    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;
-    }
-
-    // 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) {
-        return min_behavior_stop_margin_;
-      }
-    }
-    return margin_from_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_) {
-      // 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) +
-        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);
-      }
-    }
-
-    // Hold previous stop distance if necessary
+  // Hold previous stop distance if necessary
+  if (
+    std::abs(planner_data.ego_vel) < longitudinal_info_.hold_stop_velocity_threshold &&
+    prev_stop_distance_info_) {
+    // NOTE: We assume that the current trajectory's front point is ahead of the previous
+    // trajectory's front point.
+    const size_t traj_front_point_prev_seg_idx =
+      motion_utils::findFirstNearestSegmentIndexWithSoftConstraints(
+        prev_stop_distance_info_->first, planner_data.traj_points.front().pose);
+    const double diff_dist_front_points = motion_utils::calcSignedArcLength(
+      prev_stop_distance_info_->first, 0, planner_data.traj_points.front().pose.position,
+      traj_front_point_prev_seg_idx);
+
+    const double prev_zero_vel_dist = prev_stop_distance_info_->second - diff_dist_front_points;
     if (
-      std::abs(planner_data.ego_vel) < longitudinal_info_.hold_stop_velocity_threshold &&
-      prev_stop_distance_info_) {
-      // NOTE: We assume that the current trajectory's front point is ahead of the previous
-      // trajectory's front point.
-      const size_t traj_front_point_prev_seg_idx =
-        motion_utils::findFirstNearestSegmentIndexWithSoftConstraints(
-          prev_stop_distance_info_->first, planner_data.traj_points.front().pose);
-      const double diff_dist_front_points = motion_utils::calcSignedArcLength(
-        prev_stop_distance_info_->first, 0, planner_data.traj_points.front().pose.position,
-        traj_front_point_prev_seg_idx);
-
-      const double prev_zero_vel_dist = prev_stop_distance_info_->second - diff_dist_front_points;
-      if (
-        std::abs(prev_zero_vel_dist - candidate_zero_vel_dist) <
-        longitudinal_info_.hold_stop_distance_threshold) {
-        candidate_zero_vel_dist = prev_zero_vel_dist;
-      }
+      std::abs(prev_zero_vel_dist - determined_zero_vel_dist.value()) <
+      longitudinal_info_.hold_stop_distance_threshold) {
+      determined_zero_vel_dist.value() = prev_zero_vel_dist;
     }
-    return std::make_pair(candidate_zero_vel_dist, false);
-  }();
+  }
 
   // Insert stop point
   auto output_traj_points = planner_data.traj_points;
-  const auto zero_vel_idx = motion_utils::insertStopPoint(0, zero_vel_dist, output_traj_points);
+  const auto zero_vel_idx =
+    motion_utils::insertStopPoint(0, determined_zero_vel_dist.value(), output_traj_points);
   if (zero_vel_idx) {
     // virtual wall marker for stop obstacle
     const auto markers = motion_utils::createStopVirtualWallMarker(
       output_traj_points.at(*zero_vel_idx).pose, "obstacle stop", planner_data.current_time, 0,
       abs_ego_offset, "", planner_data.is_driving_forward);
     tier4_autoware_utils::appendMarkerArray(markers, &debug_data_ptr_->stop_wall_marker);
-    debug_data_ptr_->obstacles_to_stop.push_back(*closest_stop_obstacle);
+    debug_data_ptr_->obstacles_to_stop.push_back(*determined_stop_obstacle);
 
     // Publish Stop Reason
     const auto stop_pose = output_traj_points.at(*zero_vel_idx).pose;
     const auto stop_reasons_msg =
-      makeStopReasonArray(planner_data, stop_pose, *closest_stop_obstacle);
+      makeStopReasonArray(planner_data, stop_pose, *determined_stop_obstacle);
     stop_reasons_pub_->publish(stop_reasons_msg);
     velocity_factors_pub_->publish(makeVelocityFactorArray(planner_data.current_time, stop_pose));
 
-    // Publish if ego vehicle collides with the obstacle with a limit acceleration
+    // Publish if ego vehicle will over run against the stop point with a limit acceleration
+    const bool will_over_run = determined_stop_obstacle->dist_to_collide_on_decimated_traj -
+                                 determined_zero_vel_dist.value() <
+                               determined_desired_margin.value();
     const auto stop_speed_exceeded_msg =
-      createStopSpeedExceededMsg(planner_data.current_time, will_collide_with_obstacle);
+      createStopSpeedExceededMsg(planner_data.current_time, will_over_run);
     stop_speed_exceeded_pub_->publish(stop_speed_exceeded_msg);
 
-    prev_stop_distance_info_ = std::make_pair(output_traj_points, zero_vel_dist);
+    prev_stop_distance_info_ = std::make_pair(output_traj_points, determined_zero_vel_dist.value());
   }
 
   stop_planning_debug_info_.set(
     StopPlanningDebugInfo::TYPE::STOP_CURRENT_OBSTACLE_DISTANCE,
-    closest_stop_obstacle->dist_to_collide_on_decimated_traj);
+    determined_stop_obstacle->dist_to_collide_on_decimated_traj);
   stop_planning_debug_info_.set(
-    StopPlanningDebugInfo::TYPE::STOP_CURRENT_OBSTACLE_VELOCITY, closest_stop_obstacle->velocity);
+    StopPlanningDebugInfo::TYPE::STOP_CURRENT_OBSTACLE_VELOCITY,
+    determined_stop_obstacle->velocity);
 
+  // compatible implementation to the exsiting code is the choice 1, but should we change it to the
+  // choice2?
+  // choice 1
   stop_planning_debug_info_.set(
-    StopPlanningDebugInfo::TYPE::STOP_TARGET_OBSTACLE_DISTANCE,
-    margin_from_obstacle_considering_behavior_module);
+    StopPlanningDebugInfo::TYPE::STOP_TARGET_OBSTACLE_DISTANCE, determined_desired_margin.value());
+  // choice 2
+  //  stop_planning_debug_info_.set(
+  //    StopPlanningDebugInfo::TYPE::STOP_TARGET_OBSTACLE_DISTANCE,
+  //    determined_stop_obstacle->dist_to_collide_on_decimated_traj -
+  //    determined_zero_vel_dist.value());
+
   stop_planning_debug_info_.set(StopPlanningDebugInfo::TYPE::STOP_TARGET_VELOCITY, 0.0);
   stop_planning_debug_info_.set(StopPlanningDebugInfo::TYPE::STOP_TARGET_ACCELERATION, 0.0);