feat(avoidance): create detection area from latest generated path

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

planning/behavior_path_planner/config/avoidance/avoidance.param.yaml

@@ -4,8 +4,6 @@
     avoidance:
       resample_interval_for_planning: 0.3               # [m]
       resample_interval_for_output: 4.0                 # [m]
-      detection_area_right_expand_dist: 0.0             # [m]
-      detection_area_left_expand_dist: 1.0              # [m]
       drivable_area_right_bound_offset: 0.0             # [m]
       drivable_area_left_bound_offset: 0.0              # [m]
 

planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp

@@ -122,7 +122,7 @@ MarkerArray createFurthestLineStringMarkerArray(const lanelet::ConstLineStrings3
 
 MarkerArray createPolygonMarkerArray(
   const Polygon & polygon, std::string && ns, const int64_t & lane_id, const float & r,
-  const float & g, const float & b);
+  const float & g, const float & b, const float & w = 0.3);
 
 MarkerArray createObjectsMarkerArray(
   const PredictedObjects & objects, std::string && ns, const int64_t & lane_id, const float & r,

planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp

@@ -79,12 +79,6 @@ struct AvoidanceParameters
   // computational cost for latter modules.
   double resample_interval_for_output = 3.0;
 
-  // lanelet expand length for right side to find avoidance target vehicles
-  double detection_area_right_expand_dist = 0.0;
-
-  // lanelet expand length for left side to find avoidance target vehicles
-  double detection_area_left_expand_dist = 1.0;
-
   // enable avoidance to be perform only in lane with same direction
   bool use_adjacent_lane{true};
 
@@ -524,9 +518,10 @@ using MarginDataArray = std::vector<MarginData>;
  */
 struct DebugData
 {
-  std::shared_ptr<lanelet::ConstLanelets> expanded_lanelets;
   std::shared_ptr<lanelet::ConstLanelets> current_lanelets;
 
+  geometry_msgs::msg::Polygon detection_area;
+
   lanelet::ConstLineStrings3d bounds;
 
   AvoidLineArray current_shift_lines;  // in path shifter

planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp

@@ -22,6 +22,7 @@
 #include <algorithm>
 #include <memory>
 #include <string>
+#include <utility>
 #include <vector>
 
 namespace behavior_path_planner::utils::avoidance
@@ -160,6 +161,11 @@ ExtendedPredictedObject transform(
 std::vector<ExtendedPredictedObject> getSafetyCheckTargetObjects(
   const AvoidancePlanningData & data, const std::shared_ptr<const PlannerData> & planner_data,
   const std::shared_ptr<AvoidanceParameters> & parameters, const bool is_right_shift);
+
+std::pair<PredictedObjects, PredictedObjects> separateObjectsByPath(
+  const PathWithLaneId & path, const std::shared_ptr<const PlannerData> & planner_data,
+  const AvoidancePlanningData & data, const std::shared_ptr<AvoidanceParameters> & parameters,
+  DebugData & debug);
 }  // namespace behavior_path_planner::utils::avoidance
 
 #endif  // BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__UTILS_HPP_

planning/behavior_path_planner/src/marker_utils/utils.cpp

@@ -314,11 +314,11 @@ MarkerArray createFurthestLineStringMarkerArray(const lanelet::ConstLineStrings3
 
 MarkerArray createPolygonMarkerArray(
   const Polygon & polygon, std::string && ns, const int64_t & lane_id, const float & r,
-  const float & g, const float & b)
+  const float & g, const float & b, const float & w)
 {
   Marker marker = createDefaultMarker(
     "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, static_cast<int32_t>(lane_id), Marker::LINE_STRIP,
-    createMarkerScale(0.3, 0.0, 0.0), createMarkerColor(r, g, b, 0.8));
+    createMarkerScale(w, 0.0, 0.0), createMarkerColor(r, g, b, 0.8));
 
   marker.pose.orientation = tier4_autoware_utils::createMarkerOrientation(0, 0, 0, 1.0);
 

planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp

@@ -279,12 +279,9 @@ void AvoidanceModule::fillAvoidanceTargetObjects(
   using utils::avoidance::getTargetLanelets;
 
   // Separate dynamic objects based on whether they are inside or outside of the expanded lanelets.
-  const auto expanded_lanelets = getTargetLanelets(
-    planner_data_, data.current_lanelets, parameters_->detection_area_left_expand_dist,
-    parameters_->detection_area_right_expand_dist * (-1.0));
-
   const auto [object_within_target_lane, object_outside_target_lane] =
-    utils::separateObjectsByLanelets(*planner_data_->dynamic_object, expanded_lanelets);
+    utils::avoidance::separateObjectsByPath(
+      helper_.getPreviousSplineShiftPath().path, planner_data_, data, parameters_, debug);
 
   for (const auto & object : object_outside_target_lane.objects) {
     ObjectData other_object;
@@ -311,7 +308,6 @@ void AvoidanceModule::fillAvoidanceTargetObjects(
   // debug
   {
     debug.current_lanelets = std::make_shared<lanelet::ConstLanelets>(data.current_lanelets);
-    debug.expanded_lanelets = std::make_shared<lanelet::ConstLanelets>(expanded_lanelets);
 
     std::vector<AvoidanceDebugMsg> debug_info_array;
     const auto append = [&](const auto & o) {
@@ -2681,6 +2677,7 @@ void AvoidanceModule::updateDebugMarker(
   using marker_utils::createLaneletsAreaMarkerArray;
   using marker_utils::createObjectsMarkerArray;
   using marker_utils::createPathMarkerArray;
+  using marker_utils::createPolygonMarkerArray;
   using marker_utils::createPoseMarkerArray;
   using marker_utils::createShiftGradMarkerArray;
   using marker_utils::createShiftLengthMarkerArray;
@@ -2729,7 +2726,7 @@ void AvoidanceModule::updateDebugMarker(
   add(createSafetyCheckMarkerArray(data.state, getEgoPose(), debug));
 
   add(createLaneletsAreaMarkerArray(*debug.current_lanelets, "current_lanelet", 0.0, 1.0, 0.0));
-  add(createLaneletsAreaMarkerArray(*debug.expanded_lanelets, "expanded_lanelet", 0.8, 0.8, 0.0));
+  add(createPolygonMarkerArray(debug.detection_area, "detection_area", 0L, 0.16, 1.0, 0.69, 0.1));
 
   add(createOtherObjectsMarkerArray(
     data.other_objects, AvoidanceDebugFactor::OBJECT_IS_BEHIND_THRESHOLD));

planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp

@@ -50,10 +50,6 @@ AvoidanceModuleManager::AvoidanceModuleManager(
       get_parameter<double>(node, ns + "resample_interval_for_planning");
     p.resample_interval_for_output =
       get_parameter<double>(node, ns + "resample_interval_for_output");
-    p.detection_area_right_expand_dist =
-      get_parameter<double>(node, ns + "detection_area_right_expand_dist");
-    p.detection_area_left_expand_dist =
-      get_parameter<double>(node, ns + "detection_area_left_expand_dist");
     p.enable_bound_clipping = get_parameter<bool>(node, ns + "enable_bound_clipping");
     p.enable_update_path_when_object_is_gone =
       get_parameter<bool>(node, ns + "enable_update_path_when_object_is_gone");

planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp

@@ -193,10 +193,6 @@ AvoidanceByLaneChangeModuleManager::AvoidanceByLaneChangeModuleManager(
       get_parameter<double>(node, ns + "resample_interval_for_planning");
     p.resample_interval_for_output =
       get_parameter<double>(node, ns + "resample_interval_for_output");
-    p.detection_area_right_expand_dist =
-      get_parameter<double>(node, ns + "detection_area_right_expand_dist");
-    p.detection_area_left_expand_dist =
-      get_parameter<double>(node, ns + "detection_area_left_expand_dist");
     p.enable_update_path_when_object_is_gone =
       get_parameter<bool>(node, ns + "enable_update_path_when_object_is_gone");
     p.enable_force_avoidance_for_stopped_vehicle =

planning/behavior_path_planner/src/utils/avoidance/utils.cpp

@@ -142,6 +142,59 @@ double calcSignedArcLengthToFirstNearestPoint(
 
   return signed_length_on_traj - signed_length_src_offset + signed_length_dst_offset;
 }
+
+geometry_msgs::msg::Polygon createVehiclePolygon(
+  const vehicle_info_util::VehicleInfo & vehicle_info, const double offset)
+{
+  const auto & i = vehicle_info;
+  const auto & front_m = i.max_longitudinal_offset_m;
+  const auto & width_m = i.vehicle_width_m / 2.0 + offset;
+  const auto & back_m = i.rear_overhang_m;
+
+  geometry_msgs::msg::Polygon polygon{};
+
+  polygon.points.push_back(createPoint32(front_m, -width_m, 0.0));
+  polygon.points.push_back(createPoint32(front_m, width_m, 0.0));
+  polygon.points.push_back(createPoint32(-back_m, width_m, 0.0));
+  polygon.points.push_back(createPoint32(-back_m, -width_m, 0.0));
+
+  return polygon;
+}
+
+Polygon2d createOneStepPolygon(
+  const geometry_msgs::msg::Pose & p_front, const geometry_msgs::msg::Pose & p_back,
+  const geometry_msgs::msg::Polygon & base_polygon)
+{
+  Polygon2d one_step_polygon{};
+
+  {
+    geometry_msgs::msg::Polygon out_polygon{};
+    geometry_msgs::msg::TransformStamped geometry_tf{};
+    geometry_tf.transform = pose2transform(p_front);
+    tf2::doTransform(base_polygon, out_polygon, geometry_tf);
+
+    for (const auto & p : out_polygon.points) {
+      one_step_polygon.outer().push_back(Point2d(p.x, p.y));
+    }
+  }
+
+  {
+    geometry_msgs::msg::Polygon out_polygon{};
+    geometry_msgs::msg::TransformStamped geometry_tf{};
+    geometry_tf.transform = pose2transform(p_back);
+    tf2::doTransform(base_polygon, out_polygon, geometry_tf);
+
+    for (const auto & p : out_polygon.points) {
+      one_step_polygon.outer().push_back(Point2d(p.x, p.y));
+    }
+  }
+
+  Polygon2d hull_polygon{};
+  boost::geometry::convex_hull(one_step_polygon, hull_polygon);
+  boost::geometry::correct(hull_polygon);
+
+  return hull_polygon;
+}
 }  // namespace
 
 bool isOnRight(const ObjectData & obj)
@@ -1544,4 +1597,58 @@ std::vector<ExtendedPredictedObject> getSafetyCheckTargetObjects(
 
   return target_objects;
 }
+
+std::pair<PredictedObjects, PredictedObjects> separateObjectsByPath(
+  const PathWithLaneId & path, const std::shared_ptr<const PlannerData> & planner_data,
+  const AvoidancePlanningData & data, const std::shared_ptr<AvoidanceParameters> & parameters,
+  DebugData & debug)
+{
+  PredictedObjects target_objects;
+  PredictedObjects other_objects;
+
+  double max_offset = 0.0;
+  for (const auto & object_parameter : parameters->object_parameters) {
+    const auto p = object_parameter.second;
+    const auto offset = p.envelope_buffer_margin + p.safety_buffer_lateral + p.avoid_margin_lateral;
+    max_offset = std::max(max_offset, offset);
+  }
+
+  const auto detection_area =
+    createVehiclePolygon(planner_data->parameters.vehicle_info, max_offset);
+  const auto ego_idx = planner_data->findEgoIndex(path.points);
+
+  Polygon2d attention_area;
+  for (size_t i = 0; i < path.points.size() - 1; ++i) {
+    const auto & p_ego_front = path.points.at(i).point.pose;
+    const auto & p_ego_back = path.points.at(i + 1).point.pose;
+
+    const auto distance_from_ego = calcSignedArcLength(path.points, ego_idx, i);
+    if (distance_from_ego > parameters->object_check_forward_distance) {
+      break;
+    }
+
+    const auto ego_one_step_polygon = createOneStepPolygon(p_ego_front, p_ego_back, detection_area);
+
+    std::vector<Polygon2d> unions;
+    boost::geometry::union_(attention_area, ego_one_step_polygon, unions);
+    if (!unions.empty()) {
+      attention_area = unions.front();
+      boost::geometry::correct(attention_area);
+    }
+  }
+
+  debug.detection_area = toMsg(attention_area, data.reference_pose.position.z);
+
+  const auto objects = planner_data->dynamic_object->objects;
+  std::for_each(objects.begin(), objects.end(), [&](const auto & object) {
+    const auto obj_polygon = tier4_autoware_utils::toPolygon2d(object);
+    if (boost::geometry::disjoint(obj_polygon, attention_area)) {
+      other_objects.objects.push_back(object);
+    } else {
+      target_objects.objects.push_back(object);
+    }
+  });
+
+  return std::make_pair(target_objects, other_objects);
+}
 }  // namespace behavior_path_planner::utils::avoidance