feat(intersection): send intersection/intersection_occlusion rtc stat…

…us (#3511) * Merge feat/intersection-occlusion-latest (#8) Signed-off-by: Mamoru Sobue <[email protected]> * fixed virtual wall marker Signed-off-by: Mamoru Sobue <[email protected]> * use is_occluded flag Signed-off-by: Mamoru Sobue <[email protected]> * 3 rtc intersection appears Signed-off-by: Mamoru Sobue <[email protected]> * clearCooperateStatus before sendRTC not to duplicate default/occlusion_first_stop_uuid Signed-off-by: Mamoru Sobue <[email protected]> --------- Signed-off-by: Mamoru Sobue <[email protected]>
autowarefoundation · Apr 24, 2023 · 16f6434 · 16f6434
1 parent bc8ab77
commit 16f6434
Show file tree

Hide file tree

Showing 5 changed files with 63 additions and 60 deletions.
diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp
@@ -157,6 +157,7 @@ class IntersectionModule : public SceneModuleInterface
   {
     occlusion_first_stop_activated_ = activation;
   }
+  bool isOccluded() const { return is_occluded_; }
 
 private:
   rclcpp::Node & node_;
@@ -173,19 +174,19 @@ class IntersectionModule : public SceneModuleInterface
   // 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_;
+  bool is_occluded_ = false;
+  OcclusionState occlusion_state_ = OcclusionState::NONE;
   // NOTE: uuid_ is base member
   // for occlusion clearance decision
   const UUID occlusion_uuid_;
-  bool occlusion_safety_;
+  bool occlusion_safety_ = true;
   double occlusion_stop_distance_;
-  bool occlusion_activated_;
+  bool occlusion_activated_ = true;
   // for first stop in two-phase stop
   const UUID occlusion_first_stop_uuid_;  // TODO(Mamoru Sobue): replace with uuid_
-  bool occlusion_first_stop_safety_;
+  bool occlusion_first_stop_safety_ = true;
   double occlusion_first_stop_distance_;
-  bool occlusion_first_stop_activated_;
+  bool occlusion_first_stop_activated_ = true;
 
   StateMachine collision_state_machine_;     //! for stable collision checking
   StateMachine before_creep_state_machine_;  //! for two phase stop

diff --git a/planning/behavior_velocity_planner/package.xml b/planning/behavior_velocity_planner/package.xml
@@ -88,6 +88,7 @@
   <depend>tier4_v2x_msgs</depend>
   <depend>vehicle_info_util</depend>
   <depend>visualization_msgs</depend>
+  <exec_depend>grid_map_rviz_plugin</exec_depend>
 
   <test_depend>ament_cmake_ros</test_depend>
   <test_depend>ament_lint_auto</test_depend>

diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp
@@ -203,7 +203,6 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createVirtualWallMarker
 
   const auto now = this->clock_->now();
 
-  // int32_t uid = planning_utils::bitShift(module_id_);
   // TODO(Mamoru Sobue): collision stop pose depends on before/after occlusion clearance
   if (!activated_) {
     appendMarkerArray(

diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp
@@ -127,20 +127,8 @@ void IntersectionModuleManager::launchNewModules(
     const auto new_module = std::make_shared<IntersectionModule>(
       module_id, lane_id, planner_data_, intersection_param_, assoc_ids, enable_occlusion_detection,
       node_, logger_.get_child("intersection_module"), clock_);
-    const auto occlusion_uuid = new_module->getOcclusionUUID();
-    const auto occlusion_first_stop_uuid = new_module->getOcclusionFirstStopUUID();
     registerModule(std::move(new_module));
-    // default
     generateUUID(module_id);
-    updateRTCStatus(
-      getUUID(module_id), true, std::numeric_limits<double>::lowest(), path.header.stamp);
-    // occlusion
-    occlusion_rtc_interface_.updateCooperateStatus(
-      occlusion_uuid, true, std::numeric_limits<double>::lowest(),
-      std::numeric_limits<double>::lowest(), path.header.stamp);
-    rtc_interface_.updateCooperateStatus(
-      occlusion_first_stop_uuid, true, std::numeric_limits<double>::lowest(),
-      std::numeric_limits<double>::lowest(), path.header.stamp);
 
     enable_occlusion_detection = false;
   }
@@ -188,23 +176,36 @@ bool IntersectionModuleManager::hasSameParentLaneletAndTurnDirectionWithRegister
 void IntersectionModuleManager::sendRTC(const Time & stamp)
 {
   for (const auto & scene_module : scene_modules_) {
-    // default
-    const UUID uuid = getUUID(scene_module->getModuleId());
-    updateRTCStatus(uuid, scene_module->isSafe(), scene_module->getDistance(), stamp);
-    // occlusion
     const auto intersection_module = std::dynamic_pointer_cast<IntersectionModule>(scene_module);
+    const bool is_occluded = intersection_module->isOccluded();
+    const UUID uuid = getUUID(scene_module->getModuleId());
     const auto occlusion_uuid = intersection_module->getOcclusionUUID();
-    const auto occlusion_safety = intersection_module->getOcclusionSafety();
-    const auto occlusion_distance = intersection_module->getOcclusionDistance();
-    occlusion_rtc_interface_.updateCooperateStatus(
-      occlusion_uuid, occlusion_safety, occlusion_distance, occlusion_distance, stamp);
-
     const auto occlusion_first_stop_uuid = intersection_module->getOcclusionFirstStopUUID();
-    const auto occlusion_first_stop_safety = intersection_module->getOcclusionFirstStopSafety();
-    const auto occlusion_first_stop_distance = intersection_module->getOcclusionFirstStopDistance();
-    rtc_interface_.updateCooperateStatus(
-      occlusion_first_stop_uuid, occlusion_first_stop_safety, occlusion_first_stop_distance,
-      occlusion_first_stop_distance, stamp);
+    if (!is_occluded) {
+      rtc_interface_.clearCooperateStatus();
+      // default
+      updateRTCStatus(uuid, scene_module->isSafe(), scene_module->getDistance(), stamp);
+      occlusion_rtc_interface_.updateCooperateStatus(
+        occlusion_uuid, true, std::numeric_limits<double>::lowest(),
+        std::numeric_limits<double>::lowest(), stamp);
+      // send {default, occlusion} RTC status
+    } else {
+      rtc_interface_.clearCooperateStatus();
+      // occlusion
+      const auto occlusion_safety = intersection_module->getOcclusionSafety();
+      const auto occlusion_distance = intersection_module->getOcclusionDistance();
+      occlusion_rtc_interface_.updateCooperateStatus(
+        occlusion_uuid, occlusion_safety, occlusion_distance, occlusion_distance, stamp);
+
+      const auto occlusion_first_stop_safety = intersection_module->getOcclusionFirstStopSafety();
+      const auto occlusion_first_stop_distance =
+        intersection_module->getOcclusionFirstStopDistance();
+      rtc_interface_.updateCooperateStatus(
+        occlusion_first_stop_uuid, occlusion_first_stop_safety, occlusion_first_stop_distance,
+        occlusion_first_stop_distance, stamp);
+      // send {first_stop, occlusion} RTC status
+      // rtc_interface_.removeCooperateStatus(uuid);
+    }
   }
   rtc_interface_.publishCooperateStatus(stamp);  // publishRTCStatus()
   occlusion_rtc_interface_.publishCooperateStatus(stamp);
@@ -213,16 +214,22 @@ void IntersectionModuleManager::sendRTC(const Time & stamp)
 void IntersectionModuleManager::setActivation()
 {
   for (const auto & scene_module : scene_modules_) {
-    // default
-    scene_module->setActivation(rtc_interface_.isActivated(getUUID(scene_module->getModuleId())));
-    // occlusion
     const auto intersection_module = std::dynamic_pointer_cast<IntersectionModule>(scene_module);
     const auto occlusion_uuid = intersection_module->getOcclusionUUID();
-    intersection_module->setOcclusionActivation(
-      occlusion_rtc_interface_.isActivated(occlusion_uuid));
     const auto occlusion_first_stop_uuid = intersection_module->getOcclusionFirstStopUUID();
-    intersection_module->setOcclusionFirstStopActivation(
-      rtc_interface_.isActivated(occlusion_first_stop_uuid));
+    const bool is_occluded = intersection_module->isOccluded();
+    if (!is_occluded) {
+      // default
+      scene_module->setActivation(rtc_interface_.isActivated(getUUID(scene_module->getModuleId())));
+      intersection_module->setOcclusionActivation(
+        occlusion_rtc_interface_.isActivated(occlusion_uuid));
+    } else {
+      // occlusion
+      intersection_module->setOcclusionActivation(
+        occlusion_rtc_interface_.isActivated(occlusion_uuid));
+      intersection_module->setOcclusionFirstStopActivation(
+        rtc_interface_.isActivated(occlusion_first_stop_uuid));
+    }
   }
 }
 

diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp
@@ -67,12 +67,9 @@ IntersectionModule::IntersectionModule(
 : SceneModuleInterface(module_id, logger, clock),
   node_(node),
   lane_id_(lane_id),
-  is_go_out_(false),
   assoc_ids_(assoc_ids),
   enable_occlusion_detection_(enable_occlusion_detection),
   detection_divisions_(std::nullopt),
-  prev_occlusion_stop_line_pose_(std::nullopt),
-  occlusion_state_(OcclusionState::NONE),
   occlusion_uuid_(tier4_autoware_utils::generateUUID()),
   occlusion_first_stop_uuid_(tier4_autoware_utils::generateUUID())
 {
@@ -289,8 +286,7 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
     if (!default_stop_line_idx_opt) {
       occlusion_stop_required = true;
       stop_line_idx = occlusion_stop_line_idx = occlusion_stop_line_idx_opt;
-      prev_occlusion_stop_line_pose_ =
-        path_ip.points.at(occlusion_stop_line_idx_opt.value()).point.pose;
+      is_occluded_ = true;
       occlusion_state_ = OcclusionState::CREEP_SECOND_STOP_LINE;
       RCLCPP_WARN(logger_, "directly stop at occlusion stop line because collision line not found");
     } else if (before_creep_state_machine_.getState() == StateMachine::State::GO) {
@@ -300,8 +296,7 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
       // insert creep velocity [closest_idx, occlusion_stop_line)
       insert_creep_during_occlusion =
         std::make_pair(closest_idx, occlusion_stop_line_idx_opt.value());
-      prev_occlusion_stop_line_pose_ =
-        path_ip.points.at(occlusion_stop_line_idx_ip_opt.value()).point.pose;
+      is_occluded_ = true;
       occlusion_state_ = OcclusionState::CREEP_SECOND_STOP_LINE;
     } else {
       const double dist_default_stop_line = motion_utils::calcSignedArcLength(
@@ -320,32 +315,32 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
       // insert creep velocity [default_stop_line, occlusion_stop_line)
       insert_creep_during_occlusion =
         std::make_pair(default_stop_line_idx_opt.value(), occlusion_stop_line_idx_opt.value());
-      prev_occlusion_stop_line_pose_ =
-        path_ip.points.at(occlusion_stop_line_idx_ip_opt.value()).point.pose;
+      is_occluded_ = true;
       occlusion_state_ = OcclusionState::BEFORE_FIRST_STOP_LINE;
     }
-  } else if (prev_occlusion_stop_line_pose_) {
+  } else if (is_occluded_) {
     // previously occlusion existed, but now it is clear
-    const auto prev_occlusion_stop_pose_idx = motion_utils::findNearestIndex(
-      path->points, prev_occlusion_stop_line_pose_.value(), 3.0, M_PI_4);
     if (!util::isOverTargetIndex(
           *path, closest_idx, current_pose, default_stop_line_idx_opt.value())) {
       stop_line_idx = default_stop_line_idx_opt.value();
-      prev_occlusion_stop_line_pose_ = std::nullopt;
-    } else if (!util::isOverTargetIndex(
-                 *path, closest_idx, current_pose, prev_occlusion_stop_pose_idx.get())) {
-      stop_line_idx = prev_occlusion_stop_pose_idx.get();
-    } else {
-      // TODO(Mamoru Sobue): consider static occlusion limit stop line
-      prev_occlusion_stop_line_pose_ = std::nullopt;
+    } else if (
+      static_pass_judge_line_opt &&
+      !util::isOverTargetIndex(
+        *path, closest_idx, current_pose, static_pass_judge_line_opt.value())) {
+      stop_line_idx = static_pass_judge_line_opt;
     }
     occlusion_state_ = OcclusionState::CLEARED;
+    is_occluded_ = false;
     if (stop_line_idx && has_collision) {
       // do collision checking at previous occlusion stop line
       collision_stop_required = true;
     } else {
       collision_stop_required = false;
     }
+    if (is_stuck && stuck_line_idx_opt) {
+      stuck_stop_required = true;
+      stop_line_idx = static_pass_judge_line_opt;
+    }
   } else if (is_stuck && stuck_line_idx_opt) {
     stuck_stop_required = true;
     const size_t stuck_line_idx = stuck_line_idx_opt.value();
@@ -401,7 +396,7 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
   }
 
   /* make decision */
-  const double baselink2front = planner_data_->vehicle_info_.max_longitudinal_offset_m;
+  const double baselink2front = planner_data_->vehicle_info_.vehicle_length_m;
   if (!occlusion_activated_) {
     is_go_out_ = false;
     /* in case of creeping */