Skip to content

Commit

Permalink
feat: change mrm state type to internal one (#1392)
Browse files Browse the repository at this point in the history 
Signed-off-by: TetsuKawa <[email protected]>
  • Loading branch information
@TetsuKawa @mkuri
TetsuKawa authored and mkuri committed Aug 15, 2024
1 parent d7609d8 commit 835a843
Show file tree
Hide file tree
Showing 2 changed files with 61 additions and 55 deletions.
11 changes: 6 additions & 5 deletions system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,12 +25,13 @@
// Autoware
#include <tier4_autoware_utils/ros/update_param.hpp>

#include <autoware_adapi_v1_msgs/msg/mrm_state.hpp>
#include <autoware_adapi_v1_msgs/msg/operation_mode_state.hpp>
#include <autoware_auto_vehicle_msgs/msg/control_mode_report.hpp>
#include <autoware_auto_vehicle_msgs/msg/gear_command.hpp>
#include <autoware_auto_vehicle_msgs/msg/hazard_lights_command.hpp>
#include <tier4_system_msgs/msg/mrm_behavior.hpp>
#include <tier4_system_msgs/msg/mrm_behavior_status.hpp>
#include <tier4_system_msgs/msg/mrm_state.hpp>
#include <tier4_system_msgs/msg/operation_mode_availability.hpp>
#include <tier4_system_msgs/srv/operate_mrm.hpp>

Expand Down Expand Up @@ -117,9 +118,9 @@ class MrmHandler : public rclcpp::Node
void publishHazardCmd();
void publishGearCmd();

rclcpp::Publisher<autoware_adapi_v1_msgs::msg::MrmState>::SharedPtr pub_mrm_state_;
rclcpp::Publisher<tier4_system_msgs::msg::MrmState>::SharedPtr pub_mrm_state_;

autoware_adapi_v1_msgs::msg::MrmState mrm_state_;
tier4_system_msgs::msg::MrmState mrm_state_;
void publishMrmState();

// parameter callback
Expand All @@ -136,7 +137,7 @@ class MrmHandler : public rclcpp::Node
rclcpp::Client<tier4_system_msgs::srv::OperateMrm>::SharedPtr client_mrm_emergency_stop_;

bool requestMrmBehavior(
const autoware_adapi_v1_msgs::msg::MrmState::_behavior_type & mrm_behavior,
const tier4_system_msgs::msg::MrmBehavior::_type_type & mrm_behavior,
RequestType request_type) const;
void logMrmCallingResult(
const tier4_system_msgs::srv::OperateMrm::Response & result, const std::string & behavior,
Expand Down Expand Up @@ -164,7 +165,7 @@ class MrmHandler : public rclcpp::Node
void updateMrmState();
void operateMrm();
void handleFailedRequest();
autoware_adapi_v1_msgs::msg::MrmState::_behavior_type getCurrentMrmBehavior();
tier4_system_msgs::msg::MrmBehavior::_type_type getCurrentMrmBehavior();
bool isStopped();
bool isEmergency() const;
bool isArrivedAtGoal();
Expand Down
105 changes: 55 additions & 50 deletions system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -68,7 +68,7 @@ MrmHandler::MrmHandler() : Node("mrm_handler")
pub_gear_cmd_ =
create_publisher<autoware_auto_vehicle_msgs::msg::GearCommand>("~/output/gear", rclcpp::QoS{1});
pub_mrm_state_ =
create_publisher<autoware_adapi_v1_msgs::msg::MrmState>("~/output/mrm/state", rclcpp::QoS{1});
create_publisher<tier4_system_msgs::msg::MrmState>("~/output/mrm/state", rclcpp::QoS{1});

// Clients
client_mrm_pull_over_group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
Expand All @@ -95,8 +95,8 @@ MrmHandler::MrmHandler() : Node("mrm_handler")
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;
mrm_state_.state = tier4_system_msgs::msg::MrmState::NORMAL;
mrm_state_.behavior.type = tier4_system_msgs::msg::MrmBehavior::NONE;
is_operation_mode_availability_timeout = false;

// Timer
Expand Down Expand Up @@ -227,30 +227,31 @@ void MrmHandler::publishMrmState()

void MrmHandler::operateMrm()
{
using autoware_adapi_v1_msgs::msg::MrmState;
using tier4_system_msgs::msg::MrmBehavior;
using tier4_system_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) {
const auto current_mrm_behavior = MrmBehavior::NONE;
if (current_mrm_behavior == mrm_state_.behavior.type) {
return;
}
if (requestMrmBehavior(mrm_state_.behavior, RequestType::CANCEL)) {
mrm_state_.behavior = current_mrm_behavior;
if (requestMrmBehavior(mrm_state_.behavior.type, RequestType::CANCEL)) {
mrm_state_.behavior.type = current_mrm_behavior;
} else {
handleFailedRequest();
}
return;
}
if (mrm_state_.state == MrmState::MRM_OPERATING) {
const auto current_mrm_behavior = getCurrentMrmBehavior();
if (current_mrm_behavior == mrm_state_.behavior) {
if (current_mrm_behavior == mrm_state_.behavior.type) {
return;
}
if (!requestMrmBehavior(mrm_state_.behavior, RequestType::CANCEL)) {
if (!requestMrmBehavior(mrm_state_.behavior.type, RequestType::CANCEL)) {
handleFailedRequest();
} else if (requestMrmBehavior(current_mrm_behavior, RequestType::CALL)) {
mrm_state_.behavior = current_mrm_behavior;
mrm_state_.behavior.type = current_mrm_behavior;
} else {
handleFailedRequest();
}
Expand All @@ -270,21 +271,23 @@ void MrmHandler::operateMrm()

void MrmHandler::handleFailedRequest()
{
using autoware_adapi_v1_msgs::msg::MrmState;
using tier4_system_msgs::msg::MrmBehavior;
using tier4_system_msgs::msg::MrmState;

if (requestMrmBehavior(MrmState::EMERGENCY_STOP, CALL)) {
if (requestMrmBehavior(MrmBehavior::EMERGENCY_STOP, CALL)) {
if (mrm_state_.state != MrmState::MRM_OPERATING) transitionTo(MrmState::MRM_OPERATING);
} else {
transitionTo(MrmState::MRM_FAILED);
}
mrm_state_.behavior = MrmState::EMERGENCY_STOP;
mrm_state_.behavior.type = MrmBehavior::EMERGENCY_STOP;
}

bool MrmHandler::requestMrmBehavior(
const autoware_adapi_v1_msgs::msg::MrmState::_behavior_type & mrm_behavior,
const tier4_system_msgs::msg::MrmBehavior::_type_type & mrm_behavior,
RequestType request_type) const
{
using autoware_adapi_v1_msgs::msg::MrmState;
using tier4_system_msgs::msg::MrmBehavior;
using tier4_system_msgs::msg::MrmState;

auto request = std::make_shared<tier4_system_msgs::srv::OperateMrm::Request>();
if (request_type == RequestType::CALL) {
Expand All @@ -300,35 +303,35 @@ bool MrmHandler::requestMrmBehavior(
std::shared_future<std::shared_ptr<tier4_system_msgs::srv::OperateMrm::Response>> future;

const auto behavior2string = [](const int behavior) {
if (behavior == MrmState::NONE) {
if (behavior == MrmBehavior::NONE) {
return "NONE";
}
if (behavior == MrmState::PULL_OVER) {
if (behavior == MrmBehavior::PULL_OVER) {
return "PULL_OVER";
}
if (behavior == MrmState::COMFORTABLE_STOP) {
if (behavior == MrmBehavior::COMFORTABLE_STOP) {
return "COMFORTABLE_STOP";
}
if (behavior == MrmState::EMERGENCY_STOP) {
if (behavior == MrmBehavior::EMERGENCY_STOP) {
return "EMERGENCY_STOP";
}
const auto msg = "invalid behavior: " + std::to_string(behavior);
throw std::runtime_error(msg);
};

switch (mrm_behavior) {
case MrmState::NONE:
case MrmBehavior::NONE:
RCLCPP_WARN(this->get_logger(), "MRM behavior is None. Do nothing.");
return true;
case MrmState::PULL_OVER: {
case MrmBehavior::PULL_OVER: {
future = client_mrm_pull_over_->async_send_request(request).future.share();
break;
}
case MrmState::COMFORTABLE_STOP: {
case MrmBehavior::COMFORTABLE_STOP: {
future = client_mrm_comfortable_stop_->async_send_request(request).future.share();
break;
}
case MrmState::EMERGENCY_STOP: {
case MrmBehavior::EMERGENCY_STOP: {
future = client_mrm_emergency_stop_->async_send_request(request).future.share();
break;
}
Expand Down Expand Up @@ -433,7 +436,7 @@ void MrmHandler::onTimer()

void MrmHandler::transitionTo(const int new_state)
{
using autoware_adapi_v1_msgs::msg::MrmState;
using tier4_system_msgs::msg::MrmState;

const auto state2string = [](const int state) {
if (state == MrmState::NORMAL) {
Expand Down Expand Up @@ -462,8 +465,9 @@ void MrmHandler::transitionTo(const int new_state)

void MrmHandler::updateMrmState()
{
using autoware_adapi_v1_msgs::msg::MrmState;
using autoware_auto_vehicle_msgs::msg::ControlModeReport;
using tier4_system_msgs::msg::MrmBehavior;
using tier4_system_msgs::msg::MrmState;

// Check emergency
const bool is_emergency = isEmergency();
Expand Down Expand Up @@ -491,7 +495,7 @@ void MrmHandler::updateMrmState()

if (mrm_state_.state == MrmState::MRM_OPERATING) {
// TODO(TetsuKawa): Check MRC is accomplished
if (mrm_state_.behavior == MrmState::PULL_OVER) {
if (mrm_state_.behavior.type == MrmBehavior::PULL_OVER) {
if (isStopped() && isArrivedAtGoal()) {
transitionTo(MrmState::MRM_SUCCEEDED);
return;
Expand All @@ -504,7 +508,7 @@ void MrmHandler::updateMrmState()
}
} else if (mrm_state_.state == MrmState::MRM_SUCCEEDED) {
const auto current_mrm_behavior = getCurrentMrmBehavior();
if (current_mrm_behavior != mrm_state_.behavior) {
if (current_mrm_behavior != mrm_state_.behavior.type) {
transitionTo(MrmState::MRM_OPERATING);
}
} else if (mrm_state_.state == MrmState::MRM_FAILED) {
Expand All @@ -516,85 +520,86 @@ void MrmHandler::updateMrmState()
}
}

autoware_adapi_v1_msgs::msg::MrmState::_behavior_type MrmHandler::getCurrentMrmBehavior()
tier4_system_msgs::msg::MrmBehavior::_type_type MrmHandler::getCurrentMrmBehavior()
{
using autoware_adapi_v1_msgs::msg::MrmState;
using tier4_system_msgs::msg::MrmBehavior;
using tier4_system_msgs::msg::MrmState;
using tier4_system_msgs::msg::OperationModeAvailability;

// State machine
if (mrm_state_.behavior == MrmState::NONE) {
if (mrm_state_.behavior.type == MrmBehavior::NONE) {
if (is_operation_mode_availability_timeout) {
return MrmState::EMERGENCY_STOP;
return MrmBehavior::EMERGENCY_STOP;
}
if (operation_mode_availability_->pull_over) {
if (param_.use_pull_over) {
return MrmState::PULL_OVER;
return MrmBehavior::PULL_OVER;
}
}
if (operation_mode_availability_->comfortable_stop) {
if (param_.use_comfortable_stop) {
return MrmState::COMFORTABLE_STOP;
return MrmBehavior::COMFORTABLE_STOP;
}
}
if (!operation_mode_availability_->emergency_stop) {
RCLCPP_WARN(this->get_logger(), "no mrm operation available: operate emergency_stop");
}
return MrmState::EMERGENCY_STOP;
return MrmBehavior::EMERGENCY_STOP;
}
if (mrm_state_.behavior == MrmState::PULL_OVER) {
if (mrm_state_.behavior.type == MrmBehavior::PULL_OVER) {
if (is_operation_mode_availability_timeout) {
return MrmState::EMERGENCY_STOP;
return MrmBehavior::EMERGENCY_STOP;
}
if (operation_mode_availability_->pull_over) {
if (param_.use_pull_over) {
return MrmState::PULL_OVER;
return MrmBehavior::PULL_OVER;
}
}
if (operation_mode_availability_->comfortable_stop) {
if (param_.use_comfortable_stop) {
return MrmState::COMFORTABLE_STOP;
return MrmBehavior::COMFORTABLE_STOP;
}
}
if (!operation_mode_availability_->emergency_stop) {
RCLCPP_WARN(this->get_logger(), "no mrm operation available: operate emergency_stop");
}
return MrmState::EMERGENCY_STOP;
return MrmBehavior::EMERGENCY_STOP;
}
if (mrm_state_.behavior == MrmState::COMFORTABLE_STOP) {
if (mrm_state_.behavior.type == MrmBehavior::COMFORTABLE_STOP) {
if (is_operation_mode_availability_timeout) {
return MrmState::EMERGENCY_STOP;
return MrmBehavior::EMERGENCY_STOP;
}
if (isStopped() && operation_mode_availability_->pull_over) {
if (param_.use_pull_over && param_.use_pull_over_after_stopped) {
return MrmState::PULL_OVER;
return MrmBehavior::PULL_OVER;
}
}
if (operation_mode_availability_->comfortable_stop) {
if (param_.use_comfortable_stop) {
return MrmState::COMFORTABLE_STOP;
return MrmBehavior::COMFORTABLE_STOP;
}
}
if (!operation_mode_availability_->emergency_stop) {
RCLCPP_WARN(this->get_logger(), "no mrm operation available: operate emergency_stop");
}
return MrmState::EMERGENCY_STOP;
return MrmBehavior::EMERGENCY_STOP;
}
if (mrm_state_.behavior == MrmState::EMERGENCY_STOP) {
if (mrm_state_.behavior.type == MrmBehavior::EMERGENCY_STOP) {
if (is_operation_mode_availability_timeout) {
return MrmState::EMERGENCY_STOP;
return MrmBehavior::EMERGENCY_STOP;
}
if (isStopped() && operation_mode_availability_->pull_over) {
if (param_.use_pull_over && param_.use_pull_over_after_stopped) {
return MrmState::PULL_OVER;
return MrmBehavior::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 MrmBehavior::EMERGENCY_STOP;
}

return mrm_state_.behavior;
return mrm_state_.behavior.type;
}

bool MrmHandler::isStopped()
Expand Down

0 comments on commit 835a843

Please sign in to comment.