From 2d0abd122b763d65ca54e0663faf63c10d687607 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Thu, 18 Jan 2024 18:22:55 +0900 Subject: [PATCH] feat(blind_spot): consider opposite adjacent lane for wrong vehicles (#5635) Signed-off-by: Mamoru Sobue --- .../config/blind_spot.param.yaml | 1 + .../src/debug.cpp | 6 +- .../src/manager.cpp | 2 + .../src/scene.cpp | 220 ++++++++++++------ .../src/scene.hpp | 16 +- 5 files changed, 162 insertions(+), 83 deletions(-) diff --git a/planning/behavior_velocity_blind_spot_module/config/blind_spot.param.yaml b/planning/behavior_velocity_blind_spot_module/config/blind_spot.param.yaml index f9ddd3ce61099..3c22d1fe65db5 100644 --- a/planning/behavior_velocity_blind_spot_module/config/blind_spot.param.yaml +++ b/planning/behavior_velocity_blind_spot_module/config/blind_spot.param.yaml @@ -8,4 +8,5 @@ max_future_movement_time: 10.0 # [second] threshold_yaw_diff: 0.523 # [rad] adjacent_extend_width: 1.5 # [m] + opposite_adjacent_extend_width: 1.5 # [m] enable_rtc: false # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval diff --git a/planning/behavior_velocity_blind_spot_module/src/debug.cpp b/planning/behavior_velocity_blind_spot_module/src/debug.cpp index 7da494bfd5231..5cc90afb0043d 100644 --- a/planning/behavior_velocity_blind_spot_module/src/debug.cpp +++ b/planning/behavior_velocity_blind_spot_module/src/debug.cpp @@ -145,14 +145,12 @@ visualization_msgs::msg::MarkerArray BlindSpotModule::createDebugMarkerArray() appendMarkerArray( createLaneletPolygonsMarkerArray( - debug_data_.conflict_areas_for_blind_spot, "conflict_area_for_blind_spot", module_id_, 0.0, - 0.5, 0.5), + debug_data_.conflict_areas, "conflict_area", module_id_, 0.0, 0.5, 0.5), &debug_marker_array, now); appendMarkerArray( createLaneletPolygonsMarkerArray( - debug_data_.detection_areas_for_blind_spot, "detection_area_for_blind_spot", module_id_, 0.5, - 0.0, 0.0), + debug_data_.detection_areas, "detection_area", module_id_, 0.5, 0.0, 0.0), &debug_marker_array, now); appendMarkerArray( diff --git a/planning/behavior_velocity_blind_spot_module/src/manager.cpp b/planning/behavior_velocity_blind_spot_module/src/manager.cpp index d64f1ebc39a51..09d1c5a7c3270 100644 --- a/planning/behavior_velocity_blind_spot_module/src/manager.cpp +++ b/planning/behavior_velocity_blind_spot_module/src/manager.cpp @@ -48,6 +48,8 @@ BlindSpotModuleManager::BlindSpotModuleManager(rclcpp::Node & node) getOrDeclareParameter(node, ns + ".threshold_yaw_diff"); planner_param_.adjacent_extend_width = getOrDeclareParameter(node, ns + ".adjacent_extend_width"); + planner_param_.opposite_adjacent_extend_width = + getOrDeclareParameter(node, ns + ".opposite_adjacent_extend_width"); } void BlindSpotModuleManager::launchNewModules( diff --git a/planning/behavior_velocity_blind_spot_module/src/scene.cpp b/planning/behavior_velocity_blind_spot_module/src/scene.cpp index 62a5e06674d4e..efbff7e2af51d 100644 --- a/planning/behavior_velocity_blind_spot_module/src/scene.cpp +++ b/planning/behavior_velocity_blind_spot_module/src/scene.cpp @@ -105,16 +105,18 @@ bool BlindSpotModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto const auto routing_graph_ptr = planner_data_->route_handler_->getRoutingGraphPtr(); /* set stop-line and stop-judgement-line for base_link */ - int stop_line_idx = -1; const auto straight_lanelets = getStraightLanelets(lanelet_map_ptr, routing_graph_ptr, lane_id_); - if (!generateStopLine(straight_lanelets, path, &stop_line_idx)) { + const auto stoplines_idx_opt = generateStopLine(straight_lanelets, path); + if (!stoplines_idx_opt) { RCLCPP_WARN_SKIPFIRST_THROTTLE( logger_, *clock_, 1000 /* ms */, "[BlindSpotModule::run] setStopLineIdx fail"); *path = input_path; // reset path return false; } - if (stop_line_idx <= 0) { + const auto [default_stopline_idx, critical_stopline_idx] = stoplines_idx_opt.value(); + + if (default_stopline_idx <= 0) { RCLCPP_DEBUG( logger_, "[Blind Spot] stop line or pass judge line is at path[0], ignore planning."); *path = input_path; // reset path @@ -133,6 +135,8 @@ bool BlindSpotModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto return false; } const size_t closest_idx = closest_idx_opt.value(); + const auto stop_line_idx = + closest_idx > default_stopline_idx ? critical_stopline_idx : default_stopline_idx; /* set judge line dist */ const double current_vel = planner_data_->current_velocity->twist.linear.x; @@ -156,9 +160,6 @@ bool BlindSpotModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto debug_data_.virtual_wall_pose = stop_line_pose; const auto stop_pose = path->points.at(stop_line_idx).point.pose; debug_data_.stop_point_pose = stop_pose; - auto offset_pose = motion_utils::calcLongitudinalOffsetPose( - path->points, stop_pose.position, -pass_judge_line_dist); - if (offset_pose) debug_data_.judge_point_pose = *offset_pose; /* if current_state = GO, and current_pose is over judge_line, ignore planning. */ if (planner_param_.use_pass_judge_line) { @@ -235,69 +236,41 @@ std::optional BlindSpotModule::getFirstPointConflictingLanelets( } } -bool BlindSpotModule::generateStopLine( +std::optional> BlindSpotModule::generateStopLine( const lanelet::ConstLanelets straight_lanelets, - autoware_auto_planning_msgs::msg::PathWithLaneId * path, int * stop_line_idx) const + autoware_auto_planning_msgs::msg::PathWithLaneId * path) const { /* set parameters */ constexpr double interval = 0.2; const int margin_idx_dist = std::ceil(planner_param_.stop_line_margin / interval); - const int base2front_idx_dist = - std::ceil(planner_data_->vehicle_info_.max_longitudinal_offset_m / interval); + // const int base2front_idx_dist = + // std::ceil(planner_data_->vehicle_info_.max_longitudinal_offset_m / interval); /* spline interpolation */ autoware_auto_planning_msgs::msg::PathWithLaneId path_ip; if (!splineInterpolate(*path, interval, path_ip, logger_)) { - return false; + return std::nullopt; } /* generate stop point */ - int stop_idx_ip = 0; // stop point index for interpolated path. + size_t stop_idx_default_ip = 0; // stop point index for interpolated path. + size_t stop_idx_critical_ip = 0; if (straight_lanelets.size() > 0) { std::optional first_idx_conflicting_lane_opt = getFirstPointConflictingLanelets(path_ip, straight_lanelets); if (!first_idx_conflicting_lane_opt) { RCLCPP_DEBUG(logger_, "No conflicting line found."); - return false; - } - stop_idx_ip = std::max( - first_idx_conflicting_lane_opt.value() - 1 - margin_idx_dist - base2front_idx_dist, 0); - } else { - std::optional intersection_enter_point_opt = - getStartPointFromLaneLet(lane_id_); - if (!intersection_enter_point_opt) { - RCLCPP_DEBUG(logger_, "No intersection enter point found."); - return false; - } - - geometry_msgs::msg::Pose intersection_enter_pose; - intersection_enter_pose = intersection_enter_point_opt.value(); - const auto stop_idx_ip_opt = - motion_utils::findNearestIndex(path_ip.points, intersection_enter_pose, 10.0, M_PI_4); - if (stop_idx_ip_opt) { - stop_idx_ip = stop_idx_ip_opt.value(); + return std::nullopt; } - - stop_idx_ip = std::max(stop_idx_ip - base2front_idx_dist, 0); + stop_idx_default_ip = std::max(first_idx_conflicting_lane_opt.value() - 1 - margin_idx_dist, 0); + stop_idx_critical_ip = std::max(first_idx_conflicting_lane_opt.value() - 1, 0); } /* insert stop_point to use interpolated path*/ - *stop_line_idx = insertPoint(stop_idx_ip, path_ip, path); + const size_t stopline_idx_default = insertPoint(stop_idx_default_ip, path_ip, path); + const size_t stopline_idx_critical = insertPoint(stop_idx_critical_ip, path_ip, path); - /* if another stop point exist before intersection stop_line, disable judge_line. */ - bool has_prior_stopline = false; - for (int i = 0; i < *stop_line_idx; ++i) { - if (std::fabs(path->points.at(i).point.longitudinal_velocity_mps) < 0.1) { - has_prior_stopline = true; - break; - } - } - - RCLCPP_DEBUG( - logger_, "generateStopLine() : stop_idx = %d, stop_idx_ip = %d, has_prior_stopline = %d", - *stop_line_idx, stop_idx_ip, has_prior_stopline); - - return true; + return std::make_pair(stopline_idx_default, stopline_idx_critical); } void BlindSpotModule::cutPredictPathWithDuration( @@ -385,36 +358,61 @@ bool BlindSpotModule::checkObstacleInBlindSpot( const auto areas_opt = generateBlindSpotPolygons( lanelet_map_ptr, routing_graph_ptr, path, closest_idx, stop_line_pose); - if (!!areas_opt) { - debug_data_.detection_areas_for_blind_spot = areas_opt.value().detection_areas; - debug_data_.conflict_areas_for_blind_spot = areas_opt.value().conflict_areas; + if (areas_opt) { + const auto & detection_areas = areas_opt.value().detection_areas; + const auto & conflict_areas = areas_opt.value().conflict_areas; + const auto & opposite_detection_areas = areas_opt.value().opposite_detection_areas; + const auto & opposite_conflict_areas = areas_opt.value().opposite_conflict_areas; + debug_data_.detection_areas = detection_areas; + debug_data_.conflict_areas = conflict_areas; + debug_data_.detection_areas.insert( + debug_data_.detection_areas.end(), opposite_detection_areas.begin(), + opposite_detection_areas.end()); + debug_data_.conflict_areas.insert( + debug_data_.conflict_areas.end(), opposite_conflict_areas.begin(), + opposite_conflict_areas.end()); autoware_auto_perception_msgs::msg::PredictedObjects objects = *objects_ptr; cutPredictPathWithDuration(&objects, planner_param_.max_future_movement_time); // check objects in blind spot areas - bool obstacle_detected = false; for (const auto & object : objects.objects) { if (!isTargetObjectType(object)) { continue; } - const auto & detection_areas = areas_opt.value().detection_areas; - const auto & conflict_areas = areas_opt.value().conflict_areas; - const bool exist_in_detection_area = + // right direction + const bool exist_in_right_detection_area = std::any_of(detection_areas.begin(), detection_areas.end(), [&object](const auto & area) { return bg::within( to_bg2d(object.kinematics.initial_pose_with_covariance.pose.position), lanelet::utils::to2D(area)); }); - const bool exist_in_conflict_area = + // opposite direction + const bool exist_in_opposite_detection_area = std::any_of( + opposite_detection_areas.begin(), opposite_detection_areas.end(), + [&object](const auto & area) { + return bg::within( + to_bg2d(object.kinematics.initial_pose_with_covariance.pose.position), + lanelet::utils::to2D(area)); + }); + const bool exist_in_detection_area = + exist_in_right_detection_area || exist_in_opposite_detection_area; + if (!exist_in_detection_area) { + continue; + } + const bool exist_in_right_conflict_area = isPredictedPathInArea(object, conflict_areas, planner_data_->current_odometry->pose); + const bool exist_in_opposite_conflict_area = isPredictedPathInArea( + object, opposite_conflict_areas, planner_data_->current_odometry->pose); + const bool exist_in_conflict_area = + exist_in_right_conflict_area || exist_in_opposite_conflict_area; if (exist_in_detection_area && exist_in_conflict_area) { - obstacle_detected = true; debug_data_.conflicting_targets.objects.push_back(object); + return true; } } - return obstacle_detected; + return false; } else { return false; } @@ -504,6 +502,37 @@ lanelet::ConstLanelet BlindSpotModule::generateExtendedAdjacentLanelet( return new_lanelet; } +lanelet::ConstLanelet BlindSpotModule::generateExtendedOppositeAdjacentLanelet( + const lanelet::ConstLanelet lanelet, const TurnDirection direction) const +{ + const auto centerline = lanelet.centerline2d(); + const auto width = + boost::geometry::area(lanelet.polygon2d().basicPolygon()) / boost::geometry::length(centerline); + const double extend_width = + std::min(planner_param_.opposite_adjacent_extend_width, width); + const auto left_bound_ = + direction == TurnDirection::RIGHT + ? lanelet.rightBound().invert() + : lanelet::utils::getCenterlineWithOffset(lanelet.invert(), -width / 2 + extend_width); + const auto right_bound_ = + direction == TurnDirection::RIGHT + ? lanelet::utils::getCenterlineWithOffset(lanelet.invert(), width / 2 - extend_width) + : lanelet.leftBound().invert(); + lanelet::Points3d lefts, rights; + for (const auto & pt : left_bound_) { + lefts.push_back(lanelet::Point3d(pt)); + } + for (const auto & pt : right_bound_) { + rights.push_back(lanelet::Point3d(pt)); + } + const auto left_bound = lanelet::LineString3d(lanelet::InvalId, std::move(lefts)); + const auto right_bound = lanelet::LineString3d(lanelet::InvalId, std::move(rights)); + auto new_lanelet = lanelet::Lanelet(lanelet::InvalId, left_bound, right_bound); + const auto new_centerline = lanelet::utils::generateFineCenterline(new_lanelet, 5.0); + new_lanelet.setCenterline(new_centerline); + return new_lanelet; +} + std::optional BlindSpotModule::generateBlindSpotPolygons( lanelet::LaneletMapConstPtr lanelet_map_ptr, [[maybe_unused]] lanelet::routing::RoutingGraphPtr routing_graph_ptr, @@ -512,19 +541,20 @@ std::optional BlindSpotModule::generateBlindSpotPolygons( { std::vector lane_ids; lanelet::ConstLanelets blind_spot_lanelets; + lanelet::ConstLanelets blind_spot_opposite_lanelets; /* get lane ids until intersection */ for (const auto & point : path.points) { bool found_intersection_lane = false; for (const auto lane_id : point.lane_ids) { - // make lane_ids unique - if (std::find(lane_ids.begin(), lane_ids.end(), lane_id) == lane_ids.end()) { - lane_ids.push_back(lane_id); - } - if (lane_id == lane_id_) { found_intersection_lane = true; + lane_ids.push_back(lane_id); break; } + // make lane_ids unique + if (std::find(lane_ids.begin(), lane_ids.end(), lane_id) == lane_ids.end()) { + lane_ids.push_back(lane_id); + } } if (found_intersection_lane) break; } @@ -537,27 +567,49 @@ std::optional BlindSpotModule::generateBlindSpotPolygons( // additional detection area on left/right side lanelet::ConstLanelets adjacent_lanelets; + lanelet::ConstLanelets opposite_adjacent_lanelets; for (const auto i : lane_ids) { const auto lane = lanelet_map_ptr->laneletLayer.get(i); - const auto adj = std::invoke([&]() -> std::optional { - if (turn_direction_ == TurnDirection::INVALID) { + const auto adj = + turn_direction_ == TurnDirection::LEFT + ? (routing_graph_ptr->adjacentLeft(lane)) + : (turn_direction_ == TurnDirection::RIGHT ? (routing_graph_ptr->adjacentRight(lane)) + : boost::none); + const std::optional opposite_adj = + [&]() -> std::optional { + if (!!adj) { return std::nullopt; } - const auto adj_lane = (turn_direction_ == TurnDirection::LEFT) - ? routing_graph_ptr->adjacentLeft(lane) - : routing_graph_ptr->adjacentRight(lane); - - if (adj_lane) { - return *adj_lane; + if (turn_direction_ == TurnDirection::LEFT) { + // this should exist in right-hand traffic + const auto adjacent_lanes = + lanelet_map_ptr->laneletLayer.findUsages(lane.leftBound().invert()); + if (adjacent_lanes.empty()) { + return std::nullopt; + } + return adjacent_lanes.front(); } - - return std::nullopt; - }); - + if (turn_direction_ == TurnDirection::RIGHT) { + // this should exist in left-hand traffic + const auto adjacent_lanes = + lanelet_map_ptr->laneletLayer.findUsages(lane.rightBound().invert()); + if (adjacent_lanes.empty()) { + return std::nullopt; + } + return adjacent_lanes.front(); + } else { + return std::nullopt; + } + }(); if (adj) { const auto half_lanelet = generateExtendedAdjacentLanelet(adj.value(), turn_direction_); adjacent_lanelets.push_back(half_lanelet); } + if (opposite_adj) { + const auto half_lanelet = + generateExtendedOppositeAdjacentLanelet(opposite_adj.value(), turn_direction_); + opposite_adjacent_lanelets.push_back(half_lanelet); + } } const auto current_arc_ego = @@ -587,6 +639,24 @@ std::optional BlindSpotModule::generateBlindSpotPolygons( blind_spot_polygons.conflict_areas.emplace_back(std::move(conflicting_area_adj)); blind_spot_polygons.detection_areas.emplace_back(std::move(detection_area_adj)); } + // additional detection area on left/right opposite lane side + if (!opposite_adjacent_lanelets.empty()) { + const auto stop_line_arc_opposite_adj = + lanelet::utils::getLaneletLength3d(opposite_adjacent_lanelets); + const auto current_arc_opposite_adj = + stop_line_arc_opposite_adj - (stop_line_arc_ego - current_arc_ego); + const auto detection_area_start_length_opposite_adj = + stop_line_arc_opposite_adj - planner_param_.backward_length; + auto conflicting_area_opposite_adj = lanelet::utils::getPolygonFromArcLength( + opposite_adjacent_lanelets, current_arc_opposite_adj, stop_line_arc_opposite_adj); + auto detection_area_opposite_adj = lanelet::utils::getPolygonFromArcLength( + opposite_adjacent_lanelets, detection_area_start_length_opposite_adj, + stop_line_arc_opposite_adj); + blind_spot_polygons.opposite_conflict_areas.emplace_back( + std::move(conflicting_area_opposite_adj)); + blind_spot_polygons.opposite_detection_areas.emplace_back( + std::move(detection_area_opposite_adj)); + } return blind_spot_polygons; } else { return std::nullopt; @@ -677,7 +747,7 @@ lanelet::ConstLanelets BlindSpotModule::getStraightLanelets( const auto next_lanelets = routing_graph_ptr->following(prev_intersection_lanelets.front()); for (const auto & ll : next_lanelets) { const std::string turn_direction = ll.attributeOr("turn_direction", "else"); - if (!turn_direction.compare("straight")) { + if (turn_direction.compare("straight") == 0) { straight_lanelets.push_back(ll); } } diff --git a/planning/behavior_velocity_blind_spot_module/src/scene.hpp b/planning/behavior_velocity_blind_spot_module/src/scene.hpp index 3915acd3532b5..6f8450568939c 100644 --- a/planning/behavior_velocity_blind_spot_module/src/scene.hpp +++ b/planning/behavior_velocity_blind_spot_module/src/scene.hpp @@ -30,6 +30,7 @@ #include #include +#include #include namespace behavior_velocity_planner @@ -38,6 +39,8 @@ struct BlindSpotPolygons { std::vector conflict_areas; std::vector detection_areas; + std::vector opposite_conflict_areas; + std::vector opposite_detection_areas; }; class BlindSpotModule : public SceneModuleInterface @@ -50,8 +53,10 @@ class BlindSpotModule : public SceneModuleInterface geometry_msgs::msg::Pose virtual_wall_pose; geometry_msgs::msg::Pose stop_point_pose; geometry_msgs::msg::Pose judge_point_pose; - std::vector conflict_areas_for_blind_spot; - std::vector detection_areas_for_blind_spot; + std::vector conflict_areas; + std::vector detection_areas; + std::vector opposite_conflict_areas; + std::vector opposite_detection_areas; autoware_auto_perception_msgs::msg::PredictedObjects conflicting_targets; }; @@ -67,6 +72,7 @@ class BlindSpotModule : public SceneModuleInterface double threshold_yaw_diff; //! threshold of yaw difference between ego and target object double adjacent_extend_width; //! the width of extended detection/conflict area on adjacent lane + double opposite_adjacent_extend_width; }; BlindSpotModule( @@ -118,6 +124,8 @@ class BlindSpotModule : public SceneModuleInterface lanelet::ConstLanelet generateExtendedAdjacentLanelet( const lanelet::ConstLanelet lanelet, const TurnDirection direction) const; + lanelet::ConstLanelet generateExtendedOppositeAdjacentLanelet( + const lanelet::ConstLanelet lanelet, const TurnDirection direction) const; /** * @brief Make blind spot areas. Narrow area is made from closest path point to stop line index. @@ -169,9 +177,9 @@ class BlindSpotModule : public SceneModuleInterface * @param pass_judge_line_idx generated pass judge line index * @return false when generation failed */ - bool generateStopLine( + std::optional> generateStopLine( const lanelet::ConstLanelets straight_lanelets, - autoware_auto_planning_msgs::msg::PathWithLaneId * path, int * stop_line_idx) const; + autoware_auto_planning_msgs::msg::PathWithLaneId * path) const; /** * @brief Insert a point to target path