From e36e40f038ad317852026fad3b0a4c758112f599 Mon Sep 17 00:00:00 2001 From: TetsuKawa <kawaguchitnon@icloud.com> Date: Thu, 14 Dec 2023 14:10:22 +0900 Subject: [PATCH] feat: change state machine and add isArrivedAtGoal --- .../emergency_handler_core.hpp | 7 ++ .../launch/emergency_handler.launch.xml | 2 + .../emergency_handler_core.cpp | 70 ++++++++++++++++--- 3 files changed, 71 insertions(+), 8 deletions(-) diff --git a/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp b/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp index 02ccb074f48b9..51c41edb9667f 100644 --- a/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp +++ b/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp @@ -30,6 +30,7 @@ #include <tier4_system_msgs/msg/mrm_behavior_status.hpp> #include <tier4_system_msgs/msg/operation_mode_availability.hpp> #include <tier4_system_msgs/srv/operate_mrm.hpp> +#include <autoware_adapi_v1_msgs/msg/operation_mode_state.hpp> // ROS 2 core #include <rclcpp/create_timer.hpp> @@ -77,6 +78,8 @@ class EmergencyHandler : public rclcpp::Node sub_mrm_comfortable_stop_status_; rclcpp::Subscription<tier4_system_msgs::msg::MrmBehaviorStatus>::SharedPtr sub_mrm_emergency_stop_status_; + rclcpp::Subscription<autoware_adapi_v1_msgs::msg::OperationModeState>::SharedPtr + sub_operation_mode_state_; autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr prev_control_command_; nav_msgs::msg::Odometry::ConstSharedPtr odom_; @@ -85,6 +88,7 @@ class EmergencyHandler : public rclcpp::Node 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 onPrevControlCommand( const autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr msg); @@ -97,6 +101,8 @@ class EmergencyHandler : public rclcpp::Node 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<autoware_auto_control_msgs::msg::AckermannControlCommand>::SharedPtr @@ -156,6 +162,7 @@ class EmergencyHandler : public rclcpp::Node autoware_adapi_v1_msgs::msg::MrmState::_behavior_type getCurrentMrmBehavior(); bool isStopped(); bool isEmergency() const; + bool isArrivedAtGoal(); }; #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 index 0ff9d1359dc71..d85598d547a45 100644 --- a/system/emergency_handler/launch/emergency_handler.launch.xml +++ b/system/emergency_handler/launch/emergency_handler.launch.xml @@ -7,6 +7,7 @@ <arg name="input_mrm_pull_over_state" default="/system/mrm/pull_over/status"/> <arg name="input_mrm_comfortable_stop_state" default="/system/mrm/comfortable_stop/status"/> <arg name="input_mrm_emergency_stop_state" default="/system/mrm/emergency_stop/status"/> + <arg name="input_api_operation_mode_state" default="/api/operation_mode/state"/> <arg name="output_gear" default="/system/emergency/gear_cmd"/> <arg name="output_hazard" default="/system/emergency/hazard_lights_cmd"/> @@ -26,6 +27,7 @@ <remap from="~/input/mrm/pull_over/status" to="$(var input_mrm_pull_over_state)"/> <remap from="~/input/mrm/comfortable_stop/status" to="$(var input_mrm_comfortable_stop_state)"/> <remap from="~/input/mrm/emergency_stop/status" to="$(var input_mrm_emergency_stop_state)"/> + <remap from="~/input/api/operation_mode/state" to="$(var input_api_operation_mode_state)"/> <remap from="~/output/gear" to="$(var output_gear)"/> <remap from="~/output/hazard" to="$(var output_hazard)"/> diff --git a/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp b/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp index 193817ded16da..7cfca50f0d5b5 100644 --- a/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp +++ b/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp @@ -58,6 +58,9 @@ EmergencyHandler::EmergencyHandler() : Node("emergency_handler") sub_mrm_emergency_stop_status_ = create_subscription<tier4_system_msgs::msg::MrmBehaviorStatus>( "~/input/mrm/emergency_stop/status", rclcpp::QoS{1}, std::bind(&EmergencyHandler::onMrmEmergencyStopStatus, this, _1)); + sub_operation_mode_state_ = create_subscription<autoware_adapi_v1_msgs::msg::OperationModeState>( + "~/input/api/operation_mode/state", rclcpp::QoS{1}, + std::bind(&EmergencyHandler::onOperationModeState, this, _1)); // Publisher pub_control_command_ = create_publisher<autoware_auto_control_msgs::msg::AckermannControlCommand>( @@ -94,6 +97,7 @@ EmergencyHandler::EmergencyHandler() : Node("emergency_handler") mrm_comfortable_stop_status_ = std::make_shared<const tier4_system_msgs::msg::MrmBehaviorStatus>(); mrm_emergency_stop_status_ = std::make_shared<const tier4_system_msgs::msg::MrmBehaviorStatus>(); + operation_mode_state_ = std::make_shared<const autoware_adapi_v1_msgs::msg::OperationModeState>(); 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; @@ -170,6 +174,12 @@ void EmergencyHandler::onMrmEmergencyStopStatus( mrm_emergency_stop_status_ = msg; } +void EmergencyHandler::onOperationModeState( + const autoware_adapi_v1_msgs::msg::OperationModeState::ConstSharedPtr msg) +{ + operation_mode_state_ = msg; +} + autoware_auto_vehicle_msgs::msg::HazardLightsCommand EmergencyHandler::createHazardCmdMsg() { using autoware_auto_vehicle_msgs::msg::HazardLightsCommand; @@ -481,6 +491,12 @@ void EmergencyHandler::updateMrmState() } } 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; + } + } if (isStopped()) { transitionTo(MrmState::MRM_SUCCEEDED); return; @@ -519,20 +535,47 @@ autoware_adapi_v1_msgs::msg::MrmState::_behavior_type EmergencyHandler::getCurre return MrmState::EMERGENCY_STOP; } if (mrm_state_.behavior == MrmState::PULL_OVER) { - if (!operation_mode_availability_->pull_over) { - if (!operation_mode_availability_->emergency_stop) { - RCLCPP_WARN(this->get_logger(), "no mrm operation available: operate emergency_stop"); + if (operation_mode_availability_->pull_over) { + if (param_.use_pull_over) { + return MrmState::PULL_OVER; } - return MrmState::EMERGENCY_STOP; } + 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 (!operation_mode_availability_->comfortable_stop) { - if (!operation_mode_availability_->emergency_stop) { - RCLCPP_WARN(this->get_logger(), "no mrm operation available: operate emergency_stop"); + if (isStopped() && operation_mode_availability_->pull_over) { + if (param_.use_pull_over) { + return MrmState::PULL_OVER; } - return MrmState::EMERGENCY_STOP; } + 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; @@ -552,3 +595,14 @@ bool EmergencyHandler::isEmergency() const { return !operation_mode_availability_->autonomous | is_emergency_holding_; } + +bool EmergencyHandler::isArrivedAtGoal() +{ + using autoware_adapi_v1_msgs::msg::OperationModeState; + + if (operation_mode_state_->mode == OperationModeState::STOP){ + return true; + } + + return false; +}