Implement generic client

rclcpp/CMakeLists.txt

@@ -45,6 +45,7 @@ set(${PROJECT_NAME}_SRCS
   src/rclcpp/clock.cpp
   src/rclcpp/context.cpp
   src/rclcpp/contexts/default_context.cpp
+  src/rclcpp/create_generic_client.cpp
   src/rclcpp/detail/add_guard_condition_to_rcl_wait_set.cpp
   src/rclcpp/detail/resolve_parameter_overrides.cpp
   src/rclcpp/detail/rmw_implementation_specific_payload.cpp
@@ -73,6 +74,7 @@ set(${PROJECT_NAME}_SRCS
   src/rclcpp/experimental/executors/events_executor/events_executor.cpp
   src/rclcpp/experimental/timers_manager.cpp
   src/rclcpp/future_return_code.cpp
+  src/rclcpp/generic_client.cpp
   src/rclcpp/generic_publisher.cpp
   src/rclcpp/generic_subscription.cpp
   src/rclcpp/graph_listener.cpp

rclcpp/include/rclcpp/create_generic_client.hpp

@@ -0,0 +1,55 @@
+// Copyright 2023 Sony Group Corporation.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__CREATE_GENERIC_CLIENT_HPP_
+#define RCLCPP__CREATE_GENERIC_CLIENT_HPP_
+
+#include <memory>
+#include <string>
+
+#include "rclcpp/generic_client.hpp"
+#include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/node_interfaces/node_services_interface.hpp"
+#include "rclcpp/qos.hpp"
+#include "rmw/rmw.h"
+
+namespace rclcpp
+{
+/// Create a generic service client with a given type.
+/**
+ * \param[in] node_base NodeBaseInterface implementation of the node on which
+ *  to create the client.
+ * \param[in] node_graph NodeGraphInterface implementation of the node on which
+ *  to create the client.
+ * \param[in] node_services NodeServicesInterface implementation of the node on
+ *  which to create the client.
+ * \param[in] service_name The name on which the service is accessible.
+ * \param[in] service_type The name of service type, e.g. "test_msgs/srv/BasicTypes"
+ * \param[in] qos Quality of service profile for client.
+ * \param[in] group Callback group to handle the reply to service calls.
+ * \return Shared pointer to the created client.
+ */
+rclcpp::GenericClient::SharedPtr
+create_generic_client(
+  std::shared_ptr<node_interfaces::NodeBaseInterface> node_base,
+  std::shared_ptr<node_interfaces::NodeGraphInterface> node_graph,
+  std::shared_ptr<node_interfaces::NodeServicesInterface> node_services,
+  const std::string & service_name,
+  const std::string & service_type,
+  const rclcpp::QoS & qos = rclcpp::ServicesQoS(),
+  rclcpp::CallbackGroup::SharedPtr group = nullptr);
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__CREATE_GENERIC_CLIENT_HPP_

rclcpp/include/rclcpp/generic_client.hpp

@@ -0,0 +1,201 @@
+// Copyright 2023 Sony Group Corporation.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__GENERIC_CLIENT_HPP_
+#define RCLCPP__GENERIC_CLIENT_HPP_
+
+#include <map>
+#include <memory>
+#include <string>
+#include <vector>
+#include <utility>
+
+#include "rclcpp/client.hpp"
+#include "rclcpp/serialized_message.hpp"
+#include "rcpputils/shared_library.hpp"
+
+#include "rosidl_typesupport_introspection_cpp/message_introspection.hpp"
+
+namespace rclcpp
+{
+class GenericClient : public ClientBase
+{
+public:
+  using Request = void *;
+  using Response = void *;
+
+  using SharedResponse = std::shared_ptr<void>;
+
+  using Promise = std::promise<SharedResponse>;
+  using SharedPromise = std::shared_ptr<Promise>;
+
+  using Future = std::future<SharedResponse>;
+  using SharedFuture = std::shared_future<SharedResponse>;
+
+  RCLCPP_SMART_PTR_DEFINITIONS(GenericClient)
+
+  /// A convenient Client::Future and request id pair.
+  /**
+   * Public members:
+   * - future: a std::future<void *>.
+   * - request_id: the request id associated with the future.
+   *
+   * All the other methods are equivalent to the ones std::future provides.
+   */
+  struct FutureAndRequestId
+    : detail::FutureAndRequestId<Future>
+  {
+    using detail::FutureAndRequestId<Future>::FutureAndRequestId;
+
+    /// See std::future::share().
+    SharedFuture share() noexcept {return this->future.share();}
+
+    /// Move constructor.
+    FutureAndRequestId(FutureAndRequestId && other) noexcept = default;
+    /// Deleted copy constructor, each instance is a unique owner of the future.
+    FutureAndRequestId(const FutureAndRequestId & other) = delete;
+    /// Move assignment.
+    FutureAndRequestId & operator=(FutureAndRequestId && other) noexcept = default;
+    /// Deleted copy assignment, each instance is a unique owner of the future.
+    FutureAndRequestId & operator=(const FutureAndRequestId & other) = delete;
+    /// Destructor.
+    ~FutureAndRequestId() = default;
+  };
+
+  GenericClient(
+    rclcpp::node_interfaces::NodeBaseInterface * node_base,
+    rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph,
+    const std::string & service_name,
+    const std::string & service_type,
+    rcl_client_options_t & client_options);
+
+  SharedResponse
+  create_response() override;
+
+  std::shared_ptr<rmw_request_id_t>
+  create_request_header() override;
+
+  void
+  handle_response(
+    std::shared_ptr<rmw_request_id_t> request_header,
+    std::shared_ptr<void> response) override;
+
+  /// Send a request to the service server.
+  /**
+   * This method returns a `FutureAndRequestId` instance
+   * that can be passed to Executor::spin_until_future_complete() to
+   * wait until it has been completed.
+   *
+   * If the future never completes,
+   * e.g. the call to Executor::spin_until_future_complete() times out,
+   * Client::remove_pending_request() must be called to clean the client internal state.
+   * Not doing so will make the `Client` instance to use more memory each time a response is not
+   * received from the service server.
+   *
+   * ```cpp
+   * auto future = client->async_send_request(my_request);
+   * if (
+   *   rclcpp::FutureReturnCode::TIMEOUT ==
+   *   executor->spin_until_future_complete(future, timeout))
+   * {
+   *   client->remove_pending_request(future);
+   *   // handle timeout
+   * } else {
+   *   handle_response(future.get());
+   * }
+   * ```
+   *
+   * \param[in] request request to be send.
+   * \return a FutureAndRequestId instance.
+   */
+  FutureAndRequestId
+  async_send_request(const Request request);
+
+  /// Clean all pending requests older than a time_point.
+  /**
+   * \param[in] time_point Requests that were sent before this point are going to be removed.
+   * \param[inout] pruned_requests Removed requests id will be pushed to the vector
+   *  if a pointer is provided.
+   * \return number of pending requests that were removed.
+   */
+  template<typename AllocatorT = std::allocator<int64_t>>
+  size_t
+  prune_requests_older_than(
+    std::chrono::time_point<std::chrono::system_clock> time_point,
+    std::vector<int64_t, AllocatorT> * pruned_requests = nullptr)
+  {
+    std::lock_guard guard(pending_requests_mutex_);
+    auto old_size = pending_requests_.size();
+    for (auto it = pending_requests_.begin(), last = pending_requests_.end(); it != last; ) {
+      if (it->second.first < time_point) {
+        if (pruned_requests) {
+          pruned_requests->push_back(it->first);
+        }
+        it = pending_requests_.erase(it);
+      } else {
+        ++it;
+      }
+    }
+    return old_size - pending_requests_.size();
+  }
+
+  size_t
+  prune_pending_requests();
+
+  bool
+  remove_pending_request(int64_t request_id);
+
+  /// Take the next response for this client.
+  /**
+   * \sa ClientBase::take_type_erased_response().
+   *
+   * \param[out] response_out The reference to a Service Response into
+   *   which the middleware will copy the response being taken.
+   * \param[out] request_header_out The request header to be filled by the
+   *   middleware when taking, and which can be used to associte the response
+   *   to a specific request.
+   * \returns true if the response was taken, otherwise false.
+   * \throws rclcpp::exceptions::RCLError based exceptions if the underlying
+   *   rcl function fail.
+   */
+  bool
+  take_response(Response response_out, rmw_request_id_t & request_header_out)
+  {
+    return this->take_type_erased_response(&response_out, request_header_out);
+  }
+
+protected:
+  using CallbackInfoVariant = std::variant<
+    std::promise<SharedResponse>>;  // Use variant for extension
+
+  int64_t
+  async_send_request_impl(const Request request, CallbackInfoVariant value);
+
+  std::optional<CallbackInfoVariant>
+  get_and_erase_pending_request(int64_t request_number);
+
+  RCLCPP_DISABLE_COPY(GenericClient)
+
+  std::map<int64_t, std::pair<
+      std::chrono::time_point<std::chrono::system_clock>,
+      CallbackInfoVariant>> pending_requests_;
+  std::mutex pending_requests_mutex_;
+
+private:
+  std::shared_ptr<rcpputils::SharedLibrary> ts_lib_;
+  const rosidl_typesupport_introspection_cpp::MessageMembers * response_members_;
+};
+}  // namespace rclcpp
+
+#endif  // RCLCPP__GENERIC_CLIENT_HPP_

rclcpp/include/rclcpp/node.hpp

@@ -41,6 +41,7 @@
 #include "rclcpp/client.hpp"
 #include "rclcpp/clock.hpp"
 #include "rclcpp/context.hpp"
+#include "rclcpp/create_generic_client.hpp"
 #include "rclcpp/event.hpp"
 #include "rclcpp/generic_publisher.hpp"
 #include "rclcpp/generic_subscription.hpp"
@@ -320,6 +321,21 @@ class Node : public std::enable_shared_from_this<Node>
     const rclcpp::QoS & qos = rclcpp::ServicesQoS(),
     rclcpp::CallbackGroup::SharedPtr group = nullptr);
 
+  /// Create and return a GenericClient.
+  /**
+   * \param[in] service_name The name on which the service is accessible.
+   * \param[in] service_type The name of service type, e.g. "std_srvs/srv/SetBool"
+   * \param[in] qos Quality of service profile for client.
+   * \param[in] group Callback group to handle the reply to service calls.
+   * \return Shared pointer to the created GenericClient.
+   */
+  rclcpp::GenericClient::SharedPtr
+  create_generic_client(
+    const std::string & service_name,
+    const std::string & service_type,
+    const rclcpp::QoS & qos = rclcpp::ServicesQoS(),
+    rclcpp::CallbackGroup::SharedPtr group = nullptr);
+
   /// Create and return a GenericPublisher.
   /**
    * The returned pointer will never be empty, but this function can throw various exceptions, for

rclcpp/src/rclcpp/create_generic_client.cpp

@@ -0,0 +1,44 @@
+// Copyright 2023 Sony Group Corporation.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "rclcpp/create_generic_client.hpp"
+#include "rclcpp/generic_client.hpp"
+
+namespace rclcpp
+{
+rclcpp::GenericClient::SharedPtr
+create_generic_client(
+  std::shared_ptr<node_interfaces::NodeBaseInterface> node_base,
+  std::shared_ptr<node_interfaces::NodeGraphInterface> node_graph,
+  std::shared_ptr<node_interfaces::NodeServicesInterface> node_services,
+  const std::string & service_name,
+  const std::string & service_type,
+  const rclcpp::QoS & qos,
+  rclcpp::CallbackGroup::SharedPtr group)
+{
+  rcl_client_options_t options = rcl_client_get_default_options();
+  options.qos = qos.get_rmw_qos_profile();
+
+  auto cli = rclcpp::GenericClient::make_shared(
+    node_base.get(),
+    node_graph,
+    service_name,
+    service_type,
+    options);
+
+  auto cli_base_ptr = std::dynamic_pointer_cast<rclcpp::ClientBase>(cli);
+  node_services->add_client(cli_base_ptr, group);
+  return cli;
+}
+}  // namespace rclcpp