diff --git a/rclcpp_action/src/client.cpp b/rclcpp_action/src/client.cpp index d3085e288c..3c916b4ee2 100644 --- a/rclcpp_action/src/client.cpp +++ b/rclcpp_action/src/client.cpp @@ -159,12 +159,6 @@ class ClientBaseImpl // take_data for execution std::deque> data_queue_; - // Lock for unreported events - std::recursive_mutex unreported_events_mutex_; - - // number of events, that were not yet reported by is_ready - size_t num_unreported_events_ = 0; - // Lock for action_client_ std::recursive_mutex action_client_mutex_; @@ -358,9 +352,7 @@ ClientBase::is_ready(rcl_wait_set_t * wait_set) } } - size_t cnt = 0; if (is_feedback_ready) { - cnt++; std::shared_ptr feedback_message; { std::lock_guard lock(pimpl_->action_client_mutex_); @@ -374,10 +366,11 @@ ClientBase::is_ready(rcl_wait_set_t * wait_set) std::make_shared( ClientBaseData::FeedbackReadyData( ret, feedback_message))); + + return true; } if (is_status_ready) { - cnt++; std::shared_ptr status_message; { std::lock_guard lock(pimpl_->action_client_mutex_); @@ -391,10 +384,11 @@ ClientBase::is_ready(rcl_wait_set_t * wait_set) std::make_shared( ClientBaseData::StatusReadyData( ret, status_message))); + + return true; } if (is_goal_response_ready) { - cnt++; rmw_request_id_t response_header; std::shared_ptr goal_response; { @@ -404,15 +398,17 @@ ClientBase::is_ready(rcl_wait_set_t * wait_set) ret = rcl_action_take_goal_response( pimpl_->client_handle.get(), &response_header, goal_response.get()); } + std::lock_guard lock(pimpl_->data_queue_mutex_); pimpl_->data_queue_.push_back( std::make_shared( ClientBaseData::GoalResponseData( ret, response_header, goal_response))); + + return true; } if (is_result_response_ready) { - cnt++; rmw_request_id_t response_header; std::shared_ptr result_response; { @@ -421,15 +417,17 @@ ClientBase::is_ready(rcl_wait_set_t * wait_set) ret = rcl_action_take_result_response( pimpl_->client_handle.get(), &response_header, result_response.get()); } + std::lock_guard lock(pimpl_->data_queue_mutex_); pimpl_->data_queue_.push_back( std::make_shared( ClientBaseData::ResultResponseData( ret, response_header, result_response))); + + return true; } if (is_cancel_response_ready) { - cnt++; rmw_request_id_t response_header; std::shared_ptr cancel_response; { @@ -444,21 +442,10 @@ ClientBase::is_ready(rcl_wait_set_t * wait_set) std::make_shared( ClientBaseData::CancelResponseData( ret, response_header, cancel_response))); + return true; } - bool return_data_ready = false; - - { - std::lock_guard lock(pimpl_->unreported_events_mutex_); - pimpl_->num_unreported_events_ += cnt; - - if (pimpl_->num_unreported_events_ > 0) { - pimpl_->num_unreported_events_--; - return_data_ready = true; - } - } - - return return_data_ready; + return false; } void diff --git a/rclcpp_action/src/server.cpp b/rclcpp_action/src/server.cpp index d8fea64893..ab05660843 100644 --- a/rclcpp_action/src/server.cpp +++ b/rclcpp_action/src/server.cpp @@ -105,12 +105,6 @@ class ServerBaseImpl // take_data for execution std::deque> data_queue_; - // Lock for unreported events - std::recursive_mutex unreported_events_mutex_; - - // number of events, that were not yet reported by is_ready - size_t num_unreported_events_ = 0; - rclcpp::Logger logger_; }; @@ -236,11 +230,7 @@ ServerBase::is_ready(rcl_wait_set_t * wait_set) rclcpp::exceptions::throw_from_rcl_error(ret); } - - size_t cnt = 0; - if (goal_request_ready) { - cnt++; rcl_action_goal_info_t goal_info = rcl_action_get_zero_initialized_goal_info(); rmw_request_id_t request_header; std::shared_ptr message; @@ -259,10 +249,11 @@ ServerBase::is_ready(rcl_wait_set_t * wait_set) std::make_shared( ServerBaseData::GoalRequestData( ret, goal_info, request_header, message))); + + return true; } if (cancel_request_ready) { - cnt++; rmw_request_id_t request_header; // Initialize cancel request @@ -282,10 +273,11 @@ ServerBase::is_ready(rcl_wait_set_t * wait_set) ServerBaseData::CancelRequestData( ret, request, request_header))); + + return true; } if (result_request_ready) { - cnt++; // Get the result request message rmw_request_id_t request_header; std::shared_ptr result_request; @@ -301,28 +293,19 @@ ServerBase::is_ready(rcl_wait_set_t * wait_set) std::make_shared( ServerBaseData::ResultRequestData( ret, result_request, request_header))); + + return true; } if (goal_expired) { - cnt++; std::lock_guard lock(pimpl_->data_queue_mutex_); pimpl_->data_queue_.push_back( std::make_shared(ServerBaseData::GoalExpiredData())); - } - - bool return_data_ready = false; - { - std::lock_guard lock(pimpl_->unreported_events_mutex_); - pimpl_->num_unreported_events_ += cnt; - - if (pimpl_->num_unreported_events_ > 0) { - pimpl_->num_unreported_events_--; - return_data_ready = true; - } + return true; } - return return_data_ready; + return false; } std::shared_ptr