Skip to content

Commit

Permalink
fix(lane_change): activate turn signal as soon as we have the intenti…
Browse files Browse the repository at this point in the history 
…on to change lanes (autowarefoundation#8571)

* modify lane change requested condition

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>

* modify lane change requested condition

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* Update planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp

Co-authored-by: mkquda <168697710+mkquda@users.noreply.github.com>

* style(pre-commit): autofix

* fix docstring

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* modify LC turn signal logic

Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>

* Update planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp

Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com>

* minor change

Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>

---------

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>
Co-authored-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
  • Loading branch information
@mkquda @zulfaqar-azmi-t4
@pre-commit-ci @a-maumau
4 people authored and a-maumau committed Sep 2, 2024
1 parent 30c313a commit 97ff1f2
Show file tree
Hide file tree
Showing 5 changed files with 73 additions and 62 deletions.
4 changes: 3 additions & 1 deletion ...avior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -109,14 +109,16 @@ class NormalLaneChange : public LaneChangeBase

bool isStoppedAtRedTrafficLight() const override;

TurnSignalInfo get_current_turn_signal_info() override;
TurnSignalInfo get_current_turn_signal_info() const final;

protected:
lanelet::ConstLanelets getLaneChangeLanes(
const lanelet::ConstLanelets & current_lanes, Direction direction) const override;

int getNumToPreferredLane(const lanelet::ConstLanelet & lane) const override;

TurnSignalInfo get_terminal_turn_signal_info() const final;

std::vector<double> sampleLongitudinalAccValues(
const lanelet::ConstLanelets & current_lanes,
const lanelet::ConstLanelets & target_lanes) const;
Expand Down
29 changes: 27 additions & 2 deletions ...lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -213,7 +213,7 @@ class LaneChangeBase

LaneChangeModuleType getModuleType() const { return type_; }

TurnSignalDecider getTurnSignalDecider() { return planner_data_->turn_signal_decider; }
TurnSignalDecider getTurnSignalDecider() const { return planner_data_->turn_signal_decider; }

Direction getDirection() const
{
Expand All @@ -229,7 +229,7 @@ class LaneChangeBase

void resetStopPose() { lane_change_stop_pose_ = std::nullopt; }

virtual TurnSignalInfo get_current_turn_signal_info() = 0;
virtual TurnSignalInfo get_current_turn_signal_info() const = 0;

protected:
virtual int getNumToPreferredLane(const lanelet::ConstLanelet & lane) const = 0;
Expand All @@ -245,6 +245,31 @@ class LaneChangeBase
virtual lanelet::ConstLanelets getLaneChangeLanes(
const lanelet::ConstLanelets & current_lanes, Direction direction) const = 0;

virtual TurnSignalInfo get_terminal_turn_signal_info() const = 0;

TurnSignalInfo get_turn_signal(const Pose & start, const Pose & end) const
{
TurnSignalInfo turn_signal;
switch (direction_) {
case Direction::LEFT:
turn_signal.turn_signal.command = TurnIndicatorsCommand::ENABLE_LEFT;
break;
case Direction::RIGHT:
turn_signal.turn_signal.command = TurnIndicatorsCommand::ENABLE_RIGHT;
break;
default:
turn_signal.turn_signal.command = TurnIndicatorsCommand::NO_COMMAND;
break;
}

turn_signal.desired_start_point = start;
turn_signal.desired_end_point = end;
turn_signal.required_start_point = turn_signal.desired_start_point;
turn_signal.required_end_point = turn_signal.desired_end_point;

return turn_signal;
}

LaneChangeStatus status_{};
PathShifter path_shifter_{};

Expand Down
2 changes: 1 addition & 1 deletion ...ane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -73,7 +73,7 @@ double calc_stopping_distance(const LCParamPtr & lc_param_ptr);
* @return distance to last fit width position along the lane
*/
double calc_dist_to_last_fit_width(
const lanelet::ConstLanelets lanelets, const Pose & src_pose,
const lanelet::ConstLanelets & lanelets, const Pose & src_pose,
const BehaviorPathPlannerParameters & bpp_param, const double margin = 0.1);

/**
Expand Down
98 changes: 41 additions & 57 deletions planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -181,81 +181,64 @@ bool NormalLaneChange::isStoppedAtRedTrafficLight() const
status_.lane_change_path.info.length.sum());
}

TurnSignalInfo NormalLaneChange::get_current_turn_signal_info()
TurnSignalInfo NormalLaneChange::get_current_turn_signal_info() const
{
const auto original_turn_signal_info = prev_module_output_.turn_signal_info;

const auto & current_lanes = get_current_lanes();
const auto is_valid = getLaneChangeStatus().is_valid_path;
const auto & lane_change_path = getLaneChangeStatus().lane_change_path;
const auto & lane_change_param = getLaneChangeParam();

if (getModuleType() != LaneChangeModuleType::NORMAL || current_lanes.empty() || !is_valid) {
if (getModuleType() != LaneChangeModuleType::NORMAL || get_current_lanes().empty()) {
return original_turn_signal_info;
}

// check direction
TurnSignalInfo current_turn_signal_info;
const auto & current_pose = getEgoPose();
const auto direction = getDirection();
if (direction == Direction::LEFT) {
current_turn_signal_info.turn_signal.command = TurnIndicatorsCommand::ENABLE_LEFT;
} else if (direction == Direction::RIGHT) {
current_turn_signal_info.turn_signal.command = TurnIndicatorsCommand::ENABLE_RIGHT;
if (direction_ != Direction::LEFT && direction_ != Direction::RIGHT) {
return original_turn_signal_info;
}

const auto & path = prev_module_output_.path;
if (path.points.empty()) {
current_turn_signal_info.desired_start_point = current_pose;
current_turn_signal_info.required_start_point = current_pose;
current_turn_signal_info.desired_end_point = lane_change_path.info.lane_changing_end;
current_turn_signal_info.required_end_point = lane_change_path.info.lane_changing_end;
return current_turn_signal_info;
const auto & original_command = original_turn_signal_info.turn_signal.command;
if (
!path.points.empty() && original_command != TurnIndicatorsCommand::DISABLE &&
original_command != TurnIndicatorsCommand::NO_COMMAND) {
return get_terminal_turn_signal_info();
}

const auto min_length_for_turn_signal_activation =
lane_change_param.min_length_for_turn_signal_activation;
const auto & route_handler = getRouteHandler();
const auto & common_parameter = getCommonParam();
return get_turn_signal(getEgoPose(), getLaneChangePath().info.lane_changing_end);
}

TurnSignalInfo NormalLaneChange::get_terminal_turn_signal_info() const
{
const auto & lane_change_param = getLaneChangeParam();
const auto & common_param = getCommonParam();
const auto & current_pose = getEgoPose();
const auto & path = prev_module_output_.path;

const auto original_turn_signal_info = prev_module_output_.turn_signal_info;

const auto shift_intervals =
route_handler->getLateralIntervalsToPreferredLane(current_lanes.back());
getRouteHandler()->getLateralIntervalsToPreferredLane(get_current_lanes().back());
const double next_lane_change_buffer =
utils::lane_change::calcMinimumLaneChangeLength(lane_change_param, shift_intervals);
const double nearest_dist_threshold = common_parameter.ego_nearest_dist_threshold;
const double nearest_yaw_threshold = common_parameter.ego_nearest_yaw_threshold;
const double base_to_front = common_parameter.base_link2front;

const double buffer =
next_lane_change_buffer + min_length_for_turn_signal_activation + base_to_front;
const double buffer = next_lane_change_buffer +
lane_change_param.min_length_for_turn_signal_activation +
common_param.base_link2front;
const double path_length = autoware::motion_utils::calcArcLength(path.points);
const size_t current_nearest_seg_idx =
autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints(
path.points, current_pose, nearest_dist_threshold, nearest_yaw_threshold);
const double dist_to_terminal = utils::getDistanceToEndOfLane(current_pose, current_lanes);
const auto start_pose = autoware::motion_utils::calcLongitudinalOffsetPose(
path.points, 0, std::max(path_length - buffer, 0.0));
if (dist_to_terminal - base_to_front < buffer && start_pose) {
// modify turn signal
current_turn_signal_info.desired_start_point = *start_pose;
current_turn_signal_info.desired_end_point = lane_change_path.info.lane_changing_end;
current_turn_signal_info.required_start_point = current_turn_signal_info.desired_start_point;
current_turn_signal_info.required_end_point = current_turn_signal_info.desired_end_point;

const auto & original_command = original_turn_signal_info.turn_signal.command;
if (
original_command == TurnIndicatorsCommand::DISABLE ||
original_command == TurnIndicatorsCommand::NO_COMMAND) {
return current_turn_signal_info;
}

// check the priority of turn signals
return getTurnSignalDecider().overwrite_turn_signal(
path, current_pose, current_nearest_seg_idx, original_turn_signal_info,
current_turn_signal_info, nearest_dist_threshold, nearest_yaw_threshold);
}
if (!start_pose) return original_turn_signal_info;

const auto terminal_turn_signal_info =
get_turn_signal(*start_pose, getLaneChangePath().info.lane_changing_end);

const double nearest_dist_threshold = common_param.ego_nearest_dist_threshold;
const double nearest_yaw_threshold = common_param.ego_nearest_yaw_threshold;
const size_t current_nearest_seg_idx =
autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints(
path.points, current_pose, nearest_dist_threshold, nearest_yaw_threshold);

// not in the vicinity of the end of the path. return original
return original_turn_signal_info;
return getTurnSignalDecider().overwrite_turn_signal(
path, current_pose, current_nearest_seg_idx, original_turn_signal_info,
terminal_turn_signal_info, nearest_dist_threshold, nearest_yaw_threshold);
}

LaneChangePath NormalLaneChange::getLaneChangePath() const
Expand Down Expand Up @@ -319,14 +302,15 @@ BehaviorModuleOutput NormalLaneChange::generateOutput()
output.path = abort_path_->path;
insertStopPoint(get_current_lanes(), output.path);
} else {
output.path = getLaneChangePath().path;
output.path = status_.lane_change_path.path;

const auto found_extended_path = extendPath();
if (found_extended_path) {
output.path = utils::combinePath(output.path, *found_extended_path);
}
output.reference_path = getReferencePath();
output.turn_signal_info = updateOutputTurnSignal();
output.turn_signal_info =
get_turn_signal(getEgoPose(), status_.lane_change_path.info.lane_changing_end);

if (isStopState()) {
const auto current_velocity = getEgoVelocity();
Expand Down
2 changes: 1 addition & 1 deletion ...behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -58,7 +58,7 @@ double calc_stopping_distance(const LCParamPtr & lc_param_ptr)
}

double calc_dist_to_last_fit_width(
const lanelet::ConstLanelets lanelets, const Pose & src_pose,
const lanelet::ConstLanelets & lanelets, const Pose & src_pose,
const BehaviorPathPlannerParameters & bpp_param, const double margin)
{
if (lanelets.empty()) return 0.0;
Expand Down

0 comments on commit 97ff1f2

Please sign in to comment.