feat(behavior_path_planner_common): add general method for calculating turn signal for bpp modules (#6625)

add general method for calculating turn signal for bpp modules

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

Author: danielsanchezaran

planning/behavior_path_planner_common/docs/behavior_path_planner_turn_signal_design.md

@@ -4,7 +4,7 @@ Turn Signal decider determines necessary blinkers.
 
 ## Purpose / Role
 
-This module is responsible for activating a necessary blinker during driving. It uses rule-based algorithm to determine blinkers, and the details of this algorithm are described in the following sections. Note that this algorithm is strictly based on the Japanese Road Traffic Raw.
+This module is responsible for activating a necessary blinker during driving. It uses rule-based algorithm to determine blinkers, and the details of this algorithm are described in the following sections. Note that this algorithm is strictly based on the Japanese Road Traffic Law.
 
 ### Assumptions
 
@@ -16,7 +16,7 @@ Autoware has following order of priorities for turn signals.
 
 ### Limitations
 
-Currently, this algorithm can sometimes give unnatural (not wrong) blinkers in a complicated situations. This is because it tries to follow the road traffic raw and cannot solve `blinker conflicts` clearly in that environment.
+Currently, this algorithm can sometimes give unnatural (not wrong) blinkers in complicated situations. This is because it tries to follow the road traffic law and cannot solve `blinker conflicts` clearly in that environment.
 
 ## Parameters for turn signal decider
 
@@ -59,16 +59,16 @@ For left turn, right turn, avoidance, lane change, goal planner and pull out, we
 
 Turn signal decider checks each lanelet on the map if it has `turn_direction` information. If a lanelet has this information, it activates necessary blinker based on this information.
 
-- desired start point  
-  The `search_distance` for blinkers at intersections is `v * turn_signal_search_time + turn_signal_intersection_search_distance`. Then the start point becomes `search_distance` meters before the start point of the intersection lanelet(depicted in gree in the following picture), where `v` is the velocity of the ego vehicle. However, if we set `turn_signal_distance` in the lanelet, we use that length as search distance.
+- desired start point
+  The `search_distance` for blinkers at intersections is `v * turn_signal_search_time + turn_signal_intersection_search_distance`. Then the start point becomes `search_distance` meters before the start point of the intersection lanelet(depicted in green in the following picture), where `v` is the velocity of the ego vehicle. However, if we set `turn_signal_distance` in the lanelet, we use that length as search distance.
 
-- desired end point  
+- desired end point
   Terminal point of the intersection lanelet.
 
-- required start point  
+- required start point
   Initial point of the intersection lanelet.
 
-- required end point  
+- required end point
   The earliest point that satisfies the following condition. $\theta - \theta_{\textrm{end}} < \delta$, where $\theta_{\textrm{end}}$ is yaw angle of the terminal point of the lanelet, $\theta$ is the angle of a required end point and $\delta$ is the threshold defined by the user.
 
 ![section_turn_signal](../images/turn_signal_decider/left_right_turn.drawio.svg)
@@ -79,99 +79,99 @@ Avoidance can be separated into two sections, first section and second section.
 
 First section
 
-- desired start point  
+- desired start point
   `v * turn_signal_search_time` meters before the start point of the avoidance shift path.
 
-- desired end point  
+- desired end point
   Shift complete point where the path shift is completed.
 
-- required start point  
+- required start point
   Avoidance start point.
 
-- required end point  
+- required end point
   Shift complete point same as the desired end point.
 
 ![section_first_avoidance](../images/turn_signal_decider/avoidance_first_section.drawio.svg)
 
 Second section
 
-- desired start point  
+- desired start point
   Shift complete point.
 
-- desired end point  
+- desired end point
   Avoidance terminal point
 
-- required start point  
+- required start point
   Shift complete point.
 
-- required end point  
+- required end point
   Avoidance terminal point.
 
 ![section_second_avoidance](../images/turn_signal_decider/avoidance_second_section.drawio.svg)
 
 #### 3. Lane Change
 
-- desired start point  
+- desired start point
   `v * turn_signal_search_time` meters before the start point of the lane change path.
 
-- desired end point  
+- desired end point
   Terminal point of the lane change path.
 
-- required start point  
+- required start point
   Initial point of the lane change path.
 
-- required end point  
+- required end point
   Cross point with lane change path and boundary line of the adjacent lane.
 
 ![section_lane_change](../images/turn_signal_decider/lane_change.drawio.svg)
 
 #### 4. Pull out
 
-- desired start point  
+- desired start point
   Start point of the path of pull out.
 
-- desired end point  
+- desired end point
   Terminal point of the path of pull out.
 
-- required start point  
+- required start point
   Start point of the path pull out.
 
-- required end point  
+- required end point
   Terminal point of the path of pull out.
 
 ![section_pull_out](../images/turn_signal_decider/pull_out.drawio.svg)
 
 #### 5. Goal Planner
 
-- desired start point  
+- desired start point
   `v * turn_signal_search_time` meters before the start point of the pull over path.
 
-- desired end point  
+- desired end point
   Terminal point of the path of pull over.
 
-- required start point  
+- required start point
   Start point of the path of pull over.
 
-- required end point  
+- required end point
   Terminal point of the path of pull over.
 
 ![section_pull_over](../images/turn_signal_decider/pull_over.drawio.svg)
 
 When it comes to handle several blinkers, it gives priority to the first blinker that comes first. However, this rule sometimes activate unnatural blinkers, so turn signal decider uses the following five rules to decide the necessary turn signal.
 
-- pattern1  
+- pattern1
   ![pattern1](../images/turn_signal_decider/pattern1.drawio.svg)
 
-- pattern2  
+- pattern2
   ![pattern2](../images/turn_signal_decider/pattern2.drawio.svg)
 
-- pattern3  
+- pattern3
   ![pattern3](../images/turn_signal_decider/pattern3.drawio.svg)
 
-- pattern4  
+- pattern4
   ![pattern4](../images/turn_signal_decider/pattern4.drawio.svg)
 
-- pattern5  
+- pattern5
   ![pattern5](../images/turn_signal_decider/pattern5.drawio.svg)
 
 Based on these five rules, turn signal decider can solve `blinker conflicts`. The following pictures show some examples of this kind of conflicts.

planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp

@@ -21,6 +21,7 @@
 #include "motion_utils/trajectory/trajectory.hpp"
 
 #include <lanelet2_extension/regulatory_elements/Forward.hpp>
+#include <lanelet2_extension/utility/utilities.hpp>
 #include <rclcpp/rclcpp/clock.hpp>
 #include <rclcpp/rclcpp/time.hpp>
 #include <route_handler/route_handler.hpp>
@@ -165,6 +166,62 @@ struct PlannerData
   mutable std::vector<double> drivable_area_expansion_prev_curvatures{};
   mutable TurnSignalDecider turn_signal_decider;
 
+  std::pair<TurnSignalInfo, bool> getBehaviorTurnSignalInfo(
+    const PathWithLaneId & path, const size_t shift_start_idx, const size_t shift_end_idx,
+    const lanelet::ConstLanelets & current_lanelets, const double current_shift_length,
+    const bool is_driving_forward, const bool egos_lane_is_shifted,
+    const bool override_ego_stopped_check = false, const bool is_pull_out = false) const
+  {
+    if (shift_start_idx + 1 > path.points.size()) {
+      RCLCPP_WARN(rclcpp::get_logger(__func__), "index inconsistency.");
+      return std::make_pair(TurnSignalInfo{}, true);
+    }
+
+    if (shift_end_idx + 1 > path.points.size()) {
+      RCLCPP_WARN(rclcpp::get_logger(__func__), "index inconsistency.");
+      return std::make_pair(TurnSignalInfo{}, true);
+    }
+
+    std::vector<double> lengths(path.points.size(), 0.0);
+    ShiftedPath shifted_path{path, lengths};
+    ShiftLine shift_line;
+
+    {
+      const auto start_pose = path.points.at(shift_start_idx).point.pose;
+      const auto start_shift_length =
+        lanelet::utils::getArcCoordinates(current_lanelets, start_pose).distance;
+      const auto end_pose = path.points.at(shift_end_idx).point.pose;
+      const auto end_shift_length =
+        lanelet::utils::getArcCoordinates(current_lanelets, end_pose).distance;
+      shifted_path.shift_length.at(shift_start_idx) = start_shift_length;
+      shifted_path.shift_length.at(shift_end_idx) = end_shift_length;
+
+      shift_line.start = start_pose;
+      shift_line.end = end_pose;
+      shift_line.start_shift_length = start_shift_length;
+      shift_line.end_shift_length = end_shift_length;
+      shift_line.start_idx = shift_start_idx;
+      shift_line.end_idx = shift_end_idx;
+    }
+
+    return turn_signal_decider.getBehaviorTurnSignalInfo(
+      shifted_path, shift_line, current_lanelets, route_handler, parameters, self_odometry,
+      current_shift_length, is_driving_forward, egos_lane_is_shifted, override_ego_stopped_check,
+      is_pull_out);
+  }
+
+  std::pair<TurnSignalInfo, bool> getBehaviorTurnSignalInfo(
+    const ShiftedPath & path, const ShiftLine & shift_line,
+    const lanelet::ConstLanelets & current_lanelets, const double current_shift_length,
+    const bool is_driving_forward, const bool egos_lane_is_shifted,
+    const bool override_ego_stopped_check = false, const bool is_pull_out = false) const
+  {
+    return turn_signal_decider.getBehaviorTurnSignalInfo(
+      path, shift_line, current_lanelets, route_handler, parameters, self_odometry,
+      current_shift_length, is_driving_forward, egos_lane_is_shifted, override_ego_stopped_check,
+      is_pull_out);
+  }
+
   TurnIndicatorsCommand getTurnSignal(
     const PathWithLaneId & path, const TurnSignalInfo & turn_signal_info,
     TurnSignalDebugData & debug_data)

planning/behavior_path_planner_common/include/behavior_path_planner_common/turn_signal_decider.hpp

@@ -15,14 +15,24 @@
 #ifndef BEHAVIOR_PATH_PLANNER_COMMON__TURN_SIGNAL_DECIDER_HPP_
 #define BEHAVIOR_PATH_PLANNER_COMMON__TURN_SIGNAL_DECIDER_HPP_
 
+#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp"
+
 #include <behavior_path_planner_common/parameters.hpp>
+#include <behavior_path_planner_common/utils/path_shifter/path_shifter.hpp>
+#include <lanelet2_extension/utility/message_conversion.hpp>
 #include <route_handler/route_handler.hpp>
+#include <tier4_autoware_utils/geometry/boost_geometry.hpp>
+#include <tier4_autoware_utils/geometry/geometry.hpp>
 
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
 #include <autoware_auto_vehicle_msgs/msg/hazard_lights_command.hpp>
 #include <autoware_auto_vehicle_msgs/msg/turn_indicators_command.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+
+#include <boost/geometry/algorithms/intersects.hpp>
 
 #include <lanelet2_core/Forward.h>
+#include <lanelet2_routing/RoutingGraphContainer.h>
 
 #include <limits>
 #include <map>
@@ -37,6 +47,7 @@ using autoware_auto_vehicle_msgs::msg::HazardLightsCommand;
 using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand;
 using geometry_msgs::msg::Point;
 using geometry_msgs::msg::Pose;
+using nav_msgs::msg::Odometry;
 using route_handler::RouteHandler;
 
 const std::map<std::string, uint8_t> g_signal_map = {
@@ -100,6 +111,15 @@ class TurnSignalDecider
   std::pair<bool, bool> getIntersectionTurnSignalFlag();
   std::pair<Pose, double> getIntersectionPoseAndDistance();
 
+  std::pair<TurnSignalInfo, bool> getBehaviorTurnSignalInfo(
+    const ShiftedPath & path, const ShiftLine & shift_line,
+    const lanelet::ConstLanelets & current_lanelets,
+    const std::shared_ptr<RouteHandler> route_handler,
+    const BehaviorPathPlannerParameters & parameters, const Odometry::ConstSharedPtr self_odometry,
+    const double current_shift_length, const bool is_driving_forward,
+    const bool egos_lane_is_shifted, const bool override_ego_stopped_check = false,
+    const bool is_pull_out = false) const;
+
 private:
   std::optional<TurnSignalInfo> getIntersectionTurnSignalInfo(
     const PathWithLaneId & path, const Pose & current_pose, const double current_vel,
@@ -118,6 +138,132 @@ class TurnSignalDecider
     const double nearest_yaw_threshold);
   void initialize_intersection_info();
 
+  inline bool isAvoidShift(
+    const double start_shift_length, const double end_shift_length, const double threshold) const
+  {
+    return std::abs(start_shift_length) < threshold && std::abs(end_shift_length) > threshold;
+  };
+
+  inline bool isReturnShift(
+    const double start_shift_length, const double end_shift_length, const double threshold) const
+  {
+    return std::abs(start_shift_length) > threshold && std::abs(end_shift_length) < threshold;
+  };
+
+  inline bool isLeftMiddleShift(
+    const double start_shift_length, const double end_shift_length, const double threshold) const
+  {
+    return start_shift_length > threshold && end_shift_length > threshold;
+  };
+
+  inline bool isRightMiddleShift(
+    const double start_shift_length, const double end_shift_length, const double threshold) const
+  {
+    return start_shift_length < threshold && end_shift_length < threshold;
+  };
+
+  inline bool existShiftSideLane(
+    const double start_shift_length, const double end_shift_length, const bool no_left_lanes,
+    const bool no_right_lanes, const double threshold) const
+  {
+    const auto relative_shift_length = end_shift_length - start_shift_length;
+    if (isAvoidShift(start_shift_length, end_shift_length, threshold)) {
+      // Left avoid. But there is no adjacent lane. No need blinker.
+      if (relative_shift_length > 0.0 && no_left_lanes) {
+        return false;
+      }
+
+      // Right avoid. But there is no adjacent lane. No need blinker.
+      if (relative_shift_length < 0.0 && no_right_lanes) {
+        return false;
+      }
+    }
+
+    if (isReturnShift(start_shift_length, end_shift_length, threshold)) {
+      // Right return. But there is no adjacent lane. No need blinker.
+      if (relative_shift_length > 0.0 && no_right_lanes) {
+        return false;
+      }
+
+      // Left return. But there is no adjacent lane. No need blinker.
+      if (relative_shift_length < 0.0 && no_left_lanes) {
+        return false;
+      }
+    }
+
+    if (isLeftMiddleShift(start_shift_length, end_shift_length, threshold)) {
+      // Left avoid. But there is no adjacent lane. No need blinker.
+
+      if (relative_shift_length > 0.0 && no_left_lanes) {
+        return false;
+      }
+
+      // Left return. But there is no adjacent lane. No need blinker.
+      if (relative_shift_length < 0.0 && no_left_lanes) {
+        return false;
+      }
+    }
+
+    if (isRightMiddleShift(start_shift_length, end_shift_length, threshold)) {
+      // Right avoid. But there is no adjacent lane. No need blinker.
+
+      if (relative_shift_length < 0.0 && no_right_lanes) {
+        return false;
+      }
+
+      // Left avoid. But there is no adjacent lane. No need blinker.
+      if (relative_shift_length > 0.0 && no_right_lanes) {
+        return false;
+      }
+    }
+    return true;
+  };
+
+  inline bool straddleRoadBound(
+    const ShiftedPath & path, const ShiftLine & shift_line, const lanelet::ConstLanelets & lanes,
+    const vehicle_info_util::VehicleInfo & vehicle_info) const
+  {
+    using boost::geometry::intersects;
+    using tier4_autoware_utils::pose2transform;
+    using tier4_autoware_utils::transformVector;
+
+    const auto footprint = vehicle_info.createFootprint();
+
+    for (const auto & lane : lanes) {
+      for (size_t i = shift_line.start_idx; i < shift_line.end_idx; ++i) {
+        const auto transform = pose2transform(path.path.points.at(i).point.pose);
+        const auto shifted_vehicle_footprint = transformVector(footprint, transform);
+
+        if (intersects(lane.leftBound2d().basicLineString(), shifted_vehicle_footprint)) {
+          return true;
+        }
+
+        if (intersects(lane.rightBound2d().basicLineString(), shifted_vehicle_footprint)) {
+          return true;
+        }
+      }
+    }
+
+    return false;
+  };
+
+  inline bool isNearEndOfShift(
+    const double start_shift_length, const double end_shift_length, const Point & ego_pos,
+    const lanelet::ConstLanelets & original_lanes, const double threshold) const
+  {
+    using boost::geometry::within;
+    using lanelet::utils::to2D;
+    using lanelet::utils::conversion::toLaneletPoint;
+
+    if (!isReturnShift(start_shift_length, end_shift_length, threshold)) {
+      return false;
+    }
+
+    return std::any_of(original_lanes.begin(), original_lanes.end(), [&ego_pos](const auto & lane) {
+      return within(to2D(toLaneletPoint(ego_pos)), lane.polygon2d().basicPolygon());
+    });
+  };
+
   geometry_msgs::msg::Quaternion calc_orientation(const Point & src_point, const Point & dst_point);
 
   rclcpp::Logger logger_{

planning/behavior_path_planner_common/src/turn_signal_decider.cpp

@@ -33,6 +33,8 @@
 
 namespace behavior_path_planner
 {
+using motion_utils::calcSignedArcLength;
+
 double calc_distance(
   const PathWithLaneId & path, const Pose & current_pose, const size_t current_seg_idx,
   const Pose & input_point, const double nearest_dist_threshold, const double nearest_yaw_threshold)
@@ -43,6 +45,11 @@ double calc_distance(
     path.points, current_pose.position, current_seg_idx, input_point.position, nearest_seg_idx);
 }
 
+/***
+ * @brief:
+ * Gets the turn signal info after comparing the turn signal info output from the behavior path
+ * module and comparing it to turn signal info obtained from intersections.
+ */
 TurnIndicatorsCommand TurnSignalDecider::getTurnSignal(
   const std::shared_ptr<RouteHandler> & route_handler, const PathWithLaneId & path,
   const TurnSignalInfo & turn_signal_info, const Pose & current_pose, const double current_vel,
@@ -606,4 +613,145 @@ geometry_msgs::msg::Quaternion TurnSignalDecider::calc_orientation(
   const double yaw = tier4_autoware_utils::calcAzimuthAngle(src_point, dst_point);
   return tier4_autoware_utils::createQuaternionFromRPY(0.0, pitch, yaw);
 }
+
+std::pair<TurnSignalInfo, bool> TurnSignalDecider::getBehaviorTurnSignalInfo(
+  const ShiftedPath & path, const ShiftLine & shift_line,
+  const lanelet::ConstLanelets & current_lanelets,
+  const std::shared_ptr<RouteHandler> route_handler,
+  const BehaviorPathPlannerParameters & parameters, const Odometry::ConstSharedPtr self_odometry,
+  const double current_shift_length, const bool is_driving_forward, const bool egos_lane_is_shifted,
+  const bool override_ego_stopped_check, const bool is_pull_out) const
+{
+  constexpr double THRESHOLD = 0.1;
+  const auto & p = parameters;
+  const auto & rh = route_handler;
+  const auto & ego_pose = self_odometry->pose.pose;
+  const auto & ego_speed = self_odometry->twist.twist.linear.x;
+
+  if (!is_driving_forward) {
+    TurnSignalInfo turn_signal_info{};
+    turn_signal_info.hazard_signal.command = HazardLightsCommand::ENABLE;
+    const auto back_start_pose = rh->getOriginalStartPose();
+    const Pose & start_pose = self_odometry->pose.pose;
+
+    turn_signal_info.desired_start_point = back_start_pose;
+    turn_signal_info.required_start_point = back_start_pose;
+    // pull_out start_pose is same to backward driving end_pose
+    turn_signal_info.required_end_point = start_pose;
+    turn_signal_info.desired_end_point = start_pose;
+    return std::make_pair(turn_signal_info, false);
+  }
+
+  if (shift_line.start_idx + 1 > path.shift_length.size()) {
+    RCLCPP_WARN(rclcpp::get_logger(__func__), "index inconsistency.");
+    return std::make_pair(TurnSignalInfo{}, true);
+  }
+
+  if (shift_line.start_idx + 1 > path.path.points.size()) {
+    RCLCPP_WARN(rclcpp::get_logger(__func__), "index inconsistency.");
+    return std::make_pair(TurnSignalInfo{}, true);
+  }
+
+  if (shift_line.end_idx + 1 > path.shift_length.size()) {
+    RCLCPP_WARN(rclcpp::get_logger(__func__), "index inconsistency.");
+    return std::make_pair(TurnSignalInfo{}, true);
+  }
+
+  if (shift_line.end_idx + 1 > path.path.points.size()) {
+    RCLCPP_WARN(rclcpp::get_logger(__func__), "index inconsistency.");
+    return std::make_pair(TurnSignalInfo{}, true);
+  }
+
+  const auto [start_shift_length, end_shift_length] =
+    std::invoke([&path, &shift_line, &egos_lane_is_shifted]() -> std::pair<double, double> {
+      const auto temp_start_shift_length = path.shift_length.at(shift_line.start_idx);
+      const auto temp_end_shift_length = path.shift_length.at(shift_line.end_idx);
+      // Shift is done using the target lane and not current ego's lane
+      if (!egos_lane_is_shifted) {
+        return std::make_pair(temp_end_shift_length, -temp_start_shift_length);
+      }
+      return std::make_pair(temp_start_shift_length, temp_end_shift_length);
+    });
+
+  const auto relative_shift_length = end_shift_length - start_shift_length;
+
+  // If shift length is shorter than the threshold, it does not need to turn on blinkers
+  if (std::fabs(relative_shift_length) < p.turn_signal_shift_length_threshold) {
+    return std::make_pair(TurnSignalInfo{}, true);
+  }
+
+  // If the vehicle does not shift anymore, we turn off the blinker
+  if (std::fabs(end_shift_length - current_shift_length) < THRESHOLD) {
+    return std::make_pair(TurnSignalInfo{}, true);
+  }
+
+  const auto get_command = [](const auto & shift_length) {
+    return shift_length > 0.0 ? TurnIndicatorsCommand::ENABLE_LEFT
+                              : TurnIndicatorsCommand::ENABLE_RIGHT;
+  };
+
+  const auto signal_prepare_distance =
+    std::max(ego_speed * p.turn_signal_search_time, p.turn_signal_minimum_search_distance);
+  const auto ego_front_to_shift_start =
+    calcSignedArcLength(path.path.points, ego_pose.position, shift_line.start_idx) -
+    p.vehicle_info.max_longitudinal_offset_m;
+
+  if (signal_prepare_distance < ego_front_to_shift_start) {
+    return std::make_pair(TurnSignalInfo{}, false);
+  }
+
+  const auto blinker_start_pose = path.path.points.at(shift_line.start_idx).point.pose;
+  const auto blinker_end_pose = path.path.points.at(shift_line.end_idx).point.pose;
+  const auto get_start_pose = [&](const auto & ego_to_shift_start) {
+    return ego_to_shift_start > 0.0 ? ego_pose : blinker_start_pose;
+  };
+
+  TurnSignalInfo turn_signal_info{};
+  turn_signal_info.desired_start_point = get_start_pose(ego_front_to_shift_start);
+  turn_signal_info.desired_end_point = blinker_end_pose;
+  turn_signal_info.required_start_point = blinker_start_pose;
+  turn_signal_info.required_end_point = blinker_end_pose;
+  turn_signal_info.turn_signal.command = get_command(relative_shift_length);
+
+  if (!p.turn_signal_on_swerving) {
+    return std::make_pair(turn_signal_info, false);
+  }
+
+  lanelet::ConstLanelet lanelet;
+  const auto query_pose = (egos_lane_is_shifted) ? shift_line.end : shift_line.start;
+  if (!rh->getClosestLaneletWithinRoute(query_pose, &lanelet)) {
+    return std::make_pair(TurnSignalInfo{}, true);
+  }
+
+  const auto left_same_direction_lane = rh->getLeftLanelet(lanelet, true, true);
+  const auto left_opposite_lanes = rh->getLeftOppositeLanelets(lanelet);
+  const auto right_same_direction_lane = rh->getRightLanelet(lanelet, true, true);
+  const auto right_opposite_lanes = rh->getRightOppositeLanelets(lanelet);
+  const auto has_left_lane = left_same_direction_lane.has_value() || !left_opposite_lanes.empty();
+  const auto has_right_lane =
+    right_same_direction_lane.has_value() || !right_opposite_lanes.empty();
+
+  if (
+    !is_pull_out && !existShiftSideLane(
+                      start_shift_length, end_shift_length, !has_left_lane, !has_right_lane,
+                      p.turn_signal_shift_length_threshold)) {
+    return std::make_pair(TurnSignalInfo{}, true);
+  }
+
+  if (!straddleRoadBound(path, shift_line, current_lanelets, p.vehicle_info)) {
+    return std::make_pair(TurnSignalInfo{}, true);
+  }
+
+  constexpr double STOPPED_THRESHOLD = 0.1;  // [m/s]
+  if (ego_speed < STOPPED_THRESHOLD && !override_ego_stopped_check) {
+    if (isNearEndOfShift(
+          start_shift_length, end_shift_length, ego_pose.position, current_lanelets,
+          p.turn_signal_shift_length_threshold)) {
+      return std::make_pair(TurnSignalInfo{}, true);
+    }
+  }
+
+  return std::make_pair(turn_signal_info, false);
+}
+
 }  // namespace behavior_path_planner