Merge branch 'main' into feat/lidar_centerpoint/remove_deprecated_files

Author: kminoda

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp

@@ -443,9 +443,6 @@ struct AvoidLine : public ShiftLine
   // Distance from ego to end point in Frenet
   double end_longitudinal = 0.0;
 
-  // for unique_id
-  UUID id{};
-
   // for the case the point is created by merge other points
   std::vector<UUID> parent_ids{};
 

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp

@@ -404,6 +404,8 @@ class AvoidanceModule : public SceneModuleInterface
 
   bool safe_{true};
 
+  std::optional<UUID> ignore_signal_{std::nullopt};
+
   std::shared_ptr<AvoidanceHelper> helper_;
 
   std::shared_ptr<AvoidanceParameters> parameters_;

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp

@@ -176,7 +176,7 @@ double calcDistanceToAvoidStartLine(
   const std::shared_ptr<const PlannerData> & planner_data,
   const std::shared_ptr<AvoidanceParameters> & parameters);
 
-TurnSignalInfo calcTurnSignalInfo(
+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

planning/behavior_path_avoidance_module/src/scene.cpp

@@ -918,19 +918,34 @@ BehaviorModuleOutput AvoidanceModule::plan()
 
   BehaviorModuleOutput output;
 
+  const auto is_ignore_signal = [this](const UUID & uuid) {
+    if (!ignore_signal_.has_value()) {
+      return false;
+    }
+
+    return ignore_signal_.value() == uuid;
+  };
+
+  const auto update_ignore_signal = [this](const UUID & uuid, const bool is_ignore) {
+    ignore_signal_ = is_ignore ? std::make_optional(uuid) : std::nullopt;
+  };
+
   // turn signal info
   if (path_shifter_.getShiftLines().empty()) {
     output.turn_signal_info = getPreviousModuleOutput().turn_signal_info;
+  } else if (is_ignore_signal(path_shifter_.getShiftLines().front().id)) {
+    output.turn_signal_info = getPreviousModuleOutput().turn_signal_info;
   } else {
     const auto original_signal = getPreviousModuleOutput().turn_signal_info;
-    const auto new_signal = utils::avoidance::calcTurnSignalInfo(
+    const auto [new_signal, is_ignore] = utils::avoidance::calcTurnSignalInfo(
       linear_shift_path, path_shifter_.getShiftLines().front(), helper_->getEgoShift(), avoid_data_,
       planner_data_);
     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,
       planner_data_->parameters.ego_nearest_dist_threshold,
       planner_data_->parameters.ego_nearest_yaw_threshold);
+    update_ignore_signal(path_shifter_.getShiftLines().front().id, is_ignore);
   }
 
   // sparse resampling for computational cost

planning/behavior_path_avoidance_module/src/utils.cpp

@@ -250,16 +250,37 @@ 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 bool no_right_lanes, const double threshold)
 {
-  constexpr double THRESHOLD = 0.1;
   const auto relative_shift_length = end_shift_length - start_shift_length;
 
-  const auto avoid_shift =
-    std::abs(start_shift_length) < THRESHOLD && std::abs(end_shift_length) > THRESHOLD;
-  if (avoid_shift) {
+  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;
@@ -271,9 +292,7 @@ bool existShiftSideLane(
     }
   }
 
-  const auto return_shift =
-    std::abs(start_shift_length) > THRESHOLD && std::abs(end_shift_length) < THRESHOLD;
-  if (return_shift) {
+  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;
@@ -285,8 +304,7 @@ bool existShiftSideLane(
     }
   }
 
-  const auto left_middle_shift = start_shift_length > THRESHOLD && end_shift_length > THRESHOLD;
-  if (left_middle_shift) {
+  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;
@@ -298,8 +316,7 @@ bool existShiftSideLane(
     }
   }
 
-  const auto right_middle_shift = start_shift_length < THRESHOLD && end_shift_length < THRESHOLD;
-  if (right_middle_shift) {
+  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;
@@ -314,6 +331,23 @@ bool existShiftSideLane(
   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)
@@ -2323,7 +2357,7 @@ double calcDistanceToReturnDeadLine(
   return distance_to_return_dead_line;
 }
 
-TurnSignalInfo calcTurnSignalInfo(
+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)
 {
@@ -2335,22 +2369,22 @@ TurnSignalInfo calcTurnSignalInfo(
 
   if (shift_line.start_idx + 1 > path.shift_length.size()) {
     RCLCPP_WARN(rclcpp::get_logger(__func__), "index inconsistency.");
-    return {};
+    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 {};
+    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 {};
+    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 {};
+    return std::make_pair(TurnSignalInfo{}, true);
   }
 
   const auto start_shift_length = path.shift_length.at(shift_line.start_idx);
@@ -2359,12 +2393,12 @@ TurnSignalInfo calcTurnSignalInfo(
 
   // 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 {};
+    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 {};
+    return std::make_pair(TurnSignalInfo{}, true);
   }
 
   const auto get_command = [](const auto & shift_length) {
@@ -2379,7 +2413,7 @@ TurnSignalInfo calcTurnSignalInfo(
     p.vehicle_info.max_longitudinal_offset_m;
 
   if (signal_prepare_distance < ego_front_to_shift_start) {
-    return {};
+    return std::make_pair(TurnSignalInfo{}, false);
   }
 
   const auto blinker_start_pose = path.path.points.at(shift_line.start_idx).point.pose;
@@ -2396,12 +2430,12 @@ TurnSignalInfo calcTurnSignalInfo(
   turn_signal_info.turn_signal.command = get_command(relative_shift_length);
 
   if (!p.turn_signal_on_swerving) {
-    return turn_signal_info;
+    return std::make_pair(turn_signal_info, false);
   }
 
   lanelet::ConstLanelet lanelet;
   if (!rh->getClosestLaneletWithinRoute(shift_line.end, &lanelet)) {
-    return {};
+    return std::make_pair(TurnSignalInfo{}, true);
   }
 
   const auto left_same_direction_lane = rh->getLeftLanelet(lanelet, true, true);
@@ -2412,14 +2446,25 @@ TurnSignalInfo calcTurnSignalInfo(
   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)) {
-    return {};
+  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 {};
+    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 turn_signal_info;
+  return std::make_pair(turn_signal_info, false);
 }
 }  // namespace behavior_path_planner::utils::avoidance

planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp

@@ -17,9 +17,11 @@
 
 #include <rclcpp/clock.hpp>
 #include <rclcpp/logging.hpp>
+#include <tier4_autoware_utils/ros/uuid_helper.hpp>
 
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
 #include <geometry_msgs/msg/point.hpp>
+#include <unique_identifier_msgs/msg/uuid.hpp>
 
 #include <optional>
 #include <string>
@@ -31,9 +33,13 @@ using autoware_auto_planning_msgs::msg::PathPointWithLaneId;
 using autoware_auto_planning_msgs::msg::PathWithLaneId;
 using geometry_msgs::msg::Point;
 using geometry_msgs::msg::Pose;
+using tier4_autoware_utils::generateUUID;
+using unique_identifier_msgs::msg::UUID;
 
 struct ShiftLine
 {
+  ShiftLine() : id(generateUUID()) {}
+
   Pose start{};  // shift start point in absolute coordinate
   Pose end{};    // shift start point in absolute coordinate
 
@@ -45,6 +51,9 @@ struct ShiftLine
 
   size_t start_idx{};  // associated start-point index for the reference path
   size_t end_idx{};    // associated end-point index for the reference path
+
+  // for unique_id
+  UUID id{};
 };
 using ShiftLineArray = std::vector<ShiftLine>;
 

planning/route_handler/include/route_handler/route_handler.hpp

@@ -327,6 +327,7 @@ class RouteHandler
   lanelet::ConstLanelets getShoulderLanelets() const;
   bool isShoulderLanelet(const lanelet::ConstLanelet & lanelet) const;
   bool isRouteLanelet(const lanelet::ConstLanelet & lanelet) const;
+  lanelet::ConstLanelets getPreferredLanelets() const;
 
   // for path
   PathWithLaneId getCenterLinePath(

planning/route_handler/src/route_handler.cpp

@@ -433,6 +433,11 @@ lanelet::ConstLanelets RouteHandler::getRouteLanelets() const
   return route_lanelets_;
 }
 
+lanelet::ConstLanelets RouteHandler::getPreferredLanelets() const
+{
+  return preferred_lanelets_;
+}
+
 Pose RouteHandler::getStartPose() const
 {
   if (!route_ptr_) {

system/default_ad_api/src/interface.cpp

@@ -21,7 +21,7 @@ InterfaceNode::InterfaceNode(const rclcpp::NodeOptions & options) : Node("interf
 {
   const auto on_interface_version = [](auto, auto res) {
     res->major = 1;
-    res->minor = 0;
+    res->minor = 1;
     res->patch = 0;
   };
 

system/default_ad_api/test/node/interface_version.py

@@ -30,7 +30,7 @@
 
 if response.major != 1:
     exit(1)
-if response.minor != 0:
+if response.minor != 1:
     exit(1)
 if response.patch != 0:
     exit(1)