Adapts action client to new action type support layout.

ros2 · Feb 5, 2019 · b60d86f · b60d86f
1 parent c0a6b47
commit b60d86f
Show file tree

Hide file tree

Showing 3 changed files with 19 additions and 17 deletions.
diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp
@@ -287,7 +287,6 @@ class Client : public ClientBase
     auto promise = std::make_shared<std::promise<typename GoalHandle::SharedPtr>>();
     std::shared_future<typename GoalHandle::SharedPtr> future(promise->get_future());
     using GoalRequest = typename ActionT::GoalRequestService::Request;
-    // auto goal_request = std::make_shared<GoalRequest>();
     // goal_request->goal_id = this->generate_goal_id();
     // goal_request->goal = goal;
     auto goal_request = std::make_shared<GoalRequest>(goal);
@@ -429,9 +428,8 @@ class Client : public ClientBase
   std::shared_ptr<void>
   create_feedback_message() const override
   {
-    // using FeedbackMessage = typename ActionT::FeedbackMessage;
-    // return std::shared_ptr<void>(new FeedbackMessage());
-    return std::shared_ptr<void>(new Feedback());
+    using FeedbackMessage = typename ActionT::FeedbackMessage;
+    return std::shared_ptr<void>(new FeedbackMessage());
   }
 
   /// \internal
@@ -453,8 +451,9 @@ class Client : public ClientBase
       return;
     }
     typename GoalHandle::SharedPtr goal_handle = goal_handles_[goal_id];
-    // goal_handle->call_feedback_callback(goal_handle, feedback_message->feedback);
-    goal_handle->call_feedback_callback(goal_handle, feedback_message);
+    auto feedback = std::make_shared<Feedback>();
+    *feedback = feedback_message->feedback;
+    goal_handle->call_feedback_callback(goal_handle, feedback);
   }
 
   /// \internal
@@ -506,10 +505,12 @@ class Client : public ClientBase
       std::static_pointer_cast<void>(goal_result_request),
       [goal_handle, this](std::shared_ptr<void> response) mutable
       {
+        using GoalResultResponse = typename ActionT::GoalResultService::Response;
+        auto result_response = std::static_pointer_cast<GoalResultResponse>(response);
         // Wrap the response in a struct with the fields a user cares about
         Result result;
-        using GoalResultResponse = typename ActionT::GoalResultService::Response;
-        result.response = std::static_pointer_cast<GoalResultResponse>(response);
+        result.response = std::make_shared<typename ActionT::Result>();
+        *result.response = result_response->response;
         result.goal_id = goal_handle->get_goal_id();
         result.code = static_cast<ResultCode>(result.response->action_status);
         goal_handle->set_result(result);

diff --git a/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp b/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp
@@ -65,7 +65,8 @@ class ClientGoalHandle
   using Feedback = typename ActionT::Feedback;
   using FeedbackCallback =
     std::function<void (
-        typename ClientGoalHandle<ActionT>::SharedPtr, const std::shared_ptr<const Feedback>)>;
+        typename ClientGoalHandle<ActionT>::SharedPtr,
+        const std::shared_ptr<const Feedback>)>;
 
   virtual ~ClientGoalHandle();
 

diff --git a/rclcpp_action/test/test_client.cpp b/rclcpp_action/test/test_client.cpp
@@ -24,7 +24,6 @@
 #include <rclcpp/clock.hpp>
 #include <rclcpp/exceptions.hpp>
 #include <rclcpp/executors.hpp>
-#include <rclcpp/executors/multi_threaded_executor.hpp>
 #include <rclcpp/node.hpp>
 #include <rclcpp/publisher.hpp>
 #include <rclcpp/rclcpp.hpp>
@@ -63,7 +62,7 @@ class TestClient : public ::testing::Test
   using ActionCancelGoalRequest = ActionType::CancelGoalService::Request;
   using ActionCancelGoalResponse = ActionType::CancelGoalService::Response;
   using ActionStatusMessage = ActionType::GoalStatusMessage;
-  using ActionFeedbackMessage = ActionType::Feedback;
+  using ActionFeedbackMessage = ActionType::FeedbackMessage;
   using ActionFeedback = ActionType::Feedback;
 
   static void SetUpTestCase()
@@ -88,7 +87,7 @@ class TestClient : public ::testing::Test
         ActionGoalResponse::SharedPtr response)
       {
         response->stamp = clock.now();
-        response->accepted = (request->order >= 0);
+        response->accepted = (request->request.order >= 0);
         if (response->accepted) {
           goals[request->action_goal_id.uuid] = {request, response};
         }
@@ -122,13 +121,14 @@ class TestClient : public ::testing::Test
           feedback_message.sequence.push_back(0);
           feedback_publisher->publish(feedback_message);
           client_executor.spin_once();
-          if (goal_request->order > 0) {
-            feedback_message.sequence.push_back(1);
+          if (goal_request->request.order > 0) {
+            feedback_message.feedback.sequence.push_back(1);
             feedback_publisher->publish(feedback_message);
             client_executor.spin_once();
-            for (int i = 1; i < goal_request->order; ++i) {
-              feedback_message.sequence.push_back(
-                feedback_message.sequence[i] + feedback_message.sequence[i - 1]);
+            for (int i = 1; i < goal_request->request.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();
             }