feat(intersection): consider lane change in intersection based on associative ids

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

@@ -44,12 +44,7 @@ class IntersectionModuleManager : public SceneModuleManagerInterfaceWithRTC
   std::function<bool(const std::shared_ptr<SceneModuleInterface> &)> getModuleExpiredFunction(
     const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override;
 
-  bool hasSameParentLaneletAndTurnDirectionWith(
-    const lanelet::ConstLanelet & lane, const size_t module_id,
-    const std::string & turn_direction_lane) const;
-
-  bool hasSameParentLaneletAndTurnDirectionWithRegistered(
-    const lanelet::ConstLanelet & lane, const std::string & turn_direction) const;
+  bool hasSameParentLaneletAndTurnDirectionWithRegistered(const lanelet::ConstLanelet & lane) const;
 };
 
 class MergeFromPrivateModuleManager : public SceneModuleManagerInterface
@@ -67,12 +62,7 @@ class MergeFromPrivateModuleManager : public SceneModuleManagerInterface
   std::function<bool(const std::shared_ptr<SceneModuleInterface> &)> getModuleExpiredFunction(
     const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override;
 
-  bool hasSameParentLaneletAndTurnDirectionWith(
-    const lanelet::ConstLanelet & lane, const size_t module_id,
-    const std::string & turn_direction_lane) const;
-
-  bool hasSameParentLaneletAndTurnDirectionWithRegistered(
-    const lanelet::ConstLanelet & lane, const std::string & turn_direction) const;
+  bool hasSameParentLaneletAndTurnDirectionWithRegistered(const lanelet::ConstLanelet & lane) const;
 };
 }  // namespace behavior_velocity_planner
 

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

@@ -33,6 +33,7 @@
 
 #include <algorithm>
 #include <memory>
+#include <set>
 #include <string>
 #include <utility>
 #include <vector>
@@ -49,14 +50,14 @@ class IntersectionModule : public SceneModuleInterface
   {
     bool stop_required;
 
-    geometry_msgs::msg::Pose slow_wall_pose;
     geometry_msgs::msg::Pose stop_wall_pose;
     geometry_msgs::msg::Polygon stuck_vehicle_detect_area;
     geometry_msgs::msg::Polygon candidate_collision_ego_lane_polygon;
     std::vector<geometry_msgs::msg::Polygon> candidate_collision_object_polygons;
     std::vector<lanelet::ConstLanelet> intersection_detection_lanelets;
     std::vector<lanelet::CompoundPolygon3d> detection_area;
     geometry_msgs::msg::Polygon intersection_area;
+    lanelet::CompoundPolygon3d ego_lane;
     std::vector<lanelet::CompoundPolygon3d> adjacent_area;
     autoware_auto_perception_msgs::msg::PredictedObjects conflicting_targets;
     autoware_auto_perception_msgs::msg::PredictedObjects stuck_targets;
@@ -97,8 +98,8 @@ class IntersectionModule : public SceneModuleInterface
 
   IntersectionModule(
     const int64_t module_id, const int64_t lane_id, std::shared_ptr<const PlannerData> planner_data,
-    const PlannerParam & planner_param, const rclcpp::Logger logger,
-    const rclcpp::Clock::SharedPtr clock);
+    const PlannerParam & planner_param, const std::set<int> & assoc_ids,
+    const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock);
 
   /**
    * @brief plan go-stop velocity at traffic crossing with collision check between reference path
@@ -109,14 +110,19 @@ class IntersectionModule : public SceneModuleInterface
   visualization_msgs::msg::MarkerArray createDebugMarkerArray() override;
   visualization_msgs::msg::MarkerArray createVirtualWallMarkerArray() override;
 
+  const std::set<int> & getAssocIds() const { return assoc_ids_; }
+
 private:
-  int64_t lane_id_;
+  const int64_t lane_id_;
   std::string turn_direction_;
   bool has_traffic_light_;
   bool is_go_out_;
   // Parameter
   PlannerParam planner_param_;
   std::optional<util::IntersectionLanelets> intersection_lanelets_;
+  // for an intersection lane l1, its associative lanes are those that share same parent lanelet and
+  // have same turn_direction
+  const std::set<int> assoc_ids_;
 
   /**
    * @brief check collision for all lanelet area & predicted objects (call checkPathCollision() as
@@ -135,9 +141,10 @@ class IntersectionModule : public SceneModuleInterface
     const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
     const lanelet::ConstLanelets & detection_area_lanelets,
     const lanelet::ConstLanelets & adjacent_lanelets,
-    const std::optional<Polygon2d> & intersection_area,
+    const std::optional<Polygon2d> & intersection_area, const lanelet::ConstLanelet & ego_lane,
+    const lanelet::ConstLanelets & ego_lane_with_next_lane,
     const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr,
-    const int closest_idx, const Polygon2d & stuck_vehicle_detect_area, const double time_delay);
+    const int closest_idx, const double time_delay);
 
   /**
    * @brief Check if there is a stopped vehicle on the ego-lane.
@@ -161,10 +168,9 @@ class IntersectionModule : public SceneModuleInterface
    * @param ignore_dist     ignore distance from the start point of the ego-intersection lane
    * @return generated polygon
    */
-  Polygon2d generateEgoIntersectionLanePolygon(
-    lanelet::LaneletMapConstPtr lanelet_map_ptr,
-    const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx,
-    const double extra_dist, const double ignore_dist) const;
+  Polygon2d generateStuckVehicleDetectAreaPolygon(
+    const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
+    const lanelet::ConstLanelets & ego_lane_with_next_lane, const int closest_idx) const;
 
   /**
    * @brief Modify objects predicted path. remove path point if the time exceeds timer_thr.
@@ -184,7 +190,7 @@ class IntersectionModule : public SceneModuleInterface
    */
   TimeDistanceArray calcIntersectionPassingTime(
     const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx,
-    const int objective_lane_id, const double time_delay) const;
+    const double time_delay) const;
 
   /**
    * @brief check if the object has a target type for collision check
@@ -221,14 +227,11 @@ class IntersectionModule : public SceneModuleInterface
     const double margin = 0);
 
   /**
-   * @brief Get lanes including ego lanelet and next lanelet
-   * @param lanelet_map_ptr lanelet map
-   * @param path            ego-car lane
-   * @return ego lanelet and next lanelet
+   * @brief Get path polygon of intersection part and next lane part
+   * @return trimmed path polygon
    */
   lanelet::ConstLanelets getEgoLaneWithNextLane(
-    lanelet::LaneletMapConstPtr lanelet_map_ptr,
-    const autoware_auto_planning_msgs::msg::PathWithLaneId & path) const;
+    const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const double width) const;
 
   /**
    * @brief Calculate distance between closest path point and intersection lanelet along path

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

@@ -31,6 +31,7 @@
 #include <lanelet2_routing/RoutingGraph.h>
 
 #include <memory>
+#include <set>
 #include <string>
 #include <vector>
 
@@ -73,8 +74,8 @@ class MergeFromPrivateRoadModule : public SceneModuleInterface
 
   MergeFromPrivateRoadModule(
     const int64_t module_id, const int64_t lane_id, std::shared_ptr<const PlannerData> planner_data,
-    const PlannerParam & planner_param, const rclcpp::Logger logger,
-    const rclcpp::Clock::SharedPtr clock);
+    const PlannerParam & planner_param, const std::set<int> & assoc_ids,
+    const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock);
 
   /**
    * @brief plan go-stop velocity at traffic crossing with collision check between reference path
@@ -85,10 +86,11 @@ class MergeFromPrivateRoadModule : public SceneModuleInterface
   visualization_msgs::msg::MarkerArray createDebugMarkerArray() override;
   visualization_msgs::msg::MarkerArray createVirtualWallMarkerArray() override;
 
+  const std::set<int> & getAssocIds() const { return assoc_ids_; }
+
 private:
-  int64_t lane_id_;
-  std::string turn_direction_;
-  bool has_traffic_light_;
+  const int64_t lane_id_;
+  const std::set<int> assoc_ids_;
 
   autoware_auto_planning_msgs::msg::PathWithLaneId extractPathNearExitOfPrivateRoad(
     const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const double extend_length);

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

@@ -30,6 +30,7 @@
 #include <map>
 #include <memory>
 #include <optional>
+#include <set>
 #include <string>
 #include <tuple>
 #include <utility>
@@ -43,9 +44,10 @@ std::optional<size_t> insertPoint(
   const geometry_msgs::msg::Pose & in_pose,
   autoware_auto_planning_msgs::msg::PathWithLaneId * inout_path);
 
-bool hasLaneId(const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p, const int id);
-std::optional<std::pair<size_t, size_t>> findLaneIdInterval(
-  const autoware_auto_planning_msgs::msg::PathWithLaneId & p, const int lane_id);
+bool hasLaneIds(
+  const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p, const std::set<int> & ids);
+std::optional<std::pair<size_t, size_t>> findLaneIdsInterval(
+  const autoware_auto_planning_msgs::msg::PathWithLaneId & p, const std::set<int> & ids);
 std::optional<size_t> getDuplicatedPointIdx(
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
   const geometry_msgs::msg::Point & point);
@@ -55,7 +57,8 @@ std::optional<size_t> getDuplicatedPointIdx(
  */
 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);
+  const int lane_id, const std::set<int> & assoc_ids, const double detection_area_length,
+  const bool tl_arrow_solid_on = false);
 
 /**
  * @brief Generate a stop line and insert it into the path. If the stop line is defined in the map,
@@ -83,7 +86,7 @@ std::optional<StopLineIdx> generateStopLine(
  " @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::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 & path_ip, const double ip_interval,
@@ -99,8 +102,7 @@ std::optional<size_t> generateStuckStopLine(
  */
 std::optional<size_t> getFirstPointInsidePolygons(
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t lane_interval_start,
-  const size_t lane_interval_end, const int lane_id,
-  const std::vector<lanelet::CompoundPolygon3d> & polygons);
+  const size_t lane_interval_end, const std::vector<lanelet::CompoundPolygon3d> & polygons);
 
 /**
  * @brief Get stop point from map if exists

planning/behavior_velocity_planner/include/utilization/util.hpp

@@ -169,7 +169,8 @@ geometry_msgs::msg::Pose getAheadPose(
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path);
 Polygon2d generatePathPolygon(
   const PathWithLaneId & path, const size_t start_idx, const size_t end_idx, const double width);
-
+lanelet::ConstLanelet generatePathLanelet(
+  const PathWithLaneId & path, const size_t start_idx, const size_t end_idx, const double width);
 double calcJudgeLineDistWithAccLimit(
   const double velocity, const double max_stop_acceleration, const double delay_response_time);
 
@@ -292,6 +293,27 @@ boost::optional<geometry_msgs::msg::Pose> insertStopPoint(
   const geometry_msgs::msg::Point & stop_point, PathWithLaneId & output);
 boost::optional<geometry_msgs::msg::Pose> insertStopPoint(
   const geometry_msgs::msg::Point & stop_point, const size_t stop_seg_idx, PathWithLaneId & output);
+
+/*
+  @brief return 'associatvie' lanes in the intersection. 'associative' means that a lane shares same
+  or lane-changeable parent lanes with `lane` and has same turn_direction value.
+ */
+std::set<int> getAssociativeIntersectionLanelets(
+  lanelet::ConstLanelet lane, const lanelet::LaneletMapPtr lanelet_map,
+  const lanelet::routing::RoutingGraphPtr routing_graph);
+
+template <template <class> class Container>
+lanelet::ConstLanelets getConstLaneletsFromIds(
+  lanelet::LaneletMapConstPtr map, const Container<int> & ids)
+{
+  lanelet::ConstLanelets ret{};
+  for (const auto & id : ids) {
+    const auto ll = map->laneletLayer.get(id);
+    ret.push_back(ll);
+  }
+  return ret;
+}
+
 }  // namespace planning_utils
 }  // namespace behavior_velocity_planner
 

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

@@ -133,7 +133,10 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray(
       0.0, 0.0, 0.5, 0.5),
     &debug_marker_array, now);
 
-  // TODO(Mamoru Sobue): should be changed path polygon for LC
+  appendMarkerArray(
+    createLaneletPolygonsMarkerArray({debug_data_.ego_lane}, "ego_lane", lane_id_, 1, 0.647, 0.0),
+    &debug_marker_array, now);
+
   appendMarkerArray(
     debug::createPolygonMarkerArray(
       debug_data_.candidate_collision_ego_lane_polygon, "candidate_collision_ego_lane_polygon",