feat(behavior_velocity_planner): add rtc interface to behavior veloci…

…ty planner (tier4#963) * feature(behavior_velocity_planner): apply rtc interface Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com> * feature(behavior_velocity_planner): add distance calculation to crosswalk Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com> * feature(behavior_velocity_planner): fix debug data update Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com> * feature(behavior_velocity_planner): apply to intersection Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com> * feature(behavior_velocity_planner): fix stop condition Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com> * feature(behavior_velocity_planner): fix stop wall publishing Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com> * feature(behavior_velocity_planner): fix insert_stop Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com> * feature(behavior_velocity_planner): fix state transition in intersection Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com> * feature(behavior_velocity_planner): add rtc to traffic_light Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com> * feature(behavior_velocity_planner): fix distance calculation and safety Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com> * feature(behavior_velocity_planner): add stamp Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com> * fix(behavior_velocity_planner): move some function to protected Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com> * fix(behavior_velocity_planner): fix intersection module Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com> * feature(behavior_velocity_planner): add rtc_interface to blind_spot Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com> * feature(behavior_velocity_planner): add rtc to detection area Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com> * feature(behavior_velocity_planner): add rtc to no stopping area Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com> * fix(behavior_velocity_planner): fix debug in intersection Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com> * chore(behavior_velocity_planner): cosmetic change Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com> * ci(pre-commit): autofix * fix(behavior_velocity_planner): fix debug of blind_spot Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com> * feature(behavior_velocity_planner): define interface class and use it Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com> * chore(behavior_velocity_planner): small fix for diff Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com> * chore(behavior_velocity_planner): remove unnecessary function Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com> * fix(behavior_velocity_planner): fix conflicts Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
boyali · Jul 1, 2022 · 7accfa8 · 7accfa8
1 parent 02394e5
commit 7accfa8
Show file tree

Hide file tree

Showing 23 changed files with 279 additions and 83 deletions.
diff --git a/planning/behavior_velocity_planner/include/scene_module/blind_spot/manager.hpp b/planning/behavior_velocity_planner/include/scene_module/blind_spot/manager.hpp
@@ -26,7 +26,7 @@
 
 namespace behavior_velocity_planner
 {
-class BlindSpotModuleManager : public SceneModuleManagerInterface
+class BlindSpotModuleManager : public SceneModuleManagerInterfaceWithRTC
 {
 public:
   explicit BlindSpotModuleManager(rclcpp::Node & node);

diff --git a/planning/behavior_velocity_planner/include/scene_module/crosswalk/manager.hpp b/planning/behavior_velocity_planner/include/scene_module/crosswalk/manager.hpp
@@ -30,7 +30,7 @@
 
 namespace behavior_velocity_planner
 {
-class CrosswalkModuleManager : public SceneModuleManagerInterface
+class CrosswalkModuleManager : public SceneModuleManagerInterfaceWithRTC
 {
 public:
   explicit CrosswalkModuleManager(rclcpp::Node & node);

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
@@ -26,7 +26,7 @@
 
 namespace behavior_velocity_planner
 {
-class DetectionAreaModuleManager : public SceneModuleManagerInterface
+class DetectionAreaModuleManager : public SceneModuleManagerInterfaceWithRTC
 {
 public:
   explicit DetectionAreaModuleManager(rclcpp::Node & node);
@@ -35,6 +35,7 @@ class DetectionAreaModuleManager : public SceneModuleManagerInterface
 
 private:
   DetectionAreaModule::PlannerParam planner_param_;
+
   void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override;
 
   std::function<bool(const std::shared_ptr<SceneModuleInterface> &)> getModuleExpiredFunction(

diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/manager.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/manager.hpp
@@ -28,7 +28,7 @@
 
 namespace behavior_velocity_planner
 {
-class IntersectionModuleManager : public SceneModuleManagerInterface
+class IntersectionModuleManager : public SceneModuleManagerInterfaceWithRTC
 {
 public:
   explicit IntersectionModuleManager(rclcpp::Node & node);

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
@@ -26,7 +26,7 @@
 
 namespace behavior_velocity_planner
 {
-class NoStoppingAreaModuleManager : public SceneModuleManagerInterface
+class NoStoppingAreaModuleManager : public SceneModuleManagerInterfaceWithRTC
 {
 public:
   explicit NoStoppingAreaModuleManager(rclcpp::Node & node);
@@ -35,6 +35,7 @@ class NoStoppingAreaModuleManager : public SceneModuleManagerInterface
 
 private:
   NoStoppingAreaModule::PlannerParam planner_param_;
+
   void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override;
 
   std::function<bool(const std::shared_ptr<SceneModuleInterface> &)> getModuleExpiredFunction(

diff --git a/planning/behavior_velocity_planner/include/scene_module/scene_module_interface.hpp b/planning/behavior_velocity_planner/include/scene_module/scene_module_interface.hpp
@@ -17,17 +17,24 @@
 
 #include "behavior_velocity_planner/planner_data.hpp"
 
+#include <builtin_interfaces/msg/time.hpp>
+#include <rtc_interface/rtc_interface.hpp>
 #include <utilization/util.hpp>
 
 #include <autoware_auto_planning_msgs/msg/path.hpp>
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
 #include <tier4_planning_msgs/msg/stop_reason.hpp>
 #include <tier4_planning_msgs/msg/stop_reason_array.hpp>
 #include <tier4_v2x_msgs/msg/infrastructure_command_array.hpp>
+#include <unique_identifier_msgs/msg/uuid.hpp>
 
+#include <algorithm>
+#include <limits>
 #include <memory>
+#include <random>
 #include <set>
 #include <string>
+#include <unordered_map>
 
 // Debug
 #include <rclcpp/rclcpp.hpp>
@@ -36,12 +43,19 @@
 
 namespace behavior_velocity_planner
 {
+using builtin_interfaces::msg::Time;
+using rtc_interface::RTCInterface;
+using unique_identifier_msgs::msg::UUID;
 class SceneModuleInterface
 {
 public:
   explicit SceneModuleInterface(
     const int64_t module_id, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock)
-  : module_id_(module_id), logger_(logger), clock_(clock)
+  : module_id_(module_id),
+    safe_(false),
+    distance_(std::numeric_limits<double>::lowest()),
+    logger_(logger),
+    clock_(clock)
   {
   }
   virtual ~SceneModuleInterface() = default;
@@ -71,13 +85,24 @@ class SceneModuleInterface
 
   boost::optional<int> getFirstStopPathPointIndex() { return first_stop_path_point_index_; }
 
+  void setActivation(const bool activated) { activated_ = activated; }
+  bool isActivated() const { return activated_; }
+  bool isSafe() const { return safe_; }
+  double getDistance() const { return distance_; }
+
 protected:
   const int64_t module_id_;
+  bool activated_;
+  bool safe_;
+  double distance_;
   rclcpp::Logger logger_;
   rclcpp::Clock::SharedPtr clock_;
   std::shared_ptr<const PlannerData> planner_data_;
   boost::optional<tier4_v2x_msgs::msg::InfrastructureCommand> infrastructure_command_;
   boost::optional<int> first_stop_path_point_index_;
+
+  void setSafe(const bool safe) { safe_ = safe; }
+  void setDistance(const double distance) { distance_ = distance; }
 };
 
 class SceneModuleManagerInterface
@@ -113,6 +138,12 @@ class SceneModuleManagerInterface
     deleteExpiredModules(path);
   }
 
+  virtual void plan(autoware_auto_planning_msgs::msg::PathWithLaneId * path)
+  {
+    modifyPathVelocity(path);
+  }
+
+protected:
   virtual void modifyPathVelocity(autoware_auto_planning_msgs::msg::PathWithLaneId * path)
   {
     visualization_msgs::msg::MarkerArray debug_marker_array;
@@ -159,13 +190,12 @@ class SceneModuleManagerInterface
     pub_virtual_wall_->publish(virtual_wall_marker_array);
   }
 
-protected:
   virtual void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) = 0;
 
   virtual std::function<bool(const std::shared_ptr<SceneModuleInterface> &)>
   getModuleExpiredFunction(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) = 0;
 
-  void deleteExpiredModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path)
+  virtual void deleteExpiredModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path)
   {
     const auto isModuleExpired = getModuleExpiredFunction(path);
 
@@ -218,6 +248,99 @@ class SceneModuleManagerInterface
   rclcpp::Publisher<tier4_v2x_msgs::msg::InfrastructureCommandArray>::SharedPtr
     pub_infrastructure_commands_;
 };
+
+class SceneModuleManagerInterfaceWithRTC : public SceneModuleManagerInterface
+{
+public:
+  SceneModuleManagerInterfaceWithRTC(rclcpp::Node & node, const char * module_name)
+  : SceneModuleManagerInterface(node, module_name), rtc_interface_(node, module_name)
+  {
+  }
+
+  void plan(autoware_auto_planning_msgs::msg::PathWithLaneId * path) override
+  {
+    setActivation();
+    modifyPathVelocity(path);
+    sendRTC(path->header.stamp);
+  }
+
+protected:
+  RTCInterface rtc_interface_;
+  std::unordered_map<int64_t, UUID> map_uuid_;
+
+  void sendRTC(const Time & stamp)
+  {
+    for (const auto & scene_module : scene_modules_) {
+      const UUID uuid = getUUID(scene_module->getModuleId());
+      updateRTCStatus(uuid, scene_module->isSafe(), scene_module->getDistance(), stamp);
+    }
+    publishRTCStatus(stamp);
+  }
+
+  void setActivation()
+  {
+    for (const auto & scene_module : scene_modules_) {
+      const UUID uuid = getUUID(scene_module->getModuleId());
+      scene_module->setActivation(rtc_interface_.isActivated(uuid));
+    }
+  }
+
+  void updateRTCStatus(
+    const UUID & uuid, const bool safe, const double distance, const Time & stamp)
+  {
+    rtc_interface_.updateCooperateStatus(uuid, safe, distance, stamp);
+  }
+
+  void removeRTCStatus(const UUID & uuid) { rtc_interface_.removeCooperateStatus(uuid); }
+
+  void publishRTCStatus(const Time & stamp) { rtc_interface_.publishCooperateStatus(stamp); }
+
+  UUID getUUID(const int64_t & module_id) const
+  {
+    if (map_uuid_.count(module_id) == 0) {
+      const UUID uuid;
+      return uuid;
+    }
+    return map_uuid_.at(module_id);
+  }
+
+  void generateUUID(const int64_t & module_id)
+  {
+    // Generate random number
+    UUID uuid;
+    std::mt19937 gen(std::random_device{}());
+    std::independent_bits_engine<std::mt19937, 8, uint8_t> bit_eng(gen);
+    std::generate(uuid.uuid.begin(), uuid.uuid.end(), bit_eng);
+    map_uuid_.insert({module_id, uuid});
+  }
+
+  void removeUUID(const int64_t & module_id)
+  {
+    const auto result = map_uuid_.erase(module_id);
+    if (result == 0) {
+      RCLCPP_WARN_STREAM(
+        logger_, "[removeUUID] module_id = " << module_id << " is not registered.");
+    }
+  }
+
+  void deleteExpiredModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override
+  {
+    const auto isModuleExpired = getModuleExpiredFunction(path);
+
+    // Copy container to avoid iterator corruption
+    // due to scene_modules_.erase() in unregisterModule()
+    const auto copied_scene_modules = scene_modules_;
+
+    for (const auto & scene_module : copied_scene_modules) {
+      if (isModuleExpired(scene_module)) {
+        removeRTCStatus(getUUID(scene_module->getModuleId()));
+        removeUUID(scene_module->getModuleId());
+        unregisterModule(scene_module);
+      }
+    }
+  }
+};
+
 }  // namespace behavior_velocity_planner
 
 #endif  // SCENE_MODULE__SCENE_MODULE_INTERFACE_HPP_
diff --git a/planning/behavior_velocity_planner/include/scene_module/traffic_light/manager.hpp b/planning/behavior_velocity_planner/include/scene_module/traffic_light/manager.hpp
@@ -26,22 +26,23 @@
 
 namespace behavior_velocity_planner
 {
-class TrafficLightModuleManager : public SceneModuleManagerInterface
+class TrafficLightModuleManager : public SceneModuleManagerInterfaceWithRTC
 {
 public:
   explicit TrafficLightModuleManager(rclcpp::Node & node);
 
   const char * getModuleName() override { return "traffic_light"; }
 
-  void modifyPathVelocity(autoware_auto_planning_msgs::msg::PathWithLaneId * path) override;
-
 private:
   TrafficLightModule::PlannerParam planner_param_;
+
   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;
 
+  void modifyPathVelocity(autoware_auto_planning_msgs::msg::PathWithLaneId * path) override;
+
   // Debug
   rclcpp::Publisher<autoware_auto_perception_msgs::msg::LookingTrafficSignal>::SharedPtr
     pub_tl_state_;

diff --git a/planning/behavior_velocity_planner/package.xml b/planning/behavior_velocity_planner/package.xml
@@ -31,6 +31,7 @@
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
   <depend>route_handler</depend>
+  <depend>rtc_interface</depend>
   <depend>sensor_msgs</depend>
   <depend>tf2</depend>
   <depend>tf2_eigen</depend>

diff --git a/planning/behavior_velocity_planner/src/planner_manager.cpp b/planning/behavior_velocity_planner/src/planner_manager.cpp
@@ -66,7 +66,7 @@ autoware_auto_planning_msgs::msg::PathWithLaneId BehaviorVelocityPlannerManager:
 
   for (const auto & scene_manager_ptr : scene_manager_ptrs_) {
     scene_manager_ptr->updateSceneModuleInstances(planner_data, input_path_msg);
-    scene_manager_ptr->modifyPathVelocity(&output_path_msg);
+    scene_manager_ptr->plan(&output_path_msg);
     boost::optional<int> firstStopPathPointIndex = scene_manager_ptr->getFirstStopPathPointIndex();
 
     if (firstStopPathPointIndex) {

diff --git a/planning/behavior_velocity_planner/src/scene_module/blind_spot/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/blind_spot/debug.cpp
@@ -154,10 +154,9 @@ visualization_msgs::msg::MarkerArray BlindSpotModule::createVirtualWallMarkerArr
 {
   visualization_msgs::msg::MarkerArray wall_marker;
 
-  const auto state = state_machine_.getState();
   const auto now = this->clock_->now();
 
-  if (state == BlindSpotModule::State::STOP) {
+  if (!isActivated()) {
     appendMarkerArray(
       tier4_autoware_utils::createStopVirtualWallMarker(
         debug_data_.virtual_wall_pose, "blind_spot", now, lane_id_),

diff --git a/planning/behavior_velocity_planner/src/scene_module/blind_spot/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/blind_spot/manager.cpp
@@ -26,7 +26,7 @@
 namespace behavior_velocity_planner
 {
 BlindSpotModuleManager::BlindSpotModuleManager(rclcpp::Node & node)
-: SceneModuleManagerInterface(node, getModuleName())
+: SceneModuleManagerInterfaceWithRTC(node, getModuleName())
 {
   const std::string ns(getModuleName());
   planner_param_.use_pass_judge_line = node.declare_parameter(ns + ".use_pass_judge_line", false);
@@ -60,6 +60,7 @@ void BlindSpotModuleManager::launchNewModules(
     registerModule(std::make_shared<BlindSpotModule>(
       module_id, lane_id, planner_data_, planner_param_, logger_.get_child("blind_spot_module"),
       clock_));
+    generateUUID(module_id);
   }
 }
 
@@ -74,4 +75,5 @@ BlindSpotModuleManager::getModuleExpiredFunction(
     return lane_id_set.count(scene_module->getModuleId()) == 0;
   };
 }
+
 }  // namespace behavior_velocity_planner
diff --git a/planning/behavior_velocity_planner/src/scene_module/blind_spot/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/blind_spot/scene.cpp
@@ -93,6 +93,8 @@ bool BlindSpotModule::modifyPathVelocity(
     RCLCPP_DEBUG(
       logger_, "[Blind Spot] stop line or pass judge line is at path[0], ignore planning.");
     *path = input_path;  // reset path
+    setSafe(true);
+    setDistance(std::numeric_limits<double>::lowest());
     return true;
   }
 
@@ -122,7 +124,9 @@ bool BlindSpotModule::modifyPathVelocity(
     if (current_state == State::GO && is_over_pass_judge_line) {
       RCLCPP_DEBUG(logger_, "over the pass judge line. no plan needed.");
       *path = input_path;  // reset path
-      return true;         // no plan needed.
+      setSafe(true);
+      setDistance(std::numeric_limits<double>::lowest());
+      return true;  // no plan needed.
     }
   }
 
@@ -136,7 +140,10 @@ bool BlindSpotModule::modifyPathVelocity(
     has_obstacle ? State::STOP : State::GO, logger_.get_child("state_machine"), *clock_);
 
   /* set stop speed */
-  if (state_machine_.getState() == State::STOP) {
+  setSafe(state_machine_.getState() != State::STOP);
+  setDistance(tier4_autoware_utils::calcSignedArcLength(
+    path->points, current_pose.pose.position, path->points.at(stop_line_idx).point.pose.position));
+  if (!isActivated()) {
     constexpr double stop_vel = 0.0;
     util::setVelocityFrom(stop_line_idx, stop_vel, path);
 
@@ -148,7 +155,6 @@ bool BlindSpotModule::modifyPathVelocity(
   } else {
     *path = input_path;  // reset path
   }
-
   return true;
 }