refactor(avoidance_by_lane_change): update execution condition (autow…

…arefoundation#5869)

* refactor(avoidance_by_lane_change): update execution condition

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* fix lc parameter

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* fix both lane change and avoidance by lane change both are running

Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* trying to set maximum_avoid_distance

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* fix avoidance param not properly assigned

Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* fixed avoidance not running

Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* fix root lanelet

Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* removed gdb

Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* add debug

Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* fix unnecessary changes

Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* Update planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp

Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com>

---------

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com>
Signed-off-by: karishma <karishma@interpl.ai>

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,7 +39,9 @@ void AvoidanceByLaneChangeModuleManager::init(rclcpp::Node * node)
   // 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.";
@@ -139,6 +143,18 @@ void AvoidanceByLaneChangeModuleManager::init(rclcpp::Node * node)
       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 @@ AvoidanceByLaneChange::AvoidanceByLaneChange(
   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