diff --git a/rclcpp/include/rclcpp/create_service.hpp b/rclcpp/include/rclcpp/create_service.hpp new file mode 100644 index 0000000..ac78462 --- /dev/null +++ b/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 +#include +#include + +#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 rclcpp::Service::SharedPtr +create_service( + std::shared_ptr node_base, + std::shared_ptr node_services, + const std::string & service_name, + CallbackT && callback, + const rmw_qos_profile_t & qos_profile, + rclcpp::callback_group::CallbackGroup::SharedPtr group) +{ + rclcpp::AnyServiceCallback any_service_callback; + any_service_callback.set(std::forward(callback)); + + rcl_service_options_t service_options = rcl_service_get_default_options(); + service_options.qos = qos_profile; + + auto serv = Service::make_shared( + node_base->get_shared_rcl_node_handle(), + service_name, any_service_callback, service_options); + auto serv_base_ptr = std::dynamic_pointer_cast(serv); + node_services->add_service(serv_base_ptr, group); + return serv; +} + +} // namespace rclcpp + +#endif // RCLCPP__CREATE_SERVICE_HPP_ diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index c44d879..37f9b05 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -97,7 +97,8 @@ public: rclcpp::Context::SharedPtr context, const std::vector & arguments, bool use_global_arguments = true, - bool use_intra_process_comms = false); + bool use_intra_process_comms = false, + bool start_parameter_services = true); RCLCPP_PUBLIC virtual ~Node(); diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index 3b35505..25d6a68 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/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 any_service_callback; - any_service_callback.set(std::forward(callback)); - - rcl_service_options_t service_options = rcl_service_get_default_options(); - service_options.qos = qos_profile; - - auto serv = Service::make_shared( - node_base_->get_shared_rcl_node_handle(), - service_name, any_service_callback, service_options); - auto serv_base_ptr = std::dynamic_pointer_cast(serv); - node_services_->add_service(serv_base_ptr, group); - return serv; + return rclcpp::create_service( + node_base_, node_services_, + service_name, std::forward(callback), qos_profile, group); } template diff --git a/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp b/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp index 0649e24..6020bda 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp @@ -16,6 +16,7 @@ #define RCLCPP__NODE_INTERFACES__NODE_PARAMETERS_HPP_ #include +#include #include #include @@ -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,8 +48,11 @@ public: RCLCPP_PUBLIC NodeParameters( - rclcpp::node_interfaces::NodeTopicsInterface * node_topics, - bool use_intra_process); + 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, + bool start_parameter_services); RCLCPP_PUBLIC virtual @@ -105,8 +110,6 @@ public: private: RCLCPP_DISABLE_COPY(NodeParameters) - rclcpp::node_interfaces::NodeTopicsInterface * node_topics_; - mutable std::mutex mutex_; ParametersCallbackFunction parameters_callback_ = nullptr; @@ -114,6 +117,8 @@ private: std::map parameters_; Publisher::SharedPtr events_publisher_; + + std::shared_ptr parameter_service_; }; } // namespace node_interfaces diff --git a/rclcpp/include/rclcpp/parameter_service.hpp b/rclcpp/include/rclcpp/parameter_service.hpp index 6970cbc..cf5c27f 100644 --- a/rclcpp/include/rclcpp/parameter_service.hpp +++ b/rclcpp/include/rclcpp/parameter_service.hpp @@ -15,6 +15,7 @@ #ifndef RCLCPP__PARAMETER_SERVICE_HPP_ #define RCLCPP__PARAMETER_SERVICE_HPP_ +#include #include #include "rcl_interfaces/srv/describe_parameters.hpp" @@ -40,11 +41,12 @@ public: RCLCPP_PUBLIC explicit ParameterService( - const rclcpp::Node::SharedPtr node, + const std::shared_ptr node_base, + const std::shared_ptr 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::SharedPtr get_parameters_service_; rclcpp::Service::SharedPtr get_parameter_types_service_; diff --git a/rclcpp/src/rclcpp/node.cpp b/rclcpp/src/rclcpp/node.cpp index ae82aa8..5f798f4 100644 --- a/rclcpp/src/rclcpp/node.cpp +++ b/rclcpp/src/rclcpp/node.cpp @@ -45,7 +45,8 @@ Node::Node( rclcpp::contexts::default_context::get_global_default_context(), {}, true, - use_intra_process_comms) + use_intra_process_comms, + true) {} Node::Node( @@ -54,7 +55,8 @@ Node::Node( rclcpp::Context::SharedPtr context, const std::vector & arguments, bool use_global_arguments, - bool use_intra_process_comms) + bool use_intra_process_comms, + bool start_parameter_services) : node_base_(new rclcpp::node_interfaces::NodeBase( node_name, namespace_, context, arguments, use_global_arguments)), node_graph_(new rclcpp::node_interfaces::NodeGraph(node_base_.get())), @@ -63,8 +65,11 @@ 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(), - use_intra_process_comms + node_base_, + node_topics_, + node_services_, + use_intra_process_comms, + start_parameter_services )), node_clock_(new rclcpp::node_interfaces::NodeClock( node_base_, diff --git a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp index c58d106..2fd4605 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp +++ b/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, - bool use_intra_process) -: node_topics_(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, + bool start_parameter_services) { using MessageT = rcl_interfaces::msg::ParameterEvent; using PublisherT = rclcpp::Publisher; @@ -38,8 +40,12 @@ NodeParameters::NodeParameters( // TODO(wjwwood): expose this allocator through the Parameter interface. auto allocator = std::make_shared(); + if (start_parameter_services) { + parameter_service_ = std::make_shared(node_base, node_services, this); + } + events_publisher_ = rclcpp::create_publisher( - node_topics_, + node_topics.get(), "parameter_events", rmw_qos_profile_parameter_events, use_intra_process, diff --git a/rclcpp/src/rclcpp/parameter_service.cpp b/rclcpp/src/rclcpp/parameter_service.cpp index 86ca433..97a268c 100644 --- a/rclcpp/src/rclcpp/parameter_service.cpp +++ b/rclcpp/src/rclcpp/parameter_service.cpp @@ -24,120 +24,103 @@ using rclcpp::ParameterService; ParameterService::ParameterService( - const rclcpp::Node::SharedPtr node, + const std::shared_ptr node_base, + const std::shared_ptr node_services, + rclcpp::node_interfaces::NodeParametersInterface * node_params, const rmw_qos_profile_t & qos_profile) -: node_(node) { - std::weak_ptr captured_node = node_; - get_parameters_service_ = node_->create_service( - 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( + node_base, node_services, + node_name + "/" + parameter_service_names::get_parameters, + [node_params]( const std::shared_ptr, const std::shared_ptr request, std::shared_ptr 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( - std::string(node_->get_name()) + "/" + parameter_service_names::get_parameter_types, - [captured_node]( + get_parameter_types_service_ = create_service( + node_base, node_services, + node_name + "/" + parameter_service_names::get_parameter_types, + [node_params]( const std::shared_ptr, const std::shared_ptr request, std::shared_ptr 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(type); }); }, - qos_profile); + qos_profile, nullptr); - set_parameters_service_ = node_->create_service( - std::string(node_->get_name()) + "/" + parameter_service_names::set_parameters, - [captured_node]( + set_parameters_service_ = create_service( + node_base, node_services, + node_name + "/" + parameter_service_names::set_parameters, + [node_params]( const std::shared_ptr, const std::shared_ptr request, std::shared_ptr response) { - auto node = captured_node.lock(); - if (!node) { - return; - } std::vector 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( - std::string(node_->get_name()) + "/" + parameter_service_names::set_parameters_atomically, - [captured_node]( + set_parameters_atomically_service_ = create_service( + node_base, node_services, + node_name + "/" + parameter_service_names::set_parameters_atomically, + [node_params]( const std::shared_ptr, const std::shared_ptr request, std::shared_ptr response) { - auto node = captured_node.lock(); - if (!node) { - return; - } std::vector 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( - std::string(node_->get_name()) + "/" + parameter_service_names::describe_parameters, - [captured_node]( + describe_parameters_service_ = create_service( + node_base, node_services, + node_name + "/" + parameter_service_names::describe_parameters, + [node_params]( const std::shared_ptr, const std::shared_ptr request, std::shared_ptr 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( - std::string(node_->get_name()) + "/" + parameter_service_names::list_parameters, - [captured_node]( + list_parameters_service_ = create_service( + node_base, node_services, + node_name + "/" + parameter_service_names::list_parameters, + [node_params]( const std::shared_ptr, const std::shared_ptr request, std::shared_ptr 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); } diff --git a/rclcpp/test/test_time_source.cpp b/rclcpp/test/test_time_source.cpp index a6f620a..28f675b 100644 --- a/rclcpp/test/test_time_source.cpp +++ b/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(node); auto parameters_client = std::make_shared(node); using namespace std::chrono_literals; diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp index a9343ba..ace2f7c 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp @@ -101,7 +101,8 @@ public: rclcpp::Context::SharedPtr context, const std::vector & arguments, bool use_global_arguments = true, - bool use_intra_process_comms = false); + bool use_intra_process_comms = false, + bool start_parameter_services = true); RCLCPP_LIFECYCLE_PUBLIC virtual ~LifecycleNode(); diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp index 37a55af..a47e0d4 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp +++ b/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 any_service_callback; - any_service_callback.set(std::forward(callback)); - - rcl_service_options_t service_options = rcl_service_get_default_options(); - service_options.qos = qos_profile; - - auto serv = rclcpp::Service::make_shared( - node_base_->get_shared_rcl_node_handle(), - service_name, any_service_callback, service_options); - auto serv_base_ptr = std::dynamic_pointer_cast(serv); - node_services_->add_service(serv_base_ptr, group); - return serv; + return rclcpp::create_service( + node_base_, node_services_, + service_name, std::forward(callback), qos_profile, group); } template diff --git a/rclcpp_lifecycle/src/lifecycle_node.cpp b/rclcpp_lifecycle/src/lifecycle_node.cpp index 06c8228..c58d082 100644 --- a/rclcpp_lifecycle/src/lifecycle_node.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node.cpp @@ -35,6 +35,7 @@ #include "rclcpp/node_interfaces/node_services.hpp" #include "rclcpp/node_interfaces/node_timers.hpp" #include "rclcpp/node_interfaces/node_topics.hpp" +#include "rclcpp/parameter_service.hpp" #include "lifecycle_node_interface_impl.hpp" // implementation @@ -51,7 +52,8 @@ LifecycleNode::LifecycleNode( rclcpp::contexts::default_context::get_global_default_context(), {}, true, - use_intra_process_comms) + use_intra_process_comms, + true) {} LifecycleNode::LifecycleNode( @@ -60,7 +62,8 @@ LifecycleNode::LifecycleNode( rclcpp::Context::SharedPtr context, const std::vector & arguments, bool use_global_arguments, - bool use_intra_process_comms) + bool use_intra_process_comms, + bool start_parameter_services) : node_base_(new rclcpp::node_interfaces::NodeBase( node_name, namespace_, context, arguments, use_global_arguments)), node_graph_(new rclcpp::node_interfaces::NodeGraph(node_base_.get())), @@ -69,8 +72,11 @@ LifecycleNode::LifecycleNode( 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(), - use_intra_process_comms + node_base_, + node_topics_, + node_services_, + use_intra_process_comms, + start_parameter_services )), node_clock_(new rclcpp::node_interfaces::NodeClock( node_base_,