Skip to content

Commit 6bed372

Browse files
Autumn60Autumn60
committedMay 24, 2024
refactor emergency_handler_core
Signed-off-by: Autumn60 <akiro.harada@tier4.jp>
1 parent a732236 commit 6bed372

File tree

2 files changed

+66
-57
lines changed

2 files changed

+66
-57
lines changed
 

‎system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp

+3-10
Original file line numberDiff line numberDiff line change
@@ -71,21 +71,11 @@ class EmergencyHandler : public rclcpp::Node
7171

7272
autoware_auto_system_msgs::msg::HazardStatusStamped::ConstSharedPtr hazard_status_stamped_;
7373
autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr prev_control_command_;
74-
nav_msgs::msg::Odometry::ConstSharedPtr odom_;
75-
autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr control_mode_;
76-
tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr mrm_comfortable_stop_status_;
77-
tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr mrm_emergency_stop_status_;
7874

7975
void onHazardStatusStamped(
8076
const autoware_auto_system_msgs::msg::HazardStatusStamped::ConstSharedPtr msg);
8177
void onPrevControlCommand(
8278
const autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr msg);
83-
void onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg);
84-
void onControlMode(const autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg);
85-
void onMrmComfortableStopStatus(
86-
const tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr msg);
87-
void onMrmEmergencyStopStatus(
88-
const tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr msg);
8979

9080
// Publisher
9181
rclcpp::Publisher<autoware_auto_control_msgs::msg::AckermannControlCommand>::SharedPtr
@@ -141,6 +131,9 @@ class EmergencyHandler : public rclcpp::Node
141131
autoware_adapi_v1_msgs::msg::MrmState::_behavior_type getCurrentMrmBehavior();
142132
bool isStopped();
143133
bool isEmergency();
134+
bool isAutonomous();
135+
bool isComfortableStopStatusAvailable();
136+
bool isEmergencyStopStatusAvailable();
144137
};
145138

146139
#endif // EMERGENCY_HANDLER__EMERGENCY_HANDLER_CORE_HPP_

‎system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp

+63-47
Original file line numberDiff line numberDiff line change
@@ -26,10 +26,9 @@ EmergencyHandler::EmergencyHandler() : Node("emergency_handler")
2626
param_.use_parking_after_stopped = declare_parameter<bool>("use_parking_after_stopped");
2727
param_.use_comfortable_stop = declare_parameter<bool>("use_comfortable_stop");
2828
param_.turning_hazard_on.emergency = declare_parameter<bool>("turning_hazard_on.emergency");
29-
3029
using std::placeholders::_1;
3130

32-
// Subscriber
31+
// Subscriber with callback
3332
sub_hazard_status_stamped_ =
3433
create_subscription<autoware_auto_system_msgs::msg::HazardStatusStamped>(
3534
"~/input/hazard_status", rclcpp::QoS{1},
@@ -38,17 +37,41 @@ EmergencyHandler::EmergencyHandler() : Node("emergency_handler")
3837
create_subscription<autoware_auto_control_msgs::msg::AckermannControlCommand>(
3938
"~/input/prev_control_command", rclcpp::QoS{1},
4039
std::bind(&EmergencyHandler::onPrevControlCommand, this, _1));
40+
41+
// Subscriber without callback
42+
rclcpp::CallbackGroup::SharedPtr cb_group_noexec = this->create_callback_group(
43+
rclcpp::CallbackGroupType::MutuallyExclusive, false);
44+
auto subscription_options = rclcpp::SubscriptionOptions();
45+
subscription_options.callback_group = cb_group_noexec;
46+
4147
sub_odom_ = create_subscription<nav_msgs::msg::Odometry>(
42-
"~/input/odometry", rclcpp::QoS{1}, std::bind(&EmergencyHandler::onOdometry, this, _1));
43-
// subscribe control mode
48+
"~/input/odometry", rclcpp::QoS{1},
49+
[this]([[maybe_unused]] nav_msgs::msg::Odometry::SharedPtr) -> void
50+
{
51+
assert(false);
52+
}, subscription_options);
53+
4454
sub_control_mode_ = create_subscription<autoware_auto_vehicle_msgs::msg::ControlModeReport>(
45-
"~/input/control_mode", rclcpp::QoS{1}, std::bind(&EmergencyHandler::onControlMode, this, _1));
55+
"~/input/control_mode", rclcpp::QoS{1},
56+
[this]([[maybe_unused]] autoware_auto_vehicle_msgs::msg::ControlModeReport::SharedPtr) -> void
57+
{
58+
assert(false);
59+
}, subscription_options);
60+
61+
// subscribe control mode
4662
sub_mrm_comfortable_stop_status_ = create_subscription<tier4_system_msgs::msg::MrmBehaviorStatus>(
4763
"~/input/mrm/comfortable_stop/status", rclcpp::QoS{1},
48-
std::bind(&EmergencyHandler::onMrmComfortableStopStatus, this, _1));
64+
[this]([[maybe_unused]] tier4_system_msgs::msg::MrmBehaviorStatus::SharedPtr) -> void
65+
{
66+
assert(false);
67+
}, subscription_options);
68+
4969
sub_mrm_emergency_stop_status_ = create_subscription<tier4_system_msgs::msg::MrmBehaviorStatus>(
5070
"~/input/mrm/emergency_stop/status", rclcpp::QoS{1},
51-
std::bind(&EmergencyHandler::onMrmEmergencyStopStatus, this, _1));
71+
[this]([[maybe_unused]] tier4_system_msgs::msg::MrmBehaviorStatus::SharedPtr) -> void
72+
{
73+
assert(false);
74+
}, subscription_options);
5275

5376
// Publisher
5477
pub_control_command_ = create_publisher<autoware_auto_control_msgs::msg::AckermannControlCommand>(
@@ -73,13 +96,8 @@ EmergencyHandler::EmergencyHandler() : Node("emergency_handler")
7396
client_mrm_emergency_stop_group_);
7497

7598
// Initialize
76-
odom_ = std::make_shared<const nav_msgs::msg::Odometry>();
77-
control_mode_ = std::make_shared<const autoware_auto_vehicle_msgs::msg::ControlModeReport>();
7899
prev_control_command_ = autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr(
79100
new autoware_auto_control_msgs::msg::AckermannControlCommand);
80-
mrm_comfortable_stop_status_ =
81-
std::make_shared<const tier4_system_msgs::msg::MrmBehaviorStatus>();
82-
mrm_emergency_stop_status_ = std::make_shared<const tier4_system_msgs::msg::MrmBehaviorStatus>();
83101
mrm_state_.stamp = this->now();
84102
mrm_state_.state = autoware_adapi_v1_msgs::msg::MrmState::NORMAL;
85103
mrm_state_.behavior = autoware_adapi_v1_msgs::msg::MrmState::NONE;
@@ -107,29 +125,6 @@ void EmergencyHandler::onPrevControlCommand(
107125
autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr(control_command);
108126
}
109127

110-
void EmergencyHandler::onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg)
111-
{
112-
odom_ = msg;
113-
}
114-
115-
void EmergencyHandler::onControlMode(
116-
const autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg)
117-
{
118-
control_mode_ = msg;
119-
}
120-
121-
void EmergencyHandler::onMrmComfortableStopStatus(
122-
const tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr msg)
123-
{
124-
mrm_comfortable_stop_status_ = msg;
125-
}
126-
127-
void EmergencyHandler::onMrmEmergencyStopStatus(
128-
const tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr msg)
129-
{
130-
mrm_emergency_stop_status_ = msg;
131-
}
132-
133128
autoware_auto_vehicle_msgs::msg::HazardLightsCommand EmergencyHandler::createHazardCmdMsg()
134129
{
135130
using autoware_auto_vehicle_msgs::msg::HazardLightsCommand;
@@ -290,17 +285,14 @@ bool EmergencyHandler::isDataReady()
290285
return false;
291286
}
292287

293-
if (
294-
param_.use_comfortable_stop && mrm_comfortable_stop_status_->state ==
295-
tier4_system_msgs::msg::MrmBehaviorStatus::NOT_AVAILABLE) {
288+
if (param_.use_comfortable_stop && !isComfortableStopStatusAvailable()) {
296289
RCLCPP_INFO_THROTTLE(
297290
this->get_logger(), *this->get_clock(), std::chrono::milliseconds(5000).count(),
298291
"waiting for mrm comfortable stop to become available...");
299292
return false;
300293
}
301294

302-
if (
303-
mrm_emergency_stop_status_->state == tier4_system_msgs::msg::MrmBehaviorStatus::NOT_AVAILABLE) {
295+
if (!isEmergencyStopStatusAvailable()) {
304296
RCLCPP_INFO_THROTTLE(
305297
this->get_logger(), *this->get_clock(), std::chrono::milliseconds(5000).count(),
306298
"waiting for mrm emergency stop to become available...");
@@ -372,13 +364,11 @@ void EmergencyHandler::transitionTo(const int new_state)
372364
void EmergencyHandler::updateMrmState()
373365
{
374366
using autoware_adapi_v1_msgs::msg::MrmState;
375-
using autoware_auto_vehicle_msgs::msg::ControlModeReport;
376367

377368
// Check emergency
378369
const bool is_emergency = isEmergency();
379-
380370
// Get mode
381-
const bool is_auto_mode = control_mode_->mode == ControlModeReport::AUTONOMOUS;
371+
const bool is_auto_mode = isAutonomous();
382372

383373
// State Machine
384374
if (mrm_state_.state == MrmState::NORMAL) {
@@ -452,10 +442,36 @@ bool EmergencyHandler::isEmergency()
452442

453443
bool EmergencyHandler::isStopped()
454444
{
445+
auto odom = std::make_shared<nav_msgs::msg::Odometry>();
446+
rclcpp::MessageInfo msg_info;
447+
if(!sub_odom_->take(*odom, msg_info)) return false;
448+
455449
constexpr auto th_stopped_velocity = 0.001;
456-
if (odom_->twist.twist.linear.x < th_stopped_velocity) {
457-
return true;
458-
}
450+
return (odom->twist.twist.linear.x < th_stopped_velocity);
451+
}
459452

460-
return false;
453+
bool EmergencyHandler::isAutonomous()
454+
{
455+
using autoware_auto_vehicle_msgs::msg::ControlModeReport;
456+
457+
auto control_mode = std::make_shared<autoware_auto_vehicle_msgs::msg::ControlModeReport>();
458+
rclcpp::MessageInfo msg_info;
459+
if(!sub_control_mode_->take(*control_mode, msg_info)) return false;
460+
return control_mode->mode == ControlModeReport::AUTONOMOUS;
461+
}
462+
463+
bool EmergencyHandler::isComfortableStopStatusAvailable()
464+
{
465+
auto status = std::make_shared<tier4_system_msgs::msg::MrmBehaviorStatus>();
466+
rclcpp::MessageInfo msg_info;
467+
if(!sub_mrm_comfortable_stop_status_->take(*status, msg_info)) return false;
468+
return status->state != tier4_system_msgs::msg::MrmBehaviorStatus::NOT_AVAILABLE;
469+
}
470+
471+
bool EmergencyHandler::isEmergencyStopStatusAvailable()
472+
{
473+
auto status = std::make_shared<tier4_system_msgs::msg::MrmBehaviorStatus>();
474+
rclcpp::MessageInfo msg_info;
475+
if(!sub_mrm_emergency_stop_status_->take(*status, msg_info)) return false;
476+
return status->state != tier4_system_msgs::msg::MrmBehaviorStatus::NOT_AVAILABLE;
461477
}

0 commit comments

Comments
 (0)