ros2 · huchijwk · Dec 5, 2021 · Dec 9, 2021 · Dec 9, 2021 · Dec 9, 2021
diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp
@@ -121,6 +121,17 @@ class ServiceBase
   bool
   exchange_in_use_by_wait_set_state(bool in_use_state);
 
+  /// Add the Service to a wait set.
+  /**
+   * \param[in] wait_set A handle to the wait set to add the Service to.
+   * \return `true` if the Service is added successfully, `false` otherwise.
+   * \throws rclcpp::execptions::RCLError from rcl_wait_set_add_*()
+   */
+  RCLCPP_PUBLIC
+  virtual
+  bool
+  add_to_wait_set(rcl_wait_set_t * wait_set) = 0;
 rcl_ret_t ret = rcl_wait_set_add_service( 
   &rcl_wait_set_, 
   service_ptr_pair.second->get_service_handle().get(), 
   nullptr); 
 rcl_ret_t ret = rcl_wait_set_add_service( 
   &rcl_wait_set_, 
   service_ptr_pair.second->get_service_handle().get(), 
   nullptr); 
+
 protected:
   RCLCPP_DISABLE_COPY(ServiceBase)
 
@@ -353,6 +364,18 @@ class Service
     }
   }
 
+  bool
+  add_to_wait_set(rcl_wait_set_t * wait_set) override
+  {
+    if (rcl_wait_set_add_service(wait_set, get_service_handle().get(), NULL) != RCL_RET_OK) {
+      RCLCPP_ERROR(
+        rclcpp::get_node_logger(node_handle_.get()).get_child("rclcpp"),
+        "Couldn't add service to wait set: %s", rcl_get_error_string().str);
+      return false;
+    }
+    return true;
+  }
+
 private:
   RCLCPP_DISABLE_COPY(Service)
 

diff --git a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp
@@ -171,7 +171,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy
         });
       group->find_service_ptrs_if(
         [this](const rclcpp::ServiceBase::SharedPtr & service) {
-          service_handles_.push_back(service->get_service_handle());
+          service_handles_.push_back(service);
           return false;
         });
       group->find_client_ptrs_if(
@@ -223,10 +223,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy
     }
 
     for (auto service : service_handles_) {
-      if (rcl_wait_set_add_service(wait_set, service.get(), NULL) != RCL_RET_OK) {
-        RCUTILS_LOG_ERROR_NAMED(
-          "rclcpp",
-          "Couldn't add service to wait set: %s", rcl_get_error_string().str);
+      if (!service->add_to_wait_set(wait_set)) {
         return false;
       }
     }
@@ -292,7 +289,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy
   {
     auto it = service_handles_.begin();
     while (it != service_handles_.end()) {
-      auto service = get_service_by_handle(*it, weak_groups_to_nodes);
+      auto service = *it;
       if (service) {
         // Find the group for this handle and see if it can be serviced
         auto group = get_group_by_service(service, weak_groups_to_nodes);
@@ -502,7 +499,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy
   VectorRebind<const rclcpp::GuardCondition *> guard_conditions_;
 
   VectorRebind<std::shared_ptr<const rcl_subscription_t>> subscription_handles_;
-  VectorRebind<std::shared_ptr<const rcl_service_t>> service_handles_;
+  VectorRebind<std::shared_ptr<ServiceBase>> service_handles_;
   VectorRebind<std::shared_ptr<const rcl_client_t>> client_handles_;
   VectorRebind<std::shared_ptr<const rcl_timer_t>> timer_handles_;
   VectorRebind<std::shared_ptr<Waitable>> waitable_handles_;

diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_services.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_services.cpp
@@ -35,6 +35,7 @@ class TestService : public rclcpp::ServiceBase
   std::shared_ptr<void> create_request() override {return nullptr;}
   std::shared_ptr<rmw_request_id_t> create_request_header() override {return nullptr;}
   void handle_request(std::shared_ptr<rmw_request_id_t>, std::shared_ptr<void>) override {}
+  bool add_to_wait_set(rcl_wait_set_t *) override {return true;}
 };
 
 class TestClient : public rclcpp::ClientBase

diff --git a/rclcpp_lifecycle/CMakeLists.txt b/rclcpp_lifecycle/CMakeLists.txt
@@ -152,6 +152,15 @@ if(BUILD_TESTING)
       PUBLIC RCUTILS_ENABLE_FAULT_INJECTION
     )
   endif()
+  ament_add_gmock(test_lifecycle_service test/test_lifecycle_service.cpp)
+  if(TARGET test_lifecycle_service)
+    ament_target_dependencies(test_lifecycle_service
+      "rcl_lifecycle"
+      "rclcpp"
+      "test_msgs"
+    )
+    target_link_libraries(test_lifecycle_service ${PROJECT_NAME})
+  endif()
 endif()
 
 # specific order: dependents before dependencies

diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp
@@ -85,6 +85,7 @@
 
 #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
 #include "rclcpp_lifecycle/lifecycle_publisher.hpp"
+#include "rclcpp_lifecycle/lifecycle_service.hpp"
 #include "rclcpp_lifecycle/state.hpp"
 #include "rclcpp_lifecycle/transition.hpp"
 #include "rclcpp_lifecycle/visibility_control.h"
@@ -268,10 +269,14 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface,
 
   /// Create and return a Service.
   /**
-   * \sa rclcpp::Node::create_service
+   * \param[in] service_name The topic to service on.
+   * \param[in] callback User-defined callback function.
+   * \param[in] qos_profile rmw_qos_profile_t Quality of service profile for client.
+   * \param[in] group Callback group to call the service.
+   * \return Shared pointer to the created lifecycle service.
    */
   template<typename ServiceT, typename CallbackT>
-  typename rclcpp::Service<ServiceT>::SharedPtr
+  typename rclcpp_lifecycle::LifecycleService<ServiceT>::SharedPtr
   create_service(
     const std::string & service_name,
     CallbackT && callback,

diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp
@@ -34,6 +34,7 @@
 #include "rclcpp/type_support_decl.hpp"
 
 #include "lifecycle_publisher.hpp"
+#include "lifecycle_service.hpp"
 #include "rclcpp_lifecycle/visibility_control.h"
 
 #include "rclcpp_lifecycle/lifecycle_node.hpp"
@@ -119,16 +120,24 @@ LifecycleNode::create_client(
 }
 
 template<typename ServiceT, typename CallbackT>
-typename rclcpp::Service<ServiceT>::SharedPtr
+typename rclcpp_lifecycle::LifecycleService<ServiceT>::SharedPtr
 LifecycleNode::create_service(
   const std::string & service_name,
   CallbackT && callback,
   const rmw_qos_profile_t & qos_profile,
   rclcpp::CallbackGroup::SharedPtr group)
 {
-  return rclcpp::create_service<ServiceT, CallbackT>(
-    node_base_, node_services_,
-    service_name, std::forward<CallbackT>(callback), qos_profile, group);
+  rclcpp::AnyServiceCallback<ServiceT> any_service_callback;
+  any_service_callback.set(std::forward<CallbackT>(callback));
+
+  rcl_service_options_t service_options = rcl_service_get_default_options();
+  service_options.qos = qos_profile;
+
+  auto service = LifecycleService<ServiceT>::make_shared(
+    node_base_->get_shared_rcl_node_handle(), service_name, any_service_callback, service_options);
+  auto serv_base_ptr = std::dynamic_pointer_cast<rclcpp::ServiceBase>(service);
+  node_services_->add_service(serv_base_ptr, group);
+  return service;
 using PublisherT = rclcpp_lifecycle::LifecyclePublisher<MessageT, AllocatorT>; 
 return rclcpp::create_publisher<MessageT, AllocatorT, PublisherT>( 
   *this, 
   topic_name, 
   qos, 
   options); 
 std::shared_ptr<PublisherT> 
 create_publisher( 
 typename rclcpp::Service<ServiceT>::SharedPtr 
 create_service( 
 using PublisherT = rclcpp_lifecycle::LifecyclePublisher<MessageT, AllocatorT>; 
 return rclcpp::create_publisher<MessageT, AllocatorT, PublisherT>( 
   *this, 
   topic_name, 
   qos, 
   options); 
 std::shared_ptr<PublisherT> 
 create_publisher( 
 typename rclcpp::Service<ServiceT>::SharedPtr 
 create_service( 
 }
 
 template<typename AllocatorT>

diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_service.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_service.hpp
@@ -0,0 +1,86 @@
+// Copyright 2021 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_LIFECYCLE__LIFECYCLE_SERVICE_HPP_
+#define RCLCPP_LIFECYCLE__LIFECYCLE_SERVICE_HPP_
+
+#include <memory>
+#include <string>
+
+#include "rclcpp/any_service_callback.hpp"
+#include "rclcpp/service.hpp"
+
+namespace rclcpp_lifecycle
+{
+/// base class with only
+/**
+ * pure virtual functions. A managed
+ * node can then deactivate or activate
+ * the service handling.
+ * It is more a convenient interface class
+ * than a necessary base class.
+ */
+class LifecycleServiceInterface
+{
+public:
+  virtual ~LifecycleServiceInterface() {}
+  virtual void on_activate() = 0;
+  virtual void on_deactivate() = 0;
+  virtual bool is_activated() = 0;
+};
+
+/// brief child class of rclcpp Service class.
+/**
+ * Overrides all service functions to check for enabled/disabled state.
+ */
+template<typename ServiceT>
+class LifecycleService : public LifecycleServiceInterface, public rclcpp::Service<ServiceT>
+{
+public:
+  RCLCPP_SMART_PTR_DEFINITIONS(LifecycleService)
+
+  using rclcpp::Service<ServiceT>::Service;
+
+  virtual ~LifecycleService() {}
+
+  /// add to wait set
+  /**
+   * The function checks whether the communication
+   * was enabled or disabled and forwards the add_to_wait_set
+   * request to the actual rclcpp Service base class
+   */
+  bool add_to_wait_set(rcl_wait_set_t * wait_set) override
+  {
+    if (!enabled_) {
+      // The memory strategy only expects false in case of "failure",
+      // so it returns true even if no service is added.
+      return true;
+    }
+
+    return rclcpp::Service<ServiceT>::add_to_wait_set(wait_set);
+  }
+
+  void on_activate() override {enabled_ = true;}
+
+  void on_deactivate() override {enabled_ = false;}
+
+  bool is_activated() override {return enabled_;}
+
+private:
+  bool enabled_ = false;
+};
+
+}  // namespace rclcpp_lifecycle
+
+#endif  // RCLCPP_LIFECYCLE__LIFECYCLE_SERVICE_HPP_
diff --git a/rclcpp_lifecycle/test/test_lifecycle_service.cpp b/rclcpp_lifecycle/test/test_lifecycle_service.cpp
@@ -0,0 +1,87 @@
+// Copyright 2021 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.
+
+#include <gmock/gmock.h>
+#include <gtest/gtest.h>
+
+#include <chrono>
+#include <memory>
+#include <string>
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_lifecycle/lifecycle_node.hpp"
+#include "rclcpp_lifecycle/lifecycle_service.hpp"
+#include "test_msgs/srv/empty.hpp"
+
+using namespace std::literals::chrono_literals;
+
+class TestLifecycleService : public ::testing::Test
+{
+public:
+  void SetUp()
+  {
+    rclcpp::init(0, nullptr);
+    node_ = rclcpp_lifecycle::LifecycleNode::make_shared("node");
+    service_ = node_->create_service<test_msgs::srv::Empty>(
+      std::string("service"), request_callback_.AsStdFunction());
+    client_ = node_->create_client<test_msgs::srv::Empty>(std::string("service"));
+  }
+
+  void TearDown() {rclcpp::shutdown();}
+
+protected:
+  rclcpp_lifecycle::LifecycleNode::SharedPtr node_;
+  rclcpp_lifecycle::LifecycleService<test_msgs::srv::Empty>::SharedPtr service_;
+  rclcpp::Client<test_msgs::srv::Empty>::SharedPtr client_;
+
+  ::testing::MockFunction<void(
+      const test_msgs::srv::Empty::Request::SharedPtr, test_msgs::srv::Empty::Response::SharedPtr)>
+  request_callback_;
+};
+
+TEST_F(TestLifecycleService, request_on_active_service)
+{
+  service_->on_activate();
+  EXPECT_TRUE(service_->is_activated());
+
+  ::testing::MockFunction<void(std::shared_future<std::shared_ptr<test_msgs::srv::Empty_Response>>)>
+  response_callback;
+
+  auto request = std::make_shared<test_msgs::srv::Empty::Request>();
+  client_->wait_for_service();
+  auto future = client_->async_send_request(request, response_callback.AsStdFunction());
+
+  EXPECT_CALL(request_callback_, Call(::testing::_, ::testing::_)).WillOnce(::testing::Return());
+  EXPECT_CALL(response_callback, Call(::testing::_)).WillOnce(::testing::Return());
+
+  rclcpp::spin_until_future_complete(node_->get_node_base_interface(), future, 1s);
+}
+
+TEST_F(TestLifecycleService, request_on_inactive_service)
+{
+  service_->on_deactivate();
+  EXPECT_FALSE(service_->is_activated());
+
+  ::testing::MockFunction<void(std::shared_future<std::shared_ptr<test_msgs::srv::Empty_Response>>)>
+  response_callback;
+
+  auto request = std::make_shared<test_msgs::srv::Empty::Request>();
+  client_->wait_for_service();
+  auto future = client_->async_send_request(request, response_callback.AsStdFunction());
+
+  EXPECT_CALL(request_callback_, Call(::testing::_, ::testing::_)).Times(0);
+  EXPECT_CALL(response_callback, Call(::testing::_)).Times(0);
+
+  rclcpp::spin_until_future_complete(node_->get_node_base_interface(), future, 1s);
+}