feature(behavior_velocity_planner): add rtc to no stopping area

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

Hide file tree

Showing 3 changed files with 40 additions and 2 deletions.
diff --git a/planning/behavior_velocity_planner/include/scene_module/no_stopping_area/manager.hpp b/planning/behavior_velocity_planner/include/scene_module/no_stopping_area/manager.hpp
@@ -19,6 +19,8 @@
 #include "scene_module/no_stopping_area/scene_no_stopping_area.hpp"
 #include "scene_module/scene_module_interface.hpp"
 
+#include <rtc_interface/rtc_interface.hpp>
+
 #include "autoware_auto_planning_msgs/msg/path_with_lane_id.hpp"
 
 #include <functional>
@@ -35,7 +37,13 @@ class NoStoppingAreaModuleManager : public SceneModuleManagerInterface
 
 private:
   NoStoppingAreaModule::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/no_stopping_area/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/manager.cpp
@@ -61,7 +61,7 @@ std::set<int64_t> getNoStoppingAreaIdSetOnPath(
 }  // namespace
 
 NoStoppingAreaModuleManager::NoStoppingAreaModuleManager(rclcpp::Node & node)
-: SceneModuleManagerInterface(node, getModuleName())
+: SceneModuleManagerInterface(node, getModuleName()), rtc_interface_(node, "no_stopping_area")
 {
   const std::string ns(getModuleName());
   auto & pp = planner_param_;
@@ -88,6 +88,7 @@ void NoStoppingAreaModuleManager::launchNewModules(
       registerModule(std::make_shared<NoStoppingAreaModule>(
         module_id, *no_stopping_area, planner_param_, logger_.get_child("no_stopping_area_module"),
         clock_));
+      generateUUID(module_id);
     }
   }
 }
@@ -103,4 +104,25 @@ NoStoppingAreaModuleManager::getModuleExpiredFunction(
     return no_stopping_area_id_set.count(scene_module->getModuleId()) == 0;
   };
 }
+
+bool NoStoppingAreaModuleManager::getActivation(const UUID & uuid)
+{
+  return rtc_interface_.isActivated(uuid);
+}
+
+void NoStoppingAreaModuleManager::updateRTCStatus(
+  const UUID & uuid, const bool safe, const double distance, const Time & stamp)
+{
+  rtc_interface_.updateCooperateStatus(uuid, safe, distance, stamp);
+}
+
+void NoStoppingAreaModuleManager::removeRTCStatus(const UUID & uuid)
+{
+  rtc_interface_.removeCooperateStatus(uuid);
+}
+
+void NoStoppingAreaModuleManager::publishRTCStatus(const Time & stamp)
+{
+  rtc_interface_.publishCooperateStatus(stamp);
+}
 }  // namespace behavior_velocity_planner
diff --git a/...ng/behavior_velocity_planner/src/scene_module/no_stopping_area/scene_no_stopping_area.cpp b/...ng/behavior_velocity_planner/src/scene_module/no_stopping_area/scene_no_stopping_area.cpp
@@ -124,17 +124,22 @@ bool NoStoppingAreaModule::modifyPathVelocity(
   // Get stop line geometry
   const auto stop_line = getStopLineGeometry2d(original_path, planner_param_.stop_line_margin);
   if (!stop_line) {
+    setSafe(true);
     return true;
   }
   const auto stop_point =
     createTargetPoint(original_path, stop_line.value(), planner_param_.stop_margin);
   if (!stop_point) {
+    setSafe(true);
     return true;
   }
   const auto & stop_pose = stop_point->second;
+  setDistance(tier4_autoware_utils::calcSignedArcLength(
+    original_path.points, current_pose.pose.position, stop_pose.position));
   if (isOverDeadLine(original_path, current_pose.pose, stop_pose)) {
     // ego can't stop in front of no stopping area -> GO or OR
     state_machine_.setState(StateMachine::State::GO);
+    setSafe(true);
     return true;
   }
   const auto & vi = planner_data_->vehicle_info_;
@@ -150,6 +155,7 @@ bool NoStoppingAreaModule::modifyPathVelocity(
     *path, current_pose.pose, ego_space_in_front_of_stop_line,
     planner_param_.detection_area_length);
   if (stuck_vehicle_detect_area.outer().empty() && stop_line_detect_area.outer().empty()) {
+    setSafe(true);
     return true;
   }
   debug_data_.stuck_vehicle_detect_area = toGeomMsg(stuck_vehicle_detect_area);
@@ -164,14 +170,16 @@ bool NoStoppingAreaModule::modifyPathVelocity(
     is_entry_prohibited_by_stuck_vehicle || is_entry_prohibited_by_stop_line;
   if (!isStoppable(current_pose.pose, stop_point->second)) {
     state_machine_.setState(StateMachine::State::GO);
+    setSafe(true);
     return false;
   } else {
     state_machine_.setStateWithMarginTime(
       is_entry_prohibited ? StateMachine::State::STOP : StateMachine::State::GO,
       logger_.get_child("state_machine"), *clock_);
   }
 
-  if (state_machine_.getState() == StateMachine::State::STOP) {
+  setSafe(state_machine_.getState() != StateMachine::State::STOP);
+  if (!isActivated()) {
     // ----------------stop reason and stop point--------------------------
     insertStopPoint(*path, *stop_point);
     // For virtual wall