Merge pull request #304 from tier4/feat/pr2874

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

@@ -25,6 +25,7 @@
 
 #include <functional>
 #include <memory>
+#include <string>
 
 namespace behavior_velocity_planner
 {
@@ -43,9 +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 hasSameParentLaneletWith(const lanelet::ConstLanelet & lane, const size_t module_id) const;
-
-  bool hasSameParentLaneletWithRegistered(const lanelet::ConstLanelet & lane) const;
+  bool hasSameParentLaneletAndTurnDirectionWithRegistered(const lanelet::ConstLanelet & lane) const;
 };
 
 class MergeFromPrivateModuleManager : public SceneModuleManagerInterface
@@ -63,7 +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 hasSameParentLaneletWith(const lanelet::ConstLanelet & lane, const size_t module_id) const;
+  bool hasSameParentLaneletAndTurnDirectionWithRegistered(const lanelet::ConstLanelet & lane) const;
 };
 }  // namespace behavior_velocity_planner
 

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

@@ -31,6 +31,7 @@
 
 #include <algorithm>
 #include <memory>
+#include <set>
 #include <string>
 #include <utility>
 #include <vector>
@@ -47,7 +48,6 @@ class IntersectionModule : public SceneModuleInterface
   {
     bool stop_required;
 
-    geometry_msgs::msg::Pose slow_wall_pose;
     geometry_msgs::msg::Pose stop_wall_pose;
     geometry_msgs::msg::Pose stop_point_pose;
     geometry_msgs::msg::Pose judge_point_pose;
@@ -58,6 +58,7 @@ class IntersectionModule : public SceneModuleInterface
     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;
@@ -99,8 +100,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
@@ -111,14 +112,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_;
+  // 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
@@ -137,9 +143,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 int closest_idx);
 
   /**
    * @brief Check if there is a stopped vehicle on the ego-lane.
@@ -163,10 +170,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.
@@ -185,8 +191,7 @@ class IntersectionModule : public SceneModuleInterface
    * @return calculated time [s]
    */
   TimeDistanceArray calcIntersectionPassingTime(
-    const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx,
-    const int objective_lane_id) const;
+    const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx) const;
 
   /**
    * @brief check if the object has a target type for collision check
@@ -223,14 +228,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

@@ -29,6 +29,7 @@
 #include <map>
 #include <memory>
 #include <optional>
+#include <set>
 #include <string>
 #include <tuple>
 #include <utility>
@@ -42,9 +43,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);
@@ -75,7 +77,8 @@ struct StopLineIdx
  * @return nullopt if path is not intersecting with detection areas
  */
 std::pair<std::optional<size_t>, std::optional<StopLineIdx>> generateStopLine(
-  const int lane_id, const std::vector<lanelet::CompoundPolygon3d> & detection_areas,
+  const int lane_id, const std::set<int> & assoc_ids,
+  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,
   const double keep_detection_line_margin, const bool use_stuck_stopline,
@@ -93,8 +96,7 @@ std::pair<std::optional<size_t>, std::optional<StopLineIdx>> generateStopLine(
  */
 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

@@ -128,17 +128,16 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray(
       0.0),
     &debug_marker_array);
 
-  appendMarkerArray(
-    debug::createPolygonMarkerArray(
-      debug_data_.ego_lane_polygon, "ego_lane", lane_id_, now, 0.3, 0.0, 0.0, 0.0, 0.3, 0.7),
-    &debug_marker_array);
-
   appendMarkerArray(
     debug::createPolygonMarkerArray(
       debug_data_.stuck_vehicle_detect_area, "stuck_vehicle_detect_area", lane_id_, now, 0.3, 0.0,
       0.0, 0.0, 0.5, 0.5),
     &debug_marker_array, now);
 
+  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",
@@ -189,22 +188,15 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createVirtualWallMarker
   visualization_msgs::msg::MarkerArray wall_marker;
 
   const auto now = this->clock_->now();
-  const auto state = state_machine_.getState();
 
   if (debug_data_.stop_required) {
     appendMarkerArray(
       motion_utils::createStopVirtualWallMarker(
         debug_data_.stop_wall_pose, "intersection", now, module_id_),
       &wall_marker, now);
-  } else if (state == StateMachine::State::STOP) {
-    appendMarkerArray(
-      motion_utils::createStopVirtualWallMarker(
-        debug_data_.slow_wall_pose, "intersection", now, module_id_),
-      &wall_marker, now);
   }
   return wall_marker;
 }
-
 visualization_msgs::msg::MarkerArray MergeFromPrivateRoadModule::createDebugMarkerArray()
 {
   visualization_msgs::msg::MarkerArray debug_marker_array;