From 614aeeeac0ed8470cf612af06c4577b66246bc9d Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Wed, 18 Oct 2023 18:24:28 +0900 Subject: [PATCH] feat(avoidance): suppress unnecessary avoidance path Signed-off-by: satoshi-ota --- .../config/avoidance/avoidance.param.yaml | 4 + .../utils/avoidance/avoidance_module_data.hpp | 1 + .../utils/avoidance/utils.hpp | 4 + .../avoidance/avoidance_module.cpp | 1 + .../src/scene_module/avoidance/manager.cpp | 2 + .../src/utils/avoidance/utils.cpp | 297 ++++++++++-------- 6 files changed, 173 insertions(+), 136 deletions(-) diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml index b0f736916ebda..42f31fcd77c6b 100644 --- a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml +++ b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml @@ -146,6 +146,10 @@ front_distance: 30.0 # [m] behind_distance: 30.0 # [m] + # params for filtering objects that are in intersection + intersection: + yaw_deviation: 0.175 # [rad] (default: 10.0deg) + # For safety check safety_check: # safety check configuration diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp index 7976399a7ed40..04fe7ae27b50d 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp @@ -151,6 +151,7 @@ struct AvoidanceParameters double object_check_min_forward_distance{0.0}; double object_check_max_forward_distance{0.0}; double object_check_backward_distance{0.0}; + double object_check_yaw_deviation{0.0}; // if the distance between object and goal position is less than this parameter, the module ignore // the object. 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 b8b4efb7aed57..16bee504abe52 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 @@ -45,6 +45,10 @@ bool isWithinIntersection( bool isTargetObjectType( const PredictedObject & object, const std::shared_ptr & parameters); +bool isObjectOnRoadShoulder( + ObjectData & object, std::shared_ptr & route_handler, + std::shared_ptr & parameters); + double calcShiftLength( const bool & is_object_on_right, const double & overhang_dist, const double & avoid_margin); 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 5423cff11bfeb..5226ffc0b27de 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 @@ -2736,6 +2736,7 @@ void AvoidanceModule::updateDebugMarker( addObjects(data.other_objects, std::string("NotNeedAvoidance")); addObjects(data.other_objects, std::string("LessThanExecutionThreshold")); addObjects(data.other_objects, std::string("TooNearToGoal")); + addObjects(data.other_objects, std::string("ParallelToEgoLane")); } // shift line pre-process diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp index 33c6982f99fd9..c009ff8c12bcb 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp @@ -114,6 +114,8 @@ AvoidanceModuleManager::AvoidanceModuleManager( getOrDeclareParameter(*node, ns + "object_check_shiftable_ratio"); p.object_check_min_road_shoulder_width = getOrDeclareParameter(*node, ns + "object_check_min_road_shoulder_width"); + p.object_check_yaw_deviation = + getOrDeclareParameter(*node, ns + "intersection.yaw_deviation"); p.object_last_seen_threshold = getOrDeclareParameter(*node, ns + "object_last_seen_threshold"); } diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp index 54a1c0c649916..444c193b2bfe1 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp +++ b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp @@ -24,6 +24,7 @@ #include #include #include +#include #include #include @@ -308,6 +309,130 @@ bool isWithinIntersection( object_polygon, utils::toPolygon2d(lanelet::utils::to2D(polygon.basicPolygon()))); } +bool isParallelToEgoLane(const ObjectData & object, const double threshold) +{ + const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose; + const auto closest_pose = + lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object_pose.position); + const auto yaw_deviation = std::abs(calcYawDeviation(closest_pose, object_pose)); + + return yaw_deviation < threshold || yaw_deviation > M_PI - threshold; +} + +bool isObjectOnRoadShoulder( + ObjectData & object, const std::shared_ptr & route_handler, + const std::shared_ptr & parameters) +{ + using boost::geometry::return_centroid; + using boost::geometry::within; + using lanelet::geometry::distance2d; + using lanelet::geometry::toArcCoordinates; + using lanelet::utils::to2D; + + // assume that there are no parked vehicles in intersection. + std::string turn_direction = object.overhang_lanelet.attributeOr("turn_direction", "else"); + if (turn_direction == "right" || turn_direction == "left" || turn_direction == "straight") { + return false; + } + + // ============================================ <- most_left_lanelet.leftBound() + // y road shoulder + // ^ ------------------------------------------ + // | x + + // +---> --- object closest lanelet --- o ----- <- object_closest_lanelet.centerline() + // + // -------------------------------------------- + // +: object position + // o: nearest point on centerline + + lanelet::BasicPoint2d object_centroid(object.centroid.x(), object.centroid.y()); + + const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose; + const auto centerline_pose = + lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object_pose.position); + lanelet::BasicPoint3d centerline_point( + centerline_pose.position.x, centerline_pose.position.y, centerline_pose.position.z); + + bool is_left_side_parked_vehicle = false; + if (!isOnRight(object)) { + auto [object_shiftable_distance, sub_type] = [&]() { + const auto most_left_road_lanelet = + route_handler->getMostLeftLanelet(object.overhang_lanelet); + const auto most_left_lanelet_candidates = + route_handler->getLaneletMapPtr()->laneletLayer.findUsages( + most_left_road_lanelet.leftBound()); + + lanelet::ConstLanelet most_left_lanelet = most_left_road_lanelet; + const lanelet::Attribute sub_type = + most_left_lanelet.attribute(lanelet::AttributeName::Subtype); + + for (const auto & ll : most_left_lanelet_candidates) { + const lanelet::Attribute sub_type = ll.attribute(lanelet::AttributeName::Subtype); + if (sub_type.value() == "road_shoulder") { + most_left_lanelet = ll; + } + } + + const auto center_to_left_boundary = + distance2d(to2D(most_left_lanelet.leftBound().basicLineString()), to2D(centerline_point)); + + return std::make_pair( + center_to_left_boundary - 0.5 * object.object.shape.dimensions.y, sub_type); + }(); + + if (sub_type.value() != "road_shoulder") { + object_shiftable_distance += parameters->object_check_min_road_shoulder_width; + } + + const auto arc_coordinates = toArcCoordinates( + to2D(object.overhang_lanelet.centerline().basicLineString()), object_centroid); + object.shiftable_ratio = arc_coordinates.distance / object_shiftable_distance; + + is_left_side_parked_vehicle = object.shiftable_ratio > parameters->object_check_shiftable_ratio; + } + + bool is_right_side_parked_vehicle = false; + if (isOnRight(object)) { + auto [object_shiftable_distance, sub_type] = [&]() { + const auto most_right_road_lanelet = + route_handler->getMostRightLanelet(object.overhang_lanelet); + const auto most_right_lanelet_candidates = + route_handler->getLaneletMapPtr()->laneletLayer.findUsages( + most_right_road_lanelet.rightBound()); + + lanelet::ConstLanelet most_right_lanelet = most_right_road_lanelet; + const lanelet::Attribute sub_type = + most_right_lanelet.attribute(lanelet::AttributeName::Subtype); + + for (const auto & ll : most_right_lanelet_candidates) { + const lanelet::Attribute sub_type = ll.attribute(lanelet::AttributeName::Subtype); + if (sub_type.value() == "road_shoulder") { + most_right_lanelet = ll; + } + } + + const auto center_to_right_boundary = + distance2d(to2D(most_right_lanelet.rightBound().basicLineString()), to2D(centerline_point)); + + return std::make_pair( + center_to_right_boundary - 0.5 * object.object.shape.dimensions.y, sub_type); + }(); + + if (sub_type.value() != "road_shoulder") { + object_shiftable_distance += parameters->object_check_min_road_shoulder_width; + } + + const auto arc_coordinates = toArcCoordinates( + to2D(object.overhang_lanelet.centerline().basicLineString()), object_centroid); + object.shiftable_ratio = -1.0 * arc_coordinates.distance / object_shiftable_distance; + + is_right_side_parked_vehicle = + object.shiftable_ratio > parameters->object_check_shiftable_ratio; + } + + return is_left_side_parked_vehicle || is_right_side_parked_vehicle; +} + bool isForceAvoidanceTarget( ObjectData & object, const lanelet::ConstLanelets & extend_lanelets, const std::shared_ptr & planner_data, @@ -1033,6 +1158,17 @@ void filterTargetObjects( } } + const auto push_target_object = [&data, &now](auto & object, const auto & margin) { + object.last_seen = now; + object.avoid_margin = margin; + data.target_objects.push_back(object); + }; + + const auto push_ignore_object = [&data](auto & object, const auto & reason) { + object.reason = reason; + data.other_objects.push_back(object); + }; + for (auto & o : objects) { const auto & object_pose = o.object.kinematics.initial_pose_with_covariance.pose; const auto object_closest_index = findNearestIndex(path_points, object_pose.position); @@ -1042,8 +1178,7 @@ void filterTargetObjects( const auto object_parameter = parameters->object_parameters.at(object_type); if (!isTargetObjectType(o.object, parameters)) { - o.reason = AvoidanceDebugFactor::OBJECT_IS_NOT_TYPE; - data.other_objects.push_back(o); + push_ignore_object(o, AvoidanceDebugFactor::OBJECT_IS_NOT_TYPE); continue; } @@ -1051,8 +1186,7 @@ void filterTargetObjects( // 1. speed is higher than threshold. // 2. keep that speed longer than the time threshold. if (o.move_time > object_parameter.moving_time_threshold) { - o.reason = AvoidanceDebugFactor::MOVING_OBJECT; - data.other_objects.push_back(o); + push_ignore_object(o, AvoidanceDebugFactor::MOVING_OBJECT); continue; } @@ -1061,26 +1195,22 @@ void filterTargetObjects( // object is behind ego or too far. if (o.longitudinal < -parameters->object_check_backward_distance) { - o.reason = AvoidanceDebugFactor::OBJECT_IS_BEHIND_THRESHOLD; - data.other_objects.push_back(o); + push_ignore_object(o, AvoidanceDebugFactor::OBJECT_IS_BEHIND_THRESHOLD); continue; } if (o.longitudinal > parameters->object_check_max_forward_distance) { - o.reason = AvoidanceDebugFactor::OBJECT_IS_IN_FRONT_THRESHOLD; - data.other_objects.push_back(o); + push_ignore_object(o, AvoidanceDebugFactor::OBJECT_IS_IN_FRONT_THRESHOLD); continue; } // Target object is behind the path goal -> ignore. if (o.longitudinal > dist_to_goal) { - o.reason = AvoidanceDebugFactor::OBJECT_BEHIND_PATH_GOAL; - data.other_objects.push_back(o); + push_ignore_object(o, AvoidanceDebugFactor::OBJECT_BEHIND_PATH_GOAL); continue; } if (o.longitudinal + o.length / 2 + parameters->object_check_goal_distance > dist_to_goal) { - o.reason = "TooNearToGoal"; - data.other_objects.push_back(o); + push_ignore_object(o, "TooNearToGoal"); continue; } @@ -1174,14 +1304,12 @@ void filterTargetObjects( if (!!avoid_margin) { const auto shift_length = calcShiftLength(isOnRight(o), o.overhang_dist, avoid_margin.get()); if (!isShiftNecessary(isOnRight(o), shift_length)) { - o.reason = "NotNeedAvoidance"; - data.other_objects.push_back(o); + push_ignore_object(o, "NotNeedAvoidance"); continue; } if (std::abs(shift_length) < parameters->lateral_execution_threshold) { - o.reason = "LessThanExecutionThreshold"; - data.other_objects.push_back(o); + push_ignore_object(o, "LessThanExecutionThreshold"); continue; } } @@ -1190,14 +1318,11 @@ void filterTargetObjects( 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); + push_ignore_object(o, "CrosswalkUser"); } 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); + push_target_object(o, avoid_margin); } continue; } @@ -1206,21 +1331,16 @@ void filterTargetObjects( // from here condition check for vehicle type objects. if (isForceAvoidanceTarget(o, extend_lanelets, planner_data, parameters)) { - o.last_seen = now; - o.avoid_margin = avoid_margin; - data.target_objects.push_back(o); + push_target_object(o, avoid_margin); continue; } // Object is on center line -> ignore. if (std::abs(o.lateral) < parameters->threshold_distance_object_is_on_center) { - o.reason = AvoidanceDebugFactor::TOO_NEAR_TO_CENTERLINE; - data.other_objects.push_back(o); + push_ignore_object(o, AvoidanceDebugFactor::TOO_NEAR_TO_CENTERLINE); continue; } - lanelet::BasicPoint2d object_centroid(o.centroid.x(), o.centroid.y()); - /** * Is not object in adjacent lane? * - Yes -> Is parking object? @@ -1228,120 +1348,25 @@ void filterTargetObjects( * - No -> ignore this object. * - No -> the object is avoidance target no matter whether it is parking object or not. */ - const auto is_in_ego_lane = - within(object_centroid, overhang_lanelet.polygon2d().basicPolygon()); - if (is_in_ego_lane) { - /** - * TODO(Satoshi Ota) use intersection area - * under the assumption that there is no parking vehicle inside intersection, - * ignore all objects that is in the ego lane as not parking objects. - */ - std::string turn_direction = overhang_lanelet.attributeOr("turn_direction", "else"); - if (turn_direction == "right" || turn_direction == "left" || turn_direction == "straight") { - o.reason = AvoidanceDebugFactor::NOT_PARKING_OBJECT; - data.other_objects.push_back(o); + lanelet::BasicPoint2d object_centroid(o.centroid.x(), o.centroid.y()); + if (within(object_centroid, overhang_lanelet.polygon2d().basicPolygon())) { + if (isObjectOnRoadShoulder(o, rh, parameters)) { + push_target_object(o, avoid_margin); continue; } + } - const auto centerline_pose = - lanelet::utils::getClosestCenterPose(overhang_lanelet, object_pose.position); - lanelet::BasicPoint3d centerline_point( - centerline_pose.position.x, centerline_pose.position.y, centerline_pose.position.z); - - // ============================================ <- most_left_lanelet.leftBound() - // y road shoulder - // ^ ------------------------------------------ - // | x + - // +---> --- object closest lanelet --- o ----- <- object_closest_lanelet.centerline() - // - // -------------------------------------------- - // +: object position - // o: nearest point on centerline - - bool is_left_side_parked_vehicle = false; - if (!isOnRight(o)) { - auto [object_shiftable_distance, sub_type] = [&]() { - const auto most_left_road_lanelet = rh->getMostLeftLanelet(overhang_lanelet); - const auto most_left_lanelet_candidates = - rh->getLaneletMapPtr()->laneletLayer.findUsages(most_left_road_lanelet.leftBound()); - - lanelet::ConstLanelet most_left_lanelet = most_left_road_lanelet; - const lanelet::Attribute sub_type = - most_left_lanelet.attribute(lanelet::AttributeName::Subtype); - - for (const auto & ll : most_left_lanelet_candidates) { - const lanelet::Attribute sub_type = ll.attribute(lanelet::AttributeName::Subtype); - if (sub_type.value() == "road_shoulder") { - most_left_lanelet = ll; - } - } - - const auto center_to_left_boundary = distance2d( - to2D(most_left_lanelet.leftBound().basicLineString()), to2D(centerline_point)); - - return std::make_pair( - center_to_left_boundary - 0.5 * o.object.shape.dimensions.y, sub_type); - }(); - - if (sub_type.value() != "road_shoulder") { - object_shiftable_distance += parameters->object_check_min_road_shoulder_width; - } - - const auto arc_coordinates = - toArcCoordinates(to2D(overhang_lanelet.centerline().basicLineString()), object_centroid); - o.shiftable_ratio = arc_coordinates.distance / object_shiftable_distance; - - is_left_side_parked_vehicle = o.shiftable_ratio > parameters->object_check_shiftable_ratio; - } - - bool is_right_side_parked_vehicle = false; - if (isOnRight(o)) { - auto [object_shiftable_distance, sub_type] = [&]() { - const auto most_right_road_lanelet = rh->getMostRightLanelet(overhang_lanelet); - const auto most_right_lanelet_candidates = - rh->getLaneletMapPtr()->laneletLayer.findUsages(most_right_road_lanelet.rightBound()); - - lanelet::ConstLanelet most_right_lanelet = most_right_road_lanelet; - const lanelet::Attribute sub_type = - most_right_lanelet.attribute(lanelet::AttributeName::Subtype); - - for (const auto & ll : most_right_lanelet_candidates) { - const lanelet::Attribute sub_type = ll.attribute(lanelet::AttributeName::Subtype); - if (sub_type.value() == "road_shoulder") { - most_right_lanelet = ll; - } - } - - const auto center_to_right_boundary = distance2d( - to2D(most_right_lanelet.rightBound().basicLineString()), to2D(centerline_point)); - - return std::make_pair( - center_to_right_boundary - 0.5 * o.object.shape.dimensions.y, sub_type); - }(); - - if (sub_type.value() != "road_shoulder") { - object_shiftable_distance += parameters->object_check_min_road_shoulder_width; - } - - const auto arc_coordinates = - toArcCoordinates(to2D(overhang_lanelet.centerline().basicLineString()), object_centroid); - o.shiftable_ratio = -1.0 * arc_coordinates.distance / object_shiftable_distance; - - is_right_side_parked_vehicle = o.shiftable_ratio > parameters->object_check_shiftable_ratio; - } - - if (!is_left_side_parked_vehicle && !is_right_side_parked_vehicle) { - o.reason = AvoidanceDebugFactor::NOT_PARKING_OBJECT; - data.other_objects.push_back(o); - continue; - } + if (!o.is_within_intersection) { + push_target_object(o, avoid_margin); + continue; } - o.last_seen = now; - o.avoid_margin = avoid_margin; + if (isParallelToEgoLane(o, parameters->object_check_yaw_deviation)) { + push_ignore_object(o, "ParallelToEgoLane"); + continue; + } - // set data - data.target_objects.push_back(o); + push_target_object(o, avoid_margin); } }