diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 1e6e29dd67051..17ac4f3f94f57 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -231,7 +231,6 @@ system/diagnostic_graph_utils/** isamu.takagi@tier4.jp system/dummy_diag_publisher/** fumihito.ito@tier4.jp tetsuhiro.kawaguchi@tier4.jp system/dummy_infrastructure/** ryohsuke.mitsudome@tier4.jp system/duplicated_node_checker/** mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp uken.ryu@tier4.jp -system/emergency_handler/** makoto.kurihara@tier4.jp ryuta.kambe@tier4.jp tetsuhiro.kawaguchi@tier4.jp system/hazard_status_converter/** isamu.takagi@tier4.jp system/mrm_comfortable_stop_operator/** makoto.kurihara@tier4.jp tomohito.ando@tier4.jp system/mrm_emergency_stop_operator/** makoto.kurihara@tier4.jp tomohito.ando@tier4.jp diff --git a/launch/tier4_system_launch/launch/system.launch.xml b/launch/tier4_system_launch/launch/system.launch.xml index d11d00c43d55b..07f6ae8d4bdc2 100644 --- a/launch/tier4_system_launch/launch/system.launch.xml +++ b/launch/tier4_system_launch/launch/system.launch.xml @@ -2,7 +2,6 @@ - @@ -26,8 +25,6 @@ - - @@ -97,27 +94,6 @@ - - - - - - - - - - - - - - - - - - - - - @@ -127,12 +103,12 @@ - + - + diff --git a/launch/tier4_system_launch/package.xml b/launch/tier4_system_launch/package.xml index 789cf136f1152..673596fad9972 100644 --- a/launch/tier4_system_launch/package.xml +++ b/launch/tier4_system_launch/package.xml @@ -12,7 +12,6 @@ autoware_cmake component_state_monitor - emergency_handler system_error_monitor system_monitor diff --git a/launch/tier4_system_launch/system_launch.drawio.svg b/launch/tier4_system_launch/system_launch.drawio.svg index 07a33aff8e30f..987158ba00c2b 100644 --- a/launch/tier4_system_launch/system_launch.drawio.svg +++ b/launch/tier4_system_launch/system_launch.drawio.svg @@ -287,15 +287,15 @@ >
- emergency_handler.launch.xml + mrm_handler.launch.xml

-
package: emergency_handler
+
package: mrm_handler
- emergency_handler.launch.xml... + mrm_handler.launch.xml... diff --git a/system/emergency_handler/CMakeLists.txt b/system/emergency_handler/CMakeLists.txt deleted file mode 100644 index 0475cdbe57dd8..0000000000000 --- a/system/emergency_handler/CMakeLists.txt +++ /dev/null @@ -1,20 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(emergency_handler) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -ament_auto_add_library(${PROJECT_NAME} SHARED - src/emergency_handler/emergency_handler_core.cpp -) - -rclcpp_components_register_node(${PROJECT_NAME} - PLUGIN "EmergencyHandler" - EXECUTABLE ${PROJECT_NAME}_node - EXECUTOR MultiThreadedExecutor -) - -ament_auto_package(INSTALL_TO_SHARE - launch - config -) diff --git a/system/emergency_handler/README.md b/system/emergency_handler/README.md deleted file mode 100644 index f34ebcf8d9b9a..0000000000000 --- a/system/emergency_handler/README.md +++ /dev/null @@ -1,42 +0,0 @@ -# emergency_handler - -## Purpose - -Emergency Handler is a node to select proper MRM from from system failure state contained in HazardStatus. - -## Inner-workings / Algorithms - -### State Transitions - -![fail-safe-state](image/fail-safe-state.drawio.svg) - -## Inputs / Outputs - -### Input - -| Name | Type | Description | -| ----------------------------------------- | ------------------------------------------------ | ----------------------------------------------------------------------------- | -| `/system/emergency/hazard_status` | `autoware_system_msgs::msg::HazardStatusStamped` | Used to select proper MRM from system failure state contained in HazardStatus | -| `/control/vehicle_cmd` | `autoware_control_msgs::msg::Control` | Used as reference when generate Emergency Control Command | -| `/localization/kinematic_state` | `nav_msgs::msg::Odometry` | Used to decide whether vehicle is stopped or not | -| `/vehicle/status/control_mode` | `autoware_vehicle_msgs::msg::ControlModeReport` | Used to check vehicle mode: autonomous or manual | -| `/system/api/mrm/comfortable_stop/status` | `tier4_system_msgs::msg::MrmBehaviorStatus` | Used to check if MRM comfortable stop operation is available | -| `/system/api/mrm/emergency_stop/status` | `tier4_system_msgs::msg::MrmBehaviorStatus` | Used to check if MRM emergency stop operation is available | - -### Output - -| Name | Type | Description | -| ------------------------------------------ | ------------------------------------------------- | ----------------------------------------------------- | -| `/system/emergency/shift_cmd` | `autoware_vehicle_msgs::msg::GearCommand` | Required to execute proper MRM (send gear cmd) | -| `/system/emergency/hazard_cmd` | `autoware_vehicle_msgs::msg::HazardLightsCommand` | Required to execute proper MRM (send turn signal cmd) | -| `/api/fail_safe/mrm_state` | `autoware_adapi_v1_msgs::msg::MrmState` | Inform MRM execution state and selected MRM behavior | -| `/system/api/mrm/comfortable_stop/operate` | `tier4_system_msgs::srv::OperateMrm` | Execution order for MRM comfortable stop | -| `/system/api/mrm/emergency_stop/operate` | `tier4_system_msgs::srv::OperateMrm` | Execution order for MRM emergency stop | - -## Parameters - -{{ json_to_markdown("system/emergency_handler/schema/emergency_handler.schema.json") }} - -## Assumptions / Known limits - -TBD. diff --git a/system/emergency_handler/config/emergency_handler.param.yaml b/system/emergency_handler/config/emergency_handler.param.yaml deleted file mode 100644 index 2309e7f23deb0..0000000000000 --- a/system/emergency_handler/config/emergency_handler.param.yaml +++ /dev/null @@ -1,12 +0,0 @@ -# Default configuration for emergency handler ---- -/**: - ros__parameters: - update_rate: 10 - timeout_hazard_status: 0.5 - use_parking_after_stopped: false - use_comfortable_stop: false - - # setting whether to turn hazard lamp on for each situation - turning_hazard_on: - emergency: true diff --git a/system/emergency_handler/image/fail-safe-state.drawio.svg b/system/emergency_handler/image/fail-safe-state.drawio.svg deleted file mode 100644 index a9cbb1026616b..0000000000000 --- a/system/emergency_handler/image/fail-safe-state.drawio.svg +++ /dev/null @@ -1,239 +0,0 @@ - - - - - - - - - - - - -
-
-
- Normal -
-
-
- - Normal - - - - - - - - - Emergency - - - - - -
-
-
- OverrideRequesting -
-
-
- - OverrideRequesting - - - - - - - - - -
-
-
- MrmOperating -
-
-
- - MrmOperating - - - - - - - - - - -
-
-
- MrmFailed -
-
-
- - MrmFailed - - - - - - - - -
-
-
- MrmSucceeded -
-
-
- - MrmSucceeded - - - - - - - -
-
-
- no reaction -
-
-
- - no reaction - - - - - -
-
-
- failure -
-
-
- - failure - - - - - -
-
-
- warning -
-
-
- - warning - - - - - -
-
-
- emergency -
-
-
- - emergency - - - - - -
-
-
- recovery -
-
-
- - recovery - - - - - -
-
-
- success -
-
-
- - success - - - - - -
-
-
fatal
-
-
- - fatal - - - - - - - Viewer does not support full SVG 1.1 - - - diff --git a/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp b/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp deleted file mode 100644 index b77797b2a4205..0000000000000 --- a/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp +++ /dev/null @@ -1,143 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef EMERGENCY_HANDLER__EMERGENCY_HANDLER_CORE_HPP_ -#define EMERGENCY_HANDLER__EMERGENCY_HANDLER_CORE_HPP_ - -// Core -#include -#include - -// Autoware -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -// ROS 2 core -#include -#include - -#include -#include - -struct HazardLampPolicy -{ - bool emergency; -}; - -struct Param -{ - int update_rate; - double timeout_hazard_status; - bool use_parking_after_stopped; - bool use_comfortable_stop; - HazardLampPolicy turning_hazard_on{}; -}; - -class EmergencyHandler : public rclcpp::Node -{ -public: - explicit EmergencyHandler(const rclcpp::NodeOptions & options); - -private: - // Subscribers with callback - rclcpp::Subscription::SharedPtr - sub_hazard_status_stamped_; - rclcpp::Subscription::SharedPtr sub_prev_control_command_; - // Subscribers without callback - autoware::universe_utils::InterProcessPollingSubscriber sub_odom_{ - this, "~/input/odometry"}; - autoware::universe_utils::InterProcessPollingSubscriber< - autoware_vehicle_msgs::msg::ControlModeReport> - sub_control_mode_{this, "~/input/control_mode"}; - autoware::universe_utils::InterProcessPollingSubscriber - sub_mrm_comfortable_stop_status_{this, "~/input/mrm/comfortable_stop/status"}; - autoware::universe_utils::InterProcessPollingSubscriber - sub_mrm_emergency_stop_status_{this, "~/input/mrm/emergency_stop/status"}; - - autoware_system_msgs::msg::HazardStatusStamped::ConstSharedPtr hazard_status_stamped_; - autoware_control_msgs::msg::Control::ConstSharedPtr prev_control_command_; - - void onHazardStatusStamped( - const autoware_system_msgs::msg::HazardStatusStamped::ConstSharedPtr msg); - void onPrevControlCommand(const autoware_control_msgs::msg::Control::ConstSharedPtr msg); - - // Publisher - rclcpp::Publisher::SharedPtr pub_control_command_; - - // rclcpp::Publisher::SharedPtr pub_shift_; - // rclcpp::Publisher::SharedPtr pub_turn_signal_; - rclcpp::Publisher::SharedPtr pub_hazard_cmd_; - rclcpp::Publisher::SharedPtr pub_gear_cmd_; - - autoware_vehicle_msgs::msg::HazardLightsCommand createHazardCmdMsg(); - autoware_vehicle_msgs::msg::GearCommand createGearCmdMsg(); - void publishControlCommands(); - - rclcpp::Publisher::SharedPtr pub_mrm_state_; - - autoware_adapi_v1_msgs::msg::MrmState mrm_state_; - void publishMrmState(); - - // Clients - rclcpp::CallbackGroup::SharedPtr client_mrm_comfortable_stop_group_; - rclcpp::Client::SharedPtr client_mrm_comfortable_stop_; - rclcpp::CallbackGroup::SharedPtr client_mrm_emergency_stop_group_; - rclcpp::Client::SharedPtr client_mrm_emergency_stop_; - - void callMrmBehavior( - const autoware_adapi_v1_msgs::msg::MrmState::_behavior_type & mrm_behavior) const; - void cancelMrmBehavior( - const autoware_adapi_v1_msgs::msg::MrmState::_behavior_type & mrm_behavior) const; - void logMrmCallingResult( - const tier4_system_msgs::srv::OperateMrm::Response & result, const std::string & behavior, - bool is_call) const; - - // Timer - rclcpp::TimerBase::SharedPtr timer_; - - // Parameters - Param param_; - - bool isDataReady(); - void onTimer(); - - // Heartbeat - rclcpp::Time stamp_hazard_status_; - bool is_hazard_status_timeout_; - void checkHazardStatusTimeout(); - - // Algorithm - uint8_t last_gear_command_{autoware_vehicle_msgs::msg::GearCommand::DRIVE}; - void transitionTo(const int new_state); - void updateMrmState(); - void operateMrm(); - autoware_adapi_v1_msgs::msg::MrmState::_behavior_type getCurrentMrmBehavior(); - - bool isAutonomous(); - bool isDrivingBackwards(); - bool isEmergency(); - bool isStopped(); - bool isComfortableStopStatusAvailable(); - bool isEmergencyStopStatusAvailable(); -}; - -#endif // EMERGENCY_HANDLER__EMERGENCY_HANDLER_CORE_HPP_ diff --git a/system/emergency_handler/launch/emergency_handler.launch.xml b/system/emergency_handler/launch/emergency_handler.launch.xml deleted file mode 100644 index 203c13bd94e0a..0000000000000 --- a/system/emergency_handler/launch/emergency_handler.launch.xml +++ /dev/null @@ -1,35 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/system/emergency_handler/package.xml b/system/emergency_handler/package.xml deleted file mode 100644 index e16c1b60a5be0..0000000000000 --- a/system/emergency_handler/package.xml +++ /dev/null @@ -1,31 +0,0 @@ - - - - emergency_handler - 0.1.0 - The emergency_handler ROS 2 package - Makoto Kurihara - Ryuta Kambe - Tetsuhiro Kawaguchi - Apache License 2.0 - - ament_cmake_auto - autoware_cmake - - autoware_adapi_v1_msgs - autoware_control_msgs - autoware_system_msgs - autoware_universe_utils - autoware_vehicle_msgs - nav_msgs - rclcpp - rclcpp_components - tier4_system_msgs - - ament_lint_auto - autoware_lint_common - - - ament_cmake - - diff --git a/system/emergency_handler/schema/emergency_handler.schema.json b/system/emergency_handler/schema/emergency_handler.schema.json deleted file mode 100644 index 3276ae3fa1553..0000000000000 --- a/system/emergency_handler/schema/emergency_handler.schema.json +++ /dev/null @@ -1,65 +0,0 @@ -{ - "$schema": "http://json-schema.org/draft-07/schema#", - "title": "Parameters for emergency handler", - "type": "object", - "definitions": { - "emergency_handler": { - "type": "object", - "properties": { - "update_rate": { - "type": "integer", - "description": "Timer callback period.", - "default": 10 - }, - "timeout_hazard_status": { - "type": "number", - "description": "If the input `hazard_status` topic cannot be received for more than `timeout_hazard_status`, vehicle will make an emergency stop.", - "default": 0.5 - }, - "use_parking_after_stopped": { - "type": "boolean", - "description": "If this parameter is true, it will publish PARKING shift command.", - "default": "false" - }, - "use_comfortable_stop": { - "type": "boolean", - "description": "If this parameter is true, operate comfortable stop when latent faults occur.", - "default": "false" - }, - "turning_hazard_on": { - "type": "object", - "properties": { - "emergency": { - "type": "boolean", - "description": "If this parameter is true, hazard lamps will be turned on during emergency state.", - "default": "true" - } - }, - "required": ["emergency"] - } - }, - "required": [ - "update_rate", - "timeout_hazard_status", - "use_parking_after_stopped", - "use_comfortable_stop", - "turning_hazard_on" - ], - "additionalProperties": false - } - }, - "properties": { - "/**": { - "type": "object", - "properties": { - "ros__parameters": { - "$ref": "#/definitions/emergency_handler" - } - }, - "required": ["ros__parameters"], - "additionalProperties": false - } - }, - "required": ["/**"], - "additionalProperties": false -} diff --git a/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp b/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp deleted file mode 100644 index b2ee12afc9c76..0000000000000 --- a/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp +++ /dev/null @@ -1,451 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "emergency_handler/emergency_handler_core.hpp" - -#include -#include -#include - -EmergencyHandler::EmergencyHandler(const rclcpp::NodeOptions & options) -: Node("emergency_handler", options) -{ - // Parameter - param_.update_rate = declare_parameter("update_rate"); - param_.timeout_hazard_status = declare_parameter("timeout_hazard_status"); - param_.use_parking_after_stopped = declare_parameter("use_parking_after_stopped"); - param_.use_comfortable_stop = declare_parameter("use_comfortable_stop"); - param_.turning_hazard_on.emergency = declare_parameter("turning_hazard_on.emergency"); - - using std::placeholders::_1; - - // Subscribers with callback - sub_hazard_status_stamped_ = create_subscription( - "~/input/hazard_status", rclcpp::QoS{1}, - std::bind(&EmergencyHandler::onHazardStatusStamped, this, _1)); - sub_prev_control_command_ = create_subscription( - "~/input/prev_control_command", rclcpp::QoS{1}, - std::bind(&EmergencyHandler::onPrevControlCommand, this, _1)); - - // Publisher - pub_control_command_ = create_publisher( - "~/output/control_command", rclcpp::QoS{1}); - pub_hazard_cmd_ = create_publisher( - "~/output/hazard", rclcpp::QoS{1}); - pub_gear_cmd_ = - create_publisher("~/output/gear", rclcpp::QoS{1}); - pub_mrm_state_ = - create_publisher("~/output/mrm/state", rclcpp::QoS{1}); - - // Clients - client_mrm_comfortable_stop_group_ = - create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - client_mrm_comfortable_stop_ = create_client( - "~/output/mrm/comfortable_stop/operate", rmw_qos_profile_services_default, - client_mrm_comfortable_stop_group_); - client_mrm_emergency_stop_group_ = - create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - client_mrm_emergency_stop_ = create_client( - "~/output/mrm/emergency_stop/operate", rmw_qos_profile_services_default, - client_mrm_emergency_stop_group_); - - // Initialize - mrm_state_.stamp = this->now(); - mrm_state_.state = autoware_adapi_v1_msgs::msg::MrmState::NORMAL; - mrm_state_.behavior = autoware_adapi_v1_msgs::msg::MrmState::NONE; - is_hazard_status_timeout_ = false; - - // Timer - const auto update_period_ns = rclcpp::Rate(param_.update_rate).period(); - timer_ = rclcpp::create_timer( - this, get_clock(), update_period_ns, std::bind(&EmergencyHandler::onTimer, this)); -} - -void EmergencyHandler::onHazardStatusStamped( - const autoware_system_msgs::msg::HazardStatusStamped::ConstSharedPtr msg) -{ - hazard_status_stamped_ = msg; - stamp_hazard_status_ = this->now(); -} - -void EmergencyHandler::onPrevControlCommand( - const autoware_control_msgs::msg::Control::ConstSharedPtr msg) -{ - auto control_command = new autoware_control_msgs::msg::Control(*msg); - control_command->stamp = msg->stamp; - prev_control_command_ = autoware_control_msgs::msg::Control::ConstSharedPtr(control_command); -} - -autoware_vehicle_msgs::msg::HazardLightsCommand EmergencyHandler::createHazardCmdMsg() -{ - using autoware_vehicle_msgs::msg::HazardLightsCommand; - HazardLightsCommand msg; - - // Check emergency - const bool is_emergency = isEmergency(); - - if (hazard_status_stamped_->status.emergency_holding) { - // turn hazard on during emergency holding - msg.command = HazardLightsCommand::ENABLE; - } else if (is_emergency && param_.turning_hazard_on.emergency) { - // turn hazard on if vehicle is in emergency state and - // turning hazard on if emergency flag is true - msg.command = HazardLightsCommand::ENABLE; - - } else { - msg.command = HazardLightsCommand::NO_COMMAND; - } - return msg; -} - -void EmergencyHandler::publishControlCommands() -{ - using autoware_vehicle_msgs::msg::GearCommand; - - // Create timestamp - const auto stamp = this->now(); - - // Publish hazard command - pub_hazard_cmd_->publish(createHazardCmdMsg()); - - // Publish gear - { - GearCommand msg; - msg.stamp = stamp; - const auto command = [&]() { - // If stopped and use_parking is not true, send the last gear command - if (isStopped()) - return (param_.use_parking_after_stopped) ? GearCommand::PARK : last_gear_command_; - return (isDrivingBackwards()) ? GearCommand::REVERSE : GearCommand::DRIVE; - }(); - - msg.command = command; - last_gear_command_ = msg.command; - pub_gear_cmd_->publish(msg); - return; - } -} - -void EmergencyHandler::publishMrmState() -{ - mrm_state_.stamp = this->now(); - pub_mrm_state_->publish(mrm_state_); -} - -void EmergencyHandler::operateMrm() -{ - using autoware_adapi_v1_msgs::msg::MrmState; - - if (mrm_state_.state == MrmState::NORMAL) { - // Cancel MRM behavior when returning to NORMAL state - const auto current_mrm_behavior = MrmState::NONE; - if (current_mrm_behavior != mrm_state_.behavior) { - cancelMrmBehavior(mrm_state_.behavior); - mrm_state_.behavior = current_mrm_behavior; - } - return; - } - if (mrm_state_.state == MrmState::MRM_OPERATING) { - const auto current_mrm_behavior = getCurrentMrmBehavior(); - if (current_mrm_behavior != mrm_state_.behavior) { - cancelMrmBehavior(mrm_state_.behavior); - callMrmBehavior(current_mrm_behavior); - mrm_state_.behavior = current_mrm_behavior; - } - return; - } - if (mrm_state_.state == MrmState::MRM_SUCCEEDED) { - // TODO(mkuri): operate MRC behavior - // Do nothing - return; - } - if (mrm_state_.state == MrmState::MRM_FAILED) { - // Do nothing - return; - } - RCLCPP_WARN(this->get_logger(), "invalid MRM state: %d", mrm_state_.state); -} - -void EmergencyHandler::callMrmBehavior( - const autoware_adapi_v1_msgs::msg::MrmState::_behavior_type & mrm_behavior) const -{ - using autoware_adapi_v1_msgs::msg::MrmState; - - auto request = std::make_shared(); - request->operate = true; - - if (mrm_behavior == MrmState::NONE) { - RCLCPP_WARN(this->get_logger(), "MRM behavior is None. Do nothing."); - return; - } - if (mrm_behavior == MrmState::COMFORTABLE_STOP) { - auto result = client_mrm_comfortable_stop_->async_send_request(request).get(); - if (result->response.success == true) { - RCLCPP_WARN(this->get_logger(), "Comfortable stop is operated"); - } else { - RCLCPP_ERROR(this->get_logger(), "Comfortable stop is failed to operate"); - } - return; - } - if (mrm_behavior == MrmState::EMERGENCY_STOP) { - auto result = client_mrm_emergency_stop_->async_send_request(request).get(); - if (result->response.success == true) { - RCLCPP_WARN(this->get_logger(), "Emergency stop is operated"); - } else { - RCLCPP_ERROR(this->get_logger(), "Emergency stop is failed to operate"); - } - return; - } - RCLCPP_WARN(this->get_logger(), "invalid MRM behavior: %d", mrm_behavior); -} - -void EmergencyHandler::cancelMrmBehavior( - const autoware_adapi_v1_msgs::msg::MrmState::_behavior_type & mrm_behavior) const -{ - using autoware_adapi_v1_msgs::msg::MrmState; - - auto request = std::make_shared(); - request->operate = false; - - if (mrm_behavior == MrmState::NONE) { - // Do nothing - return; - } - if (mrm_behavior == MrmState::COMFORTABLE_STOP) { - auto result = client_mrm_comfortable_stop_->async_send_request(request).get(); - if (result->response.success == true) { - RCLCPP_WARN(this->get_logger(), "Comfortable stop is canceled"); - } else { - RCLCPP_ERROR(this->get_logger(), "Comfortable stop is failed to cancel"); - } - return; - } - if (mrm_behavior == MrmState::EMERGENCY_STOP) { - auto result = client_mrm_emergency_stop_->async_send_request(request).get(); - if (result->response.success == true) { - RCLCPP_WARN(this->get_logger(), "Emergency stop is canceled"); - } else { - RCLCPP_ERROR(this->get_logger(), "Emergency stop is failed to cancel"); - } - return; - } - RCLCPP_WARN(this->get_logger(), "invalid MRM behavior: %d", mrm_behavior); -} - -bool EmergencyHandler::isDataReady() -{ - if (!hazard_status_stamped_) { - RCLCPP_INFO_THROTTLE( - this->get_logger(), *this->get_clock(), std::chrono::milliseconds(5000).count(), - "waiting for hazard_status_stamped msg..."); - return false; - } - - if (param_.use_comfortable_stop && !isComfortableStopStatusAvailable()) { - RCLCPP_INFO_THROTTLE( - this->get_logger(), *this->get_clock(), std::chrono::milliseconds(5000).count(), - "waiting for mrm comfortable stop to become available..."); - return false; - } - - if (!isEmergencyStopStatusAvailable()) { - RCLCPP_INFO_THROTTLE( - this->get_logger(), *this->get_clock(), std::chrono::milliseconds(5000).count(), - "waiting for mrm emergency stop to become available..."); - return false; - } - - return true; -} - -void EmergencyHandler::checkHazardStatusTimeout() -{ - if ((this->now() - stamp_hazard_status_).seconds() > param_.timeout_hazard_status) { - is_hazard_status_timeout_ = true; - RCLCPP_WARN_THROTTLE( - this->get_logger(), *this->get_clock(), std::chrono::milliseconds(1000).count(), - "heartbeat_hazard_status is timeout"); - } else { - is_hazard_status_timeout_ = false; - } -} - -void EmergencyHandler::onTimer() -{ - if (!isDataReady()) { - return; - } - - // Check whether heartbeat hazard_status is timeout - checkHazardStatusTimeout(); - - // Update Emergency State - updateMrmState(); - - // Publish control commands - publishControlCommands(); - operateMrm(); - publishMrmState(); -} - -void EmergencyHandler::transitionTo(const int new_state) -{ - using autoware_adapi_v1_msgs::msg::MrmState; - - const auto state2string = [](const int state) { - if (state == MrmState::NORMAL) { - return "NORMAL"; - } - if (state == MrmState::MRM_OPERATING) { - return "MRM_OPERATING"; - } - if (state == MrmState::MRM_SUCCEEDED) { - return "MRM_SUCCEEDED"; - } - if (state == MrmState::MRM_FAILED) { - return "MRM_FAILED"; - } - - const auto msg = "invalid state: " + std::to_string(state); - throw std::runtime_error(msg); - }; - - RCLCPP_DEBUG( - this->get_logger(), "MRM State changed: %s -> %s", state2string(mrm_state_.state), - state2string(new_state)); - - mrm_state_.state = new_state; -} - -void EmergencyHandler::updateMrmState() -{ - using autoware_adapi_v1_msgs::msg::MrmState; - using autoware_vehicle_msgs::msg::ControlModeReport; - - // Check emergency - const bool is_emergency = isEmergency(); - - // Get mode - const bool is_auto_mode = isAutonomous(); - - // State Machine - if (mrm_state_.state == MrmState::NORMAL) { - // NORMAL - if (is_auto_mode && is_emergency) { - transitionTo(MrmState::MRM_OPERATING); - return; - } - } else { - // Emergency - // Send recovery events if "not emergency" - if (!is_emergency) { - transitionTo(MrmState::NORMAL); - return; - } - - if (mrm_state_.state == MrmState::MRM_OPERATING) { - // TODO(Kenji Miyake): Check MRC is accomplished - if (isStopped()) { - transitionTo(MrmState::MRM_SUCCEEDED); - return; - } - } else if (mrm_state_.state == MrmState::MRM_SUCCEEDED) { - // Do nothing(only checking common recovery events) - } else if (mrm_state_.state == MrmState::MRM_FAILED) { - // Do nothing(only checking common recovery events) - } else { - const auto msg = "invalid state: " + std::to_string(mrm_state_.state); - throw std::runtime_error(msg); - } - } -} - -autoware_adapi_v1_msgs::msg::MrmState::_behavior_type EmergencyHandler::getCurrentMrmBehavior() -{ - using autoware_adapi_v1_msgs::msg::MrmState; - using autoware_system_msgs::msg::HazardStatus; - - // Get hazard level - auto level = hazard_status_stamped_->status.level; - if (is_hazard_status_timeout_) { - level = HazardStatus::SINGLE_POINT_FAULT; - } - - // State machine - if (mrm_state_.behavior == MrmState::NONE) { - if (level == HazardStatus::LATENT_FAULT) { - if (param_.use_comfortable_stop) { - return MrmState::COMFORTABLE_STOP; - } - return MrmState::EMERGENCY_STOP; - } - if (level == HazardStatus::SINGLE_POINT_FAULT) { - return MrmState::EMERGENCY_STOP; - } - } - if (mrm_state_.behavior == MrmState::COMFORTABLE_STOP) { - if (level == HazardStatus::SINGLE_POINT_FAULT) { - return MrmState::EMERGENCY_STOP; - } - } - - return mrm_state_.behavior; -} - -bool EmergencyHandler::isAutonomous() -{ - using autoware_vehicle_msgs::msg::ControlModeReport; - auto mode = sub_control_mode_.takeData(); - if (mode == nullptr) return false; - return mode->mode == ControlModeReport::AUTONOMOUS; -} - -bool EmergencyHandler::isEmergency() -{ - return hazard_status_stamped_->status.emergency || - hazard_status_stamped_->status.emergency_holding || is_hazard_status_timeout_; -} - -bool EmergencyHandler::isStopped() -{ - auto odom = sub_odom_.takeData(); - if (odom == nullptr) return false; - constexpr auto th_stopped_velocity = 0.001; - return (std::abs(odom->twist.twist.linear.x) < th_stopped_velocity); -} - -bool EmergencyHandler::isComfortableStopStatusAvailable() -{ - auto status = sub_mrm_comfortable_stop_status_.takeData(); - if (status == nullptr) return false; - return status->state != tier4_system_msgs::msg::MrmBehaviorStatus::NOT_AVAILABLE; -} - -bool EmergencyHandler::isEmergencyStopStatusAvailable() -{ - auto status = sub_mrm_emergency_stop_status_.takeData(); - if (status == nullptr) return false; - return status->state != tier4_system_msgs::msg::MrmBehaviorStatus::NOT_AVAILABLE; -} - -bool EmergencyHandler::isDrivingBackwards() -{ - auto odom = sub_odom_.takeData(); - if (odom == nullptr) return false; - constexpr auto th_moving_backwards = -0.001; - return odom->twist.twist.linear.x < th_moving_backwards; -} - -#include -RCLCPP_COMPONENTS_REGISTER_NODE(EmergencyHandler)