Merge branch 'autowarefoundation:main' into feat/distortion_correction_node_update_azimuth_and_distance

Author: vividf

Diff for: launch/tier4_perception_launch/launch/perception.launch.xml

@@ -104,7 +104,7 @@
   <!-- Radar long range integration methods -->
   <arg
     name="use_radar_tracking_fusion"
-    default="false"
+    default="true"
     description="if use_radar_tracking_fusion:=true, radar information is merged in tracking launch. Otherwise, radar information is merged in detection launch."
   />
 

Diff for: planning/behavior_path_planner/config/avoidance/avoidance.param.yaml

@@ -190,7 +190,8 @@
           max_deviation_from_lane: 0.5                  # [m]
         # avoidance distance parameters
         longitudinal:
-          prepare_time: 2.0                             # [s]
+          min_prepare_time: 1.0                         # [s]
+          max_prepare_time: 2.0                         # [s]
           min_prepare_distance: 1.0                     # [m]
           min_slow_down_speed: 1.38                     # [m/s]
           buf_slow_down_speed: 0.56                     # [m/s]

Diff for: planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp

@@ -211,7 +211,8 @@ struct AvoidanceParameters
   double stop_buffer{0.0};
 
   // start avoidance after this time to avoid sudden path change
-  double prepare_time{0.0};
+  double min_prepare_time{0.0};
+  double max_prepare_time{0.0};
 
   // Even if the vehicle speed is zero, avoidance will start after a distance of this much.
   double min_prepare_distance{0.0};

Diff for: planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/helper.hpp

@@ -91,10 +91,16 @@ class AvoidanceHelper
     return std::max(getEgoSpeed(), values.at(idx));
   }
 
+  double getMinimumPrepareDistance() const
+  {
+    const auto & p = parameters_;
+    return std::max(getEgoSpeed() * p->min_prepare_time, p->min_prepare_distance);
+  }
+
   double getNominalPrepareDistance() const
   {
     const auto & p = parameters_;
-    return std::max(getEgoSpeed() * p->prepare_time, p->min_prepare_distance);
+    return std::max(getEgoSpeed() * p->max_prepare_time, p->min_prepare_distance);
   }
 
   double getNominalAvoidanceDistance(const double shift_length) const
@@ -185,7 +191,8 @@ class AvoidanceHelper
       max_shift_length, getLateralMinJerkLimit(), getEgoSpeed());
 
     return std::clamp(
-      1.5 * dynamic_distance, parameters_->object_check_min_forward_distance,
+      1.5 * dynamic_distance + getNominalPrepareDistance(),
+      parameters_->object_check_min_forward_distance,
       parameters_->object_check_max_forward_distance);
   }
 

Diff for: planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp

@@ -437,10 +437,27 @@ bool AvoidanceModule::canYieldManeuver(const AvoidancePlanningData & data) const
 {
   // transit yield maneuver only when the avoidance maneuver is not initiated.
   if (helper_.isShifted()) {
+    RCLCPP_DEBUG(getLogger(), "avoidance maneuver already initiated.");
     return false;
   }
 
+  // prevent sudden steering.
+  const auto registered_lines = path_shifter_.getShiftLines();
+  if (!registered_lines.empty()) {
+    const size_t idx = planner_data_->findEgoIndex(path_shifter_.getReferencePath().points);
+    const auto to_shift_start_point = calcSignedArcLength(
+      path_shifter_.getReferencePath().points, idx, registered_lines.front().start_idx);
+    if (to_shift_start_point < helper_.getMinimumPrepareDistance()) {
+      RCLCPP_DEBUG(
+        getLogger(),
+        "Distance to shift start point is less than minimum prepare distance. The distance is not "
+        "enough.");
+      return false;
+    }
+  }
+
   if (!data.stop_target_object) {
+    RCLCPP_DEBUG(getLogger(), "can pass by the object safely without avoidance maneuver.");
     return true;
   }
 
@@ -920,8 +937,9 @@ AvoidOutlines AvoidanceModule::generateAvoidOutline(
   const auto & base_link2rear = planner_data_->parameters.base_link2rear;
 
   // Calculate feasible shift length
-  const auto get_shift_length =
-    [&](auto & object, const auto & desire_shift_length) -> boost::optional<double> {
+  const auto get_shift_profile =
+    [&](
+      auto & object, const auto & desire_shift_length) -> std::optional<std::pair<double, double>> {
     // use each object param
     const auto object_type = utils::getHighestProbLabel(object.object.classification);
     const auto object_parameter = parameters_->object_parameters.at(object_type);
@@ -931,55 +949,57 @@ AvoidOutlines AvoidanceModule::generateAvoidOutline(
     const auto avoiding_shift = desire_shift_length - current_ego_shift;
     const auto nominal_avoid_distance = helper_.getMaxAvoidanceDistance(avoiding_shift);
 
+    // calculate remaining distance.
+    const auto prepare_distance = helper_.getNominalPrepareDistance();
+    const auto & additional_buffer_longitudinal =
+      object_parameter.use_conservative_buffer_longitudinal
+        ? planner_data_->parameters.base_link2front
+        : 0.0;
+    const auto constant = object_parameter.safety_buffer_longitudinal +
+                          additional_buffer_longitudinal + prepare_distance;
+    const auto has_enough_distance = object.longitudinal > constant + nominal_avoid_distance;
+    const auto remaining_distance = object.longitudinal - constant;
+    const auto avoidance_distance =
+      has_enough_distance ? nominal_avoid_distance : remaining_distance;
+
+    // nominal case. avoidable.
+    if (has_enough_distance) {
+      return std::make_pair(desire_shift_length, avoidance_distance);
+    }
+
     if (!isBestEffort(parameters_->policy_lateral_margin)) {
-      return desire_shift_length;
+      return std::make_pair(desire_shift_length, avoidance_distance);
     }
 
     // ego already has enough positive shift.
     const auto has_enough_positive_shift = avoiding_shift < -1e-3 && desire_shift_length > 1e-3;
     if (is_object_on_right && has_enough_positive_shift) {
-      return desire_shift_length;
+      return std::make_pair(desire_shift_length, avoidance_distance);
     }
 
     // ego already has enough negative shift.
     const auto has_enough_negative_shift = avoiding_shift > 1e-3 && desire_shift_length < -1e-3;
     if (!is_object_on_right && has_enough_negative_shift) {
-      return desire_shift_length;
+      return std::make_pair(desire_shift_length, avoidance_distance);
     }
 
     // don't relax shift length since it can stop in front of the object.
     if (object.is_stoppable && !parameters_->use_shorten_margin_immediately) {
-      return desire_shift_length;
+      return std::make_pair(desire_shift_length, avoidance_distance);
     }
 
-    // calculate remaining distance.
-    const auto prepare_distance = helper_.getNominalPrepareDistance();
-    const auto & additional_buffer_longitudinal =
-      object_parameter.use_conservative_buffer_longitudinal
-        ? planner_data_->parameters.base_link2front
-        : 0.0;
-    const auto constant = object_parameter.safety_buffer_longitudinal +
-                          additional_buffer_longitudinal + prepare_distance;
-    const auto has_enough_distance = object.longitudinal > constant + nominal_avoid_distance;
-    const auto remaining_distance = object.longitudinal - constant;
-
     // the avoidance path is already approved
     const auto & object_pos = object.object.kinematics.initial_pose_with_covariance.pose.position;
     const auto is_approved = (helper_.getShift(object_pos) > 0.0 && is_object_on_right) ||
                              (helper_.getShift(object_pos) < 0.0 && !is_object_on_right);
     if (is_approved) {
-      return desire_shift_length;
+      return std::make_pair(desire_shift_length, avoidance_distance);
     }
 
     // prepare distance is not enough. unavoidable.
     if (remaining_distance < 1e-3) {
       object.reason = AvoidanceDebugFactor::REMAINING_DISTANCE_LESS_THAN_ZERO;
-      return boost::none;
-    }
-
-    // nominal case. avoidable.
-    if (has_enough_distance) {
-      return desire_shift_length;
+      return std::nullopt;
     }
 
     // calculate lateral jerk.
@@ -988,13 +1008,13 @@ AvoidOutlines AvoidanceModule::generateAvoidOutline(
 
     // relax lateral jerk limit. avoidable.
     if (required_jerk < helper_.getLateralMaxJerkLimit()) {
-      return desire_shift_length;
+      return std::make_pair(desire_shift_length, avoidance_distance);
     }
 
     // avoidance distance is not enough. unavoidable.
     if (!isBestEffort(parameters_->policy_deceleration)) {
       object.reason = AvoidanceDebugFactor::TOO_LARGE_JERK;
-      return boost::none;
+      return std::nullopt;
     }
 
     // output avoidance path under lateral jerk constraints.
@@ -1003,7 +1023,7 @@ AvoidOutlines AvoidanceModule::generateAvoidOutline(
 
     if (std::abs(feasible_relative_shift_length) < parameters_->lateral_execution_threshold) {
       object.reason = "LessThanExecutionThreshold";
-      return boost::none;
+      return std::nullopt;
     }
 
     const auto feasible_shift_length =
@@ -1017,7 +1037,7 @@ AvoidOutlines AvoidanceModule::generateAvoidOutline(
       RCLCPP_WARN_THROTTLE(
         getLogger(), *clock_, 1000, "feasible shift length is not enough to avoid. ");
       object.reason = AvoidanceDebugFactor::TOO_LARGE_JERK;
-      return boost::none;
+      return std::nullopt;
     }
 
     {
@@ -1026,7 +1046,7 @@ AvoidOutlines AvoidanceModule::generateAvoidOutline(
         std::abs(avoiding_shift), feasible_relative_shift_length);
     }
 
-    return feasible_shift_length;
+    return std::make_pair(feasible_shift_length, avoidance_distance);
   };
 
   const auto is_forward_object = [](const auto & object) { return object.longitudinal > 0.0; };
@@ -1061,9 +1081,9 @@ AvoidOutlines AvoidanceModule::generateAvoidOutline(
     // use each object param
     const auto object_type = utils::getHighestProbLabel(o.object.classification);
     const auto object_parameter = parameters_->object_parameters.at(object_type);
-    const auto feasible_shift_length = get_shift_length(o, desire_shift_length);
+    const auto feasible_shift_profile = get_shift_profile(o, desire_shift_length);
 
-    if (!feasible_shift_length) {
+    if (!feasible_shift_profile.has_value()) {
       if (o.avoid_required && is_forward_object(o)) {
         break;
       } else {
@@ -1072,10 +1092,8 @@ AvoidOutlines AvoidanceModule::generateAvoidOutline(
     }
 
     // use absolute dist for return-to-center, relative dist from current for avoiding.
-    const auto feasible_avoid_distance =
-      helper_.getMaxAvoidanceDistance(feasible_shift_length.get() - current_ego_shift);
     const auto feasible_return_distance =
-      helper_.getMaxAvoidanceDistance(feasible_shift_length.get());
+      helper_.getMaxAvoidanceDistance(feasible_shift_profile.value().first);
 
     AvoidLine al_avoid;
     {
@@ -1091,14 +1109,15 @@ AvoidOutlines AvoidanceModule::generateAvoidOutline(
 
       // start point (use previous linear shift length as start shift length.)
       al_avoid.start_longitudinal = [&]() {
-        const auto nearest_avoid_distance = std::max(to_shift_end - feasible_avoid_distance, 1e-3);
+        const auto nearest_avoid_distance =
+          std::max(to_shift_end - feasible_shift_profile.value().second, 1e-3);
 
         if (data.to_start_point > to_shift_end) {
           return nearest_avoid_distance;
         }
 
         const auto minimum_avoid_distance =
-          helper_.getMinAvoidanceDistance(feasible_shift_length.get() - current_ego_shift);
+          helper_.getMinAvoidanceDistance(feasible_shift_profile.value().first - current_ego_shift);
         const auto furthest_avoid_distance = std::max(to_shift_end - minimum_avoid_distance, 1e-3);
 
         return std::clamp(data.to_start_point, nearest_avoid_distance, furthest_avoid_distance);
@@ -1110,7 +1129,7 @@ AvoidOutlines AvoidanceModule::generateAvoidOutline(
       al_avoid.start_shift_length = helper_.getLinearShift(al_avoid.start.position);
 
       // end point
-      al_avoid.end_shift_length = feasible_shift_length.get();
+      al_avoid.end_shift_length = feasible_shift_profile.value().first;
       al_avoid.end_longitudinal = to_shift_end;
 
       // misc
@@ -1125,7 +1144,7 @@ AvoidOutlines AvoidanceModule::generateAvoidOutline(
       const auto to_shift_start = o.longitudinal + offset;
 
       // start point
-      al_return.start_shift_length = feasible_shift_length.get();
+      al_return.start_shift_length = feasible_shift_profile.value().first;
       al_return.start_longitudinal = to_shift_start;
 
       // end point
@@ -1652,6 +1671,10 @@ void AvoidanceModule::applySmallShiftFilter(
       continue;
     }
 
+    if (s.start_longitudinal < helper_.getMinimumPrepareDistance()) {
+      continue;
+    }
+
     shift_lines.push_back(s);
   }
 }
@@ -2405,9 +2428,8 @@ AvoidLineArray AvoidanceModule::findNewShiftLine(
   for (size_t i = 0; i < shift_lines.size(); ++i) {
     const auto & candidate = shift_lines.at(i);
 
-    // new shift points must exist in front of Ego
-    // this value should be larger than -eps consider path shifter calculation error.
-    if (candidate.start_idx < avoid_data_.ego_closest_path_index) {
+    // prevent sudden steering.
+    if (candidate.start_longitudinal < helper_.getMinimumPrepareDistance()) {
       break;
     }
 

Diff for: planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp

@@ -221,7 +221,8 @@ AvoidanceModuleManager::AvoidanceModuleManager(
   // avoidance maneuver (longitudinal)
   {
     std::string ns = "avoidance.avoidance.longitudinal.";
-    p.prepare_time = getOrDeclareParameter<double>(*node, ns + "prepare_time");
+    p.min_prepare_time = getOrDeclareParameter<double>(*node, ns + "min_prepare_time");
+    p.max_prepare_time = getOrDeclareParameter<double>(*node, ns + "max_prepare_time");
     p.min_prepare_distance = getOrDeclareParameter<double>(*node, ns + "min_prepare_distance");
     p.min_slow_down_speed = getOrDeclareParameter<double>(*node, ns + "min_slow_down_speed");
     p.buf_slow_down_speed = getOrDeclareParameter<double>(*node, ns + "buf_slow_down_speed");
@@ -390,7 +391,8 @@ void AvoidanceModuleManager::updateModuleParams(const std::vector<rclcpp::Parame
 
   {
     const std::string ns = "avoidance.avoidance.longitudinal.";
-    updateParam<double>(parameters, ns + "prepare_time", p->prepare_time);
+    updateParam<double>(parameters, ns + "min_prepare_time", p->min_prepare_time);
+    updateParam<double>(parameters, ns + "max_prepare_time", p->max_prepare_time);
     updateParam<double>(parameters, ns + "min_slow_down_speed", p->min_slow_down_speed);
     updateParam<double>(parameters, ns + "buf_slow_down_speed", p->buf_slow_down_speed);
   }

Diff for: planning/behavior_velocity_out_of_lane_module/config/out_of_lane.param.yaml

@@ -17,6 +17,7 @@
         use_predicted_paths: true  # if true, use the predicted paths to estimate future positions.
                                    # if false, assume the object moves at constant velocity along *all* lanelets it currently is located in.
         predicted_path_min_confidence : 0.1  # when using predicted paths, ignore the ones whose confidence is lower than this value.
+        distance_buffer: 1.0 # [m] distance buffer used to determine if a collision will occur in the other lane
 
       overlap:
         minimum_distance: 0.0  # [m] minimum distance inside a lanelet for an overlap to be considered
@@ -26,6 +27,7 @@
         skip_if_over_max_decel: true  # if true, do not take an action that would cause more deceleration than the maximum allowed
         precision: 0.1  # [m] precision when inserting a stop pose in the path
         distance_buffer: 1.5  # [m] buffer distance to try to keep between the ego footprint and lane
+        min_duration: 1.0  # [s] minimum duration needed before a decision can be canceled
         slowdown:
           distance_threshold: 30.0 # [m] insert a slowdown when closer than this distance from an overlap
           velocity: 2.0  # [m/s] slowdown velocity