add wait_for_action_server() for action clients (#598)

* add wait_for_action_server() for action clients

Signed-off-by: William Woodall <[email protected]>

* Handle negative timeouts in wait_for_service() and wait_for_action_server() methods.

* Fix uncrustify errors.

* Ignore take failure on services for connext

Author: wjwwood

Diff for: .gitignore

@@ -0,0 +1 @@
+.DS_Store

Diff for: rclcpp/src/rclcpp/client.cpp

@@ -129,13 +129,15 @@ ClientBase::wait_for_service_nanoseconds(std::chrono::nanoseconds timeout)
   }
   // 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)) {
+  std::chrono::nanoseconds time_to_wait =
+    timeout > std::chrono::nanoseconds(0) ?
+    timeout - (std::chrono::steady_clock::now() - start) :
+    std::chrono::nanoseconds::max();
+  if (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->context_)) {
       return false;
@@ -156,8 +158,11 @@ ClientBase::wait_for_service_nanoseconds(std::chrono::nanoseconds timeout)
       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));
+    if (timeout > std::chrono::nanoseconds(0)) {
+      time_to_wait = timeout - (std::chrono::steady_clock::now() - start);
+    }
+    // if timeout is negative, time_to_wait will never reach zero
+  } while (time_to_wait > std::chrono::nanoseconds(0));
   return false;  // timeout exceeded while waiting for the server to be ready
 }
 

Diff for: rclcpp_action/include/rclcpp_action/client.hpp

@@ -17,6 +17,8 @@
 
 #include <rclcpp/macros.hpp>
 #include <rclcpp/node_interfaces/node_base_interface.hpp>
+#include <rclcpp/node_interfaces/node_logging_interface.hpp>
+#include <rclcpp/node_interfaces/node_graph_interface.hpp>
 #include <rclcpp/logger.hpp>
 #include <rclcpp/time.hpp>
 #include <rclcpp/waitable.hpp>
@@ -25,6 +27,7 @@
 #include <rosidl_typesupport_cpp/action_type_support.hpp>
 
 #include <algorithm>
+#include <chrono>
 #include <functional>
 #include <future>
 #include <map>
@@ -57,6 +60,22 @@ class ClientBase : public rclcpp::Waitable
   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)
+    );
+  }
+
   // -------------
   // Waitables API
 
@@ -107,11 +126,17 @@ class ClientBase : public rclcpp::Waitable
   RCLCPP_ACTION_PUBLIC
   ClientBase(
     rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
+    rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph,
     rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging,
     const std::string & action_name,
     const rosidl_action_type_support_t * type_support,
     const rcl_action_client_options_t & options);
 
+  /// 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);
+
   // -----------------------------------------------------
   // API for communication between ClientBase and Client<>
   using ResponseCallback = std::function<void (std::shared_ptr<void> response)>;
@@ -242,12 +267,13 @@ class Client : public ClientBase
 
   Client(
     rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
+    rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph,
     rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging,
     const std::string & action_name,
     const rcl_action_client_options_t client_options = rcl_action_client_get_default_options()
   )
   : ClientBase(
-      node_base, node_logging, action_name,
+      node_base, node_graph, node_logging, action_name,
       rosidl_typesupport_cpp::get_action_type_support_handle<ActionT>(),
       client_options)
   {

Diff for: rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp

@@ -19,6 +19,8 @@
 
 #include <memory>
 
+#include "rclcpp/logging.hpp"
+#include "rclcpp_action/client_goal_handle.hpp"
 #include "rclcpp_action/exceptions.hpp"
 
 namespace rclcpp_action

Diff for: rclcpp_action/include/rclcpp_action/create_client.hpp

@@ -63,7 +63,11 @@ create_client(
     };
 
   std::shared_ptr<Client<ActionT>> action_client(
-    new Client<ActionT>(node->get_node_base_interface(), node->get_node_logging_interface(), name),
+    new Client<ActionT>(
+      node->get_node_base_interface(),
+      node->get_node_graph_interface(),
+      node->get_node_logging_interface(),
+      name),
     deleter);
 
   node->get_node_waitables_interface()->add_waitable(action_client, group);

Diff for: rclcpp_action/src/client.cpp

@@ -34,11 +34,13 @@ class ClientBaseImpl
 public:
   ClientBaseImpl(
     rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
+    rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph,
     rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging,
     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()),
+  : node_graph_(node_graph),
+    node_handle(node_base->get_shared_rcl_node_handle()),
     logger(node_logging->get_logger().get_child("rclcpp_acton")),
     random_bytes_generator(std::random_device{} ())
   {
@@ -96,6 +98,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;
@@ -117,18 +121,98 @@ class ClientBaseImpl
 
 ClientBase::ClientBase(
   rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
+  rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph,
   rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging,
   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, node_logging, action_name, type_support, client_options))
+: pimpl_(new ClientBaseImpl(
+      node_base, node_graph, node_logging, action_name, type_support, client_options))
 {
 }
 
 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::nanoseconds(0) ?
+    timeout - (std::chrono::steady_clock::now() - start) :
+    std::chrono::nanoseconds::max();
+  if (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);
+  }
+  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
+    if (timeout > std::chrono::nanoseconds(0)) {
+      time_to_wait = timeout - (std::chrono::steady_clock::now() - start);
+    }
+    // if timeout is negative, time_to_wait will never reach zero
+  } while (time_to_wait > std::chrono::nanoseconds(0));
+  return false;  // timeout exceeded while waiting for the server to be ready
+}
+
 rclcpp::Logger
 ClientBase::get_logger()
 {

Diff for: rclcpp_action/src/server.cpp

@@ -217,11 +217,17 @@ ServerBase::execute_goal_request_received()
     &request_header,
     message.get());
 
-  if (RCL_RET_OK != ret) {
+  pimpl_->goal_request_ready_ = false;
+
+  if (RCL_RET_ACTION_SERVER_TAKE_FAILED == ret) {
+    // Ignore take failure because connext fails if it receives a sample without valid data.
+    // This happens when a client shuts down and connext receives a sample saying the client is
+    // no longer alive.
+    return;
+  } else if (RCL_RET_OK != ret) {
     rclcpp::exceptions::throw_from_rcl_error(ret);
   }
 
-  pimpl_->goal_request_ready_ = false;
   GoalID uuid = get_goal_id_from_goal_request(message.get());
   convert(uuid, &goal_info);
 
@@ -295,7 +301,14 @@ ServerBase::execute_cancel_request_received()
     &request_header,
     request.get());
 
-  if (RCL_RET_OK != ret) {
+  pimpl_->cancel_request_ready_ = false;
+
+  if (RCL_RET_ACTION_SERVER_TAKE_FAILED == ret) {
+    // Ignore take failure because connext fails if it receives a sample without valid data.
+    // This happens when a client shuts down and connext receives a sample saying the client is
+    // no longer alive.
+    return;
+  } else if (RCL_RET_OK != ret) {
     rclcpp::exceptions::throw_from_rcl_error(ret);
   }
 
@@ -361,7 +374,14 @@ ServerBase::execute_result_request_received()
   ret = rcl_action_take_result_request(
     pimpl_->action_server_.get(), &request_header, result_request.get());
 
-  if (RCL_RET_OK != ret) {
+  pimpl_->result_request_ready_ = false;
+
+  if (RCL_RET_ACTION_SERVER_TAKE_FAILED == ret) {
+    // Ignore take failure because connext fails if it receives a sample without valid data.
+    // This happens when a client shuts down and connext receives a sample saying the client is
+    // no longer alive.
+    return;
+  } else if (RCL_RET_OK != ret) {
     rclcpp::exceptions::throw_from_rcl_error(ret);
   }