diff --git a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml index d00a44c244815..2e55ab47ba75b 100644 --- a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml +++ b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml @@ -6,6 +6,7 @@ backward_length_buffer_for_end_of_lane: 3.0 # [m] lane_change_finish_judge_buffer: 2.0 # [m] + min_length_for_turn_signal_activation: 10.0 #[m] lane_changing_lateral_jerk: 0.5 # [m/s3] lateral_acc_switching_velocity: 4.0 #[m/s] diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp index 67aa9b3bfb5ea..61584ed06cd8b 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp @@ -137,6 +137,8 @@ class LaneChangeBase const BehaviorPathPlannerParameters & getCommonParam() const { return planner_data_->parameters; } + LaneChangeParameters getLaneChangeParam() const { return *lane_change_parameters_; } + bool isCancelEnabled() const { return lane_change_parameters_->enable_cancel_lane_change; } bool isAbortEnabled() const { return lane_change_parameters_->enable_abort_lane_change; } @@ -165,6 +167,10 @@ class LaneChangeBase std::string getModuleTypeStr() const { return std::string{magic_enum::enum_name(type_)}; } + LaneChangeModuleType getModuleType() const { return type_; } + + TurnSignalDecider getTurnSignalDecider() { return planner_data_->turn_signal_decider; } + Direction getDirection() const { if (direction_ == Direction::NONE && !status_.lane_change_path.path.points.empty()) { diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp index 70e4241458d53..fa7011bcdce27 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp @@ -93,6 +93,9 @@ class LaneChangeInterface : public SceneModuleInterface MarkerArray getModuleVirtualWall() override; + TurnSignalInfo getCurrentTurnSignalInfo( + const PathWithLaneId & path, const TurnSignalInfo & original_turn_signal_info); + protected: std::shared_ptr parameters_; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp index faa231d2930cd..d1c4482f0aff9 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp @@ -35,6 +35,9 @@ struct LaneChangeParameters int longitudinal_acc_sampling_num{10}; int lateral_acc_sampling_num{10}; + // turn signal + double min_length_for_turn_signal_activation{10.0}; + // acceleration data double min_longitudinal_acc{-1.0}; double max_longitudinal_acc{1.0}; diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 539cb8cd52399..102ddac21edf8 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -778,6 +778,10 @@ LaneChangeParameters BehaviorPathPlannerNode::getLaneChangeParam() p.lateral_acc_sampling_num = declare_parameter(parameter("lateral_acceleration_sampling_num")); + // min length + p.min_length_for_turn_signal_activation = + declare_parameter(parameter("min_length_for_turn_signal_activation")); + // acceleration p.min_longitudinal_acc = declare_parameter(parameter("min_longitudinal_acc")); p.max_longitudinal_acc = declare_parameter(parameter("max_longitudinal_acc")); diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp index bcf4cf77543b4..536dee15252de 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp @@ -249,6 +249,9 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() getPreviousModuleOutput().reference_path, getPreviousModuleOutput().path); module_type_->updateLaneChangeStatus(); + // change turn signal when the vehicle reaches at the end of the path for waiting lane change + out.turn_signal_info = getCurrentTurnSignalInfo(*out.path, out.turn_signal_info); + if (!module_type_->isValidPath()) { path_candidate_ = std::make_shared(); return out; @@ -407,6 +410,84 @@ void LaneChangeInterface::acceptVisitor(const std::shared_ptrgetLaneChangeStatus().lane_change_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 || target_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.lane_changing_end; + current_turn_signal_info.required_end_point = lane_change_path.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(target_lanes.back()); + const double next_lane_change_buffer = + utils::calcMinimumLaneChangeLength(common_parameter, 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 buffer = next_lane_change_buffer + min_length_for_turn_signal_activation; + const double path_length = motion_utils::calcArcLength(path.points); + const auto & front_point = path.points.front().point.pose.position; + const size_t & current_nearest_seg_idx = + motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, nearest_dist_threshold, nearest_yaw_threshold); + const double length_front_to_ego = motion_utils::calcSignedArcLength( + path.points, front_point, static_cast(0), current_pose.position, + current_nearest_seg_idx); + const auto start_pose = + motion_utils::calcLongitudinalOffsetPose(path.points, 0, std::max(path_length - buffer, 0.0)); + if (path_length - length_front_to_ego < 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.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; +} + void SceneModuleVisitor::visitLaneChangeInterface(const LaneChangeInterface * interface) const { lane_change_visitor_ = interface->get_debug_msg_array();