diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp index 856681d538db0..303d7449e40dd 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp @@ -151,8 +151,8 @@ void AvoidanceByLaneChange::fillAvoidanceTargetObjects( const auto [object_within_target_lane, object_outside_target_lane] = utils::path_safety_checker::separateObjectsByLanelets( *planner_data_->dynamic_object, data.current_lanelets, - [](const auto & obj, const auto & lane) { - return utils::path_safety_checker::isPolygonOverlapLanelet(obj, lane); + [](const auto & obj, const auto & lane, const auto yaw_threshold) { + return utils::path_safety_checker::isPolygonOverlapLanelet(obj, lane, yaw_threshold); }); // Assume that the maximum allocation for data.other object is the sum of diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp index d6f03f79186d8..b74cdbc6a569b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp @@ -145,8 +145,8 @@ PredictedObjects extractObjectsInExpandedPullOverLanes( route_handler, left_side, backward_distance, forward_distance, bound_offset); const auto [objects_in_lanes, others] = utils::path_safety_checker::separateObjectsByLanelets( - objects, lanes, [](const auto & obj, const auto & lanelet) { - return utils::path_safety_checker::isPolygonOverlapLanelet(obj, lanelet); + objects, lanes, [](const auto & obj, const auto & lanelet, const auto yaw_threshold) { + return utils::path_safety_checker::isPolygonOverlapLanelet(obj, lanelet, yaw_threshold); }); return objects_in_lanes; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp index 4fd6fce325f49..f73f989174b54 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp @@ -56,18 +56,24 @@ using tier4_planning_msgs::msg::PathPointWithLaneId; * * @param objects The predicted objects to filter. * @param lanelet + * @param yaw_threshold * @return result. */ -bool isCentroidWithinLanelet(const PredictedObject & object, const lanelet::ConstLanelet & lanelet); +bool isCentroidWithinLanelet( + const PredictedObject & object, const lanelet::ConstLanelet & lanelet, + const double yaw_threshold); /** * @brief Filters objects based on object polygon overlapping with lanelet. * * @param objects The predicted objects to filter. * @param lanelet + * @param yaw_threshold * @return result. */ -bool isPolygonOverlapLanelet(const PredictedObject & object, const lanelet::ConstLanelet & lanelet); +bool isPolygonOverlapLanelet( + const PredictedObject & object, const lanelet::ConstLanelet & lanelet, + const double yaw_threshold); bool isPolygonOverlapLanelet( const PredictedObject & object, const autoware::universe_utils::Polygon2d & lanelet_polygon); @@ -168,7 +174,9 @@ void filterObjectsByClass( */ std::pair, std::vector> separateObjectIndicesByLanelets( const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets, - const std::function & condition); + const std::function & + condition, + const double yaw_threshold = M_PI); /** * @brief Separate the objects into two part based on whether the object is within lanelet. @@ -176,7 +184,9 @@ std::pair, std::vector> separateObjectIndicesByLanel */ std::pair separateObjectsByLanelets( const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets, - const std::function & condition); + const std::function & + condition, + const double yaw_threshold = M_PI); /** * @brief Get the predicted path from an object. diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp index 469be03eb905c..95a498b7b8475 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp @@ -57,15 +57,34 @@ bool is_within_circle( namespace autoware::behavior_path_planner::utils::path_safety_checker { -bool isCentroidWithinLanelet(const PredictedObject & object, const lanelet::ConstLanelet & lanelet) +bool isCentroidWithinLanelet( + const PredictedObject & object, const lanelet::ConstLanelet & lanelet, const double yaw_threshold) { - const auto & object_pos = object.kinematics.initial_pose_with_covariance.pose.position; - lanelet::BasicPoint2d object_centroid(object_pos.x, object_pos.y); - return boost::geometry::within(object_centroid, lanelet.polygon2d().basicPolygon()); + const auto & object_pose = object.kinematics.initial_pose_with_covariance.pose; + const auto closest_pose = lanelet::utils::getClosestCenterPose(lanelet, object_pose.position); + if ( + std::abs(autoware::universe_utils::calcYawDeviation(closest_pose, object_pose)) > + yaw_threshold) { + return false; + } + + return boost::geometry::within( + lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object_pose.position)) + .basicPoint(), + lanelet.polygon2d().basicPolygon()); } -bool isPolygonOverlapLanelet(const PredictedObject & object, const lanelet::ConstLanelet & lanelet) +bool isPolygonOverlapLanelet( + const PredictedObject & object, const lanelet::ConstLanelet & lanelet, const double yaw_threshold) { + const auto & object_pose = object.kinematics.initial_pose_with_covariance.pose; + const auto closest_pose = lanelet::utils::getClosestCenterPose(lanelet, object_pose.position); + if ( + std::abs(autoware::universe_utils::calcYawDeviation(closest_pose, object_pose)) > + yaw_threshold) { + return false; + } + const auto lanelet_polygon = utils::toPolygon2d(lanelet); return isPolygonOverlapLanelet(object, lanelet_polygon); } @@ -174,7 +193,9 @@ void filterObjectsByClass( std::pair, std::vector> separateObjectIndicesByLanelets( const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets, - const std::function & condition) + const std::function & + condition, + const double yaw_threshold) { if (target_lanelets.empty()) { return {}; @@ -184,7 +205,9 @@ std::pair, std::vector> separateObjectIndicesByLanel std::vector other_indices; for (size_t i = 0; i < objects.objects.size(); i++) { - const auto filter = [&](const auto & llt) { return condition(objects.objects.at(i), llt); }; + const auto filter = [&](const auto & llt) { + return condition(objects.objects.at(i), llt, yaw_threshold); + }; const auto found = std::find_if(target_lanelets.begin(), target_lanelets.end(), filter); if (found != target_lanelets.end()) { target_indices.push_back(i); @@ -198,13 +221,15 @@ std::pair, std::vector> separateObjectIndicesByLanel std::pair separateObjectsByLanelets( const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets, - const std::function & condition) + const std::function & + condition, + const double yaw_threshold) { PredictedObjects target_objects; PredictedObjects other_objects; const auto [target_indices, other_indices] = - separateObjectIndicesByLanelets(objects, target_lanelets, condition); + separateObjectIndicesByLanelets(objects, target_lanelets, condition, yaw_threshold); target_objects.objects.reserve(target_indices.size()); other_objects.objects.reserve(other_indices.size()); diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp index 7b30745057743..03b6e13cb2d3e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp @@ -74,8 +74,9 @@ class PullOutPlannerBase *dynamic_objects, parameters_.th_moving_object_velocity); auto [pull_out_lane_stop_objects, others] = utils::path_safety_checker::separateObjectsByLanelets( - stop_objects, pull_out_lanes, [](const auto & obj, const auto & lane) { - return utils::path_safety_checker::isPolygonOverlapLanelet(obj, lane); + stop_objects, pull_out_lanes, + [](const auto & obj, const auto & lane, const auto yaw_threshold) { + return utils::path_safety_checker::isPolygonOverlapLanelet(obj, lane, yaw_threshold); }); utils::path_safety_checker::filterObjectsByClass( pull_out_lane_stop_objects, parameters_.object_types_to_check_for_path_generation); diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp index 00b6649851f87..9b94d3505c405 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp @@ -1217,8 +1217,9 @@ PredictedObjects StartPlannerModule::filterStopObjectsInPullOutLanes( // filter for objects located in pull out lanes and moving at a speed below the threshold auto [stop_objects_in_pull_out_lanes, others] = utils::path_safety_checker::separateObjectsByLanelets( - stop_objects, pull_out_lanes, [](const auto & obj, const auto & lane) { - return utils::path_safety_checker::isPolygonOverlapLanelet(obj, lane); + stop_objects, pull_out_lanes, + [](const auto & obj, const auto & lane, const auto yaw_threshold) { + return utils::path_safety_checker::isPolygonOverlapLanelet(obj, lane, yaw_threshold); }); const auto path = planner_data_->route_handler->getCenterLinePath( diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp index 03fe48c6ab441..4272d0eaf9c5d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp @@ -2064,21 +2064,35 @@ std::vector getSafetyCheckTargetObjects( return {}; } + const auto is_moving = [¶meters](const auto & object) { + const auto & object_twist = object.kinematics.initial_twist_with_covariance.twist; + const auto object_vel_norm = std::hypot(object_twist.linear.x, object_twist.linear.y); + const auto object_parameter = + parameters->object_parameters.at(utils::getHighestProbLabel(object.classification)); + return object_vel_norm > object_parameter.moving_speed_threshold; + }; + + const auto filter = + [&is_moving](const auto & object, const auto & lanelet, [[maybe_unused]] const auto unused) { + // filter by yaw deviation only when the object is moving because the head direction is not + // reliable while object is stopping. + const auto yaw_threshold = is_moving(object) ? M_PI_2 : M_PI; + return utils::path_safety_checker::isCentroidWithinLanelet(object, lanelet, yaw_threshold); + }; + // check right lanes if (check_right_lanes) { const auto check_lanes = getAdjacentLane(closest_lanelet, planner_data, p, true); if (p->check_other_object) { const auto [targets, others] = utils::path_safety_checker::separateObjectsByLanelets( - to_predicted_objects(data.other_objects), check_lanes, - utils::path_safety_checker::isCentroidWithinLanelet); + to_predicted_objects(data.other_objects), check_lanes, filter); append(targets); } if (p->check_unavoidable_object) { const auto [targets, others] = utils::path_safety_checker::separateObjectsByLanelets( - to_predicted_objects(unavoidable_objects), check_lanes, - utils::path_safety_checker::isCentroidWithinLanelet); + to_predicted_objects(unavoidable_objects), check_lanes, filter); append(targets); } @@ -2092,15 +2106,13 @@ std::vector getSafetyCheckTargetObjects( if (p->check_other_object) { const auto [targets, others] = utils::path_safety_checker::separateObjectsByLanelets( - to_predicted_objects(data.other_objects), check_lanes, - utils::path_safety_checker::isCentroidWithinLanelet); + to_predicted_objects(data.other_objects), check_lanes, filter); append(targets); } if (p->check_unavoidable_object) { const auto [targets, others] = utils::path_safety_checker::separateObjectsByLanelets( - to_predicted_objects(unavoidable_objects), check_lanes, - utils::path_safety_checker::isCentroidWithinLanelet); + to_predicted_objects(unavoidable_objects), check_lanes, filter); append(targets); } @@ -2114,15 +2126,13 @@ std::vector getSafetyCheckTargetObjects( if (p->check_other_object) { const auto [targets, others] = utils::path_safety_checker::separateObjectsByLanelets( - to_predicted_objects(data.other_objects), check_lanes, - utils::path_safety_checker::isCentroidWithinLanelet); + to_predicted_objects(data.other_objects), check_lanes, filter); append(targets); } if (p->check_unavoidable_object) { const auto [targets, others] = utils::path_safety_checker::separateObjectsByLanelets( - to_predicted_objects(unavoidable_objects), check_lanes, - utils::path_safety_checker::isCentroidWithinLanelet); + to_predicted_objects(unavoidable_objects), check_lanes, filter); append(targets); }