diff --git a/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml b/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml index da2c6da93038c..80114889d7e74 100644 --- a/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml +++ b/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml @@ -4,7 +4,6 @@ th_arrived_distance: 1.0 th_stopped_velocity: 0.01 th_stopped_time: 1.0 - th_blinker_on_lateral_offset: 1.0 collision_check_margin: 1.0 collision_check_distance_from_end: 1.0 # shift pull out @@ -30,3 +29,7 @@ backward_search_resolution: 2.0 backward_path_update_duration: 3.0 ignore_distance_from_lane_end: 15.0 + # turns signal + th_turn_signal_on_lateral_offset: 1.0 + intersection_search_length: 30.0 + length_ratio_for_turn_signal_deactivation_near_intersection: 0.5 diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_start_planner_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_start_planner_design.md index 1c3b91851fb14..5cf862feee90a 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_start_planner_design.md +++ b/planning/behavior_path_planner/docs/behavior_path_planner_start_planner_design.md @@ -49,14 +49,16 @@ PullOutPath --o PullOutPlannerBase ## General parameters for start_planner -| Name | Unit | Type | Description | Default value | -| :-------------------------------- | :---- | :----- | :------------------------------------------------------------------- | :------------ | -| th_arrived_distance_m | [m] | double | distance threshold for arrival of path termination | 1.0 | -| th_stopped_velocity_mps | [m/s] | double | velocity threshold for arrival of path termination | 0.01 | -| th_stopped_time_sec | [s] | double | time threshold for arrival of path termination | 1.0 | -| th_blinker_on_lateral_offset | [m] | double | lateral distance threshold for turning on blinker | 1.0 | -| collision_check_margin | [m] | double | Obstacle collision check margin | 1.0 | -| collision_check_distance_from_end | [m] | double | collision check distance from end point. currently only for pull out | 15.0 | +| Name | Unit | Type | Description | Default value | +| :---------------------------------------------------------- | :---- | :----- | :------------------------------------------------------------------- | :------------ | +| th_arrived_distance_m | [m] | double | distance threshold for arrival of path termination | 1.0 | +| th_stopped_velocity_mps | [m/s] | double | velocity threshold for arrival of path termination | 0.01 | +| th_stopped_time_sec | [s] | double | time threshold for arrival of path termination | 1.0 | +| th_turn_signal_on_lateral_offset | [m] | double | lateral distance threshold for turning on blinker | 1.0 | +| intersection_search_length | [m] | double | check if intersections exist whithin this length | 30.0 | +| length_ratio_for_turn_signal_deactivation_near_intersection | [m] | double | deactivate turn signal of this module near intersection | 0.5 | +| collision_check_margin | [m] | double | Obstacle collision check margin | 1.0 | +| collision_check_distance_from_end | [m] | double | collision check distance from end point. currently only for pull out | 15.0 | ## **Safe check with obstacles in shoulder lane** diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp index 6d896852f4de2..02a3b6169166a 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp @@ -28,7 +28,9 @@ struct StartPlannerParameters double th_arrived_distance; double th_stopped_velocity; double th_stopped_time; - double th_blinker_on_lateral_offset; + double th_turn_signal_on_lateral_offset; + double intersection_search_length; + double length_ratio_for_turn_signal_deactivation_near_intersection; double collision_check_margin; double collision_check_distance_from_end; // shift pull out 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 39f73151f909e..95c053147f7e5 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -1073,7 +1073,11 @@ StartPlannerParameters BehaviorPathPlannerNode::getStartPlannerParam() p.th_arrived_distance = declare_parameter(ns + "th_arrived_distance"); p.th_stopped_velocity = declare_parameter(ns + "th_stopped_velocity"); p.th_stopped_time = declare_parameter(ns + "th_stopped_time"); - p.th_blinker_on_lateral_offset = declare_parameter(ns + "th_blinker_on_lateral_offset"); + p.th_turn_signal_on_lateral_offset = + declare_parameter(ns + "th_turn_signal_on_lateral_offset"); + p.intersection_search_length = declare_parameter(ns + "intersection_search_length"); + p.length_ratio_for_turn_signal_deactivation_near_intersection = + declare_parameter(ns + "length_ratio_for_turn_signal_deactivation_near_intersection"); p.collision_check_margin = declare_parameter(ns + "collision_check_margin"); p.collision_check_distance_from_end = declare_parameter(ns + "collision_check_distance_from_end"); diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index 49515a41a5935..127082dfa510e 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -815,10 +815,10 @@ TurnSignalInfo StartPlannerModule::calcTurnSignalInfo() const const auto lane = planner_data_->route_handler->getLaneletMapPtr()->laneletLayer.get(lane_id); const double lateral_offset = lanelet::utils::getLateralDistanceToCenterline(lane, start_pose); - if (distance_from_end < 0.0 && lateral_offset > parameters_->th_blinker_on_lateral_offset) { + if (distance_from_end < 0.0 && lateral_offset > parameters_->th_turn_signal_on_lateral_offset) { turn_signal.turn_signal.command = TurnIndicatorsCommand::ENABLE_RIGHT; } else if ( - distance_from_end < 0.0 && lateral_offset < -parameters_->th_blinker_on_lateral_offset) { + distance_from_end < 0.0 && lateral_offset < -parameters_->th_turn_signal_on_lateral_offset) { turn_signal.turn_signal.command = TurnIndicatorsCommand::ENABLE_LEFT; } else { turn_signal.turn_signal.command = TurnIndicatorsCommand::DISABLE; @@ -826,9 +826,54 @@ TurnSignalInfo StartPlannerModule::calcTurnSignalInfo() const turn_signal.desired_start_point = start_pose; turn_signal.required_start_point = start_pose; - turn_signal.required_end_point = end_pose; turn_signal.desired_end_point = end_pose; + // check if intersection exists whithin search length + const bool is_near_intersection = std::invoke([&]() { + const double check_length = parameters_->intersection_search_length; + double accumulated_length = 0.0; + for (size_t i = 0; i < path.points.size() - 1; ++i) { + const auto & p = path.points.at(i); + for (const auto & lane : planner_data_->route_handler->getLaneletsFromIds(p.lane_ids)) { + const std::string turn_direction = lane.attributeOr("turn_direction", "else"); + if (turn_direction == "right" || turn_direction == "left" || turn_direction == "straight") { + return true; + } + } + accumulated_length += tier4_autoware_utils::calcDistance2d(p, path.points.at(i + 1)); + if (accumulated_length > check_length) { + return false; + } + } + return false; + }); + + if (is_near_intersection) { + // offset required end pose with ration to activate turn signal for intersection + turn_signal.required_end_point = std::invoke([&]() { + const double length_start_to_end = + motion_utils::calcSignedArcLength(path.points, start_pose.position, end_pose.position); + const auto ratio = std::clamp( + parameters_->length_ratio_for_turn_signal_deactivation_near_intersection, 0.0, 1.0); + + const double required_end_length = length_start_to_end * ratio; + double accumulated_length = 0.0; + const size_t start_idx = motion_utils::findNearestIndex(path.points, start_pose.position); + for (size_t i = start_idx; i < path.points.size() - 1; ++i) { + accumulated_length += + tier4_autoware_utils::calcDistance2d(path.points.at(i), path.points.at(i + 1)); + if (accumulated_length > required_end_length) { + // find requrired end point + return path.points.at(i).point.pose; + } + } + // not found required end point + return end_pose; + }); + } else { + turn_signal.required_end_point = end_pose; + } + return turn_signal; }