feat(behavior_velocity_detection_area): use base class without RTC (#…

…9802) * feat(behavior_velocity_detection_area): use base class without RTC Signed-off-by: Takayuki Murooka <[email protected]> * fix Signed-off-by: Takayuki Murooka <[email protected]> * fix Signed-off-by: Takayuki Murooka <[email protected]> --------- Signed-off-by: Takayuki Murooka <[email protected]>
autowarefoundation · Dec 27, 2024 · 5de2e4a · 5de2e4a
1 parent f211728
commit 5de2e4a
Show file tree

Hide file tree

Showing 6 changed files with 15 additions and 28 deletions.
diff --git a/...planner/autoware_behavior_velocity_detection_area_module/config/detection_area.param.yaml b/...planner/autoware_behavior_velocity_detection_area_module/config/detection_area.param.yaml
@@ -8,5 +8,4 @@
       state_clear_time: 2.0
       hold_stop_margin_distance: 0.0
       distance_to_judge_over_stop_line: 0.5
-      enable_rtc: false # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval
       suppress_pass_judge_when_stopping: false
diff --git a/...ng/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/package.xml b/...ng/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/package.xml
@@ -18,7 +18,6 @@
   <buildtool_depend>eigen3_cmake_module</buildtool_depend>
 
   <depend>autoware_behavior_velocity_planner_common</depend>
-  <depend>autoware_behavior_velocity_rtc_interface</depend>
   <depend>autoware_lanelet2_extension</depend>
   <depend>autoware_motion_utils</depend>
   <depend>autoware_planning_msgs</depend>

diff --git a/...ehavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.cpp b/...ehavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.cpp
@@ -34,8 +34,7 @@ using autoware::universe_utils::getOrDeclareParameter;
 using lanelet::autoware::DetectionArea;
 
 DetectionAreaModuleManager::DetectionAreaModuleManager(rclcpp::Node & node)
-: SceneModuleManagerInterfaceWithRTC(
-    node, getModuleName(), getEnableRTC(node, std::string(getModuleName()) + ".enable_rtc"))
+: SceneModuleManagerInterface(node, getModuleName())
 {
   const std::string ns(DetectionAreaModuleManager::getModuleName());
   planner_param_.stop_margin = getOrDeclareParameter<double>(node, ns + ".stop_margin");
@@ -66,25 +65,20 @@ void DetectionAreaModuleManager::launchNewModules(
       registerModule(std::make_shared<DetectionAreaModule>(
         module_id, lane_id, *detection_area_with_lane_id.first, planner_param_,
         logger_.get_child("detection_area_module"), clock_));
-      generateUUID(module_id);
-      updateRTCStatus(
-        getUUID(module_id), true, State::WAITING_FOR_EXECUTION,
-        std::numeric_limits<double>::lowest(), path.header.stamp);
     }
   }
 }
 
-std::function<bool(const std::shared_ptr<SceneModuleInterfaceWithRTC> &)>
+std::function<bool(const std::shared_ptr<SceneModuleInterface> &)>
 DetectionAreaModuleManager::getModuleExpiredFunction(
   const tier4_planning_msgs::msg::PathWithLaneId & path)
 {
   const auto detection_area_id_set = planning_utils::getRegElemIdSetOnPath<DetectionArea>(
     path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose);
 
-  return
-    [detection_area_id_set](const std::shared_ptr<SceneModuleInterfaceWithRTC> & scene_module) {
-      return detection_area_id_set.count(scene_module->getModuleId()) == 0;
-    };
+  return [detection_area_id_set](const std::shared_ptr<SceneModuleInterface> & scene_module) {
+    return detection_area_id_set.count(scene_module->getModuleId()) == 0;
+  };
 }
 
 }  // namespace autoware::behavior_velocity_planner

diff --git a/...ehavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.hpp b/...ehavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.hpp
@@ -19,7 +19,7 @@
 
 #include <autoware/behavior_velocity_planner_common/plugin_interface.hpp>
 #include <autoware/behavior_velocity_planner_common/plugin_wrapper.hpp>
-#include <autoware/behavior_velocity_rtc_interface/scene_module_interface_with_rtc.hpp>
+#include <autoware/behavior_velocity_planner_common/scene_module_interface.hpp>
 #include <rclcpp/rclcpp.hpp>
 
 #include <tier4_planning_msgs/msg/path_with_lane_id.hpp>
@@ -29,7 +29,7 @@
 
 namespace autoware::behavior_velocity_planner
 {
-class DetectionAreaModuleManager : public SceneModuleManagerInterfaceWithRTC
+class DetectionAreaModuleManager : public SceneModuleManagerInterface<>
 {
 public:
   explicit DetectionAreaModuleManager(rclcpp::Node & node);
@@ -41,8 +41,8 @@ class DetectionAreaModuleManager : public SceneModuleManagerInterfaceWithRTC
 
   void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override;
 
-  std::function<bool(const std::shared_ptr<SceneModuleInterfaceWithRTC> &)>
-  getModuleExpiredFunction(const tier4_planning_msgs::msg::PathWithLaneId & path) override;
+  std::function<bool(const std::shared_ptr<SceneModuleInterface> &)> getModuleExpiredFunction(
+    const tier4_planning_msgs::msg::PathWithLaneId & path) override;
 };
 
 class DetectionAreaModulePlugin : public PluginWrapper<DetectionAreaModuleManager>

diff --git a/.../behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp b/.../behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp
@@ -37,7 +37,7 @@ DetectionAreaModule::DetectionAreaModule(
   const lanelet::autoware::DetectionArea & detection_area_reg_elem,
   const PlannerParam & planner_param, const rclcpp::Logger & logger,
   const rclcpp::Clock::SharedPtr clock)
-: SceneModuleInterfaceWithRTC(module_id, logger, clock),
+: SceneModuleInterface(module_id, logger, clock),
   lane_id_(lane_id),
   detection_area_reg_elem_(detection_area_reg_elem),
   state_(State::GO),
@@ -105,12 +105,10 @@ bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path)
     modified_stop_line_seg_idx = current_seg_idx;
   }
 
-  setDistance(stop_dist);
-
   // Check state
-  setSafe(detection_area::can_clear_stop_state(
-    last_obstacle_found_time_, clock_->now(), planner_param_.state_clear_time));
-  if (isActivated()) {
+  const bool is_safe = detection_area::can_clear_stop_state(
+    last_obstacle_found_time_, clock_->now(), planner_param_.state_clear_time);
+  if (is_safe) {
     last_obstacle_found_time_ = {};
     if (!planner_param_.suppress_pass_judge_when_stopping || !is_stopped) {
       state_ = State::GO;
@@ -139,7 +137,6 @@ bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path)
         dead_line_seg_idx);
       if (dist_from_ego_to_dead_line < 0.0) {
         RCLCPP_WARN(logger_, "[detection_area] vehicle is over dead line");
-        setSafe(true);
         return true;
       }
     }
@@ -152,7 +149,6 @@ bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path)
   if (
     state_ != State::STOP &&
     dist_from_ego_to_stop < -planner_param_.distance_to_judge_over_stop_line) {
-    setSafe(true);
     return true;
   }
 
@@ -169,7 +165,6 @@ bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path)
       RCLCPP_WARN_THROTTLE(
         logger_, *clock_, std::chrono::milliseconds(1000).count(),
         "[detection_area] vehicle is over stop border");
-      setSafe(true);
       return true;
     }
   }

diff --git a/.../behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.hpp b/.../behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.hpp
@@ -23,8 +23,8 @@
 
 #define EIGEN_MPL2_ONLY
 #include <Eigen/Core>
+#include <autoware/behavior_velocity_planner_common/scene_module_interface.hpp>
 #include <autoware/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp>
-#include <autoware/behavior_velocity_rtc_interface/scene_module_interface_with_rtc.hpp>
 #include <autoware_lanelet2_extension/regulatory_elements/detection_area.hpp>
 #include <rclcpp/rclcpp.hpp>
 
@@ -38,7 +38,7 @@ using PathIndexWithPoint2d = std::pair<size_t, Point2d>;                // front
 using PathIndexWithOffset = std::pair<size_t, double>;                  // front index, offset
 using tier4_planning_msgs::msg::PathWithLaneId;
 
-class DetectionAreaModule : public SceneModuleInterfaceWithRTC
+class DetectionAreaModule : public SceneModuleInterface
 {
 public:
   enum class State { GO, STOP };