feature(behavior_velocity_planner): add rtc to detection area

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>
autowarefoundation · May 27, 2022 · e85d3f9 · e85d3f9
1 parent f7a262c
commit e85d3f9
Show file tree

Hide file tree

Showing 3 changed files with 52 additions and 16 deletions.
diff --git a/planning/behavior_velocity_planner/include/scene_module/detection_area/manager.hpp b/planning/behavior_velocity_planner/include/scene_module/detection_area/manager.hpp
@@ -16,6 +16,7 @@
 #define SCENE_MODULE__DETECTION_AREA__MANAGER_HPP_
 
 #include <rclcpp/rclcpp.hpp>
+#include <rtc_interface/rtc_interface.hpp>
 #include <scene_module/detection_area/scene.hpp>
 #include <scene_module/scene_module_interface.hpp>
 
@@ -35,7 +36,13 @@ class DetectionAreaModuleManager : public SceneModuleManagerInterface
 
 private:
   DetectionAreaModule::PlannerParam planner_param_;
+  rtc_interface::RTCInterface rtc_interface_;
   void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override;
+  bool getActivation(const UUID & uuid) override;
+  void updateRTCStatus(
+    const UUID & uuid, const bool safe, const double distance, const Time & stamp) override;
+  void removeRTCStatus(const UUID & uuid) override;
+  void publishRTCStatus(const Time & stamp) override;
 
   std::function<bool(const std::shared_ptr<SceneModuleInterface> &)> getModuleExpiredFunction(
     const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override;

diff --git a/planning/behavior_velocity_planner/src/scene_module/detection_area/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/detection_area/manager.cpp
@@ -59,7 +59,7 @@ std::set<int64_t> getDetectionAreaIdSetOnPath(
 }  // namespace
 
 DetectionAreaModuleManager::DetectionAreaModuleManager(rclcpp::Node & node)
-: SceneModuleManagerInterface(node, getModuleName())
+: SceneModuleManagerInterface(node, getModuleName()), rtc_interface_(node, "detection_area")
 {
   const std::string ns(getModuleName());
   planner_param_.stop_margin = node.declare_parameter(ns + ".stop_margin", 0.0);
@@ -80,6 +80,7 @@ void DetectionAreaModuleManager::launchNewModules(
       registerModule(std::make_shared<DetectionAreaModule>(
         module_id, *detection_area, planner_param_, logger_.get_child("detection_area_module"),
         clock_));
+      generateUUID(module_id);
     }
   }
 }
@@ -95,4 +96,25 @@ DetectionAreaModuleManager::getModuleExpiredFunction(
     return detection_area_id_set.count(scene_module->getModuleId()) == 0;
   };
 }
+
+bool DetectionAreaModuleManager::getActivation(const UUID & uuid)
+{
+  return rtc_interface_.isActivated(uuid);
+}
+
+void DetectionAreaModuleManager::updateRTCStatus(
+  const UUID & uuid, const bool safe, const double distance, const Time & stamp)
+{
+  rtc_interface_.updateCooperateStatus(uuid, safe, distance, stamp);
+}
+
+void DetectionAreaModuleManager::removeRTCStatus(const UUID & uuid)
+{
+  rtc_interface_.removeCooperateStatus(uuid);
+}
+
+void DetectionAreaModuleManager::publishRTCStatus(const Time & stamp)
+{
+  rtc_interface_.publishCooperateStatus(stamp);
+}
 }  // namespace behavior_velocity_planner
diff --git a/planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp
@@ -228,19 +228,31 @@ bool DetectionAreaModule::modifyPathVelocity(
     last_obstacle_found_time_ = std::make_shared<const rclcpp::Time>(clock_->now());
   }
 
-  // Check state
-  if (canClearStopState()) {
-    state_ = State::GO;
-    last_obstacle_found_time_ = {};
-    return true;
-  }
-
   // Get stop line geometry
   const auto stop_line = getStopLineGeometry2d();
 
   // Get self pose
   const auto & self_pose = planner_data_->current_pose.pose;
 
+  // Get stop point
+  const auto stop_point = createTargetPoint(original_path, stop_line, planner_param_.stop_margin);
+  if (!stop_point) {
+    return true;
+  }
+
+  const auto & stop_pose = stop_point->second;
+
+  setDistance(tier4_autoware_utils::calcSignedArcLength(
+    path->points, self_pose.position, stop_pose.position));
+
+  // Check state
+  setSafe(canClearStopState());
+  if (isActivated()) {
+    state_ = State::GO;
+    last_obstacle_found_time_ = {};
+    return true;
+  }
+
   // Force ignore objects after dead_line
   if (planner_param_.use_dead_line) {
     // Use '-' for margin because it's the backward distance from stop line
@@ -253,21 +265,15 @@ bool DetectionAreaModule::modifyPathVelocity(
 
       if (isOverLine(original_path, self_pose, dead_line_pose)) {
         RCLCPP_WARN(logger_, "[detection_area] vehicle is over dead line");
+        setSafe(true);
         return true;
       }
     }
   }
 
-  // Get stop point
-  const auto stop_point = createTargetPoint(original_path, stop_line, planner_param_.stop_margin);
-  if (!stop_point) {
-    return true;
-  }
-
-  const auto & stop_pose = stop_point->second;
-
   // Ignore objects detected after stop_line if not in STOP state
   if (state_ != State::STOP && isOverLine(original_path, self_pose, stop_pose)) {
+    setSafe(true);
     return true;
   }
 
@@ -277,6 +283,7 @@ bool DetectionAreaModule::modifyPathVelocity(
       RCLCPP_WARN_THROTTLE(
         logger_, *clock_, std::chrono::milliseconds(1000).count(),
         "[detection_area] vehicle is over stop border");
+      setSafe(true);
       return true;
     }
   }