refactor(intersection): code clean (autowarefoundation#2071)

* avoid lanelet <-> id as much as possible
* return detection_lanelets and conflicting_lanelets in getObjectiveLanelets
Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

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

@@ -174,8 +174,8 @@ class IntersectionModule : public SceneModuleInterface
   bool checkCollision(
     lanelet::LaneletMapConstPtr lanelet_map_ptr,
     const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
-    const std::vector<int> & detection_area_lanelet_ids,
-    const std::vector<int> & adjacent_lanelet_ids,
+    const lanelet::ConstLanelets & detection_area_lanelets,
+    const lanelet::ConstLanelets & adjacent_lanelets,
     const std::optional<Polygon2d> & intersection_area,
     const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr,
     const int closest_idx, const Polygon2d & stuck_vehicle_detect_area);
@@ -276,7 +276,7 @@ class IntersectionModule : public SceneModuleInterface
    * @return true if the given pose belongs to any target lanelet
    */
   bool checkAngleForTargetLanelets(
-    const geometry_msgs::msg::Pose & pose, const std::vector<int> & target_lanelet_ids,
+    const geometry_msgs::msg::Pose & pose, const lanelet::ConstLanelets & target_lanelet_ids,
     const double margin = 0);
 
   /**

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

@@ -30,6 +30,7 @@
 #include <memory>
 #include <optional>
 #include <string>
+#include <tuple>
 #include <vector>
 
 namespace behavior_velocity_planner
@@ -56,10 +57,9 @@ bool getDuplicatedPointIdx(
 /**
  * @brief get objective polygons for detection area
  */
-bool getDetectionLanelets(
+std::tuple<lanelet::ConstLanelets, lanelet::ConstLanelets> getObjectiveLanelets(
   lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr,
-  const int lane_id, const double detection_area_length,
-  lanelet::ConstLanelets * detection_lanelets_result, const bool tl_arrow_solid_on = false);
+  const int lane_id, const double detection_area_length, const bool tl_arrow_solid_on = false);
 
 struct StopLineIdx
 {
@@ -78,7 +78,7 @@ struct StopLineIdx
  * lane)
  * @param stop_line_idx   generated stop line index
  * @param pass_judge_line_idx  generated stop line index
- * @return false when generation failed
+ * @return false when path is not intersecting with detection area, or stop_line is behind path[0]
  */
 bool generateStopLine(
   const int lane_id, const std::vector<lanelet::CompoundPolygon3d> detection_areas,
@@ -118,9 +118,9 @@ int getFirstPointInsidePolygons(
  * @param stop_pose stop point defined on map
  * @return true when the stop point is defined on map.
  */
-bool getStopPoseIndexFromMap(
+bool getStopLineIndexFromMap(
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int lane_id,
-  const std::shared_ptr<const PlannerData> & planner_data, int & stop_idx_ip, int dist_thr,
+  const std::shared_ptr<const PlannerData> & planner_data, int * stop_idx_ip, int dist_thr,
   const rclcpp::Logger logger);
 
 std::vector<lanelet::CompoundPolygon3d> getPolygon3dFromLaneletsVec(
@@ -129,6 +129,9 @@ std::vector<lanelet::CompoundPolygon3d> getPolygon3dFromLaneletsVec(
 std::vector<lanelet::CompoundPolygon3d> getPolygon3dFromLanelets(
   const lanelet::ConstLanelets & ll_vec, double clip_length);
 
+std::vector<lanelet::CompoundPolygon3d> getPolygon3dFromLanelets(
+  const lanelet::ConstLanelets & ll_vec);
+
 std::vector<int> getLaneletIdsFromLaneletsVec(const std::vector<lanelet::ConstLanelets> & ll_vec);
 
 double calcArcLengthFromPath(
@@ -155,7 +158,7 @@ bool isBeforeTargetIndex(
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx,
   const geometry_msgs::msg::Pose & current_pose, const int target_idx);
 
-std::vector<int> extendedAdjacentDirectionLanes(
+lanelet::ConstLanelets extendedAdjacentDirectionLanes(
   const lanelet::LaneletMapPtr map, const lanelet::routing::RoutingGraphPtr routing_graph,
   lanelet::ConstLanelet lane);
 

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

@@ -84,7 +84,8 @@ bool IntersectionModule::modifyPathVelocity(
   RCLCPP_DEBUG(logger_, "lane_id = %ld, state = %s", lane_id_, toString(current_state).c_str());
 
   /* get current pose */
-  geometry_msgs::msg::PoseStamped current_pose = planner_data_->current_pose;
+  const geometry_msgs::msg::PoseStamped current_pose = planner_data_->current_pose;
+  const double current_vel = planner_data_->current_velocity->twist.linear.x;
 
   /* get lanelet map */
   const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr();
@@ -94,24 +95,22 @@ bool IntersectionModule::modifyPathVelocity(
     planner_data_->route_handler_->getLaneletMapPtr()->laneletLayer.get(lane_id_);
 
   /* get detection area*/
-  lanelet::ConstLanelets detection_area_lanelets;
+  /* dynamically change detection area based on tl_arrow_solid_on */
   const bool tl_arrow_solid_on =
     util::isTrafficLightArrowActivated(assigned_lanelet, planner_data_->traffic_light_id_map);
-  util::getDetectionLanelets(
+  auto && [detection_lanelets, conflicting_lanelets] = util::getObjectiveLanelets(
     lanelet_map_ptr, routing_graph_ptr, lane_id_, planner_param_.detection_area_length,
-    &detection_area_lanelets, tl_arrow_solid_on);
-  std::vector<lanelet::CompoundPolygon3d> detection_areas =
-    util::getPolygon3dFromLanelets(detection_area_lanelets, planner_param_.detection_area_length);
-  std::vector<int> detection_area_lanelet_ids =
-    util::getLaneletIdsFromLaneletsVec({detection_area_lanelets});
-
-  if (detection_areas.empty()) {
+    tl_arrow_solid_on);
+  if (detection_lanelets.empty()) {
     RCLCPP_DEBUG(logger_, "no detection area. skip computation.");
     setSafe(true);
     setDistance(std::numeric_limits<double>::lowest());
     return true;
   }
-  debug_data_.detection_area = detection_areas;
+  const std::vector<lanelet::CompoundPolygon3d> detection_area =
+    util::getPolygon3dFromLanelets(detection_lanelets, planner_param_.detection_area_length);
+
+  debug_data_.detection_area = detection_area;
 
   /* get intersection area */
   const auto intersection_area = util::getIntersectionArea(assigned_lanelet, lanelet_map_ptr);
@@ -121,18 +120,14 @@ bool IntersectionModule::modifyPathVelocity(
   }
 
   /* get adjacent lanelets */
-  const auto adjacent_lanelet_ids =
+  auto adjacent_lanelets =
     util::extendedAdjacentDirectionLanes(lanelet_map_ptr, routing_graph_ptr, assigned_lanelet);
-  std::vector<lanelet::CompoundPolygon3d> adjacent_lanes;
-  for (auto && adjacent_lanelet_id : adjacent_lanelet_ids) {
-    adjacent_lanes.push_back(lanelet_map_ptr->laneletLayer.get(adjacent_lanelet_id).polygon3d());
-  }
-  debug_data_.adjacent_area = adjacent_lanes;
+  debug_data_.adjacent_area = util::getPolygon3dFromLanelets(adjacent_lanelets);
 
   /* set stop-line and stop-judgement-line for base_link */
   util::StopLineIdx stop_line_idxs;
   if (!util::generateStopLine(
-        lane_id_, detection_areas, planner_data_, planner_param_.stop_line_margin,
+        lane_id_, detection_area, planner_data_, planner_param_.stop_line_margin,
         planner_param_.keep_detection_line_margin, path, *path, &stop_line_idxs,
         logger_.get_child("util"))) {
     RCLCPP_WARN_SKIPFIRST_THROTTLE(logger_, *clock_, 1000 /* ms */, "setStopLineIdx fail");
@@ -143,13 +138,6 @@ bool IntersectionModule::modifyPathVelocity(
   const int stop_line_idx = stop_line_idxs.stop_line_idx;
   const int pass_judge_line_idx = stop_line_idxs.pass_judge_line_idx;
   const int keep_detection_line_idx = stop_line_idxs.keep_detection_line_idx;
-  if (stop_line_idx <= 0) {
-    RCLCPP_DEBUG(logger_, "stop line line is at path[0], ignore planning.");
-    RCLCPP_DEBUG(logger_, "===== plan end =====");
-    setSafe(true);
-    setDistance(std::numeric_limits<double>::lowest());
-    return true;
-  }
 
   /* calc closest index */
   int closest_idx = -1;
@@ -168,15 +156,16 @@ bool IntersectionModule::modifyPathVelocity(
      */
     const bool is_before_keep_detection_line =
       util::isBeforeTargetIndex(*path, closest_idx, current_pose.pose, keep_detection_line_idx);
-    if (
-      is_before_keep_detection_line && std::fabs(planner_data_->current_velocity->twist.linear.x) <
-                                         planner_param_.keep_detection_vel_thr) {
+    const bool keep_detection = is_before_keep_detection_line &&
+                                std::fabs(current_vel) < planner_param_.keep_detection_vel_thr;
+    if (keep_detection) {
       RCLCPP_DEBUG(
         logger_,
         "over the pass judge line, but before keep detection line and low speed, "
         "continue planning");
       // no return here, keep planning
     } else if (is_go_out_ && !external_stop) {
+      // TODO(Mamoru Sobue): refactor this block
       RCLCPP_DEBUG(logger_, "over the keep_detection line and not low speed. no plan needed.");
       RCLCPP_DEBUG(logger_, "===== plan end =====");
       setSafe(true);
@@ -196,10 +185,11 @@ bool IntersectionModule::modifyPathVelocity(
   const auto stuck_vehicle_detect_area = generateEgoIntersectionLanePolygon(
     lanelet_map_ptr, *path, closest_idx, stop_line_idx, detect_length,
     planner_param_.stuck_vehicle_detect_dist);
-  bool is_stuck = checkStuckVehicleInIntersection(objects_ptr, stuck_vehicle_detect_area);
-  bool has_collision = checkCollision(
-    lanelet_map_ptr, *path, detection_area_lanelet_ids, adjacent_lanelet_ids, intersection_area,
-    objects_ptr, closest_idx, stuck_vehicle_detect_area);
+  const bool is_stuck = checkStuckVehicleInIntersection(objects_ptr, stuck_vehicle_detect_area);
+  const bool has_collision = checkCollision(
+    lanelet_map_ptr, *path, detection_lanelets, adjacent_lanelets, intersection_area, objects_ptr,
+    closest_idx, stuck_vehicle_detect_area);
+  // TODO(Mamoru Sobue): process external_go at the top of this function
   bool is_entry_prohibited = (has_collision || is_stuck);
   if (external_go) {
     is_entry_prohibited = false;
@@ -217,10 +207,12 @@ bool IntersectionModule::modifyPathVelocity(
     path->points.at(stop_line_idx).point.pose.position));
 
   if (!isActivated()) {
+    // if RTC says intersection entry is 'dangerous', insert stop_line(v == 0.0) in this block
     constexpr double v = 0.0;
     is_go_out_ = false;
     int stop_line_idx_stop = stop_line_idx;
     int pass_judge_line_idx_stop = pass_judge_line_idx;
+    // TODO(Mamoru Sobue): refactor this block
     if (planner_param_.use_stuck_stopline && is_stuck) {
       int stuck_stop_line_idx = -1;
       int stuck_pass_judge_line_idx = -1;
@@ -281,8 +273,9 @@ void IntersectionModule::cutPredictPathWithDuration(
 bool IntersectionModule::checkCollision(
   lanelet::LaneletMapConstPtr lanelet_map_ptr,
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
-  const std::vector<int> & detection_area_lanelet_ids,
-  const std::vector<int> & adjacent_lanelet_ids, const std::optional<Polygon2d> & intersection_area,
+  const lanelet::ConstLanelets & detection_area_lanelets,
+  const lanelet::ConstLanelets & adjacent_lanelets,
+  const std::optional<Polygon2d> & intersection_area,
   const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr,
   const int closest_idx, const Polygon2d & stuck_vehicle_detect_area)
 {
@@ -330,18 +323,17 @@ bool IntersectionModule::checkCollision(
       const auto intersection_area_2d = intersection_area.value();
       const auto is_in_intersection_area = bg::within(obj_poly, intersection_area_2d);
       const auto is_in_adjacent_lanelets = checkAngleForTargetLanelets(
-        object_direction, adjacent_lanelet_ids, planner_param_.detection_area_margin);
+        object_direction, adjacent_lanelets, planner_param_.detection_area_margin);
       if (is_in_adjacent_lanelets) continue;
       if (is_in_intersection_area) {
         target_objects.objects.push_back(object);
       } else if (checkAngleForTargetLanelets(
-                   object_direction, detection_area_lanelet_ids,
+                   object_direction, detection_area_lanelets,
                    planner_param_.detection_area_margin)) {
         target_objects.objects.push_back(object);
       }
     } else if (checkAngleForTargetLanelets(
-                 object_direction, detection_area_lanelet_ids,
-                 planner_param_.detection_area_margin)) {
+                 object_direction, detection_area_lanelets, planner_param_.detection_area_margin)) {
       // intersection_area is not available, use detection_area_with_margin as before
       target_objects.objects.push_back(object);
     }
@@ -683,11 +675,10 @@ bool IntersectionModule::isTargetExternalInputStatus(const int target_status)
 }
 
 bool IntersectionModule::checkAngleForTargetLanelets(
-  const geometry_msgs::msg::Pose & pose, const std::vector<int> & target_lanelet_ids,
+  const geometry_msgs::msg::Pose & pose, const lanelet::ConstLanelets & target_lanelets,
   const double margin)
 {
-  for (const int lanelet_id : target_lanelet_ids) {
-    const auto ll = planner_data_->route_handler_->getLaneletMapPtr()->laneletLayer.get(lanelet_id);
+  for (const auto & ll : target_lanelets) {
     if (!lanelet::utils::isInLanelet(pose, ll, margin)) {
       continue;
     }
@@ -738,7 +729,7 @@ double IntersectionModule::calcDistanceUntilIntersectionLanelet(
     return 0.0;
   }
 
-  double distance = util::calcArcLengthFromPath(path, closest_idx, dst_idx);
+  double distance = motion_utils::calcSignedArcLength(path.points, closest_idx, dst_idx);
   const auto & lane_first_point = assigned_lanelet.centerline2d().front();
   distance += std::hypot(
     path.points.at(dst_idx).point.pose.position.x - lane_first_point.x(),

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

@@ -65,24 +65,23 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity(
   const auto routing_graph_ptr = planner_data_->route_handler_->getRoutingGraphPtr();
 
   /* get detection area */
-  lanelet::ConstLanelets detection_area_lanelets;
-  util::getDetectionLanelets(
+  auto && [detection_lanelets, conflicting_lanelets] = util::getObjectiveLanelets(
     lanelet_map_ptr, routing_graph_ptr, lane_id_, planner_param_.detection_area_length,
-    &detection_area_lanelets);
-  std::vector<lanelet::CompoundPolygon3d> detection_areas =
-    util::getPolygon3dFromLanelets(detection_area_lanelets, planner_param_.detection_area_length);
-  if (detection_areas.empty()) {
+    false /* tl_arrow_solid on does not matter here*/);
+  if (detection_lanelets.empty()) {
     RCLCPP_DEBUG(logger_, "no detection area. skip computation.");
     return true;
   }
-  debug_data_.detection_area = detection_areas;
+  const auto detection_area =
+    util::getPolygon3dFromLanelets(detection_lanelets, planner_param_.detection_area_length);
+  debug_data_.detection_area = detection_area;
 
   /* set stop-line and stop-judgement-line for base_link */
   util::StopLineIdx stop_line_idxs;
   const auto private_path =
     extractPathNearExitOfPrivateRoad(*path, planner_data_->vehicle_info_.vehicle_length_m);
   if (!util::generateStopLine(
-        lane_id_, detection_areas, planner_data_, planner_param_.stop_line_margin,
+        lane_id_, detection_area, planner_data_, planner_param_.stop_line_margin,
         0.0 /* unnecessary in merge_from_private */, path, private_path, &stop_line_idxs,
         logger_.get_child("util"))) {
     RCLCPP_WARN_SKIPFIRST_THROTTLE(logger_, *clock_, 1000 /* ms */, "setStopLineIdx fail");