diff --git a/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp b/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp index b2d835778e93e..1ea48d8820fc1 100644 --- a/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp +++ b/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp @@ -39,6 +39,7 @@ #include #include +#include struct HazardLampPolicy { @@ -95,6 +96,9 @@ class MrmHandler : public rclcpp::Node void onOperationModeAvailability( const tier4_system_msgs::msg::OperationModeAvailability::ConstSharedPtr msg); + void onRecoverMrm( + const std_srvs::srv::Trigger::Request::SharedPtr, + const std_srvs::srv::Trigger::Response::SharedPtr response); // Publisher @@ -119,6 +123,9 @@ class MrmHandler : public rclcpp::Node rclcpp::CallbackGroup::SharedPtr client_mrm_emergency_stop_group_; rclcpp::Client::SharedPtr client_mrm_emergency_stop_; + // Services + rclcpp::Service::SharedPtr service_recover_mrm_; + bool requestMrmBehavior( const autoware_adapi_v1_msgs::msg::MrmState::_behavior_type & mrm_behavior, RequestType request_type) const; diff --git a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp index e8c057152ad94..aff0adc3c555f 100644 --- a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp +++ b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp @@ -69,6 +69,12 @@ MrmHandler::MrmHandler(const rclcpp::NodeOptions & options) : Node("mrm_handler" "~/output/mrm/emergency_stop/operate", rmw_qos_profile_services_default, client_mrm_emergency_stop_group_); + // Services + service_recover_mrm_ = create_service( + "/system/clear_mrm", + std::bind(&MrmHandler::onRecoverMrm, this, std::placeholders::_1, std::placeholders::_2), + rmw_qos_profile_services_default); + // Initialize mrm_state_.stamp = this->now(); mrm_state_.state = autoware_adapi_v1_msgs::msg::MrmState::NORMAL; @@ -582,5 +588,17 @@ bool MrmHandler::isArrivedAtGoal() return state->mode == OperationModeState::STOP; } +void MrmHandler::onRecoverMrm( + const std_srvs::srv::Trigger::Request::SharedPtr, + const std_srvs::srv::Trigger::Response::SharedPtr response) +{ + if (!param_.is_mrm_recoverable) { + is_mrm_holding_ = false; + response->success = true; + } else { + response->success = false; + } +} + #include RCLCPP_COMPONENTS_REGISTER_NODE(MrmHandler)