From 2d476e8f534ef06a18852fadc4a99c5534ba2c82 Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Sat, 18 Nov 2023 17:38:44 +0900 Subject: [PATCH] fixup! feat(avoidance): suppress unnecessary avoidance path --- .../utils/avoidance/utils.hpp | 16 - .../src/utils/avoidance/utils.cpp | 339 ++++++++---------- 2 files changed, 149 insertions(+), 206 deletions(-) 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 3399e1cbfafa8..d703c89bf658a 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,22 +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); - -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/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp index 59e19dafde300..c55415cb3eb9c 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp +++ b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp @@ -233,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) { @@ -497,6 +494,148 @@ 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 isSatisfiedWithNonVehicleCodition( + 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 isSatisfiedWithVehicleCodition( + 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; + } + } + + 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; +} +} // 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) { @@ -1132,120 +1271,6 @@ void compensateDetectionLost( } } -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 isSatisfiedWithNonVehicleCodition( - 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 isSatisfiedWithVehicleCodition( - 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; - } - } - - if (!object.is_within_intersection) { - return true; - } - - if (isParallelToEgoLane(object, parameters->object_check_yaw_deviation)) { - object.reason = "ParallelToEgoLane"; - return false; - } - - return true; -} - double getRoadShoulderDistance( ObjectData & object, const AvoidancePlanningData & data, const std::shared_ptr & planner_data, @@ -1351,28 +1376,6 @@ std::optional getAvoidMargin( return std::min(soft_lateral_distance_limit, max_avoid_margin); } -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; -} - void filterTargetObjects( ObjectDataArray & objects, AvoidancePlanningData & data, [[maybe_unused]] DebugData & debug, const std::shared_ptr & planner_data, @@ -1381,21 +1384,15 @@ void filterTargetObjects( if (data.current_lanelets.empty()) { return; } - // const auto & vehicle_width = planner_data->parameters.vehicle_width; - const rclcpp::Time now = rclcpp::Clock(RCL_ROS_TIME).now(); + 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); }; - // const auto push_ignore_object = [&data](auto & object, const auto & reason) { - // object.reason = reason; - // data.other_objects.push_back(object); - // }; - for (auto & o : objects) { - if (!isSatisfiedWithCommonCondition(o, data, planner_data, parameters)) { + if (!filtering_utils::isSatisfiedWithCommonCondition(o, data, planner_data, parameters)) { data.other_objects.push_back(o); continue; } @@ -1403,62 +1400,24 @@ void filterTargetObjects( o.to_road_shoulder_distance = getRoadShoulderDistance(o, data, planner_data, parameters); o.avoid_margin = getAvoidMargin(o, planner_data, parameters); - // // 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; - // } - - // // 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); - // }(); - - // if (!!avoid_margin) { - // const auto shift_length = calcShiftLength(isOnRight(o), o.overhang_dist, - // avoid_margin.get()); if (!isShiftNecessary(isOnRight(o), shift_length)) { - // push_ignore_object(o, "NotNeedAvoidance"); - // continue; - // } - - // if (std::abs(shift_length) < parameters->lateral_execution_threshold) { - // push_ignore_object(o, "LessThanExecutionThreshold"); - // continue; - // } - // } - if (isNoNeedAvoidanceBehavior(o, parameters)) { + if (filtering_utils::isNoNeedAvoidanceBehavior(o, parameters)) { data.other_objects.push_back(o); continue; } - if (isVehicleTypeObject(o)) { - if (isSatisfiedWithVehicleCodition(o, data, planner_data, parameters)) { + if (filtering_utils::isVehicleTypeObject(o)) { + if (filtering_utils::isSatisfiedWithVehicleCodition(o, data, planner_data, parameters)) { data.other_objects.push_back(o); continue; } } else { - if (isSatisfiedWithNonVehicleCodition(o, data, planner_data, parameters)) { + if (filtering_utils::isSatisfiedWithNonVehicleCodition(o, data, planner_data, parameters)) { data.other_objects.push_back(o); continue; } } push_target_object(o); - // push_target_object(o, avoid_margin); } }