From 7a041b17eda2ed40c392f51542bd62c117f43d88 Mon Sep 17 00:00:00 2001 From: Tomoya Kimura Date: Sun, 5 Dec 2021 19:25:13 +0900 Subject: [PATCH] feat: add emergency_handler package (#13) * release v0.4.0 * remove ROS1 packages temporarily Signed-off-by: mitsudome-r * Revert "remove ROS1 packages temporarily" This reverts commit bb6fdb15dd23df7eab978525dd7a1541cf61df4a. Signed-off-by: mitsudome-r * add COLCON_IGNORE to ros1 packages Signed-off-by: mitsudome-r * Rename launch files to launch.xml (#28) * ROS2 Porting: emergency_handler (#40) * Pull emergency handler package from master for porting * Fix CMakeList - Change to format similar to the simple_simulation_planner - Doesn't compile due to src files * Rename of core implementation to be compliant with ROS2 naming guidelines * Use the node/core naming convention - Add _core file to hold the implementation - _node file now holds the executable - Modify CMakeLists to point to the new executable * Conversion of msg types - Compiles now - Add back functions * Convert node intrinsics to ROS2 - Add subscriptions - Add publisher - Add timer subscription - Add publishing and logging * Rearrange files and folders * Fix cmake and package xml * Interface with parameters and fix launch file - Fix configuration files - Correct main implementation - Clean up - Fix headers * Use correct timer implementation * Use throttle logs * Use class method to get time instead of static method * Make parameter file agnostic to node name Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> * fix duration unit for RCLCPP_*_THROTTLE (#75) Signed-off-by: Takamasa Horibe * Adjust copyright notice on 532 out of 699 source files (#143) * Use quotes for includes where appropriate (#144) * Use quotes for includes where appropriate * Fix lint tests * Make tests pass hopefully * Add linters (#206) * Ros2 v0.8.0 emergency handler (#280) * Ros2 v0.8.0 fix packages2 (#354) * fix topic name * fix duration rate * fix sensing.yaml * fix topic name * add latch-option to autoware_state_monitor * fix timer callback * fix autoware state monitor config * fix sensing.yaml * fix emergency handler (#361) * Sync with Ros2 v0.8.0 beta (#393) * add nullptr check when publish concatenate data (#369) * Add warning msg when concat pointcloud is not published (#388) Signed-off-by: Kenji Miyake * add timeout notification stamp msgs (#363) * add timeout notification stamp msgs * fix uncursify * delete timeout notification stamped * Revert "delete timeout notification stamped" This reverts commit 365d29209f6a7f5ec75eb80c5d8c2ef38daeae79. * fix message Co-authored-by: Taichi Higashide Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> * fix state_timeout (#399) Signed-off-by: Kosuke Murakami * fix param name (#412) * add timeout_ignore_state to emergency_handler (#415) * add timeout_ignore_state in emergency_handler * do not ignore is_state_timeout_->is_timeout * sort condition * Ros2 fix topic name part1 (#408) * Fix topic name of lane_departure_checker debug Signed-off-by: Takagi, Isamu * Fix topic name of mpc_follower debug Signed-off-by: Takagi, Isamu * Fix topic name of velocity_controller debug Signed-off-by: Takagi, Isamu * Fix topic name of motion_velocity_optimizer debug Signed-off-by: Takagi, Isamu * Fix topic name of lane_change_planner debug Signed-off-by: Takagi, Isamu * Fix topic name of behavior_velocity_planner debug Signed-off-by: Takagi, Isamu * Fix topic name of obstacle_avoidance_planner debug Signed-off-by: Takagi, Isamu * Fix topic name of behavior_velocity_planner Signed-off-by: Takagi, Isamu * Fix topic name of motion_velocity_optimizer Signed-off-by: Takagi, Isamu * Fix topic name of lane_departure_checker Signed-off-by: Takagi, Isamu * Fix topic name of mpc_follower Signed-off-by: Takagi, Isamu * Fix topic name of behavior_velocity_planner Signed-off-by: Takagi, Isamu * Fix topic name of velocity_controller Signed-off-by: Takagi, Isamu * Fix topic name of lane_change_planner Signed-off-by: Takagi, Isamu * Fix topic name of obstacle_avoidance_planner Signed-off-by: Takagi, Isamu * Fix topic name of obstacle_stop_planner Signed-off-by: Takagi, Isamu * Fix topic name of costmap_generator Signed-off-by: Takagi, Isamu * Fix topic name of freespace_planner Signed-off-by: Takagi, Isamu * Fix topic name of surround_obstacle_checker Signed-off-by: Takagi, Isamu * Fix topic name of costmap_generator Signed-off-by: Takagi, Isamu * Fix topic name of emergency_handler Signed-off-by: Takagi, Isamu * Fix lint errors Signed-off-by: Takagi, Isamu * Fix typo Signed-off-by: Takagi, Isamu * fix launch arg (#426) Signed-off-by: Kosuke Murakami * add use_sim-time option (#454) * Fix for rolling (#1226) * Replace doc by description Signed-off-by: Kenji Miyake * Replace ns by push-ros-namespace Signed-off-by: Kenji Miyake * Remove state_timeout_checker (#1247) Signed-off-by: Kenji Miyake * Remove use_sim_time for set_parameter (#1260) Signed-off-by: wep21 * Prevent emergency hold during manual driving (#1390) * Prevent emergency hold during manual driving Signed-off-by: Kenji Miyake * Update README.md * Fix typo * Fix -Wunused-parameter (#1836) * Fix -Wunused-parameter Signed-off-by: Kenji Miyake * Fix mistake Signed-off-by: Kenji Miyake * fix spell * Fix lint issues Signed-off-by: Kenji Miyake * Ignore flake8 warnings Signed-off-by: Kenji Miyake Co-authored-by: Hiroki OTA * Add autoware api (#1979) * enable autonomous recovery (#1904) * Use EmergencyState instead of deprecated EmergencyMode (#2030) * Use EmergencyState instead of deprecated EmergencyMode Signed-off-by: Kenji Miyake * Use stamped type Signed-off-by: Kenji Miyake * add sort-package-xml hook in pre-commit (#1881) * add sort xml hook in pre-commit * change retval to exit_status * rename * add prettier plugin-xml * use early return * add license note * add tier4 license * restore prettier * change license order * move local hooks to public repo * move prettier-xml to pre-commit-hooks-ros * update version for bug-fix * apply pre-commit * Add takeover-request feature in emergency_handler (#2032) * Add takeover-request feature in emergency_handler Signed-off-by: Kenji Miyake * Rename tor to takeover_request Signed-off-by: Kenji Miyake * Use transitionTo Signed-off-by: Kenji Miyake * Replace image URL Signed-off-by: Kenji Miyake * Add error handling Signed-off-by: Kenji Miyake * Fix state transition Signed-off-by: Kenji Miyake * Fix state transition Signed-off-by: Kenji Miyake * Fix cpplint Signed-off-by: Kenji Miyake * Return after state transition Signed-off-by: Kenji Miyake * Fix state transition Signed-off-by: Kenji Miyake * Add initialization for control mode (#2118) Signed-off-by: wep21 * Change formatter to clang-format and black (#2332) * Revert "Temporarily comment out pre-commit hooks" This reverts commit 748e9cdb145ce12f8b520bcbd97f5ff899fc28a3. * Replace ament_lint_common with autoware_lint_common Signed-off-by: Kenji Miyake * Remove ament_cmake_uncrustify and ament_clang_format Signed-off-by: Kenji Miyake * Apply Black Signed-off-by: Kenji Miyake * Apply clang-format Signed-off-by: Kenji Miyake * Fix build errors Signed-off-by: Kenji Miyake * Fix for cpplint * Fix include double quotes to angle brackets Signed-off-by: Kenji Miyake * Apply clang-format Signed-off-by: Kenji Miyake * Fix build errors Signed-off-by: Kenji Miyake * Add COLCON_IGNORE (#500) Signed-off-by: Kenji Miyake * [emergency_handler]support autoware.auto msg (#513) * support auto msg * add readme * remove VehicleStateReport/VehicleStateCommand/VehicleControlCommand (#549) * fix autoware_error_monitor * fix state monitor * fix emergency handler(vehicle_state_report) * fix emergency Handler(vehicle_state_command) * fix shift_decider * fix emergency_handler(vehicle_control_command) * fix topic name * fix readme * Update system/autoware_state_monitor/Readme.md Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> * fix format * Update system/autoware_state_monitor/launch/autoware_state_monitor.launch.xml Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> * fix typo Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> * [autowere_web_controller/autoware_state_monitor/emergency_handler]fix some packages (#603) * fix vehicle_engage.js * fix autoware_state_monitor param * fix emergency_handler * oh * update autoware_state.js * fix topic name (#679) * Fix/psim topics emergency handler awapi (#702) * fix emergency handler * fix awapi * remove unused topic * remove duplecated vehicle cmd Co-authored-by: mitsudome-r Co-authored-by: Nikolai Morin Co-authored-by: Jilada Eccleston Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> Co-authored-by: Takamasa Horibe Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Co-authored-by: Taichi Higashide Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> Co-authored-by: Kosuke Murakami Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com> Co-authored-by: Hiroki OTA Co-authored-by: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com> Co-authored-by: Takeshi Miura <57553950+1222-takeshi@users.noreply.github.com> --- system/emergency_handler/CMakeLists.txt | 39 +++ system/emergency_handler/README.md | 51 +++ .../config/emergency_handler.param.yaml | 13 + .../emergency_handler_core.hpp | 120 +++++++ .../launch/emergency_handler.launch.xml | 29 ++ system/emergency_handler/package.xml | 26 ++ .../emergency_handler_core.cpp | 318 ++++++++++++++++++ .../emergency_handler_node.cpp | 29 ++ 8 files changed, 625 insertions(+) create mode 100644 system/emergency_handler/CMakeLists.txt create mode 100644 system/emergency_handler/README.md create mode 100644 system/emergency_handler/config/emergency_handler.param.yaml create mode 100644 system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp create mode 100644 system/emergency_handler/launch/emergency_handler.launch.xml create mode 100644 system/emergency_handler/package.xml create mode 100644 system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp create mode 100644 system/emergency_handler/src/emergency_handler/emergency_handler_node.cpp 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; +}