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 index 5d0088c4dd641..6d0e7c26b7248 100644 --- 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 + #include "autoware_auto_planning_msgs/msg/path_with_lane_id.hpp" #include @@ -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 &)> 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 index 82e4211f52c6f..bb063dfbce38e 100644 --- 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 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( 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/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/scene_no_stopping_area.cpp b/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/scene_no_stopping_area.cpp index f738d06a09aa1..ea9568a42ac3e 100644 --- a/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/scene_no_stopping_area.cpp +++ b/planning/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,6 +170,7 @@ 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( @@ -171,7 +178,8 @@ bool NoStoppingAreaModule::modifyPathVelocity( 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