Skip to content

Commit

Permalink
refactor(lane_change): move getCurrentTurnSignalInfo to scene file
Browse files Browse the repository at this point in the history 
Signed-off-by: Zulfaqar Azmi <[email protected]>
  • Loading branch information
@zulfaqar-azmi-t4
zulfaqar-azmi-t4 committed May 8, 2024
1 parent edc7878 commit ecfcd6b
Show file tree
Hide file tree
Showing 5 changed files with 82 additions and 82 deletions.
3 changes: 0 additions & 3 deletions ...g/behavior_path_lane_change_module/include/behavior_path_lane_change_module/interface.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -93,9 +93,6 @@ class LaneChangeInterface : public SceneModuleInterface

MarkerArray getModuleVirtualWall() override;

TurnSignalInfo getCurrentTurnSignalInfo(
const PathWithLaneId & path, const TurnSignalInfo & original_turn_signal_info);

// TODO(someone): remove this, and use base class function
[[deprecated]] BehaviorModuleOutput run() override
{
Expand Down
2 changes: 2 additions & 0 deletions planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -101,6 +101,8 @@ class NormalLaneChange : public LaneChangeBase

bool isStoppedAtRedTrafficLight() const override;

TurnSignalInfo get_current_turn_signal_info() override;

protected:
lanelet::ConstLanelets getCurrentLanes() const override;

Expand Down
2 changes: 2 additions & 0 deletions ...ior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -187,6 +187,8 @@ class LaneChangeBase

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

virtual TurnSignalInfo get_current_turn_signal_info() = 0;

protected:
virtual lanelet::ConstLanelets getCurrentLanes() const = 0;

Expand Down
80 changes: 1 addition & 79 deletions planning/behavior_path_lane_change_module/src/interface.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -135,8 +135,7 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval()

BehaviorModuleOutput out = getPreviousModuleOutput();
module_type_->insertStopPoint(module_type_->getLaneChangeStatus().current_lanes, out.path);
out.turn_signal_info =
getCurrentTurnSignalInfo(out.path, getPreviousModuleOutput().turn_signal_info);
out.turn_signal_info = module_type_->get_current_turn_signal_info();

const auto & lane_change_debug = module_type_->getDebugData();
for (const auto & [uuid, data] : lane_change_debug.collision_check_objects) {
Expand Down Expand Up @@ -367,81 +366,4 @@ void LaneChangeInterface::updateSteeringFactorPtr(
{output.start_distance_to_path_change, output.finish_distance_to_path_change},
PlanningBehavior::LANE_CHANGE, steering_factor_direction, SteeringFactor::APPROACHING, "");
}

TurnSignalInfo LaneChangeInterface::getCurrentTurnSignalInfo(
const PathWithLaneId & path, const TurnSignalInfo & original_turn_signal_info)
{
const auto & current_lanes = module_type_->getLaneChangeStatus().current_lanes;
const auto & is_valid = module_type_->getLaneChangeStatus().is_valid_path;
const auto & lane_change_path = module_type_->getLaneChangeStatus().lane_change_path;
const auto & lane_change_param = module_type_->getLaneChangeParam();

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

// check direction
TurnSignalInfo current_turn_signal_info;
const auto & current_pose = module_type_->getEgoPose();
const auto direction = module_type_->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 (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 & min_length_for_turn_signal_activation =
lane_change_param.min_length_for_turn_signal_activation;
const auto & route_handler = module_type_->getRouteHandler();
const auto & common_parameter = module_type_->getCommonParam();
const auto shift_intervals =
route_handler->getLateralIntervalsToPreferredLane(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 path_length = motion_utils::calcArcLength(path.points);
const size_t & current_nearest_seg_idx =
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 =
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 module_type_->getTurnSignalDecider().use_prior_turn_signal(
path, current_pose, current_nearest_seg_idx, original_turn_signal_info,
current_turn_signal_info, nearest_dist_threshold, nearest_yaw_threshold);
}

// not in the vicinity of the end of the path. return original
return original_turn_signal_info;
}
} // namespace behavior_path_planner
77 changes: 77 additions & 0 deletions planning/behavior_path_lane_change_module/src/scene.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -136,6 +136,83 @@ bool NormalLaneChange::isStoppedAtRedTrafficLight() const
status_.lane_change_path.info.length.sum());
}

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

const auto & current_lanes = getLaneChangeStatus().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) {
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;
}

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 & 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();
const auto shift_intervals =
route_handler->getLateralIntervalsToPreferredLane(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 path_length = motion_utils::calcArcLength(path.points);
const size_t & current_nearest_seg_idx =
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 =
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().use_prior_turn_signal(
path, current_pose, current_nearest_seg_idx, original_turn_signal_info,
current_turn_signal_info, nearest_dist_threshold, nearest_yaw_threshold);
}

// not in the vicinity of the end of the path. return original
return original_turn_signal_info;
}

LaneChangePath NormalLaneChange::getLaneChangePath() const
{
return status_.lane_change_path;
Expand Down

0 comments on commit ecfcd6b

Please sign in to comment.