add wait_for_action_server() for action clients (#598)

* add wait_for_action_server() for action clients

Signed-off-by: William Woodall <william@osrfoundation.org>

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

* Fix uncrustify errors.

* Ignore take failure on services for connext

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 @@ public:
   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 @@ protected:
   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 @@ public:
 
   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)
   {

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

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);