rclcpp_action: take and execute service entities in priority.

Signed-off-by: Tomoya.Fujita <tomoya.fujita825@gmail.com>

rclcpp_action/src/client.cpp

@@ -550,21 +550,8 @@ ClientBase::clear_on_ready_callback()
 std::shared_ptr<void>
 ClientBase::take_data()
 {
-  if (pimpl_->is_feedback_ready) {
-    std::shared_ptr<void> feedback_message = this->create_feedback_message();
-    rcl_ret_t ret = rcl_action_take_feedback(
-      pimpl_->client_handle.get(), feedback_message.get());
-    return std::static_pointer_cast<void>(
-      std::make_shared<std::tuple<rcl_ret_t, std::shared_ptr<void>>>(
-        ret, feedback_message));
-  } else if (pimpl_->is_status_ready) {
-    std::shared_ptr<void> status_message = this->create_status_message();
-    rcl_ret_t ret = rcl_action_take_status(
-      pimpl_->client_handle.get(), status_message.get());
-    return std::static_pointer_cast<void>(
-      std::make_shared<std::tuple<rcl_ret_t, std::shared_ptr<void>>>(
-        ret, status_message));
-  } else if (pimpl_->is_goal_response_ready) {
+  // Take the service entities 1st to apply to the client
+  if (pimpl_->is_goal_response_ready) {
     rmw_request_id_t response_header;
     std::shared_ptr<void> goal_response = this->create_goal_response();
     rcl_ret_t ret = rcl_action_take_goal_response(
@@ -588,6 +575,20 @@ ClientBase::take_data()
     return std::static_pointer_cast<void>(
       std::make_shared<std::tuple<rcl_ret_t, rmw_request_id_t, std::shared_ptr<void>>>(
         ret, response_header, cancel_response));
+  } else if (pimpl_->is_feedback_ready) {
+    std::shared_ptr<void> feedback_message = this->create_feedback_message();
+    rcl_ret_t ret = rcl_action_take_feedback(
+      pimpl_->client_handle.get(), feedback_message.get());
+    return std::static_pointer_cast<void>(
+      std::make_shared<std::tuple<rcl_ret_t, std::shared_ptr<void>>>(
+        ret, feedback_message));
+  } else if (pimpl_->is_status_ready) {
+    std::shared_ptr<void> status_message = this->create_status_message();
+    rcl_ret_t ret = rcl_action_take_status(
+      pimpl_->client_handle.get(), status_message.get());
+    return std::static_pointer_cast<void>(
+      std::make_shared<std::tuple<rcl_ret_t, std::shared_ptr<void>>>(
+        ret, status_message));
   } else {
     throw std::runtime_error("Taking data from action client but nothing is ready");
   }
@@ -625,27 +626,8 @@ ClientBase::execute(std::shared_ptr<void> & data)
     throw std::runtime_error("'data' is empty");
   }
 
-  if (pimpl_->is_feedback_ready) {
-    auto shared_ptr = std::static_pointer_cast<std::tuple<rcl_ret_t, std::shared_ptr<void>>>(data);
-    auto ret = std::get<0>(*shared_ptr);
-    pimpl_->is_feedback_ready = false;
-    if (RCL_RET_OK == ret) {
-      auto feedback_message = std::get<1>(*shared_ptr);
-      this->handle_feedback_message(feedback_message);
-    } else if (RCL_RET_ACTION_CLIENT_TAKE_FAILED != ret) {
-      rclcpp::exceptions::throw_from_rcl_error(ret, "error taking feedback");
-    }
-  } else if (pimpl_->is_status_ready) {
-    auto shared_ptr = std::static_pointer_cast<std::tuple<rcl_ret_t, std::shared_ptr<void>>>(data);
-    auto ret = std::get<0>(*shared_ptr);
-    pimpl_->is_status_ready = false;
-    if (RCL_RET_OK == ret) {
-      auto status_message = std::get<1>(*shared_ptr);
-      this->handle_status_message(status_message);
-    } else if (RCL_RET_ACTION_CLIENT_TAKE_FAILED != ret) {
-      rclcpp::exceptions::throw_from_rcl_error(ret, "error taking status");
-    }
-  } else if (pimpl_->is_goal_response_ready) {
+  // Take the service entities 1st to apply to the client
+  if (pimpl_->is_goal_response_ready) {
     auto shared_ptr = std::static_pointer_cast<
       std::tuple<rcl_ret_t, rmw_request_id_t, std::shared_ptr<void>>>(data);
     auto ret = std::get<0>(*shared_ptr);
@@ -681,6 +663,26 @@ ClientBase::execute(std::shared_ptr<void> & data)
     } else if (RCL_RET_ACTION_CLIENT_TAKE_FAILED != ret) {
       rclcpp::exceptions::throw_from_rcl_error(ret, "error taking cancel response");
     }
+  } else if (pimpl_->is_feedback_ready) {
+    auto shared_ptr = std::static_pointer_cast<std::tuple<rcl_ret_t, std::shared_ptr<void>>>(data);
+    auto ret = std::get<0>(*shared_ptr);
+    pimpl_->is_feedback_ready = false;
+    if (RCL_RET_OK == ret) {
+      auto feedback_message = std::get<1>(*shared_ptr);
+      this->handle_feedback_message(feedback_message);
+    } else if (RCL_RET_ACTION_CLIENT_TAKE_FAILED != ret) {
+      rclcpp::exceptions::throw_from_rcl_error(ret, "error taking feedback");
+    }
+  } else if (pimpl_->is_status_ready) {
+    auto shared_ptr = std::static_pointer_cast<std::tuple<rcl_ret_t, std::shared_ptr<void>>>(data);
+    auto ret = std::get<0>(*shared_ptr);
+    pimpl_->is_status_ready = false;
+    if (RCL_RET_OK == ret) {
+      auto status_message = std::get<1>(*shared_ptr);
+      this->handle_status_message(status_message);
+    } else if (RCL_RET_ACTION_CLIENT_TAKE_FAILED != ret) {
+      rclcpp::exceptions::throw_from_rcl_error(ret, "error taking status");
+    }
   } else {
     throw std::runtime_error("Executing action client but nothing is ready");
   }

rclcpp_action/test/test_client.cpp

@@ -122,28 +122,28 @@ protected:
           goal_status.status = rclcpp_action::GoalStatus::STATUS_EXECUTING;
           status_message.status_list.push_back(goal_status);
           status_publisher->publish(status_message);
-          client_executor.spin_once();
+          client_executor.spin_some();
           ActionFeedbackMessage feedback_message;
           feedback_message.goal_id.uuid = goal_request->goal_id.uuid;
           feedback_message.feedback.sequence.push_back(0);
           feedback_publisher->publish(feedback_message);
-          client_executor.spin_once();
+          client_executor.spin_some();
           if (goal_request->goal.order > 0) {
             feedback_message.feedback.sequence.push_back(1);
             feedback_publisher->publish(feedback_message);
-            client_executor.spin_once();
+            client_executor.spin_some();
             for (size_t i = 1; i < static_cast<size_t>(goal_request->goal.order); ++i) {
               feedback_message.feedback.sequence.push_back(
                 feedback_message.feedback.sequence[i] +
                 feedback_message.feedback.sequence[i - 1]);
               feedback_publisher->publish(feedback_message);
-              client_executor.spin_once();
+              client_executor.spin_some();
             }
           }
           goal_status.status = rclcpp_action::GoalStatus::STATUS_SUCCEEDED;
           status_message.status_list[0] = goal_status;
           status_publisher->publish(status_message);
-          client_executor.spin_once();
+          client_executor.spin_some();
           response->result.sequence = feedback_message.feedback.sequence;
           response->status = rclcpp_action::GoalStatus::STATUS_SUCCEEDED;
           goals.erase(request->goal_id.uuid);
@@ -192,7 +192,7 @@ protected:
           }
         }
         status_publisher->publish(status_message);
-        client_executor.spin_once();
+        client_executor.spin_some();
       });
     ASSERT_TRUE(cancel_service != nullptr);
     allocator.deallocate(cancel_service_name, allocator.state);
@@ -556,6 +556,7 @@ TEST_F(TestClientAgainstServer, async_send_goal_with_feedback_callback_wait_for_
   dual_spin_until_future_complete(future_goal_handle);
   auto goal_handle = future_goal_handle.get();
   EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_ACCEPTED, goal_handle->get_status());
+  EXPECT_EQ(0, feedback_count);
   EXPECT_TRUE(goal_handle->is_feedback_aware());
   EXPECT_FALSE(goal_handle->is_result_aware());
   auto future_result = action_client->async_get_result(goal_handle);