diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml index 1725a4d0193d..bfcaeb820183 100644 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml +++ b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml @@ -10,6 +10,8 @@ intersection_velocity: 2.778 # 2.778m/s = 10.0km/h intersection_max_accel: 0.5 # m/ss detection_area_margin: 0.5 # [m] + detection_area_right_margin: 0.5 # [m] + detection_area_left_margin: 0.5 # [m] detection_area_length: 200.0 # [m] min_predicted_path_confidence: 0.05 collision_start_margin_time: 5.0 # [s] diff --git a/map/lanelet2_extension/include/lanelet2_extension/utility/utilities.hpp b/map/lanelet2_extension/include/lanelet2_extension/utility/utilities.hpp index 2a5d48deee54..6737da3e4ec8 100644 --- a/map/lanelet2_extension/include/lanelet2_extension/utility/utilities.hpp +++ b/map/lanelet2_extension/include/lanelet2_extension/utility/utilities.hpp @@ -35,6 +35,10 @@ lanelet::LineString3d generateFineCenterline( const lanelet::ConstLanelet & lanelet_obj, const double resolution = 5.0); lanelet::ConstLineString3d getCenterlineWithOffset( const lanelet::ConstLanelet & lanelet_obj, const double offset, const double resolution = 5.0); +lanelet::ConstLineString3d getRightBoundWithOffset( + const lanelet::ConstLanelet & lanelet_obj, const double offset, const double resolution = 5.0); +lanelet::ConstLineString3d getLeftBoundWithOffset( + const lanelet::ConstLanelet & lanelet_obj, const double offset, const double resolution = 5.0); lanelet::ConstLanelet getExpandedLanelet( const lanelet::ConstLanelet & lanelet_obj, const double left_offset, const double right_offset); diff --git a/map/lanelet2_extension/lib/utilities.cpp b/map/lanelet2_extension/lib/utilities.cpp index 450da120e137..6a274488bb6b 100644 --- a/map/lanelet2_extension/lib/utilities.cpp +++ b/map/lanelet2_extension/lib/utilities.cpp @@ -282,7 +282,7 @@ lanelet::ConstLineString3d getCenterlineWithOffset( const int num_segments = std::max(static_cast(ceil(longer_distance / resolution)), 1); // Resample points - const auto left_points = lanelet::utils::resamplePoints(lanelet_obj.leftBound(), num_segments); + const auto left_points = resamplePoints(lanelet_obj.leftBound(), num_segments); const auto right_points = resamplePoints(lanelet_obj.rightBound(), num_segments); // Create centerline @@ -303,6 +303,65 @@ lanelet::ConstLineString3d getCenterlineWithOffset( return static_cast(centerline); } +lanelet::ConstLineString3d getRightBoundWithOffset( + const lanelet::ConstLanelet & lanelet_obj, const double offset, const double resolution) +{ + // Get length of longer border + const double left_length = lanelet::geometry::length(lanelet_obj.leftBound()); + const double right_length = lanelet::geometry::length(lanelet_obj.rightBound()); + const double longer_distance = (left_length > right_length) ? left_length : right_length; + const int num_segments = std::max(static_cast(ceil(longer_distance / resolution)), 1); + + // Resample points + const auto left_points = resamplePoints(lanelet_obj.leftBound(), num_segments); + const auto right_points = resamplePoints(lanelet_obj.rightBound(), num_segments); + + // Create centerline + lanelet::LineString3d rightBound(lanelet::utils::getId()); + for (int i = 0; i < num_segments + 1; i++) { + // Add ID for the average point of left and right + const auto vec_left_2_right = (right_points.at(i) - left_points.at(i)).normalized(); + + const auto offset_right_basic_point = right_points.at(i) + vec_left_2_right * offset; + + const lanelet::Point3d rightBound_point( + lanelet::utils::getId(), offset_right_basic_point.x(), offset_right_basic_point.y(), + offset_right_basic_point.z()); + rightBound.push_back(rightBound_point); + } + return static_cast(rightBound); +} + +lanelet::ConstLineString3d getLeftBoundWithOffset( + const lanelet::ConstLanelet & lanelet_obj, const double offset, const double resolution) +{ + // Get length of longer border + const double left_length = lanelet::geometry::length(lanelet_obj.leftBound()); + const double right_length = lanelet::geometry::length(lanelet_obj.rightBound()); + const double longer_distance = (left_length > right_length) ? left_length : right_length; + const int num_segments = std::max(static_cast(ceil(longer_distance / resolution)), 1); + + // Resample points + const auto left_points = resamplePoints(lanelet_obj.leftBound(), num_segments); + const auto right_points = resamplePoints(lanelet_obj.rightBound(), num_segments); + + // Create centerline + lanelet::LineString3d leftBound(lanelet::utils::getId()); + for (int i = 0; i < num_segments + 1; i++) { + // Add ID for the average point of left and right + + const auto vec_right_2_left = (left_points.at(i) - right_points.at(i)).normalized(); + + const auto offset_left_basic_point = left_points.at(i) + vec_right_2_left * offset; + + const lanelet::Point3d leftBound_point( + lanelet::utils::getId(), offset_left_basic_point.x(), offset_left_basic_point.y(), + offset_left_basic_point.z()); + leftBound.push_back(leftBound_point); + } + return static_cast(leftBound); +} + lanelet::ConstLanelet getExpandedLanelet( const lanelet::ConstLanelet & lanelet_obj, const double left_offset, const double right_offset) { diff --git a/planning/behavior_velocity_planner/config/intersection.param.yaml b/planning/behavior_velocity_planner/config/intersection.param.yaml index 1725a4d0193d..bfcaeb820183 100644 --- a/planning/behavior_velocity_planner/config/intersection.param.yaml +++ b/planning/behavior_velocity_planner/config/intersection.param.yaml @@ -10,6 +10,8 @@ intersection_velocity: 2.778 # 2.778m/s = 10.0km/h intersection_max_accel: 0.5 # m/ss detection_area_margin: 0.5 # [m] + detection_area_right_margin: 0.5 # [m] + detection_area_left_margin: 0.5 # [m] detection_area_length: 200.0 # [m] min_predicted_path_confidence: 0.05 collision_start_margin_time: 5.0 # [s] diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp index d55b301d17ee..04352513f927 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp @@ -89,6 +89,7 @@ class IntersectionModule : public SceneModuleInterface geometry_msgs::msg::Pose stop_point_pose; geometry_msgs::msg::Pose judge_point_pose; geometry_msgs::msg::Polygon ego_lane_polygon; + std::vector detection_area_with_margin; geometry_msgs::msg::Polygon stuck_vehicle_detect_area; geometry_msgs::msg::Polygon candidate_collision_ego_lane_polygon; std::vector candidate_collision_object_polygons; @@ -111,8 +112,12 @@ class IntersectionModule : public SceneModuleInterface double intersection_velocity; //! used for intersection passing time double intersection_max_acc; //! used for calculating intersection velocity double detection_area_margin; //! used for detecting objects in detection area - double detection_area_length; //! used to create detection area polygon - double detection_area_angle_thr; //! threshold in checking the angle of detecting objects + double detection_area_right_margin; //! used for detecting objects in detection area only right + //! direction + double detection_area_left_margin; //! used for detecting objects in detection area only left + //! direction + double detection_area_length; //! used to create detection area polygon + double detection_area_angle_thr; //! threshold in checking the angle of detecting objects double min_predicted_path_confidence; //! minimum confidence value of predicted path to use for collision detection double external_input_timeout; //! used to disable external input @@ -149,7 +154,6 @@ class IntersectionModule : public SceneModuleInterface * actual collision check algorithm inside this function) * @param lanelet_map_ptr lanelet map * @param path ego-car lane - * @param detection_areas collision check is performed for vehicles that exist in this area * @param detection_area_lanelet_ids angle check is performed for obstacles using this lanelet * ids * @param objects_ptr target objects @@ -159,7 +163,6 @@ class IntersectionModule : public SceneModuleInterface bool checkCollision( lanelet::LaneletMapConstPtr lanelet_map_ptr, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const std::vector & detection_areas, const std::vector & detection_area_lanelet_ids, const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr, const int closest_idx); diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_merge_from_private_road.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_merge_from_private_road.hpp index 09ac15e48c0c..208fee06504d 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_merge_from_private_road.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_merge_from_private_road.hpp @@ -101,6 +101,8 @@ class MergeFromPrivateRoadModule : public SceneModuleInterface { double decel_velocity; double detection_area_length; + double detection_area_right_margin; + double detection_area_left_margin; double stop_line_margin; double stop_duration_sec; }; diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp index 6a1a8b0c4f3f..fa5aee9044a4 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp @@ -56,9 +56,11 @@ bool hasDuplicatedPoint( */ bool getObjectiveLanelets( lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr, - const int lane_id, const double detection_area_length, - std::vector * conflicting_lanelets_result, - std::vector * objective_lanelets_result, const rclcpp::Logger logger); + const int lane_id, const double detection_area_length, const double right_margin, + const double left_margin, std::vector * conflicting_lanelets_result, + std::vector * objective_lanelets_result, + std::vector * objective_lanelets_with_margin_result, + const rclcpp::Logger logger); /** * @brief Generate a stop line and insert it into the path. If the stop line is defined in the map, @@ -107,6 +109,8 @@ double calcArcLengthFromPath( const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path, const size_t src_idx, const size_t dst_idx); +lanelet::ConstLanelet generateOffsetLanelet( + const lanelet::ConstLanelet lanelet, double right_margin, double left_margin); } // namespace util } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp index 878b19765252..2e729a032933 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp @@ -203,6 +203,11 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( createLaneletPolygonsMarkerArray(debug_data_.detection_area, "detection_area", lane_id_), current_time, &debug_marker_array); + appendMarkerArray( + createLaneletPolygonsMarkerArray( + debug_data_.detection_area_with_margin, "detection_area_with_margin", lane_id_), + current_time, &debug_marker_array); + appendMarkerArray( createPolygonMarkerArray(debug_data_.ego_lane_polygon, "ego_lane", lane_id_, 0.0, 0.3, 0.7), current_time, &debug_marker_array); diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp index b46f18a139a7..5fd3a95e5c2f 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp @@ -75,6 +75,8 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) ip.intersection_velocity = node.declare_parameter(ns + ".intersection_velocity", 10.0 / 3.6); ip.intersection_max_acc = node.declare_parameter(ns + ".intersection_max_accel", 0.5); ip.detection_area_margin = node.declare_parameter(ns + ".detection_area_margin", 0.5); + ip.detection_area_right_margin = node.declare_parameter(ns + ".detection_area_right_margin", 0.5); + ip.detection_area_left_margin = node.declare_parameter(ns + ".detection_area_left_margin", 0.5); ip.detection_area_length = node.declare_parameter(ns + ".detection_area_length", 200.0); ip.detection_area_angle_thr = node.declare_parameter(ns + ".detection_area_angle_threshold", M_PI / 4.0); diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp index 4395302d9092..afb6bcf27740 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp @@ -92,14 +92,20 @@ bool IntersectionModule::modifyPathVelocity( /* get detection area and conflicting area */ std::vector detection_area_lanelets; std::vector conflicting_area_lanelets; + std::vector detection_area_lanelets_with_margin; util::getObjectiveLanelets( lanelet_map_ptr, routing_graph_ptr, lane_id_, planner_param_.detection_area_length, - &conflicting_area_lanelets, &detection_area_lanelets, logger_); + planner_param_.detection_area_right_margin, planner_param_.detection_area_left_margin, + &conflicting_area_lanelets, &detection_area_lanelets, &detection_area_lanelets_with_margin, + logger_); std::vector conflicting_areas = util::getPolygon3dFromLaneletsVec( conflicting_area_lanelets, planner_param_.detection_area_length); std::vector detection_areas = util::getPolygon3dFromLaneletsVec( detection_area_lanelets, planner_param_.detection_area_length); + std::vector detection_areas_with_margin = + util::getPolygon3dFromLaneletsVec( + detection_area_lanelets_with_margin, planner_param_.detection_area_length); std::vector conflicting_area_lanelet_ids = util::getLaneletIdsFromLaneletsVec(conflicting_area_lanelets); std::vector detection_area_lanelet_ids = @@ -110,6 +116,7 @@ bool IntersectionModule::modifyPathVelocity( return true; } debug_data_.detection_area = detection_areas; + debug_data_.detection_area_with_margin = detection_areas_with_margin; /* set stop-line and stop-judgement-line for base_link */ int stop_line_idx = -1; @@ -153,8 +160,8 @@ bool IntersectionModule::modifyPathVelocity( const auto objects_ptr = planner_data_->predicted_objects; /* calculate dynamic collision around detection area */ - bool has_collision = checkCollision( - lanelet_map_ptr, *path, detection_areas, detection_area_lanelet_ids, objects_ptr, closest_idx); + bool has_collision = + checkCollision(lanelet_map_ptr, *path, detection_area_lanelet_ids, objects_ptr, closest_idx); bool is_stuck = checkStuckVehicleInIntersection( lanelet_map_ptr, *path, closest_idx, stop_line_idx, objects_ptr); bool is_entry_prohibited = (has_collision || is_stuck); @@ -226,7 +233,6 @@ void IntersectionModule::cutPredictPathWithDuration( bool IntersectionModule::checkCollision( lanelet::LaneletMapConstPtr lanelet_map_ptr, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const std::vector & detection_areas, const std::vector & detection_area_lanelet_ids, const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr, const int closest_idx) @@ -256,24 +262,11 @@ bool IntersectionModule::checkCollision( continue; // TODO(Kenji Miyake): check direction? } - // keep vehicle in detection_area - const Point2d obj_point( - object.kinematics.initial_pose_with_covariance.pose.position.x, - object.kinematics.initial_pose_with_covariance.pose.position.y); - for (const auto & detection_area : detection_areas) { - const auto detection_poly = lanelet::utils::to2D(detection_area).basicPolygon(); - const double dist_to_detection_area = - boost::geometry::distance(obj_point, toBoostPoly(detection_poly)); - if (dist_to_detection_area > planner_param_.detection_area_margin) { - // ignore the object far from detection area - continue; - } - // check direction of objects - const auto object_direction = getObjectPoseWithVelocityDirection(object.kinematics); - if (checkAngleForTargetLanelets(object_direction, detection_area_lanelet_ids)) { - target_objects.objects.push_back(object); - break; - } + // check direction of objects + const auto object_direction = getObjectPoseWithVelocityDirection(object.kinematics); + if (checkAngleForTargetLanelets(object_direction, detection_area_lanelet_ids)) { + target_objects.objects.push_back(object); + break; } } diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp index 92f42ed7a290..9d8374d25c14 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp @@ -67,10 +67,13 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity( /* get detection area */ std::vector detection_area_lanelets; std::vector conflicting_area_lanelets; + std::vector detection_area_lanelets_with_margin; util::getObjectiveLanelets( lanelet_map_ptr, routing_graph_ptr, lane_id_, planner_param_.detection_area_length, - &conflicting_area_lanelets, &detection_area_lanelets, logger_); + planner_param_.detection_area_right_margin, planner_param_.detection_area_left_margin, + &conflicting_area_lanelets, &detection_area_lanelets, &detection_area_lanelets_with_margin, + logger_); std::vector conflicting_areas = util::getPolygon3dFromLaneletsVec( conflicting_area_lanelets, planner_param_.detection_area_length); if (conflicting_areas.empty()) { diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp index b0a91cba575f..0a81d6b5d336 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp @@ -322,9 +322,11 @@ bool getStopPoseIndexFromMap( bool getObjectiveLanelets( lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr, - const int lane_id, const double detection_area_length, + const int lane_id, const double detection_area_length, double right_margin, double left_margin, std::vector * conflicting_lanelets_result, - std::vector * objective_lanelets_result, const rclcpp::Logger logger) + std::vector * objective_lanelets_result, + std::vector * objective_lanelets_with_margin_result, + const rclcpp::Logger logger) { const auto & assigned_lanelet = lanelet_map_ptr->laneletLayer.get(lane_id); @@ -365,6 +367,7 @@ bool getObjectiveLanelets( std::vector // conflicting lanes with "lane_id" conflicting_lanelets_ex_yield_ego; // excluding ego lanes and yield lanes std::vector objective_lanelets; // final objective lanelets + std::vector objective_lanelets_with_margin; // final objective lanelets // exclude yield lanelets and ego lanelets from objective_lanelets for (const auto & conflicting_lanelet : conflicting_lanelets) { @@ -374,8 +377,11 @@ bool getObjectiveLanelets( if (lanelet::utils::contains(ego_lanelets, conflicting_lanelet)) { continue; } + const auto objective_lanelet_with_margin = + generateOffsetLanelet(conflicting_lanelet, right_margin, left_margin); conflicting_lanelets_ex_yield_ego.push_back({conflicting_lanelet}); objective_lanelets.push_back({conflicting_lanelet}); + objective_lanelets_with_margin.push_back({objective_lanelet_with_margin}); } // get possible lanelet path that reaches conflicting_lane longer than given length @@ -395,6 +401,7 @@ bool getObjectiveLanelets( *conflicting_lanelets_result = conflicting_lanelets_ex_yield_ego; *objective_lanelets_result = objective_lanelets_sequences; + *objective_lanelets_with_margin_result = objective_lanelets_with_margin; std::stringstream ss_c, ss_y, ss_e, ss_o, ss_os; for (const auto & l : conflicting_lanelets) { @@ -462,5 +469,32 @@ double calcArcLengthFromPath( return length; } +lanelet::ConstLanelet generateOffsetLanelet( + const lanelet::ConstLanelet lanelet, double right_margin, double left_margin) +{ + lanelet::Points3d lefts, rights; + + const double right_offset = right_margin; + const double left_offset = left_margin; + const auto offset_rightBound = lanelet::utils::getRightBoundWithOffset(lanelet, right_offset); + const auto offset_leftBound = lanelet::utils::getLeftBoundWithOffset(lanelet, left_offset); + + const auto original_left_bound = offset_leftBound; + const auto original_right_bound = offset_rightBound; + + for (const auto & pt : original_left_bound) { + lefts.push_back(lanelet::Point3d(pt)); + } + for (const auto & pt : original_right_bound) { + rights.push_back(lanelet::Point3d(pt)); + } + const auto left_bound = lanelet::LineString3d(lanelet::InvalId, lefts); + const auto right_bound = lanelet::LineString3d(lanelet::InvalId, rights); + auto lanelet_with_margin = lanelet::Lanelet(lanelet::InvalId, left_bound, right_bound); + const auto centerline = lanelet::utils::generateFineCenterline(lanelet_with_margin, 5.0); + lanelet_with_margin.setCenterline(centerline); + return std::move(lanelet_with_margin); +} + } // namespace util } // namespace behavior_velocity_planner