Skip to content

Commit

Permalink
fix(ad_service_state_monitor): lock shread variables to prevent race …
Browse files Browse the repository at this point in the history 
…condition (autowarefoundation#1395) (autowarefoundation#103)

fix(ad_service_state_monitor): lock shread variables to prevent race … (autowarefoundation#1395)

* fix(ad_service_state_monitor): lock shread variables to prevent race condition

state_machine_ and state_input_ are shared by multiple callback groups (threads).
So, mutex locks have been applied because it may cause race conditions.

Signed-off-by: Yuuki Takano <yuuki.takano@tier4.jp>

* fix(ad_service_state_monitor): lock state_input_

state_input_ must be locked before accessing

Signed-off-by: Yuuki Takano <yuuki.takano@tier4.jp>

* fix(ad_service_state_monitor): minimamize the steps of the critical sections

- Unlock the locks as earlier as possible.
- rename lock variables to improve readability

Signed-off-by: Yuuki Takano <yuuki.takano@tier4.jp>

* fix(ad_service_state_monitor_node): remove uneccesary unlock

Signed-off-by: Yuuki Takano <yuuki.takano@tier4.jp>

Signed-off-by: Yuuki Takano <yuuki.takano@tier4.jp>
Co-authored-by: ytakano <ytakanoster@gmail.com>
  • Loading branch information
@h-ohta @ytakano
h-ohta and ytakano committed Sep 5, 2022
1 parent 339e6a8 commit ac7b04e
Show file tree
Hide file tree
Showing 3 changed files with 82 additions and 32 deletions.
4 changes: 4 additions & 0 deletions ..._service_state_monitor/include/ad_service_state_monitor/ad_service_state_monitor_node.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -121,6 +121,10 @@ class AutowareStateMonitorNode : public rclcpp::Node
StateInput state_input_;
StateParam state_param_;

// Lock Variables
std::mutex lock_state_machine_;
std::mutex lock_state_input_;

// Diagnostic Updater
diagnostic_updater::Updater updater_;

Expand Down
108 changes: 76 additions & 32 deletions ...service_state_monitor/src/ad_service_state_monitor_node/ad_service_state_monitor_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -78,22 +78,25 @@ geometry_msgs::msg::PoseStamped::SharedPtr getCurrentPose(const tf2_ros::Buffer
void AutowareStateMonitorNode::onAutowareEngage(
const autoware_auto_vehicle_msgs::msg::Engage::ConstSharedPtr msg)
{
std::lock_guard<std::mutex> lock(lock_state_input_);
state_input_.autoware_engage = msg;
}

void AutowareStateMonitorNode::onVehicleControlMode(
const autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg)
{
std::lock_guard<std::mutex> lock(lock_state_input_);
state_input_.control_mode_ = msg;
}

void AutowareStateMonitorNode::onRoute(
const autoware_auto_planning_msgs::msg::HADMapRoute::ConstSharedPtr msg)
{
state_input_.route = msg;

// Get goal pose
{
std::lock_guard<std::mutex> lock(lock_state_input_);
state_input_.route = msg;

// Get goal pose
geometry_msgs::msg::Pose::SharedPtr p = std::make_shared<geometry_msgs::msg::Pose>();
*p = msg->goal_pose;
state_input_.goal_pose = geometry_msgs::msg::Pose::ConstSharedPtr(p);
Expand All @@ -107,6 +110,8 @@ void AutowareStateMonitorNode::onRoute(

void AutowareStateMonitorNode::onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg)
{
std::lock_guard<std::mutex> lock(lock_state_input_);

state_input_.odometry = msg;

state_input_.odometry_buffer.push_back(msg);
Expand All @@ -130,17 +135,26 @@ bool AutowareStateMonitorNode::onShutdownService(
const std::shared_ptr<std_srvs::srv::Trigger::Response> response)
{
(void)request_header;
state_input_.is_finalizing = true;

{
std::lock_guard<std::mutex> lock(lock_state_input_);
state_input_.is_finalizing = true;
}

const auto t_start = this->get_clock()->now();
constexpr double timeout = 3.0;
while (rclcpp::ok()) {
// rclcpp::spin_some(this->get_node_base_interface());

if (state_machine_->getCurrentState() == AutowareState::Finalizing) {
response->success = true;
response->message = "Shutdown Autoware.";
return true;
{
std::unique_lock<std::mutex> lock(lock_state_machine_);
if (state_machine_->getCurrentState() == AutowareState::Finalizing) {
lock.unlock();

response->success = true;
response->message = "Shutdown Autoware.";
return true;
}
}

if ((this->get_clock()->now() - t_start).seconds() > timeout) {
Expand All @@ -162,22 +176,40 @@ bool AutowareStateMonitorNode::onResetRouteService(
[[maybe_unused]] const std::shared_ptr<std_srvs::srv::Trigger::Request> request,
const std::shared_ptr<std_srvs::srv::Trigger::Response> response)
{
if (state_machine_->getCurrentState() != AutowareState::WaitingForEngage) {
response->success = false;
response->message = "Reset route can be accepted only under WaitingForEngage.";
return true;
{
std::unique_lock<std::mutex> lock(lock_state_machine_);
if (state_machine_->getCurrentState() != AutowareState::WaitingForEngage) {
lock.unlock();

response->success = false;
response->message = "Reset route can be accepted only under WaitingForEngage.";
return true;
}
}

state_input_.is_route_reset_required = true;
{
std::lock_guard<std::mutex> lock(lock_state_input_);
state_input_.is_route_reset_required = true;
}

const auto t_start = this->now();
constexpr double timeout = 3.0;
while (rclcpp::ok()) {
if (state_machine_->getCurrentState() == AutowareState::WaitingForRoute) {
state_input_.is_route_reset_required = false;
response->success = true;
response->message = "Reset route.";
return true;
{
// To avoid dead lock, 2-phase lock is required here.
// If you change the order of the locks below, it may be dead-lock.
std::unique_lock<std::mutex> lock_state_input(lock_state_input_);
std::unique_lock<std::mutex> lock_state_machine(lock_state_machine_);
if (state_machine_->getCurrentState() == AutowareState::WaitingForRoute) {
state_input_.is_route_reset_required = false;

lock_state_machine.unlock();
lock_state_input.unlock();

response->success = true;
response->message = "Reset route.";
return true;
}
}

if ((this->now() - t_start).seconds() > timeout) {
Expand All @@ -196,21 +228,32 @@ bool AutowareStateMonitorNode::onResetRouteService(

void AutowareStateMonitorNode::onTimer()
{
// Prepare state input
state_input_.current_pose = getCurrentPose(tf_buffer_);
if (state_input_.current_pose == nullptr) {
RCLCPP_WARN_THROTTLE(
this->get_logger(), *this->get_clock(), 5000 /* ms */,
"Fail lookupTransform base_link to map");
}
AutowareState prev_autoware_state;
AutowareState autoware_state;

state_input_.topic_stats = getTopicStats();
state_input_.param_stats = getParamStats();
state_input_.tf_stats = getTfStats();
state_input_.current_time = this->now();
// Update state
const auto prev_autoware_state = state_machine_->getCurrentState();
const auto autoware_state = state_machine_->updateState(state_input_);
{
std::unique_lock<std::mutex> lock_state_input(lock_state_input_);

// Prepare state input
state_input_.current_pose = getCurrentPose(tf_buffer_);
if (state_input_.current_pose == nullptr) {
RCLCPP_WARN_THROTTLE(
this->get_logger(), *this->get_clock(), 5000 /* ms */,
"Fail lookupTransform base_link to map");
}

state_input_.topic_stats = getTopicStats();
state_input_.param_stats = getParamStats();
state_input_.tf_stats = getTfStats();
state_input_.current_time = this->now();

// Update state
// To avoid dead lock, 2-phase lock is required here.
std::lock_guard<std::mutex> lock_state_machine(lock_state_machine_);

prev_autoware_state = state_machine_->getCurrentState();
autoware_state = state_machine_->updateState(state_input_);
}

if (autoware_state != prev_autoware_state) {
RCLCPP_INFO(
Expand Down Expand Up @@ -367,6 +410,7 @@ TfStats AutowareStateMonitorNode::getTfStats() const

bool AutowareStateMonitorNode::isEngaged()
{
std::lock_guard<std::mutex> lock(lock_state_input_);
if (!state_input_.autoware_engage) {
return false;
}
Expand Down
2 changes: 2 additions & 0 deletions system/ad_service_state_monitor/src/ad_service_state_monitor_node/diagnostics.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -50,6 +50,7 @@ void AutowareStateMonitorNode::checkTopicStatus(
{
int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK;

std::lock_guard<std::mutex> lock(lock_state_input_);
const auto & topic_stats = state_input_.topic_stats;

// OK
Expand Down Expand Up @@ -125,6 +126,7 @@ void AutowareStateMonitorNode::checkTFStatus(
{
int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK;

std::lock_guard<std::mutex> lock(lock_state_input_);
const auto & tf_stats = state_input_.tf_stats;

// OK
Expand Down

0 comments on commit ac7b04e

Please sign in to comment.