fix(avoidance): cherr pick avoidance hotfix PRs

planning/behavior_path_avoidance_by_lane_change_module/config/avoidance_by_lc.param.yaml

@@ -12,64 +12,80 @@
           moving_time_threshold: 1.0                   # [s]
           max_expand_ratio: 0.0                        # [-]
           envelope_buffer_margin: 0.3                  # [m]
-          avoid_margin_lateral: 1.0                    # [m]
-          safety_buffer_lateral: 0.7                   # [m]
+          lateral_margin:
+            soft_margin: 0.0                           # [m]
+            hard_margin: 0.0                           # [m]
+            hard_margin_for_parked_vehicle: 0.0        # [m]
         truck:
           execute_num: 2
           moving_speed_threshold: 1.0                  # 3.6km/h
           moving_time_threshold: 1.0
           max_expand_ratio: 0.0
           envelope_buffer_margin: 0.3
-          avoid_margin_lateral: 1.0
-          safety_buffer_lateral: 0.7
+          lateral_margin:
+            soft_margin: 0.0                           # [m]
+            hard_margin: 0.0                           # [m]
+            hard_margin_for_parked_vehicle: 0.0        # [m]
         bus:
           execute_num: 2
           moving_speed_threshold: 1.0                  # 3.6km/h
           moving_time_threshold: 1.0
           max_expand_ratio: 0.0
           envelope_buffer_margin: 0.3
-          avoid_margin_lateral: 1.0
-          safety_buffer_lateral: 0.7
+          lateral_margin:
+            soft_margin: 0.0                           # [m]
+            hard_margin: 0.0                           # [m]
+            hard_margin_for_parked_vehicle: 0.0        # [m]
         trailer:
           execute_num: 2
           moving_speed_threshold: 1.0                  # 3.6km/h
           moving_time_threshold: 1.0
           max_expand_ratio: 0.0
           envelope_buffer_margin: 0.3
-          avoid_margin_lateral: 1.0
-          safety_buffer_lateral: 0.7
+          lateral_margin:
+            soft_margin: 0.0                           # [m]
+            hard_margin: 0.0                           # [m]
+            hard_margin_for_parked_vehicle: 0.0        # [m]
         unknown:
           execute_num: 1
           moving_speed_threshold: 0.28                 # 1.0km/h
           moving_time_threshold: 1.0
           max_expand_ratio: 0.0
           envelope_buffer_margin: 0.3
-          avoid_margin_lateral: 1.0
-          safety_buffer_lateral: 0.0
+          lateral_margin:
+            soft_margin: 0.0                           # [m]
+            hard_margin: 0.0                           # [m]
+            hard_margin_for_parked_vehicle: 0.0        # [m]
         bicycle:
           execute_num: 2
           moving_speed_threshold: 0.28                 # 1.0km/h
           moving_time_threshold: 1.0
           max_expand_ratio: 0.0
           envelope_buffer_margin: 0.8
-          avoid_margin_lateral: 1.0
-          safety_buffer_lateral: 1.0
+          lateral_margin:
+            soft_margin: 0.0                           # [m]
+            hard_margin: 0.0                           # [m]
+            hard_margin_for_parked_vehicle: 0.0        # [m]
         motorcycle:
           execute_num: 2
           moving_speed_threshold: 1.0                  # 3.6km/h
           moving_time_threshold: 1.0
           max_expand_ratio: 0.0
           envelope_buffer_margin: 0.8
-          avoid_margin_lateral: 1.0
-          safety_buffer_lateral: 1.0
+          lateral_margin:
+            soft_margin: 0.0                           # [m]
+            hard_margin: 0.0                           # [m]
+            hard_margin_for_parked_vehicle: 0.0        # [m]
         pedestrian:
           execute_num: 2
           moving_speed_threshold: 0.28                 # 1.0km/h
           moving_time_threshold: 1.0
           max_expand_ratio: 0.0
           envelope_buffer_margin: 0.8
-          avoid_margin_lateral: 1.0
-          safety_buffer_lateral: 1.0
+          lateral_margin:
+            soft_margin: 0.0                           # [m]
+            hard_margin: 0.0                           # [m]
+            hard_margin_for_parked_vehicle: 0.0        # [m]
         lower_distance_for_polygon_expansion: 0.0      # [m]
         upper_distance_for_polygon_expansion: 1.0      # [m]
 

planning/behavior_path_avoidance_by_lane_change_module/package.xml

@@ -22,6 +22,7 @@
   <depend>behavior_path_lane_change_module</depend>
   <depend>behavior_path_planner</depend>
   <depend>behavior_path_planner_common</depend>
+  <depend>lanelet2_extension</depend>
   <depend>motion_utils</depend>
   <depend>pluginlib</depend>
   <depend>rclcpp</depend>

planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp

@@ -72,10 +72,12 @@ void AvoidanceByLaneChangeModuleManager::init(rclcpp::Node * node)
       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");
+      param.lateral_soft_margin =
+        getOrDeclareParameter<double>(*node, ns + "lateral_margin.soft_margin");
+      param.lateral_hard_margin =
+        getOrDeclareParameter<double>(*node, ns + "lateral_margin.hard_margin");
+      param.lateral_hard_margin_for_parked_vehicle =
+        getOrDeclareParameter<double>(*node, ns + "lateral_margin.hard_margin_for_parked_vehicle");
       return param;
     };
 
@@ -130,11 +132,14 @@ void AvoidanceByLaneChangeModuleManager::init(rclcpp::Node * node)
   }
 
   {
-    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");
+    const std::string ns = "avoidance.target_filtering.avoidance_for_ambiguous_vehicle.";
+    p.enable_avoidance_for_ambiguous_vehicle = getOrDeclareParameter<bool>(*node, ns + "enable");
+    p.closest_distance_to_wait_and_see_for_ambiguous_vehicle =
+      getOrDeclareParameter<double>(*node, ns + "closest_distance_to_wait_and_see");
+    p.time_threshold_for_ambiguous_vehicle =
+      getOrDeclareParameter<double>(*node, ns + "condition.time_threshold");
+    p.distance_threshold_for_ambiguous_vehicle =
+      getOrDeclareParameter<double>(*node, ns + "condition.distance_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 =

planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp

@@ -20,6 +20,7 @@
 #include "behavior_path_planner_common/utils/path_utils.hpp"
 #include "behavior_path_planner_common/utils/utils.hpp"
 
+#include <lanelet2_extension/utility/utilities.hpp>
 #include <rclcpp/logging.hpp>
 
 #include <boost/geometry/algorithms/centroid.hpp>
@@ -29,16 +30,6 @@
 
 namespace behavior_path_planner
 {
-namespace
-{
-lanelet::BasicLineString3d toLineString3d(const std::vector<Point> & bound)
-{
-  lanelet::BasicLineString3d ret{};
-  std::for_each(
-    bound.begin(), bound.end(), [&](const auto & p) { ret.emplace_back(p.x, p.y, p.z); });
-  return ret;
-}
-}  // namespace
 AvoidanceByLaneChange::AvoidanceByLaneChange(
   const std::shared_ptr<LaneChangeParameters> & parameters,
   std::shared_ptr<AvoidanceByLCParameters> avoidance_parameters)
@@ -95,9 +86,11 @@ bool AvoidanceByLaneChange::specialRequiredCheck() const
   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;
+  const auto lateral_hard_margin = std::max(
+    nearest_object_parameter.lateral_hard_margin,
+    nearest_object_parameter.lateral_hard_margin_for_parked_vehicle);
+  const auto avoid_margin = lateral_hard_margin * nearest_object.distance_factor +
+                            nearest_object_parameter.lateral_soft_margin + 0.5 * ego_width;
 
   avoidance_helper_->setData(planner_data_);
   const auto shift_length = avoidance_helper_->getShiftLength(
@@ -161,19 +154,17 @@ AvoidancePlanningData AvoidanceByLaneChange::calcAvoidancePlanningData(
   // expand drivable lanes
   std::for_each(
     data.current_lanelets.begin(), data.current_lanelets.end(), [&](const auto & lanelet) {
-      data.drivable_lanes.push_back(utils::avoidance::generateExpandDrivableLanes(
+      data.drivable_lanes.push_back(utils::avoidance::generateExpandedDrivableLanes(
         lanelet, planner_data_, avoidance_parameters_));
     });
 
   // calc drivable bound
   const auto shorten_lanes =
     utils::cutOverlappedLanes(data.reference_path_rough, data.drivable_lanes);
-  data.left_bound = toLineString3d(utils::calcBound(
-    planner_data_->route_handler, shorten_lanes, avoidance_parameters_->use_hatched_road_markings,
-    avoidance_parameters_->use_intersection_areas, true));
-  data.right_bound = toLineString3d(utils::calcBound(
-    planner_data_->route_handler, shorten_lanes, avoidance_parameters_->use_hatched_road_markings,
-    avoidance_parameters_->use_intersection_areas, false));
+  data.left_bound = utils::calcBound(
+    data.reference_path_rough, planner_data_, shorten_lanes, false, false, false, true);
+  data.right_bound = utils::calcBound(
+    data.reference_path_rough, planner_data_, shorten_lanes, false, false, false, false);
 
   // get related objects from dynamic_objects, and then separates them as target objects and non
   // target objects
@@ -183,7 +174,7 @@ AvoidancePlanningData AvoidanceByLaneChange::calcAvoidancePlanningData(
 }
 
 void AvoidanceByLaneChange::fillAvoidanceTargetObjects(
-  AvoidancePlanningData & data, DebugData & debug) const
+  AvoidancePlanningData & data, [[maybe_unused]] DebugData & debug) const
 {
   const auto p = std::dynamic_pointer_cast<AvoidanceParameters>(avoidance_parameters_);
 
@@ -223,7 +214,9 @@ void AvoidanceByLaneChange::fillAvoidanceTargetObjects(
       [&](const auto & object) { return createObjectData(data, object); });
   }
 
-  utils::avoidance::filterTargetObjects(target_lane_objects, data, debug, planner_data_, p);
+  utils::avoidance::filterTargetObjects(
+    target_lane_objects, data, avoidance_parameters_->object_check_max_forward_distance,
+    planner_data_, p);
 }
 
 ObjectData AvoidanceByLaneChange::createObjectData(
@@ -232,6 +225,7 @@ ObjectData AvoidanceByLaneChange::createObjectData(
   using boost::geometry::return_centroid;
   using motion_utils::findNearestIndex;
   using tier4_autoware_utils::calcDistance2d;
+  using tier4_autoware_utils::calcLateralDeviation;
 
   const auto p = std::dynamic_pointer_cast<AvoidanceParameters>(avoidance_parameters_);
 
@@ -263,12 +257,15 @@ ObjectData AvoidanceByLaneChange::createObjectData(
   utils::avoidance::fillObjectMovingTime(object_data, stopped_objects_, p);
 
   // Calc lateral deviation from path to target object.
-  object_data.lateral =
-    tier4_autoware_utils::calcLateralDeviation(object_closest_pose, object_pose.position);
+  object_data.to_centerline =
+    lanelet::utils::getArcCoordinates(data.current_lanelets, object_pose).distance;
+  object_data.direction = calcLateralDeviation(object_closest_pose, object_pose.position) > 0.0
+                            ? Direction::LEFT
+                            : Direction::RIGHT;
 
   // Find the footprint point closest to the path, set to object_data.overhang_distance.
-  object_data.overhang_dist = utils::avoidance::calcEnvelopeOverhangDistance(
-    object_data, data.reference_path, object_data.overhang_pose.position);
+  object_data.overhang_points =
+    utils::avoidance::calcEnvelopeOverhangDistance(object_data, data.reference_path);
 
   // Check whether the the ego should avoid the object.
   const auto & vehicle_width = planner_data_->parameters.vehicle_width;

planning/behavior_path_avoidance_by_lane_change_module/test/test_behavior_path_planner_node_interface.cpp

@@ -12,10 +12,11 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "ament_index_cpp/get_package_share_directory.hpp"
 #include "behavior_path_planner/behavior_path_planner_node.hpp"
-#include "planning_interface_test_manager/planning_interface_test_manager.hpp"
-#include "planning_interface_test_manager/planning_interface_test_manager_utils.hpp"
+
+#include <ament_index_cpp/get_package_share_directory.hpp>
+#include <planning_test_utils/planning_interface_test_manager.hpp>
+#include <planning_test_utils/planning_interface_test_manager_utils.hpp>
 
 #include <gtest/gtest.h>