Added support for services

rclcpp/include/rclcpp/callback_group.hpp

@@ -22,6 +22,7 @@
 
 #include <rclcpp/subscription.hpp>
 #include <rclcpp/timer.hpp>
+#include <rclcpp/service.hpp>
 
 namespace rclcpp
 {
@@ -62,9 +63,16 @@ private:
     timer_ptrs_.push_back(timer_ptr);
   }
 
+  void
+  add_service(const service::ServiceBase::SharedPtr &service_ptr)
+  {
+    service_ptrs_.push_back(service_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::atomic_bool can_be_taken_from_;
 
 };

rclcpp/include/rclcpp/client.hpp

@@ -0,0 +1,65 @@
+/* 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/macros.hpp>
+
+namespace rclcpp
+{
+
+// Forward declaration for friend statement
+namespace node {class Node;}
+
+namespace client
+{
+
+template<typename ServiceT>
+class Client
+{
+public:
+  RCLCPP_MAKE_SHARED_DEFINITIONS(Client);
+
+  Client(ros_middleware_interface::ClientHandle client_handle,
+         std::string& service_name)
+    : client_handle_(client_handle), service_name_(service_name)
+  {}
+
+  std::shared_ptr<typename ServiceT::Response>
+  send_request(std::shared_ptr<typename ServiceT::Request> &req)
+  {
+    ::ros_middleware_interface::send_request(client_handle_, req.get());
+
+    std::shared_ptr<typename ServiceT::Response> res = std::make_shared<typename ServiceT::Response>();
+    ::ros_middleware_interface::receive_response(client_handle_, res.get());
+    return res;
+  }
+
+private:
+  ros_middleware_interface::ClientHandle client_handle_;
+  std::string service_name_;
+
+};
+
+} /* namespace client */
+} /* namespace rclcpp */
+
+#endif /* RCLCPP_RCLCPP_CLIENT_HPP_ */

rclcpp/include/rclcpp/executor.hpp

@@ -106,6 +106,7 @@ protected:
     // Either the subscription or the timer will be set, but not both
     rclcpp::subscription::SubscriptionBase::SharedPtr subscription;
     rclcpp::timer::TimerBase::SharedPtr timer;
+    rclcpp::service::ServiceBase::SharedPtr service;
     // These are used to keep the scope on the containing items
     rclcpp::callback_group::CallbackGroup::SharedPtr callback_group;
     rclcpp::node::Node::SharedPtr node;
@@ -126,6 +127,10 @@ protected:
     {
       execute_subscription(any_exec->subscription);
     }
+    if (any_exec->service)
+    {
+      execute_service(any_exec->service);
+    }
     // Reset the callback_group, regardless of type
     any_exec->callback_group->can_be_taken_from_.store(true);
     // Wake the wait, because it may need to be recalculated or work that
@@ -161,6 +166,26 @@ protected:
     timer->callback_();
   }
 
+  static void
+  execute_service(
+    rclcpp::service::ServiceBase::SharedPtr &service)
+  {
+    std::shared_ptr<void> request = service->create_request();
+    bool taken = ros_middleware_interface::take_request(
+      service->service_handle_,
+      request.get());
+    if (taken)
+    {
+      service->handle_request(request);
+    }
+    else
+    {
+      std::cout << "[rclcpp::error] take failed for service on service: "
+                << service->get_service_name()
+                << std::endl;
+    }
+  }
+
 /*** Populate class storage from stored weak node pointers and wait. ***/
 
   void
@@ -170,6 +195,7 @@ protected:
     bool has_invalid_weak_nodes = false;
     std::vector<rclcpp::subscription::SubscriptionBase::SharedPtr> subs;
     std::vector<rclcpp::timer::TimerBase::SharedPtr> timers;
+    std::vector<rclcpp::service::ServiceBase::SharedPtr> services;
     for (auto &weak_node : weak_nodes_)
     {
       auto node = weak_node.lock();
@@ -193,6 +219,10 @@ protected:
         {
           timers.push_back(timer);
         }
+        for (auto &service : group->service_ptrs_)
+        {
+          services.push_back(service);
+        }
       }
     }
     // Clean up any invalid nodes, if they were detected
@@ -224,6 +254,28 @@ protected:
         subscription->subscription_handle_.data_;
       subscriber_handle_index += 1;
     }
+
+    // Use the number of services to allocate memory in the handles
+    size_t number_of_services = services.size();
+    ros_middleware_interface::ServiceHandles service_handles;
+    service_handles.service_count_ = number_of_services;
+    // TODO: Avoid redundant malloc's
+    service_handles.services_ = static_cast<void **>(
+      std::malloc(sizeof(void *) * number_of_services));
+    if (service_handles.services_ == NULL)
+    {
+      // TODO: Use a different error here? maybe std::bad_alloc?
+      throw std::runtime_error("Could not malloc for service pointers.");
+    }
+    // Then fill the ServiceHandles with ready services
+    size_t service_handle_index = 0;
+    for (auto &service : services)
+    {
+      service_handles.services_[service_handle_index] = \
+        service->service_handle_.data_;
+      service_handle_index += 1;
+    }
+
     // Use the number of guard conditions to allocate memory in the handles
     // Add 2 to the number for the ctrl-c guard cond and the executor's
     size_t start_of_timer_guard_conds = 2;
@@ -254,9 +306,11 @@ protected:
         timer->guard_condition_.data_;
       guard_cond_handle_index += 1;
     }
+
     // Now wait on the waitable subscriptions and timers
     ros_middleware_interface::wait(subscriber_handles,
                                    guard_condition_handles,
+                                   service_handles,
                                    nonblocking);
     // If ctrl-c guard condition, return directly
     if (guard_condition_handles.guard_conditions_[0] != 0)
@@ -264,6 +318,7 @@ protected:
       // Make sure to free memory
       // TODO: Remove theses when "Avoid redundant malloc's" todo is addressed
       std::free(subscriber_handles.subscribers_);
+      std::free(service_handles.services_);
       std::free(guard_condition_handles.guard_conditions_);
       return;
     }
@@ -286,9 +341,20 @@ protected:
         guard_condition_handles_.push_back(handle);
       }
     }
+    // Then the services
+    for (size_t i = 0; i < number_of_services; ++i)
+    {
+      void *handle = service_handles.services_[i];
+      if (handle)
+      {
+        service_handles_.push_back(handle);
+      }
+    }
+
     // Make sure to free memory
     // TODO: Remove theses when "Avoid redundant malloc's" todo is addressed
     std::free(subscriber_handles.subscribers_);
+    std::free(service_handles.services_);
     std::free(guard_condition_handles.guard_conditions_);
   }
 
@@ -352,6 +418,35 @@ protected:
     return rclcpp::timer::TimerBase::SharedPtr();
   }
 
+  rclcpp::service::ServiceBase::SharedPtr
+  get_service_by_handle(void * service_handle)
+  {
+    for (auto weak_node : weak_nodes_)
+    {
+      auto node = weak_node.lock();
+      if (!node)
+      {
+        continue;
+      }
+      for (auto weak_group : node->callback_groups_)
+      {
+        auto group = weak_group.lock();
+        if (!group)
+        {
+          continue;
+        }
+        for (auto service : group->service_ptrs_)
+        {
+          if (service->service_handle_.data_ == service_handle)
+          {
+            return service;
+          }
+        }
+      }
+    }
+    return rclcpp::service::ServiceBase::SharedPtr();
+  }
+
   void
   remove_subscriber_handle_from_subscriber_handles(void *handle)
   {
@@ -364,6 +459,12 @@ protected:
     guard_condition_handles_.remove(handle);
   }
 
+  void
+  remove_service_handle_from_service_handles(void *handle)
+  {
+    service_handles_.remove(handle);
+  }
+
   rclcpp::node::Node::SharedPtr
   get_node_by_group(rclcpp::callback_group::CallbackGroup::SharedPtr &group)
   {
@@ -516,6 +617,67 @@ protected:
     }
   }
 
+  rclcpp::callback_group::CallbackGroup::SharedPtr
+  get_group_by_service(
+    rclcpp::service::ServiceBase::SharedPtr &service)
+  {
+    for (auto &weak_node : weak_nodes_)
+    {
+      auto node = weak_node.lock();
+      if (!node)
+      {
+        continue;
+      }
+      for (auto &weak_group : node->callback_groups_)
+      {
+        auto group = weak_group.lock();
+        for (auto &serv : group->service_ptrs_)
+        {
+          if (serv == service)
+          {
+            return group;
+          }
+        }
+      }
+    }
+    return rclcpp::callback_group::CallbackGroup::SharedPtr();
+  }
+
+  void
+  get_next_service(std::shared_ptr<AnyExecutable> &any_exec)
+  {
+    for (auto handle : service_handles_)
+    {
+      auto service = get_service_by_handle(handle);
+      if (service)
+      {
+        // Find the group for this handle and see if it can be serviced
+        auto group = get_group_by_service(service);
+        if (!group)
+        {
+          // Group was not found, meaning the service is not valid...
+          // Remove it from the ready list and continue looking
+          remove_service_handle_from_service_handles(handle);
+          continue;
+        }
+        if (!group->can_be_taken_from_.load())
+        {
+          // Group is mutually exclusive and is being used, so skip it for now
+          // Leave it to be checked next time, but continue searching
+          continue;
+        }
+        // Otherwise it is safe to set and return the any_exec
+        any_exec->service = service;
+        any_exec->callback_group = group;
+        any_exec->node = get_node_by_group(group);
+        remove_service_handle_from_service_handles(handle);
+        return;
+      }
+      // Else, the service is no longer valid, remove it and continue
+      remove_service_handle_from_service_handles(handle);
+    }
+  }
+
   std::shared_ptr<AnyExecutable>
   get_next_ready_executable()
   {
@@ -532,6 +694,12 @@ protected:
     {
       return any_exec;
     }
+    // Check the services to see if there are any that are ready
+    get_next_service(any_exec);
+    if (any_exec->service)
+    {
+      return any_exec;
+    }
     // If there is neither a ready timer nor subscription, return a null ptr
     any_exec.reset();
     return any_exec;
@@ -580,6 +748,8 @@ private:
   SubscriberHandles subscriber_handles_;
   typedef std::list<void*> GuardConditionHandles;
   GuardConditionHandles guard_condition_handles_;
+  typedef std::list<void*> ServiceHandles;
+  ServiceHandles service_handles_;
 
 };
 

rclcpp/include/rclcpp/node.hpp

@@ -21,9 +21,11 @@
 #include <string>
 
 #include <rclcpp/callback_group.hpp>
+#include <rclcpp/client.hpp>
 #include <rclcpp/context.hpp>
 #include <rclcpp/macros.hpp>
 #include <rclcpp/publisher.hpp>
+#include <rclcpp/service.hpp>
 #include <rclcpp/subscription.hpp>
 #include <rclcpp/timer.hpp>
 
@@ -89,6 +91,20 @@ public:
   typedef std::weak_ptr<CallbackGroup> CallbackGroupWeakPtr;
   typedef std::list<CallbackGroupWeakPtr> CallbackGroupWeakPtrList;
 
+  /* Create and return a Client. */
+  template <typename ServiceT>
+  typename rclcpp::client::Client<ServiceT>::SharedPtr
+  create_client(std::string service_name);
+
+  /* Create and return a Service. */
+  template <typename ServiceT>
+  typename rclcpp::service::Service<ServiceT>::SharedPtr
+  create_service(
+    std::string service_name,
+    std::function<void(const std::shared_ptr<typename ServiceT::Request> &,
+                       std::shared_ptr<typename ServiceT::Response>&)> callback,
+    rclcpp::callback_group::CallbackGroup::SharedPtr group=nullptr);
+
 private:
   RCLCPP_DISABLE_COPY(Node);
 
@@ -106,6 +122,7 @@ private:
 
   size_t number_of_subscriptions_;
   size_t number_of_timers_;
+  size_t number_of_services_;
 
 };
 

rclcpp/include/rclcpp/node_impl.hpp

@@ -23,6 +23,7 @@
 #include <ros_middleware_interface/handles.h>
 
 #include <ros_middleware_interface/get_type_support_handle.h>
+#include <ros_middleware_interface/get_service_type_support_handle.h>
 
 #include <rclcpp/contexts/default_context.hpp>
 
@@ -41,7 +42,7 @@ Node::Node(std::string node_name)
 
 Node::Node(std::string node_name, context::Context::SharedPtr context)
   : name_(node_name), context_(context),
-    number_of_subscriptions_(0), number_of_timers_(0)
+    number_of_subscriptions_(0), number_of_timers_(0), number_of_services_(0)
 {
   node_handle_ = ::ros_middleware_interface::create_node();
   using rclcpp::callback_group::CallbackGroupType;
@@ -162,4 +163,65 @@ Node::create_wall_timer(
     group);
 }
 
+template<typename ServiceT>
+typename client::Client<ServiceT>::SharedPtr
+Node::create_client(std::string service_name)
+{
+  namespace rmi = ::ros_middleware_interface;
+
+  auto &service_type_support_handle = rmi::get_service_type_support_handle<ServiceT>();
+
+  auto client_handle = rmi::create_client(this->node_handle_,
+                                          service_type_support_handle,
+                                          service_name.c_str());
+
+  using namespace rclcpp::client;
+
+  auto cli = Client<ServiceT>::make_shared(client_handle,
+                                           service_name);
+
+  return cli;
+}
+
+template <typename ServiceT>
+typename service::Service<ServiceT>::SharedPtr
+Node::create_service(
+  std::string service_name,
+  std::function<void(const std::shared_ptr<typename ServiceT::Request> &,
+                     std::shared_ptr<typename ServiceT::Response>&)> callback,
+  rclcpp::callback_group::CallbackGroup::SharedPtr group)
+{
+  namespace rmi = ::ros_middleware_interface;
+
+  auto &service_type_support_handle = rmi::get_service_type_support_handle<ServiceT>();
+
+  auto service_handle = rmi::create_service(this->node_handle_,
+                                            service_type_support_handle,
+                                            service_name.c_str());
+
+  using namespace rclcpp::service;
+
+  auto serv = Service<ServiceT>::make_shared(service_handle,
+                                             service_name,
+                                             callback);
+  auto serv_base_ptr = std::dynamic_pointer_cast<ServiceBase>(serv);
+
+  if (group)
+  {
+    if (!group_in_node(group))
+    {
+      // TODO: use custom exception
+      throw std::runtime_error("Cannot create timer, group not in node.");
+    }
+    group->add_service(serv_base_ptr);
+  }
+  else
+  {
+    default_callback_group_->add_service(serv_base_ptr);
+  }
+  number_of_services_++;
+
+  return serv;
+}
+
 #endif /* RCLCPP_RCLCPP_NODE_IMPL_HPP_ */

rclcpp/include/rclcpp/service.hpp

@@ -0,0 +1,113 @@
+/* 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_SERVICE_HPP_
+#define RCLCPP_RCLCPP_SERVICE_HPP_
+
+#include <memory>
+
+#include <ros_middleware_interface/functions.h>
+#include <ros_middleware_interface/handles.h>
+
+#include <rclcpp/macros.hpp>
+
+namespace rclcpp
+{
+
+// Forward declaration for friend statement
+namespace node {class Node;}
+
+namespace service
+{
+
+class ServiceBase
+{
+  friend class rclcpp::executor::Executor;
+public:
+  RCLCPP_MAKE_SHARED_DEFINITIONS(ServiceBase);
+
+  ServiceBase(
+    ros_middleware_interface::ServiceHandle service_handle,
+    std::string &service_name)
+    : service_handle_(service_handle), service_name_(service_name)
+  {}
+
+  std::string get_service_name()
+  {
+    return this->service_name_;
+  }
+
+  ros_middleware_interface::ServiceHandle get_service_handle()
+  {
+    return this->service_handle_;
+  }
+
+  virtual std::shared_ptr<void> create_request() = 0;
+  virtual void handle_request(std::shared_ptr<void> &request) = 0;
+
+private:
+  RCLCPP_DISABLE_COPY(ServiceBase);
+
+  ros_middleware_interface::ServiceHandle service_handle_;
+  std::string service_name_;
+
+};
+
+template<typename ServiceT>
+class Service : public ServiceBase
+{
+public:
+  typedef std::function<
+    void(const std::shared_ptr<typename ServiceT::Request> &,
+         std::shared_ptr<typename ServiceT::Response>&)> CallbackType;
+  RCLCPP_MAKE_SHARED_DEFINITIONS(Service);
+
+  Service(
+    ros_middleware_interface::ServiceHandle service_handle,
+    std::string &service_name,
+    CallbackType callback)
+    : ServiceBase(service_handle, service_name), callback_(callback)
+  {}
+
+  std::shared_ptr<void> create_request()
+  {
+    return std::shared_ptr<void>(new typename ServiceT::Request());
+  }
+
+  void handle_request(std::shared_ptr<void> &request)
+  {
+    auto typed_request = std::static_pointer_cast<typename ServiceT::Request>(request);
+    auto response = std::shared_ptr<typename ServiceT::Response>(new typename ServiceT::Response);
+    callback_(typed_request, response);
+    send_response(typed_request, response);
+  }
+
+  void send_response(
+    std::shared_ptr<typename ServiceT::Request> &request,
+    std::shared_ptr<typename ServiceT::Response> &response)
+  {
+    ::ros_middleware_interface::send_response(get_service_handle(), request.get(), response.get());
+  }
+
+private:
+  RCLCPP_DISABLE_COPY(Service);
+
+  CallbackType callback_;
+};
+
+} /* namespace service */
+} /* namespace rclcpp */
+
+#endif /* RCLCPP_RCLCPP_SERVICE_HPP_ */