Added support for services

rclcpp/include/rclcpp/callback_group.hpp

@@ -22,6 +22,8 @@
 
 #include <rclcpp/subscription.hpp>
 #include <rclcpp/timer.hpp>
+#include <rclcpp/service.hpp>
+#include <rclcpp/client.hpp>
 
 namespace rclcpp
 {
@@ -62,9 +64,23 @@ class CallbackGroup
     timer_ptrs_.push_back(timer_ptr);
   }
 
+  void
+  add_service(const service::ServiceBase::SharedPtr &service_ptr)
+  {
+    service_ptrs_.push_back(service_ptr);
+  }
+
+  void
+  add_client(const client::ClientBase::SharedPtr &client_ptr)
+  {
+    client_ptrs_.push_back(client_ptr);
+  }
+
   CallbackGroupType type_;
   std::vector<subscription::SubscriptionBase::SharedPtr> subscription_ptrs_;
   std::vector<timer::TimerBase::SharedPtr> timer_ptrs_;
+  std::vector<service::ServiceBase::SharedPtr> service_ptrs_;
+  std::vector<client::ClientBase::SharedPtr> client_ptrs_;
   std::atomic_bool can_be_taken_from_;
 
 };

rclcpp/include/rclcpp/client.hpp

@@ -0,0 +1,149 @@
+/* Copyright 2014 Open Source Robotics Foundation, Inc.
+ *
+ * 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_RCLCPP_CLIENT_HPP_
+#define RCLCPP_RCLCPP_CLIENT_HPP_
+
+#include <memory>
+
+#include <ros_middleware_interface/functions.h>
+#include <ros_middleware_interface/handles.h>
+
+#include <rclcpp/utilities.hpp>
+#include <rclcpp/macros.hpp>
+#include <future>
+#include <map>
+#include <utility>
+
+namespace rclcpp
+{
+
+// Forward declaration for friend statement
+namespace node {class Node;}
+
+namespace client
+{
+
+class ClientBase
+{
+  friend class rclcpp::executor::Executor;
+public:
+  RCLCPP_MAKE_SHARED_DEFINITIONS(ClientBase);
+
+  ClientBase(
+    ros_middleware_interface::ClientHandle client_handle,
+    std::string &service_name)
+    : client_handle_(client_handle), service_name_(service_name)
+  {}
+
+  std::string get_service_name()
+  {
+    return this->service_name_;
+  }
+
+  ros_middleware_interface::ClientHandle get_client_handle()
+  {
+    return this->client_handle_;
+  }
+
+  virtual std::shared_ptr<void> create_response() = 0;
+  virtual std::shared_ptr<void> create_request_header() = 0;
+  virtual void handle_response(std::shared_ptr<void> &response, std::shared_ptr<void> &req_id) = 0;
+
+private:
+  RCLCPP_DISABLE_COPY(ClientBase);
+
+  ros_middleware_interface::ClientHandle client_handle_;
+  std::string service_name_;
+
+};
+
+template<typename ServiceT>
+class Client : public ClientBase
+{
+public:
+  typedef std::promise<typename ServiceT::Response::Ptr> Promise;
+  typedef std::shared_ptr<Promise> SharedPromise;
+  typedef std::shared_future<typename ServiceT::Response::Ptr> SharedFuture;
+
+  typedef std::function<void(SharedFuture)> CallbackType;
+
+  RCLCPP_MAKE_SHARED_DEFINITIONS(Client);
+
+  Client(ros_middleware_interface::ClientHandle client_handle,
+         std::string& service_name)
+    : ClientBase(client_handle, service_name)
+  {}
+
+  std::shared_ptr<void> get_response(int64_t sequence_number)
+  {
+    auto pair = this->pending_requests_[sequence_number];
+    return pair.second;
+  }
+
+  std::shared_ptr<void> create_response()
+  {
+    return std::shared_ptr<void>(new typename ServiceT::Response());
+  }
+
+  std::shared_ptr<void> create_request_header()
+  {
+    return std::shared_ptr<void>(new ros_middleware_interface::RequestId());
+  }
+
+  void handle_response(std::shared_ptr<void> &response, std::shared_ptr<void> &req_id)
+  {
+    auto typed_req_id = std::static_pointer_cast<ros_middleware_interface::RequestId>(req_id);
+    auto typed_response = std::static_pointer_cast<typename ServiceT::Response>(response);
+    int64_t sequence_number = typed_req_id->sequence_number;
+    auto tuple = this->pending_requests_[sequence_number];
+    auto call_promise = std::get<0>(tuple);
+    auto callback = std::get<1>(tuple);
+    auto future = std::get<2>(tuple);
+    this->pending_requests_.erase(sequence_number);
+    call_promise->set_value(typed_response);
+    callback(future);
+  }
+
+  SharedFuture async_send_request(
+    typename ServiceT::Request::Ptr &request,
+    typename ServiceT::Response::Ptr &response)
+  {
+    return async_send_request(request, response, [] (SharedFuture f) { });
+  }
+
+  SharedFuture async_send_request(
+    typename ServiceT::Request::Ptr &request,
+    typename ServiceT::Response::Ptr &response,
+    CallbackType cb)
+  {
+    int64_t sequence_number = ::ros_middleware_interface::send_request(get_client_handle(), request.get());
+
+    SharedPromise call_promise = std::make_shared<Promise>();
+    SharedFuture f(call_promise->get_future());
+    pending_requests_[sequence_number] = std::make_tuple(call_promise, cb, f);
+    return f;
+  }
+
+private:
+  RCLCPP_DISABLE_COPY(Client);
+
+  std::map<int64_t, std::tuple<SharedPromise, CallbackType, SharedFuture> > pending_requests_;
+};
+
+} /* namespace client */
+} /* namespace rclcpp */
+
+#endif /* RCLCPP_RCLCPP_CLIENT_HPP_ */