From 981d3a04990631d441ac11f2b08ee5e249c80526 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Tue, 13 Feb 2024 13:37:45 +0900 Subject: [PATCH] feat(goal_planne): check objects within the area between ego edge and boudary of pull_over_lanes (#6369) Signed-off-by: Mamoru Sobue --- .../config/goal_planner.param.yaml | 2 + .../goal_planner_module.hpp | 1 + .../goal_planner_parameters.hpp | 2 + .../util.hpp | 15 ++- .../src/goal_planner_module.cpp | 117 +++++++++++++++--- .../src/manager.cpp | 4 + .../src/util.cpp | 69 ++++++++++- 7 files changed, 193 insertions(+), 17 deletions(-) diff --git a/planning/behavior_path_goal_planner_module/config/goal_planner.param.yaml b/planning/behavior_path_goal_planner_module/config/goal_planner.param.yaml index 46f39813f74d3..f2c8142f8145f 100644 --- a/planning/behavior_path_goal_planner_module/config/goal_planner.param.yaml +++ b/planning/behavior_path_goal_planner_module/config/goal_planner.param.yaml @@ -39,6 +39,8 @@ object_recognition_collision_check_max_extra_stopping_margin: 1.0 th_moving_object_velocity: 1.0 detection_bound_offset: 15.0 + outer_road_detection_offset: 1.0 + inner_road_detection_offset: 0.0 # pull over pull_over: diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp index 20884b009667e..1cb0a3b98c8f2 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp @@ -221,6 +221,7 @@ struct GoalPlannerDebugData { FreespacePlannerDebugData freespace_planner{}; std::vector ego_polygons_expanded{}; + lanelet::ConstLanelet expanded_pull_over_lane_between_ego{}; }; struct LastApprovalData diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_parameters.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_parameters.hpp index 2310c0c4d422c..972bb774517b6 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_parameters.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_parameters.hpp @@ -75,6 +75,8 @@ struct GoalPlannerParameters double object_recognition_collision_check_max_extra_stopping_margin{0.0}; double th_moving_object_velocity{0.0}; double detection_bound_offset{0.0}; + double outer_road_detection_offset{0.0}; + double inner_road_detection_offset{0.0}; // pull over general params double pull_over_minimum_request_length{0.0}; diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/util.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/util.hpp index be56d9dc30196..b6f671cfc94f8 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/util.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/util.hpp @@ -45,9 +45,19 @@ using visualization_msgs::msg::MarkerArray; lanelet::ConstLanelets getPullOverLanes( const RouteHandler & route_handler, const bool left_side, const double backward_distance, const double forward_distance); + +/* + * @brief expand pull_over_lanes to the opposite side of drivable roads by bound_offset. + * bound_offset must be positive regardless of left_side is true/false + */ lanelet::ConstLanelets generateExpandedPullOverLanes( const RouteHandler & route_handler, const bool left_side, const double backward_distance, - const double forward_distance, double bound_offset); + const double forward_distance, const double bound_offset); + +lanelet::ConstLanelets generateBetweenEgoAndExpandedPullOverLanes( + const lanelet::ConstLanelets & pull_over_lanes, const bool left_side, + const geometry_msgs::msg::Pose ego_pose, const vehicle_info_util::VehicleInfo & vehicle_info, + const double outer_road_offset, const double inner_road_offset); PredictedObjects extractObjectsInExpandedPullOverLanes( const RouteHandler & route_handler, const bool left_side, const double backward_distance, const double forward_distance, double bound_offset, const PredictedObjects & objects); @@ -85,6 +95,9 @@ MarkerArray createTextsMarkerArray( const std::vector & poses, std::string && ns, const std_msgs::msg::ColorRGBA & color); MarkerArray createGoalCandidatesMarkerArray( const GoalCandidates & goal_candidates, const std_msgs::msg::ColorRGBA & color); +MarkerArray createLaneletPolygonMarkerArray( + const lanelet::CompoundPolygon3d & polygon, const std_msgs::msg::Header & header, + const std::string & ns, const std_msgs::msg::ColorRGBA & color); MarkerArray createNumObjectsToAvoidTextsMarkerArray( const GoalCandidates & goal_candidates, std::string && ns, const std_msgs::msg::ColorRGBA & color); diff --git a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp index b59a8f01fa4ab..9558579fd6ddc 100644 --- a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -1867,6 +1867,50 @@ void GoalPlannerModule::updateSafetyCheckTargetObjectsData( goal_planner_data_.ego_predicted_path = ego_predicted_path; } +static std::vector filterObjectsByWithinPolicy( + const std::shared_ptr & objects, + const lanelet::ConstLanelets & target_lanes, + const std::shared_ptr & + params) +{ + // implanted part of behavior_path_planner::utils::path_safety_checker::filterObjects() and + // createTargetObjectsOnLane() + + // Guard + if (objects->objects.empty()) { + return {}; + } + + const double ignore_object_velocity_threshold = params->ignore_object_velocity_threshold; + const auto & target_object_types = params->object_types_to_check; + + PredictedObjects filtered_objects = utils::path_safety_checker::filterObjectsByVelocity( + *objects, ignore_object_velocity_threshold, false); + + utils::path_safety_checker::filterObjectsByClass(filtered_objects, target_object_types); + + std::vector within_filtered_objects; + for (const auto & target_lane : target_lanes) { + const auto lane_poly = target_lane.polygon2d().basicPolygon(); + for (const auto & filtered_object : filtered_objects.objects) { + const auto object_bbox = tier4_autoware_utils::toPolygon2d(filtered_object); + if (boost::geometry::within(object_bbox, lane_poly)) { + within_filtered_objects.push_back(filtered_object); + } + } + } + + const double safety_check_time_horizon = params->safety_check_time_horizon; + const double safety_check_time_resolution = params->safety_check_time_resolution; + + std::vector refined_filtered_objects; + for (const auto & within_filtered_object : within_filtered_objects) { + refined_filtered_objects.push_back(utils::path_safety_checker::transform( + within_filtered_object, safety_check_time_horizon, safety_check_time_resolution)); + } + return refined_filtered_objects; +} + std::pair GoalPlannerModule::isSafePath() const { if (!thread_safe_data_.get_pull_over_path()) { @@ -1904,17 +1948,54 @@ std::pair GoalPlannerModule::isSafePath() const ego_predicted_path_params_, pull_over_path.points, current_pose, current_velocity, ego_seg_idx, is_object_front, limit_to_max_velocity); - // filtering objects with velocity, position and class - const auto filtered_objects = utils::path_safety_checker::filterObjects( - dynamic_object, route_handler, pull_over_lanes, current_pose.position, - objects_filtering_params_); + // ========================================================================================== + // if ego is before the entry of pull_over_lanes, the beginning of the safety check area + // should be from the entry of pull_over_lanes + // ========================================================================================== + const Pose ego_pose_for_expand = std::invoke([&]() { + // get first road lane in pull over lanes segment + const auto fist_road_lane = std::invoke([&]() { + const auto first_pull_over_lane = pull_over_lanes.front(); + if (!route_handler->isShoulderLanelet(first_pull_over_lane)) { + return first_pull_over_lane; + } + const auto road_lane_opt = left_side_parking_ + ? route_handler->getRightLanelet(first_pull_over_lane) + : route_handler->getLeftLanelet(first_pull_over_lane); + if (road_lane_opt) { + return road_lane_opt.value(); + } + return first_pull_over_lane; + }); + // generate first road lane pose + Pose first_road_pose{}; + const auto first_road_point = + lanelet::utils::conversion::toGeomMsgPt(fist_road_lane.centerline().front()); + const double lane_yaw = lanelet::utils::getLaneletAngle(fist_road_lane, first_road_point); + first_road_pose.position = first_road_point; + first_road_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(lane_yaw); + // if current ego pose is before pull over lanes segment, use first road lanelet center pose + if ( + calcSignedArcLength(pull_over_path.points, first_road_pose.position, current_pose.position) < + 0.0) { + return first_road_pose; + } + // if current ego pose is in pull over lanes segment, use current ego pose + return current_pose; + }); // filtering objects based on the current position's lane - const auto target_objects_on_lane = utils::path_safety_checker::createTargetObjectsOnLane( - pull_over_lanes, route_handler, filtered_objects, objects_filtering_params_); - - utils::parking_departure::updateSafetyCheckTargetObjectsData( - goal_planner_data_, filtered_objects, target_objects_on_lane, ego_predicted_path); + const auto expanded_pull_over_lanes_between_ego = + goal_planner_utils::generateBetweenEgoAndExpandedPullOverLanes( + pull_over_lanes, left_side_parking_, ego_pose_for_expand, + planner_data_->parameters.vehicle_info, parameters_->outer_road_detection_offset, + parameters_->inner_road_detection_offset); + const auto merged_expanded_pull_over_lanes = + lanelet::utils::combineLaneletsShape(expanded_pull_over_lanes_between_ego); + debug_data_.expanded_pull_over_lane_between_ego = merged_expanded_pull_over_lanes; + + const auto filtered_objects = filterObjectsByWithinPolicy( + dynamic_object, {merged_expanded_pull_over_lanes}, objects_filtering_params_); const double hysteresis_factor = prev_data_.safety_status.is_safe ? 1.0 : parameters_->hysteresis_factor_expand_rate; @@ -1922,13 +2003,12 @@ std::pair GoalPlannerModule::isSafePath() const const bool current_is_safe = std::invoke([&]() { if (parameters_->safety_check_params.method == "RSS") { return behavior_path_planner::utils::path_safety_checker::checkSafetyWithRSS( - pull_over_path, ego_predicted_path, target_objects_on_lane.on_current_lane, - goal_planner_data_.collision_check, planner_data_->parameters, - safety_check_params_->rss_params, objects_filtering_params_->use_all_predicted_path, - hysteresis_factor); + pull_over_path, ego_predicted_path, filtered_objects, goal_planner_data_.collision_check, + planner_data_->parameters, safety_check_params_->rss_params, + objects_filtering_params_->use_all_predicted_path, hysteresis_factor); } else if (parameters_->safety_check_params.method == "integral_predicted_polygon") { return utils::path_safety_checker::checkSafetyWithIntegralPredictedPolygon( - ego_predicted_path, vehicle_info_, target_objects_on_lane.on_current_lane, + ego_predicted_path, vehicle_info_, filtered_objects, objects_filtering_params_->check_all_predicted_path, parameters_->safety_check_params.integral_predicted_polygon_params, goal_planner_data_.collision_check); @@ -2046,6 +2126,15 @@ void GoalPlannerModule::setDebugData() } debug_marker_.markers.push_back(marker); + if (parameters_->safety_check_params.enable_safety_check) { + tier4_autoware_utils::appendMarkerArray( + goal_planner_utils::createLaneletPolygonMarkerArray( + debug_data_.expanded_pull_over_lane_between_ego.polygon3d(), header, + "expanded_pull_over_lane_between_ego", + tier4_autoware_utils::createMarkerColor(1.0, 0.7, 0.0, 0.999)), + &debug_marker_); + } + // Visualize debug poses const auto & debug_poses = thread_safe_data_.get_pull_over_path()->debug_poses; for (size_t i = 0; i < debug_poses.size(); ++i) { diff --git a/planning/behavior_path_goal_planner_module/src/manager.cpp b/planning/behavior_path_goal_planner_module/src/manager.cpp index c6fe97874f012..6ee1ba28ed278 100644 --- a/planning/behavior_path_goal_planner_module/src/manager.cpp +++ b/planning/behavior_path_goal_planner_module/src/manager.cpp @@ -102,6 +102,10 @@ void GoalPlannerModuleManager::init(rclcpp::Node * node) ns + "object_recognition_collision_check_max_extra_stopping_margin"); p.th_moving_object_velocity = node->declare_parameter(ns + "th_moving_object_velocity"); p.detection_bound_offset = node->declare_parameter(ns + "detection_bound_offset"); + p.outer_road_detection_offset = + node->declare_parameter(ns + "outer_road_detection_offset"); + p.inner_road_detection_offset = + node->declare_parameter(ns + "inner_road_detection_offset"); } // pull over general params diff --git a/planning/behavior_path_goal_planner_module/src/util.cpp b/planning/behavior_path_goal_planner_module/src/util.cpp index 8ad90d26f58f2..a43592deb3c4e 100644 --- a/planning/behavior_path_goal_planner_module/src/util.cpp +++ b/planning/behavior_path_goal_planner_module/src/util.cpp @@ -17,6 +17,7 @@ #include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner_common/utils/utils.hpp" +#include #include #include #include @@ -78,13 +79,62 @@ lanelet::ConstLanelets getPullOverLanes( lanelet::ConstLanelets generateExpandedPullOverLanes( const RouteHandler & route_handler, const bool left_side, const double backward_distance, - const double forward_distance, double bound_offset) + const double forward_distance, const double bound_offset) { const auto pull_over_lanes = getPullOverLanes(route_handler, left_side, backward_distance, forward_distance); return left_side ? lanelet::utils::getExpandedLanelets(pull_over_lanes, bound_offset, 0.0) - : lanelet::utils::getExpandedLanelets(pull_over_lanes, 0.0, bound_offset); + : lanelet::utils::getExpandedLanelets(pull_over_lanes, 0.0, -bound_offset); +} + +static double getOffsetToLanesBoundary( + const lanelet::ConstLanelets & lanelet_sequence, const geometry_msgs::msg::Pose target_pose, + const bool left_side) +{ + lanelet::ConstLanelet closest_lanelet; + lanelet::utils::query::getClosestLanelet(lanelet_sequence, target_pose, &closest_lanelet); + + // the boundary closer to ego. if left_side, take right boundary + const auto & boundary3d = left_side ? closest_lanelet.rightBound() : closest_lanelet.leftBound(); + const auto boundary = lanelet::utils::to2D(boundary3d); + using lanelet::utils::conversion::toLaneletPoint; + const auto arc_coords = lanelet::geometry::toArcCoordinates( + boundary, lanelet::utils::to2D(toLaneletPoint(target_pose.position)).basicPoint()); + return arc_coords.distance; +} + +lanelet::ConstLanelets generateBetweenEgoAndExpandedPullOverLanes( + const lanelet::ConstLanelets & pull_over_lanes, const bool left_side, + const geometry_msgs::msg::Pose ego_pose, const vehicle_info_util::VehicleInfo & vehicle_info, + const double outer_road_offset, const double inner_road_offset) +{ + const double front_overhang = vehicle_info.front_overhang_m, + wheel_base = vehicle_info.wheel_base_m, wheel_tread = vehicle_info.wheel_tread_m; + const double side_overhang = + left_side ? vehicle_info.left_overhang_m : vehicle_info.right_overhang_m; + const double ego_length_to_front = wheel_base + front_overhang; + const double ego_width_to_front = + !left_side ? (-wheel_tread / 2.0 - side_overhang) : (wheel_tread / 2.0 + side_overhang); + tier4_autoware_utils::Point2d front_edge_local{ego_length_to_front, ego_width_to_front}; + const auto front_edge_glob = tier4_autoware_utils::transformPoint( + front_edge_local, tier4_autoware_utils::pose2transform(ego_pose)); + geometry_msgs::msg::Pose ego_front_pose; + ego_front_pose.position = + createPoint(front_edge_glob.x(), front_edge_glob.y(), ego_pose.position.z); + + // ========================================================================================== + // NOTE: the point which is on the right side of a directed line has negative distance + // getExpandedLanelet(1.0, -2.0) expands a lanelet by 1.0 to the left and by 2.0 to the right + // ========================================================================================== + const double ego_offset_to_closer_boundary = + getOffsetToLanesBoundary(pull_over_lanes, ego_front_pose, left_side); + return left_side ? lanelet::utils::getExpandedLanelets( + pull_over_lanes, outer_road_offset, + ego_offset_to_closer_boundary - inner_road_offset) + : lanelet::utils::getExpandedLanelets( + pull_over_lanes, ego_offset_to_closer_boundary + inner_road_offset, + -outer_road_offset); } PredictedObjects extractObjectsInExpandedPullOverLanes( @@ -233,6 +283,21 @@ MarkerArray createGoalCandidatesMarkerArray( return marker_array; } +MarkerArray createLaneletPolygonMarkerArray( + const lanelet::CompoundPolygon3d & polygon, const std_msgs::msg::Header & header, + const std::string & ns, const std_msgs::msg::ColorRGBA & color) +{ + visualization_msgs::msg::MarkerArray marker_array{}; + auto marker = createDefaultMarker( + header.frame_id, header.stamp, ns, 0, visualization_msgs::msg::Marker::LINE_STRIP, + createMarkerScale(0.1, 0.0, 0.0), color); + for (const auto & p : polygon) { + marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + } + marker_array.markers.push_back(marker); + return marker_array; +} + double calcLateralDeviationBetweenPaths( const PathWithLaneId & reference_path, const PathWithLaneId & target_path) {