fix(avoidance): the module ignored merging objects unexpectedly

planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp

@@ -132,7 +132,7 @@ void AvoidanceByLaneChangeModuleManager::init(rclcpp::Node * node)
   }
 
   {
-    const std::string ns = "avoidance.target_filtering.force_avoidance.";
+    const std::string ns = "avoidance.target_filtering.avoidance_for_ambiguous_vehicle.";
     p.enable_force_avoidance_for_stopped_vehicle =
       getOrDeclareParameter<bool>(*node, ns + "enable");
     p.threshold_time_force_avoidance_for_stopped_vehicle =

planning/behavior_path_avoidance_module/config/avoidance.param.yaml

@@ -153,7 +153,7 @@
           backward_distance: 10.0                        # [m]
 
         # params for avoidance of vehicle type objects that are ambiguous as to whether they are parked.
-        force_avoidance:
+        avoidance_for_ambiguous_vehicle:
           enable: false                                  # [-]
           time_threshold: 1.0                            # [s]
           distance_threshold: 1.0                        # [m]

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp

@@ -420,6 +420,12 @@ struct ObjectData  // avoidance target
   // is parked vehicle on road shoulder
   bool is_parked{false};
 
+  // is driving on ego current lane
+  bool is_on_ego_lane{false};
+
+  // is ambiguous stopped vehicle.
+  bool is_ambiguous{false};
+
   // object direction.
   Direction direction{Direction::NONE};
 

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp

@@ -17,6 +17,7 @@
 
 #include "behavior_path_avoidance_module/data_structs.hpp"
 #include "behavior_path_avoidance_module/utils.hpp"
+#include "behavior_path_planner_common/utils/utils.hpp"
 
 #include <motion_utils/distance/distance.hpp>
 
@@ -134,6 +135,24 @@ class AvoidanceHelper
       shift_length, getLateralMaxJerkLimit(), getAvoidanceEgoSpeed());
   }
 
+  double getFrontConstantDistance(const ObjectData & object) const
+  {
+    const auto object_type = utils::getHighestProbLabel(object.object.classification);
+    const auto object_parameter = parameters_->object_parameters.at(object_type);
+    const auto & additional_buffer_longitudinal =
+      object_parameter.use_conservative_buffer_longitudinal ? data_->parameters.base_link2front
+                                                            : 0.0;
+    return object_parameter.safety_buffer_longitudinal + additional_buffer_longitudinal;
+  }
+
+  double getRearConstantDistance(const ObjectData & object) const
+  {
+    const auto object_type = utils::getHighestProbLabel(object.object.classification);
+    const auto object_parameter = parameters_->object_parameters.at(object_type);
+    return object_parameter.safety_buffer_longitudinal + data_->parameters.base_link2rear +
+           object.length;
+  }
+
   double getEgoShift() const
   {
     validate();

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp

@@ -140,7 +140,7 @@ AvoidanceParameters getParameter(rclcpp::Node * node)
   }
 
   {
-    const std::string ns = "avoidance.target_filtering.force_avoidance.";
+    const std::string ns = "avoidance.target_filtering.avoidance_for_ambiguous_vehicle.";
     p.enable_force_avoidance_for_stopped_vehicle =
       getOrDeclareParameter<bool>(*node, ns + "enable");
     p.threshold_time_force_avoidance_for_stopped_vehicle =

planning/behavior_path_avoidance_module/src/debug.cpp

@@ -1,4 +1,4 @@
 // Copyright 2021 Tier IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -188,10 +188,25 @@
   return msg;
 }
 
+MarkerArray createOverhangLaneletMarkerArray(const ObjectDataArray & objects, std::string && ns)
+{
+  MarkerArray msg;
+  msg.markers.reserve(objects.size());
+
+  for (const auto & object : objects) {
+    appendMarkerArray(
+      marker_utils::createLaneletsAreaMarkerArray(
+        {object.overhang_lanelet}, std::string(ns), 0.0, 0.0, 1.0),
+      &msg);
+  }
+
+  return msg;
+}
+
 MarkerArray avoidableObjectsMarkerArray(const ObjectDataArray & objects, std::string && ns)
 {
   MarkerArray msg;
-  msg.markers.reserve(objects.size() * 4);
+  msg.markers.reserve(objects.size() * 5);
 
   appendMarkerArray(
     createObjectsCubeMarkerArray(
@@ -202,14 +217,15 @@
   appendMarkerArray(createObjectInfoMarkerArray(objects, ns + "_info"), &msg);
   appendMarkerArray(createObjectPolygonMarkerArray(objects, ns + "_envelope_polygon"), &msg);
   appendMarkerArray(createToDrivableBoundDistance(objects, ns + "_to_drivable_bound"), &msg);
+  appendMarkerArray(createOverhangLaneletMarkerArray(objects, ns + "_overhang_lanelet"), &msg);
 
   return msg;
 }
 
 MarkerArray unAvoidableObjectsMarkerArray(const ObjectDataArray & objects, std::string && ns)
 {
   MarkerArray msg;
-  msg.markers.reserve(objects.size() * 4);
+  msg.markers.reserve(objects.size() * 5);
 
   appendMarkerArray(
     createObjectsCubeMarkerArray(
@@ -220,6 +236,7 @@
   appendMarkerArray(createObjectInfoMarkerArray(objects, ns + "_info"), &msg);
   appendMarkerArray(createObjectPolygonMarkerArray(objects, ns + "_envelope_polygon"), &msg);
   appendMarkerArray(createToDrivableBoundDistance(objects, ns + "_to_drivable_bound"), &msg);
+  appendMarkerArray(createOverhangLaneletMarkerArray(objects, ns + "_overhang_lanelet"), &msg);
 
   return msg;
 }
@@ -451,6 +468,10 @@
   appendMarkerArray(
     createObjectInfoMarkerArray(filtered_objects, "others_" + convertToSnakeCase(ns) + "_info"),
     &msg);
+  appendMarkerArray(
+    createOverhangLaneletMarkerArray(
+      filtered_objects, "others_" + convertToSnakeCase(ns) + "_overhang_lanelet"),
+    &msg);
 
   return msg;
 }

planning/behavior_path_avoidance_module/src/scene.cpp

@@ -1,4 +1,4 @@
 // Copyright 2023 TIER IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -638,21 +638,16 @@
   const auto o_front = data.stop_target_object.value();
   const auto object_type = utils::getHighestProbLabel(o_front.object.classification);
   const auto object_parameter = parameters_->object_parameters.at(object_type);
-  const auto & additional_buffer_longitudinal =
-    object_parameter.use_conservative_buffer_longitudinal
-      ? planner_data_->parameters.base_link2front
-      : 0.0;
+  const auto constant_distance = helper_->getFrontConstantDistance(o_front);
   const auto & vehicle_width = planner_data_->parameters.vehicle_width;
 
   const auto max_avoid_margin = object_parameter.lateral_hard_margin * o_front.distance_factor +
                                 object_parameter.lateral_soft_margin + 0.5 * vehicle_width;
 
-  const auto variable = helper_->getSharpAvoidanceDistance(
+  const auto avoidance_distance = helper_->getSharpAvoidanceDistance(
     helper_->getShiftLength(o_front, utils::avoidance::isOnRight(o_front), max_avoid_margin));
-  const auto constant = helper_->getNominalPrepareDistance() +
-                        object_parameter.safety_buffer_longitudinal +
-                        additional_buffer_longitudinal;
-  const auto total_avoid_distance = variable + constant;
+  const auto prepare_distance = helper_->getNominalPrepareDistance();
+  const auto total_avoid_distance = prepare_distance + avoidance_distance + constant_distance;
 
   dead_pose_ = calcLongitudinalOffsetPose(
     data.reference_path.points, getEgoPosition(), o_front.longitudinal - total_avoid_distance);
@@ -1434,16 +1429,14 @@
 
   const auto avoid_margin = object_parameter.lateral_hard_margin * object.distance_factor +
                             object_parameter.lateral_soft_margin + 0.5 * vehicle_width;
-  const auto variable = helper_->getMinAvoidanceDistance(
+  const auto avoidance_distance = helper_->getMinAvoidanceDistance(
     helper_->getShiftLength(object, utils::avoidance::isOnRight(object), avoid_margin));
-  const auto & additional_buffer_longitudinal =
-    object_parameter.use_conservative_buffer_longitudinal
-      ? planner_data_->parameters.base_link2front
-      : 0.0;
-  const auto constant = p->min_prepare_distance + object_parameter.safety_buffer_longitudinal +
-                        additional_buffer_longitudinal + p->stop_buffer;
-
-  return object.longitudinal - std::min(variable + constant, p->stop_max_distance);
+  const auto constant_distance = helper_->getFrontConstantDistance(object);
+
+  return object.longitudinal -
+         std::min(
+           avoidance_distance + constant_distance + p->min_prepare_distance + p->stop_buffer,
+           p->stop_max_distance);
 }
 
 void AvoidanceModule::insertReturnDeadLine(

planning/behavior_path_avoidance_module/src/shift_line_generator.cpp

@@ -1,4 +1,4 @@
 // Copyright 2023 TIER IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -122,224 +122,213 @@
   AvoidancePlanningData & data, [[maybe_unused]] DebugData & debug) const
 {
   // To be consistent with changes in the ego position, the current shift length is considered.
   const auto current_ego_shift = helper_->getEgoShift();
-  const auto & base_link2rear = data_->parameters.base_link2rear;
 
   // Calculate feasible shift length
   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);
     const auto is_object_on_right = utils::avoidance::isOnRight(object);
 
     // use absolute dist for return-to-center, relative dist from current for avoiding.
     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 ? 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 constant_distance = helper_->getFrontConstantDistance(object);
+    const auto has_enough_distance =
+      object.longitudinal > constant_distance + prepare_distance + nominal_avoid_distance;
+    const auto remaining_distance = object.longitudinal - constant_distance - prepare_distance;
     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 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 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 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 std::make_pair(desire_shift_length, avoidance_distance);
     }
 
     // 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 std::make_pair(desire_shift_length, avoidance_distance);
     }
 
     // prepare distance is not enough. unavoidable.
     if (avoidance_distance < 1e-3) {
       object.reason = AvoidanceDebugFactor::REMAINING_DISTANCE_LESS_THAN_ZERO;
       return std::nullopt;
     }
 
     // calculate lateral jerk.
     const auto required_jerk = PathShifter::calcJerkFromLatLonDistance(
       avoiding_shift, avoidance_distance, helper_->getAvoidanceEgoSpeed());
 
     // relax lateral jerk limit. avoidable.
     if (required_jerk < helper_->getLateralMaxJerkLimit()) {
       return std::make_pair(desire_shift_length, avoidance_distance);
     }
 
     constexpr double LON_DIST_BUFFER = 1e-3;
 
     // avoidance distance is not enough. unavoidable.
     if (!isBestEffort(parameters_->policy_deceleration)) {
       if (avoidance_distance < helper_->getMinAvoidanceDistance(avoiding_shift) + LON_DIST_BUFFER) {
         object.reason = AvoidanceDebugFactor::REMAINING_DISTANCE_LESS_THAN_ZERO;
         return std::nullopt;
       } else {
         object.reason = AvoidanceDebugFactor::TOO_LARGE_JERK;
         return std::nullopt;
       }
     }
 
     // output avoidance path under lateral jerk constraints.
     const auto feasible_relative_shift_length = PathShifter::calcLateralDistFromJerk(
       avoidance_distance, helper_->getLateralMaxJerkLimit(), helper_->getAvoidanceEgoSpeed());
 
     if (std::abs(feasible_relative_shift_length) < parameters_->lateral_execution_threshold) {
       object.reason = "LessThanExecutionThreshold";
       return std::nullopt;
     }
 
     const auto feasible_shift_length =
       desire_shift_length > 0.0 ? feasible_relative_shift_length + current_ego_shift
                                 : -1.0 * feasible_relative_shift_length + current_ego_shift;
 
     if (
       avoidance_distance <
       helper_->getMinAvoidanceDistance(feasible_shift_length) + LON_DIST_BUFFER) {
       object.reason = AvoidanceDebugFactor::REMAINING_DISTANCE_LESS_THAN_ZERO;
       return std::nullopt;
     }
 
     const double LAT_DIST_BUFFER = desire_shift_length > 0.0 ? 1e-3 : -1e-3;
 
     const auto lateral_hard_margin = object.is_parked
                                        ? object_parameter.lateral_hard_margin_for_parked_vehicle
                                        : object_parameter.lateral_hard_margin;
     const auto infeasible =
       std::abs(feasible_shift_length - object.overhang_points.front().first) - LAT_DIST_BUFFER <
       0.5 * data_->parameters.vehicle_width + lateral_hard_margin;
     if (infeasible) {
       RCLCPP_DEBUG(rclcpp::get_logger(""), "feasible shift length is not enough to avoid. ");
       object.reason = AvoidanceDebugFactor::TOO_LARGE_JERK;
       return std::nullopt;
     }
 
     return std::make_pair(feasible_shift_length - LAT_DIST_BUFFER, avoidance_distance);
   };
 
   const auto is_forward_object = [](const auto & object) { return object.longitudinal > 0.0; };
 
   const auto is_valid_shift_line = [](const auto & s) {
     return s.start_longitudinal > 0.0 && s.start_longitudinal < s.end_longitudinal;
   };
 
   AvoidOutlines outlines;
   for (auto & o : data.target_objects) {
     if (!o.avoid_margin.has_value()) {
       o.reason = AvoidanceDebugFactor::INSUFFICIENT_LATERAL_MARGIN;
       if (o.avoid_required && is_forward_object(o)) {
         break;
       } else {
         continue;
       }
     }
 
     const auto is_object_on_right = utils::avoidance::isOnRight(o);
     const auto desire_shift_length =
       helper_->getShiftLength(o, is_object_on_right, o.avoid_margin.value());
     if (utils::avoidance::isSameDirectionShift(is_object_on_right, desire_shift_length)) {
       o.reason = AvoidanceDebugFactor::SAME_DIRECTION_SHIFT;
       if (o.avoid_required && is_forward_object(o)) {
         break;
       } else {
         continue;
       }
     }
 
-    // use each object param
-    const auto object_type = utils::getHighestProbLabel(o.object.classification);
-    const auto object_parameter = parameters_->object_parameters.at(object_type);
+    // calculate feasible shift length based on behavior policy
     const auto feasible_shift_profile = get_shift_profile(o, desire_shift_length);
-
     if (!feasible_shift_profile.has_value()) {
       if (o.avoid_required && is_forward_object(o)) {
         break;
       } else {
         continue;
       }
     }
 
     // use absolute dist for return-to-center, relative dist from current for avoiding.
     const auto feasible_return_distance =
       helper_->getMaxAvoidanceDistance(feasible_shift_profile.value().first);
 
     AvoidLine al_avoid;
     {
-      const auto & additional_buffer_longitudinal =
-        object_parameter.use_conservative_buffer_longitudinal ? data_->parameters.base_link2front
-                                                              : 0.0;
-      const auto offset =
-        object_parameter.safety_buffer_longitudinal + additional_buffer_longitudinal;
-      const auto to_shift_end = o.longitudinal - offset;
+      const auto constant_distance = helper_->getFrontConstantDistance(o);
+      const auto to_shift_end = o.longitudinal - constant_distance;
       const auto path_front_to_ego = data.arclength_from_ego.at(data.ego_closest_path_index);
 
       // 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_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_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);
       }();
 
       al_avoid.start_idx = utils::avoidance::findPathIndexFromArclength(
         data.arclength_from_ego, al_avoid.start_longitudinal + path_front_to_ego);
       al_avoid.start = data.reference_path.points.at(al_avoid.start_idx).point.pose;
       al_avoid.start_shift_length = helper_->getLinearShift(al_avoid.start.position);
 
       // end point
       al_avoid.end_shift_length = feasible_shift_profile.value().first;
       al_avoid.end_longitudinal = to_shift_end;
 
       // misc
       al_avoid.id = generateUUID();
       al_avoid.object = o;
       al_avoid.object_on_right = utils::avoidance::isOnRight(o);
     }
 
     AvoidLine al_return;
     {
-      const auto offset = object_parameter.safety_buffer_longitudinal + base_link2rear + o.length;
-      const auto to_shift_start = o.longitudinal + offset;
+      const auto constant_distance = helper_->getRearConstantDistance(o);
+      const auto to_shift_start = o.longitudinal + constant_distance;
 
       // start point
       al_return.start_shift_length = feasible_shift_profile.value().first;

planning/behavior_path_avoidance_module/src/utils.cpp

@@ -1,4 +1,4 @@
 // Copyright 2023 TIER IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -478,6 +478,14 @@
     object_polygon, utils::toPolygon2d(lanelet::utils::to2D(polygon.basicPolygon())));
 }
 
+bool isOnEgoLane(const ObjectData & object)
+{
+  const auto & object_pos = object.object.kinematics.initial_pose_with_covariance.pose.position;
+  return boost::geometry::within(
+    lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object_pos)).basicPoint(),
+    object.overhang_lanelet.polygon2d().basicPolygon());
+}
+
 bool isParallelToEgoLane(const ObjectData & object, const double threshold)
 {
   const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose;
@@ -525,6 +533,10 @@
   using lanelet::utils::to2D;
   using lanelet::utils::conversion::toLaneletPoint;
 
+  if (object.is_within_intersection) {
+    return false;
+  }
+
   const auto & object_pos = object.object.kinematics.initial_pose_with_covariance.pose.position;
   const auto centerline_pos =
     lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object_pos).position;
@@ -613,45 +625,45 @@
   return is_left_side_parked_vehicle || is_right_side_parked_vehicle;
 }
 
-bool isForceAvoidanceTarget(
+bool isAmbiguousStoppedVehicle(
   ObjectData & object, const AvoidancePlanningData & data,
   const std::shared_ptr<const PlannerData> & planner_data,
   const std::shared_ptr<AvoidanceParameters> & parameters)
 {
-  if (!parameters->enable_force_avoidance_for_stopped_vehicle) {
-    return false;
-  }
-
   const auto stop_time_longer_than_threshold =
     object.stop_time > parameters->threshold_time_force_avoidance_for_stopped_vehicle;
 
   if (!stop_time_longer_than_threshold) {
     return false;
   }
 
   const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose;
   const auto is_moving_distance_longer_than_threshold =
     tier4_autoware_utils::calcDistance2d(object.init_pose, object_pose) >
     parameters->force_avoidance_distance_threshold;
 
   if (is_moving_distance_longer_than_threshold) {
     return false;
   }
 
   if (object.is_within_intersection) {
     RCLCPP_DEBUG(rclcpp::get_logger(__func__), "object is in the intersection area.");
     return false;
   }
 
   const auto rh = planner_data->route_handler;
 
   if (
     !!rh->getRoutingGraphPtr()->right(object.overhang_lanelet) &&
     !!rh->getRoutingGraphPtr()->left(object.overhang_lanelet)) {
     RCLCPP_DEBUG(rclcpp::get_logger(__func__), "object isn't on the edge lane.");
     return false;
   }
 
+  if (!object.is_on_ego_lane) {
+    return true;
+  }
+
   const auto & ego_pose = planner_data->self_odometry->pose.pose;
 
   // force avoidance for stopped vehicle
@@ -777,25 +789,18 @@
   ObjectData & object, const AvoidancePlanningData & data,
   const std::shared_ptr<const PlannerData> & planner_data,
   const std::shared_ptr<AvoidanceParameters> & parameters)
 {
-  using boost::geometry::within;
-  using lanelet::utils::to2D;
-  using lanelet::utils::conversion::toLaneletPoint;
-
   object.behavior = getObjectBehavior(object, parameters);
-  object.is_within_intersection = isWithinIntersection(object, planner_data->route_handler);
+  object.is_on_ego_lane = isOnEgoLane(object);
+  object.is_ambiguous = isAmbiguousStoppedVehicle(object, data, planner_data, parameters);
 
   // from here condition check for vehicle type objects.
-  if (isForceAvoidanceTarget(object, data, planner_data, parameters)) {
+  if (object.is_ambiguous && parameters->enable_force_avoidance_for_stopped_vehicle) {
     return true;
   }
 
   // check vehicle shift ratio
-  const auto & object_pos = object.object.kinematics.initial_pose_with_covariance.pose.position;
-  const auto on_ego_driving_lane = within(
-    to2D(toLaneletPoint(object_pos)).basicPoint(),
-    object.overhang_lanelet.polygon2d().basicPolygon());
-  if (on_ego_driving_lane) {
+  if (object.is_on_ego_lane) {
     if (object.is_parked) {
       return true;
     } else {
@@ -1755,6 +1760,8 @@
     }
 
     if (filtering_utils::isVehicleTypeObject(o)) {
+      o.is_within_intersection =
+        filtering_utils::isWithinIntersection(o, planner_data->route_handler);
       o.is_parked = filtering_utils::isParkedVehicle(o, planner_data->route_handler, parameters);
       o.avoid_margin = filtering_utils::getAvoidMargin(o, planner_data, parameters);