From 020ec257d7862d48d8bfcee57c1f63314e23ccf6 Mon Sep 17 00:00:00 2001 From: Daniel Sanchez Date: Fri, 15 Mar 2024 18:12:40 +0900 Subject: [PATCH 1/2] Add general turnSignal method to start planner Signed-off-by: Daniel Sanchez --- .../start_planner_module.hpp | 4 +- .../src/start_planner_module.cpp | 139 +++++++----------- 2 files changed, 56 insertions(+), 87 deletions(-) diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp index 14846a05a0929..89faafe95727c 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp @@ -33,6 +33,7 @@ #include +#include #include #include @@ -239,6 +240,7 @@ class StartPlannerModule : public SceneModuleInterface std::vector> start_planners_; PullOutStatus status_; mutable StartPlannerDebugData debug_data_; + std::optional ignore_signal_{std::nullopt}; std::deque odometry_buffer_; @@ -264,7 +266,7 @@ class StartPlannerModule : public SceneModuleInterface std::shared_ptr lane_departure_checker_; // turn signal - TurnSignalInfo calcTurnSignalInfo() const; + TurnSignalInfo calcTurnSignalInfo(); void incrementPathIndex(); PathWithLaneId getCurrentPath() const; diff --git a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp index bb47182d953cc..70568742aaa22 100644 --- a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp @@ -1215,103 +1215,70 @@ bool StartPlannerModule::hasFinishedCurrentPath() return is_near_target && isStopped(); } -TurnSignalInfo StartPlannerModule::calcTurnSignalInfo() const +TurnSignalInfo StartPlannerModule::calcTurnSignalInfo() { - TurnSignalInfo turn_signal{}; // output + const auto path = getFullPath(); + if (path.points.empty()) return getPreviousModuleOutput().turn_signal_info; const Pose & current_pose = planner_data_->self_odometry->pose.pose; - const Pose & start_pose = status_.pull_out_path.start_pose; - const Pose & end_pose = status_.pull_out_path.end_pose; - - // turn on hazard light when backward driving - if (!status_.driving_forward) { - turn_signal.hazard_signal.command = HazardLightsCommand::ENABLE; - const auto back_start_pose = planner_data_->route_handler->getOriginalStartPose(); - turn_signal.desired_start_point = back_start_pose; - turn_signal.required_start_point = back_start_pose; - // pull_out start_pose is same to backward driving end_pose - turn_signal.required_end_point = start_pose; - turn_signal.desired_end_point = start_pose; - return turn_signal; - } - - // turn on right signal until passing pull_out end point - const auto path = getFullPath(); - // pull out path does not overlap - const double distance_from_end = - motion_utils::calcSignedArcLength(path.points, end_pose.position, current_pose.position); + const auto shift_start_idx = + motion_utils::findNearestIndex(path.points, status_.pull_out_path.start_pose.position); + const auto shift_end_idx = + motion_utils::findNearestIndex(path.points, status_.pull_out_path.end_pose.position); + const lanelet::ConstLanelets current_lanes = utils::getCurrentLanes(planner_data_); + + const auto is_ignore_signal = [this](const lanelet::Id & id) { + if (!ignore_signal_.has_value()) { + return false; + } + return ignore_signal_.value() == id; + }; - if (path.points.empty()) { - return {}; + const auto update_ignore_signal = [this](const lanelet::Id & id, const bool is_ignore) { + return is_ignore ? std::make_optional(id) : std::nullopt; + }; + + lanelet::Lanelet closest_lanelet; + lanelet::utils::query::getClosestLanelet(current_lanes, current_pose, &closest_lanelet); + + if (is_ignore_signal(closest_lanelet.id())) { + return getPreviousModuleOutput().turn_signal_info; } - // calculate lateral offset from pull out target lane center line - lanelet::ConstLanelet closest_road_lane; - const double backward_path_length = - planner_data_->parameters.backward_path_length + parameters_->max_back_distance; - const auto road_lanes = utils::getExtendedCurrentLanes( - planner_data_, backward_path_length, std::numeric_limits::max(), - /*forward_only_in_route*/ true); - lanelet::utils::query::getClosestLanelet(road_lanes, start_pose, &closest_road_lane); - const double lateral_offset = - lanelet::utils::getLateralDistanceToCenterline(closest_road_lane, start_pose); - - turn_signal.turn_signal.command = std::invoke([&]() { - if (distance_from_end >= 0.0) return TurnIndicatorsCommand::DISABLE; - if (lateral_offset > parameters_->th_turn_signal_on_lateral_offset) - return TurnIndicatorsCommand::ENABLE_RIGHT; - if (lateral_offset < -parameters_->th_turn_signal_on_lateral_offset) - return TurnIndicatorsCommand::ENABLE_LEFT; - return TurnIndicatorsCommand::DISABLE; - }); + const double current_shift_length = + lanelet::utils::getArcCoordinates(current_lanes, current_pose).distance; - turn_signal.desired_start_point = start_pose; - turn_signal.required_start_point = start_pose; - turn_signal.desired_end_point = end_pose; - - // check if intersection exists within search length - const bool is_near_intersection = std::invoke([&]() { - const double check_length = parameters_->intersection_search_length; - double accumulated_length = 0.0; - const size_t current_idx = motion_utils::findNearestIndex(path.points, current_pose.position); - for (size_t i = current_idx; i < path.points.size() - 1; ++i) { - const auto & p = path.points.at(i); - for (const auto & lane : planner_data_->route_handler->getLaneletsFromIds(p.lane_ids)) { - const std::string turn_direction = lane.attributeOr("turn_direction", "else"); - if (turn_direction == "right" || turn_direction == "left" || turn_direction == "straight") { - return true; - } - } - accumulated_length += tier4_autoware_utils::calcDistance2d(p, path.points.at(i + 1)); - if (accumulated_length > check_length) { - return false; - } - } - return false; - }); + constexpr bool egos_lane_is_shifted = true; + constexpr bool is_pull_out = true; - turn_signal.required_end_point = std::invoke([&]() { - if (is_near_intersection) return end_pose; - const double length_start_to_end = - motion_utils::calcSignedArcLength(path.points, start_pose.position, end_pose.position); - const auto ratio = std::clamp( - parameters_->length_ratio_for_turn_signal_deactivation_near_intersection, 0.0, 1.0); - - const double required_end_length = length_start_to_end * ratio; - double accumulated_length = 0.0; - const size_t start_idx = motion_utils::findNearestIndex(path.points, start_pose.position); - for (size_t i = start_idx; i < path.points.size() - 1; ++i) { - accumulated_length += - tier4_autoware_utils::calcDistance2d(path.points.at(i), path.points.at(i + 1)); - if (accumulated_length > required_end_length) { - return path.points.at(i).point.pose; - } + // In Geometric pull out, the ego stops once and then steers the wheels to the opposite direction. + // This sometimes causes the getBehaviorTurnSignalInfo method to detect the ego as stopped and + // close to complete its shift, so it wrongly turns off the blinkers, this override helps avoid + // this issue. + const bool override_ego_stopped_check = std::invoke([&]() { + if (status_.planner_type != PlannerType::GEOMETRIC) { + return false; } - // not found required end point - return end_pose; + constexpr double distance_threshold = 1.0; + const auto stop_point = status_.pull_out_path.partial_paths.front().points.back(); + const double distance_from_ego_to_stop_point = std::abs(motion_utils::calcSignedArcLength( + path.points, stop_point.point.pose.position, current_pose.position)); + return distance_from_ego_to_stop_point < distance_threshold; }); - return turn_signal; + const auto [new_signal, is_ignore] = planner_data_->getBehaviorTurnSignalInfo( + path, shift_start_idx, shift_end_idx, current_lanes, current_shift_length, + status_.driving_forward, egos_lane_is_shifted, override_ego_stopped_check, is_pull_out); + ignore_signal_ = update_ignore_signal(closest_lanelet.id(), is_ignore); + + const auto original_signal = getPreviousModuleOutput().turn_signal_info; + const auto current_seg_idx = planner_data_->findEgoSegmentIndex(path.points); + const auto output_turn_signal_info = planner_data_->turn_signal_decider.use_prior_turn_signal( + path, current_pose, current_seg_idx, original_signal, new_signal, + planner_data_->parameters.ego_nearest_dist_threshold, + planner_data_->parameters.ego_nearest_yaw_threshold); + + return output_turn_signal_info; } bool StartPlannerModule::isSafePath() const From 419c9e108a6568bfb3cfa76138f6f738614157bb Mon Sep 17 00:00:00 2001 From: Daniel Sanchez Date: Mon, 18 Mar 2024 09:38:05 +0900 Subject: [PATCH 2/2] add description to ignore signal Signed-off-by: Daniel Sanchez --- .../start_planner_module.hpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp index 89faafe95727c..a30d7c379f80c 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp @@ -240,6 +240,9 @@ class StartPlannerModule : public SceneModuleInterface std::vector> start_planners_; PullOutStatus status_; mutable StartPlannerDebugData debug_data_; + + // Keeps track of lanelets that should be ignored when calculating the turnSignalInfo for this + // module's output. If the ego vehicle is in this lanelet, the calculation is skipped. std::optional ignore_signal_{std::nullopt}; std::deque odometry_buffer_;