From 35ca12bc164e4a1b75a654e6a21eb3e8e0d1b748 Mon Sep 17 00:00:00 2001 From: TetsuKawa Date: Tue, 13 Feb 2024 22:56:37 +0900 Subject: [PATCH 01/11] feat: add mrm_handler Signed-off-by: TetsuKawa --- system/mrm_handler/CMakeLists.txt | 15 + system/mrm_handler/README.md | 44 ++ .../mrm_handler/config/mrm_handler.param.yaml | 17 + system/mrm_handler/image/mrm-state.svg | 362 +++++++++++ .../include/mrm_handler/mrm_handler_core.hpp | 160 +++++ .../mrm_handler/launch/mrm_handler.launch.xml | 39 ++ system/mrm_handler/package.xml | 31 + .../schema/mrm_handler.schema.json | 95 +++ .../src/mrm_handler/mrm_handler_core.cpp | 591 ++++++++++++++++++ .../src/mrm_handler/mrm_handler_node.cpp | 32 + 10 files changed, 1386 insertions(+) create mode 100644 system/mrm_handler/CMakeLists.txt create mode 100644 system/mrm_handler/README.md create mode 100644 system/mrm_handler/config/mrm_handler.param.yaml create mode 100644 system/mrm_handler/image/mrm-state.svg create mode 100644 system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp create mode 100644 system/mrm_handler/launch/mrm_handler.launch.xml create mode 100644 system/mrm_handler/package.xml create mode 100644 system/mrm_handler/schema/mrm_handler.schema.json create mode 100644 system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp create mode 100644 system/mrm_handler/src/mrm_handler/mrm_handler_node.cpp diff --git a/system/mrm_handler/CMakeLists.txt b/system/mrm_handler/CMakeLists.txt new file mode 100644 index 0000000000000..55e57c125b052 --- /dev/null +++ b/system/mrm_handler/CMakeLists.txt @@ -0,0 +1,15 @@ +cmake_minimum_required(VERSION 3.14) +project(mrm_handler) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_executable(mrm_handler + src/mrm_handler/mrm_handler_node.cpp + src/mrm_handler/mrm_handler_core.cpp +) + +ament_auto_package(INSTALL_TO_SHARE + launch + config +) \ No newline at end of file diff --git a/system/mrm_handler/README.md b/system/mrm_handler/README.md new file mode 100644 index 0000000000000..ef194e1a02a14 --- /dev/null +++ b/system/mrm_handler/README.md @@ -0,0 +1,44 @@ +# mrm_handler + +## Purpose + +MRM Handler is a node to select proper MRM from from system failure state contained in OperationModeAvailability. + +## Inner-workings / Algorithms + +### State Transitions + +![mrm-state](image/mrm-state.svg) + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| -------------------------------------- | ---------------------------------------------------- | --------------------------------------------------------------------------------------------------- | +| `/localization/kinematic_state` | `nav_msgs::msg::Odometry` | Used to decide whether vehicle is stopped or not | +| `/system/operation_mode/availability` | `autoware_adapi_v1_msgs::msg::OperationModeState` | Used to select proper MRM from system available mrm behavior contained in operationModeAvailability | +| `/vehicle/status/control_mode` | `autoware_auto_vehicle_msgs::msg::ControlModeReport` | Used to check vehicle mode: autonomous or manual | +| `/system/mrm/emergency_stop/status` | `tier4_system_msgs::msg::MrmBehaviorStatus` | Used to check if MRM emergency stop operation is available | +| `/system/mrm/comfortable_stop/status` | `tier4_system_msgs::msg::MrmBehaviorStatus` | Used to check if MRM comfortable stop operation is available | +| `/system/mrm/pull_over_manager/status` | `tier4_system_msgs::msg::MrmBehaviorStatus` | Used to check if MRM pull over operation is available | +| `/api/mrm/emergency_stop/status` | `tier4_system_msgs::msg::MrmBehaviorStatus` | Used to check if MRM pull over operation is finished | + +### Output + +| Name | Type | Description | +| --------------------------------------- | ------------------------------------------------------ | ----------------------------------------------------- | +| `/system/emergency/gear_cmd` | `autoware_auto_vehicle_msgs::msg::GearCommand` | Required to execute proper MRM (send gear cmd) | +| `/system/emergency/hazard_lights_cmd` | `autoware_auto_vehicle_msgs::msg::HazardLightsCommand` | Required to execute proper MRM (send turn signal cmd) | +| `/system/fail_safe/mrm_state` | `autoware_adapi_v1_msgs::msg::MrmState` | Inform MRM execution state and selected MRM behavior | +| `/system/mrm/emergency_stop/operate` | `tier4_system_msgs::srv::OperateMrm` | Execution order for MRM emergency stop | +| `/system/mrm/comfortable_stop/operate` | `tier4_system_msgs::srv::OperateMrm` | Execution order for MRM comfortable stop | +| `/system/mrm/pull_over_manager/operate` | `tier4_system_msgs::srv::OperateMrm` | Execution order for MRM pull over | + +## Parameters + +{{ json_to_markdown("system/mrm_handler/schema/mrm_handler.schema.json") }} + +## Assumptions / Known limits + +TBD. \ No newline at end of file diff --git a/system/mrm_handler/config/mrm_handler.param.yaml b/system/mrm_handler/config/mrm_handler.param.yaml new file mode 100644 index 0000000000000..b0452006118f2 --- /dev/null +++ b/system/mrm_handler/config/mrm_handler.param.yaml @@ -0,0 +1,17 @@ +# Default configuration for mrm handler +--- +/**: + ros__parameters: + update_rate: 10 + timeout_operation_mode_availability: 0.5 + use_emergency_holding: false + timeout_emergency_recovery: 5.0 + timeout_takeover_request: 10.0 + use_takeover_request: false + use_parking_after_stopped: false + use_pull_over: false + use_comfortable_stop: false + + # setting whether to turn hazard lamp on for each situation + turning_hazard_on: + emergency: true \ No newline at end of file diff --git a/system/mrm_handler/image/mrm-state.svg b/system/mrm_handler/image/mrm-state.svg new file mode 100644 index 0000000000000..13a2956b6f15c --- /dev/null +++ b/system/mrm_handler/image/mrm-state.svg @@ -0,0 +1,362 @@ + + + + + + + + + + + +
+
+
+ not emergency +
+
+
+ + not emergency + + + + + + + + + + + +
+
+
+ emergency +
+
+
+ + emergency + + + + + + +
+
+
+ NORMAL +
+
+
+ + NORMAL + + + + + + + +
+
+
+ succeeded +
+
+
+ + succeeded + + + + + + + +
+
+
+ failed +
+
+
+ + failed + + + + + + + + +
+
+
+ MRM_OPERATING +
+
+
+ + MRM_OPERATING + + + + + + + +
+
+
+ pull over unavailable +
+ comfortable_stop available (ca) +
+
+
+ + pull over unavailable... + + + + + + + +
+
+
+ pull over available (pa) +
+
+
+ + pull over available (pa) + + + + + + + +
+
+
+ pull over unavailable +
+ comfortable_stop unavailable +
+ emergency_stop available (ea) +
+
+
+ + pull over unavailable... + + + + + + + + + + + +
+
+
+ pull_over +
+
+
+ + pull_over + + + + + + + +
+
+
+ ea +
+
+
+ + ea + + + + + +
+
+
+ ea +
+
+
+ + ea + + + + + + +
+
+
+ emergency_stop +
+
+
+ + emergency_stop + + + + + + + +
+
+
+ ca +
+
+
+ + ca + + + + + + +
+
+
+ comfortable_stop +
+
+
+ + comfortable_stop + + + + + + + + + +
+
+
+ pa +
+
+
+ + pa + + + + + + +
+
+
+ MRM_SUCCEEDED +
+
+
+ + MRM_SUCCEEDED + + + + + + +
+
+
+ MRM_FAILED +
+
+
+ + MRM_FAILED + + + + + + + Text is not SVG - cannot display + + + diff --git a/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp b/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp new file mode 100644 index 0000000000000..27dc43deb1d6b --- /dev/null +++ b/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp @@ -0,0 +1,160 @@ +// 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 MRM_HANDLER__MRM_HANDLER_CORE_HPP_ +#define MRM_HANDLER__MRM_HANDLER_CORE_HPP_ + +// Core +#include +#include +#include +#include + +// Autoware +#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_operation_mode_availability; + bool use_emergency_holding; + double timeout_emergency_recovery; + double timeout_takeover_request; + bool use_takeover_request; + bool use_parking_after_stopped; + bool use_pull_over; + bool use_comfortable_stop; + HazardLampPolicy turning_hazard_on{}; +}; + +class MrmHandler : public rclcpp::Node +{ +public: + MrmHandler(); + +private: + // Subscribers + rclcpp::Subscription::SharedPtr sub_odom_; + rclcpp::Subscription::SharedPtr + sub_control_mode_; + rclcpp::Subscription::SharedPtr + sub_operation_mode_availability_; + rclcpp::Subscription::SharedPtr + sub_mrm_pull_over_status_; + rclcpp::Subscription::SharedPtr + sub_mrm_comfortable_stop_status_; + rclcpp::Subscription::SharedPtr + sub_mrm_emergency_stop_status_; + rclcpp::Subscription::SharedPtr + sub_operation_mode_state_; + + nav_msgs::msg::Odometry::ConstSharedPtr odom_; + autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr control_mode_; + tier4_system_msgs::msg::OperationModeAvailability::ConstSharedPtr operation_mode_availability_; + tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr mrm_pull_over_status_; + tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr mrm_comfortable_stop_status_; + tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr mrm_emergency_stop_status_; + autoware_adapi_v1_msgs::msg::OperationModeState::ConstSharedPtr operation_mode_state_; + + void onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg); + void onControlMode(const autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg); + void onOperationModeAvailability( + const tier4_system_msgs::msg::OperationModeAvailability::ConstSharedPtr msg); + void onMrmPullOverStatus(const tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr msg); + void onMrmComfortableStopStatus( + const tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr msg); + void onMrmEmergencyStopStatus( + const tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr msg); + void onOperationModeState( + const autoware_adapi_v1_msgs::msg::OperationModeState::ConstSharedPtr msg); + + // Publisher + + // rclcpp::Publisher::SharedPtr pub_shift_; + // rclcpp::Publisher::SharedPtr pub_turn_signal_; + rclcpp::Publisher::SharedPtr + pub_hazard_cmd_; + rclcpp::Publisher::SharedPtr pub_gear_cmd_; + + autoware_auto_vehicle_msgs::msg::HazardLightsCommand createHazardCmdMsg(); + autoware_auto_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_pull_over_group_; + rclcpp::Client::SharedPtr client_mrm_pull_over_; + 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_operation_mode_availability_; + std::optional stamp_autonomous_become_unavailable_ = std::nullopt; + + // Algorithm + rclcpp::Time takeover_requested_time_; + bool is_takeover_request_ = false; + bool is_emergency_holding_ = false; + void transitionTo(const int new_state); + void updateMrmState(); + void operateMrm(); + autoware_adapi_v1_msgs::msg::MrmState::_behavior_type getCurrentMrmBehavior(); + bool isStopped(); + bool isEmergency() const; + bool isArrivedAtGoal(); +}; + +#endif // MRM_HANDLER__MRM_HANDLER_CORE_HPP_ \ No newline at end of file diff --git a/system/mrm_handler/launch/mrm_handler.launch.xml b/system/mrm_handler/launch/mrm_handler.launch.xml new file mode 100644 index 0000000000000..60fa302347452 --- /dev/null +++ b/system/mrm_handler/launch/mrm_handler.launch.xml @@ -0,0 +1,39 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/system/mrm_handler/package.xml b/system/mrm_handler/package.xml new file mode 100644 index 0000000000000..8dd2cdd980fee --- /dev/null +++ b/system/mrm_handler/package.xml @@ -0,0 +1,31 @@ + + + + mrm_handler + 0.1.0 + The mrm_handler ROS 2 package + Makoto Kurihara + Ryuta Kambe + Tetsuhiro Kawaguchi + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + autoware_adapi_v1_msgs + autoware_auto_control_msgs + autoware_auto_system_msgs + autoware_auto_vehicle_msgs + nav_msgs + rclcpp + std_msgs + std_srvs + tier4_system_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + \ No newline at end of file diff --git a/system/mrm_handler/schema/mrm_handler.schema.json b/system/mrm_handler/schema/mrm_handler.schema.json new file mode 100644 index 0000000000000..ccaf718c6dfe6 --- /dev/null +++ b/system/mrm_handler/schema/mrm_handler.schema.json @@ -0,0 +1,95 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for mrm handler", + "type": "object", + "definitions": { + "mrm_handler": { + "type": "object", + "properties": { + "update_rate": { + "type": "integer", + "description": "Timer callback period.", + "default": 10 + }, + "timeout_operation_mode_availability": { + "type": "number", + "description": "If the input `operation_mode_availability` topic cannot be received for more than `timeout_operation_mode_availability`, vehicle will make an emergency stop.", + "default": 0.5 + }, + "use_emergency_holding": { + "type": "boolean", + "description": "If this parameter is true, the handler does not recover to the NORMAL state.", + "default": false + }, + "timeout_emergency_recovery": { + "type": "number", + "description": "If the duration of the EMERGENCY state is longer than `timeout_emergency_recovery`, it does not recover to the NORMAL state.", + "default": 5.0 + }, + "timeout_takeover_request": { + "type": "number", + "description": "Transition to MRR_OPERATING if the time from the last takeover request exceeds `timeout_takeover_request`.", + "default": 10.0 + }, + "use_takeover_request": { + "type": "boolean", + "description": "If this parameter is true, the handler will record the time and make take over request to the driver when emergency state occurs.", + "default": "false" + }, + "use_parking_after_stopped": { + "type": "boolean", + "description": "If this parameter is true, it will publish PARKING shift command.", + "default": "false" + }, + "use_pull_over": { + "type": "boolean", + "description": "If this parameter is true, operate pull over when latent faults occur.", + "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_timeout_operation_mode_availability", + "use_emergency_holding", + "timeout_emergency_recovery", + "timeout_takeover_request", + "use_takeover_request", + "use_parking_after_stopped", + "use_pull_over", + "use_comfortable_stop", + "turning_hazard_on" + ], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/mrm_handler" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} \ No newline at end of file diff --git a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp new file mode 100644 index 0000000000000..f72c7c5936379 --- /dev/null +++ b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp @@ -0,0 +1,591 @@ +// Copyright 2024 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 "mrm_handler/mrm_handler_core.hpp" + +#include +#include +#include +#include + +MrmHandler::MrmHandler() : Node("mrm_handler") +{ + // Parameter + param_.update_rate = declare_parameter("update_rate", 10); + param_.timeout_operation_mode_availability = + declare_parameter("timeout_operation_mode_availability", 0.5); + param_.use_emergency_holding = declare_parameter("use_emergency_holding", false); + param_.timeout_emergency_recovery = declare_parameter("timeout_emergency_recovery", 5.0); + 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_.use_pull_over = declare_parameter("use_pull_over", false); + param_.use_comfortable_stop = declare_parameter("use_comfortable_stop", false); + param_.turning_hazard_on.emergency = declare_parameter("turning_hazard_on.emergency", true); + + using std::placeholders::_1; + + // Subscriber + sub_odom_ = create_subscription( + "~/input/odometry", rclcpp::QoS{1}, std::bind(&MrmHandler::onOdometry, this, _1)); + // subscribe control mode + sub_control_mode_ = create_subscription( + "~/input/control_mode", rclcpp::QoS{1}, std::bind(&MrmHandler::onControlMode, this, _1)); + sub_operation_mode_availability_ = + create_subscription( + "~/input/operation_mode_availability", rclcpp::QoS{1}, + std::bind(&MrmHandler::onOperationModeAvailability, this, _1)); + sub_mrm_pull_over_status_ = create_subscription( + "~/input/mrm/pull_over/status", rclcpp::QoS{1}, + std::bind(&MrmHandler::onMrmPullOverStatus, this, _1)); + sub_mrm_comfortable_stop_status_ = create_subscription( + "~/input/mrm/comfortable_stop/status", rclcpp::QoS{1}, + std::bind(&MrmHandler::onMrmComfortableStopStatus, this, _1)); + sub_mrm_emergency_stop_status_ = create_subscription( + "~/input/mrm/emergency_stop/status", rclcpp::QoS{1}, + std::bind(&MrmHandler::onMrmEmergencyStopStatus, this, _1)); + sub_operation_mode_state_ = create_subscription( + "~/input/api/operation_mode/state", rclcpp::QoS{1}, + std::bind(&MrmHandler::onOperationModeState, this, _1)); + + // Publisher + 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_pull_over_group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + client_mrm_pull_over_ = create_client( + "~/output/mrm/pull_over/operate", rmw_qos_profile_services_default, + client_mrm_pull_over_group_); + 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 + odom_ = std::make_shared(); + control_mode_ = std::make_shared(); + mrm_pull_over_status_ = std::make_shared(); + mrm_comfortable_stop_status_ = + std::make_shared(); + mrm_emergency_stop_status_ = std::make_shared(); + operation_mode_state_ = std::make_shared(); + 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; + + // Timer + const auto update_period_ns = rclcpp::Rate(param_.update_rate).period(); + timer_ = rclcpp::create_timer( + this, get_clock(), update_period_ns, std::bind(&MrmHandler::onTimer, this)); +} + +void MrmHandler::onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg) +{ + odom_ = msg; +} + +void MrmHandler::onControlMode( + const autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg) +{ + control_mode_ = msg; +} + +void MrmHandler::onOperationModeAvailability( + const tier4_system_msgs::msg::OperationModeAvailability::ConstSharedPtr msg) +{ + stamp_operation_mode_availability_ = this->now(); + + if (!param_.use_emergency_holding) { + operation_mode_availability_ = msg; + return; + } + + if (!is_emergency_holding_) { + if (msg->autonomous) { + stamp_autonomous_become_unavailable_.reset(); + } else if (!msg->autonomous) { + if (!stamp_autonomous_become_unavailable_.has_value()) { + stamp_autonomous_become_unavailable_.emplace(this->now()); + } else { + const auto emergency_duration = + (this->now() - stamp_autonomous_become_unavailable_.value()).seconds(); + if (emergency_duration > param_.timeout_emergency_recovery) { + is_emergency_holding_ = true; + } + } + } + } + operation_mode_availability_ = msg; +} + +void MrmHandler::onMrmPullOverStatus( + const tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr msg) +{ + mrm_pull_over_status_ = msg; +} + +void MrmHandler::onMrmComfortableStopStatus( + const tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr msg) +{ + mrm_comfortable_stop_status_ = msg; +} + +void MrmHandler::onMrmEmergencyStopStatus( + const tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr msg) +{ + mrm_emergency_stop_status_ = msg; +} + +void MrmHandler::onOperationModeState( + const autoware_adapi_v1_msgs::msg::OperationModeState::ConstSharedPtr msg) +{ + operation_mode_state_ = msg; +} + +autoware_auto_vehicle_msgs::msg::HazardLightsCommand MrmHandler::createHazardCmdMsg() +{ + using autoware_auto_vehicle_msgs::msg::HazardLightsCommand; + HazardLightsCommand msg; + + // Check emergency + const bool is_emergency = isEmergency(); + + if (is_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 MrmHandler::publishControlCommands() +{ + using autoware_auto_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; + if (param_.use_parking_after_stopped && isStopped()) { + msg.command = GearCommand::PARK; + } else { + msg.command = GearCommand::DRIVE; + } + pub_gear_cmd_->publish(msg); + } +} + +void MrmHandler::publishMrmState() +{ + mrm_state_.stamp = this->now(); + pub_mrm_state_->publish(mrm_state_); +} + +void MrmHandler::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 MrmHandler::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::PULL_OVER) { + auto result = client_mrm_pull_over_->async_send_request(request).get(); + if (result->response.success == true) { + RCLCPP_WARN(this->get_logger(), "Pull over is operated"); + } else { + RCLCPP_ERROR(this->get_logger(), "Pull over is failed to operate"); + } + 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 MrmHandler::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::PULL_OVER) { + auto result = client_mrm_pull_over_->async_send_request(request).get(); + if (result->response.success == true) { + RCLCPP_WARN(this->get_logger(), "Pull over is canceled"); + } else { + RCLCPP_ERROR(this->get_logger(), "Pull over is failed to cancel"); + } + 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 MrmHandler::isDataReady() +{ + if (!operation_mode_availability_) { + RCLCPP_INFO_THROTTLE( + this->get_logger(), *this->get_clock(), std::chrono::milliseconds(5000).count(), + "waiting for operation_mode_availability msg..."); + return false; + } + + if ( + param_.use_pull_over && + mrm_pull_over_status_->state == tier4_system_msgs::msg::MrmBehaviorStatus::NOT_AVAILABLE) { + RCLCPP_INFO_THROTTLE( + this->get_logger(), *this->get_clock(), std::chrono::milliseconds(5000).count(), + "waiting for mrm pull over to become available..."); + return false; + } + + if ( + param_.use_comfortable_stop && mrm_comfortable_stop_status_->state == + tier4_system_msgs::msg::MrmBehaviorStatus::NOT_AVAILABLE) { + 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 ( + mrm_emergency_stop_status_->state == tier4_system_msgs::msg::MrmBehaviorStatus::NOT_AVAILABLE) { + 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 MrmHandler::onTimer() +{ + if (!isDataReady()) { + return; + } + const bool is_operation_mode_availability_timeout = + (this->now() - stamp_operation_mode_availability_).seconds() > + param_.timeout_operation_mode_availability; + if (is_operation_mode_availability_timeout) { + RCLCPP_WARN_THROTTLE( + this->get_logger(), *this->get_clock(), std::chrono::milliseconds(1000).count(), + "heartbeat operation_mode_availability is timeout"); + mrm_state_.state = autoware_adapi_v1_msgs::msg::MrmState::MRM_OPERATING; + publishControlCommands(); + return; + } + + // Update Emergency State + updateMrmState(); + + // Publish control commands + publishControlCommands(); + operateMrm(); + publishMrmState(); +} + +void MrmHandler::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_INFO( + this->get_logger(), "MRM State changed: %s -> %s", state2string(mrm_state_.state), + state2string(new_state)); + + mrm_state_.state = new_state; +} + +void MrmHandler::updateMrmState() +{ + using autoware_adapi_v1_msgs::msg::MrmState; + using autoware_auto_vehicle_msgs::msg::ControlModeReport; + + // Check emergency + const bool is_emergency = isEmergency(); + + // 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 (mrm_state_.state == MrmState::NORMAL) { + // NORMAL + if (is_auto_mode && is_emergency) { + if (param_.use_takeover_request) { + takeover_requested_time_ = this->get_clock()->now(); + is_takeover_request_ = true; + return; + } else { + transitionTo(MrmState::MRM_OPERATING); + return; + } + } + } else { + // Emergency + // Send recovery events if "not emergency" or "takeover done" + if (!is_emergency) { + transitionTo(MrmState::NORMAL); + return; + } + // TODO(Kenji Miyake): Check if human can safely override, for example using DSM + if (is_takeover_done) { + transitionTo(MrmState::NORMAL); + return; + } + + if (is_takeover_request_) { + const auto time_from_takeover_request = this->get_clock()->now() - takeover_requested_time_; + if (time_from_takeover_request.seconds() > param_.timeout_takeover_request) { + transitionTo(MrmState::MRM_OPERATING); + return; + } + } else if (mrm_state_.state == MrmState::MRM_OPERATING) { + // TODO(Kenji Miyake): Check MRC is accomplished + if (mrm_state_.behavior == MrmState::PULL_OVER) { + if (isStopped() && isArrivedAtGoal()) { + transitionTo(MrmState::MRM_SUCCEEDED); + return; + } + } else { + if (isStopped()) { + transitionTo(MrmState::MRM_SUCCEEDED); + return; + } + } + } else if (mrm_state_.state == MrmState::MRM_SUCCEEDED) { + const auto current_mrm_behavior = getCurrentMrmBehavior(); + if (current_mrm_behavior != mrm_state_.behavior) { + transitionTo(MrmState::MRM_OPERATING); + } + } 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 MrmHandler::getCurrentMrmBehavior() +{ + using autoware_adapi_v1_msgs::msg::MrmState; + using tier4_system_msgs::msg::OperationModeAvailability; + + // State machine + if (mrm_state_.behavior == MrmState::NONE) { + if (operation_mode_availability_->pull_over) { + if (param_.use_pull_over) { + return MrmState::PULL_OVER; + } + } + if (operation_mode_availability_->comfortable_stop) { + if (param_.use_comfortable_stop) { + return MrmState::COMFORTABLE_STOP; + } + } + if (!operation_mode_availability_->emergency_stop) { + RCLCPP_WARN(this->get_logger(), "no mrm operation available: operate emergency_stop"); + } + return MrmState::EMERGENCY_STOP; + } + if (mrm_state_.behavior == MrmState::PULL_OVER) { + if (operation_mode_availability_->pull_over) { + if (param_.use_pull_over) { + return MrmState::PULL_OVER; + } + } + if (operation_mode_availability_->comfortable_stop) { + if (param_.use_comfortable_stop) { + return MrmState::COMFORTABLE_STOP; + } + } + if (!operation_mode_availability_->emergency_stop) { + RCLCPP_WARN(this->get_logger(), "no mrm operation available: operate emergency_stop"); + } + return MrmState::EMERGENCY_STOP; + } + if (mrm_state_.behavior == MrmState::COMFORTABLE_STOP) { + if (isStopped() && operation_mode_availability_->pull_over) { + if (param_.use_pull_over) { + return MrmState::PULL_OVER; + } + } + if (operation_mode_availability_->comfortable_stop) { + if (param_.use_comfortable_stop) { + return MrmState::COMFORTABLE_STOP; + } + } + if (!operation_mode_availability_->emergency_stop) { + RCLCPP_WARN(this->get_logger(), "no mrm operation available: operate emergency_stop"); + } + return MrmState::EMERGENCY_STOP; + } + if (mrm_state_.behavior == MrmState::EMERGENCY_STOP) { + if (isStopped() && operation_mode_availability_->pull_over) { + if (param_.use_pull_over) { + return MrmState::PULL_OVER; + } + } + if (!operation_mode_availability_->emergency_stop) { + RCLCPP_WARN(this->get_logger(), "no mrm operation available: operate emergency_stop"); + } + return MrmState::EMERGENCY_STOP; + } + + return mrm_state_.behavior; +} + +bool MrmHandler::isStopped() +{ + constexpr auto th_stopped_velocity = 0.001; + if (odom_->twist.twist.linear.x < th_stopped_velocity) { + return true; + } + + return false; +} + +bool MrmHandler::isEmergency() const +{ + return !operation_mode_availability_->autonomous || is_emergency_holding_; +} + +bool MrmHandler::isArrivedAtGoal() +{ + using autoware_adapi_v1_msgs::msg::OperationModeState; + + if (operation_mode_state_->mode == OperationModeState::STOP) { + return true; + } + + return false; +} \ No newline at end of file diff --git a/system/mrm_handler/src/mrm_handler/mrm_handler_node.cpp b/system/mrm_handler/src/mrm_handler/mrm_handler_node.cpp new file mode 100644 index 0000000000000..738e2d1e909fb --- /dev/null +++ b/system/mrm_handler/src/mrm_handler/mrm_handler_node.cpp @@ -0,0 +1,32 @@ +// Copyright 2024 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 "mrm_handler/mrm_handler_core.hpp" + +#include + +#include + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + rclcpp::executors::MultiThreadedExecutor executor; + auto node = std::make_shared(); + executor.add_node(node); + executor.spin(); + executor.remove_node(node); + rclcpp::shutdown(); + + return 0; +} \ No newline at end of file From a9a5e1e92ac055169dea3b20b952cd11e4e11f80 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 14 Feb 2024 03:11:12 +0000 Subject: [PATCH 02/11] style(pre-commit): autofix Signed-off-by: TetsuKawa --- system/mrm_handler/CMakeLists.txt | 2 +- system/mrm_handler/README.md | 2 +- system/mrm_handler/config/mrm_handler.param.yaml | 2 +- system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp | 2 +- system/mrm_handler/launch/mrm_handler.launch.xml | 2 +- system/mrm_handler/package.xml | 2 +- system/mrm_handler/schema/mrm_handler.schema.json | 2 +- system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp | 4 ++-- system/mrm_handler/src/mrm_handler/mrm_handler_node.cpp | 2 +- 9 files changed, 10 insertions(+), 10 deletions(-) diff --git a/system/mrm_handler/CMakeLists.txt b/system/mrm_handler/CMakeLists.txt index 55e57c125b052..7c2174f03a6f3 100644 --- a/system/mrm_handler/CMakeLists.txt +++ b/system/mrm_handler/CMakeLists.txt @@ -12,4 +12,4 @@ ament_auto_add_executable(mrm_handler ament_auto_package(INSTALL_TO_SHARE launch config -) \ No newline at end of file +) diff --git a/system/mrm_handler/README.md b/system/mrm_handler/README.md index ef194e1a02a14..811eb8046a06a 100644 --- a/system/mrm_handler/README.md +++ b/system/mrm_handler/README.md @@ -41,4 +41,4 @@ MRM Handler is a node to select proper MRM from from system failure state contai ## Assumptions / Known limits -TBD. \ No newline at end of file +TBD. diff --git a/system/mrm_handler/config/mrm_handler.param.yaml b/system/mrm_handler/config/mrm_handler.param.yaml index b0452006118f2..292d459f69d88 100644 --- a/system/mrm_handler/config/mrm_handler.param.yaml +++ b/system/mrm_handler/config/mrm_handler.param.yaml @@ -14,4 +14,4 @@ # setting whether to turn hazard lamp on for each situation turning_hazard_on: - emergency: true \ No newline at end of file + emergency: true 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 27dc43deb1d6b..26c5657815c2d 100644 --- a/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp +++ b/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp @@ -157,4 +157,4 @@ class MrmHandler : public rclcpp::Node bool isArrivedAtGoal(); }; -#endif // MRM_HANDLER__MRM_HANDLER_CORE_HPP_ \ No newline at end of file +#endif // MRM_HANDLER__MRM_HANDLER_CORE_HPP_ diff --git a/system/mrm_handler/launch/mrm_handler.launch.xml b/system/mrm_handler/launch/mrm_handler.launch.xml index 60fa302347452..562134f5301e2 100644 --- a/system/mrm_handler/launch/mrm_handler.launch.xml +++ b/system/mrm_handler/launch/mrm_handler.launch.xml @@ -36,4 +36,4 @@ - \ No newline at end of file + diff --git a/system/mrm_handler/package.xml b/system/mrm_handler/package.xml index 8dd2cdd980fee..e62091f2984e6 100644 --- a/system/mrm_handler/package.xml +++ b/system/mrm_handler/package.xml @@ -28,4 +28,4 @@ ament_cmake - \ No newline at end of file + diff --git a/system/mrm_handler/schema/mrm_handler.schema.json b/system/mrm_handler/schema/mrm_handler.schema.json index ccaf718c6dfe6..f371363cb0aa5 100644 --- a/system/mrm_handler/schema/mrm_handler.schema.json +++ b/system/mrm_handler/schema/mrm_handler.schema.json @@ -92,4 +92,4 @@ }, "required": ["/**"], "additionalProperties": false -} \ No newline at end of file +} 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 f72c7c5936379..e1008dfcec9f0 100644 --- a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp +++ b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp @@ -190,7 +190,7 @@ void MrmHandler::publishControlCommands() // Create timestamp const auto stamp = this->now(); - + // Publish hazard command pub_hazard_cmd_->publish(createHazardCmdMsg()); @@ -588,4 +588,4 @@ bool MrmHandler::isArrivedAtGoal() } return false; -} \ No newline at end of file +} diff --git a/system/mrm_handler/src/mrm_handler/mrm_handler_node.cpp b/system/mrm_handler/src/mrm_handler/mrm_handler_node.cpp index 738e2d1e909fb..e82c46915d074 100644 --- a/system/mrm_handler/src/mrm_handler/mrm_handler_node.cpp +++ b/system/mrm_handler/src/mrm_handler/mrm_handler_node.cpp @@ -29,4 +29,4 @@ int main(int argc, char ** argv) rclcpp::shutdown(); return 0; -} \ No newline at end of file +} From 4b5e0f3dc62b587ee8d1d1fa65a863758ac98e09 Mon Sep 17 00:00:00 2001 From: TetsuKawa <70682030+TetsuKawa@users.noreply.github.com> Date: Wed, 14 Feb 2024 16:49:15 +0900 Subject: [PATCH 03/11] modify: update README Signed-off-by: TetsuKawa --- system/mrm_handler/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/system/mrm_handler/README.md b/system/mrm_handler/README.md index 811eb8046a06a..d1e251ab7d8e2 100644 --- a/system/mrm_handler/README.md +++ b/system/mrm_handler/README.md @@ -2,7 +2,7 @@ ## Purpose -MRM Handler is a node to select proper MRM from from system failure state contained in OperationModeAvailability. +MRM Handler is a node to select a proper MRM from a system failure state contained in OperationModeAvailability. ## Inner-workings / Algorithms From 708dea035202c4ce8522190858e118ce5bb11c1e Mon Sep 17 00:00:00 2001 From: TetsuKawa <70682030+TetsuKawa@users.noreply.github.com> Date: Wed, 14 Feb 2024 16:52:40 +0900 Subject: [PATCH 04/11] feat: refactor isArrivedAtGoal() Signed-off-by: TetsuKawa --- system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) 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 e1008dfcec9f0..de25c67b3d0cb 100644 --- a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp +++ b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp @@ -583,9 +583,5 @@ bool MrmHandler::isArrivedAtGoal() { using autoware_adapi_v1_msgs::msg::OperationModeState; - if (operation_mode_state_->mode == OperationModeState::STOP) { - return true; - } - - return false; + return operation_mode_state_->mode == OperationModeState::STOP; } From 74de6dc7704ce61dbd19fd65c68c6655cdb9a2e5 Mon Sep 17 00:00:00 2001 From: TetsuKawa <70682030+TetsuKawa@users.noreply.github.com> Date: Mon, 19 Feb 2024 15:57:57 +0900 Subject: [PATCH 05/11] modify: fix error massages gramatically. Signed-off-by: TetsuKawa --- .../mrm_handler/src/mrm_handler/mrm_handler_core.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) 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 de25c67b3d0cb..01fccf87dd7d8 100644 --- a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp +++ b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp @@ -264,7 +264,7 @@ void MrmHandler::callMrmBehavior( if (result->response.success == true) { RCLCPP_WARN(this->get_logger(), "Pull over is operated"); } else { - RCLCPP_ERROR(this->get_logger(), "Pull over is failed to operate"); + RCLCPP_ERROR(this->get_logger(), "Pull over failed to operate"); } return; } @@ -273,7 +273,7 @@ void MrmHandler::callMrmBehavior( 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"); + RCLCPP_ERROR(this->get_logger(), "Comfortable stop failed to operate"); } return; } @@ -282,7 +282,7 @@ void MrmHandler::callMrmBehavior( 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"); + RCLCPP_ERROR(this->get_logger(), "Emergency stop failed to operate"); } return; } @@ -306,7 +306,7 @@ void MrmHandler::cancelMrmBehavior( if (result->response.success == true) { RCLCPP_WARN(this->get_logger(), "Pull over is canceled"); } else { - RCLCPP_ERROR(this->get_logger(), "Pull over is failed to cancel"); + RCLCPP_ERROR(this->get_logger(), "Pull over failed to cancel"); } return; } @@ -315,7 +315,7 @@ void MrmHandler::cancelMrmBehavior( 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"); + RCLCPP_ERROR(this->get_logger(), "Comfortable stop failed to cancel"); } return; } @@ -324,7 +324,7 @@ void MrmHandler::cancelMrmBehavior( 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"); + RCLCPP_ERROR(this->get_logger(), "Emergency stop failed to cancel"); } return; } From efaaafe1a7cfff2685854c9ade340a2c8f317004 Mon Sep 17 00:00:00 2001 From: TetsuKawa <70682030+TetsuKawa@users.noreply.github.com> Date: Mon, 19 Feb 2024 16:18:26 +0900 Subject: [PATCH 06/11] feat: update the person in charge of the unimplemented parts Signed-off-by: TetsuKawa --- system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 01fccf87dd7d8..539c3ada54f0a 100644 --- a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp +++ b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp @@ -456,7 +456,7 @@ void MrmHandler::updateMrmState() transitionTo(MrmState::NORMAL); return; } - // TODO(Kenji Miyake): Check if human can safely override, for example using DSM + // TODO(TetsuKawa): Check if human can safely override, for example using DSM if (is_takeover_done) { transitionTo(MrmState::NORMAL); return; @@ -469,7 +469,7 @@ void MrmHandler::updateMrmState() return; } } else if (mrm_state_.state == MrmState::MRM_OPERATING) { - // TODO(Kenji Miyake): Check MRC is accomplished + // TODO(TetsuKawa): Check MRC is accomplished if (mrm_state_.behavior == MrmState::PULL_OVER) { if (isStopped() && isArrivedAtGoal()) { transitionTo(MrmState::MRM_SUCCEEDED); From b71d0bf08441b49a6ffc52146db08487be656ed9 Mon Sep 17 00:00:00 2001 From: TetsuKawa <70682030+TetsuKawa@users.noreply.github.com> Date: Wed, 21 Feb 2024 17:08:34 +0900 Subject: [PATCH 07/11] modify: fix typo in schema.json Signed-off-by: TetsuKawa --- system/mrm_handler/schema/mrm_handler.schema.json | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/system/mrm_handler/schema/mrm_handler.schema.json b/system/mrm_handler/schema/mrm_handler.schema.json index f371363cb0aa5..86b18449cb734 100644 --- a/system/mrm_handler/schema/mrm_handler.schema.json +++ b/system/mrm_handler/schema/mrm_handler.schema.json @@ -65,7 +65,7 @@ }, "required": [ "update_rate", - "timeout_timeout_operation_mode_availability", + "timeout_operation_mode_availability", "use_emergency_holding", "timeout_emergency_recovery", "timeout_takeover_request", From 217e87f7f2ea761d859fc7aa758a0def86a51c99 Mon Sep 17 00:00:00 2001 From: TetsuKawa Date: Tue, 27 Feb 2024 11:10:18 +0900 Subject: [PATCH 08/11] modify: fix copyright Signed-off-by: TetsuKawa --- system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp | 2 +- system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp | 2 +- system/mrm_handler/src/mrm_handler/mrm_handler_node.cpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) 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 26c5657815c2d..8a6ec81cbafff 100644 --- a/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp +++ b/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. +// Copyright 2024 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. 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 539c3ada54f0a..976f5b3164abd 100644 --- a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp +++ b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp @@ -1,4 +1,4 @@ -// Copyright 2024 Tier IV, Inc. +// Copyright 2024 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. diff --git a/system/mrm_handler/src/mrm_handler/mrm_handler_node.cpp b/system/mrm_handler/src/mrm_handler/mrm_handler_node.cpp index e82c46915d074..4aaab3296f64b 100644 --- a/system/mrm_handler/src/mrm_handler/mrm_handler_node.cpp +++ b/system/mrm_handler/src/mrm_handler/mrm_handler_node.cpp @@ -1,4 +1,4 @@ -// Copyright 2024 Tier IV, Inc. +// Copyright 2024 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. From 23eb0a0b1b5eb24a33ffa54c4daa18985fab5ba9 Mon Sep 17 00:00:00 2001 From: TetsuKawa Date: Tue, 27 Feb 2024 11:29:02 +0900 Subject: [PATCH 09/11] modify: fix mistakes in README Signed-off-by: TetsuKawa --- system/mrm_handler/README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/system/mrm_handler/README.md b/system/mrm_handler/README.md index d1e251ab7d8e2..fbfd228d51d75 100644 --- a/system/mrm_handler/README.md +++ b/system/mrm_handler/README.md @@ -17,12 +17,12 @@ MRM Handler is a node to select a proper MRM from a system failure state contain | Name | Type | Description | | -------------------------------------- | ---------------------------------------------------- | --------------------------------------------------------------------------------------------------- | | `/localization/kinematic_state` | `nav_msgs::msg::Odometry` | Used to decide whether vehicle is stopped or not | -| `/system/operation_mode/availability` | `autoware_adapi_v1_msgs::msg::OperationModeState` | Used to select proper MRM from system available mrm behavior contained in operationModeAvailability | +| `/system/operation_mode/availability` | `tier4_system_msgs::msg::OperationModeAvailability` | Used to select proper MRM from system available mrm behavior contained in operationModeAvailability | | `/vehicle/status/control_mode` | `autoware_auto_vehicle_msgs::msg::ControlModeReport` | Used to check vehicle mode: autonomous or manual | | `/system/mrm/emergency_stop/status` | `tier4_system_msgs::msg::MrmBehaviorStatus` | Used to check if MRM emergency stop operation is available | | `/system/mrm/comfortable_stop/status` | `tier4_system_msgs::msg::MrmBehaviorStatus` | Used to check if MRM comfortable stop operation is available | | `/system/mrm/pull_over_manager/status` | `tier4_system_msgs::msg::MrmBehaviorStatus` | Used to check if MRM pull over operation is available | -| `/api/mrm/emergency_stop/status` | `tier4_system_msgs::msg::MrmBehaviorStatus` | Used to check if MRM pull over operation is finished | +| `/api/operation_mode/state` | `tier4_system_msgs::msg::MrmBehaviorStatus` | Used to check whether the current operation mode is AUTO or STOP. | ### Output From 3920a71b0d5d957e1c9f1b9d7266c3cffa56add6 Mon Sep 17 00:00:00 2001 From: TetsuKawa Date: Tue, 27 Feb 2024 13:32:53 +0900 Subject: [PATCH 10/11] modify: correct a type mistake in README Signed-off-by: TetsuKawa --- system/mrm_handler/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/system/mrm_handler/README.md b/system/mrm_handler/README.md index fbfd228d51d75..29f2c1bfb4770 100644 --- a/system/mrm_handler/README.md +++ b/system/mrm_handler/README.md @@ -22,7 +22,7 @@ MRM Handler is a node to select a proper MRM from a system failure state contain | `/system/mrm/emergency_stop/status` | `tier4_system_msgs::msg::MrmBehaviorStatus` | Used to check if MRM emergency stop operation is available | | `/system/mrm/comfortable_stop/status` | `tier4_system_msgs::msg::MrmBehaviorStatus` | Used to check if MRM comfortable stop operation is available | | `/system/mrm/pull_over_manager/status` | `tier4_system_msgs::msg::MrmBehaviorStatus` | Used to check if MRM pull over operation is available | -| `/api/operation_mode/state` | `tier4_system_msgs::msg::MrmBehaviorStatus` | Used to check whether the current operation mode is AUTO or STOP. | +| `/api/operation_mode/state` | `autoware_adapi_v1_msgs::msg::OperationModeState` | Used to check whether the current operation mode is AUTO or STOP. | ### Output From e6eb715efa712fe7f9cc044c5928a4b2709bae00 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 27 Feb 2024 04:34:47 +0000 Subject: [PATCH 11/11] style(pre-commit): autofix Signed-off-by: TetsuKawa --- system/mrm_handler/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/system/mrm_handler/README.md b/system/mrm_handler/README.md index 29f2c1bfb4770..4944d8a833643 100644 --- a/system/mrm_handler/README.md +++ b/system/mrm_handler/README.md @@ -22,7 +22,7 @@ MRM Handler is a node to select a proper MRM from a system failure state contain | `/system/mrm/emergency_stop/status` | `tier4_system_msgs::msg::MrmBehaviorStatus` | Used to check if MRM emergency stop operation is available | | `/system/mrm/comfortable_stop/status` | `tier4_system_msgs::msg::MrmBehaviorStatus` | Used to check if MRM comfortable stop operation is available | | `/system/mrm/pull_over_manager/status` | `tier4_system_msgs::msg::MrmBehaviorStatus` | Used to check if MRM pull over operation is available | -| `/api/operation_mode/state` | `autoware_adapi_v1_msgs::msg::OperationModeState` | Used to check whether the current operation mode is AUTO or STOP. | +| `/api/operation_mode/state` | `autoware_adapi_v1_msgs::msg::OperationModeState` | Used to check whether the current operation mode is AUTO or STOP. | ### Output