@@ -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
@@ -1687,59 +1693,6 @@ PathSafetyStatus NormalLaneChange::evaluateApprovedPathWithUnsafeHysteresis(
1687
1693
return {};
1688
1694
}
1689
1695
1690
- TurnSignalInfo NormalLaneChange::calcTurnSignalInfo () const
1691
- {
1692
- const auto get_blinker_pose = [](const PathWithLaneId & path, const double length) {
1693
- double accumulated_length = 0.0 ;
1694
- for (size_t i = 0 ; i < path.points .size () - 1 ; ++i) {
1695
- accumulated_length +=
1696
- tier4_autoware_utils::calcDistance2d (path.points .at (i), path.points .at (i + 1 ));
1697
- if (accumulated_length > length) {
1698
- return path.points .at (i).point .pose ;
1699
- }
1700
- }
1701
-
1702
- return path.points .front ().point .pose ;
1703
- };
1704
-
1705
- const auto & path = status_.lane_change_path ;
1706
- const auto & shifted_path = path.shifted_path .path ;
1707
-
1708
- TurnSignalInfo turn_signal_info{};
1709
-
1710
- if (path.path .points .empty ()) {
1711
- return turn_signal_info;
1712
- }
1713
-
1714
- // desired start pose = prepare start pose
1715
- turn_signal_info.desired_start_point = std::invoke ([&]() {
1716
- const auto blinker_start_duration = planner_data_->parameters .turn_signal_search_time ;
1717
- const auto diff_time = path.info .duration .prepare - blinker_start_duration;
1718
- if (diff_time < 1e-5 ) {
1719
- return path.path .points .front ().point .pose ;
1720
- }
1721
-
1722
- const auto current_twist = getEgoTwist ();
1723
- const auto diff_length = std::abs (current_twist.linear .x ) * diff_time;
1724
- return get_blinker_pose (path.path , diff_length);
1725
- });
1726
-
1727
- // desired end pose
1728
- const auto length_ratio =
1729
- std::clamp (lane_change_parameters_->length_ratio_for_turn_signal_deactivation , 0.0 , 1.0 );
1730
- const auto desired_end_length = path.info .length .lane_changing * length_ratio;
1731
- turn_signal_info.desired_end_point = get_blinker_pose (shifted_path, desired_end_length);
1732
-
1733
- // required start pose = lane changing start pose
1734
- turn_signal_info.required_start_point = path.info .shift_line .start ;
1735
-
1736
- // required end pose = in the middle of the lane change
1737
- const auto mid_lane_change_length = path.info .length .lane_changing / 2 ;
1738
- turn_signal_info.required_end_point = get_blinker_pose (shifted_path, mid_lane_change_length);
1739
-
1740
- return turn_signal_info;
1741
- }
1742
-
1743
1696
bool NormalLaneChange::isValidPath (const PathWithLaneId & path) const
1744
1697
{
1745
1698
const auto & route_handler = planner_data_->route_handler ;
0 commit comments