@@ -480,14 +480,20 @@ void NormalLaneChange::resetParameters()
480
480
481
481
TurnSignalInfo NormalLaneChange::updateOutputTurnSignal () const
482
482
{
483
- TurnSignalInfo turn_signal_info = calcTurnSignalInfo ();
484
- const auto [turn_signal_command, distance_to_vehicle_front] = utils::getPathTurnSignal (
485
- status_.current_lanes , status_.lane_change_path .shifted_path ,
486
- status_.lane_change_path .info .shift_line , getEgoPose (), getEgoTwist ().linear .x ,
487
- planner_data_->parameters );
488
- turn_signal_info.turn_signal .command = turn_signal_command.command ;
489
-
490
- return turn_signal_info;
483
+ const auto & pose = getEgoPose ();
484
+ const auto & current_lanes = status_.current_lanes ;
485
+ const auto & shift_line = status_.lane_change_path .info .shift_line ;
486
+ const auto & shift_path = status_.lane_change_path .shifted_path ;
487
+ const auto current_shift_length = lanelet::utils::getArcCoordinates (current_lanes, pose).distance ;
488
+ constexpr bool is_driving_forward = true ;
489
+ // The getBehaviorTurnSignalInfo method expects the shifted line to be generated off of the ego's
490
+ // current lane, lane change is different, so we set this flag to false.
491
+ constexpr bool egos_lane_is_shifted = false ;
492
+
493
+ const auto [new_signal, is_ignore] = planner_data_->getBehaviorTurnSignalInfo (
494
+ shift_path, shift_line, current_lanes, current_shift_length, is_driving_forward,
495
+ egos_lane_is_shifted);
496
+ return new_signal;
491
497
}
492
498
493
499
lanelet::ConstLanelets NormalLaneChange::getCurrentLanes () const
@@ -1686,59 +1692,6 @@ PathSafetyStatus NormalLaneChange::evaluateApprovedPathWithUnsafeHysteresis(
1686
1692
return {};
1687
1693
}
1688
1694
1689
- TurnSignalInfo NormalLaneChange::calcTurnSignalInfo () const
1690
- {
1691
- const auto get_blinker_pose = [](const PathWithLaneId & path, const double length) {
1692
- double accumulated_length = 0.0 ;
1693
- for (size_t i = 0 ; i < path.points .size () - 1 ; ++i) {
1694
- accumulated_length +=
1695
- tier4_autoware_utils::calcDistance2d (path.points .at (i), path.points .at (i + 1 ));
1696
- if (accumulated_length > length) {
1697
- return path.points .at (i).point .pose ;
1698
- }
1699
- }
1700
-
1701
- return path.points .front ().point .pose ;
1702
- };
1703
-
1704
- const auto & path = status_.lane_change_path ;
1705
- const auto & shifted_path = path.shifted_path .path ;
1706
-
1707
- TurnSignalInfo turn_signal_info{};
1708
-
1709
- if (path.path .points .empty ()) {
1710
- return turn_signal_info;
1711
- }
1712
-
1713
- // desired start pose = prepare start pose
1714
- turn_signal_info.desired_start_point = std::invoke ([&]() {
1715
- const auto blinker_start_duration = planner_data_->parameters .turn_signal_search_time ;
1716
- const auto diff_time = path.info .duration .prepare - blinker_start_duration;
1717
- if (diff_time < 1e-5 ) {
1718
- return path.path .points .front ().point .pose ;
1719
- }
1720
-
1721
- const auto current_twist = getEgoTwist ();
1722
- const auto diff_length = std::abs (current_twist.linear .x ) * diff_time;
1723
- return get_blinker_pose (path.path , diff_length);
1724
- });
1725
-
1726
- // desired end pose
1727
- const auto length_ratio =
1728
- std::clamp (lane_change_parameters_->length_ratio_for_turn_signal_deactivation , 0.0 , 1.0 );
1729
- const auto desired_end_length = path.info .length .lane_changing * length_ratio;
1730
- turn_signal_info.desired_end_point = get_blinker_pose (shifted_path, desired_end_length);
1731
-
1732
- // required start pose = lane changing start pose
1733
- turn_signal_info.required_start_point = path.info .shift_line .start ;
1734
-
1735
- // required end pose = in the middle of the lane change
1736
- const auto mid_lane_change_length = path.info .length .lane_changing / 2 ;
1737
- turn_signal_info.required_end_point = get_blinker_pose (shifted_path, mid_lane_change_length);
1738
-
1739
- return turn_signal_info;
1740
- }
1741
-
1742
1695
bool NormalLaneChange::isValidPath (const PathWithLaneId & path) const
1743
1696
{
1744
1697
const auto & route_handler = planner_data_->route_handler ;
0 commit comments