diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp index a536fa1b568cb..223db73e72a4c 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp @@ -443,9 +443,6 @@ struct AvoidLine : public ShiftLine // Distance from ego to end point in Frenet double end_longitudinal = 0.0; - // for unique_id - UUID id{}; - // for the case the point is created by merge other points std::vector parent_ids{}; diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp index a74c3a69d7bce..82e2c7af32f8d 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp @@ -404,6 +404,8 @@ class AvoidanceModule : public SceneModuleInterface bool safe_{true}; + std::optional ignore_signal_{std::nullopt}; + std::shared_ptr helper_; std::shared_ptr parameters_; diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp index 02d8d68cf7a56..2d61571392eef 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp @@ -176,7 +176,7 @@ double calcDistanceToAvoidStartLine( const std::shared_ptr & planner_data, const std::shared_ptr & parameters); -TurnSignalInfo calcTurnSignalInfo( +std::pair calcTurnSignalInfo( const ShiftedPath & path, const ShiftLine & shift_line, const double current_shift_length, const AvoidancePlanningData & data, const std::shared_ptr & planner_data); } // namespace behavior_path_planner::utils::avoidance diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp index 75529c35c2af5..27cd15cc525df 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -918,12 +918,26 @@ BehaviorModuleOutput AvoidanceModule::plan() BehaviorModuleOutput output; + const auto is_ignore_signal = [this](const UUID & uuid) { + if (!ignore_signal_.has_value()) { + return false; + } + + return ignore_signal_.value() == uuid; + }; + + const auto update_ignore_signal = [this](const UUID & uuid, const bool is_ignore) { + ignore_signal_ = is_ignore ? std::make_optional(uuid) : std::nullopt; + }; + // turn signal info if (path_shifter_.getShiftLines().empty()) { output.turn_signal_info = getPreviousModuleOutput().turn_signal_info; + } else if (is_ignore_signal(path_shifter_.getShiftLines().front().id)) { + output.turn_signal_info = getPreviousModuleOutput().turn_signal_info; } else { const auto original_signal = getPreviousModuleOutput().turn_signal_info; - const auto new_signal = utils::avoidance::calcTurnSignalInfo( + const auto [new_signal, is_ignore] = utils::avoidance::calcTurnSignalInfo( linear_shift_path, path_shifter_.getShiftLines().front(), helper_->getEgoShift(), avoid_data_, planner_data_); const auto current_seg_idx = planner_data_->findEgoSegmentIndex(spline_shift_path.path.points); @@ -931,6 +945,7 @@ BehaviorModuleOutput AvoidanceModule::plan() spline_shift_path.path, getEgoPose(), current_seg_idx, original_signal, new_signal, planner_data_->parameters.ego_nearest_dist_threshold, planner_data_->parameters.ego_nearest_yaw_threshold); + update_ignore_signal(path_shifter_.getShiftLines().front().id, is_ignore); } // sparse resampling for computational cost diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp index 6a44350dd48c6..df775ea6b2c0a 100644 --- a/planning/behavior_path_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_avoidance_module/src/utils.cpp @@ -250,16 +250,37 @@ void pushUniqueVector(T & base_vector, const T & additional_vector) base_vector.insert(base_vector.end(), additional_vector.begin(), additional_vector.end()); } +bool isAvoidShift( + const double start_shift_length, const double end_shift_length, const double threshold) +{ + return std::abs(start_shift_length) < threshold && std::abs(end_shift_length) > threshold; +} + +bool isReturnShift( + const double start_shift_length, const double end_shift_length, const double threshold) +{ + return std::abs(start_shift_length) > threshold && std::abs(end_shift_length) < threshold; +} + +bool isLeftMiddleShift( + const double start_shift_length, const double end_shift_length, const double threshold) +{ + return start_shift_length > threshold && end_shift_length > threshold; +} + +bool isRightMiddleShift( + const double start_shift_length, const double end_shift_length, const double threshold) +{ + return start_shift_length < threshold && end_shift_length < threshold; +} + bool existShiftSideLane( const double start_shift_length, const double end_shift_length, const bool no_left_lanes, - const bool no_right_lanes) + const bool no_right_lanes, const double threshold) { - constexpr double THRESHOLD = 0.1; const auto relative_shift_length = end_shift_length - start_shift_length; - const auto avoid_shift = - std::abs(start_shift_length) < THRESHOLD && std::abs(end_shift_length) > THRESHOLD; - if (avoid_shift) { + if (isAvoidShift(start_shift_length, end_shift_length, threshold)) { // Left avoid. But there is no adjacent lane. No need blinker. if (relative_shift_length > 0.0 && no_left_lanes) { return false; @@ -271,9 +292,7 @@ bool existShiftSideLane( } } - const auto return_shift = - std::abs(start_shift_length) > THRESHOLD && std::abs(end_shift_length) < THRESHOLD; - if (return_shift) { + if (isReturnShift(start_shift_length, end_shift_length, threshold)) { // Right return. But there is no adjacent lane. No need blinker. if (relative_shift_length > 0.0 && no_right_lanes) { return false; @@ -285,8 +304,7 @@ bool existShiftSideLane( } } - const auto left_middle_shift = start_shift_length > THRESHOLD && end_shift_length > THRESHOLD; - if (left_middle_shift) { + if (isLeftMiddleShift(start_shift_length, end_shift_length, threshold)) { // Left avoid. But there is no adjacent lane. No need blinker. if (relative_shift_length > 0.0 && no_left_lanes) { return false; @@ -298,8 +316,7 @@ bool existShiftSideLane( } } - const auto right_middle_shift = start_shift_length < THRESHOLD && end_shift_length < THRESHOLD; - if (right_middle_shift) { + if (isRightMiddleShift(start_shift_length, end_shift_length, threshold)) { // Right avoid. But there is no adjacent lane. No need blinker. if (relative_shift_length < 0.0 && no_right_lanes) { return false; @@ -314,6 +331,23 @@ bool existShiftSideLane( return true; } +bool isNearEndOfShift( + const double start_shift_length, const double end_shift_length, const Point & ego_pos, + const lanelet::ConstLanelets & original_lanes, const double threshold) +{ + using boost::geometry::within; + using lanelet::utils::to2D; + using lanelet::utils::conversion::toLaneletPoint; + + if (!isReturnShift(start_shift_length, end_shift_length, threshold)) { + return false; + } + + return std::any_of(original_lanes.begin(), original_lanes.end(), [&ego_pos](const auto & lane) { + return within(to2D(toLaneletPoint(ego_pos)), lane.polygon2d().basicPolygon()); + }); +} + bool straddleRoadBound( const ShiftedPath & path, const ShiftLine & shift_line, const lanelet::ConstLanelets & lanes, const vehicle_info_util::VehicleInfo & vehicle_info) @@ -2323,7 +2357,7 @@ double calcDistanceToReturnDeadLine( return distance_to_return_dead_line; } -TurnSignalInfo calcTurnSignalInfo( +std::pair calcTurnSignalInfo( const ShiftedPath & path, const ShiftLine & shift_line, const double current_shift_length, const AvoidancePlanningData & data, const std::shared_ptr & planner_data) { @@ -2335,22 +2369,22 @@ TurnSignalInfo calcTurnSignalInfo( if (shift_line.start_idx + 1 > path.shift_length.size()) { RCLCPP_WARN(rclcpp::get_logger(__func__), "index inconsistency."); - return {}; + return std::make_pair(TurnSignalInfo{}, true); } if (shift_line.start_idx + 1 > path.path.points.size()) { RCLCPP_WARN(rclcpp::get_logger(__func__), "index inconsistency."); - return {}; + return std::make_pair(TurnSignalInfo{}, true); } if (shift_line.end_idx + 1 > path.shift_length.size()) { RCLCPP_WARN(rclcpp::get_logger(__func__), "index inconsistency."); - return {}; + return std::make_pair(TurnSignalInfo{}, true); } if (shift_line.end_idx + 1 > path.path.points.size()) { RCLCPP_WARN(rclcpp::get_logger(__func__), "index inconsistency."); - return {}; + return std::make_pair(TurnSignalInfo{}, true); } const auto start_shift_length = path.shift_length.at(shift_line.start_idx); @@ -2359,12 +2393,12 @@ TurnSignalInfo calcTurnSignalInfo( // If shift length is shorter than the threshold, it does not need to turn on blinkers if (std::fabs(relative_shift_length) < p.turn_signal_shift_length_threshold) { - return {}; + return std::make_pair(TurnSignalInfo{}, true); } // If the vehicle does not shift anymore, we turn off the blinker if (std::fabs(path.shift_length.at(shift_line.end_idx) - current_shift_length) < THRESHOLD) { - return {}; + return std::make_pair(TurnSignalInfo{}, true); } const auto get_command = [](const auto & shift_length) { @@ -2379,7 +2413,7 @@ TurnSignalInfo calcTurnSignalInfo( p.vehicle_info.max_longitudinal_offset_m; if (signal_prepare_distance < ego_front_to_shift_start) { - return {}; + return std::make_pair(TurnSignalInfo{}, false); } const auto blinker_start_pose = path.path.points.at(shift_line.start_idx).point.pose; @@ -2396,12 +2430,12 @@ TurnSignalInfo calcTurnSignalInfo( turn_signal_info.turn_signal.command = get_command(relative_shift_length); if (!p.turn_signal_on_swerving) { - return turn_signal_info; + return std::make_pair(turn_signal_info, false); } lanelet::ConstLanelet lanelet; if (!rh->getClosestLaneletWithinRoute(shift_line.end, &lanelet)) { - return {}; + return std::make_pair(TurnSignalInfo{}, true); } const auto left_same_direction_lane = rh->getLeftLanelet(lanelet, true, true); @@ -2412,14 +2446,25 @@ TurnSignalInfo calcTurnSignalInfo( const auto has_right_lane = right_same_direction_lane.has_value() || !right_opposite_lanes.empty(); - if (!existShiftSideLane(start_shift_length, end_shift_length, !has_left_lane, !has_right_lane)) { - return {}; + if (!existShiftSideLane( + start_shift_length, end_shift_length, !has_left_lane, !has_right_lane, + p.turn_signal_shift_length_threshold)) { + return std::make_pair(TurnSignalInfo{}, true); } if (!straddleRoadBound(path, shift_line, data.current_lanelets, p.vehicle_info)) { - return {}; + return std::make_pair(TurnSignalInfo{}, true); + } + + constexpr double STOPPED_THRESHOLD = 0.1; // [m/s] + if (ego_speed < STOPPED_THRESHOLD) { + if (isNearEndOfShift( + start_shift_length, end_shift_length, ego_pose.position, data.current_lanelets, + p.turn_signal_shift_length_threshold)) { + return std::make_pair(TurnSignalInfo{}, true); + } } - return turn_signal_info; + return std::make_pair(turn_signal_info, false); } } // namespace behavior_path_planner::utils::avoidance diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp index 165a11ebb54b4..67f581123057e 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp @@ -17,9 +17,11 @@ #include #include +#include #include #include +#include #include #include @@ -31,9 +33,13 @@ using autoware_auto_planning_msgs::msg::PathPointWithLaneId; using autoware_auto_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; +using tier4_autoware_utils::generateUUID; +using unique_identifier_msgs::msg::UUID; struct ShiftLine { + ShiftLine() : id(generateUUID()) {} + Pose start{}; // shift start point in absolute coordinate Pose end{}; // shift start point in absolute coordinate @@ -45,6 +51,9 @@ struct ShiftLine size_t start_idx{}; // associated start-point index for the reference path size_t end_idx{}; // associated end-point index for the reference path + + // for unique_id + UUID id{}; }; using ShiftLineArray = std::vector;