diff --git a/system/emergency_handler/CMakeLists.txt b/system/emergency_handler/CMakeLists.txt new file mode 100644 index 0000000000000..a1daea31bf6ee --- /dev/null +++ b/system/emergency_handler/CMakeLists.txt @@ -0,0 +1,39 @@ +cmake_minimum_required(VERSION 3.5) +project(emergency_handler) + +### Compile options +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +### Dependencies +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +### Target executable +set(EMERGENCY_HANDLER_SRC + src/emergency_handler/emergency_handler_core.cpp) + +set(EMERGENCY_HANDLER_HEADERS + include/emergency_handler/emergency_handler_core.hpp) + +ament_auto_add_executable(emergency_handler + src/emergency_handler/emergency_handler_node.cpp + ${EMERGENCY_HANDLER_SRC} + ${EMERGENCY_HANDLER_HEADERS} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package(INSTALL_TO_SHARE + launch + config +) diff --git a/system/emergency_handler/README.md b/system/emergency_handler/README.md new file mode 100644 index 0000000000000..253f53a07c3d5 --- /dev/null +++ b/system/emergency_handler/README.md @@ -0,0 +1,51 @@ +# 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](https://tier4.github.io/autoware.proj/tree/main/design/apis/image/fail-safe-state.drawio.svg) + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| --------------------------------- | ---------------------------------------------------------- | ----------------------------------------------------------------------------- | +| `/system/emergency/hazard_status` | `autoware_auto_system_msgs::msg::HazardStatusStamped` | Used to select proper MRM from system failure state contained in HazardStatus | +| `/control/vehicle_cmd` | `autoware_auto_control_msgs::msg::AckermannControlCommand` | 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_auto_vehicle_msgs::msg::ControlModeReport` | Used to check vehicle mode: autonomous or manual. | + +### Output + +| Name | Type | Description | +| ----------------------------------- | ---------------------------------------------------------- | ----------------------------------------------------- | +| `/system/emergency/control_cmd` | `autoware_auto_control_msgs::msg::AckermannControlCommand` | Required to execute proper MRM | +| `/system/emergency/shift_cmd` | `autoware_auto_vehicle_msgs::msg::GearCommand` | Required to execute proper MRM (send gear cmd) | +| `/system/emergency/hazard_cmd` | `autoware_auto_vehicle_msgs::msg::HazardLightsCommand` | Required to execute proper MRM (send turn signal cmd) | +| `/system/emergency/emergency_state` | `autoware_auto_system_msgs::msg::EmergencyStateStamped` | Used to inform the emergency situation of the vehicle | + +## Parameters + +### Node Parameters + +| Name | Type | Default Value | Explanation | +| ----------- | ---- | ------------- | ---------------------- | +| update_rate | int | `10` | Timer callback period. | + +### Core Parameters + +| Name | Type | Default Value | Explanation | +| --------------------------- | ------ | ------------- | --------------------------------------------------------------------------------------------------------------------------------- | +| timeout_hazard_status | double | `0.5` | If the input `hazard_status` topic cannot be received for more than `timeout_hazard_status`, vehicle will make an emergency stop. | +| use_parking_after_stopped | bool | `false` | If this parameter is true, it will publish PARKING shift command. | +| turning_hazard_on.emergency | bool | `true` | If this parameter is true, hazard lamps will be turned on during emergency state. | + +## Assumptions / Known limits + +TBD. diff --git a/system/emergency_handler/config/emergency_handler.param.yaml b/system/emergency_handler/config/emergency_handler.param.yaml new file mode 100644 index 0000000000000..912e48460cdae --- /dev/null +++ b/system/emergency_handler/config/emergency_handler.param.yaml @@ -0,0 +1,13 @@ +# Default configuration for emergency handler +--- +/**: + ros__parameters: + update_rate: 10 + timeout_hazard_status: 0.5 + timeout_takeover_request: 10.0 + use_takeover_request: false + use_parking_after_stopped: false + + # setting whether to turn hazard lamp on for each situation + turning_hazard_on: + emergency: true diff --git a/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp b/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp new file mode 100644 index 0000000000000..669aee2f80dba --- /dev/null +++ b/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp @@ -0,0 +1,120 @@ +// 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 + +// ROS2 core +#include +#include +#include + +#include +#include + +struct HazardLampPolicy +{ + bool emergency; +}; + +struct Param +{ + int update_rate; + double timeout_hazard_status; + double timeout_takeover_request; + bool use_takeover_request; + bool use_parking_after_stopped; + HazardLampPolicy turning_hazard_on{}; +}; + +class EmergencyHandler : public rclcpp::Node +{ +public: + EmergencyHandler(); + +private: + // Subscribers + rclcpp::Subscription::SharedPtr + sub_hazard_status_stamped_; + rclcpp::Subscription::SharedPtr + sub_prev_control_command_; + rclcpp::Subscription::SharedPtr sub_odom_; + rclcpp::Subscription::SharedPtr + sub_control_mode_; + + autoware_auto_system_msgs::msg::HazardStatusStamped::ConstSharedPtr hazard_status_stamped_; + autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr prev_control_command_; + nav_msgs::msg::Odometry::ConstSharedPtr odom_; + autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr control_mode_; + + void onHazardStatusStamped( + const autoware_auto_system_msgs::msg::HazardStatusStamped::ConstSharedPtr msg); + void onPrevControlCommand( + const autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr msg); + void onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg); + void onControlMode(const autoware_auto_vehicle_msgs::msg::ControlModeReport::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_; + rclcpp::Publisher::SharedPtr pub_emergency_state_; + + autoware_auto_vehicle_msgs::msg::HazardLightsCommand createHazardCmdMsg(); + autoware_auto_vehicle_msgs::msg::GearCommand createGearCmdMsg(); + void publishControlCommands(); + + // Timer + rclcpp::TimerBase::SharedPtr timer_; + + // Parameters + Param param_; + + bool isDataReady(); + void onTimer(); + + // Heartbeat + std::shared_ptr> + heartbeat_hazard_status_; + + // Algorithm + autoware_auto_system_msgs::msg::EmergencyState::_state_type emergency_state_{ + autoware_auto_system_msgs::msg::EmergencyState::NORMAL}; + rclcpp::Time takeover_requested_time_; + + void transitionTo(const int new_state); + void updateEmergencyState(); + bool isStopped(); + bool isEmergency(const autoware_auto_system_msgs::msg::HazardStatus & hazard_status); + autoware_auto_control_msgs::msg::AckermannControlCommand selectAlternativeControlCommand(); +}; + +#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 new file mode 100644 index 0000000000000..e12ac740362f0 --- /dev/null +++ b/system/emergency_handler/launch/emergency_handler.launch.xml @@ -0,0 +1,29 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/system/emergency_handler/package.xml b/system/emergency_handler/package.xml new file mode 100644 index 0000000000000..1860d7a7820b3 --- /dev/null +++ b/system/emergency_handler/package.xml @@ -0,0 +1,26 @@ + + + emergency_handler + 0.1.0 + The emergency_handler ROS2 package + Kenji Miyake + Apache License 2.0 + + ament_cmake_auto + + autoware_auto_control_msgs + autoware_auto_system_msgs + autoware_auto_vehicle_msgs + autoware_utils + nav_msgs + rclcpp + std_msgs + std_srvs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp b/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp new file mode 100644 index 0000000000000..7b117eff30439 --- /dev/null +++ b/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp @@ -0,0 +1,318 @@ +// 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() : Node("emergency_handler") +{ + // Parameter + param_.update_rate = declare_parameter("update_rate", 10); + param_.timeout_hazard_status = declare_parameter("timeout_hazard_status", 0.5); + param_.timeout_takeover_request = declare_parameter("timeout_takeover_request", 10.0); + param_.use_takeover_request = declare_parameter("use_takeover_request", false); + param_.use_parking_after_stopped = declare_parameter("use_parking_after_stopped", false); + param_.turning_hazard_on.emergency = declare_parameter("turning_hazard_on.emergency", true); + + using std::placeholders::_1; + + // Subscriber + 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)); + sub_odom_ = create_subscription( + "~/input/odometry", rclcpp::QoS{1}, std::bind(&EmergencyHandler::onOdometry, this, _1)); + // subscribe control mode + sub_control_mode_ = create_subscription( + "~/input/control_mode", rclcpp::QoS{1}, std::bind(&EmergencyHandler::onControlMode, this, _1)); + + // Heartbeat + heartbeat_hazard_status_ = std::make_shared< + HeaderlessHeartbeatChecker>( + *this, "~/input/hazard_status", param_.timeout_hazard_status); + + // 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_emergency_state_ = create_publisher( + "~/output/emergency_state", rclcpp::QoS{1}); + + // Initialize + odom_ = std::make_shared(); + control_mode_ = std::make_shared(); + prev_control_command_ = autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr( + new autoware_auto_control_msgs::msg::AckermannControlCommand); + + // Timer + auto timer_callback = std::bind(&EmergencyHandler::onTimer, this); + const auto update_period_ns = rclcpp::Rate(param_.update_rate).period(); + + timer_ = std::make_shared>( + this->get_clock(), update_period_ns, std::move(timer_callback), + this->get_node_base_interface()->get_context()); + this->get_node_timers_interface()->add_timer(timer_, nullptr); +} + +void EmergencyHandler::onHazardStatusStamped( + const autoware_auto_system_msgs::msg::HazardStatusStamped::ConstSharedPtr msg) +{ + hazard_status_stamped_ = msg; +} + +void EmergencyHandler::onPrevControlCommand( + const autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr msg) +{ + auto control_command = new autoware_auto_control_msgs::msg::AckermannControlCommand(*msg); + control_command->stamp = msg->stamp; + prev_control_command_ = + autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr(control_command); +} + +void EmergencyHandler::onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg) +{ + odom_ = msg; +} + +void EmergencyHandler::onControlMode( + const autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg) +{ + control_mode_ = msg; +} + +autoware_auto_vehicle_msgs::msg::HazardLightsCommand EmergencyHandler::createHazardCmdMsg() +{ + using autoware_auto_vehicle_msgs::msg::HazardLightsCommand; + HazardLightsCommand msg; + + // Check emergency + const bool is_emergency = isEmergency(hazard_status_stamped_->status); + + 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_auto_vehicle_msgs::msg::GearCommand; + + // Create timestamp + const auto stamp = this->now(); + + // Publish ControlCommand + { + autoware_auto_control_msgs::msg::AckermannControlCommand msg; + msg = selectAlternativeControlCommand(); + msg.stamp = stamp; + pub_control_command_->publish(msg); + } + + // Publish hazard command + pub_hazard_cmd_->publish(createHazardCmdMsg()); + + // Publish gear + if (param_.use_parking_after_stopped && isStopped()) { + GearCommand msg; + msg.stamp = stamp; + msg.command = GearCommand::PARK; + pub_gear_cmd_->publish(msg); + } + + // Publish Emergency State + { + autoware_auto_system_msgs::msg::EmergencyState emergency_state; + emergency_state.stamp = stamp; + emergency_state.state = emergency_state_; + pub_emergency_state_->publish(emergency_state); + } +} + +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; + } + + return true; +} + +void EmergencyHandler::onTimer() +{ + if (!isDataReady()) { + return; + } + if (heartbeat_hazard_status_->isTimeout()) { + RCLCPP_WARN_THROTTLE( + this->get_logger(), *this->get_clock(), std::chrono::milliseconds(1000).count(), + "heartbeat_hazard_status is timeout"); + emergency_state_ = autoware_auto_system_msgs::msg::EmergencyState::MRM_OPERATING; + publishControlCommands(); + return; + } + + // Update Emergency State + updateEmergencyState(); + + // Publish control commands + publishControlCommands(); +} + +void EmergencyHandler::transitionTo(const int new_state) +{ + using autoware_auto_system_msgs::msg::EmergencyState; + + const auto state2string = [](const int state) { + if (state == EmergencyState::NORMAL) { + return "NORMAL"; + } + if (state == EmergencyState::OVERRIDE_REQUESTING) { + return "OVERRIDE_REQUESTING"; + } + if (state == EmergencyState::MRM_OPERATING) { + return "MRM_OPERATING"; + } + if (state == EmergencyState::MRM_SUCCEEDED) { + return "MRM_SUCCEEDED"; + } + if (state == EmergencyState::MRM_FAILED) { + return "MRM_FAILED"; + } + + const auto msg = "invalid state: " + std::to_string(state); + throw std::runtime_error(msg); + }; + + RCLCPP_INFO( + this->get_logger(), "EmergencyState changed: %s -> %s", state2string(emergency_state_), + state2string(new_state)); + + emergency_state_ = new_state; +} + +void EmergencyHandler::updateEmergencyState() +{ + using autoware_auto_system_msgs::msg::EmergencyState; + using autoware_auto_vehicle_msgs::msg::ControlModeReport; + + // Check emergency + const bool is_emergency = isEmergency(hazard_status_stamped_->status); + + // Get mode + const bool is_auto_mode = control_mode_->mode == ControlModeReport::AUTONOMOUS; + const bool is_takeover_done = control_mode_->mode == ControlModeReport::MANUAL; + + // State Machine + if (emergency_state_ == EmergencyState::NORMAL) { + // NORMAL + if (is_auto_mode && is_emergency) { + if (param_.use_takeover_request) { + takeover_requested_time_ = this->get_clock()->now(); + transitionTo(EmergencyState::OVERRIDE_REQUESTING); + return; + } else { + transitionTo(EmergencyState::MRM_OPERATING); + return; + } + } + } else { + // Emergency + // Send recovery events if "not emergency" or "takeover done" + if (!is_emergency) { + transitionTo(EmergencyState::NORMAL); + return; + } + // TODO(Kenji Miyake): Check if human can safely override, for example using DSM + if (is_takeover_done) { + transitionTo(EmergencyState::NORMAL); + return; + } + + if (emergency_state_ == EmergencyState::OVERRIDE_REQUESTING) { + const auto time_from_takeover_request = this->get_clock()->now() - takeover_requested_time_; + if (time_from_takeover_request.seconds() > param_.timeout_takeover_request) { + transitionTo(EmergencyState::MRM_OPERATING); + return; + } + } else if (emergency_state_ == EmergencyState::MRM_OPERATING) { + // TODO(Kenji Miyake): Check MRC is accomplished + if (isStopped()) { + transitionTo(EmergencyState::MRM_SUCCEEDED); + return; + } + } else if (emergency_state_ == EmergencyState::MRM_SUCCEEDED) { + // Do nothing(only checking common recovery events) + } else if (emergency_state_ == EmergencyState::MRM_FAILED) { + // Do nothing(only checking common recovery events) + } else { + const auto msg = "invalid state: " + std::to_string(emergency_state_); + throw std::runtime_error(msg); + } + } +} + +bool EmergencyHandler::isEmergency( + const autoware_auto_system_msgs::msg::HazardStatus & hazard_status) +{ + return hazard_status.emergency || hazard_status.emergency_holding; +} + +bool EmergencyHandler::isStopped() +{ + constexpr auto th_stopped_velocity = 0.001; + if (odom_->twist.twist.linear.x < th_stopped_velocity) { + return true; + } + + return false; +} + +autoware_auto_control_msgs::msg::AckermannControlCommand +EmergencyHandler::selectAlternativeControlCommand() +{ + // TODO(jilaada): Add safe_stop planner + + // Emergency Stop + { + autoware_auto_control_msgs::msg::AckermannControlCommand emergency_stop_cmd; + emergency_stop_cmd.lateral = prev_control_command_->lateral; + emergency_stop_cmd.longitudinal.speed = 0.0; + emergency_stop_cmd.longitudinal.acceleration = -2.5; + + return emergency_stop_cmd; + } +} diff --git a/system/emergency_handler/src/emergency_handler/emergency_handler_node.cpp b/system/emergency_handler/src/emergency_handler/emergency_handler_node.cpp new file mode 100644 index 0000000000000..a923083cafc15 --- /dev/null +++ b/system/emergency_handler/src/emergency_handler/emergency_handler_node.cpp @@ -0,0 +1,29 @@ +// 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 + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + + return 0; +}