diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp index ac854f6e957ab..e5bd2abb8a07c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp @@ -108,7 +108,7 @@ 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( @@ -116,6 +116,8 @@ class NormalLaneChange : public LaneChangeBase int getNumToPreferredLane(const lanelet::ConstLanelet & lane) const override; + TurnSignalInfo get_terminal_turn_signal_info() const final; + std::vector sampleLongitudinalAccValues( const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) const; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp index 3203745367c36..3c3ba8607e021 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp @@ -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 { @@ -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; @@ -251,6 +251,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_{}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp index fe1783b103a54..2351a14249c2f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp @@ -187,81 +187,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 @@ -325,14 +308,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();