From 9a5e5c49e11303320e0a6a9b709e65855acbb9cd Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Fri, 24 Nov 2023 19:12:02 +0900 Subject: [PATCH] feat(avoidance): suppress unnecessary avoidance path (#5387) * docs(avoidance): update flowchart Signed-off-by: satoshi-ota * feat(avoidance): suppress unnecessary avoidance path Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../config/avoidance/avoidance.param.yaml | 4 + .../behavior_path_planner_avoidance_design.md | 143 ++++ .../utils/avoidance/avoidance_module_data.hpp | 5 +- .../utils/avoidance/utils.hpp | 21 +- .../avoidance/avoidance_module.cpp | 8 +- .../src/scene_module/avoidance/manager.cpp | 2 + .../src/utils/avoidance/utils.cpp | 736 ++++++++++-------- 7 files changed, 578 insertions(+), 341 deletions(-) diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml index ad06a1a9f6a10..d4394d1710e69 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/docs/behavior_path_planner_avoidance_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md index da96567ed542a..c4580bb1e82c3 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md +++ b/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md @@ -209,6 +209,149 @@ The closer the object is to the shoulder, the larger the value of $ratio$ (theor In order to prevent chattering of recognition results, once an obstacle is targeted, it is hold for a while even if it disappears. This is effective when recognition is unstable. However, since it will result in over-detection (increase a number of false-positive), it is necessary to adjust parameters according to the recognition accuracy (if `object_last_seen_threshold = 0.0`, the recognition result is 100% trusted). +### Flowchart + +```plantuml +@startuml +skinparam defaultTextAlignment center +skinparam noteTextAlignment left + +title object filtering flowchart +start + +if(object is satisfied with common target condition ?) then (yes) +if(vehicle can pass by the object without avoidance ?) then (yes) +:return false; +stop +else (\n no) +endif +else (\n no) +:return false; +stop +endif + +if(object is vehicle type ?) then (yes) +if(object is satisfied with vehicle type target condition ?) then (yes) +else (\n no) +:return false; +stop +endif +else (\n no) +if(object is satisfied with non-vehicle type target condition ?) then (yes) +else (\n no) +:return false; +stop +endif +endif + +#FF006C :return true; +stop +@enduml +``` + +```plantuml +@startuml +skinparam defaultTextAlignment center +skinparam noteTextAlignment left + +title filtering flow for all types object +start + +partition isSatisfiedWithCommonCondition() { +if(object is avoidance target type ?) then (yes) +if(object is moving more than threshold time ?) then (yes) +:return false; +stop +else (\n no) +if(object is farther than forward distance threshold ?) then (yes) +:return false; +stop +else (\n no) +If(object is farther than backward distance threshold ?) then (yes) +:return false; +stop +else (\n no) +endif +endif +endif +else (\n no) +:return false; +stop +endif +#FF006C :return true; +stop +} + +@enduml +``` + +```plantuml +@startuml +skinparam defaultTextAlignment center +skinparam noteTextAlignment left + +title filtering flow for vehicle type objects +start + +partition isSatisfiedWithVehicleCodition() { +if(object is force avoidance target ?) then (yes) +#FF006C :return true; +stop +else (\n no) +if(object is nearer lane centerline than threshold ?) then (yes) +:return false; +stop +else (\n no) +if(object is on same lane for ego ?) then (yes) +if(object is shifting right or left side road shoulder more than threshold ?) then (yes) +#FF006C :return true; +stop +else (\n no) +:return false; +stop +endif +else (\n no) +if(object is in intersection ?) then (no) +#FF006C :return true; +stop +else (\n yes) +if(object pose is paralell to ego lane ?) then (no) +#FF006C :return true; +stop +else (\n yes) +:return false; +stop +endif +endif +endif +endif +endif +} + +@enduml +``` + +```plantuml +@startuml +skinparam defaultTextAlignment center +skinparam noteTextAlignment left + +title filtering flow for non-vehicle type objects +start + +partition isSatisfiedWithNonVehicleCodition() { +if(object is nearer crosswalk than threshold ?) then (yes) +:return false; +stop +else (\n no) +endif +#FF006C :return true; +stop +} + +@enduml +``` + ## Overview of algorithm for avoidance path generation ### How to prevent shift line chattering that is caused by perception noise 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 60cf9b1244236..c90321ea649a6 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 @@ -159,6 +159,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. @@ -394,8 +395,7 @@ struct ObjectData // avoidance target std::string reason{""}; // lateral avoid margin - // NOTE: If margin is less than the minimum margin threshold, boost::none will be set. - boost::optional avoid_margin{boost::none}; + std::optional avoid_margin{std::nullopt}; }; using ObjectDataArray = std::vector; @@ -482,6 +482,7 @@ struct AvoidancePlanningData // current driving lanelet lanelet::ConstLanelets current_lanelets; + lanelet::ConstLanelets extend_lanelets; // output path ShiftedPath candidate_path; 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 2e04935e37336..7ed455ea5350e 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 @@ -33,18 +33,6 @@ using behavior_path_planner::utils::path_safety_checker::PredictedPathWithPolygo bool isOnRight(const ObjectData & obj); -bool isVehicleTypeObject(const ObjectData & object); - -bool isWithinCrosswalk( - const ObjectData & object, - const std::shared_ptr & overall_graphs); - -bool isWithinIntersection( - const ObjectData & object, const std::shared_ptr & route_handler); - -bool isTargetObjectType( - const PredictedObject & object, const std::shared_ptr & parameters); - double calcShiftLength( const bool & is_object_on_right, const double & overhang_dist, const double & avoid_margin); @@ -106,6 +94,10 @@ lanelet::ConstLanelets getTargetLanelets( lanelet::ConstLanelets getCurrentLanesFromPath( const PathWithLaneId & path, const std::shared_ptr & planner_data); +lanelet::ConstLanelets getExtendLanes( + const lanelet::ConstLanelets & lanelets, const Pose & ego_pose, + const std::shared_ptr & planner_data); + void insertDecelPoint( const Point & p_src, const double offset, const double velocity, PathWithLaneId & path, boost::optional & p_out); @@ -139,6 +131,11 @@ void filterTargetObjects( const std::shared_ptr & planner_data, const std::shared_ptr & parameters); +double getRoadShoulderDistance( + ObjectData & object, const AvoidancePlanningData & data, + const std::shared_ptr & planner_data, + const std::shared_ptr & parameters); + double extendToRoadShoulderDistanceWithPolygon( const std::shared_ptr & rh, const lanelet::ConstLineString3d & target_line, const double to_road_shoulder_distance, 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 b3cff8bb26d8a..3124f00932375 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 @@ -265,6 +265,9 @@ void AvoidanceModule::fillFundamentalData(AvoidancePlanningData & data, DebugDat data.current_lanelets = utils::avoidance::getCurrentLanesFromPath( *getPreviousModuleOutput().reference_path, planner_data_); + data.extend_lanelets = + utils::avoidance::getExtendLanes(data.current_lanelets, getEgoPose(), planner_data_); + // expand drivable lanes std::for_each( data.current_lanelets.begin(), data.current_lanelets.end(), [&](const auto & lanelet) { @@ -1058,7 +1061,7 @@ AvoidOutlines AvoidanceModule::generateAvoidOutline( AvoidOutlines outlines; for (auto & o : data.target_objects) { - if (!o.avoid_margin) { + if (!o.avoid_margin.has_value()) { o.reason = AvoidanceDebugFactor::INSUFFICIENT_LATERAL_MARGIN; if (o.avoid_required && is_forward_object(o)) { break; @@ -1069,7 +1072,7 @@ AvoidOutlines AvoidanceModule::generateAvoidOutline( const auto is_object_on_right = utils::avoidance::isOnRight(o); const auto desire_shift_length = - helper_.getShiftLength(o, is_object_on_right, o.avoid_margin.get()); + helper_.getShiftLength(o, is_object_on_right, o.avoid_margin.value()); if (utils::avoidance::isSameDirectionShift(is_object_on_right, desire_shift_length)) { o.reason = AvoidanceDebugFactor::SAME_DIRECTION_SHIFT; if (o.avoid_required && is_forward_object(o)) { @@ -2813,6 +2816,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 f71024dcc5034..f4807223503fa 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 ce4c9ad7ac72d..d6427a1fe8c36 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp +++ b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp @@ -25,6 +25,7 @@ #include #include #include +#include #include #include @@ -232,11 +233,8 @@ void pushUniqueVector(T & base_vector, const T & additional_vector) } // namespace -bool isOnRight(const ObjectData & obj) +namespace filtering_utils { - return obj.lateral < 0.0; -} - bool isTargetObjectType( const PredictedObject & object, const std::shared_ptr & parameters) { @@ -313,8 +311,132 @@ 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, + ObjectData & object, const AvoidancePlanningData & data, const std::shared_ptr & planner_data, const std::shared_ptr & parameters) { @@ -350,7 +472,7 @@ bool isForceAvoidanceTarget( bool not_parked_object = true; // check traffic light - const auto to_traffic_light = getDistanceToNextTrafficLight(object_pose, extend_lanelets); + const auto to_traffic_light = getDistanceToNextTrafficLight(object_pose, data.extend_lanelets); { not_parked_object = to_traffic_light < parameters->object_ignore_section_traffic_light_in_front_distance; @@ -358,7 +480,7 @@ bool isForceAvoidanceTarget( // check crosswalk const auto to_crosswalk = - utils::getDistanceToCrosswalk(ego_pose, extend_lanelets, *rh->getOverallGraphPtr()) - + utils::getDistanceToCrosswalk(ego_pose, data.extend_lanelets, *rh->getOverallGraphPtr()) - object.longitudinal; { const auto stop_for_crosswalk = @@ -372,6 +494,181 @@ bool isForceAvoidanceTarget( return !not_parked_object; } +bool isSatisfiedWithCommonCondition( + ObjectData & object, const AvoidancePlanningData & data, + const std::shared_ptr & planner_data, + const std::shared_ptr & parameters) +{ + // Step1. filtered by target object type. + if (!isTargetObjectType(object.object, parameters)) { + object.reason = AvoidanceDebugFactor::OBJECT_IS_NOT_TYPE; + return false; + } + + // Step2. filtered stopped objects. + const auto object_type = utils::getHighestProbLabel(object.object.classification); + const auto object_parameter = parameters->object_parameters.at(object_type); + if (object.move_time > object_parameter.moving_time_threshold) { + object.reason = AvoidanceDebugFactor::MOVING_OBJECT; + return false; + } + + // Step3. filtered by longitudinal distance. + const auto & ego_pos = planner_data->self_odometry->pose.pose.position; + fillLongitudinalAndLengthByClosestEnvelopeFootprint(data.reference_path_rough, ego_pos, object); + + if (object.longitudinal < -parameters->object_check_backward_distance) { + object.reason = AvoidanceDebugFactor::OBJECT_IS_BEHIND_THRESHOLD; + return false; + } + + if (object.longitudinal > parameters->object_check_max_forward_distance) { + object.reason = AvoidanceDebugFactor::OBJECT_IS_IN_FRONT_THRESHOLD; + return false; + } + + // Step4. filtered by distance between object and goal position. + // TODO(Satoshi OTA): remove following two conditions after it can execute avoidance and goal + // planner module simultaneously. + const auto & rh = planner_data->route_handler; + const auto ego_idx = planner_data->findEgoIndex(data.reference_path_rough.points); + const auto to_goal_distance = + rh->isInGoalRouteSection(data.current_lanelets.back()) + ? calcSignedArcLength( + data.reference_path_rough.points, ego_idx, data.reference_path_rough.points.size() - 1) + : std::numeric_limits::max(); + + if (object.longitudinal > to_goal_distance) { + object.reason = AvoidanceDebugFactor::OBJECT_BEHIND_PATH_GOAL; + return false; + } + + if ( + object.longitudinal + object.length / 2 + parameters->object_check_goal_distance > + to_goal_distance) { + object.reason = "TooNearToGoal"; + return false; + } + + return true; +} + +bool isSatisfiedWithNonVehicleCondition( + ObjectData & object, [[maybe_unused]] const AvoidancePlanningData & data, + const std::shared_ptr & planner_data, + [[maybe_unused]] const std::shared_ptr & parameters) +{ + // avoidance module ignore pedestrian and bicycle around crosswalk + if (isWithinCrosswalk(object, planner_data->route_handler->getOverallGraphPtr())) { + object.reason = "CrosswalkUser"; + return false; + } + + return true; +} + +bool isSatisfiedWithVehicleCondition( + ObjectData & object, const AvoidancePlanningData & data, + const std::shared_ptr & planner_data, + const std::shared_ptr & parameters) +{ + using boost::geometry::within; + + object.is_within_intersection = isWithinIntersection(object, planner_data->route_handler); + + // from here condition check for vehicle type objects. + if (isForceAvoidanceTarget(object, data, planner_data, parameters)) { + return true; + } + + // Object is on center line -> ignore. + if (std::abs(object.lateral) < parameters->threshold_distance_object_is_on_center) { + object.reason = AvoidanceDebugFactor::TOO_NEAR_TO_CENTERLINE; + return false; + } + + lanelet::BasicPoint2d object_centroid(object.centroid.x(), object.centroid.y()); + const auto on_ego_driving_lane = + within(object_centroid, object.overhang_lanelet.polygon2d().basicPolygon()); + if (on_ego_driving_lane) { + if (isObjectOnRoadShoulder(object, planner_data->route_handler, parameters)) { + return true; + } else { + object.reason = AvoidanceDebugFactor::NOT_PARKING_OBJECT; + return false; + } + } + + if (!object.is_within_intersection) { + return true; + } + + if (isParallelToEgoLane(object, parameters->object_check_yaw_deviation)) { + object.reason = "ParallelToEgoLane"; + return false; + } + + return true; +} + +bool isNoNeedAvoidanceBehavior( + ObjectData & object, const std::shared_ptr & parameters) +{ + if (!object.avoid_margin.has_value()) { + return false; + } + + const auto shift_length = + calcShiftLength(isOnRight(object), object.overhang_dist, object.avoid_margin.value()); + if (!isShiftNecessary(isOnRight(object), shift_length)) { + object.reason = "NotNeedAvoidance"; + return true; + } + + if (std::abs(shift_length) < parameters->lateral_execution_threshold) { + object.reason = "LessThanExecutionThreshold"; + return true; + } + + return false; +} + +std::optional getAvoidMargin( + const ObjectData & object, const std::shared_ptr & planner_data, + const std::shared_ptr & parameters) +{ + const auto & vehicle_width = planner_data->parameters.vehicle_width; + const auto object_type = utils::getHighestProbLabel(object.object.classification); + const auto object_parameter = parameters->object_parameters.at(object_type); + + const auto max_avoid_margin = object_parameter.safety_buffer_lateral * object.distance_factor + + object_parameter.avoid_margin_lateral + 0.5 * vehicle_width; + const auto min_avoid_margin = object_parameter.safety_buffer_lateral + 0.5 * vehicle_width; + const auto soft_lateral_distance_limit = + object.to_road_shoulder_distance - parameters->soft_road_shoulder_margin - 0.5 * vehicle_width; + const auto hard_lateral_distance_limit = + object.to_road_shoulder_distance - parameters->hard_road_shoulder_margin - 0.5 * vehicle_width; + + // Step1. check avoidable or not. + if (hard_lateral_distance_limit < min_avoid_margin) { + return std::nullopt; + } + + // Step2. check if it should expand road shoulder margin. + if (soft_lateral_distance_limit < min_avoid_margin) { + return min_avoid_margin; + } + + // Step3. nominal case. avoid margin is limited by soft constraint. + return std::min(soft_lateral_distance_limit, max_avoid_margin); +} +} // namespace filtering_utils + +bool isOnRight(const ObjectData & obj) +{ + return obj.lateral < 0.0; +} + double calcShiftLength( const bool & is_object_on_right, const double & overhang_dist, const double & avoid_margin) { @@ -627,7 +924,7 @@ std::vector generateObstaclePolygonsForDrivableArea( } // check if avoid marin is calculated - if (!object.avoid_margin) { + if (!object.avoid_margin.has_value()) { continue; } @@ -636,7 +933,7 @@ std::vector generateObstaclePolygonsForDrivableArea( // generate obstacle polygon const double diff_poly_buffer = - object.avoid_margin.get() - object_parameter.envelope_buffer_margin - vehicle_width / 2.0; + object.avoid_margin.value() - object_parameter.envelope_buffer_margin - vehicle_width / 2.0; const auto obj_poly = tier4_autoware_utils::expandPolygon(object.envelope_poly, diff_poly_buffer); const bool is_left = 0 < object.lateral; @@ -707,6 +1004,33 @@ lanelet::ConstLanelets getCurrentLanesFromPath( start_lane, p.backward_path_length, p.forward_path_length); } +lanelet::ConstLanelets getExtendLanes( + const lanelet::ConstLanelets & lanelets, const Pose & ego_pose, + const std::shared_ptr & planner_data) +{ + lanelet::ConstLanelets extend_lanelets = lanelets; + + while (rclcpp::ok()) { + const double lane_length = lanelet::utils::getLaneletLength2d(extend_lanelets); + const auto arc_coordinates = lanelet::utils::getArcCoordinates(extend_lanelets, ego_pose); + const auto forward_length = lane_length - arc_coordinates.length; + + if (forward_length > planner_data->parameters.forward_path_length) { + break; + } + + const auto next_lanelets = planner_data->route_handler->getNextLanelets(extend_lanelets.back()); + + if (next_lanelets.empty()) { + break; + } + + extend_lanelets.push_back(next_lanelets.front()); + } + + return extend_lanelets; +} + void insertDecelPoint( const Point & p_src, const double offset, const double velocity, PathWithLaneId & path, boost::optional & p_out) @@ -1007,361 +1331,123 @@ void compensateDetectionLost( } } -void filterTargetObjects( - ObjectDataArray & objects, AvoidancePlanningData & data, DebugData & debug, +double getRoadShoulderDistance( + ObjectData & object, const AvoidancePlanningData & data, const std::shared_ptr & planner_data, 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; - if (data.current_lanelets.empty()) { - return; - } - - const auto & rh = planner_data->route_handler; - const auto & path_points = data.reference_path_rough.points; - const auto & ego_pos = planner_data->self_odometry->pose.pose.position; - const auto & vehicle_width = planner_data->parameters.vehicle_width; - const rclcpp::Time now = rclcpp::Clock(RCL_ROS_TIME).now(); - - // for goal - const auto ego_idx = planner_data->findEgoIndex(path_points); - const auto dist_to_goal = rh->isInGoalRouteSection(data.current_lanelets.back()) - ? calcSignedArcLength(path_points, ego_idx, path_points.size() - 1) - : std::numeric_limits::max(); - - // extend lanelets if the reference path is cut for lane change. - const auto & ego_pose = planner_data->self_odometry->pose.pose; - lanelet::ConstLanelets extend_lanelets = data.current_lanelets; - while (rclcpp::ok()) { - const double lane_length = lanelet::utils::getLaneletLength2d(extend_lanelets); - const auto arclength = lanelet::utils::getArcCoordinates(extend_lanelets, ego_pose); - const auto next_lanelets = rh->getNextLanelets(extend_lanelets.back()); - - if (next_lanelets.empty()) { - break; - } + const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose; + const auto object_closest_index = + findNearestIndex(data.reference_path_rough.points, object_pose.position); + const auto object_closest_pose = + data.reference_path_rough.points.at(object_closest_index).point.pose; - if (lane_length - arclength.length < planner_data->parameters.forward_path_length) { - extend_lanelets.push_back(next_lanelets.front()); - } else { - break; - } + const auto rh = planner_data->route_handler; + if (!rh->getClosestLaneletWithinRoute(object_closest_pose, &object.overhang_lanelet)) { + return 0.0; } - 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); - const auto object_closest_pose = path_points.at(object_closest_index).point.pose; + double road_shoulder_distance = std::numeric_limits::max(); - const auto object_type = utils::getHighestProbLabel(o.object.classification); - const auto object_parameter = parameters->object_parameters.at(object_type); + const bool get_left = isOnRight(object) && parameters->use_adjacent_lane; + const bool get_right = !isOnRight(object) && parameters->use_adjacent_lane; + const bool get_opposite = parameters->use_opposite_lane; - if (!isTargetObjectType(o.object, parameters)) { - o.reason = AvoidanceDebugFactor::OBJECT_IS_NOT_TYPE; - data.other_objects.push_back(o); - continue; - } - - // if following condition are satisfied, ignored the objects as moving objects. - // 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); - continue; - } - - // calc longitudinal distance from ego to closest target object footprint point. - fillLongitudinalAndLengthByClosestEnvelopeFootprint(data.reference_path_rough, ego_pos, o); - - // 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); - continue; - } - if (o.longitudinal > parameters->object_check_max_forward_distance) { - o.reason = AvoidanceDebugFactor::OBJECT_IS_IN_FRONT_THRESHOLD; - data.other_objects.push_back(o); - 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); - continue; - } + lanelet::BasicPoint3d p_overhang( + object.overhang_pose.position.x, object.overhang_pose.position.y, + object.overhang_pose.position.z); - if (o.longitudinal + o.length / 2 + parameters->object_check_goal_distance > dist_to_goal) { - o.reason = "TooNearToGoal"; - data.other_objects.push_back(o); - continue; - } + lanelet::ConstLineString3d target_line{}; - lanelet::ConstLanelet overhang_lanelet; - if (!rh->getClosestLaneletWithinRoute(object_closest_pose, &overhang_lanelet)) { - continue; + const auto update_road_to_shoulder_distance = [&](const auto & target_lanelet) { + const auto lines = + rh->getFurthestLinestring(target_lanelet, get_right, get_left, get_opposite, true); + const auto & line = isOnRight(object) ? lines.back() : lines.front(); + const auto d = boost::geometry::distance(object.envelope_poly, to2D(line.basicLineString())); + if (d < road_shoulder_distance) { + road_shoulder_distance = d; + target_line = line; } + }; - if (overhang_lanelet.id()) { - o.overhang_lanelet = overhang_lanelet; - lanelet::BasicPoint3d overhang_basic_pose( - o.overhang_pose.position.x, o.overhang_pose.position.y, o.overhang_pose.position.z); - - const bool get_left = isOnRight(o) && parameters->use_adjacent_lane; - const bool get_right = !isOnRight(o) && parameters->use_adjacent_lane; - const bool get_opposite = parameters->use_opposite_lane; - - lanelet::ConstLineString3d target_line{}; - o.to_road_shoulder_distance = std::numeric_limits::max(); - const auto update_road_to_shoulder_distance = [&](const auto & target_lanelet) { - const auto lines = - rh->getFurthestLinestring(target_lanelet, get_right, get_left, get_opposite, true); - const auto & line = isOnRight(o) ? lines.back() : lines.front(); - const auto d = boost::geometry::distance(o.envelope_poly, to2D(line.basicLineString())); - if (d < o.to_road_shoulder_distance) { - o.to_road_shoulder_distance = d; - target_line = line; - } - }; - - // current lanelet - { - update_road_to_shoulder_distance(overhang_lanelet); - - o.to_road_shoulder_distance = extendToRoadShoulderDistanceWithPolygon( - rh, target_line, o.to_road_shoulder_distance, overhang_lanelet, o.overhang_pose.position, - overhang_basic_pose, parameters->use_hatched_road_markings, - parameters->use_intersection_areas); - } - - // previous lanelet - lanelet::ConstLanelets previous_lanelet{}; - if (rh->getPreviousLaneletsWithinRoute(overhang_lanelet, &previous_lanelet)) { - update_road_to_shoulder_distance(previous_lanelet.front()); - - o.to_road_shoulder_distance = extendToRoadShoulderDistanceWithPolygon( - rh, target_line, o.to_road_shoulder_distance, previous_lanelet.front(), - o.overhang_pose.position, overhang_basic_pose, parameters->use_hatched_road_markings, - parameters->use_intersection_areas); - } + // current lanelet + { + update_road_to_shoulder_distance(object.overhang_lanelet); - // next lanelet - lanelet::ConstLanelet next_lanelet{}; - if (rh->getNextLaneletWithinRoute(overhang_lanelet, &next_lanelet)) { - update_road_to_shoulder_distance(next_lanelet); + road_shoulder_distance = extendToRoadShoulderDistanceWithPolygon( + rh, target_line, road_shoulder_distance, object.overhang_lanelet, + object.overhang_pose.position, p_overhang, parameters->use_hatched_road_markings, + parameters->use_intersection_areas); + } - o.to_road_shoulder_distance = extendToRoadShoulderDistanceWithPolygon( - rh, target_line, o.to_road_shoulder_distance, next_lanelet, o.overhang_pose.position, - overhang_basic_pose, parameters->use_hatched_road_markings, - parameters->use_intersection_areas); - } + // previous lanelet + lanelet::ConstLanelets previous_lanelet{}; + if (rh->getPreviousLaneletsWithinRoute(object.overhang_lanelet, &previous_lanelet)) { + update_road_to_shoulder_distance(previous_lanelet.front()); - debug.bounds.push_back(target_line); - } + road_shoulder_distance = extendToRoadShoulderDistanceWithPolygon( + rh, target_line, road_shoulder_distance, previous_lanelet.front(), + object.overhang_pose.position, p_overhang, parameters->use_hatched_road_markings, + parameters->use_intersection_areas); + } - // calculate avoid_margin dynamically - // NOTE: This calculation must be after calculating to_road_shoulder_distance. - const auto max_avoid_margin = object_parameter.safety_buffer_lateral * o.distance_factor + - object_parameter.avoid_margin_lateral + 0.5 * vehicle_width; - const auto min_avoid_margin = object_parameter.safety_buffer_lateral + 0.5 * vehicle_width; - const auto soft_lateral_distance_limit = - o.to_road_shoulder_distance - parameters->soft_road_shoulder_margin - 0.5 * vehicle_width; - const auto hard_lateral_distance_limit = - o.to_road_shoulder_distance - parameters->hard_road_shoulder_margin - 0.5 * vehicle_width; - - const auto avoid_margin = [&]() -> boost::optional { - // Step1. check avoidable or not. - if (hard_lateral_distance_limit < min_avoid_margin) { - return boost::none; - } + // next lanelet + lanelet::ConstLanelet next_lanelet{}; + if (rh->getNextLaneletWithinRoute(object.overhang_lanelet, &next_lanelet)) { + update_road_to_shoulder_distance(next_lanelet); - // Step2. check if it should expand road shoulder margin. - if (soft_lateral_distance_limit < min_avoid_margin) { - return min_avoid_margin; - } + road_shoulder_distance = extendToRoadShoulderDistanceWithPolygon( + rh, target_line, road_shoulder_distance, next_lanelet, object.overhang_pose.position, + p_overhang, parameters->use_hatched_road_markings, parameters->use_intersection_areas); + } - // Step3. nominal case. avoid margin is limited by soft constraint. - return std::min(soft_lateral_distance_limit, max_avoid_margin); - }(); + return road_shoulder_distance; +} - 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); - continue; - } +void filterTargetObjects( + ObjectDataArray & objects, AvoidancePlanningData & data, [[maybe_unused]] DebugData & debug, + const std::shared_ptr & planner_data, + const std::shared_ptr & parameters) +{ + if (data.current_lanelets.empty()) { + return; + } - if (std::abs(shift_length) < parameters->lateral_execution_threshold) { - o.reason = "LessThanExecutionThreshold"; - data.other_objects.push_back(o); - continue; - } - } + const rclcpp::Time now = rclcpp::Clock(RCL_ROS_TIME).now(); + const auto push_target_object = [&data, &now](auto & object) { + object.last_seen = now; + data.target_objects.push_back(object); + }; - // 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); - } + for (auto & o : objects) { + if (!filtering_utils::isSatisfiedWithCommonCondition(o, data, planner_data, parameters)) { + data.other_objects.push_back(o); continue; } - o.is_within_intersection = isWithinIntersection(o, rh); + o.to_road_shoulder_distance = getRoadShoulderDistance(o, data, planner_data, parameters); + o.avoid_margin = filtering_utils::getAvoidMargin(o, planner_data, parameters); - // 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); - 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; + if (filtering_utils::isNoNeedAvoidanceBehavior(o, parameters)) { data.other_objects.push_back(o); continue; } - lanelet::BasicPoint2d object_centroid(o.centroid.x(), o.centroid.y()); - - /** - * Is not object in adjacent lane? - * - Yes -> Is parking object? - * - Yes -> the object is avoidance target. - * - 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; + if (filtering_utils::isVehicleTypeObject(o)) { + if (!filtering_utils::isSatisfiedWithVehicleCondition(o, data, planner_data, parameters)) { data.other_objects.push_back(o); 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; + } else { + if (!filtering_utils::isSatisfiedWithNonVehicleCondition(o, data, planner_data, parameters)) { data.other_objects.push_back(o); continue; } } - o.last_seen = now; - o.avoid_margin = avoid_margin; - - // set data - data.target_objects.push_back(o); + push_target_object(o); } }