refactor(avoidance_by_lane_change): update execution condition

planning/behavior_path_avoidance_by_lane_change_module/config/avoidance_by_lc.param.yaml

@@ -85,3 +85,11 @@
           bicycle: false                               # [-]
           motorcycle: false                            # [-]
           pedestrian: false                            # [-]
+
+      constraints:
+        # lateral constraints
+        lateral:
+          velocity: [1.0, 1.38, 11.1]                   # [m/s]
+          max_accel_values: [0.5, 0.5, 0.5]             # [m/ss]
+          min_jerk_values: [0.2, 0.2, 0.2]              # [m/sss]
+          max_jerk_values: [1.0, 1.0, 1.0]              # [m/sss]

planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/data_structs.hpp

@@ -16,8 +16,6 @@
 
 #include "behavior_path_avoidance_module/data_structs.hpp"
 
-#include <vector>
-
 namespace behavior_path_planner
 {
 struct AvoidanceByLCParameters : public AvoidanceParameters
@@ -27,6 +25,10 @@ struct AvoidanceByLCParameters : public AvoidanceParameters
 
   // execute only when lane change end point is before the object.
   bool execute_only_when_lane_change_finish_before_object{false};
+
+  explicit AvoidanceByLCParameters(const AvoidanceParameters & param) : AvoidanceParameters(param)
+  {
+  }
 };
 }  // namespace behavior_path_planner
 

planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/interface.hpp

@@ -40,6 +40,8 @@ class AvoidanceByLaneChangeInterface : public LaneChangeInterface
 
   bool isExecutionRequested() const override;
 
+  void processOnEntry() override;
+
 protected:
   void updateRTCStatus(const double start_distance, const double finish_distance) override;
 };

planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/scene.hpp

@@ -16,13 +16,15 @@
 #define BEHAVIOR_PATH_AVOIDANCE_BY_LANE_CHANGE_MODULE__SCENE_HPP_
 
 #include "behavior_path_avoidance_by_lane_change_module/data_structs.hpp"
+#include "behavior_path_avoidance_module/helper.hpp"
 #include "behavior_path_lane_change_module/scene.hpp"
 
 #include <memory>
 
 namespace behavior_path_planner
 {
 using AvoidanceDebugData = DebugData;
+using helper::avoidance::AvoidanceHelper;
 
 class AvoidanceByLaneChange : public NormalLaneChange
 {
@@ -46,6 +48,7 @@ class AvoidanceByLaneChange : public NormalLaneChange
 
   ObjectDataArray registered_objects_;
   mutable ObjectDataArray stopped_objects_;
+  std::shared_ptr<AvoidanceHelper> avoidance_helper_;
 
   ObjectData createObjectData(
     const AvoidancePlanningData & data, const PredictedObject & object) const;

planning/behavior_path_avoidance_by_lane_change_module/src/interface.cpp

@@ -45,7 +45,11 @@ AvoidanceByLaneChangeInterface::AvoidanceByLaneChangeInterface(
 
 bool AvoidanceByLaneChangeInterface::isExecutionRequested() const
 {
-  return module_type_->specialRequiredCheck() && module_type_->isLaneChangeRequired();
+  return module_type_->isLaneChangeRequired() && module_type_->specialRequiredCheck();
+}
+void AvoidanceByLaneChangeInterface::processOnEntry()
+{
+  waitApproval();
 }
 
 void AvoidanceByLaneChangeInterface::updateRTCStatus(

planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp

@@ -14,6 +14,8 @@
 
 #include "behavior_path_avoidance_by_lane_change_module/manager.hpp"
 
+#include "behavior_path_avoidance_by_lane_change_module/data_structs.hpp"
+#include "behavior_path_avoidance_module/parameter_helper.hpp"
 #include "tier4_autoware_utils/ros/parameter.hpp"
 #include "tier4_autoware_utils/ros/update_param.hpp"
 
@@ -37,108 +39,122 @@
   // init lane change manager
   LaneChangeModuleManager::init(node);
 
-  AvoidanceByLCParameters p{};
+  const auto avoidance_params = getParameter(node);
+  AvoidanceByLCParameters p(avoidance_params);
+
   // unique parameters
   {
     const std::string ns = "avoidance_by_lane_change.";
     p.execute_object_longitudinal_margin =
       getOrDeclareParameter<double>(*node, ns + "execute_object_longitudinal_margin");
     p.execute_only_when_lane_change_finish_before_object =
       getOrDeclareParameter<bool>(*node, ns + "execute_only_when_lane_change_finish_before_object");
   }
 
   // general params
   {
     const std::string ns = "avoidance.";
     p.resample_interval_for_planning =
       getOrDeclareParameter<double>(*node, ns + "resample_interval_for_planning");
     p.resample_interval_for_output =
       getOrDeclareParameter<double>(*node, ns + "resample_interval_for_output");
   }
 
   // target object
   {
     const auto get_object_param = [&](std::string && ns) {
       ObjectParameter param{};
       param.execute_num = getOrDeclareParameter<int>(*node, ns + "execute_num");
       param.moving_speed_threshold =
         getOrDeclareParameter<double>(*node, ns + "moving_speed_threshold");
       param.moving_time_threshold =
         getOrDeclareParameter<double>(*node, ns + "moving_time_threshold");
       param.max_expand_ratio = getOrDeclareParameter<double>(*node, ns + "max_expand_ratio");
       param.envelope_buffer_margin =
         getOrDeclareParameter<double>(*node, ns + "envelope_buffer_margin");
       param.avoid_margin_lateral =
         getOrDeclareParameter<double>(*node, ns + "avoid_margin_lateral");
       param.safety_buffer_lateral =
         getOrDeclareParameter<double>(*node, ns + "safety_buffer_lateral");
       return param;
     };
 
     const std::string ns = "avoidance_by_lane_change.target_object.";
     p.object_parameters.emplace(
       ObjectClassification::MOTORCYCLE, get_object_param(ns + "motorcycle."));
     p.object_parameters.emplace(ObjectClassification::CAR, get_object_param(ns + "car."));
     p.object_parameters.emplace(ObjectClassification::TRUCK, get_object_param(ns + "truck."));
     p.object_parameters.emplace(ObjectClassification::TRAILER, get_object_param(ns + "trailer."));
     p.object_parameters.emplace(ObjectClassification::BUS, get_object_param(ns + "bus."));
     p.object_parameters.emplace(
       ObjectClassification::PEDESTRIAN, get_object_param(ns + "pedestrian."));
     p.object_parameters.emplace(ObjectClassification::BICYCLE, get_object_param(ns + "bicycle."));
     p.object_parameters.emplace(ObjectClassification::UNKNOWN, get_object_param(ns + "unknown."));
 
     p.lower_distance_for_polygon_expansion =
       getOrDeclareParameter<double>(*node, ns + "lower_distance_for_polygon_expansion");
     p.upper_distance_for_polygon_expansion =
       getOrDeclareParameter<double>(*node, ns + "upper_distance_for_polygon_expansion");
   }
 
   // target filtering
   {
     const auto set_target_flag = [&](const uint8_t & object_type, const std::string & ns) {
       if (p.object_parameters.count(object_type) == 0) {
         return;
       }
       p.object_parameters.at(object_type).is_avoidance_target =
         getOrDeclareParameter<bool>(*node, ns);
     };
 
     const std::string ns = "avoidance.target_filtering.";
     set_target_flag(ObjectClassification::CAR, ns + "target_type.car");
     set_target_flag(ObjectClassification::TRUCK, ns + "target_type.truck");
     set_target_flag(ObjectClassification::TRAILER, ns + "target_type.trailer");
     set_target_flag(ObjectClassification::BUS, ns + "target_type.bus");
     set_target_flag(ObjectClassification::PEDESTRIAN, ns + "target_type.pedestrian");
     set_target_flag(ObjectClassification::BICYCLE, ns + "target_type.bicycle");
     set_target_flag(ObjectClassification::MOTORCYCLE, ns + "target_type.motorcycle");
     set_target_flag(ObjectClassification::UNKNOWN, ns + "target_type.unknown");
 
     p.object_check_goal_distance =
       getOrDeclareParameter<double>(*node, ns + "object_check_goal_distance");
     p.threshold_distance_object_is_on_center =
       getOrDeclareParameter<double>(*node, ns + "threshold_distance_object_is_on_center");
     p.object_check_shiftable_ratio =
       getOrDeclareParameter<double>(*node, ns + "object_check_shiftable_ratio");
     p.object_check_min_road_shoulder_width =
       getOrDeclareParameter<double>(*node, ns + "object_check_min_road_shoulder_width");
     p.object_last_seen_threshold =
       getOrDeclareParameter<double>(*node, ns + "object_last_seen_threshold");
   }
 
   {
     const std::string ns = "avoidance.target_filtering.force_avoidance.";
     p.enable_force_avoidance_for_stopped_vehicle =
       getOrDeclareParameter<bool>(*node, ns + "enable");
     p.threshold_time_force_avoidance_for_stopped_vehicle =
       getOrDeclareParameter<double>(*node, ns + "time_threshold");
     p.object_ignore_section_traffic_light_in_front_distance =
       getOrDeclareParameter<double>(*node, ns + "ignore_area.traffic_light.front_distance");
     p.object_ignore_section_crosswalk_in_front_distance =
       getOrDeclareParameter<double>(*node, ns + "ignore_area.crosswalk.front_distance");
     p.object_ignore_section_crosswalk_behind_distance =
       getOrDeclareParameter<double>(*node, ns + "ignore_area.crosswalk.behind_distance");
   }
 
+  // avoidance maneuver (longitudinal)
+  {
+    const std::string ns = "avoidance.avoidance.longitudinal.";
+    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");
+    p.nominal_avoidance_speed =
+      getOrDeclareParameter<double>(*node, ns + "nominal_avoidance_speed");
+  }
+
   {
     const std::string ns = "avoidance.target_filtering.detection_area.";
     p.use_static_detection_area = getOrDeclareParameter<bool>(*node, ns + "static");

planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp

@@ -20,6 +20,8 @@
 #include "behavior_path_planner_common/utils/path_utils.hpp"
 #include "behavior_path_planner_common/utils/utils.hpp"
 
+#include <rclcpp/logging.hpp>
+
 #include <boost/geometry/algorithms/centroid.hpp>
 #include <boost/geometry/strategies/cartesian/centroid_bashein_detmer.hpp>
 
@@ -41,56 +43,75 @@
   const std::shared_ptr<LaneChangeParameters> & parameters,
   std::shared_ptr<AvoidanceByLCParameters> avoidance_parameters)
 : NormalLaneChange(parameters, LaneChangeModuleType::AVOIDANCE_BY_LANE_CHANGE, Direction::NONE),
-  avoidance_parameters_(std::move(avoidance_parameters))
+  avoidance_parameters_(std::move(avoidance_parameters)),
+  avoidance_helper_{std::make_shared<AvoidanceHelper>(avoidance_parameters_)}
 {
 }
 
 bool AvoidanceByLaneChange::specialRequiredCheck() const
 {
   const auto & data = avoidance_data_;
 
-  if (!status_.is_safe) {
+  if (data.target_objects.empty()) {
     return false;
   }
 
   const auto & object_parameters = avoidance_parameters_->object_parameters;
-  const auto is_more_than_threshold =
-    std::any_of(object_parameters.begin(), object_parameters.end(), [&](const auto & p) {
-      const auto & objects = avoidance_data_.target_objects;
-
-      const size_t num = std::count_if(objects.begin(), objects.end(), [&p](const auto & object) {
-        const auto target_class =
-          utils::getHighestProbLabel(object.object.classification) == p.first;
-        return target_class && object.avoid_required;
-      });
 
-      return num >= p.second.execute_num;
-    });
+  const auto count_target_object = [&](const auto sum, const auto & p) {
+    const auto & objects = avoidance_data_.target_objects;
 
-  if (!is_more_than_threshold) {
-    return false;
-  }
+    const auto is_avoidance_target = [&p](const auto & object) {
+      const auto target_class = utils::getHighestProbLabel(object.object.classification) == p.first;
+      return target_class && object.avoid_required;
+    };
 
-  const auto & front_object = data.target_objects.front();
+    return sum + std::count_if(objects.begin(), objects.end(), is_avoidance_target);
+  };
+  const auto num_of_avoidance_targets =
+    std::accumulate(object_parameters.begin(), object_parameters.end(), 0UL, count_target_object);
 
-  const auto THRESHOLD = avoidance_parameters_->execute_object_longitudinal_margin;
-  if (front_object.longitudinal < THRESHOLD) {
+  if (num_of_avoidance_targets < 1) {
     return false;
   }
 
-  const auto path = status_.lane_change_path.path;
-  const auto to_lc_end = motion_utils::calcSignedArcLength(
-    status_.lane_change_path.path.points, getEgoPose().position,
-    status_.lane_change_path.info.shift_line.end.position);
-  const auto execute_only_when_lane_change_finish_before_object =
-    avoidance_parameters_->execute_only_when_lane_change_finish_before_object;
-  const auto not_enough_distance = front_object.longitudinal < to_lc_end;
-
-  if (execute_only_when_lane_change_finish_before_object && not_enough_distance) {
+  const auto current_lanes = getCurrentLanes();
+  if (current_lanes.empty()) {
     return false;
   }
 
-  return true;
+  const auto & nearest_object = data.target_objects.front();
+
+  // get minimum lane change distance
+  const auto shift_intervals =
+    getRouteHandler()->getLateralIntervalsToPreferredLane(current_lanes.back(), direction_);
+  const double minimum_lane_change_length = utils::lane_change::calcMinimumLaneChangeLength(
+    *lane_change_parameters_, shift_intervals,
+    lane_change_parameters_->backward_length_buffer_for_end_of_lane);
+
+  // get minimum avoid distance
+
+  const auto ego_width = getCommonParam().vehicle_width;
+  const auto nearest_object_type = utils::getHighestProbLabel(nearest_object.object.classification);
+  const auto nearest_object_parameter =
+    avoidance_parameters_->object_parameters.at(nearest_object_type);
+  const auto avoid_margin =
+    nearest_object_parameter.safety_buffer_lateral * nearest_object.distance_factor +
+    nearest_object_parameter.avoid_margin_lateral + 0.5 * ego_width;
+
+  avoidance_helper_->setData(planner_data_);
+  const auto shift_length = avoidance_helper_->getShiftLength(
+    nearest_object, utils::avoidance::isOnRight(nearest_object), avoid_margin);
+
+  const auto maximum_avoid_distance = avoidance_helper_->getMaxAvoidanceDistance(shift_length);
+
+  RCLCPP_DEBUG(
+    logger_,
+    "nearest_object.longitudinal %.3f, minimum_lane_change_length %.3f, maximum_avoid_distance "
+    "%.3f",
+    nearest_object.longitudinal, minimum_lane_change_length, maximum_avoid_distance);
+
+  return nearest_object.longitudinal > std::max(minimum_lane_change_length, maximum_avoid_distance);
 }
 
 bool AvoidanceByLaneChange::specialExpiredCheck() const