Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat: change mrm state type to internal one #1392

Merged
merged 2 commits into from
Jul 4, 2024
Merged

feat: change mrm state type to internal one #1392

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
8b87168
feat: change mrm state type to internal one
TetsuKawa Jul 4, 2024
a274542
Merge branch 'feat/mrm-v0.6-integ-for-psim-test-based-on-x2-v3.0.0' i…
TetsuKawa Jul 4, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
11 changes: 6 additions & 5 deletions system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp
View file
Open in desktop
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
Open in desktop
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
Loading