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.

Sign up for GitHub

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(behavior_path_planner): fix the condition of turn signal lighting #728

Merged
merged 4 commits into from
Apr 27, 2022
Merged

fix(behavior_path_planner): fix the condition of turn signal lighting #728

Changes from 3 commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
e5654a9
fix(behavior_path_planner): fix the condition of turn signal lighting
k-obitsu Apr 19, 2022
93756d5
fix(behavior_path_planner): fix the condition of turn signal lighting
k-obitsu Apr 25, 2022
86af502
ci(pre-commit): autofix
pre-commit-ci[bot] Apr 25, 2022
48d03a7
Update planning/behavior_path_planner/src/turn_signal_decider.cpp
k-obitsu Apr 27, 2022
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
50 changes: 27 additions & 23 deletions planning/behavior_path_planner/src/turn_signal_decider.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -67,42 +67,46 @@ std::pair<TurnIndicatorsCommand, double> TurnSignalDecider::getIntersectionTurnS
double accumulated_distance = 0;

auto prev_point = path.points.front();
auto prev_lane_id = lanelet::InvalId;
auto lane_attribute = std::string("none");
for (const auto & path_point : path.points) {
accumulated_distance +=
const double path_point_distance =
tier4_autoware_utils::calcDistance3d(prev_point.point, path_point.point);
accumulated_distance += path_point_distance;
prev_point = path_point;
const double distance_from_vehicle_front =
accumulated_distance - vehicle_pose_frenet.length - base_link2front_;
if (distance_from_vehicle_front < 0.0) {
continue;
if (distance_from_vehicle_front > intersection_search_distance_) {
return std::make_pair(turn_signal, distance);
}
// TODO(Horibe): Route Handler should be a library.
for (const auto & lane : route_handler.getLaneletsFromIds(path_point.lane_ids)) {
if (lane.id() == prev_lane_id) {
continue;
// judgement of lighting of turn_signal
bool lighting_turn_signal = false;
if (lane.attributeOr("turn_direction", std::string("none")) != lane_attribute) {
if (
distance_from_vehicle_front <
lane.attributeOr("turn_signal_distance", intersection_search_distance_) &&
path_point_distance > 0.0) {
rej55 marked this conversation as resolved.
Show resolved Hide resolved
lighting_turn_signal = true;
}
} else {
if (
lane.hasAttribute("turn_direction") &&
distance_from_vehicle_front < path_point_distance && distance_from_vehicle_front > 0) {
lighting_turn_signal = true;
}
}
prev_lane_id = lane.id();
lane_attribute = lane.attributeOr("turn_direction", std::string("none"));

if (
lane.attributeOr("turn_signal_distance", std::numeric_limits<double>::max()) <
distance_from_vehicle_front) {
continue;
}
if (lane.attributeOr("turn_direction", std::string("none")) == "left") {
turn_signal.command = TurnIndicatorsCommand::ENABLE_LEFT;
distance = distance_from_vehicle_front;
return std::make_pair(turn_signal, distance);
}
if (lane.attributeOr("turn_direction", std::string("none")) == "right") {
turn_signal.command = TurnIndicatorsCommand::ENABLE_RIGHT;
if (lighting_turn_signal == true) {
k-obitsu marked this conversation as resolved.
Show resolved Hide resolved
if (lane_attribute == std::string("left")) {
turn_signal.command = TurnIndicatorsCommand::ENABLE_LEFT;
} else if (lane_attribute == std::string("right")) {
turn_signal.command = TurnIndicatorsCommand::ENABLE_RIGHT;
}
distance = distance_from_vehicle_front;
return std::make_pair(turn_signal, distance);
}
}
if (distance_from_vehicle_front > intersection_search_distance_) {
return std::make_pair(turn_signal, distance);
}
}
return std::make_pair(turn_signal, distance);
}
Expand Down