Autostart parameter services

rclcpp/include/rclcpp/create_service.hpp

@@ -0,0 +1,58 @@
+// Copyright 2018 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__CREATE_SERVICE_HPP_
+#define RCLCPP__CREATE_SERVICE_HPP_
+
+#include <memory>
+#include <string>
+#include <utility>
+
+#include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/node_interfaces/node_services_interface.hpp"
+#include "rclcpp/visibility_control.hpp"
+#include "rmw/rmw.h"
+
+namespace rclcpp
+{
+
+/// Create a service with a given type.
+/// \internal
+template<typename ServiceT, typename CallbackT>
+typename rclcpp::Service<ServiceT>::SharedPtr
+create_service(
+  std::shared_ptr<node_interfaces::NodeBaseInterface> node_base,
+  std::shared_ptr<node_interfaces::NodeServicesInterface> node_services,
+  const std::string & service_name,
+  CallbackT && callback,
+  const rmw_qos_profile_t & qos_profile,
+  rclcpp::callback_group::CallbackGroup::SharedPtr 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 serv = Service<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<ServiceBase>(serv);
+  node_services->add_service(serv_base_ptr, group);
+  return serv;
+}
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__CREATE_SERVICE_HPP_

rclcpp/include/rclcpp/node_impl.hpp

@@ -39,6 +39,7 @@
 #include "rclcpp/intra_process_manager.hpp"
 #include "rclcpp/parameter.hpp"
 #include "rclcpp/create_publisher.hpp"
+#include "rclcpp/create_service.hpp"
 #include "rclcpp/create_subscription.hpp"
 #include "rclcpp/type_support_decl.hpp"
 #include "rclcpp/visibility_control.hpp"
@@ -184,18 +185,9 @@ Node::create_service(
   const rmw_qos_profile_t & qos_profile,
   rclcpp::callback_group::CallbackGroup::SharedPtr 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 serv = Service<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<ServiceBase>(serv);
-  node_services_->add_service(serv_base_ptr, group);
-  return serv;
+  return rclcpp::create_service<ServiceT, CallbackT>(
+    node_base_, node_services_,
+    service_name, std::forward<CallbackT>(callback), qos_profile, group);
 }
 
 template<typename CallbackT>

rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp

@@ -16,6 +16,7 @@
 #define RCLCPP__NODE_INTERFACES__NODE_PARAMETERS_HPP_
 
 #include <map>
+#include <memory>
 #include <string>
 #include <vector>
 
@@ -30,6 +31,7 @@
 #include "rclcpp/node_interfaces/node_services_interface.hpp"
 #include "rclcpp/node_interfaces/node_topics_interface.hpp"
 #include "rclcpp/parameter.hpp"
+#include "rclcpp/parameter_service.hpp"
 #include "rclcpp/publisher.hpp"
 #include "rclcpp/visibility_control.hpp"
 
@@ -46,7 +48,9 @@ class NodeParameters : public NodeParametersInterface
 
   RCLCPP_PUBLIC
   NodeParameters(
-    rclcpp::node_interfaces::NodeTopicsInterface * node_topics,
+    const node_interfaces::NodeBaseInterface::SharedPtr node_base,
+    const node_interfaces::NodeTopicsInterface::SharedPtr node_topics,
+    const node_interfaces::NodeServicesInterface::SharedPtr node_services,
     bool use_intra_process);
 
   RCLCPP_PUBLIC
@@ -105,15 +109,15 @@ class NodeParameters : public NodeParametersInterface
 private:
   RCLCPP_DISABLE_COPY(NodeParameters)
 
-  rclcpp::node_interfaces::NodeTopicsInterface * node_topics_;
-
   mutable std::mutex mutex_;
 
   ParametersCallbackFunction parameters_callback_ = nullptr;
 
   std::map<std::string, rclcpp::parameter::ParameterVariant> parameters_;
 
   Publisher<rcl_interfaces::msg::ParameterEvent>::SharedPtr events_publisher_;
+
+  std::shared_ptr<ParameterService> parameter_service_;
 };
 
 }  // namespace node_interfaces

rclcpp/include/rclcpp/parameter_service.hpp

@@ -15,6 +15,7 @@
 #ifndef RCLCPP__PARAMETER_SERVICE_HPP_
 #define RCLCPP__PARAMETER_SERVICE_HPP_
 
+#include <memory>
 #include <string>
 
 #include "rcl_interfaces/srv/describe_parameters.hpp"
@@ -40,11 +41,12 @@ class ParameterService
 
   RCLCPP_PUBLIC
   explicit ParameterService(
-    const rclcpp::Node::SharedPtr node,
+    const std::shared_ptr<node_interfaces::NodeBaseInterface> node_base,
+    const std::shared_ptr<node_interfaces::NodeServicesInterface> node_services,
+    node_interfaces::NodeParametersInterface * node_params,
     const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters);
 
 private:
-  const rclcpp::Node::SharedPtr node_;
   rclcpp::Service<rcl_interfaces::srv::GetParameters>::SharedPtr get_parameters_service_;
   rclcpp::Service<rcl_interfaces::srv::GetParameterTypes>::SharedPtr
     get_parameter_types_service_;

rclcpp/src/rclcpp/node.cpp

@@ -63,7 +63,9 @@ Node::Node(
   node_topics_(new rclcpp::node_interfaces::NodeTopics(node_base_.get())),
   node_services_(new rclcpp::node_interfaces::NodeServices(node_base_.get())),
   node_parameters_(new rclcpp::node_interfaces::NodeParameters(
-      node_topics_.get(),
+      node_base_,
+      node_topics_,
+      node_services_,
       use_intra_process_comms
     )),
   node_clock_(new rclcpp::node_interfaces::NodeClock(

rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp

@@ -28,9 +28,11 @@
 using rclcpp::node_interfaces::NodeParameters;
 
 NodeParameters::NodeParameters(
-  rclcpp::node_interfaces::NodeTopicsInterface * node_topics,
+  const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
+  const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics,
+  const rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services,
   bool use_intra_process)
-: node_topics_(node_topics)
+: parameter_service_(std::make_shared<ParameterService>(node_base, node_services, this))
 {
   using MessageT = rcl_interfaces::msg::ParameterEvent;
   using PublisherT = rclcpp::Publisher<MessageT>;
@@ -39,7 +41,7 @@ NodeParameters::NodeParameters(
   auto allocator = std::make_shared<AllocatorT>();
 
   events_publisher_ = rclcpp::create_publisher<MessageT, AllocatorT, PublisherT>(
-    node_topics_,
+    node_topics.get(),
     "parameter_events",
     rmw_qos_profile_parameter_events,
     use_intra_process,

rclcpp/src/rclcpp/parameter_service.cpp

@@ -24,120 +24,103 @@
 using rclcpp::ParameterService;
 
 ParameterService::ParameterService(
-  const rclcpp::Node::SharedPtr node,
+  const std::shared_ptr<rclcpp::node_interfaces::NodeBaseInterface> node_base,
+  const std::shared_ptr<rclcpp::node_interfaces::NodeServicesInterface> node_services,
+  rclcpp::node_interfaces::NodeParametersInterface * node_params,
   const rmw_qos_profile_t & qos_profile)
-: node_(node)
 {
-  std::weak_ptr<rclcpp::Node> captured_node = node_;
-  get_parameters_service_ = node_->create_service<rcl_interfaces::srv::GetParameters>(
-    std::string(node_->get_name()) + "/" + parameter_service_names::get_parameters,
-    [captured_node](
+  const std::string node_name = node_base->get_name();
+
+  get_parameters_service_ = create_service<rcl_interfaces::srv::GetParameters>(
+    node_base, node_services,
+    std::string(node_name) + "/" + parameter_service_names::get_parameters,
+    [node_params](
       const std::shared_ptr<rmw_request_id_t>,
       const std::shared_ptr<rcl_interfaces::srv::GetParameters::Request> request,
       std::shared_ptr<rcl_interfaces::srv::GetParameters::Response> response)
     {
-      auto node = captured_node.lock();
-      if (!node) {
-        return;
-      }
-      auto values = node->get_parameters(request->names);
+      auto values = node_params->get_parameters(request->names);
       for (auto & pvariant : values) {
         response->values.push_back(pvariant.get_parameter_value());
       }
     },
-    qos_profile);
+    qos_profile, nullptr);
 
-  get_parameter_types_service_ = node_->create_service<rcl_interfaces::srv::GetParameterTypes>(
-    std::string(node_->get_name()) + "/" + parameter_service_names::get_parameter_types,
-    [captured_node](
+  get_parameter_types_service_ = create_service<rcl_interfaces::srv::GetParameterTypes>(
+    node_base, node_services,
+    std::string(node_name) + "/" + parameter_service_names::get_parameter_types,
+    [node_params](
       const std::shared_ptr<rmw_request_id_t>,
       const std::shared_ptr<rcl_interfaces::srv::GetParameterTypes::Request> request,
       std::shared_ptr<rcl_interfaces::srv::GetParameterTypes::Response> response)
     {
-      auto node = captured_node.lock();
-      if (!node) {
-        return;
-      }
-      auto types = node->get_parameter_types(request->names);
+      auto types = node_params->get_parameter_types(request->names);
       std::transform(types.cbegin(), types.cend(),
       std::back_inserter(response->types), [](const uint8_t & type) {
         return static_cast<rclcpp::parameter::ParameterType>(type);
       });
     },
-    qos_profile);
+    qos_profile, nullptr);
 
-  set_parameters_service_ = node_->create_service<rcl_interfaces::srv::SetParameters>(
-    std::string(node_->get_name()) + "/" + parameter_service_names::set_parameters,
-    [captured_node](
+  set_parameters_service_ = create_service<rcl_interfaces::srv::SetParameters>(
+    node_base, node_services,
+    std::string(node_name) + "/" + parameter_service_names::set_parameters,
+    [node_params](
       const std::shared_ptr<rmw_request_id_t>,
       const std::shared_ptr<rcl_interfaces::srv::SetParameters::Request> request,
       std::shared_ptr<rcl_interfaces::srv::SetParameters::Response> response)
     {
-      auto node = captured_node.lock();
-      if (!node) {
-        return;
-      }
       std::vector<rclcpp::parameter::ParameterVariant> pvariants;
       for (auto & p : request->parameters) {
         pvariants.push_back(rclcpp::parameter::ParameterVariant::from_parameter(p));
       }
-      auto results = node->set_parameters(pvariants);
+      auto results = node_params->set_parameters(pvariants);
       response->results = results;
     },
-    qos_profile);
+    qos_profile, nullptr);
 
-  set_parameters_atomically_service_ =
-    node_->create_service<rcl_interfaces::srv::SetParametersAtomically>(
-    std::string(node_->get_name()) + "/" + parameter_service_names::set_parameters_atomically,
-    [captured_node](
+  set_parameters_atomically_service_ = create_service<rcl_interfaces::srv::SetParametersAtomically>(
+    node_base, node_services,
+    std::string(node_name) + "/" + parameter_service_names::set_parameters_atomically,
+    [node_params](
       const std::shared_ptr<rmw_request_id_t>,
       const std::shared_ptr<rcl_interfaces::srv::SetParametersAtomically::Request> request,
       std::shared_ptr<rcl_interfaces::srv::SetParametersAtomically::Response> response)
     {
-      auto node = captured_node.lock();
-      if (!node) {
-        return;
-      }
       std::vector<rclcpp::parameter::ParameterVariant> pvariants;
       std::transform(request->parameters.cbegin(), request->parameters.cend(),
       std::back_inserter(pvariants),
       [](const rcl_interfaces::msg::Parameter & p) {
         return rclcpp::parameter::ParameterVariant::from_parameter(p);
       });
-      auto result = node->set_parameters_atomically(pvariants);
+      auto result = node_params->set_parameters_atomically(pvariants);
       response->result = result;
     },
-    qos_profile);
+    qos_profile, nullptr);
 
-  describe_parameters_service_ = node_->create_service<rcl_interfaces::srv::DescribeParameters>(
-    std::string(node_->get_name()) + "/" + parameter_service_names::describe_parameters,
-    [captured_node](
+  describe_parameters_service_ = create_service<rcl_interfaces::srv::DescribeParameters>(
+    node_base, node_services,
+    std::string(node_name) + "/" + parameter_service_names::describe_parameters,
+    [node_params](
       const std::shared_ptr<rmw_request_id_t>,
       const std::shared_ptr<rcl_interfaces::srv::DescribeParameters::Request> request,
       std::shared_ptr<rcl_interfaces::srv::DescribeParameters::Response> response)
     {
-      auto node = captured_node.lock();
-      if (!node) {
-        return;
-      }
-      auto descriptors = node->describe_parameters(request->names);
+      auto descriptors = node_params->describe_parameters(request->names);
       response->descriptors = descriptors;
     },
-    qos_profile);
+    qos_profile, nullptr);
 
-  list_parameters_service_ = node_->create_service<rcl_interfaces::srv::ListParameters>(
-    std::string(node_->get_name()) + "/" + parameter_service_names::list_parameters,
-    [captured_node](
+  list_parameters_service_ = create_service<rcl_interfaces::srv::ListParameters>(
+    node_base, node_services,
+    std::string(node_name) + "/" + parameter_service_names::list_parameters,
+    [node_params](
       const std::shared_ptr<rmw_request_id_t>,
       const std::shared_ptr<rcl_interfaces::srv::ListParameters::Request> request,
       std::shared_ptr<rcl_interfaces::srv::ListParameters::Response> response)
     {
-      auto node = captured_node.lock();
-      if (!node) {
-        return;
-      }
-      auto result = node->list_parameters(request->prefixes, request->depth);
+      auto result = node_params->list_parameters(request->prefixes, request->depth);
       response->result = result;
     },
-    qos_profile);
+    qos_profile, nullptr);
 }

rclcpp/test/test_time_source.cpp

@@ -334,7 +334,6 @@ TEST_F(TestTimeSource, parameter_activation) {
   ts.attachClock(ros_clock);
   EXPECT_FALSE(ros_clock->ros_time_is_active());
 
-  auto parameter_service = std::make_shared<rclcpp::ParameterService>(node);
   auto parameters_client = std::make_shared<rclcpp::SyncParametersClient>(node);
 
   using namespace std::chrono_literals;

rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp

@@ -23,6 +23,7 @@
 #include "rclcpp/intra_process_manager.hpp"
 #include "rclcpp/parameter.hpp"
 #include "rclcpp/create_publisher.hpp"
+#include "rclcpp/create_service.hpp"
 #include "rclcpp/create_subscription.hpp"
 #include "rclcpp/type_support_decl.hpp"
 
@@ -176,18 +177,9 @@ LifecycleNode::create_service(
   const rmw_qos_profile_t & qos_profile,
   rclcpp::callback_group::CallbackGroup::SharedPtr 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 serv = rclcpp::Service<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>(serv);
-  node_services_->add_service(serv_base_ptr, group);
-  return serv;
+  return rclcpp::create_service<ServiceT, CallbackT>(
+    node_base_, node_services_,
+    service_name, std::forward<CallbackT>(callback), qos_profile, group);
 }
 
 template<typename ParameterT>