From fbb9a2003722ef56b2bc69e5a5845c5808b81442 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Fri, 23 Jun 2023 12:04:35 +0900 Subject: [PATCH] feat(avoidance): improve avoidance judgement logic for pedestrian & bicycle (#4016) * feat(avoidance): don't avoid pedestrian and bicycle on crosswalk Signed-off-by: satoshi-ota * feat(avoidance): avoid pedestrian/bicycle near centerline Signed-off-by: satoshi-ota * feat(utils): use geometry distance Signed-off-by: satoshi-ota * chore(avoidance): add comment Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../utils/avoidance/utils.hpp | 6 ++ .../avoidance/avoidance_module.cpp | 1 + .../src/utils/avoidance/utils.cpp | 67 +++++++++++++++++++ 3 files changed, 74 insertions(+) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp index 515ed8cae5a1d..9343893444277 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp @@ -52,6 +52,12 @@ struct PolygonPoint bool isOnRight(const ObjectData & obj); +bool isVehicleTypeObject(const ObjectData & object); + +bool isWithinCrosswalk( + const ObjectData & object, + const std::shared_ptr & overall_graphs); + bool isTargetObjectType( const PredictedObject & object, const std::shared_ptr & parameters); diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index f4ec17403db41..a295766e4337e 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -3133,6 +3133,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"))); diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp index e85add452dc29..f42496c2a6910 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp +++ b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp @@ -387,6 +387,54 @@ 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 & overall_graphs) +{ + using Point = boost::geometry::model::d2::point_xy; + + const auto & p = object.object.kinematics.initial_pose_with_covariance.pose.position; + const Point p_object{p.x, p.y}; + + // get conflicting crosswalk crosswalk + constexpr int PEDESTRIAN_GRAPH_ID = 1; + const auto conflicts = + overall_graphs->conflictingInGraph(object.overhang_lanelet, PEDESTRIAN_GRAPH_ID); + + constexpr double THRESHOLD = 2.0; + for (const auto & crosswalk : conflicts) { + auto polygon = crosswalk.polygon2d().basicPolygon(); + + boost::geometry::correct(polygon); + + // ignore objects around the crosswalk + if (boost::geometry::distance(p_object, polygon) < THRESHOLD) { + return true; + } + } + + return false; +} + double calcShiftLength( const bool & is_object_on_right, const double & overhang_dist, const double & avoid_margin) { @@ -1078,6 +1126,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; } @@ -1215,6 +1264,24 @@ void filterTargetObjects( } } + // for non vehicle type object + if (!isVehicleTypeObject(o)) { + if (isWithinCrosswalk(o, rh->getOverallGraphPtr())) { + // avoidance module ignore pedestrian and bicycle around crosswalk + o.reason = "CrosswalkUser"; + data.other_objects.push_back(o); + } else { + // if there is no crosswalk near the object, avoidance module avoids pedestrian and bicycle + // no matter how it is shifted. + o.last_seen = now; + o.avoid_margin = avoid_margin; + data.target_objects.push_back(o); + } + continue; + } + + // from here condition check for vehicle type objects. + // force avoidance for stopped vehicle { const auto to_traffic_light =