feat(behavior_path_avoidance_module): use the general calc turn signal method (#6626)

use the general calc turn signal method

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

Author: danielsanchezaran

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp

@@ -176,9 +176,6 @@ double calcDistanceToAvoidStartLine(
   const std::shared_ptr<const PlannerData> & planner_data,
   const std::shared_ptr<AvoidanceParameters> & parameters);
 
-std::pair<TurnSignalInfo, bool> calcTurnSignalInfo(
-  const ShiftedPath & path, const ShiftLine & shift_line, const double current_shift_length,
-  const AvoidancePlanningData & data, const std::shared_ptr<const PlannerData> & planner_data);
 }  // namespace behavior_path_planner::utils::avoidance
 
 #endif  // BEHAVIOR_PATH_AVOIDANCE_MODULE__UTILS_HPP_

planning/behavior_path_avoidance_module/src/scene.cpp

@@ -942,9 +942,13 @@ BehaviorModuleOutput AvoidanceModule::plan()
     output.turn_signal_info = getPreviousModuleOutput().turn_signal_info;
   } else {
     const auto original_signal = getPreviousModuleOutput().turn_signal_info;
-    const auto [new_signal, is_ignore] = utils::avoidance::calcTurnSignalInfo(
-      linear_shift_path, path_shifter_.getShiftLines().front(), helper_->getEgoShift(), avoid_data_,
-      planner_data_);
+
+    constexpr bool is_driving_forward = true;
+    constexpr bool egos_lane_is_shifted = true;
+    const auto [new_signal, is_ignore] = planner_data_->getBehaviorTurnSignalInfo(
+      linear_shift_path, path_shifter_.getShiftLines().front(), avoid_data_.current_lanelets,
+      helper_->getEgoShift(), is_driving_forward, egos_lane_is_shifted);
+
     const auto current_seg_idx = planner_data_->findEgoSegmentIndex(spline_shift_path.path.points);
     output.turn_signal_info = planner_data_->turn_signal_decider.use_prior_turn_signal(
       spline_shift_path.path, getEgoPose(), current_seg_idx, original_signal, new_signal,

planning/behavior_path_avoidance_module/src/utils.cpp

@@ -250,132 +250,6 @@ void pushUniqueVector(T & base_vector, const T & additional_vector)
   base_vector.insert(base_vector.end(), additional_vector.begin(), additional_vector.end());
 }
 
-bool isAvoidShift(
-  const double start_shift_length, const double end_shift_length, const double threshold)
-{
-  return std::abs(start_shift_length) < threshold && std::abs(end_shift_length) > threshold;
-}
-
-bool isReturnShift(
-  const double start_shift_length, const double end_shift_length, const double threshold)
-{
-  return std::abs(start_shift_length) > threshold && std::abs(end_shift_length) < threshold;
-}
-
-bool isLeftMiddleShift(
-  const double start_shift_length, const double end_shift_length, const double threshold)
-{
-  return start_shift_length > threshold && end_shift_length > threshold;
-}
-
-bool isRightMiddleShift(
-  const double start_shift_length, const double end_shift_length, const double threshold)
-{
-  return start_shift_length < threshold && end_shift_length < threshold;
-}
-
-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 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;
-}
-
-bool isNearEndOfShift(
-  const double start_shift_length, const double end_shift_length, const Point & ego_pos,
-  const lanelet::ConstLanelets & original_lanes, const double threshold)
-{
-  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());
-  });
-}
-
-bool straddleRoadBound(
-  const ShiftedPath & path, const ShiftLine & shift_line, const lanelet::ConstLanelets & lanes,
-  const vehicle_info_util::VehicleInfo & vehicle_info)
-{
-  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;
-}
-
 }  // namespace
 
 namespace filtering_utils
@@ -2367,115 +2241,4 @@ double calcDistanceToReturnDeadLine(
 
   return distance_to_return_dead_line;
 }
-
-std::pair<TurnSignalInfo, bool> calcTurnSignalInfo(
-  const ShiftedPath & path, const ShiftLine & shift_line, const double current_shift_length,
-  const AvoidancePlanningData & data, const std::shared_ptr<const PlannerData> & planner_data)
-{
-  constexpr double THRESHOLD = 0.1;
-  const auto & p = planner_data->parameters;
-  const auto & rh = planner_data->route_handler;
-  const auto & ego_pose = planner_data->self_odometry->pose.pose;
-  const auto & ego_speed = planner_data->self_odometry->twist.twist.linear.x;
-
-  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 = path.shift_length.at(shift_line.start_idx);
-  const auto end_shift_length = path.shift_length.at(shift_line.end_idx);
-  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(path.shift_length.at(shift_line.end_idx) - 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 ? 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;
-  if (!rh->getClosestLaneletWithinRoute(shift_line.end, &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 (!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, data.current_lanelets, p.vehicle_info)) {
-    return std::make_pair(TurnSignalInfo{}, true);
-  }
-
-  constexpr double STOPPED_THRESHOLD = 0.1;  // [m/s]
-  if (ego_speed < STOPPED_THRESHOLD) {
-    if (isNearEndOfShift(
-          start_shift_length, end_shift_length, ego_pose.position, data.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::utils::avoidance