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

…towarefoundation#4525)

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

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

* refactor(avoidance): remove unused marker utils

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

---------

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/avoidance/debug.hpp

@@ -49,9 +49,6 @@ using visualization_msgs::msg::MarkerArray;
 MarkerArray createEgoStatusMarkerArray(
   const AvoidancePlanningData & data, const Pose & p_ego, std::string && ns);
 
-MarkerArray createSafetyCheckMarkerArray(
-  const AvoidanceState & state, const Pose & pose, const DebugData & data);
-
 MarkerArray createAvoidLineMarkerArray(
   const AvoidLineArray & shift_points, std::string && ns, const float & r, const float & g,
   const float & b, const double & w);

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};
 
@@ -513,35 +507,15 @@ struct ShiftLineData
   std::vector<std::vector<double>> shift_line_history;
 };
 
-/*
- * Data struct for longitudinal margin
- */
-struct MarginData
-{
-  Pose pose{};
-
-  bool enough_lateral_margin{true};
-
-  double longitudinal_distance{std::numeric_limits<double>::max()};
-
-  double longitudinal_margin{std::numeric_limits<double>::lowest()};
-
-  double vehicle_width;
-
-  double base_link2front;
-
-  double base_link2rear;
-};
-using MarginDataArray = std::vector<MarginData>;
-
 /*
  * Debug information for marker array
  */
 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
@@ -576,14 +550,9 @@ struct DebugData
   // shift path
   std::vector<double> proposed_spline_shift;
 
-  bool exist_adjacent_objects{false};
-
   // future pose
   PathWithLaneId path_with_planned_velocity;
 
-  // margin
-  MarginDataArray margin_data_array;
-
   // avoidance require objects
   ObjectDataArray unavoidable_objects;
 

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/avoidance/debug.cpp

@@ -222,62 +222,6 @@ MarkerArray createEgoStatusMarkerArray(
   return msg;
 }
 
-MarkerArray createSafetyCheckMarkerArray(
-  const AvoidanceState & state, const Pose & pose, const DebugData & data)
-{
-  const auto current_time = rclcpp::Clock{RCL_ROS_TIME}.now();
-  MarkerArray msg;
-
-  if (data.exist_adjacent_objects) {
-    auto marker = createDefaultMarker(
-      "map", current_time, "safety_alert", 0L, Marker::CYLINDER, createMarkerScale(0.2, 0.2, 2.0),
-      createMarkerColor(1.0, 1.0, 0.0, 0.8));
-
-    marker.color = state == AvoidanceState::YIELD ? createMarkerColor(1.0, 0.0, 0.0, 0.8)
-                                                  : createMarkerColor(1.0, 1.0, 0.0, 0.8);
-
-    marker.pose = calcOffsetPose(pose, 0.0, 1.5, 1.0);
-    msg.markers.push_back(marker);
-
-    marker.pose = calcOffsetPose(pose, 0.0, -1.5, 1.0);
-    marker.id++;
-    msg.markers.push_back(marker);
-  }
-
-  if (state == AvoidanceState::YIELD) {
-    return msg;
-  }
-
-  {
-    auto marker = createDefaultMarker(
-      "map", current_time, "safety_longitudinal_margin", 0L, Marker::CUBE,
-      createMarkerScale(3.0, 1.5, 1.5), createMarkerColor(1.0, 1.0, 1.0, 0.1));
-
-    for (const auto & m : data.margin_data_array) {
-      if (m.enough_lateral_margin) {
-        continue;
-      }
-
-      constexpr double max_x = 10.0;
-
-      const auto offset = 0.5 * (m.base_link2front + m.base_link2rear) - m.base_link2rear;
-      const auto diff = m.longitudinal_distance - m.longitudinal_margin;
-      const auto scale_x = std::min(max_x, 2.0 * (m.base_link2front + m.base_link2rear + diff));
-
-      const auto ratio = std::clamp(diff / max_x, 0.0, 1.0);
-
-      marker.pose = calcOffsetPose(m.pose, offset, 0.0, 0.0);
-      marker.pose.position.z += 1.0;
-      marker.scale = createMarkerScale(scale_x, 2.0 * m.vehicle_width, 2.0);
-      marker.color = createMarkerColor(1.0 - ratio, ratio, 0.0, 0.1);
-      marker.id++;
-      msg.markers.push_back(marker);
-    }
-  }
-
-  return msg;
-}
-
 MarkerArray createAvoidLineMarkerArray(
   const AvoidLineArray & shift_lines, std::string && ns, const float & r, const float & g,
   const float & b, const double & w)

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

@@ -266,12 +266,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;
@@ -298,7 +295,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) {
@@ -2654,6 +2650,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;
@@ -2663,7 +2660,6 @@ void AvoidanceModule::updateDebugMarker(
   using marker_utils::avoidance_marker::createOtherObjectsMarkerArray;
   using marker_utils::avoidance_marker::createOverhangFurthestLineStringMarkerArray;
   using marker_utils::avoidance_marker::createPredictedVehiclePositions;
-  using marker_utils::avoidance_marker::createSafetyCheckMarkerArray;
   using marker_utils::avoidance_marker::createUnsafeObjectsMarkerArray;
   using marker_utils::avoidance_marker::makeOverhangToRoadShoulderMarkerArray;
   using tier4_autoware_utils::appendMarkerArray;
@@ -2699,10 +2695,8 @@ void AvoidanceModule::updateDebugMarker(
     helper_.getPreviousLinearShiftPath().path, "prev_linear_shift", 0, 0.5, 0.4, 0.6));
   add(createPoseMarkerArray(data.reference_pose, "reference_pose", 0, 0.9, 0.3, 0.3));
 
-  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_cancel_maneuver = get_parameter<bool>(node, ns + "enable_cancel_maneuver");
     p.enable_force_avoidance_for_stopped_vehicle =

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_force_avoidance_for_stopped_vehicle =
       get_parameter<bool>(node, ns + "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)
@@ -1557,4 +1610,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