From 2c87439387a065270a1da57bd945cafe427bedf0 Mon Sep 17 00:00:00 2001 From: yutaka Date: Fri, 9 Jun 2023 21:08:56 +0900 Subject: [PATCH 1/6] feat(behavior_path_planner): enable lane change to activate turn signal when stopping Signed-off-by: yutaka --- planning/behavior_path_planner/CMakeLists.txt | 2 +- .../scene_module/lane_change/base_class.hpp | 4 ++ .../scene_module/lane_change/interface.cpp | 49 +++++++++++++++++++ 3 files changed, 54 insertions(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/CMakeLists.txt b/planning/behavior_path_planner/CMakeLists.txt index f521407053e27..4463620dcc4a9 100644 --- a/planning/behavior_path_planner/CMakeLists.txt +++ b/planning/behavior_path_planner/CMakeLists.txt @@ -7,7 +7,7 @@ autoware_package() find_package(OpenCV REQUIRED) find_package(magic_enum CONFIG REQUIRED) -set(COMPILE_WITH_OLD_ARCHITECTURE TRUE) +set(COMPILE_WITH_OLD_ARCHITECTURE FALSE) set(common_src src/steering_factor_interface.cpp 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..b934a7b99d9b3 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 @@ -165,6 +165,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/src/scene_module/lane_change/interface.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp index bcf4cf77543b4..8fe0041e7bfbb 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,55 @@ 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 + const auto & target_lanes = module_type_->getLaneChangeStatus().lane_change_lanes; + if ( + !out.path->points.empty() && !target_lanes.empty() && + module_type_->getModuleType() == LaneChangeModuleType::NORMAL) { + const double minimum_lc_turn_signal_length = 10.0; + 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 + minimum_lc_turn_signal_length; + const double path_length = motion_utils::calcArcLength(out.path->points); + const auto & current_pose = module_type_->getEgoPose(); + const auto & front_point = out.path->points.front().point.pose.position; + const size_t & current_nearest_seg_idx = + motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + out.path->points, current_pose, nearest_dist_threshold, nearest_yaw_threshold); + const double length_front_to_ego = motion_utils::calcSignedArcLength( + out.path->points, front_point, static_cast(0), current_pose.position, + current_nearest_seg_idx); + const auto start_pose = motion_utils::calcLongitudinalOffsetPose( + out.path->points, 0, std::max(path_length - buffer, 0.0)); + if (path_length - length_front_to_ego < buffer && start_pose) { + // modify turn signal + const auto new_turn_signal_info = [&](const Direction & direction) { + TurnSignalInfo new_turn_signal_info; + new_turn_signal_info.desired_start_point = *start_pose; + new_turn_signal_info.desired_end_point = out.path->points.back().point.pose; + new_turn_signal_info.required_start_point = new_turn_signal_info.desired_start_point; + new_turn_signal_info.required_end_point = new_turn_signal_info.desired_end_point; + if (direction == Direction::LEFT) { + new_turn_signal_info.turn_signal.command = TurnIndicatorsCommand::ENABLE_LEFT; + } else if (direction == Direction::RIGHT) { + new_turn_signal_info.turn_signal.command = TurnIndicatorsCommand::ENABLE_RIGHT; + } + return new_turn_signal_info; + }; + const auto lc_turn_signal_info = new_turn_signal_info(module_type_->getDirection()); + out.turn_signal_info = module_type_->getTurnSignalDecider().use_prior_turn_signal( + *out.path, current_pose, current_nearest_seg_idx, out.turn_signal_info, lc_turn_signal_info, + nearest_dist_threshold, nearest_yaw_threshold); + } + } + if (!module_type_->isValidPath()) { path_candidate_ = std::make_shared(); return out; From ae82c022d95c1b6842f86e4f770f627795bf2fcd Mon Sep 17 00:00:00 2001 From: yutaka Date: Fri, 9 Jun 2023 21:44:51 +0900 Subject: [PATCH 2/6] update Signed-off-by: yutaka --- .../scene_module/lane_change/interface.hpp | 3 + .../scene_module/lane_change/interface.cpp | 114 ++++++++++-------- 2 files changed, 70 insertions(+), 47 deletions(-) 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/src/scene_module/lane_change/interface.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp index 8fe0041e7bfbb..260de6ad8ad13 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 @@ -250,53 +250,7 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() module_type_->updateLaneChangeStatus(); // change turn signal when the vehicle reaches at the end of the path for waiting lane change - const auto & target_lanes = module_type_->getLaneChangeStatus().lane_change_lanes; - if ( - !out.path->points.empty() && !target_lanes.empty() && - module_type_->getModuleType() == LaneChangeModuleType::NORMAL) { - const double minimum_lc_turn_signal_length = 10.0; - 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 + minimum_lc_turn_signal_length; - const double path_length = motion_utils::calcArcLength(out.path->points); - const auto & current_pose = module_type_->getEgoPose(); - const auto & front_point = out.path->points.front().point.pose.position; - const size_t & current_nearest_seg_idx = - motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - out.path->points, current_pose, nearest_dist_threshold, nearest_yaw_threshold); - const double length_front_to_ego = motion_utils::calcSignedArcLength( - out.path->points, front_point, static_cast(0), current_pose.position, - current_nearest_seg_idx); - const auto start_pose = motion_utils::calcLongitudinalOffsetPose( - out.path->points, 0, std::max(path_length - buffer, 0.0)); - if (path_length - length_front_to_ego < buffer && start_pose) { - // modify turn signal - const auto new_turn_signal_info = [&](const Direction & direction) { - TurnSignalInfo new_turn_signal_info; - new_turn_signal_info.desired_start_point = *start_pose; - new_turn_signal_info.desired_end_point = out.path->points.back().point.pose; - new_turn_signal_info.required_start_point = new_turn_signal_info.desired_start_point; - new_turn_signal_info.required_end_point = new_turn_signal_info.desired_end_point; - if (direction == Direction::LEFT) { - new_turn_signal_info.turn_signal.command = TurnIndicatorsCommand::ENABLE_LEFT; - } else if (direction == Direction::RIGHT) { - new_turn_signal_info.turn_signal.command = TurnIndicatorsCommand::ENABLE_RIGHT; - } - return new_turn_signal_info; - }; - const auto lc_turn_signal_info = new_turn_signal_info(module_type_->getDirection()); - out.turn_signal_info = module_type_->getTurnSignalDecider().use_prior_turn_signal( - *out.path, current_pose, current_nearest_seg_idx, out.turn_signal_info, lc_turn_signal_info, - nearest_dist_threshold, nearest_yaw_threshold); - } - } + out.turn_signal_info = getCurrentTurnSignalInfo(*out.path, out.turn_signal_info); if (!module_type_->isValidPath()) { path_candidate_ = std::make_shared(); @@ -456,6 +410,72 @@ 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; + + if ( + module_type_->getModuleType() == LaneChangeModuleType::NORMAL || !target_lanes.empty() || + !is_valid) { + return current_turn_signal_info; + } + + // check direction + 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 double minimum_lc_turn_signal_length = 10.0; + 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 + minimum_lc_turn_signal_length; + 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; + } + + 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); +} + void SceneModuleVisitor::visitLaneChangeInterface(const LaneChangeInterface * interface) const { lane_change_visitor_ = interface->get_debug_msg_array(); From a2f3da3ee844a3074861fd6fb845190b455adb7e Mon Sep 17 00:00:00 2001 From: yutaka Date: Fri, 9 Jun 2023 21:46:24 +0900 Subject: [PATCH 3/6] update Signed-off-by: yutaka --- .../src/scene_module/lane_change/interface.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 260de6ad8ad13..62bb00653811c 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 @@ -420,7 +420,7 @@ TurnSignalInfo LaneChangeInterface::getCurrentTurnSignalInfo( const auto & lane_change_path = module_type_->getLaneChangeStatus().lane_change_path; if ( - module_type_->getModuleType() == LaneChangeModuleType::NORMAL || !target_lanes.empty() || + module_type_->getModuleType() != LaneChangeModuleType::NORMAL || !target_lanes.empty() || !is_valid) { return current_turn_signal_info; } From 0f6b491dec159188ecff0ad976e54b5b2f993566 Mon Sep 17 00:00:00 2001 From: yutaka Date: Sun, 11 Jun 2023 01:35:39 +0900 Subject: [PATCH 4/6] update Signed-off-by: yutaka --- .../src/scene_module/lane_change/interface.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) 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 62bb00653811c..366763db589c2 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 @@ -413,19 +413,18 @@ 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; if ( - module_type_->getModuleType() != LaneChangeModuleType::NORMAL || !target_lanes.empty() || + module_type_->getModuleType() != LaneChangeModuleType::NORMAL || target_lanes.empty() || !is_valid) { - return current_turn_signal_info; + 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) { From cc0e977e059833aa4eb8b0b81eec59e5c9ab3b31 Mon Sep 17 00:00:00 2001 From: yutaka Date: Sun, 11 Jun 2023 01:44:23 +0900 Subject: [PATCH 5/6] update Signed-off-by: yutaka --- planning/behavior_path_planner/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/CMakeLists.txt b/planning/behavior_path_planner/CMakeLists.txt index 4463620dcc4a9..f521407053e27 100644 --- a/planning/behavior_path_planner/CMakeLists.txt +++ b/planning/behavior_path_planner/CMakeLists.txt @@ -7,7 +7,7 @@ autoware_package() find_package(OpenCV REQUIRED) find_package(magic_enum CONFIG REQUIRED) -set(COMPILE_WITH_OLD_ARCHITECTURE FALSE) +set(COMPILE_WITH_OLD_ARCHITECTURE TRUE) set(common_src src/steering_factor_interface.cpp From 0c34bc9b072dabf468c797fbe714d8c7786f2299 Mon Sep 17 00:00:00 2001 From: yutaka Date: Tue, 13 Jun 2023 12:52:50 +0900 Subject: [PATCH 6/6] update Signed-off-by: yutaka --- .../config/lane_change/lane_change.param.yaml | 1 + .../scene_module/lane_change/base_class.hpp | 2 ++ .../lane_change/lane_change_module_data.hpp | 3 +++ .../src/behavior_path_planner_node.cpp | 4 ++++ .../scene_module/lane_change/interface.cpp | 23 +++++++++++++++---- 5 files changed, 28 insertions(+), 5 deletions(-) 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 b934a7b99d9b3..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; } 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 366763db589c2..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 @@ -416,6 +416,7 @@ TurnSignalInfo LaneChangeInterface::getCurrentTurnSignalInfo( const auto & target_lanes = module_type_->getLaneChangeStatus().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() || @@ -441,7 +442,8 @@ TurnSignalInfo LaneChangeInterface::getCurrentTurnSignalInfo( return current_turn_signal_info; } - const double minimum_lc_turn_signal_length = 10.0; + 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 = @@ -451,7 +453,7 @@ TurnSignalInfo LaneChangeInterface::getCurrentTurnSignalInfo( 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 + minimum_lc_turn_signal_length; + 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 = @@ -468,11 +470,22 @@ TurnSignalInfo LaneChangeInterface::getCurrentTurnSignalInfo( 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); } - 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