Throw from Client::async_get_result if the goal handle was invalidated due to a failed result request

jacobperron · jacobperron · commit 9cb17766a66e · 2020-09-15T11:37:39.000-07:00
Propagate error message from a failed result request.

Also set result awareness to false if the result request fails so the user can also check before
being hit with an exception.

Signed-off-by: Jacob Perron &lt;jacob@openrobotics.org&gt;
diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp
@@ -407,7 +407,7 @@ class Client : public ClientBase
   /// Asynchronously get the result for an active goal.
   /**
    * \throws exceptions::UnknownGoalHandleError If the goal unknown or already reached a terminal
-   *   state.
+   *   state, or if there was an error requesting the result.
    * \param[in] goal_handle The goal handle for which to get the result.
    * \param[in] result_callback Optional callback that is called when the result is received.
    * \return A future that is set to the goal result when the goal is finished.
@@ -421,6 +421,11 @@ class Client : public ClientBase
     if (goal_handles_.count(goal_handle->get_goal_id()) == 0) {
       throw exceptions::UnknownGoalHandleError();
     }
+    if (goal_handle->is_invalidated()) {
+      // This case can happen if there was a failure to send the result request
+      // during the goal response callback
+      throw goal_handle->invalidate_exception_;
+    }
     if (result_callback) {
       // This will override any previously registered callback
       goal_handle->set_result_callback(result_callback);
@@ -511,7 +516,7 @@ class Client : public ClientBase
     while (it != goal_handles_.end()) {
       typename GoalHandle::SharedPtr goal_handle = it->second.lock();
       if (goal_handle) {
-        goal_handle->invalidate();
+        goal_handle->invalidate(exceptions::UnawareGoalHandleError());
       }
       it = goal_handles_.erase(it);
     }
@@ -642,9 +647,9 @@ class Client : public ClientBase
           std::lock_guard<std::mutex> lock(goal_handles_mutex_);
           goal_handles_.erase(goal_handle->get_goal_id());
         });
-    } catch (rclcpp::exceptions::RCLError &) {
+    } catch (rclcpp::exceptions::RCLError & ex) {
       // This will cause an exception when the user tries to access the result
-      goal_handle->invalidate();
+      goal_handle->invalidate(exceptions::UnawareGoalHandleError(ex.message));
     }
   }
 
diff --git a/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp b/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp
@@ -26,6 +26,7 @@
 #include <memory>
 #include <mutex>
 
+#include "rclcpp_action/exceptions.hpp"
 #include "rclcpp_action/types.hpp"
 #include "rclcpp_action/visibility_control.hpp"
 
@@ -145,10 +146,15 @@ class ClientGoalHandle
   set_result(const WrappedResult & wrapped_result);
 
   void
-  invalidate();
+  invalidate(const exceptions::UnawareGoalHandleError & ex);
+
+  bool
+  is_invalidated() const;
 
   GoalInfo info_;
 
+  std::exception_ptr invalidate_exception_{nullptr};
+
   bool is_result_aware_{false};
   std::promise<WrappedResult> result_promise_;
   std::shared_future<WrappedResult> result_future_;
diff --git a/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp b/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp
@@ -138,11 +138,20 @@ ClientGoalHandle<ActionT>::set_result_awareness(bool awareness)
 
 template<typename ActionT>
 void
-ClientGoalHandle<ActionT>::invalidate()
+ClientGoalHandle<ActionT>::invalidate(const exceptions::UnawareGoalHandleError & ex)
 {
   std::lock_guard<std::mutex> guard(handle_mutex_);
+  is_result_aware_ = false;
+  invalidate_exception_ = std::make_exception_ptr(ex);
   status_ = GoalStatus::STATUS_UNKNOWN;
-  result_promise_.set_exception(std::make_exception_ptr(exceptions::UnawareGoalHandleError()));
+  result_promise_.set_exception(invalidate_exception_);
+}
+
+template<typename ActionT>
+bool
+ClientGoalHandle<ActionT>::is_invalidated() const
+{
+  return invalidate_exception_ != nullptr;
 }
 
 template<typename ActionT>
diff --git a/rclcpp_action/include/rclcpp_action/exceptions.hpp b/rclcpp_action/include/rclcpp_action/exceptions.hpp
@@ -16,6 +16,7 @@
 #define RCLCPP_ACTION__EXCEPTIONS_HPP_
 
 #include <stdexcept>
+#include <string>
 
 namespace rclcpp_action
 {
@@ -33,8 +34,9 @@ class UnknownGoalHandleError : public std::invalid_argument
 class UnawareGoalHandleError : public std::runtime_error
 {
 public:
-  UnawareGoalHandleError()
-  : std::runtime_error("Goal handle is not tracking the goal result.")
+  UnawareGoalHandleError(
+    const std::string & message = "Goal handle is not tracking the goal result.")
+  : std::runtime_error(message)
   {
   }
 };

Original file line number	Diff line number	Diff line change
`@@ -16,6 +16,7 @@`
`16`	`16`	`#define RCLCPP_ACTION__EXCEPTIONS_HPP_`
`17`	`17`
`18`	`18`	`#include <stdexcept>`
	`19`	`+#include <string>`
`19`	`20`
`20`	`21`	`namespace rclcpp_action`
`21`	`22`	`{`
`@@ -33,8 +34,9 @@ class UnknownGoalHandleError : public std::invalid_argument`
`33`	`34`	`class UnawareGoalHandleError : public std::runtime_error`
`34`	`35`	`{`
`35`	`36`	`public:`
`36`		`- UnawareGoalHandleError()`
`37`		`- : std::runtime_error("Goal handle is not tracking the goal result.")`
	`37`	`+ UnawareGoalHandleError(`
	`38`	`+ const std::string & message = "Goal handle is not tracking the goal result.")`
	`39`	`+ : std::runtime_error(message)`
`38`	`40`	`{`
`39`	`41`	`}`
`40`	`42`	`};`