feat(behavior_velocity_planner::intersection): add occlusion detectio…

…n feature (autowarefoundation#3458)

* migrated

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

* WIP

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

* IntersectionModuleManager own one rtc_interface for intersection_occlusion

Signed-off-by: Mamoru Sobue <hilo.soblin@gmail.com>

* divide occlusion safety and activated

Signed-off-by: Mamoru Sobue <hilo.soblin@gmail.com>

* fixed to update occlusion cooperate status at construction

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

* fixed getOcclusionFirstStopSafety()

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

* fixed not to show both intersection and intersection_occlusion  after passed 1st stop line

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

* fixed the intersection_occlusion/inersection stop position afte r CLEARED

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

* if occlusion cleared when eog is before 1st stop line, set stop line to 1st stop line and clear prev_occlusion_stop_line_pose_

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

* (misc) fix viz, sync param

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

* fixed vehicle footprint offset calculation

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

* add occcupancy_grid_map method/param var to launcher

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

* migrated latest

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

* use static pass judge line

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

* removed some params

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

* organized param

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

* add occlusion enable flag

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

* revert occupancy grid settings in this PR

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

* remove comment

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

* fixed pass judge line collision detection to original

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

* style(pre-commit): autofix

* use vehicle_length for static pass judge line

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

* fixed virtual wall marker

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

---------

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>
Signed-off-by: Mamoru Sobue <hilo.soblin@gmail.com>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Signed-off-by: Mingyu Li <mingyu.li@tier4.jp>

common/rtc_manager_rviz_plugin/src/rtc_manager_panel.cpp

@@ -90,6 +90,9 @@ std::string getModuleName(const uint8_t module_type)
     case Module::OCCLUSION_SPOT: {
       return "occlusion_spot";
     }
+    case Module::INTERSECTION_OCCLUSION: {
+      return "intersection_occlusion";
+    }
   }
   return "NONE";
 }
@@ -111,7 +114,7 @@ bool isPathChangeModule(const uint8_t module_type)
 RTCManagerPanel::RTCManagerPanel(QWidget * parent) : rviz_common::Panel(parent)
 {
   // TODO(tanaka): replace this magic number to Module::SIZE
-  const size_t module_size = 18;
+  const size_t module_size = 19;
   auto_modes_.reserve(module_size);
   auto * v_layout = new QVBoxLayout;
   auto vertical_header = new QHeaderView(Qt::Vertical);

planning/behavior_velocity_planner/config/intersection.param.yaml

@@ -28,5 +28,16 @@
         collision_end_margin_time: 6.0 # [s] this + state_transit_margin_time should be higher to account for collision with slow/decelerating object
         keep_detection_vel_thr: 0.833 # == 3.0km/h. keep detection if ego is ego.vel < keep_detection_vel_thr
 
+      occlusion:
+        enable: true
+        occlusion_detection_area_length: 70.0 # [m]
+        occlusion_creep_velocity: 0.8333 # the creep velocity to occlusion limit stop line
+        free_space_max: 43
+        occupied_min: 58
+        do_dp: true
+        before_creep_stop_time: 1.0 # [s]
+        min_vehicle_brake_for_rss: -2.5 # [m/s^2]
+        max_vehicle_velocity_for_rss: 16.66 # [m/s] == 60kmph
+
     merge_from_private:
       stop_duration_sec: 1.0

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

@@ -26,6 +26,7 @@
 #include <functional>
 #include <memory>
 #include <string>
+#include <unordered_map>
 
 namespace behavior_velocity_planner
 {
@@ -38,13 +39,22 @@ class IntersectionModuleManager : public SceneModuleManagerInterfaceWithRTC
 
 private:
   IntersectionModule::PlannerParam intersection_param_;
+  // additional for INTERSECTION_OCCLUSION
+  RTCInterface occlusion_rtc_interface_;
+  std::unordered_map<int64_t, UUID> occlusion_map_uuid_;
 
   void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override;
 
   std::function<bool(const std::shared_ptr<SceneModuleInterface> &)> getModuleExpiredFunction(
     const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override;
 
   bool hasSameParentLaneletAndTurnDirectionWithRegistered(const lanelet::ConstLanelet & lane) const;
+
+  /* called from SceneModuleInterfaceWithRTC::plan */
+  void sendRTC(const Time & stamp) override;
+  void setActivation() override;
+  /* called from SceneModuleInterface::updateSceneModuleInstances */
+  void deleteExpiredModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override;
 };
 
 class MergeFromPrivateModuleManager : public SceneModuleManagerInterface

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

@@ -15,6 +15,7 @@
 #ifndef SCENE_MODULE__INTERSECTION__SCENE_INTERSECTION_HPP_
 #define SCENE_MODULE__INTERSECTION__SCENE_INTERSECTION_HPP_
 
+#include <grid_map_core/grid_map_core.hpp>
 #include <motion_utils/motion_utils.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <scene_module/intersection/util_type.hpp>
@@ -27,6 +28,7 @@
 #include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
 #include <geometry_msgs/msg/point.hpp>
+#include <grid_map_msgs/msg/grid_map.hpp>
 
 #include <lanelet2_core/LaneletMap.h>
 #include <lanelet2_routing/RoutingGraph.h>
@@ -48,9 +50,9 @@ class IntersectionModule : public SceneModuleInterface
 public:
   struct DebugData
   {
-    bool stop_required = false;
-
-    geometry_msgs::msg::Pose stop_wall_pose;
+    geometry_msgs::msg::Pose collision_stop_wall_pose;
+    geometry_msgs::msg::Pose occlusion_stop_wall_pose;
+    geometry_msgs::msg::Pose occlusion_first_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;
@@ -62,6 +64,8 @@ class IntersectionModule : public SceneModuleInterface
     autoware_auto_perception_msgs::msg::PredictedObjects conflicting_targets;
     autoware_auto_perception_msgs::msg::PredictedObjects stuck_targets;
     geometry_msgs::msg::Pose predicted_obj_pose;
+    geometry_msgs::msg::Point nearest_occlusion_point;
+    geometry_msgs::msg::Point nearest_occlusion_projection_point;
   };
 
 public:
@@ -102,12 +106,33 @@ class IntersectionModule : public SceneModuleInterface
       double collision_end_margin_time;       //! end margin time to check collision
       double keep_detection_vel_thr;  //! keep detection if ego is ego.vel < keep_detection_vel_thr
     } collision_detection;
+    struct Occlusion
+    {
+      bool enable;
+      double occlusion_detection_area_length;  //! used for occlusion detection
+      double occlusion_creep_velocity;         //! the creep velocity to occlusion limit stop lline
+      int free_space_max;
+      int occupied_min;
+      bool do_dp;
+      double before_creep_stop_time;
+      double min_vehicle_brake_for_rss;
+      double max_vehicle_velocity_for_rss;
+    } occlusion;
+  };
+
+  enum OcclusionState {
+    NONE,
+    BEFORE_FIRST_STOP_LINE,
+    WAIT_FIRST_STOP_LINE,
+    CREEP_SECOND_STOP_LINE,
+    CLEARED,
   };
 
   IntersectionModule(
     const int64_t module_id, const int64_t lane_id, std::shared_ptr<const PlannerData> planner_data,
     const PlannerParam & planner_param, const std::set<int> & assoc_ids,
-    const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock);
+    const bool enable_occlusion_detection, rclcpp::Node & node, const rclcpp::Logger logger,
+    const rclcpp::Clock::SharedPtr clock);
 
   /**
    * @brief plan go-stop velocity at traffic crossing with collision check between reference path
@@ -120,18 +145,50 @@ class IntersectionModule : public SceneModuleInterface
 
   const std::set<int> & getAssocIds() const { return assoc_ids_; }
 
+  UUID getOcclusionUUID() const { return occlusion_uuid_; }
+  bool getOcclusionSafety() const { return occlusion_safety_; }
+  double getOcclusionDistance() const { return occlusion_stop_distance_; }
+  UUID getOcclusionFirstStopUUID() const { return occlusion_first_stop_uuid_; }
+  bool getOcclusionFirstStopSafety() const { return occlusion_first_stop_safety_; }
+  double getOcclusionFirstStopDistance() const { return occlusion_first_stop_distance_; }
+  void setOcclusionActivation(const bool activation) { occlusion_activated_ = activation; }
+  void setOcclusionFirstStopActivation(const bool activation)
+  {
+    occlusion_first_stop_activated_ = activation;
+  }
+
 private:
+  rclcpp::Node & node_;
   const int64_t lane_id_;
   std::string turn_direction_;
-  bool has_traffic_light_;
-  bool is_go_out_;
+  bool is_go_out_ = false;
   // 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_;
 
+  // for occlusion detection
+  const bool enable_occlusion_detection_;
+  std::optional<std::vector<util::DetectionLaneDivision>> detection_divisions_;
+  std::optional<geometry_msgs::msg::Pose> prev_occlusion_stop_line_pose_;
+  OcclusionState occlusion_state_;
+  // NOTE: uuid_ is base member
+  // for occlusion clearance decision
+  const UUID occlusion_uuid_;
+  bool occlusion_safety_;
+  double occlusion_stop_distance_;
+  bool occlusion_activated_;
+  // for first stop in two-phase stop
+  const UUID occlusion_first_stop_uuid_;  // TODO(Mamoru Sobue): replace with uuid_
+  bool occlusion_first_stop_safety_;
+  double occlusion_first_stop_distance_;
+  bool occlusion_first_stop_activated_;
+
+  StateMachine collision_state_machine_;     //! for stable collision checking
+  StateMachine before_creep_state_machine_;  //! for two phase stop
+
   /**
    * @brief check collision for all lanelet area & predicted objects (call checkPathCollision() as
    * actual collision check algorithm inside this function)
@@ -258,13 +315,22 @@ class IntersectionModule : public SceneModuleInterface
     lanelet::ConstLanelets & ego_lane_with_next_lane, lanelet::ConstLanelet & closest_lanelet,
     const Polygon2d & stuck_vehicle_detect_area,
     const autoware_auto_perception_msgs::msg::PredictedObject & object) const;
-  StateMachine state_machine_;  //! for state
+
+  std::optional<size_t> findNearestOcclusionProjectedPosition(
+    const nav_msgs::msg::OccupancyGrid & occ_grid,
+    const std::vector<lanelet::CompoundPolygon3d> & detection_areas,
+    const lanelet::CompoundPolygon3d & first_detection_area,
+    const autoware_auto_planning_msgs::msg::PathWithLaneId & path_ip, const double interval,
+    const std::pair<size_t, size_t> & lane_interval,
+    const std::vector<util::DetectionLaneDivision> & lane_divisions,
+    const double occlusion_dist_thr) const;
 
   // Debug
   mutable DebugData debug_data_;
 
   std::shared_ptr<motion_utils::VirtualWallMarkerCreator> virtual_wall_marker_creator_ =
     std::make_shared<motion_utils::VirtualWallMarkerCreator>();
+  rclcpp::Publisher<grid_map_msgs::msg::GridMap>::SharedPtr occlusion_grid_pub_;
 };
 }  // namespace behavior_velocity_planner
 

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

@@ -60,7 +60,7 @@ IntersectionLanelets getObjectiveLanelets(
   const int lane_id, const std::set<int> & assoc_ids,
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
   const std::pair<size_t, size_t> lane_interval, const double detection_area_length,
-  const bool tl_arrow_solid_on = false);
+  const double occlusion_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,
@@ -72,7 +72,7 @@ IntersectionLanelets getObjectiveLanelets(
  " @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::optional<StopLineIdx> generateStopLine(
+std::optional<size_t> generateCollisionStopLine(
   const int lane_id, const lanelet::CompoundPolygon3d & first_detection_area,
   const std::shared_ptr<const PlannerData> & planner_data, const double stop_line_margin,
   autoware_auto_planning_msgs::msg::PathWithLaneId * original_path,
@@ -94,6 +94,25 @@ std::optional<size_t> generateStuckStopLine(
   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);
 
+std::optional<std::pair<size_t, size_t>> generateOcclusionStopLines(
+  const int lane_id, const std::vector<lanelet::CompoundPolygon3d> & detection_areas,
+  const std::shared_ptr<const PlannerData> & planner_data, const double collision_stop_line_margin,
+  const size_t occlusion_projection_index, const double occlusion_extra_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);
+
+std::optional<size_t> generateStaticPassJudgeLine(
+  const lanelet::CompoundPolygon3d & first_detection_area,
+  autoware_auto_planning_msgs::msg::PathWithLaneId * original_path,
+  const autoware_auto_planning_msgs::msg::PathWithLaneId & path_ip, const double ip_interval,
+  const std::pair<size_t, size_t> lane_interval,
+  const std::shared_ptr<const PlannerData> & planner_data);
+
+std::optional<size_t> getFirstPointInsidePolygon(
+  const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
+  const std::pair<size_t, size_t> lane_interval, const lanelet::CompoundPolygon3d & polygon);
+
 /**
  * @brief Calculate first path index that is in the polygon.
  * @param path     target path
@@ -160,6 +179,10 @@ bool isTrafficLightArrowActivated(
   lanelet::ConstLanelet lane,
   const std::map<int, autoware_auto_perception_msgs::msg::TrafficSignalStamped> & tl_infos);
 
+std::vector<DetectionLaneDivision> generateDetectionLaneDivisions(
+  lanelet::ConstLanelets detection_lanelets,
+  const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution);
+
 }  // namespace util
 }  // namespace behavior_velocity_planner
 

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

@@ -29,20 +29,29 @@ struct IntersectionLanelets
   lanelet::ConstLanelets attention;
   lanelet::ConstLanelets conflicting;
   lanelet::ConstLanelets adjacent;
+  lanelet::ConstLanelets occlusion_attention;  // for occlusion detection
   std::vector<lanelet::CompoundPolygon3d> attention_area;
   std::vector<lanelet::CompoundPolygon3d> conflicting_area;
   std::vector<lanelet::CompoundPolygon3d> adjacent_area;
+  std::vector<lanelet::CompoundPolygon3d> occlusion_attention_area;
   // the first area intersecting with the path
   // even if lane change/re-routing happened on the intersection, these areas area are supposed to
   // be invariant under the 'associative' lanes.
   std::optional<lanelet::CompoundPolygon3d> first_conflicting_area;
   std::optional<lanelet::CompoundPolygon3d> first_detection_area;
 };
 
-struct StopLineIdx
+enum class StopReason : int {
+  STUCK,
+  COLLISION,
+  OCCLUSION,
+};
+
+struct DetectionLaneDivision
 {
-  size_t pass_judge_line = 0;
-  size_t collision_stop_line = 0;
+  int lane_id;
+  // discrete fine lines from left to right
+  std::vector<lanelet::ConstLineString2d> divisions;
 };
 
 }  // namespace behavior_velocity_planner::util

planning/behavior_velocity_planner/include/scene_module/scene_module_interface.hpp

@@ -58,6 +58,7 @@ using tier4_autoware_utils::StopWatch;
 using tier4_debug_msgs::msg::Float64Stamped;
 using tier4_planning_msgs::msg::StopFactor;
 using tier4_planning_msgs::msg::StopReason;
+using tier4_rtc_msgs::msg::Module;
 using unique_identifier_msgs::msg::UUID;
 
 class SceneModuleInterface
@@ -66,6 +67,7 @@ class SceneModuleInterface
   explicit SceneModuleInterface(
     const int64_t module_id, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock)
   : module_id_(module_id),
+    activated_(false),
     safe_(false),
     distance_(std::numeric_limits<double>::lowest()),
     logger_(logger),
@@ -126,16 +128,15 @@ class SceneModuleInterface
   {
     const auto & p = planner_data_;
     return motion_utils::findFirstNearestSegmentIndexWithSoftConstraints(
-      points, p->current_odometry->pose, p->ego_nearest_dist_threshold,
-      p->ego_nearest_yaw_threshold);
+      points, p->current_odometry->pose, p->ego_nearest_dist_threshold);
   }
 };
 
 class SceneModuleManagerInterface
 {
 public:
   SceneModuleManagerInterface(rclcpp::Node & node, [[maybe_unused]] const char * module_name)
-  : clock_(node.get_clock()), logger_(node.get_logger())
+  : node_(node), clock_(node.get_clock()), logger_(node.get_logger())
   {
     const auto ns = std::string("~/debug/") + module_name;
     pub_debug_ = node.create_publisher<visualization_msgs::msg::MarkerArray>(ns, 1);
@@ -307,6 +308,7 @@ class SceneModuleManagerInterface
   std::shared_ptr<const PlannerData> planner_data_;
 
   boost::optional<int> first_stop_path_point_index_;
+  rclcpp::Node & node_;
   rclcpp::Clock::SharedPtr clock_;
   // Debug
   bool is_publish_debug_path_ = {false};  // note : this is very heavy debug topic option
@@ -342,7 +344,7 @@ class SceneModuleManagerInterfaceWithRTC : public SceneModuleManagerInterface
   RTCInterface rtc_interface_;
   std::unordered_map<int64_t, UUID> map_uuid_;
 
-  void sendRTC(const Time & stamp)
+  virtual void sendRTC(const Time & stamp)
   {
     for (const auto & scene_module : scene_modules_) {
       const UUID uuid = getUUID(scene_module->getModuleId());
@@ -351,7 +353,7 @@ class SceneModuleManagerInterfaceWithRTC : public SceneModuleManagerInterface
     publishRTCStatus(stamp);
   }
 
-  void setActivation()
+  virtual void setActivation()
   {
     for (const auto & scene_module : scene_modules_) {
       const UUID uuid = getUUID(scene_module->getModuleId());

planning/behavior_velocity_planner/package.xml

@@ -55,15 +55,18 @@
   <depend>autoware_auto_perception_msgs</depend>
   <depend>autoware_auto_planning_msgs</depend>
   <depend>autoware_auto_tf2</depend>
+  <depend>cv_bridge</depend>
   <depend>diagnostic_msgs</depend>
   <depend>eigen</depend>
   <depend>geometry_msgs</depend>
+  <depend>grid_map_cv</depend>
   <depend>grid_map_ros</depend>
   <depend>grid_map_utils</depend>
   <depend>interpolation</depend>
   <depend>lanelet2_extension</depend>
   <depend>libboost-dev</depend>
   <depend>libopencv-dev</depend>
+  <depend>magic_enum</depend>
   <depend>message_filters</depend>
   <depend>motion_utils</depend>
   <depend>motion_velocity_smoother</depend>