diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/config/detection_area.param.yaml b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/config/detection_area.param.yaml
index b49b43794685c..9294795274066 100644
--- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/config/detection_area.param.yaml
+++ b/planning/behavior_velocity_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/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/package.xml
index 90aaddf96feef..ff91cf40a32a6 100644
--- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/package.xml
+++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/package.xml
@@ -18,7 +18,6 @@
eigen3_cmake_module
autoware_behavior_velocity_planner_common
- autoware_behavior_velocity_rtc_interface
autoware_lanelet2_extension
autoware_motion_utils
autoware_planning_msgs
diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.cpp
index 62f5b88699a37..fb2c17d9e3745 100644
--- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.cpp
+++ b/planning/behavior_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(node, ns + ".stop_margin");
@@ -66,25 +65,20 @@ void DetectionAreaModuleManager::launchNewModules(
registerModule(std::make_shared(
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::lowest(), path.header.stamp);
}
}
}
-std::function &)>
+std::function &)>
DetectionAreaModuleManager::getModuleExpiredFunction(
const tier4_planning_msgs::msg::PathWithLaneId & path)
{
const auto detection_area_id_set = planning_utils::getRegElemIdSetOnPath(
path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose);
- return
- [detection_area_id_set](const std::shared_ptr & scene_module) {
- return detection_area_id_set.count(scene_module->getModuleId()) == 0;
- };
+ return [detection_area_id_set](const std::shared_ptr & scene_module) {
+ return detection_area_id_set.count(scene_module->getModuleId()) == 0;
+ };
}
} // namespace autoware::behavior_velocity_planner
diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.hpp
index 1fbcc16461ebc..4b34ac4a45298 100644
--- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.hpp
+++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.hpp
@@ -19,7 +19,7 @@
#include
#include
-#include
+#include
#include
#include
@@ -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 &)>
- getModuleExpiredFunction(const tier4_planning_msgs::msg::PathWithLaneId & path) override;
+ std::function &)> getModuleExpiredFunction(
+ const tier4_planning_msgs::msg::PathWithLaneId & path) override;
};
class DetectionAreaModulePlugin : public PluginWrapper
diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp
index 6d7754624485c..031c45753022e 100644
--- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp
+++ b/planning/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/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.hpp
index bdf2d05a35bcb..9224cf4624687 100644
--- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.hpp
+++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.hpp
@@ -23,8 +23,8 @@
#define EIGEN_MPL2_ONLY
#include
+#include
#include
-#include
#include
#include
@@ -38,7 +38,7 @@ using PathIndexWithPoint2d = std::pair; // front
using PathIndexWithOffset = std::pair; // front index, offset
using tier4_planning_msgs::msg::PathWithLaneId;
-class DetectionAreaModule : public SceneModuleInterfaceWithRTC
+class DetectionAreaModule : public SceneModuleInterface
{
public:
enum class State { GO, STOP };