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 286151b90fdf5..3d8b14227a421 100644 --- a/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp +++ b/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp @@ -40,6 +40,7 @@ #include #include +#include struct HazardLampPolicy { @@ -105,6 +106,8 @@ class MrmHandler : public rclcpp::Node const tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr msg); void onOperationModeState( const autoware_adapi_v1_msgs::msg::OperationModeState::ConstSharedPtr msg); + void onRecoverMrm(const std_srvs::srv::Trigger::Request::SharedPtr, + const std_srvs::srv::Trigger::Response::SharedPtr response); // Publisher @@ -135,6 +138,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 9d31c68f161b5..1633b089c3828 100644 --- a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp +++ b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp @@ -86,6 +86,13 @@ MrmHandler::MrmHandler() : 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 odom_ = std::make_shared(); control_mode_ = std::make_shared(); @@ -619,3 +626,15 @@ bool MrmHandler::isArrivedAtGoal() return operation_mode_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; + } +} \ No newline at end of file