refactor(intersection): divide stop line function, cache lanelet obje…

…ct (#2799)

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp

@@ -17,6 +17,7 @@
 
 #include <motion_utils/motion_utils.hpp>
 #include <rclcpp/rclcpp.hpp>
+#include <scene_module/intersection/util_type.hpp>
 #include <scene_module/scene_module_interface.hpp>
 #include <tier4_autoware_utils/tier4_autoware_utils.hpp>
 #include <utilization/boost_geometry_helper.hpp>
@@ -114,6 +115,7 @@ class IntersectionModule : public SceneModuleInterface
   bool is_go_out_;
   // Parameter
   PlannerParam planner_param_;
+  std::optional<util::IntersectionLanelets> intersection_lanelets_;
 
   /**
    * @brief check collision for all lanelet area & predicted objects (call checkPathCollision() as

planning/behavior_velocity_planner/include/scene_module/intersection/scene_merge_from_private_road.hpp

@@ -95,6 +95,7 @@ class MergeFromPrivateRoadModule : public SceneModuleInterface
 
   // Parameter
   PlannerParam planner_param_;
+  std::optional<util::IntersectionLanelets> intersection_lanelets_;
 
   StateMachine state_machine_;  //! for state
 

planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp

@@ -17,6 +17,7 @@
 
 #include <rclcpp/rclcpp.hpp>
 #include <scene_module/intersection/scene_intersection.hpp>
+#include <scene_module/intersection/util_type.hpp>
 
 #include <geometry_msgs/msg/point.hpp>
 
@@ -52,17 +53,10 @@ std::optional<size_t> getDuplicatedPointIdx(
 /**
  * @brief get objective polygons for detection area
  */
-std::tuple<lanelet::ConstLanelets, lanelet::ConstLanelets> getObjectiveLanelets(
+IntersectionLanelets getObjectiveLanelets(
   lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr,
   const int lane_id, const double detection_area_length, const bool tl_arrow_solid_on = false);
 
-struct StopLineIdx
-{
-  size_t first_inside_lane = 0;
-  size_t pass_judge_line = 0;
-  size_t stop_line = 0;
-};
-
 /**
  * @brief Generate a stop line and insert it into the path. If the stop line is defined in the map,
  * read it from the map; otherwise, generate a stop line at a position where it will not collide.
@@ -73,13 +67,27 @@ struct StopLineIdx
  " @param use_stuck_stopline if true, a stop line is generated at the beginning of intersection lane
  * @return nullopt if path is not intersecting with detection areas
  */
-std::pair<std::optional<size_t>, std::optional<StopLineIdx>> generateStopLine(
+std::optional<StopLineIdx> generateStopLine(
   const int lane_id, const std::vector<lanelet::CompoundPolygon3d> & detection_areas,
-  const std::vector<lanelet::CompoundPolygon3d> & conflicting_areas,
+  const std::shared_ptr<const PlannerData> & planner_data, const double stop_line_margin,
+  autoware_auto_planning_msgs::msg::PathWithLaneId * original_path,
+  const autoware_auto_planning_msgs::msg::PathWithLaneId & path_ip, const double interval,
+  const std::pair<size_t, size_t> lane_interaval, const rclcpp::Logger logger);
+
+/**
+ * @brief Generate a stop line for stuck vehicle
+ * @param conflicting_areas used to generate stop line for stuck vehicle
+ * @param original_path   ego-car lane
+ * @param target_path     target lane to insert stop point (part of ego-car lane or same to ego-car
+ * lane)
+ " @param use_stuck_stopline if true, a stop line is generated at the beginning of intersection lane
+ */
+std::optional<size_t> generateStuckStopLine(
+  const int lane_id, const std::vector<lanelet::CompoundPolygon3d> & conflicting_areas,
   const std::shared_ptr<const PlannerData> & planner_data, const double stop_line_margin,
   const bool use_stuck_stopline, autoware_auto_planning_msgs::msg::PathWithLaneId * original_path,
-  const autoware_auto_planning_msgs::msg::PathWithLaneId & target_path, const rclcpp::Logger logger,
-  const rclcpp::Clock::SharedPtr clock);
+  const autoware_auto_planning_msgs::msg::PathWithLaneId & path_ip, const double ip_interval,
+  const std::pair<size_t, size_t> lane_interval, const rclcpp::Logger logger);
 
 /**
  * @brief Calculate first path index that is in the polygon.
@@ -136,7 +144,7 @@ bool isBeforeTargetIndex(
   const geometry_msgs::msg::Pose & current_pose, const int target_idx);
 
 lanelet::ConstLanelets extendedAdjacentDirectionLanes(
-  const lanelet::LaneletMapPtr map, const lanelet::routing::RoutingGraphPtr routing_graph,
+  lanelet::LaneletMapConstPtr map, const lanelet::routing::RoutingGraphPtr routing_graph,
   lanelet::ConstLanelet lane);
 
 std::optional<Polygon2d> getIntersectionArea(

planning/behavior_velocity_planner/include/scene_module/intersection/util_type.hpp

@@ -0,0 +1,43 @@
+// Copyright 2022 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SCENE_MODULE__INTERSECTION__UTIL_TYPE_HPP_
+#define SCENE_MODULE__INTERSECTION__UTIL_TYPE_HPP_
+
+#include <lanelet2_core/primitives/Lanelet.h>
+
+#include <vector>
+
+namespace behavior_velocity_planner::util
+{
+struct IntersectionLanelets
+{
+  bool tl_arrow_solid_on;
+  lanelet::ConstLanelets attention;
+  lanelet::ConstLanelets conflicting;
+  lanelet::ConstLanelets adjacent;
+  std::vector<lanelet::CompoundPolygon3d> attention_area;
+  std::vector<lanelet::CompoundPolygon3d> conflicting_area;
+  std::vector<lanelet::CompoundPolygon3d> adjacent_area;
+};
+
+struct StopLineIdx
+{
+  size_t pass_judge_line = 0;
+  size_t collision_stop_line = 0;
+};
+
+}  // namespace behavior_velocity_planner::util
+
+#endif  // SCENE_MODULE__INTERSECTION__UTIL_TYPE_HPP_

planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp

@@ -96,16 +96,19 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
 
   /* get detection area*/
   /* dynamically change detection area based on tl_arrow_solid_on */
-  [[maybe_unused]] const bool has_tl = util::hasAssociatedTrafficLight(assigned_lanelet);
   const bool tl_arrow_solid_on =
     util::isTrafficLightArrowActivated(assigned_lanelet, planner_data_->traffic_light_id_map);
-  auto && [detection_lanelets, conflicting_lanelets] = util::getObjectiveLanelets(
-    lanelet_map_ptr, routing_graph_ptr, lane_id_, planner_param_.detection_area_length,
-    tl_arrow_solid_on);
-  const std::vector<lanelet::CompoundPolygon3d> detection_area =
-    util::getPolygon3dFromLanelets(detection_lanelets, planner_param_.detection_area_length);
-  const std::vector<lanelet::CompoundPolygon3d> conflicting_area =
-    util::getPolygon3dFromLanelets(conflicting_lanelets);
+  if (
+    !intersection_lanelets_.has_value() ||
+    intersection_lanelets_.value().tl_arrow_solid_on != tl_arrow_solid_on) {
+    intersection_lanelets_ = util::getObjectiveLanelets(
+      lanelet_map_ptr, routing_graph_ptr, lane_id_, planner_param_.detection_area_length,
+      tl_arrow_solid_on);
+  }
+  const auto & detection_lanelets = intersection_lanelets_.value().attention;
+  const auto & adjacent_lanelets = intersection_lanelets_.value().adjacent;
+  const auto & detection_area = intersection_lanelets_.value().attention_area;
+  const auto & conflicting_area = intersection_lanelets_.value().conflicting_area;
   debug_data_.detection_area = detection_area;
 
   /* get intersection area */
@@ -115,25 +118,34 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
     debug_data_.intersection_area = toGeomPoly(intersection_area_2d);
   }
 
-  /* get adjacent lanelets */
-  const auto adjacent_lanelets =
-    util::extendedAdjacentDirectionLanes(lanelet_map_ptr, routing_graph_ptr, assigned_lanelet);
-  debug_data_.adjacent_area = util::getPolygon3dFromLanelets(adjacent_lanelets);
-
-  /* set stop lines for base_link */
-  const auto [stuck_line_idx_opt, stop_lines_idx_opt] = util::generateStopLine(
-    lane_id_, detection_area, conflicting_area, planner_data_, planner_param_.stop_line_margin,
-    planner_param_.use_stuck_stopline, path, *path, logger_.get_child("util"), clock_);
-  if (!stuck_line_idx_opt.has_value()) {
-    // returns here if path is not intersecting with conflicting areas
-    RCLCPP_DEBUG_SKIPFIRST_THROTTLE(
-      logger_, *clock_, 1000 /* ms */, "setStopLineIdx for stuck line fail");
+  /* spline interpolation */
+  constexpr double interval = 0.2;
+  autoware_auto_planning_msgs::msg::PathWithLaneId path_ip;
+  if (!splineInterpolate(*path, interval, path_ip, logger_)) {
+    RCLCPP_DEBUG_SKIPFIRST_THROTTLE(logger_, *clock_, 1000 /* ms */, "splineInterpolate failed");
     RCLCPP_DEBUG(logger_, "===== plan end =====");
     setSafe(true);
     setDistance(std::numeric_limits<double>::lowest());
     return false;
   }
-  const auto stuck_line_idx = stuck_line_idx_opt.value();
+  const auto lane_interval_ip_opt = util::findLaneIdInterval(path_ip, lane_id_);
+  if (!lane_interval_ip_opt.has_value()) {
+    RCLCPP_WARN(logger_, "Path has no interval on intersection lane %ld", lane_id_);
+    RCLCPP_DEBUG(logger_, "===== plan end =====");
+    setSafe(true);
+    setDistance(std::numeric_limits<double>::lowest());
+    return false;
+  }
+
+  const auto stuck_line_idx_opt = util::generateStuckStopLine(
+    lane_id_, conflicting_area, planner_data_, planner_param_.stop_line_margin,
+    planner_param_.use_stuck_stopline, path, path_ip, interval, lane_interval_ip_opt.value(),
+    logger_.get_child("util"));
+
+  /* set stop lines for base_link */
+  const auto stop_lines_idx_opt = util::generateStopLine(
+    lane_id_, detection_area, planner_data_, planner_param_.stop_line_margin, path, path_ip,
+    interval, lane_interval_ip_opt.value(), logger_.get_child("util"));
 
   /* calc closest index */
   const auto closest_idx_opt =
@@ -149,7 +161,7 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
   const size_t closest_idx = closest_idx_opt.get();
 
   if (stop_lines_idx_opt.has_value()) {
-    const auto stop_line_idx = stop_lines_idx_opt.value().stop_line;
+    const auto stop_line_idx = stop_lines_idx_opt.value().collision_stop_line;
     const auto pass_judge_line_idx = stop_lines_idx_opt.value().pass_judge_line;
 
     const bool is_over_pass_judge_line =
@@ -189,29 +201,35 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
     closest_idx, stuck_vehicle_detect_area, time_delay);
 
   /* calculate final stop lines */
-  int stop_line_idx_final =
-    stop_lines_idx_opt.has_value() ? stop_lines_idx_opt.value().stop_line : -1;
+  std::optional<size_t> stop_line_idx = std::nullopt;
   if (external_go) {
     is_entry_prohibited = false;
   } else if (external_stop) {
     is_entry_prohibited = true;
-  } else if (is_stuck || has_collision) {
+    stop_line_idx = stop_lines_idx_opt.has_value()
+                      ? std::make_optional<size_t>(stop_lines_idx_opt.value().collision_stop_line)
+                      : std::nullopt;
+  } else if (is_stuck && stuck_line_idx_opt.has_value()) {
     is_entry_prohibited = true;
     const double dist_stuck_stopline = motion_utils::calcSignedArcLength(
-      path->points, path->points.at(stuck_line_idx).point.pose.position,
+      path->points, path->points.at(stuck_line_idx_opt.value()).point.pose.position,
       path->points.at(closest_idx).point.pose.position);
     const bool is_over_stuck_stopline =
-      util::isOverTargetIndex(*path, closest_idx, current_pose, stuck_line_idx) &&
+      util::isOverTargetIndex(*path, closest_idx, current_pose, stuck_line_idx_opt.value()) &&
       dist_stuck_stopline > planner_param_.stop_overshoot_margin;
-    if (is_stuck && !is_over_stuck_stopline) {
-      stop_line_idx_final = stuck_line_idx;
-    } else if (
-      ((is_stuck && is_over_stuck_stopline) || has_collision) && stop_lines_idx_opt.has_value()) {
-      stop_line_idx_final = stop_lines_idx_opt.value().stop_line;
+    if (!is_over_stuck_stopline) {
+      stop_line_idx = stuck_line_idx_opt.value();
+    } else if (is_over_stuck_stopline && stop_lines_idx_opt.has_value()) {
+      stop_line_idx = stop_lines_idx_opt.value().collision_stop_line;
     }
+  } else if (has_collision) {
+    is_entry_prohibited = true;
+    stop_line_idx = stop_lines_idx_opt.has_value()
+                      ? std::make_optional<size_t>(stop_lines_idx_opt.value().collision_stop_line)
+                      : std::nullopt;
   }
 
-  if (stop_line_idx_final == -1) {
+  if (!stop_line_idx.has_value()) {
     RCLCPP_DEBUG(logger_, "detection_area is empty, no plan needed");
     RCLCPP_DEBUG(logger_, "===== plan end =====");
     setSafe(true);
@@ -226,31 +244,31 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
   setSafe(state_machine_.getState() == StateMachine::State::GO);
   setDistance(motion_utils::calcSignedArcLength(
     path->points, planner_data_->current_odometry->pose.position,
-    path->points.at(stop_line_idx_final).point.pose.position));
+    path->points.at(stop_line_idx.value()).point.pose.position));
 
   if (!isActivated()) {
     // if RTC says intersection entry is 'dangerous', insert stop_line(v == 0.0) in this block
     is_go_out_ = false;
 
     constexpr double v = 0.0;
-    planning_utils::setVelocityFromIndex(stop_line_idx_final, v, path);
+    planning_utils::setVelocityFromIndex(stop_line_idx.value(), v, path);
     debug_data_.stop_required = true;
     const double base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m;
     debug_data_.stop_wall_pose =
-      planning_utils::getAheadPose(stop_line_idx_final, base_link2front, *path);
+      planning_utils::getAheadPose(stop_line_idx.value(), base_link2front, *path);
 
     // Get stop point and stop factor
     {
       tier4_planning_msgs::msg::StopFactor stop_factor;
-      stop_factor.stop_pose = path->points.at(stop_line_idx_final).point.pose;
+      stop_factor.stop_pose = path->points.at(stop_line_idx.value()).point.pose;
       const auto stop_factor_conflict =
         planning_utils::toRosPoints(debug_data_.conflicting_targets);
       const auto stop_factor_stuck = planning_utils::toRosPoints(debug_data_.stuck_targets);
       stop_factor.stop_factor_points =
         planning_utils::concatVector(stop_factor_conflict, stop_factor_stuck);
       planning_utils::appendStopReason(stop_factor, stop_reason);
 
-      const auto & stop_pose = path->points.at(stop_line_idx_final).point.pose;
+      const auto & stop_pose = path->points.at(stop_line_idx.value()).point.pose;
       velocity_factor_.set(
         path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::UNKNOWN);
     }

planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp

@@ -64,32 +64,42 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity(PathWithLaneId * path, StopR
   const auto routing_graph_ptr = planner_data_->route_handler_->getRoutingGraphPtr();
 
   /* get detection area */
-  auto && [detection_lanelets, conflicting_lanelets] = util::getObjectiveLanelets(
-    lanelet_map_ptr, routing_graph_ptr, lane_id_, planner_param_.detection_area_length,
-    false /* tl_arrow_solid on does not matter here*/);
-  if (detection_lanelets.empty()) {
-    RCLCPP_DEBUG(logger_, "no detection area. skip computation.");
-    return true;
+  if (!intersection_lanelets_.has_value()) {
+    intersection_lanelets_ = util::getObjectiveLanelets(
+      lanelet_map_ptr, routing_graph_ptr, lane_id_, planner_param_.detection_area_length,
+      false /* tl_arrow_solid on does not matter here*/);
   }
-  const auto detection_area =
-    util::getPolygon3dFromLanelets(detection_lanelets, planner_param_.detection_area_length);
-  const std::vector<lanelet::CompoundPolygon3d> conflicting_area =
-    util::getPolygon3dFromLanelets(conflicting_lanelets);
-  debug_data_.detection_area = detection_area;
+  const auto & detection_area = intersection_lanelets_.value().attention_area;
 
   /* set stop-line and stop-judgement-line for base_link */
-  const auto private_path =
-    extractPathNearExitOfPrivateRoad(*path, planner_data_->vehicle_info_.vehicle_length_m);
-  const auto [stuck_line_idx_opt, stop_lines_idx_opt] = util::generateStopLine(
-    lane_id_, detection_area, conflicting_area, planner_data_, planner_param_.stop_line_margin,
-    false /* same */, path, *path, logger_.get_child("util"), clock_);
+  /* spline interpolation */
+  constexpr double interval = 0.2;
+  autoware_auto_planning_msgs::msg::PathWithLaneId path_ip;
+  if (!splineInterpolate(*path, interval, path_ip, logger_)) {
+    RCLCPP_DEBUG_SKIPFIRST_THROTTLE(logger_, *clock_, 1000 /* ms */, "splineInterpolate failed");
+    RCLCPP_DEBUG(logger_, "===== plan end =====");
+    setSafe(true);
+    setDistance(std::numeric_limits<double>::lowest());
+    return false;
+  }
+  const auto lane_interval_ip_opt = util::findLaneIdInterval(path_ip, lane_id_);
+  if (!lane_interval_ip_opt.has_value()) {
+    RCLCPP_WARN(logger_, "Path has no interval on intersection lane %ld", lane_id_);
+    RCLCPP_DEBUG(logger_, "===== plan end =====");
+    setSafe(true);
+    setDistance(std::numeric_limits<double>::lowest());
+    return false;
+  }
+
+  const auto stop_lines_idx_opt = util::generateStopLine(
+    lane_id_, detection_area, planner_data_, planner_param_.stop_line_margin, path, path_ip,
+    interval, lane_interval_ip_opt.value(), logger_.get_child("util"));
   if (!stop_lines_idx_opt.has_value()) {
     RCLCPP_WARN_SKIPFIRST_THROTTLE(logger_, *clock_, 1000 /* ms */, "setStopLineIdx fail");
     return false;
   }
 
-  const auto & stop_lines_idx = stop_lines_idx_opt.value();
-  const size_t stop_line_idx = stop_lines_idx.stop_line;
+  const size_t stop_line_idx = stop_lines_idx_opt.value().collision_stop_line;
   if (stop_line_idx == 0) {
     RCLCPP_DEBUG(logger_, "stop line is at path[0], ignore planning.");
     return true;
@@ -98,8 +108,6 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity(PathWithLaneId * path, StopR
   debug_data_.virtual_wall_pose = planning_utils::getAheadPose(
     stop_line_idx, planner_data_->vehicle_info_.max_longitudinal_offset_m, *path);
   debug_data_.stop_point_pose = path->points.at(stop_line_idx).point.pose;
-  const size_t first_inside_lane_idx = stop_lines_idx.first_inside_lane;
-  debug_data_.first_collision_point = path->points.at(first_inside_lane_idx).point.pose.position;
 
   /* set stop speed */
   if (state_machine_.getState() == StateMachine::State::STOP) {
@@ -109,7 +117,6 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity(PathWithLaneId * path, StopR
     /* get stop point and stop factor */
     tier4_planning_msgs::msg::StopFactor stop_factor;
     stop_factor.stop_pose = debug_data_.stop_point_pose;
-    stop_factor.stop_factor_points.emplace_back(debug_data_.first_collision_point);
     planning_utils::appendStopReason(stop_factor, stop_reason);
     const auto & stop_pose = path->points.at(stop_line_idx).point.pose;
     velocity_factor_.set(