feat(avoidance): don't avoid pedestrian and bicycle on crosswalk

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

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

@@ -28,6 +28,12 @@ using behavior_path_planner::PlannerData;
 
 bool isOnRight(const ObjectData & obj);
 
+bool isVehicleTypeObject(const ObjectData & object);
+
+bool isWithinCrosswalk(
+  const ObjectData & object,
+  const std::shared_ptr<const lanelet::routing::RoutingGraphContainer> & overall_graphs);
+
 bool isTargetObjectType(
   const PredictedObject & object, const std::shared_ptr<AvoidanceParameters> & parameters);
 

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

@@ -3178,6 +3178,7 @@ void AvoidanceModule::updateDebugMarker(
   add(createOtherObjectsMarkerArray(data.other_objects, AvoidanceDebugFactor::OBJECT_IS_NOT_TYPE));
   add(createOtherObjectsMarkerArray(data.other_objects, AvoidanceDebugFactor::NOT_PARKING_OBJECT));
   add(createOtherObjectsMarkerArray(data.other_objects, std::string("MovingObject")));
+  add(createOtherObjectsMarkerArray(data.other_objects, std::string("CrosswalkUser")));
   add(createOtherObjectsMarkerArray(data.other_objects, std::string("OutOfTargetArea")));
   add(createOtherObjectsMarkerArray(data.other_objects, std::string("NotNeedAvoidance")));
   add(createOtherObjectsMarkerArray(data.other_objects, std::string("LessThanExecutionThreshold")));

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

@@ -110,6 +110,53 @@ bool isTargetObjectType(
   return parameters->object_parameters.at(t).enable;
 }
 
+bool isVehicleTypeObject(const ObjectData & object)
+{
+  const auto t = utils::getHighestProbLabel(object.object.classification);
+
+  if (t == ObjectClassification::UNKNOWN) {
+    return false;
+  }
+
+  if (t == ObjectClassification::PEDESTRIAN) {
+    return false;
+  }
+
+  if (t == ObjectClassification::BICYCLE) {
+    return false;
+  }
+
+  return true;
+}
+
+bool isWithinCrosswalk(
+  const ObjectData & object,
+  const std::shared_ptr<const lanelet::routing::RoutingGraphContainer> & overall_graphs)
+{
+  using Point = boost::geometry::model::d2::point_xy<double>;
+  using boost::geometry::correct;
+  using boost::geometry::within;
+
+  const auto & p = object.object.kinematics.initial_pose_with_covariance.pose.position;
+  const Point p_object{p.x, p.y};
+
+  constexpr int PEDESTRIAN_GRAPH_ID = 1;
+  const auto conflicts =
+    overall_graphs->conflictingInGraph(object.overhang_lanelet, PEDESTRIAN_GRAPH_ID);
+
+  for (const auto & crosswalk : conflicts) {
+    auto polygon = crosswalk.polygon2d().basicPolygon();
+
+    correct(polygon);
+
+    if (within(p_object, polygon)) {
+      return true;
+    }
+  }
+
+  return false;
+}
+
 double calcShiftLength(
   const bool & is_object_on_right, const double & overhang_dist, const double & avoid_margin)
 {
@@ -752,6 +799,7 @@ void filterTargetObjects(
     const auto object_parameter = parameters->object_parameters.at(t);
 
     if (!isTargetObjectType(o.object, parameters)) {
+      o.reason = AvoidanceDebugFactor::OBJECT_IS_NOT_TYPE;
       data.other_objects.push_back(o);
       continue;
     }
@@ -842,6 +890,12 @@ void filterTargetObjects(
       }
     }
 
+    if (!isVehicleTypeObject(o) && isWithinCrosswalk(o, rh->getOverallGraphPtr())) {
+      o.reason = "CrosswalkUser";
+      data.other_objects.push_back(o);
+      continue;
+    }
+
     // calculate avoid_margin dynamically
     // NOTE: This calculation must be after calculating to_road_shoulder_distance.
     const double max_avoid_margin = object_parameter.safety_buffer_lateral * o.distance_factor +