From 020ec257d7862d48d8bfcee57c1f63314e23ccf6 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <danielsanchezaran@gmail.com>
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 <danielsanchezaran@gmail.com>
---
.../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 <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
+#include <lanelet2_core/Forward.h>
#include <tf2/utils.h>
#include <atomic>
@@ -239,6 +240,7 @@ class StartPlannerModule : public SceneModuleInterface
std::vector<std::shared_ptr<PullOutPlannerBase>> start_planners_;
PullOutStatus status_;
mutable StartPlannerDebugData debug_data_;
+ std::optional<lanelet::Id> ignore_signal_{std::nullopt};
std::deque<nav_msgs::msg::Odometry::ConstSharedPtr> odometry_buffer_;
@@ -264,7 +266,7 @@ class StartPlannerModule : public SceneModuleInterface
std::shared_ptr<LaneDepartureChecker> 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<double>::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 <danielsanchezaran@gmail.com>
Date: Mon, 18 Mar 2024 09:38:05 +0900
Subject: [PATCH 2/2] add description to ignore signal
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
---
.../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<std::shared_ptr<PullOutPlannerBase>> 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<lanelet::Id> ignore_signal_{std::nullopt};
std::deque<nav_msgs::msg::Odometry::ConstSharedPtr> odometry_buffer_;