From 744765cb7398e98c8a8c4aeee726731c8c7426ac Mon Sep 17 00:00:00 2001 From: Daniel Sanchez Date: Fri, 15 Mar 2024 17:45:28 +0900 Subject: [PATCH] use the general calc turn signal method Signed-off-by: Daniel Sanchez --- .../behavior_path_avoidance_module/utils.hpp | 3 - .../src/scene.cpp | 10 +- .../src/utils.cpp | 237 ------------------ 3 files changed, 7 insertions(+), 243 deletions(-) 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 2d61571392eef..9dc23ea4ec80e 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,9 +176,6 @@ double calcDistanceToAvoidStartLine( const std::shared_ptr & planner_data, const std::shared_ptr & parameters); -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 #endif // BEHAVIOR_PATH_AVOIDANCE_MODULE__UTILS_HPP_ diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp index 67eeab9e4ea66..5a41359fe6a65 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -942,9 +942,13 @@ BehaviorModuleOutput AvoidanceModule::plan() output.turn_signal_info = getPreviousModuleOutput().turn_signal_info; } else { const auto original_signal = getPreviousModuleOutput().turn_signal_info; - const auto [new_signal, is_ignore] = utils::avoidance::calcTurnSignalInfo( - linear_shift_path, path_shifter_.getShiftLines().front(), helper_->getEgoShift(), avoid_data_, - planner_data_); + + constexpr bool is_driving_forward = true; + constexpr bool egos_lane_is_shifted = true; + const auto [new_signal, is_ignore] = planner_data_->getBehaviorTurnSignalInfo( + linear_shift_path, path_shifter_.getShiftLines().front(), avoid_data_.current_lanelets, + helper_->getEgoShift(), is_driving_forward, egos_lane_is_shifted); + const auto current_seg_idx = planner_data_->findEgoSegmentIndex(spline_shift_path.path.points); output.turn_signal_info = planner_data_->turn_signal_decider.use_prior_turn_signal( spline_shift_path.path, getEgoPose(), current_seg_idx, original_signal, new_signal, diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp index a7e41825bfe96..f24a514a9ac72 100644 --- a/planning/behavior_path_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_avoidance_module/src/utils.cpp @@ -250,132 +250,6 @@ 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 double threshold) -{ - const auto relative_shift_length = end_shift_length - start_shift_length; - - 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; - } - - // Right avoid. But there is no adjacent lane. No need blinker. - if (relative_shift_length < 0.0 && no_right_lanes) { - return false; - } - } - - 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; - } - - // Left return. But there is no adjacent lane. No need blinker. - if (relative_shift_length < 0.0 && no_left_lanes) { - return false; - } - } - - 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; - } - - // Left return. But there is no adjacent lane. No need blinker. - if (relative_shift_length < 0.0 && no_left_lanes) { - return false; - } - } - - 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; - } - - // Left avoid. But there is no adjacent lane. No need blinker. - if (relative_shift_length > 0.0 && no_right_lanes) { - return false; - } - } - - 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) -{ - using boost::geometry::intersects; - using tier4_autoware_utils::pose2transform; - using tier4_autoware_utils::transformVector; - - const auto footprint = vehicle_info.createFootprint(); - - for (const auto & lane : lanes) { - for (size_t i = shift_line.start_idx; i < shift_line.end_idx; ++i) { - const auto transform = pose2transform(path.path.points.at(i).point.pose); - const auto shifted_vehicle_footprint = transformVector(footprint, transform); - - if (intersects(lane.leftBound2d().basicLineString(), shifted_vehicle_footprint)) { - return true; - } - - if (intersects(lane.rightBound2d().basicLineString(), shifted_vehicle_footprint)) { - return true; - } - } - } - - return false; -} - } // namespace namespace filtering_utils @@ -2367,115 +2241,4 @@ double calcDistanceToReturnDeadLine( return distance_to_return_dead_line; } - -std::pair calcTurnSignalInfo( - const ShiftedPath & path, const ShiftLine & shift_line, const double current_shift_length, - const AvoidancePlanningData & data, const std::shared_ptr & planner_data) -{ - constexpr double THRESHOLD = 0.1; - const auto & p = planner_data->parameters; - const auto & rh = planner_data->route_handler; - const auto & ego_pose = planner_data->self_odometry->pose.pose; - const auto & ego_speed = planner_data->self_odometry->twist.twist.linear.x; - - if (shift_line.start_idx + 1 > path.shift_length.size()) { - RCLCPP_WARN(rclcpp::get_logger(__func__), "index inconsistency."); - 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 std::make_pair(TurnSignalInfo{}, true); - } - - if (shift_line.end_idx + 1 > path.shift_length.size()) { - RCLCPP_WARN(rclcpp::get_logger(__func__), "index inconsistency."); - 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 std::make_pair(TurnSignalInfo{}, true); - } - - const auto start_shift_length = path.shift_length.at(shift_line.start_idx); - const auto end_shift_length = path.shift_length.at(shift_line.end_idx); - const auto relative_shift_length = end_shift_length - start_shift_length; - - // 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 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 std::make_pair(TurnSignalInfo{}, true); - } - - const auto get_command = [](const auto & shift_length) { - return shift_length > 0.0 ? TurnIndicatorsCommand::ENABLE_LEFT - : TurnIndicatorsCommand::ENABLE_RIGHT; - }; - - const auto signal_prepare_distance = - std::max(ego_speed * p.turn_signal_search_time, p.turn_signal_minimum_search_distance); - const auto ego_front_to_shift_start = - calcSignedArcLength(path.path.points, ego_pose.position, shift_line.start_idx) - - p.vehicle_info.max_longitudinal_offset_m; - - if (signal_prepare_distance < ego_front_to_shift_start) { - return std::make_pair(TurnSignalInfo{}, false); - } - - const auto blinker_start_pose = path.path.points.at(shift_line.start_idx).point.pose; - const auto blinker_end_pose = path.path.points.at(shift_line.end_idx).point.pose; - const auto get_start_pose = [&](const auto & ego_to_shift_start) { - return ego_to_shift_start ? ego_pose : blinker_start_pose; - }; - - TurnSignalInfo turn_signal_info{}; - turn_signal_info.desired_start_point = get_start_pose(ego_front_to_shift_start); - turn_signal_info.desired_end_point = blinker_end_pose; - turn_signal_info.required_start_point = blinker_start_pose; - turn_signal_info.required_end_point = blinker_end_pose; - turn_signal_info.turn_signal.command = get_command(relative_shift_length); - - if (!p.turn_signal_on_swerving) { - return std::make_pair(turn_signal_info, false); - } - - lanelet::ConstLanelet lanelet; - if (!rh->getClosestLaneletWithinRoute(shift_line.end, &lanelet)) { - return std::make_pair(TurnSignalInfo{}, true); - } - - const auto left_same_direction_lane = rh->getLeftLanelet(lanelet, true, true); - const auto left_opposite_lanes = rh->getLeftOppositeLanelets(lanelet); - const auto right_same_direction_lane = rh->getRightLanelet(lanelet, true, true); - const auto right_opposite_lanes = rh->getRightOppositeLanelets(lanelet); - const auto has_left_lane = left_same_direction_lane.has_value() || !left_opposite_lanes.empty(); - 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, - p.turn_signal_shift_length_threshold)) { - return std::make_pair(TurnSignalInfo{}, true); - } - - if (!straddleRoadBound(path, shift_line, data.current_lanelets, p.vehicle_info)) { - 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 std::make_pair(turn_signal_info, false); -} } // namespace behavior_path_planner::utils::avoidance