Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

fix(turn_signal, lane_change, goal_planner): add optional to tackle lane change turn signal and pull over turn signal #1468

Merged
merged 2 commits into from
Aug 16, 2024
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Original file line number Diff line number Diff line change
Expand Up @@ -1880,6 +1880,8 @@ TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo()
constexpr bool is_driving_forward = true;

constexpr bool is_pull_out = false;
constexpr bool is_lane_change = false;
constexpr bool is_pull_over = true;
const bool override_ego_stopped_check = std::invoke([&]() {
if (thread_safe_data_.getPullOverPlannerType() == PullOverPlannerType::SHIFT) {
return false;
Expand All @@ -1895,7 +1897,7 @@ TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo()

const auto [new_signal, is_ignore] = planner_data_->getBehaviorTurnSignalInfo(
path, shift_start_idx, shift_end_idx, current_lanes, current_shift_length, is_driving_forward,
egos_lane_is_shifted, override_ego_stopped_check, is_pull_out);
egos_lane_is_shifted, override_ego_stopped_check, is_pull_out, is_lane_change, is_pull_over);
ignore_signal_ = update_ignore_signal(closest_lanelet.id(), is_ignore);

return new_signal;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -578,10 +578,12 @@ TurnSignalInfo NormalLaneChange::updateOutputTurnSignal() const
// The getBehaviorTurnSignalInfo method expects the shifted line to be generated off of the ego's
// current lane, lane change is different, so we set this flag to false.
constexpr bool egos_lane_is_shifted = false;
constexpr bool is_pull_out = false;
constexpr bool is_lane_change = true;

const auto [new_signal, is_ignore] = planner_data_->getBehaviorTurnSignalInfo(
shift_path, shift_line, current_lanes, current_shift_length, is_driving_forward,
egos_lane_is_shifted);
egos_lane_is_shifted, is_pull_out, is_lane_change);
return new_signal;
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -174,7 +174,8 @@ struct PlannerData
const PathWithLaneId & path, const size_t shift_start_idx, const size_t shift_end_idx,
const lanelet::ConstLanelets & current_lanelets, const double current_shift_length,
const bool is_driving_forward, const bool egos_lane_is_shifted,
const bool override_ego_stopped_check = false, const bool is_pull_out = false) const
const bool override_ego_stopped_check = false, const bool is_pull_out = false,
const bool is_lane_change = false, const bool is_pull_over = false) const
{
if (shift_start_idx + 1 > path.points.size()) {
RCLCPP_WARN(rclcpp::get_logger(__func__), "index inconsistency.");
Expand Down Expand Up @@ -211,7 +212,7 @@ struct PlannerData
return turn_signal_decider.getBehaviorTurnSignalInfo(
shifted_path, shift_line, current_lanelets, route_handler, parameters, self_odometry,
current_shift_length, is_driving_forward, egos_lane_is_shifted, override_ego_stopped_check,
is_pull_out);
is_pull_out, is_lane_change, is_pull_over);
}

std::pair<TurnSignalInfo, bool> getBehaviorTurnSignalInfo(
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -134,7 +134,8 @@ class TurnSignalDecider
const BehaviorPathPlannerParameters & parameters, const Odometry::ConstSharedPtr self_odometry,
const double current_shift_length, const bool is_driving_forward,
const bool egos_lane_is_shifted, const bool override_ego_stopped_check = false,
const bool is_pull_out = false) const;
const bool is_pull_out = false, const bool is_lane_change = false,
const bool is_pull_over = false) const;

private:
std::optional<TurnSignalInfo> getIntersectionTurnSignalInfo(
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -652,7 +652,8 @@ std::pair<TurnSignalInfo, bool> TurnSignalDecider::getBehaviorTurnSignalInfo(
const std::shared_ptr<RouteHandler> route_handler,
const BehaviorPathPlannerParameters & parameters, const Odometry::ConstSharedPtr self_odometry,
const double current_shift_length, const bool is_driving_forward, const bool egos_lane_is_shifted,
const bool override_ego_stopped_check, const bool is_pull_out) const
const bool override_ego_stopped_check, const bool is_pull_out, const bool is_lane_change,
const bool is_pull_over) const
{
using autoware::universe_utils::getPose;

Expand Down Expand Up @@ -770,15 +771,18 @@ std::pair<TurnSignalInfo, bool> TurnSignalDecider::getBehaviorTurnSignalInfo(
right_same_direction_lane.has_value() || !right_opposite_lanes.empty();

if (
!is_pull_out && !existShiftSideLane(
start_shift_length, end_shift_length, !has_left_lane, !has_right_lane,
p.turn_signal_shift_length_threshold)) {
(!is_pull_out && !is_lane_change && !is_pull_over) &&
!existShiftSideLane(
start_shift_length, end_shift_length, !has_left_lane, !has_right_lane,
p.turn_signal_shift_length_threshold)) {
return std::make_pair(TurnSignalInfo(p_path_start, p_path_end), true);
}

// Check if the ego will cross lane bounds.
// Note that pull out requires blinkers, even if the ego does not cross lane bounds
if (!is_pull_out && !straddleRoadBound(path, shift_line, current_lanelets, p.vehicle_info)) {
if (
(!is_pull_out && !is_pull_over) &&
!straddleRoadBound(path, shift_line, current_lanelets, p.vehicle_info)) {
return std::make_pair(TurnSignalInfo(p_path_start, p_path_end), true);
}

Expand Down
Loading