add wait_for_action_server() for action clients

wjwwood · wjwwood · commit c7c1738398ca · 2018-12-04T13:25:54.000-08:00
Signed-off-by: William Woodall &lt;william@osrfoundation.org&gt;
diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp
@@ -25,6 +25,7 @@
 #include <rosidl_typesupport_cpp/action_type_support.hpp>
 
 #include <algorithm>
+#include <chrono>
 #include <functional>
 #include <future>
 #include <map>
@@ -52,13 +53,30 @@ class ClientBase : public rclcpp::Waitable
   RCLCPP_ACTION_PUBLIC
   ClientBase(
     rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
+    rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph,
     const std::string & action_name,
     const rosidl_action_type_support_t * type_support,
     const rcl_action_client_options_t & options);
 
   RCLCPP_ACTION_PUBLIC
   virtual ~ClientBase();
 
+  /// Return true if there is an action server that is ready to take goal requests.
+  RCLCPP_ACTION_PUBLIC
+  bool
+  action_server_is_ready() const;
+
+  /// Wait for action_server_is_ready() to become true, or until the given timeout is reached.
+  template<typename RatioT = std::milli>
+  bool
+  wait_for_action_server(
+    std::chrono::duration<int64_t, RatioT> timeout = std::chrono::duration<int64_t, RatioT>(-1))
+  {
+    return wait_for_action_server_nanoseconds(
+      std::chrono::duration_cast<std::chrono::nanoseconds>(timeout)
+    );
+  }
+
   RCLCPP_ACTION_PUBLIC
   size_t
   get_number_of_ready_subscriptions() override;
@@ -92,6 +110,11 @@ class ClientBase : public rclcpp::Waitable
   execute() override;
 
 protected:
+  /// Wait for action_server_is_ready() to become true, or until the given timeout is reached.
+  RCLCPP_ACTION_PUBLIC
+  bool
+  wait_for_action_server_nanoseconds(std::chrono::nanoseconds timeout);
+
   using ResponseCallback =
     std::function<void(std::shared_ptr<void> response)>;
 
@@ -167,18 +190,20 @@ class Client : public ClientBase
 
   Client(
     rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
+    rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph,
     const std::string & action_name
   )
-  : Client(node_base, action_name, rcl_action_client_get_default_options())
+  : Client(node_base, node_graph, action_name, rcl_action_client_get_default_options())
   {
   }
 
   Client(
     rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
+    rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph,
     const std::string & action_name, const rcl_action_client_options_t & client_options
   )
   : ClientBase(
-      node_base, action_name,
+      node_base, node_graph, action_name,
       rosidl_typesupport_cpp::get_action_type_support_handle<ACTION>(),
       client_options)
   {
diff --git a/rclcpp_action/src/client.cpp b/rclcpp_action/src/client.cpp
@@ -31,10 +31,13 @@ class ClientBaseImpl
 public:
   ClientBaseImpl(
     rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
+    rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph,
     const std::string & action_name,
     const rosidl_action_type_support_t * type_support,
     const rcl_action_client_options_t & client_options)
-  : node_handle(node_base->get_shared_rcl_node_handle()),
+  : context_(node_base->get_context()),
+    node_graph_(node_graph),
+    node_handle(node_base->get_shared_rcl_node_handle()),
     logger(rclcpp::get_logger(rcl_node_get_logger_name(
       node_handle.get())).get_child("rclcpp")),
     random_bytes_generator(std::random_device{}())
@@ -93,6 +96,8 @@ class ClientBaseImpl
   bool is_cancel_response_ready{false};
   bool is_result_response_ready{false};
 
+  rclcpp::Context::SharedPtr context_;
+  rclcpp::node_interfaces::NodeGraphInterface::WeakPtr node_graph_;
   std::shared_ptr<rcl_action_client_t> client_handle{nullptr};
   std::shared_ptr<rcl_node_t> node_handle{nullptr};
   rclcpp::Logger logger;
@@ -115,11 +120,12 @@ class ClientBaseImpl
 
 ClientBase::ClientBase(
   rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
+  rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph,
   const std::string & action_name,
   const rosidl_action_type_support_t * type_support,
   const rcl_action_client_options_t & client_options)
     : pimpl_(new ClientBaseImpl(
-      node_base, action_name,
+      node_base, node_graph, action_name,
       type_support, client_options))
 {
 }
@@ -128,6 +134,79 @@ ClientBase::~ClientBase()
 {
 }
 
+bool
+ClientBase::action_server_is_ready() const
+{
+  bool is_ready;
+  rcl_ret_t ret = rcl_action_server_is_available(
+    this->pimpl_->node_handle.get(),
+    this->pimpl_->client_handle.get(),
+    &is_ready);
+  if (RCL_RET_NODE_INVALID == ret) {
+    const rcl_node_t * node_handle = this->pimpl_->node_handle.get();
+    if (node_handle && !rcl_context_is_valid(node_handle->context)) {
+      // context is shutdown, do a soft failure
+      return false;
+    }
+  }
+  if (ret != RCL_RET_OK) {
+    rclcpp::exceptions::throw_from_rcl_error(ret, "rcl_action_server_is_available failed");
+  }
+  return is_ready;
+}
+
+bool
+ClientBase::wait_for_action_server_nanoseconds(std::chrono::nanoseconds timeout)
+{
+  auto start = std::chrono::steady_clock::now();
+  // make an event to reuse, rather than create a new one each time
+  auto node_ptr = pimpl_->node_graph_.lock();
+  if (!node_ptr) {
+    throw rclcpp::exceptions::InvalidNodeError();
+  }
+  auto event = node_ptr->get_graph_event();
+  // check to see if the server is ready immediately
+  if (this->action_server_is_ready()) {
+    return true;
+  }
+  if (timeout == std::chrono::nanoseconds(0)) {
+    // check was non-blocking, return immediately
+    return false;
+  }
+  // update the time even on the first loop to account for time spent in the first call
+  // to this->server_is_ready()
+  std::chrono::nanoseconds time_to_wait = timeout - (std::chrono::steady_clock::now() - start);
+  if (timeout > std::chrono::nanoseconds(0) && time_to_wait < std::chrono::nanoseconds(0)) {
+    // Do not allow the time_to_wait to become negative when timeout was originally positive.
+    // Setting time_to_wait to 0 will allow one non-blocking wait because of the do-while.
+    time_to_wait = std::chrono::nanoseconds(0);
+  }
+  // continue forever if timeout is negative, otherwise continue until out of time_to_wait
+  do {
+    if (!rclcpp::ok(this->pimpl_->context_)) {
+      return false;
+    }
+    // Limit each wait to 100ms to workaround an issue specific to the Connext RMW implementation.
+    // A race condition means that graph changes for services becoming available may trigger the
+    // wait set to wake up, but then not be reported as ready immediately after the wake up
+    // (see https://github.com/ros2/rmw_connext/issues/201)
+    // If no other graph events occur, the wait set will not be triggered again until the timeout
+    // has been reached, despite the service being available, so we artificially limit the wait
+    // time to limit the delay.
+    node_ptr->wait_for_graph_change(
+      event, std::min(time_to_wait, std::chrono::nanoseconds(RCL_MS_TO_NS(100))));
+    // Because of the aforementioned race condition, we check if the service is ready even if the
+    // graph event wasn't triggered.
+    event->check_and_clear();
+    if (this->action_server_is_ready()) {
+      return true;
+    }
+    // server is not ready, loop if there is time left
+    time_to_wait = timeout - (std::chrono::steady_clock::now() - start);
+  } while (time_to_wait > std::chrono::nanoseconds(0) || timeout < std::chrono::nanoseconds(0));
+  return false;  // timeout exceeded while waiting for the server to be ready
+}
+
 rclcpp::Logger
 ClientBase::get_logger()
 {
