Refactor to use common function

Signed-off-by: Zulfaqar Azmi <[email protected]>

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp

@@ -316,6 +316,10 @@ bool is_before_terminal(
   const PredictedObject & object);
 
 double calc_angle_to_lanelet_segment(const lanelet::ConstLanelets & lanelets, const Pose & pose);
+
+ExtendedPredictedObjects transform_to_extended_objects(
+  const CommonDataPtr & common_data_ptr, const std::vector<PredictedObject> & objects,
+  const bool check_prepare_phase);
 }  // namespace autoware::behavior_path_planner::utils::lane_change
 
 namespace autoware::behavior_path_planner::utils::lane_change::debug

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp

@@ -997,7 +997,6 @@ lane_change::TargetObjects NormalLaneChange::getTargetObjects(
 FilteredByLanesExtendedObjects NormalLaneChange::filterObjects() const
 {
   const auto & route_handler = getRouteHandler();
-  const auto & common_parameters = planner_data_->parameters;
   auto objects = *planner_data_->dynamic_object;
   utils::path_safety_checker::filterObjectsByClass(
     objects, lane_change_parameters_->object_types_to_check);
@@ -1054,46 +1053,21 @@ FilteredByLanesExtendedObjects NormalLaneChange::filterObjects() const
       return is_within_vel_th(object) && ahead_of_ego;
     });
 
-  ExtendedPredictedObjects extended_target_lane_leading_objects;
   const auto is_check_prepare_phase = check_prepare_phase();
-  std::for_each(
-    filtered_by_lanes_objects.target_lane_leading.begin(),
-    filtered_by_lanes_objects.target_lane_leading.end(), [&](const auto & object) {
-      auto extended_predicted_object = utils::lane_change::transform(
-        object, common_parameters, *lane_change_parameters_, is_check_prepare_phase);
-      extended_target_lane_leading_objects.push_back(extended_predicted_object);
-    });
-
-  ExtendedPredictedObjects extended_target_lane_trailing_objects;
-  std::for_each(
-    filtered_by_lanes_objects.target_lane_trailing.begin(),
-    filtered_by_lanes_objects.target_lane_trailing.end(), [&](const auto & object) {
-      auto extended_predicted_object = utils::lane_change::transform(
-        object, common_parameters, *lane_change_parameters_, is_check_prepare_phase);
-      extended_target_lane_trailing_objects.push_back(extended_predicted_object);
-    });
-
-  ExtendedPredictedObjects extended_current_lane_objects;
-  std::for_each(
-    filtered_by_lanes_objects.current_lane.begin(), filtered_by_lanes_objects.current_lane.end(),
-    [&](const auto & object) {
-      auto extended_predicted_object = utils::lane_change::transform(
-        object, common_parameters, *lane_change_parameters_, is_check_prepare_phase);
-      extended_current_lane_objects.push_back(extended_predicted_object);
-    });
-
-  ExtendedPredictedObjects extended_other_lane_objects;
-  std::for_each(
-    filtered_by_lanes_objects.other_lane.begin(), filtered_by_lanes_objects.other_lane.end(),
-    [&](const auto & object) {
-      auto extended_predicted_object = utils::lane_change::transform(
-        object, common_parameters, *lane_change_parameters_, is_check_prepare_phase);
-      extended_other_lane_objects.push_back(extended_predicted_object);
-    });
+  const auto target_lane_leading_extended_objects =
+    utils::lane_change::transform_to_extended_objects(
+      common_data_ptr_, filtered_by_lanes_objects.target_lane_leading, is_check_prepare_phase);
+  const auto target_lane_trailing_extended_objects =
+    utils::lane_change::transform_to_extended_objects(
+      common_data_ptr_, filtered_by_lanes_objects.target_lane_trailing, is_check_prepare_phase);
+  const auto current_lane_extended_objects = utils::lane_change::transform_to_extended_objects(
+    common_data_ptr_, filtered_by_lanes_objects.current_lane, is_check_prepare_phase);
+  const auto other_lane_extended_objects = utils::lane_change::transform_to_extended_objects(
+    common_data_ptr_, filtered_by_lanes_objects.other_lane, is_check_prepare_phase);
 
   FilteredByLanesExtendedObjects lane_change_target_objects(
-    extended_current_lane_objects, extended_target_lane_leading_objects,
-    extended_target_lane_trailing_objects, extended_other_lane_objects);
+    current_lane_extended_objects, target_lane_leading_extended_objects,
+    target_lane_trailing_extended_objects, other_lane_extended_objects);
   lane_change_debug_.filtered_objects = lane_change_target_objects;
   return lane_change_target_objects;
 }

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp

@@ -1354,6 +1354,23 @@ double calc_angle_to_lanelet_segment(const lanelet::ConstLanelets & lanelets, co
   const auto closest_pose = lanelet::utils::getClosestCenterPose(closest_lanelet, pose.position);
   return std::abs(autoware::universe_utils::calcYawDeviation(closest_pose, pose));
 }
+
+ExtendedPredictedObjects transform_to_extended_objects(
+  const CommonDataPtr & common_data_ptr, const std::vector<PredictedObject> & objects,
+  const bool check_prepare_phase)
+{
+  ExtendedPredictedObjects extended_objects;
+  extended_objects.reserve(objects.size());
+
+  const auto & bpp_param = *common_data_ptr->bpp_param_ptr;
+  const auto & lc_param = *common_data_ptr->lc_param_ptr;
+  std::transform(
+    objects.begin(), objects.end(), std::back_inserter(extended_objects), [&](const auto & object) {
+      return utils::lane_change::transform(object, bpp_param, lc_param, check_prepare_phase);
+    });
+
+  return extended_objects;
+}
 }  // namespace autoware::behavior_path_planner::utils::lane_change
 
 namespace autoware::behavior_path_planner::utils::lane_change::debug