Skip to content

Commit

Permalink
fix(bpp): overwrite turn signal by latter module (autowarefoundation#…
Browse files Browse the repository at this point in the history
…7045)

* fix(bpp): overwrite turn signal by latter module

Signed-off-by: satoshi-ota <[email protected]>

* fix(avoidance): return previous module turn signal if it's in success state

Signed-off-by: satoshi-ota <[email protected]>

* fix(avoidance): don't output turn signal if there is huge lateral deveation

Signed-off-by: satoshi-ota <[email protected]>

* refactor: small update

Signed-off-by: satoshi-ota <[email protected]>

* chore: use lower case

Signed-off-by: satoshi-ota <[email protected]>

* refactor: remove redundant function call

Signed-off-by: satoshi-ota <[email protected]>

---------

Signed-off-by: satoshi-ota <[email protected]>
  • Loading branch information
satoshi-ota authored Jun 5, 2024
1 parent 65329cf commit f1dae5e
Show file tree
Hide file tree
Showing 2 changed files with 63 additions and 8 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -64,6 +64,17 @@ struct TurnSignalInfo
hazard_signal.command = HazardLightsCommand::NO_COMMAND;
}

TurnSignalInfo(const Pose & start, const Pose & end)
{
turn_signal.command = TurnIndicatorsCommand::NO_COMMAND;
hazard_signal.command = HazardLightsCommand::NO_COMMAND;

desired_start_point = start;
desired_end_point = end;
required_start_point = start;
required_end_point = end;
}

// desired turn signal
TurnIndicatorsCommand turn_signal;
HazardLightsCommand hazard_signal;
Expand Down Expand Up @@ -93,6 +104,11 @@ class TurnSignalDecider
const TurnSignalInfo & intersection_signal_info, const TurnSignalInfo & behavior_signal_info,
const double nearest_dist_threshold, const double nearest_yaw_threshold);

TurnSignalInfo overwrite_turn_signal(
const PathWithLaneId & path, const Pose & current_pose, const size_t current_seg_idx,
const TurnSignalInfo & original_signal, const TurnSignalInfo & new_signal,
const double nearest_dist_threshold, const double nearest_yaw_threshold) const;

TurnSignalInfo use_prior_turn_signal(
const PathWithLaneId & path, const Pose & current_pose, const size_t current_seg_idx,
const TurnSignalInfo & original_signal, const TurnSignalInfo & new_signal,
Expand Down
55 changes: 47 additions & 8 deletions planning/behavior_path_planner_common/src/turn_signal_decider.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -392,6 +392,40 @@ TurnIndicatorsCommand TurnSignalDecider::resolve_turn_signal(
return intersection_signal_info.turn_signal;
}

TurnSignalInfo TurnSignalDecider::overwrite_turn_signal(
const PathWithLaneId & path, const Pose & current_pose, const size_t current_seg_idx,
const TurnSignalInfo & original_signal, const TurnSignalInfo & new_signal,
const double nearest_dist_threshold, const double nearest_yaw_threshold) const
{
if (original_signal.turn_signal.command == TurnIndicatorsCommand::NO_COMMAND) {
return new_signal;
}

if (original_signal.turn_signal.command == TurnIndicatorsCommand::DISABLE) {
return new_signal;
}

const auto get_distance = [&](const Pose & input_point) {
const size_t nearest_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints(
path.points, input_point, nearest_dist_threshold, nearest_yaw_threshold);
return motion_utils::calcSignedArcLength(
path.points, current_pose.position, current_seg_idx, input_point.position,
nearest_seg_idx) -
base_link2front_;
};

const auto & original_desired_end_point = original_signal.desired_end_point;
const auto & new_desired_start_point = new_signal.desired_start_point;

const double dist_to_original_desired_end = get_distance(original_desired_end_point);
const double dist_to_new_desired_start = get_distance(new_desired_start_point);
if (dist_to_new_desired_start > dist_to_original_desired_end) {
return original_signal;
}

return new_signal;
}

TurnSignalInfo TurnSignalDecider::use_prior_turn_signal(
const PathWithLaneId & path, const Pose & current_pose, const size_t current_seg_idx,
const TurnSignalInfo & original_signal, const TurnSignalInfo & new_signal,
Expand Down Expand Up @@ -615,6 +649,8 @@ std::pair<TurnSignalInfo, bool> TurnSignalDecider::getBehaviorTurnSignalInfo(
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
{
using tier4_autoware_utils::getPose;

const auto & p = parameters;
const auto & rh = route_handler;
const auto & ego_pose = self_odometry->pose.pose;
Expand Down Expand Up @@ -667,16 +703,19 @@ std::pair<TurnSignalInfo, bool> TurnSignalDecider::getBehaviorTurnSignalInfo(

const auto relative_shift_length = end_shift_length - start_shift_length;

const auto p_path_start = getPose(path.path.points.front());
const auto p_path_end = getPose(path.path.points.back());

// 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);
return std::make_pair(TurnSignalInfo(p_path_start, p_path_end), true);
}

// If the vehicle does not shift anymore, we turn off the blinker
if (
std::fabs(end_shift_length - current_shift_length) <
p.turn_signal_remaining_shift_length_threshold) {
return std::make_pair(TurnSignalInfo{}, true);
return std::make_pair(TurnSignalInfo(p_path_start, p_path_end), true);
}

const auto get_command = [](const auto & shift_length) {
Expand All @@ -691,7 +730,7 @@ std::pair<TurnSignalInfo, bool> TurnSignalDecider::getBehaviorTurnSignalInfo(
p.vehicle_info.max_longitudinal_offset_m;

if (signal_prepare_distance < ego_front_to_shift_start) {
return std::make_pair(TurnSignalInfo{}, false);
return std::make_pair(TurnSignalInfo(p_path_start, p_path_end), false);
}

const auto blinker_start_pose = path.path.points.at(shift_line.start_idx).point.pose;
Expand All @@ -708,13 +747,13 @@ std::pair<TurnSignalInfo, bool> TurnSignalDecider::getBehaviorTurnSignalInfo(
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);
return std::make_pair(TurnSignalInfo(p_path_start, p_path_end), false);
}

lanelet::ConstLanelet lanelet;
const auto query_pose = (egos_lane_is_shifted) ? shift_line.end : shift_line.start;
if (!rh->getClosestLaneletWithinRoute(query_pose, &lanelet)) {
return std::make_pair(TurnSignalInfo{}, true);
return std::make_pair(TurnSignalInfo(p_path_start, p_path_end), true);
}

const auto left_same_direction_lane = rh->getLeftLanelet(lanelet, true, true);
Expand All @@ -729,13 +768,13 @@ std::pair<TurnSignalInfo, bool> TurnSignalDecider::getBehaviorTurnSignalInfo(
!is_pull_out && !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);
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)) {
return std::make_pair(TurnSignalInfo{}, true);
return std::make_pair(TurnSignalInfo(p_path_start, p_path_end), true);
}

// If the ego has stopped and its close to completing its shift, turn off the blinkers
Expand All @@ -744,7 +783,7 @@ std::pair<TurnSignalInfo, bool> TurnSignalDecider::getBehaviorTurnSignalInfo(
if (isNearEndOfShift(
start_shift_length, end_shift_length, ego_pose.position, current_lanelets,
p.turn_signal_shift_length_threshold)) {
return std::make_pair(TurnSignalInfo{}, true);
return std::make_pair(TurnSignalInfo(p_path_start, p_path_end), true);
}
}

Expand Down

0 comments on commit f1dae5e

Please sign in to comment.