Add new parameter, also create function

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

Author: zulfaqar-azmi-t4

planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp

@@ -174,6 +174,8 @@ class NormalLaneChange : public LaneChangeBase
 
   bool isVehicleStuck(const lanelet::ConstLanelets & current_lanes) const;
 
+  bool check_prepare_phase() const;
+
   double calcMaximumLaneChangeLength(
     const lanelet::ConstLanelet & current_terminal_lanelet, const double max_acc) const;
 

planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/data_structs.hpp

@@ -112,7 +112,8 @@ struct LaneChangeParameters
   double max_longitudinal_acc{1.0};
 
   // collision check
-  bool enable_prepare_segment_collision_check{true};
+  bool enable_prepare_segment_collision_check_in_general_areas{false};
+  bool enable_prepare_segment_collision_check_in_intersection{true};
   double prepare_segment_ignore_object_velocity_thresh{0.1};
   bool check_objects_on_current_lanes{true};
   bool check_objects_on_other_lanes{true};

planning/behavior_path_lane_change_module/src/manager.cpp

@@ -68,8 +68,10 @@ void LaneChangeModuleManager::initParams(rclcpp::Node * node)
   p.max_longitudinal_acc = getOrDeclareParameter<double>(*node, parameter("max_longitudinal_acc"));
 
   // collision check
-  p.enable_prepare_segment_collision_check =
-    getOrDeclareParameter<bool>(*node, parameter("enable_prepare_segment_collision_check"));
+  p.enable_prepare_segment_collision_check_in_general_areas = getOrDeclareParameter<bool>(
+    *node, parameter("enable_prepare_segment_collision_check.general_areas"));
+  p.enable_prepare_segment_collision_check_in_intersection = getOrDeclareParameter<bool>(
+    *node, parameter("enable_prepare_segment_collision_check.intersection"));
   p.prepare_segment_ignore_object_velocity_thresh = getOrDeclareParameter<double>(
     *node, parameter("prepare_segment_ignore_object_velocity_thresh"));
   p.check_objects_on_current_lanes =

planning/behavior_path_lane_change_module/src/scene.cpp

@@ -862,38 +862,26 @@ LaneChangeTargetObjects NormalLaneChange::getTargetObjects(
   target_objects.other_lane.reserve(target_obj_index.other_lane.size());
 
   // objects in current lane
-
-  lanelet::ConstLanelet current_lane;
-  if (getRouteHandler()->getClosestLaneletWithinRoute(getEgoPose(), &current_lane)) {
-  }
-  const auto ego_footprint =
-    utils::lane_change::getEgoCurrentFootprint(getEgoPose(), common_parameters.vehicle_info);
-  const auto is_ego_within_intersection =
-    utils::lane_change::isWithinIntersection(getRouteHandler(), current_lane, ego_footprint);
-
-  const auto check_prepare_phase =
-    is_ego_within_intersection || lane_change_parameters_->enable_prepare_segment_collision_check;
-
   for (const auto & obj_idx : target_obj_index.current_lane) {
     const auto extended_object = utils::lane_change::transform(
       objects.objects.at(obj_idx), common_parameters, *lane_change_parameters_,
-      check_prepare_phase);
+      check_prepare_phase());
     target_objects.current_lane.push_back(extended_object);
   }
 
   // objects in target lane
   for (const auto & obj_idx : target_obj_index.target_lane) {
     const auto extended_object = utils::lane_change::transform(
       objects.objects.at(obj_idx), common_parameters, *lane_change_parameters_,
-      check_prepare_phase);
+      check_prepare_phase());
     target_objects.target_lane.push_back(extended_object);
   }
 
   // objects in other lane
   for (const auto & obj_idx : target_obj_index.other_lane) {
     const auto extended_object = utils::lane_change::transform(
       objects.objects.at(obj_idx), common_parameters, *lane_change_parameters_,
-      check_prepare_phase);
+      check_prepare_phase());
     target_objects.other_lane.push_back(extended_object);
   }
 
@@ -951,25 +939,14 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject(
     target_backward_polygons.push_back(lane_polygon);
   }
 
-  lanelet::ConstLanelet current_lane;
-  if (!route_handler.getClosestLaneletWithinRoute(getEgoPose(), &current_lane)) {
-  }
-  const auto ego_footprint =
-    utils::lane_change::getEgoCurrentFootprint(getEgoPose(), common_parameters.vehicle_info);
-  const auto is_ego_within_intersection =
-    utils::lane_change::isWithinIntersection(getRouteHandler(), current_lane, ego_footprint);
-
-  const auto check_prepare_phase =
-    is_ego_within_intersection || lane_change_parameters_->enable_prepare_segment_collision_check;
-
   LaneChangeTargetObjectIndices filtered_obj_indices;
   for (size_t i = 0; i < objects.objects.size(); ++i) {
     const auto & object = objects.objects.at(i);
     const auto obj_velocity_norm = std::hypot(
       object.kinematics.initial_twist_with_covariance.twist.linear.x,
       object.kinematics.initial_twist_with_covariance.twist.linear.y);
     const auto extended_object = utils::lane_change::transform(
-      object, common_parameters, *lane_change_parameters_, check_prepare_phase);
+      object, common_parameters, *lane_change_parameters_, check_prepare_phase());
 
     const auto obj_polygon = tier4_autoware_utils::toPolygon2d(object);
 
@@ -2108,4 +2085,29 @@ void NormalLaneChange::updateStopTime()
   stop_watch_.tic("stop_time");
 }
 
+bool NormalLaneChange::check_prepare_phase() const
+{
+  const auto & route_handler = getRouteHandler();
+  const auto & vehicle_info = getCommonParam().vehicle_info;
+
+  const auto check_prepare_phase_in_intersection = std::invoke([&]() {
+    if (!lane_change_parameters_->enable_prepare_segment_collision_check_in_intersection) {
+      return false;
+    }
+
+    lanelet::ConstLanelet current_lane;
+    if (!route_handler->getClosestLaneletWithinRoute(getEgoPose(), &current_lane)) {
+      RCLCPP_DEBUG(logger_, "unable to retreive current lane");
+      return false;
+    }
+
+    const auto ego_footprint =
+      utils::lane_change::getEgoCurrentFootprint(getEgoPose(), vehicle_info);
+    return utils::lane_change::isWithinIntersection(getRouteHandler(), current_lane, ego_footprint);
+  });
+
+  return check_prepare_phase_in_intersection ||
+         lane_change_parameters_->enable_prepare_segment_collision_check_in_general_areas;
+}
+
 }  // namespace behavior_path_planner