From 8fb8b31ec7a98f313bfac6ab2913629d9ef406d7 Mon Sep 17 00:00:00 2001 From: Janosch Machowinski Date: Mon, 7 Aug 2023 11:50:26 +0000 Subject: [PATCH 1/2] fix(ClientGoalHandle): Made mutex recursive to prevent deadlocks This prevents deadlocks in cases, were e.g. get_status() would be called on the handle in a callback of the handle. Signed-off-by: Janosch Machowinski --- .../rclcpp_action/client_goal_handle.hpp | 2 +- .../rclcpp_action/client_goal_handle_impl.hpp | 22 +++++++++---------- 2 files changed, 12 insertions(+), 12 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp b/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp index dd3a40671c..73987ec887 100644 --- a/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp +++ b/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp @@ -163,7 +163,7 @@ class ClientGoalHandle ResultCallback result_callback_{nullptr}; int8_t status_{GoalStatus::STATUS_ACCEPTED}; - std::mutex handle_mutex_; + std::recursive_mutex handle_mutex_; }; } // namespace rclcpp_action diff --git a/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp b/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp index 0c25e52433..d12b4fc5b3 100644 --- a/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp +++ b/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp @@ -59,7 +59,7 @@ template std::shared_future::WrappedResult> ClientGoalHandle::async_get_result() { - std::lock_guard guard(handle_mutex_); + std::lock_guard guard(handle_mutex_); if (!is_result_aware_) { throw exceptions::UnawareGoalHandleError(); } @@ -70,7 +70,7 @@ template void ClientGoalHandle::set_result(const WrappedResult & wrapped_result) { - std::lock_guard guard(handle_mutex_); + std::lock_guard guard(handle_mutex_); status_ = static_cast(wrapped_result.code); result_promise_.set_value(wrapped_result); if (result_callback_) { @@ -82,7 +82,7 @@ template void ClientGoalHandle::set_feedback_callback(FeedbackCallback callback) { - std::lock_guard guard(handle_mutex_); + std::lock_guard guard(handle_mutex_); feedback_callback_ = callback; } @@ -90,7 +90,7 @@ template void ClientGoalHandle::set_result_callback(ResultCallback callback) { - std::lock_guard guard(handle_mutex_); + std::lock_guard guard(handle_mutex_); result_callback_ = callback; } @@ -98,7 +98,7 @@ template int8_t ClientGoalHandle::get_status() { - std::lock_guard guard(handle_mutex_); + std::lock_guard guard(handle_mutex_); return status_; } @@ -106,7 +106,7 @@ template void ClientGoalHandle::set_status(int8_t status) { - std::lock_guard guard(handle_mutex_); + std::lock_guard guard(handle_mutex_); status_ = status; } @@ -114,7 +114,7 @@ template bool ClientGoalHandle::is_feedback_aware() { - std::lock_guard guard(handle_mutex_); + std::lock_guard guard(handle_mutex_); return feedback_callback_ != nullptr; } @@ -122,7 +122,7 @@ template bool ClientGoalHandle::is_result_aware() { - std::lock_guard guard(handle_mutex_); + std::lock_guard guard(handle_mutex_); return is_result_aware_; } @@ -130,7 +130,7 @@ template bool ClientGoalHandle::set_result_awareness(bool awareness) { - std::lock_guard guard(handle_mutex_); + std::lock_guard guard(handle_mutex_); bool previous = is_result_aware_; is_result_aware_ = awareness; return previous; @@ -140,7 +140,7 @@ template void ClientGoalHandle::invalidate(const exceptions::UnawareGoalHandleError & ex) { - std::lock_guard guard(handle_mutex_); + std::lock_guard guard(handle_mutex_); // Guard against multiple calls if (is_invalidated()) { return; @@ -168,7 +168,7 @@ ClientGoalHandle::call_feedback_callback( RCLCPP_ERROR(rclcpp::get_logger("rclcpp_action"), "Sent feedback to wrong goal handle."); return; } - std::lock_guard guard(handle_mutex_); + std::lock_guard guard(handle_mutex_); if (nullptr == feedback_callback_) { // Normal, some feedback messages may arrive after the goal result. RCLCPP_DEBUG(rclcpp::get_logger("rclcpp_action"), "Received feedback but goal ignores it."); From 944fac8db2ad9757b305ec17873824f5485120ea Mon Sep 17 00:00:00 2001 From: Janosch Machowinski Date: Mon, 21 Aug 2023 10:40:33 +0000 Subject: [PATCH 2/2] test(rclcpp_action): Added test for deadlocks during access of a goal handle This test checks, if the code deadlocks, if methods on the goal handle are called from the callbacks. Signed-off-by: Janosch Machowinski Apply suggestions from code review Co-authored-by: Tomoya Fujita --- rclcpp_action/test/test_client.cpp | 80 ++++++++++++++++++++++++++++++ 1 file changed, 80 insertions(+) diff --git a/rclcpp_action/test/test_client.cpp b/rclcpp_action/test/test_client.cpp index b94a82d500..b180c3f825 100644 --- a/rclcpp_action/test/test_client.cpp +++ b/rclcpp_action/test/test_client.cpp @@ -852,6 +852,86 @@ TEST_F(TestClientAgainstServer, async_cancel_some_goals_with_callback) EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_CANCELED, goal_handle0->get_status()); } +TEST_F(TestClientAgainstServer, deadlock_in_callbacks) +{ + std::atomic feedback_callback_called = false; + std::atomic response_callback_called = false; + std::atomic result_callback_called = false; + std::atomic no_deadlock = false; + + std::thread tr = std::thread( + [&]() { + auto action_client = rclcpp_action::create_client(client_node, action_name); + ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); + + ActionGoal goal; + + using GoalHandle = rclcpp_action::ClientGoalHandle; + rclcpp_action::Client::SendGoalOptions ops; + ops.feedback_callback = + [&feedback_callback_called](const GoalHandle::SharedPtr handle, + ActionType::Feedback::ConstSharedPtr) { + // call functions on the handle that acquire the lock + handle->get_status(); + handle->is_feedback_aware(); + handle->is_result_aware(); + + feedback_callback_called = true; + }; + ops.goal_response_callback = [&response_callback_called]( + const GoalHandle::SharedPtr & handle) { + // call functions on the handle that acquire the lock + handle->get_status(); + handle->is_feedback_aware(); + handle->is_result_aware(); + + response_callback_called = true; + }; + ops.result_callback = [&result_callback_called]( + const GoalHandle::WrappedResult &) { + result_callback_called = true; + }; + + goal.order = 6; + auto future_goal_handle = action_client->async_send_goal(goal, ops); + dual_spin_until_future_complete(future_goal_handle); + auto goal_handle = future_goal_handle.get(); + + ASSERT_TRUE(goal_handle); + + ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock.get_clock_handle(), RCL_S_TO_NS(2))); + + auto result_future = action_client->async_get_result(goal_handle); + dual_spin_until_future_complete(result_future); + + EXPECT_TRUE(result_future.valid()); + auto result = result_future.get(); + + no_deadlock = true; + }); + + auto start_time = std::chrono::system_clock::now(); + + while (std::chrono::system_clock::now() - start_time < std::chrono::milliseconds(2000) && + !no_deadlock) + { + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + + if (no_deadlock) { + tr.join(); + } else { + // In case of a failure, the thread is assumed to be in a deadlock. + // We detach the thread so we don't block further tests. + tr.detach(); + } + + EXPECT_TRUE(no_deadlock); + EXPECT_TRUE(response_callback_called); + EXPECT_TRUE(result_callback_called); + EXPECT_TRUE(feedback_callback_called); +} + TEST_F(TestClientAgainstServer, send_rcl_errors) { auto action_client = rclcpp_action::create_client(client_node, action_name);