From c07875eb544ae4873e850dc327bca6ccd2c8ca5e Mon Sep 17 00:00:00 2001 From: Brian Chen Date: Wed, 13 Jul 2022 18:22:28 -0700 Subject: [PATCH 1/3] configure service introspection via node options & parameters Signed-off-by: Brian Chen --- rclcpp/CMakeLists.txt | 1 + rclcpp/include/rclcpp/client.hpp | 3 +- rclcpp/include/rclcpp/create_client.hpp | 42 ++- rclcpp/include/rclcpp/create_service.hpp | 43 ++- rclcpp/include/rclcpp/node.hpp | 42 +++ rclcpp/include/rclcpp/node_impl.hpp | 84 ++++- .../node_interfaces/node_parameters.hpp | 3 +- .../node_service_introspection.hpp | 61 ++++ .../node_service_introspection_interface.hpp | 47 +++ rclcpp/include/rclcpp/node_options.hpp | 13 + rclcpp/include/rclcpp/parameter_service.hpp | 5 + rclcpp/src/rclcpp/node.cpp | 13 +- .../node_interfaces/node_parameters.cpp | 7 +- .../node_service_introspection.cpp | 134 +++++++ rclcpp/src/rclcpp/node_options.cpp | 16 + rclcpp/src/rclcpp/parameter_service.cpp | 28 +- rclcpp/test/rclcpp/CMakeLists.txt | 12 + .../test_node_service_introspection.cpp | 99 ++++++ rclcpp/test/rclcpp/test_client.cpp | 16 +- rclcpp/test/rclcpp/test_service.cpp | 22 +- .../rclcpp/test_service_introspection.cpp | 331 ++++++++++++++++++ .../include/rclcpp_action/client.hpp | 5 +- .../include/rclcpp_action/create_client.hpp | 3 + rclcpp_action/src/client.cpp | 11 +- rclcpp_action/test/test_client.cpp | 1 + rclcpp_lifecycle/src/lifecycle_node.cpp | 3 +- 26 files changed, 988 insertions(+), 57 deletions(-) create mode 100644 rclcpp/include/rclcpp/node_interfaces/node_service_introspection.hpp create mode 100644 rclcpp/include/rclcpp/node_interfaces/node_service_introspection_interface.hpp create mode 100644 rclcpp/src/rclcpp/node_interfaces/node_service_introspection.cpp create mode 100644 rclcpp/test/rclcpp/node_interfaces/test_node_service_introspection.cpp create mode 100644 rclcpp/test/rclcpp/test_service_introspection.cpp diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 3abd999f3d..3a65318de9 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -78,6 +78,7 @@ set(${PROJECT_NAME}_SRCS src/rclcpp/node_interfaces/node_logging.cpp src/rclcpp/node_interfaces/node_parameters.cpp src/rclcpp/node_interfaces/node_services.cpp + src/rclcpp/node_interfaces/node_service_introspection.cpp src/rclcpp/node_interfaces/node_time_source.cpp src/rclcpp/node_interfaces/node_timers.cpp src/rclcpp/node_interfaces/node_topics.cpp diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index c3a2210a71..c4f8bc8bec 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -16,14 +16,15 @@ #define RCLCPP__CLIENT_HPP_ #include +#include #include -#include #include #include #include // NOLINT, cpplint doesn't think this is a cpp std header #include #include #include +#include #include #include // NOLINT #include diff --git a/rclcpp/include/rclcpp/create_client.hpp b/rclcpp/include/rclcpp/create_client.hpp index 00e6ff3c0e..d2720ca0cd 100644 --- a/rclcpp/include/rclcpp/create_client.hpp +++ b/rclcpp/include/rclcpp/create_client.hpp @@ -19,6 +19,7 @@ #include #include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_clock_interface.hpp" #include "rclcpp/node_interfaces/node_services_interface.hpp" #include "rclcpp/qos.hpp" #include "rmw/rmw.h" @@ -44,15 +45,15 @@ create_client( std::shared_ptr node_base, std::shared_ptr node_graph, std::shared_ptr node_services, + std::shared_ptr node_clock, const std::string & service_name, - const rclcpp::QoS & qos = rclcpp::ServicesQoS(), - rclcpp::CallbackGroup::SharedPtr group = nullptr) + const rclcpp::QoS & qos, + rclcpp::CallbackGroup::SharedPtr group, + bool enable_service_introspection) { return create_client( - node_base, node_graph, node_services, - service_name, - qos.get_rmw_qos_profile(), - group); + node_base, node_graph, node_services, node_clock, service_name, + qos.get_rmw_qos_profile(), group, enable_service_introspection); } /// Create a service client with a given type. @@ -63,12 +64,39 @@ create_client( std::shared_ptr node_base, std::shared_ptr node_graph, std::shared_ptr node_services, + std::shared_ptr node_clock, + const std::string & service_name, + const rmw_qos_profile_t & qos_profile, + rclcpp::CallbackGroup::SharedPtr group, + bool enable_service_introspection) +{ + return create_client( + node_base, node_graph, node_services, node_clock, service_name, qos_profile, + rcl_publisher_get_default_options().qos, group, enable_service_introspection); +} + +/// Create a service client with a given type and qos profiles +/// \internal +template +typename rclcpp::Client::SharedPtr +create_client( + std::shared_ptr node_base, + std::shared_ptr node_graph, + std::shared_ptr node_services, + std::shared_ptr node_clock, const std::string & service_name, const rmw_qos_profile_t & qos_profile, - rclcpp::CallbackGroup::SharedPtr group) + const rmw_qos_profile_t & service_event_publisher_qos_profile, + rclcpp::CallbackGroup::SharedPtr group, + bool enable_service_introspection) { rcl_client_options_t options = rcl_client_get_default_options(); options.qos = qos_profile; + if (enable_service_introspection) { + options.enable_service_introspection = enable_service_introspection; + options.clock = node_clock->get_clock()->get_clock_handle(); + options.event_publisher_options.qos = service_event_publisher_qos_profile; + } auto cli = rclcpp::Client::make_shared( node_base.get(), diff --git a/rclcpp/include/rclcpp/create_service.hpp b/rclcpp/include/rclcpp/create_service.hpp index 42c253a526..b3cc2983df 100644 --- a/rclcpp/include/rclcpp/create_service.hpp +++ b/rclcpp/include/rclcpp/create_service.hpp @@ -20,6 +20,7 @@ #include #include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_clock_interface.hpp" #include "rclcpp/node_interfaces/node_services_interface.hpp" #include "rclcpp/visibility_control.hpp" #include "rmw/rmw.h" @@ -43,33 +44,65 @@ typename rclcpp::Service::SharedPtr create_service( std::shared_ptr node_base, std::shared_ptr node_services, + std::shared_ptr node_clock, const std::string & service_name, CallbackT && callback, const rclcpp::QoS & qos, - rclcpp::CallbackGroup::SharedPtr group) + rclcpp::CallbackGroup::SharedPtr group, + bool enable_service_introspection) { return create_service( - node_base, node_services, service_name, - std::forward(callback), qos.get_rmw_qos_profile(), group); + node_base, node_services, node_clock, service_name, std::forward(callback), + qos.get_rmw_qos_profile(), group, enable_service_introspection); } -/// Create a service with a given type. + +/// 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, + std::shared_ptr node_clock, + const std::string & service_name, + CallbackT && callback, + const rmw_qos_profile_t & qos_profile, + rclcpp::CallbackGroup::SharedPtr group, + bool enable_service_introspection +) +{ + return create_service( + node_base, node_services, node_clock, service_name, std::forward(callback), + qos_profile, rcl_publisher_get_default_options().qos, group, enable_service_introspection); +} + +/// Create a service with a given type and qos profiles /// \internal template typename rclcpp::Service::SharedPtr create_service( std::shared_ptr node_base, std::shared_ptr node_services, + std::shared_ptr node_clock, const std::string & service_name, CallbackT && callback, const rmw_qos_profile_t & qos_profile, - rclcpp::CallbackGroup::SharedPtr group) + const rmw_qos_profile_t & service_event_publisher_qos_profile, + rclcpp::CallbackGroup::SharedPtr group, + bool enable_service_introspection +) { 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; + if (enable_service_introspection) { + service_options.enable_service_introspection = enable_service_introspection; + service_options.clock = node_clock->get_clock()->get_clock_handle(); + service_options.event_publisher_options.qos = service_event_publisher_qos_profile; + } auto serv = Service::make_shared( node_base->get_shared_rcl_node_handle(), diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index 7ecb67e9b1..32b1419e8a 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -53,6 +53,7 @@ #include "rclcpp/node_interfaces/node_logging_interface.hpp" #include "rclcpp/node_interfaces/node_parameters_interface.hpp" #include "rclcpp/node_interfaces/node_services_interface.hpp" +#include "rclcpp/node_interfaces/node_service_introspection_interface.hpp" #include "rclcpp/node_interfaces/node_time_source_interface.hpp" #include "rclcpp/node_interfaces/node_timers_interface.hpp" #include "rclcpp/node_interfaces/node_topics_interface.hpp" @@ -283,6 +284,22 @@ class Node : public std::enable_shared_from_this const rclcpp::QoS & qos = rclcpp::ServicesQoS(), rclcpp::CallbackGroup::SharedPtr group = nullptr); + /// Create and return a Client. + /** + * \param[in] service_name The name on which the service is accessible. + * \param[in] qos Quality of service profile for client. + * \param[in] service_event_publisher_qos QOS profile for the service event publisher. + * \param[in] group Callback group to handle the reply to service calls. + * \return Shared pointer to the created client. + */ + template + typename Client::SharedPtr + create_client( + const std::string & service_name, + const rclcpp::QoS & qos, + const rclcpp::QoS & service_event_publisher_qos, + rclcpp::CallbackGroup::SharedPtr group); + /// Create and return a Service. /** * \param[in] service_name The topic to service on. @@ -317,6 +334,25 @@ class Node : public std::enable_shared_from_this const rclcpp::QoS & qos = rclcpp::ServicesQoS(), rclcpp::CallbackGroup::SharedPtr group = nullptr); + + /// Create and return a Service. + /** + * \param[in] service_name The topic to service on. + * \param[in] callback User-defined callback function. + * \param[in] qos Quality of service profile for the service. + * \param[in] service_event_publisher_qos QOS profile for the service event publisher. + * \param[in] group Callback group to call the service. + * \return Shared pointer to the created service. + */ + template + typename rclcpp::Service::SharedPtr + create_service( + const std::string & service_name, + CallbackT && callback, + const rclcpp::QoS & qos, + const rclcpp::QoS & service_event_publisher_qos, + rclcpp::CallbackGroup::SharedPtr group); + /// Create and return a GenericPublisher. /** * The returned pointer will never be empty, but this function can throw various exceptions, for @@ -1439,6 +1475,11 @@ class Node : public std::enable_shared_from_this rclcpp::node_interfaces::NodeServicesInterface::SharedPtr get_node_services_interface(); + /// Return the Node's internal NodeServicesInterface implementation. + RCLCPP_PUBLIC + rclcpp::node_interfaces::NodeServiceIntrospectionInterface::SharedPtr + get_node_service_introspection_interface(); + /// Return the Node's internal NodeWaitablesInterface implementation. RCLCPP_PUBLIC rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr @@ -1587,6 +1628,7 @@ class Node : public std::enable_shared_from_this rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_; rclcpp::node_interfaces::NodeTimeSourceInterface::SharedPtr node_time_source_; rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr node_waitables_; + rclcpp::node_interfaces::NodeServiceIntrospectionInterface::SharedPtr node_service_introspection_; const rclcpp::NodeOptions node_options_; const std::string sub_namespace_; diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index d6b090d4d1..b8325aa923 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -143,13 +143,41 @@ Node::create_client( const rclcpp::QoS & qos, rclcpp::CallbackGroup::SharedPtr group) { - return rclcpp::create_client( + typename Client::SharedPtr cli = rclcpp::create_client( node_base_, node_graph_, node_services_, + node_clock_, extend_name_with_sub_namespace(service_name, this->get_sub_namespace()), qos, - group); + group, + node_options_.enable_service_introspection()); + + node_service_introspection_->register_client(cli); + return cli; +} + +template +typename Client::SharedPtr +Node::create_client( + const std::string & service_name, + const rclcpp::QoS & qos, + const rclcpp::QoS & service_event_publisher_qos, + rclcpp::CallbackGroup::SharedPtr group) +{ + typename Client::SharedPtr cli = rclcpp::create_client( + node_base_, + node_graph_, + node_services_, + node_clock_, + extend_name_with_sub_namespace(service_name, this->get_sub_namespace()), + qos.get_rmw_qos_profile(), + service_event_publisher_qos.get_rmw_qos_profile(), + group, + node_options_.enable_service_introspection()); + + node_service_introspection_->register_client(cli); + return cli; } template @@ -159,13 +187,18 @@ Node::create_client( const rmw_qos_profile_t & qos_profile, rclcpp::CallbackGroup::SharedPtr group) { - return rclcpp::create_client( + typename Client::SharedPtr cli = rclcpp::create_client( node_base_, node_graph_, node_services_, + node_clock_, extend_name_with_sub_namespace(service_name, this->get_sub_namespace()), qos_profile, - group); + group, + node_options_.enable_service_introspection()); + + node_service_introspection_->register_client(cli); + return cli; } template @@ -176,13 +209,42 @@ Node::create_service( const rclcpp::QoS & qos, rclcpp::CallbackGroup::SharedPtr group) { - return rclcpp::create_service( + typename rclcpp::Service::SharedPtr serv = rclcpp::create_service( node_base_, node_services_, + node_clock_, extend_name_with_sub_namespace(service_name, this->get_sub_namespace()), std::forward(callback), qos, - group); + group, + node_options_.enable_service_introspection()); + + node_service_introspection_->register_service(serv); + return serv; +} + +template +typename rclcpp::Service::SharedPtr +Node::create_service( + const std::string & service_name, + CallbackT && callback, + const rclcpp::QoS & qos, + const rclcpp::QoS & service_event_publisher_qos, + rclcpp::CallbackGroup::SharedPtr group) +{ + typename rclcpp::Service::SharedPtr serv = rclcpp::create_service( + node_base_, + node_services_, + node_clock_, + extend_name_with_sub_namespace(service_name, this->get_sub_namespace()), + std::forward(callback), + qos.get_rmw_qos_profile(), + service_event_publisher_qos.get_rmw_qos_profile(), + group, + node_options_.enable_service_introspection()); + + node_service_introspection_->register_service(serv); + return serv; } template @@ -193,13 +255,19 @@ Node::create_service( const rmw_qos_profile_t & qos_profile, rclcpp::CallbackGroup::SharedPtr group) { - return rclcpp::create_service( + typename rclcpp::Service::SharedPtr serv = rclcpp::create_service( node_base_, node_services_, + node_clock_, extend_name_with_sub_namespace(service_name, this->get_sub_namespace()), std::forward(callback), qos_profile, - group); + group, + node_options_.enable_service_introspection() + ); + + node_service_introspection_->register_service(serv); + return serv; } template diff --git a/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp b/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp index ffbb400c11..e963411517 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp @@ -108,7 +108,8 @@ class NodeParameters : public NodeParametersInterface const rclcpp::QoS & parameter_event_qos, const rclcpp::PublisherOptionsBase & parameter_event_publisher_options, bool allow_undeclared_parameters, - bool automatically_declare_parameters_from_overrides); + bool automatically_declare_parameters_from_overrides, + bool enable_service_introspection_for_parameter_service); RCLCPP_PUBLIC virtual diff --git a/rclcpp/include/rclcpp/node_interfaces/node_service_introspection.hpp b/rclcpp/include/rclcpp/node_interfaces/node_service_introspection.hpp new file mode 100644 index 0000000000..59beac5a12 --- /dev/null +++ b/rclcpp/include/rclcpp/node_interfaces/node_service_introspection.hpp @@ -0,0 +1,61 @@ +// Copyright 2022 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__NODE_INTERFACES__NODE_SERVICE_INTROSPECTION_HPP_ +#define RCLCPP__NODE_INTERFACES__NODE_SERVICE_INTROSPECTION_HPP_ + +#include +#include "rclcpp/macros.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_parameters_interface.hpp" +#include "rclcpp/node_interfaces/node_service_introspection_interface.hpp" +#include "rclcpp/service.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ +namespace node_interfaces +{ +class NodeServiceIntrospection : public NodeServiceIntrospectionInterface +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(NodeServiceIntrospection) + + RCLCPP_PUBLIC + explicit NodeServiceIntrospection( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr & node_base, + const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & node_parameters); + + RCLCPP_PUBLIC + size_t + register_service(rclcpp::ServiceBase::SharedPtr service) override; + + RCLCPP_PUBLIC + size_t + register_client(rclcpp::ClientBase::SharedPtr client) override; + + RCLCPP_PUBLIC + ~NodeServiceIntrospection() override = default; + +private: + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_; + std::vector services_; + std::vector clients_; + rclcpp::node_interfaces::PostSetParametersCallbackHandle::SharedPtr + post_set_parameters_callback_handle_; +}; + +} // namespace node_interfaces +} // namespace rclcpp +#endif // RCLCPP__NODE_INTERFACES__NODE_SERVICE_INTROSPECTION_HPP_ diff --git a/rclcpp/include/rclcpp/node_interfaces/node_service_introspection_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_service_introspection_interface.hpp new file mode 100644 index 0000000000..e671e4d3bd --- /dev/null +++ b/rclcpp/include/rclcpp/node_interfaces/node_service_introspection_interface.hpp @@ -0,0 +1,47 @@ +// Copyright 2022 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__NODE_INTERFACES__NODE_SERVICE_INTROSPECTION_INTERFACE_HPP_ +#define RCLCPP__NODE_INTERFACES__NODE_SERVICE_INTROSPECTION_INTERFACE_HPP_ + +#include "rclcpp/macros.hpp" +#include "rclcpp/visibility_control.hpp" +#include "rclcpp/service.hpp" +#include "rclcpp/client.hpp" + +namespace rclcpp +{ +namespace node_interfaces +{ +class NodeServiceIntrospectionInterface +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(NodeServiceIntrospectionInterface) + + RCLCPP_PUBLIC + size_t + virtual register_service(rclcpp::ServiceBase::SharedPtr service) = 0; + + RCLCPP_PUBLIC + size_t + virtual register_client(rclcpp::ClientBase::SharedPtr client) = 0; + + RCLCPP_PUBLIC + virtual + ~NodeServiceIntrospectionInterface() = default; +}; + +} // namespace node_interfaces +} // namespace rclcpp +#endif // RCLCPP__NODE_INTERFACES__NODE_SERVICE_INTROSPECTION_INTERFACE_HPP_ diff --git a/rclcpp/include/rclcpp/node_options.hpp b/rclcpp/include/rclcpp/node_options.hpp index 47a97fd3c4..59dcd53c5a 100644 --- a/rclcpp/include/rclcpp/node_options.hpp +++ b/rclcpp/include/rclcpp/node_options.hpp @@ -45,6 +45,7 @@ class NodeOptions * - use_global_arguments = true * - use_intra_process_comms = false * - enable_topic_statistics = false + * - enable_service_introspection = false * - start_parameter_services = true * - start_parameter_event_publisher = true * - clock_type = RCL_ROS_TIME @@ -201,6 +202,16 @@ class NodeOptions bool enable_topic_statistics() const; + /// Return the enable_service_introspection flag + RCLCPP_PUBLIC + bool + enable_service_introspection() const; + + /// Set the enable_service_introspection flag + RCLCPP_PUBLIC + NodeOptions & + enable_service_introspection(bool enable_service_introspection); + /// Set the enable_topic_statistics flag, return this for parameter idiom. /** * If true, topic statistics collection and publication will be enabled @@ -411,6 +422,8 @@ class NodeOptions bool enable_topic_statistics_ {false}; + bool enable_service_introspection_ {false}; + bool start_parameter_services_ {true}; bool start_parameter_event_publisher_ {true}; diff --git a/rclcpp/include/rclcpp/parameter_service.hpp b/rclcpp/include/rclcpp/parameter_service.hpp index a952939aca..9fd1d65b28 100644 --- a/rclcpp/include/rclcpp/parameter_service.hpp +++ b/rclcpp/include/rclcpp/parameter_service.hpp @@ -45,12 +45,15 @@ class ParameterService ParameterService( const std::shared_ptr node_base, const std::shared_ptr node_services, + const std::shared_ptr node_clock, rclcpp::node_interfaces::NodeParametersInterface * node_params, const rmw_qos_profile_t & qos_profile) : ParameterService( node_base, node_services, + node_clock, node_params, + false, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos_profile))) {} @@ -58,7 +61,9 @@ class ParameterService ParameterService( const std::shared_ptr node_base, const std::shared_ptr node_services, + const std::shared_ptr node_clock, rclcpp::node_interfaces::NodeParametersInterface * node_params, + bool enable_service_introspection = false, const rclcpp::QoS & qos_profile = rclcpp::ParametersQoS()); private: diff --git a/rclcpp/src/rclcpp/node.cpp b/rclcpp/src/rclcpp/node.cpp index c1fbdb1f98..d473534786 100644 --- a/rclcpp/src/rclcpp/node.cpp +++ b/rclcpp/src/rclcpp/node.cpp @@ -32,6 +32,7 @@ #include "rclcpp/node_interfaces/node_graph.hpp" #include "rclcpp/node_interfaces/node_logging.hpp" #include "rclcpp/node_interfaces/node_parameters.hpp" +#include "rclcpp/node_interfaces/node_service_introspection.hpp" #include "rclcpp/node_interfaces/node_services.hpp" #include "rclcpp/node_interfaces/node_time_source.hpp" #include "rclcpp/node_interfaces/node_timers.hpp" @@ -193,7 +194,8 @@ Node::Node( get_parameter_events_qos(*node_base_, options), options.parameter_event_publisher_options(), options.allow_undeclared_parameters(), - options.automatically_declare_parameters_from_overrides() + options.automatically_declare_parameters_from_overrides(), + options.enable_service_introspection() )), node_time_source_(new rclcpp::node_interfaces::NodeTimeSource( node_base_, @@ -207,6 +209,9 @@ Node::Node( options.use_clock_thread() )), node_waitables_(new rclcpp::node_interfaces::NodeWaitables(node_base_.get())), + node_service_introspection_(new rclcpp::node_interfaces::NodeServiceIntrospection( + node_base_, + node_parameters_)), node_options_(options), sub_namespace_(""), effective_namespace_(create_effective_namespace(this->get_namespace(), sub_namespace_)) @@ -593,6 +598,12 @@ Node::get_node_services_interface() return node_services_; } +rclcpp::node_interfaces::NodeServiceIntrospectionInterface::SharedPtr +Node::get_node_service_introspection_interface() +{ + return node_service_introspection_; +} + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr Node::get_node_parameters_interface() { diff --git a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp index 9dafcba381..f3e5ae46ad 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp @@ -76,7 +76,8 @@ NodeParameters::NodeParameters( const rclcpp::QoS & parameter_event_qos, const rclcpp::PublisherOptionsBase & parameter_event_publisher_options, bool allow_undeclared_parameters, - bool automatically_declare_parameters_from_overrides) + bool automatically_declare_parameters_from_overrides, + bool enable_service_introspection_for_parameter_service) : allow_undeclared_(allow_undeclared_parameters), events_publisher_(nullptr), node_logging_(node_logging), @@ -91,7 +92,9 @@ NodeParameters::NodeParameters( publisher_options.allocator = std::make_shared(); if (start_parameter_services) { - parameter_service_ = std::make_shared(node_base, node_services, this); + parameter_service_ = std::make_shared( + node_base, node_services, node_clock, + this, enable_service_introspection_for_parameter_service); } if (start_parameter_event_publisher) { diff --git a/rclcpp/src/rclcpp/node_interfaces/node_service_introspection.cpp b/rclcpp/src/rclcpp/node_interfaces/node_service_introspection.cpp new file mode 100644 index 0000000000..0c958b1658 --- /dev/null +++ b/rclcpp/src/rclcpp/node_interfaces/node_service_introspection.cpp @@ -0,0 +1,134 @@ +// Copyright 2022 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 +#include "rclcpp/node_interfaces/node_service_introspection.hpp" +#include "rcl/service_introspection.h" +#include "rcl/client.h" +#include "rcl/service.h" + +using rclcpp::node_interfaces::NodeServiceIntrospection; + + +NodeServiceIntrospection::NodeServiceIntrospection( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr & node_base, + const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & node_parameters) +: node_base_(node_base) +{ + // declare service introspection parameters + if (!node_parameters->has_parameter("publish_service_events")) { + node_parameters->declare_parameter("publish_service_events", rclcpp::ParameterValue(true)); + } + if (!node_parameters->has_parameter("publish_service_content")) { + node_parameters->declare_parameter("publish_service_content", rclcpp::ParameterValue(true)); + } + if (!node_parameters->has_parameter("publish_client_events")) { + node_parameters->declare_parameter("publish_client_events", rclcpp::ParameterValue(true)); + } + if (!node_parameters->has_parameter("publish_client_content")) { + node_parameters->declare_parameter("publish_client_content", rclcpp::ParameterValue(true)); + } + + std::function &)> + configure_service_introspection_callback = + [this](const std::vector & parameters) { + rcl_ret_t ret; + for (const auto & param : parameters) { + if (param.get_name() == "publish_service_events") { + for (auto srv = services_.begin(); srv != services_.end(); ++srv) { + if (srv->expired()) { + srv = services_.erase(srv); + } else { + ret = rcl_service_introspection_configure_server_service_events( + srv->lock()->get_service_handle().get(), + this->node_base_->get_rcl_node_handle(), + param.get_value()); + if (RCL_RET_OK != ret) { + throw std::runtime_error( + std::string( + "Failed to configure service introspection events with error ") + + std::to_string(ret)); + } + } + } + } else if (param.get_name() == "publish_client_events") { + for (auto clt = clients_.begin(); clt != clients_.end(); ++clt) { + if (clt->expired()) { + clt = clients_.erase(clt); + } else { + ret = rcl_service_introspection_configure_client_service_events( + clt->lock()->get_client_handle().get(), + this->node_base_->get_rcl_node_handle(), + param.get_value()); + if (RCL_RET_OK != ret) { + throw std::runtime_error( + std::string( + "Failed to configure service introspection events with error ") + + std::to_string(ret)); + } + } + } + } else if (param.get_name() == "publish_service_content") { + for (auto srv = services_.begin(); srv != services_.end(); ++srv) { + if (srv->expired()) { + srv = services_.erase(srv); + } else { + ret = rcl_service_introspection_configure_server_service_event_message_payload( + srv->lock()->get_service_handle().get(), param.get_value()); + if (RCL_RET_OK != ret) { + throw std::runtime_error( + std::string( + "Failed to configure service introspection message payload with error ") + + std::to_string(ret)); + } + } + } + } else if (param.get_name() == "publish_client_content") { + for (auto clt = clients_.begin(); clt != clients_.end(); ++clt) { + if (clt->expired()) { + clt = clients_.erase(clt); + } else { + ret = rcl_service_introspection_configure_client_service_event_message_payload( + clt->lock()->get_client_handle().get(), param.get_value()); + if (RCL_RET_OK != ret) { + throw std::runtime_error( + std::string( + "Failed to configure service introspection message payload with error ") + + std::to_string(ret)); + } + } + } + } + } + }; + // register callbacks + post_set_parameters_callback_handle_ = node_parameters->add_post_set_parameters_callback( + configure_service_introspection_callback); +} + +size_t +NodeServiceIntrospection::register_client(rclcpp::ClientBase::SharedPtr client) +{ + std::weak_ptr weak_client = client; + clients_.push_back(weak_client); + return clients_.size(); +} + +size_t +NodeServiceIntrospection::register_service(rclcpp::ServiceBase::SharedPtr service) +{ + std::weak_ptr weak_service = service; + services_.push_back(weak_service); + return services_.size(); +} diff --git a/rclcpp/src/rclcpp/node_options.cpp b/rclcpp/src/rclcpp/node_options.cpp index b90a4b27e7..5d58decfe2 100644 --- a/rclcpp/src/rclcpp/node_options.cpp +++ b/rclcpp/src/rclcpp/node_options.cpp @@ -75,6 +75,7 @@ NodeOptions::operator=(const NodeOptions & other) this->enable_rosout_ = other.enable_rosout_; this->use_intra_process_comms_ = other.use_intra_process_comms_; this->enable_topic_statistics_ = other.enable_topic_statistics_; + this->enable_service_introspection_ = other.enable_service_introspection_; this->start_parameter_services_ = other.start_parameter_services_; this->start_parameter_event_publisher_ = other.start_parameter_event_publisher_; this->clock_type_ = other.clock_type_; @@ -102,6 +103,7 @@ NodeOptions::get_rcl_node_options() const node_options_->use_global_arguments = this->use_global_arguments_; node_options_->enable_rosout = this->enable_rosout_; node_options_->rosout_qos = this->rosout_qos_.get_rmw_qos_profile(); + node_options_->enable_service_introspection = this->enable_service_introspection_; int c_argc = 0; std::unique_ptr c_argv; @@ -228,6 +230,20 @@ NodeOptions::enable_topic_statistics() const return this->enable_topic_statistics_; } +bool +NodeOptions::enable_service_introspection() const +{ + return this->enable_service_introspection_; +} + +NodeOptions & +NodeOptions::enable_service_introspection(bool enable_service_introspection) +{ + this->node_options_.reset(); // reset node options to make it be recreated on next access. + this->enable_service_introspection_ = enable_service_introspection; + return *this; +} + NodeOptions & NodeOptions::enable_topic_statistics(bool enable_topic_statistics) { diff --git a/rclcpp/src/rclcpp/parameter_service.cpp b/rclcpp/src/rclcpp/parameter_service.cpp index 0923798339..829cd01fbb 100644 --- a/rclcpp/src/rclcpp/parameter_service.cpp +++ b/rclcpp/src/rclcpp/parameter_service.cpp @@ -20,21 +20,25 @@ #include #include "rclcpp/logging.hpp" +#include "rclcpp/create_service.hpp" #include "./parameter_service_names.hpp" +#include "rclcpp/node_interfaces/node_clock_interface.hpp" using rclcpp::ParameterService; ParameterService::ParameterService( const std::shared_ptr node_base, const std::shared_ptr node_services, + const std::shared_ptr node_clock, rclcpp::node_interfaces::NodeParametersInterface * node_params, + bool enable_service_introspection, const rclcpp::QoS & qos_profile) { const std::string node_name = node_base->get_name(); get_parameters_service_ = create_service( - node_base, node_services, + node_base, node_services, node_clock, node_name + "/" + parameter_service_names::get_parameters, [node_params]( const std::shared_ptr, @@ -52,10 +56,10 @@ ParameterService::ParameterService( RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "Failed to get parameters: %s", ex.what()); } }, - qos_profile, nullptr); + qos_profile, nullptr, enable_service_introspection); get_parameter_types_service_ = create_service( - node_base, node_services, + node_base, node_services, node_clock, node_name + "/" + parameter_service_names::get_parameter_types, [node_params]( const std::shared_ptr, @@ -73,10 +77,10 @@ ParameterService::ParameterService( RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "Failed to get parameter types: %s", ex.what()); } }, - qos_profile, nullptr); + qos_profile, nullptr, enable_service_introspection); set_parameters_service_ = create_service( - node_base, node_services, + node_base, node_services, node_clock, node_name + "/" + parameter_service_names::set_parameters, [node_params]( const std::shared_ptr, @@ -98,10 +102,10 @@ ParameterService::ParameterService( response->results.push_back(result); } }, - qos_profile, nullptr); + qos_profile, nullptr, enable_service_introspection); set_parameters_atomically_service_ = create_service( - node_base, node_services, + node_base, node_services, node_clock, node_name + "/" + parameter_service_names::set_parameters_atomically, [node_params]( const std::shared_ptr, @@ -125,10 +129,10 @@ ParameterService::ParameterService( response->result.reason = "One or more parameters were not declared before setting"; } }, - qos_profile, nullptr); + qos_profile, nullptr, enable_service_introspection); describe_parameters_service_ = create_service( - node_base, node_services, + node_base, node_services, node_clock, node_name + "/" + parameter_service_names::describe_parameters, [node_params]( const std::shared_ptr, @@ -142,10 +146,10 @@ ParameterService::ParameterService( RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "Failed to describe parameters: %s", ex.what()); } }, - qos_profile, nullptr); + qos_profile, nullptr, enable_service_introspection); list_parameters_service_ = create_service( - node_base, node_services, + node_base, node_services, node_clock, node_name + "/" + parameter_service_names::list_parameters, [node_params]( const std::shared_ptr, @@ -155,5 +159,5 @@ ParameterService::ParameterService( auto result = node_params->list_parameters(request->prefixes, request->depth); response->result = result; }, - qos_profile, nullptr); + qos_profile, nullptr, enable_service_introspection); } diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index 1e3b7a65ca..bdf7ef82d9 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -508,6 +508,18 @@ if(TARGET test_service) ) target_link_libraries(test_service ${PROJECT_NAME} mimick) endif() +# ament_add_gtest(test_service_introspection test_service_introspection.cpp) +ament_add_gmock(test_service_introspection test_service_introspection.cpp) +if(TARGET test_service) + ament_target_dependencies(test_service_introspection + "rcl_interfaces" + "rmw" + "rosidl_runtime_cpp" + "rosidl_typesupport_cpp" + "test_msgs" + ) + target_link_libraries(test_service_introspection ${PROJECT_NAME} mimick) +endif() # Creating and destroying nodes is slow with Connext, so this needs larger timeout. ament_add_gtest(test_subscription test_subscription.cpp TIMEOUT 120) if(TARGET test_subscription) diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_service_introspection.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_service_introspection.cpp new file mode 100644 index 0000000000..c588d0b20d --- /dev/null +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_service_introspection.cpp @@ -0,0 +1,99 @@ +// Copyright 2022 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 + +#include + +#include "rclcpp/node_interfaces/node_service_introspection.hpp" +#include "rclcpp/node_interfaces/node_services_interface.hpp" +#include "rclcpp/node.hpp" +#include "rclcpp/node_options.hpp" +#include "rcl/service_introspection.h" + +class TestService : public rclcpp::ServiceBase +{ +public: + explicit TestService(rclcpp::Node * node) + : rclcpp::ServiceBase(node->get_node_base_interface()->get_shared_rcl_node_handle()) {} + + std::shared_ptr create_request() override {return nullptr;} + std::shared_ptr create_request_header() override {return nullptr;} + void handle_request(std::shared_ptr, std::shared_ptr) override {} +}; + +class TestClient : public rclcpp::ClientBase +{ +public: + explicit TestClient(rclcpp::Node * node) + : rclcpp::ClientBase(node->get_node_base_interface().get(), node->get_node_graph_interface()) {} + + std::shared_ptr create_response() override {return nullptr;} + std::shared_ptr create_request_header() override {return nullptr;} + void handle_response( + std::shared_ptr, std::shared_ptr) override {} +}; +class TestNodeServiceIntrospection : public ::testing::Test +{ +public: + void SetUp() + { + rclcpp::init(0, nullptr); + } + + void TearDown() + { + rclcpp::shutdown(); + } +}; + +TEST_F(TestNodeServiceIntrospection, construct_from_node) +{ + std::shared_ptr node = std::make_shared( + "node", "ns", + rclcpp::NodeOptions().enable_service_introspection(true)); + + // This dynamic cast is not necessary for the unittest itself, but instead is used to ensure + // the proper type is being tested and covered. + auto * node_service_introspection = + dynamic_cast( + node->get_node_service_introspection_interface().get()); + + ASSERT_NE(nullptr, node_service_introspection); + ASSERT_TRUE(node->get_parameter(RCL_SERVICE_INTROSPECTION_PUBLISH_CLIENT_PARAMETER)); + ASSERT_TRUE(node->get_parameter(RCL_SERVICE_INTROSPECTION_PUBLISH_SERVICE_PARAMETER)); + ASSERT_TRUE( + node->get_parameter(RCL_SERVICE_INTROSPECTION_PUBLISH_CLIENT_EVENT_CONTENT_PARAMETER)); + ASSERT_TRUE( + node->get_parameter(RCL_SERVICE_INTROSPECTION_PUBLISH_SERVICE_EVENT_CONTENT_PARAMETER)); +} + +TEST_F(TestNodeServiceIntrospection, register_services_and_clients) +{ + std::shared_ptr node = std::make_shared( + "node", "ns", + rclcpp::NodeOptions().enable_service_introspection(true)); + + // This dynamic cast is not necessary for the unittest itself, but instead is used to ensure + // the proper type is being tested and covered. + auto * node_service_introspection = + dynamic_cast( + node->get_node_service_introspection_interface().get()); + + auto service = std::make_shared(node.get()); + auto client = std::make_shared(node.get()); + + ASSERT_EQ(1, node_service_introspection->register_service(service)); + ASSERT_EQ(1, node_service_introspection->register_client(client)); +} diff --git a/rclcpp/test/rclcpp/test_client.cpp b/rclcpp/test/rclcpp/test_client.cpp index 9070e1caa9..ead263cc98 100644 --- a/rclcpp/test/rclcpp/test_client.cpp +++ b/rclcpp/test/rclcpp/test_client.cpp @@ -131,9 +131,11 @@ TEST_F(TestClient, construction_with_free_function) { node->get_node_base_interface(), node->get_node_graph_interface(), node->get_node_services_interface(), + node->get_node_clock_interface(), "service", rmw_qos_profile_services_default, - nullptr); + nullptr, + true); } { ASSERT_THROW( @@ -142,9 +144,11 @@ TEST_F(TestClient, construction_with_free_function) { node->get_node_base_interface(), node->get_node_graph_interface(), node->get_node_services_interface(), + node->get_node_clock_interface(), "invalid_?service", rmw_qos_profile_services_default, - nullptr); + nullptr, + true); }, rclcpp::exceptions::InvalidServiceNameError); } { @@ -152,9 +156,11 @@ TEST_F(TestClient, construction_with_free_function) { node->get_node_base_interface(), node->get_node_graph_interface(), node->get_node_services_interface(), + node->get_node_clock_interface(), "service", rclcpp::ServicesQoS(), - nullptr); + nullptr, + true); } { ASSERT_THROW( @@ -163,9 +169,11 @@ TEST_F(TestClient, construction_with_free_function) { node->get_node_base_interface(), node->get_node_graph_interface(), node->get_node_services_interface(), + node->get_node_clock_interface(), "invalid_?service", rclcpp::ServicesQoS(), - nullptr); + nullptr, + true); }, rclcpp::exceptions::InvalidServiceNameError); } } diff --git a/rclcpp/test/rclcpp/test_service.cpp b/rclcpp/test/rclcpp/test_service.cpp index a3c361cfde..dd4d49aca6 100644 --- a/rclcpp/test/rclcpp/test_service.cpp +++ b/rclcpp/test/rclcpp/test_service.cpp @@ -30,7 +30,7 @@ using namespace std::chrono_literals; -class TestService : public ::testing::Test +class TestServiceIntrospection : public ::testing::Test { protected: static void SetUpTestCase() @@ -87,7 +87,7 @@ class TestServiceSub : public ::testing::Test /* Testing service construction and destruction. */ -TEST_F(TestService, construction_and_destruction) { +TEST_F(TestServiceIntrospection, construction_and_destruction) { using rcl_interfaces::srv::ListParameters; auto callback = [](const ListParameters::Request::SharedPtr, ListParameters::Response::SharedPtr) { @@ -128,7 +128,7 @@ TEST_F(TestServiceSub, construction_and_destruction) { } } -TEST_F(TestService, construction_and_destruction_rcl_errors) { +TEST_F(TestServiceIntrospection, construction_and_destruction_rcl_errors) { auto callback = [](const test_msgs::srv::Empty::Request::SharedPtr, test_msgs::srv::Empty::Response::SharedPtr) {}; @@ -148,7 +148,7 @@ TEST_F(TestService, construction_and_destruction_rcl_errors) { } /* Testing basic getters */ -TEST_F(TestService, basic_public_getters) { +TEST_F(TestServiceIntrospection, basic_public_getters) { using rcl_interfaces::srv::ListParameters; auto callback = [](const ListParameters::Request::SharedPtr, ListParameters::Response::SharedPtr) { @@ -188,7 +188,7 @@ TEST_F(TestService, basic_public_getters) { } } -TEST_F(TestService, take_request) { +TEST_F(TestServiceIntrospection, take_request) { auto callback = [](const test_msgs::srv::Empty::Request::SharedPtr, test_msgs::srv::Empty::Response::SharedPtr) {}; @@ -216,7 +216,7 @@ TEST_F(TestService, take_request) { } } -TEST_F(TestService, send_response) { +TEST_F(TestServiceIntrospection, send_response) { auto callback = [](const test_msgs::srv::Empty::Request::SharedPtr, test_msgs::srv::Empty::Response::SharedPtr) {}; @@ -242,7 +242,7 @@ TEST_F(TestService, send_response) { /* Testing on_new_request callbacks. */ -TEST_F(TestService, on_new_request_callback) { +TEST_F(TestServiceIntrospection, on_new_request_callback) { auto server_callback = [](const test_msgs::srv::Empty::Request::SharedPtr, test_msgs::srv::Empty::Response::SharedPtr) {FAIL();}; @@ -312,7 +312,7 @@ TEST_F(TestService, on_new_request_callback) { EXPECT_THROW(server->set_on_new_request_callback(invalid_cb), std::invalid_argument); } -TEST_F(TestService, rcl_service_response_publisher_get_actual_qos_error) { +TEST_F(TestServiceIntrospection, rcl_service_response_publisher_get_actual_qos_error) { auto mock = mocking_utils::patch_and_return( "lib:rclcpp", rcl_service_response_publisher_get_actual_qos, nullptr); auto callback = @@ -324,7 +324,7 @@ TEST_F(TestService, rcl_service_response_publisher_get_actual_qos_error) { std::runtime_error("failed to get service's response publisher qos settings: error not set")); } -TEST_F(TestService, rcl_service_request_subscription_get_actual_qos_error) { +TEST_F(TestServiceIntrospection, rcl_service_request_subscription_get_actual_qos_error) { auto mock = mocking_utils::patch_and_return( "lib:rclcpp", rcl_service_request_subscription_get_actual_qos, nullptr); auto callback = @@ -337,7 +337,7 @@ TEST_F(TestService, rcl_service_request_subscription_get_actual_qos_error) { } -TEST_F(TestService, server_qos) { +TEST_F(TestServiceIntrospection, server_qos) { rclcpp::ServicesQoS qos_profile; qos_profile.liveliness(rclcpp::LivelinessPolicy::Automatic); rclcpp::Duration duration(std::chrono::nanoseconds(1)); @@ -359,7 +359,7 @@ TEST_F(TestService, server_qos) { EXPECT_EQ(qos_profile, rs_qos); } -TEST_F(TestService, server_qos_depth) { +TEST_F(TestServiceIntrospection, server_qos_depth) { using namespace std::literals::chrono_literals; uint64_t server_cb_count_ = 0; diff --git a/rclcpp/test/rclcpp/test_service_introspection.cpp b/rclcpp/test/rclcpp/test_service_introspection.cpp new file mode 100644 index 0000000000..4ffb8c6253 --- /dev/null +++ b/rclcpp/test/rclcpp/test_service_introspection.cpp @@ -0,0 +1,331 @@ +// Copyright 2017 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 +#include + +#include +#include + +#include "gmock/gmock.h" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/exceptions.hpp" +#include "rclcpp/logging.hpp" +#include "rclcpp/node_options.hpp" +#include "rclcpp/parameter.hpp" + +#include "../mocking_utils/patch.hpp" +#include "../utils/rclcpp_gtest_macros.hpp" + +#include "test_msgs/srv/basic_types.hpp" +#include "service_msgs/msg/service_event_info.hpp" + +using namespace std::chrono_literals; +using test_msgs::srv::BasicTypes; +using service_msgs::msg::ServiceEventInfo; + + +class TestServiceIntrospection : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + void SetUp() + { + node = std::make_shared( + "my_node", "/ns", + rclcpp::NodeOptions().enable_service_introspection(true)); + + auto srv_callback = + [](const BasicTypes::Request::SharedPtr & req, const BasicTypes::Response::SharedPtr & resp) { + resp->set__bool_value(!req->bool_value); + resp->set__int64_value(req->int64_value); + return resp; + }; + + auto callback = [this](const std::shared_ptr & msg) { + events.push_back(msg); + (void)msg; + }; + + client = node->create_client("service"); + service = node->create_service("service", srv_callback); + sub = node->create_subscription("service/_service_event", 10, callback); + events.clear(); + } + + void TearDown() + { + node.reset(); + } + + rclcpp::Node::SharedPtr node; + rclcpp::Client::SharedPtr client; + rclcpp::Service::SharedPtr service; + rclcpp::Subscription::SharedPtr sub; + std::vector> events; + std::chrono::milliseconds timeout = std::chrono::milliseconds(1000); +}; + +/* + Testing service construction and destruction with service introspection enabled + */ +TEST_F(TestServiceIntrospection, construction_and_destruction) +{ + auto callback = + [](const BasicTypes::Request::SharedPtr &, const BasicTypes::Response::SharedPtr &) {}; + { + auto service = node->create_service("test_create_service", callback); + EXPECT_NE(nullptr, service->get_service_handle()); + const rclcpp::ServiceBase * const_service_base = service.get(); + EXPECT_NE(nullptr, const_service_base->get_service_handle()); + } + + { + ASSERT_THROW( + { + auto service = node->create_service("invalid_service?", callback); + }, rclcpp::exceptions::InvalidServiceNameError); + } +} + +TEST_F(TestServiceIntrospection, service_introspection_nominal) +{ + auto request = std::make_shared(); + request->set__bool_value(true); + request->set__int64_value(42); + + auto future = client->async_send_request(request); + ASSERT_EQ( + rclcpp::FutureReturnCode::SUCCESS, + rclcpp::spin_until_future_complete(node, future, timeout)); + + BasicTypes::Response::SharedPtr response = future.get(); + ASSERT_EQ(response->bool_value, false); + ASSERT_EQ(response->int64_value, 42); + + // wrap up work to get all the service_event messages + auto start = std::chrono::steady_clock::now(); + while (events.size() < 4 && (std::chrono::steady_clock::now() - start) < timeout) { + rclcpp::spin_some(node); + } + + std::map> event_map; + for (auto & event : events) { + event_map[event->info.event_type] = event; + } + ASSERT_EQ(event_map.size(), 4U); + + rmw_gid_t client_gid; + rmw_get_gid_for_client(rcl_client_get_rmw_handle(client->get_client_handle().get()), &client_gid); + std::array client_gid_arr; + std::move(std::begin(client_gid.data), std::end(client_gid.data), client_gid_arr.begin()); + ASSERT_THAT( + client_gid_arr, + testing::Eq(event_map[ServiceEventInfo::REQUEST_SENT]->info.client_gid)); + + ASSERT_EQ( + event_map[ServiceEventInfo::REQUEST_SENT]->info.sequence_number, + event_map[ServiceEventInfo::REQUEST_RECEIVED]->info.sequence_number); + ASSERT_EQ( + event_map[ServiceEventInfo::RESPONSE_SENT]->info.sequence_number, + event_map[ServiceEventInfo::RESPONSE_RECEIVED]->info.sequence_number); + ASSERT_EQ( + event_map[ServiceEventInfo::REQUEST_SENT]->info.sequence_number, + event_map[ServiceEventInfo::RESPONSE_SENT]->info.sequence_number); + ASSERT_EQ( + event_map[ServiceEventInfo::REQUEST_RECEIVED]->info.sequence_number, + event_map[ServiceEventInfo::RESPONSE_RECEIVED]->info.sequence_number); + + ASSERT_EQ(event_map[ServiceEventInfo::REQUEST_SENT]->request[0].int64_value, 42); + ASSERT_EQ(event_map[ServiceEventInfo::REQUEST_SENT]->request[0].bool_value, true); + ASSERT_EQ(event_map[ServiceEventInfo::RESPONSE_SENT]->response[0].int64_value, 42); + ASSERT_EQ(event_map[ServiceEventInfo::RESPONSE_SENT]->response[0].bool_value, false); + ASSERT_EQ(event_map[ServiceEventInfo::RESPONSE_SENT]->request.size(), 0U); + ASSERT_EQ(event_map[ServiceEventInfo::REQUEST_RECEIVED]->response.size(), 0U); +} + +TEST_F(TestServiceIntrospection, service_introspection_enable_disable_events) +{ + node->set_parameter(rclcpp::Parameter("publish_service_events", false)); + node->set_parameter(rclcpp::Parameter("publish_client_events", false)); + + auto request = std::make_shared(); + request->set__bool_value(true); + request->set__int64_value(42); + auto future = client->async_send_request(request); + ASSERT_EQ( + rclcpp::FutureReturnCode::SUCCESS, + rclcpp::spin_until_future_complete(node, future, timeout)); + auto start = std::chrono::steady_clock::now(); + while ((std::chrono::steady_clock::now() - start) < timeout) { + rclcpp::spin_some(node); + } + EXPECT_EQ(events.size(), 0U); + + events.clear(); + node->set_parameter(rclcpp::Parameter("publish_service_events", false)); + node->set_parameter(rclcpp::Parameter("publish_client_events", true)); + future = client->async_send_request(request); + ASSERT_EQ( + rclcpp::FutureReturnCode::SUCCESS, + rclcpp::spin_until_future_complete(node, future, timeout)); + start = std::chrono::steady_clock::now(); + while ((std::chrono::steady_clock::now() - start) < timeout) { + rclcpp::spin_some(node); + } + EXPECT_EQ(events.size(), 2U); + + + events.clear(); + node->set_parameter(rclcpp::Parameter("publish_service_events", true)); + node->set_parameter(rclcpp::Parameter("publish_client_events", false)); + future = client->async_send_request(request); + ASSERT_EQ( + rclcpp::FutureReturnCode::SUCCESS, + rclcpp::spin_until_future_complete(node, future, timeout)); + start = std::chrono::steady_clock::now(); + while ((std::chrono::steady_clock::now() - start) < timeout) { + rclcpp::spin_some(node); + } + EXPECT_EQ(events.size(), 2U); + + events.clear(); + node->set_parameter(rclcpp::Parameter("publish_service_events", true)); + node->set_parameter(rclcpp::Parameter("publish_client_events", true)); + future = client->async_send_request(request); + ASSERT_EQ( + rclcpp::FutureReturnCode::SUCCESS, + rclcpp::spin_until_future_complete(node, future, timeout)); + start = std::chrono::steady_clock::now(); + while (events.size() < 4 && (std::chrono::steady_clock::now() - start) < timeout) { + rclcpp::spin_some(node); + } + EXPECT_EQ(events.size(), 4U); +} + +TEST_F(TestServiceIntrospection, service_introspection_enable_disable_event_content) +{ + node->set_parameter(rclcpp::Parameter("publish_service_content", false)); + node->set_parameter(rclcpp::Parameter("publish_client_content", false)); + + auto request = std::make_shared(); + request->set__bool_value(true); + request->set__int64_value(42); + auto future = client->async_send_request(request); + ASSERT_EQ( + rclcpp::FutureReturnCode::SUCCESS, + rclcpp::spin_until_future_complete(node, future, timeout)); + auto start = std::chrono::steady_clock::now(); + while (events.size() < 4 && (std::chrono::steady_clock::now() - start) < timeout) { + rclcpp::spin_some(node); + } + EXPECT_EQ(events.size(), 4U); + for (const auto & event : events) { + EXPECT_EQ(event->request.size(), 0U); + EXPECT_EQ(event->response.size(), 0U); + } + + + events.clear(); + node->set_parameter(rclcpp::Parameter("publish_service_content", false)); + node->set_parameter(rclcpp::Parameter("publish_client_content", true)); + future = client->async_send_request(request); + ASSERT_EQ( + rclcpp::FutureReturnCode::SUCCESS, + rclcpp::spin_until_future_complete(node, future, timeout)); + start = std::chrono::steady_clock::now(); + while (events.size() < 4 && (std::chrono::steady_clock::now() - start) < timeout) { + rclcpp::spin_some(node); + } + EXPECT_EQ(events.size(), 4U); + for (const auto & event : events) { + switch (event->info.event_type) { + case ServiceEventInfo::REQUEST_SENT: + EXPECT_EQ(event->request.size(), 1U); + break; + case ServiceEventInfo::REQUEST_RECEIVED: + EXPECT_EQ(event->request.size(), 0U); + break; + case ServiceEventInfo::RESPONSE_SENT: + EXPECT_EQ(event->response.size(), 0U); + break; + case ServiceEventInfo::RESPONSE_RECEIVED: + EXPECT_EQ(event->response.size(), 1U); + break; + } + } + + events.clear(); + node->set_parameter(rclcpp::Parameter("publish_service_content", true)); + node->set_parameter(rclcpp::Parameter("publish_client_content", false)); + future = client->async_send_request(request); + ASSERT_EQ( + rclcpp::FutureReturnCode::SUCCESS, + rclcpp::spin_until_future_complete(node, future, timeout)); + start = std::chrono::steady_clock::now(); + while (events.size() < 4 && (std::chrono::steady_clock::now() - start) < timeout) { + rclcpp::spin_some(node); + } + EXPECT_EQ(events.size(), 4U); + for (const auto & event : events) { + switch (event->info.event_type) { + case ServiceEventInfo::REQUEST_SENT: + EXPECT_EQ(event->request.size(), 0U); + break; + case ServiceEventInfo::REQUEST_RECEIVED: + EXPECT_EQ(event->request.size(), 1U); + break; + case ServiceEventInfo::RESPONSE_SENT: + EXPECT_EQ(event->response.size(), 1U); + break; + case ServiceEventInfo::RESPONSE_RECEIVED: + EXPECT_EQ(event->response.size(), 0U); + break; + } + } + + events.clear(); + node->set_parameter(rclcpp::Parameter("publish_service_content", true)); + node->set_parameter(rclcpp::Parameter("publish_client_content", true)); + future = client->async_send_request(request); + ASSERT_EQ( + rclcpp::FutureReturnCode::SUCCESS, + rclcpp::spin_until_future_complete(node, future, timeout)); + start = std::chrono::steady_clock::now(); + while (events.size() < 4 && (std::chrono::steady_clock::now() - start) < timeout) { + rclcpp::spin_some(node); + } + EXPECT_EQ(events.size(), 4U); + for (const auto & event : events) { + switch (event->info.event_type) { + case ServiceEventInfo::REQUEST_SENT: + case ServiceEventInfo::REQUEST_RECEIVED: + EXPECT_EQ(event->request.size(), 1U); + break; + case ServiceEventInfo::RESPONSE_SENT: + case ServiceEventInfo::RESPONSE_RECEIVED: + EXPECT_EQ(event->response.size(), 1U); + break; + } + } +} diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp index a9bf2a5008..184b1c667c 100644 --- a/rclcpp_action/include/rclcpp_action/client.hpp +++ b/rclcpp_action/include/rclcpp_action/client.hpp @@ -31,6 +31,7 @@ #include "rclcpp/exceptions.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/node_interfaces/node_base_interface.hpp" +#include #include "rclcpp/node_interfaces/node_logging_interface.hpp" #include "rclcpp/node_interfaces/node_graph_interface.hpp" #include "rclcpp/logger.hpp" @@ -185,6 +186,7 @@ class ClientBase : public rclcpp::Waitable ClientBase( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock, rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, const std::string & action_name, const rosidl_action_type_support_t * type_support, @@ -392,12 +394,13 @@ class Client : public ClientBase Client( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock, rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, const std::string & action_name, const rcl_action_client_options_t & client_options = rcl_action_client_get_default_options() ) : ClientBase( - node_base, node_graph, node_logging, action_name, + node_base, node_graph, node_clock, node_logging, action_name, rosidl_typesupport_cpp::get_action_type_support_handle(), client_options) { diff --git a/rclcpp_action/include/rclcpp_action/create_client.hpp b/rclcpp_action/include/rclcpp_action/create_client.hpp index f594bca78d..f262117624 100644 --- a/rclcpp_action/include/rclcpp_action/create_client.hpp +++ b/rclcpp_action/include/rclcpp_action/create_client.hpp @@ -44,6 +44,7 @@ typename Client::SharedPtr create_client( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface, rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_interface, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_interface, rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_interface, rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr node_waitables_interface, const std::string & name, @@ -83,6 +84,7 @@ create_client( new Client( node_base_interface, node_graph_interface, + node_clock_interface, node_logging_interface, name, options), @@ -111,6 +113,7 @@ create_client( return rclcpp_action::create_client( node->get_node_base_interface(), node->get_node_graph_interface(), + node->get_node_clock_interface(), node->get_node_logging_interface(), node->get_node_waitables_interface(), name, diff --git a/rclcpp_action/src/client.cpp b/rclcpp_action/src/client.cpp index e687ab3400..69042a525f 100644 --- a/rclcpp_action/src/client.cpp +++ b/rclcpp_action/src/client.cpp @@ -23,6 +23,7 @@ #include "rcl_action/action_client.h" #include "rcl_action/wait.h" #include "rclcpp/node_interfaces/node_base_interface.hpp" +#include #include "rclcpp/node_interfaces/node_logging_interface.hpp" #include "rclcpp_action/client.hpp" @@ -37,12 +38,14 @@ class ClientBaseImpl ClientBaseImpl( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock, rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, const std::string & action_name, const rosidl_action_type_support_t * type_support, const rcl_action_client_options_t & client_options) : node_graph_(node_graph), node_handle(node_base->get_shared_rcl_node_handle()), + node_clock_(std::move(node_clock)), logger(node_logging->get_logger().get_child("rclcpp_action")), random_bytes_generator(std::random_device{}()) { @@ -68,8 +71,8 @@ class ClientBaseImpl }); *client_handle = rcl_action_get_zero_initialized_client(); rcl_ret_t ret = rcl_action_client_init( - client_handle.get(), node_handle.get(), type_support, - action_name.c_str(), &client_options); + client_handle.get(), node_handle.get(), node_clock_->get_clock()->get_clock_handle(), + type_support, action_name.c_str(), &client_options); if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error( ret, "could not initialize rcl action client"); @@ -105,6 +108,7 @@ class ClientBaseImpl // node_handle must be destroyed after client_handle to prevent memory leak std::shared_ptr node_handle{nullptr}; std::shared_ptr client_handle{nullptr}; + rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_{nullptr}; rclcpp::Logger logger; using ResponseCallback = std::function response)>; @@ -125,12 +129,13 @@ class ClientBaseImpl ClientBase::ClientBase( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock, rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, const std::string & action_name, const rosidl_action_type_support_t * type_support, const rcl_action_client_options_t & client_options) : pimpl_(new ClientBaseImpl( - node_base, node_graph, node_logging, action_name, type_support, client_options)) + node_base, node_graph, node_clock, node_logging, action_name, type_support, client_options)) { } diff --git a/rclcpp_action/test/test_client.cpp b/rclcpp_action/test/test_client.cpp index b94a82d500..7d03ec0870 100644 --- a/rclcpp_action/test/test_client.cpp +++ b/rclcpp_action/test/test_client.cpp @@ -321,6 +321,7 @@ TEST_F(TestClient, construction_and_destruction_callback_group) rclcpp_action::create_client( client_node->get_node_base_interface(), client_node->get_node_graph_interface(), + client_node->get_node_clock_interface(), client_node->get_node_logging_interface(), client_node->get_node_waitables_interface(), action_name, diff --git a/rclcpp_lifecycle/src/lifecycle_node.cpp b/rclcpp_lifecycle/src/lifecycle_node.cpp index 723c7bc8c1..835d250200 100644 --- a/rclcpp_lifecycle/src/lifecycle_node.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node.cpp @@ -100,7 +100,8 @@ LifecycleNode::LifecycleNode( options.parameter_event_qos(), options.parameter_event_publisher_options(), options.allow_undeclared_parameters(), - options.automatically_declare_parameters_from_overrides() + options.automatically_declare_parameters_from_overrides(), + options.enable_service_introspection() )), node_time_source_(new rclcpp::node_interfaces::NodeTimeSource( node_base_, From d46e57e035ff2ac8874fb2455a2e0508e1ddc7a0 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 6 Feb 2023 21:28:02 +0000 Subject: [PATCH 2/3] Implementation of service introspection. To do this, we add a new method on the Client and Service classes that allows the user to change the introspection method at runtime. These end up calling into the rcl layer to do the actual configuration, at which point service introspection messages will be sent as configured. Signed-off-by: Chris Lalancette --- rclcpp/CMakeLists.txt | 1 - rclcpp/include/rclcpp/client.hpp | 36 ++++- rclcpp/include/rclcpp/create_client.hpp | 42 +----- rclcpp/include/rclcpp/create_service.hpp | 43 +----- rclcpp/include/rclcpp/node.hpp | 42 ------ rclcpp/include/rclcpp/node_impl.hpp | 84 ++--------- .../node_interfaces/node_parameters.hpp | 3 +- .../node_service_introspection.hpp | 61 -------- .../node_service_introspection_interface.hpp | 47 ------ rclcpp/include/rclcpp/node_options.hpp | 13 -- rclcpp/include/rclcpp/parameter_service.hpp | 5 - rclcpp/include/rclcpp/service.hpp | 35 ++++- rclcpp/src/rclcpp/client.cpp | 3 +- rclcpp/src/rclcpp/node.cpp | 13 +- .../node_interfaces/node_parameters.cpp | 7 +- .../node_service_introspection.cpp | 134 ------------------ rclcpp/src/rclcpp/node_options.cpp | 16 --- rclcpp/src/rclcpp/parameter_service.cpp | 28 ++-- rclcpp/src/rclcpp/service.cpp | 5 +- rclcpp/test/rclcpp/CMakeLists.txt | 1 - .../test_node_service_introspection.cpp | 99 ------------- rclcpp/test/rclcpp/test_client.cpp | 16 +-- rclcpp/test/rclcpp/test_service.cpp | 22 +-- .../rclcpp/test_service_introspection.cpp | 96 +++++++------ .../include/rclcpp_action/client.hpp | 5 +- .../include/rclcpp_action/create_client.hpp | 3 - rclcpp_action/src/client.cpp | 11 +- rclcpp_action/test/test_client.cpp | 1 - rclcpp_lifecycle/src/lifecycle_node.cpp | 3 +- 29 files changed, 180 insertions(+), 695 deletions(-) delete mode 100644 rclcpp/include/rclcpp/node_interfaces/node_service_introspection.hpp delete mode 100644 rclcpp/include/rclcpp/node_interfaces/node_service_introspection_interface.hpp delete mode 100644 rclcpp/src/rclcpp/node_interfaces/node_service_introspection.cpp delete mode 100644 rclcpp/test/rclcpp/node_interfaces/test_node_service_introspection.cpp diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 3a65318de9..3abd999f3d 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -78,7 +78,6 @@ set(${PROJECT_NAME}_SRCS src/rclcpp/node_interfaces/node_logging.cpp src/rclcpp/node_interfaces/node_parameters.cpp src/rclcpp/node_interfaces/node_services.cpp - src/rclcpp/node_interfaces/node_service_introspection.cpp src/rclcpp/node_interfaces/node_time_source.cpp src/rclcpp/node_interfaces/node_timers.cpp src/rclcpp/node_interfaces/node_topics.cpp diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index c4f8bc8bec..ef1b08bfca 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -32,8 +32,10 @@ #include "rcl/client.h" #include "rcl/error_handling.h" #include "rcl/event_callback.h" +#include "rcl/service_introspection.h" #include "rcl/wait.h" +#include "rclcpp/clock.hpp" #include "rclcpp/detail/cpp_callback_trampoline.hpp" #include "rclcpp/exceptions.hpp" #include "rclcpp/expand_topic_or_service_name.hpp" @@ -471,12 +473,12 @@ class Client : public ClientBase : ClientBase(node_base, node_graph) { using rosidl_typesupport_cpp::get_service_type_support_handle; - auto service_type_support_handle = + service_type_support_handle_ = get_service_type_support_handle(); rcl_ret_t ret = rcl_client_init( this->get_client_handle().get(), this->get_rcl_node_handle(), - service_type_support_handle, + service_type_support_handle_, service_name.c_str(), &client_options); if (ret != RCL_RET_OK) { @@ -782,6 +784,33 @@ class Client : public ClientBase return old_size - pending_requests_.size(); } + /// Configure client introspection. + /** + * \param[in] clock clock to use to generate introspection timestamps + * \param[in] qos_service_event_pub QoS settings to use when creating the introspection publisher + * \param[in] introspection_state the state to set introspection to + */ + void + configure_introspection( + Clock::SharedPtr clock, const QoS & qos_service_event_pub, + rcl_service_introspection_state_t introspection_state) + { + rcl_publisher_options_t pub_opts = rcl_publisher_get_default_options(); + pub_opts.qos = qos_service_event_pub.get_rmw_qos_profile(); + + rcl_ret_t ret = rcl_client_configure_service_introspection( + client_handle_.get(), + node_handle_.get(), + clock->get_clock_handle(), + service_type_support_handle_, + pub_opts, + introspection_state); + + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "failed to configure client introspection"); + } + } + protected: using CallbackTypeValueVariant = std::tuple; using CallbackWithRequestTypeValueVariant = std::tuple< @@ -832,6 +861,9 @@ class Client : public ClientBase CallbackInfoVariant>> pending_requests_; std::mutex pending_requests_mutex_; + +private: + const rosidl_service_type_support_t * service_type_support_handle_; }; } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/create_client.hpp b/rclcpp/include/rclcpp/create_client.hpp index d2720ca0cd..00e6ff3c0e 100644 --- a/rclcpp/include/rclcpp/create_client.hpp +++ b/rclcpp/include/rclcpp/create_client.hpp @@ -19,7 +19,6 @@ #include #include "rclcpp/node_interfaces/node_base_interface.hpp" -#include "rclcpp/node_interfaces/node_clock_interface.hpp" #include "rclcpp/node_interfaces/node_services_interface.hpp" #include "rclcpp/qos.hpp" #include "rmw/rmw.h" @@ -45,15 +44,15 @@ create_client( std::shared_ptr node_base, std::shared_ptr node_graph, std::shared_ptr node_services, - std::shared_ptr node_clock, const std::string & service_name, - const rclcpp::QoS & qos, - rclcpp::CallbackGroup::SharedPtr group, - bool enable_service_introspection) + const rclcpp::QoS & qos = rclcpp::ServicesQoS(), + rclcpp::CallbackGroup::SharedPtr group = nullptr) { return create_client( - node_base, node_graph, node_services, node_clock, service_name, - qos.get_rmw_qos_profile(), group, enable_service_introspection); + node_base, node_graph, node_services, + service_name, + qos.get_rmw_qos_profile(), + group); } /// Create a service client with a given type. @@ -64,39 +63,12 @@ create_client( std::shared_ptr node_base, std::shared_ptr node_graph, std::shared_ptr node_services, - std::shared_ptr node_clock, - const std::string & service_name, - const rmw_qos_profile_t & qos_profile, - rclcpp::CallbackGroup::SharedPtr group, - bool enable_service_introspection) -{ - return create_client( - node_base, node_graph, node_services, node_clock, service_name, qos_profile, - rcl_publisher_get_default_options().qos, group, enable_service_introspection); -} - -/// Create a service client with a given type and qos profiles -/// \internal -template -typename rclcpp::Client::SharedPtr -create_client( - std::shared_ptr node_base, - std::shared_ptr node_graph, - std::shared_ptr node_services, - std::shared_ptr node_clock, const std::string & service_name, const rmw_qos_profile_t & qos_profile, - const rmw_qos_profile_t & service_event_publisher_qos_profile, - rclcpp::CallbackGroup::SharedPtr group, - bool enable_service_introspection) + rclcpp::CallbackGroup::SharedPtr group) { rcl_client_options_t options = rcl_client_get_default_options(); options.qos = qos_profile; - if (enable_service_introspection) { - options.enable_service_introspection = enable_service_introspection; - options.clock = node_clock->get_clock()->get_clock_handle(); - options.event_publisher_options.qos = service_event_publisher_qos_profile; - } auto cli = rclcpp::Client::make_shared( node_base.get(), diff --git a/rclcpp/include/rclcpp/create_service.hpp b/rclcpp/include/rclcpp/create_service.hpp index b3cc2983df..42c253a526 100644 --- a/rclcpp/include/rclcpp/create_service.hpp +++ b/rclcpp/include/rclcpp/create_service.hpp @@ -20,7 +20,6 @@ #include #include "rclcpp/node_interfaces/node_base_interface.hpp" -#include "rclcpp/node_interfaces/node_clock_interface.hpp" #include "rclcpp/node_interfaces/node_services_interface.hpp" #include "rclcpp/visibility_control.hpp" #include "rmw/rmw.h" @@ -44,65 +43,33 @@ typename rclcpp::Service::SharedPtr create_service( std::shared_ptr node_base, std::shared_ptr node_services, - std::shared_ptr node_clock, const std::string & service_name, CallbackT && callback, const rclcpp::QoS & qos, - rclcpp::CallbackGroup::SharedPtr group, - bool enable_service_introspection) + rclcpp::CallbackGroup::SharedPtr group) { return create_service( - node_base, node_services, node_clock, service_name, std::forward(callback), - qos.get_rmw_qos_profile(), group, enable_service_introspection); + node_base, node_services, service_name, + std::forward(callback), qos.get_rmw_qos_profile(), group); } - -/// 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, - std::shared_ptr node_clock, - const std::string & service_name, - CallbackT && callback, - const rmw_qos_profile_t & qos_profile, - rclcpp::CallbackGroup::SharedPtr group, - bool enable_service_introspection -) -{ - return create_service( - node_base, node_services, node_clock, service_name, std::forward(callback), - qos_profile, rcl_publisher_get_default_options().qos, group, enable_service_introspection); -} - -/// Create a service with a given type and qos profiles +/// 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, - std::shared_ptr node_clock, const std::string & service_name, CallbackT && callback, const rmw_qos_profile_t & qos_profile, - const rmw_qos_profile_t & service_event_publisher_qos_profile, - rclcpp::CallbackGroup::SharedPtr group, - bool enable_service_introspection -) + rclcpp::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; - if (enable_service_introspection) { - service_options.enable_service_introspection = enable_service_introspection; - service_options.clock = node_clock->get_clock()->get_clock_handle(); - service_options.event_publisher_options.qos = service_event_publisher_qos_profile; - } auto serv = Service::make_shared( node_base->get_shared_rcl_node_handle(), diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index 32b1419e8a..7ecb67e9b1 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -53,7 +53,6 @@ #include "rclcpp/node_interfaces/node_logging_interface.hpp" #include "rclcpp/node_interfaces/node_parameters_interface.hpp" #include "rclcpp/node_interfaces/node_services_interface.hpp" -#include "rclcpp/node_interfaces/node_service_introspection_interface.hpp" #include "rclcpp/node_interfaces/node_time_source_interface.hpp" #include "rclcpp/node_interfaces/node_timers_interface.hpp" #include "rclcpp/node_interfaces/node_topics_interface.hpp" @@ -284,22 +283,6 @@ class Node : public std::enable_shared_from_this const rclcpp::QoS & qos = rclcpp::ServicesQoS(), rclcpp::CallbackGroup::SharedPtr group = nullptr); - /// Create and return a Client. - /** - * \param[in] service_name The name on which the service is accessible. - * \param[in] qos Quality of service profile for client. - * \param[in] service_event_publisher_qos QOS profile for the service event publisher. - * \param[in] group Callback group to handle the reply to service calls. - * \return Shared pointer to the created client. - */ - template - typename Client::SharedPtr - create_client( - const std::string & service_name, - const rclcpp::QoS & qos, - const rclcpp::QoS & service_event_publisher_qos, - rclcpp::CallbackGroup::SharedPtr group); - /// Create and return a Service. /** * \param[in] service_name The topic to service on. @@ -334,25 +317,6 @@ class Node : public std::enable_shared_from_this const rclcpp::QoS & qos = rclcpp::ServicesQoS(), rclcpp::CallbackGroup::SharedPtr group = nullptr); - - /// Create and return a Service. - /** - * \param[in] service_name The topic to service on. - * \param[in] callback User-defined callback function. - * \param[in] qos Quality of service profile for the service. - * \param[in] service_event_publisher_qos QOS profile for the service event publisher. - * \param[in] group Callback group to call the service. - * \return Shared pointer to the created service. - */ - template - typename rclcpp::Service::SharedPtr - create_service( - const std::string & service_name, - CallbackT && callback, - const rclcpp::QoS & qos, - const rclcpp::QoS & service_event_publisher_qos, - rclcpp::CallbackGroup::SharedPtr group); - /// Create and return a GenericPublisher. /** * The returned pointer will never be empty, but this function can throw various exceptions, for @@ -1475,11 +1439,6 @@ class Node : public std::enable_shared_from_this rclcpp::node_interfaces::NodeServicesInterface::SharedPtr get_node_services_interface(); - /// Return the Node's internal NodeServicesInterface implementation. - RCLCPP_PUBLIC - rclcpp::node_interfaces::NodeServiceIntrospectionInterface::SharedPtr - get_node_service_introspection_interface(); - /// Return the Node's internal NodeWaitablesInterface implementation. RCLCPP_PUBLIC rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr @@ -1628,7 +1587,6 @@ class Node : public std::enable_shared_from_this rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_; rclcpp::node_interfaces::NodeTimeSourceInterface::SharedPtr node_time_source_; rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr node_waitables_; - rclcpp::node_interfaces::NodeServiceIntrospectionInterface::SharedPtr node_service_introspection_; const rclcpp::NodeOptions node_options_; const std::string sub_namespace_; diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index b8325aa923..d6b090d4d1 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -143,41 +143,13 @@ Node::create_client( const rclcpp::QoS & qos, rclcpp::CallbackGroup::SharedPtr group) { - typename Client::SharedPtr cli = rclcpp::create_client( + return rclcpp::create_client( node_base_, node_graph_, node_services_, - node_clock_, extend_name_with_sub_namespace(service_name, this->get_sub_namespace()), qos, - group, - node_options_.enable_service_introspection()); - - node_service_introspection_->register_client(cli); - return cli; -} - -template -typename Client::SharedPtr -Node::create_client( - const std::string & service_name, - const rclcpp::QoS & qos, - const rclcpp::QoS & service_event_publisher_qos, - rclcpp::CallbackGroup::SharedPtr group) -{ - typename Client::SharedPtr cli = rclcpp::create_client( - node_base_, - node_graph_, - node_services_, - node_clock_, - extend_name_with_sub_namespace(service_name, this->get_sub_namespace()), - qos.get_rmw_qos_profile(), - service_event_publisher_qos.get_rmw_qos_profile(), - group, - node_options_.enable_service_introspection()); - - node_service_introspection_->register_client(cli); - return cli; + group); } template @@ -187,18 +159,13 @@ Node::create_client( const rmw_qos_profile_t & qos_profile, rclcpp::CallbackGroup::SharedPtr group) { - typename Client::SharedPtr cli = rclcpp::create_client( + return rclcpp::create_client( node_base_, node_graph_, node_services_, - node_clock_, extend_name_with_sub_namespace(service_name, this->get_sub_namespace()), qos_profile, - group, - node_options_.enable_service_introspection()); - - node_service_introspection_->register_client(cli); - return cli; + group); } template @@ -209,42 +176,13 @@ Node::create_service( const rclcpp::QoS & qos, rclcpp::CallbackGroup::SharedPtr group) { - typename rclcpp::Service::SharedPtr serv = rclcpp::create_service( + return rclcpp::create_service( node_base_, node_services_, - node_clock_, extend_name_with_sub_namespace(service_name, this->get_sub_namespace()), std::forward(callback), qos, - group, - node_options_.enable_service_introspection()); - - node_service_introspection_->register_service(serv); - return serv; -} - -template -typename rclcpp::Service::SharedPtr -Node::create_service( - const std::string & service_name, - CallbackT && callback, - const rclcpp::QoS & qos, - const rclcpp::QoS & service_event_publisher_qos, - rclcpp::CallbackGroup::SharedPtr group) -{ - typename rclcpp::Service::SharedPtr serv = rclcpp::create_service( - node_base_, - node_services_, - node_clock_, - extend_name_with_sub_namespace(service_name, this->get_sub_namespace()), - std::forward(callback), - qos.get_rmw_qos_profile(), - service_event_publisher_qos.get_rmw_qos_profile(), - group, - node_options_.enable_service_introspection()); - - node_service_introspection_->register_service(serv); - return serv; + group); } template @@ -255,19 +193,13 @@ Node::create_service( const rmw_qos_profile_t & qos_profile, rclcpp::CallbackGroup::SharedPtr group) { - typename rclcpp::Service::SharedPtr serv = rclcpp::create_service( + return rclcpp::create_service( node_base_, node_services_, - node_clock_, extend_name_with_sub_namespace(service_name, this->get_sub_namespace()), std::forward(callback), qos_profile, - group, - node_options_.enable_service_introspection() - ); - - node_service_introspection_->register_service(serv); - return serv; + group); } template diff --git a/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp b/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp index e963411517..ffbb400c11 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp @@ -108,8 +108,7 @@ class NodeParameters : public NodeParametersInterface const rclcpp::QoS & parameter_event_qos, const rclcpp::PublisherOptionsBase & parameter_event_publisher_options, bool allow_undeclared_parameters, - bool automatically_declare_parameters_from_overrides, - bool enable_service_introspection_for_parameter_service); + bool automatically_declare_parameters_from_overrides); RCLCPP_PUBLIC virtual diff --git a/rclcpp/include/rclcpp/node_interfaces/node_service_introspection.hpp b/rclcpp/include/rclcpp/node_interfaces/node_service_introspection.hpp deleted file mode 100644 index 59beac5a12..0000000000 --- a/rclcpp/include/rclcpp/node_interfaces/node_service_introspection.hpp +++ /dev/null @@ -1,61 +0,0 @@ -// Copyright 2022 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__NODE_INTERFACES__NODE_SERVICE_INTROSPECTION_HPP_ -#define RCLCPP__NODE_INTERFACES__NODE_SERVICE_INTROSPECTION_HPP_ - -#include -#include "rclcpp/macros.hpp" -#include "rclcpp/node_interfaces/node_base_interface.hpp" -#include "rclcpp/node_interfaces/node_parameters_interface.hpp" -#include "rclcpp/node_interfaces/node_service_introspection_interface.hpp" -#include "rclcpp/service.hpp" -#include "rclcpp/visibility_control.hpp" - -namespace rclcpp -{ -namespace node_interfaces -{ -class NodeServiceIntrospection : public NodeServiceIntrospectionInterface -{ -public: - RCLCPP_SMART_PTR_ALIASES_ONLY(NodeServiceIntrospection) - - RCLCPP_PUBLIC - explicit NodeServiceIntrospection( - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr & node_base, - const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & node_parameters); - - RCLCPP_PUBLIC - size_t - register_service(rclcpp::ServiceBase::SharedPtr service) override; - - RCLCPP_PUBLIC - size_t - register_client(rclcpp::ClientBase::SharedPtr client) override; - - RCLCPP_PUBLIC - ~NodeServiceIntrospection() override = default; - -private: - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_; - std::vector services_; - std::vector clients_; - rclcpp::node_interfaces::PostSetParametersCallbackHandle::SharedPtr - post_set_parameters_callback_handle_; -}; - -} // namespace node_interfaces -} // namespace rclcpp -#endif // RCLCPP__NODE_INTERFACES__NODE_SERVICE_INTROSPECTION_HPP_ diff --git a/rclcpp/include/rclcpp/node_interfaces/node_service_introspection_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_service_introspection_interface.hpp deleted file mode 100644 index e671e4d3bd..0000000000 --- a/rclcpp/include/rclcpp/node_interfaces/node_service_introspection_interface.hpp +++ /dev/null @@ -1,47 +0,0 @@ -// Copyright 2022 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__NODE_INTERFACES__NODE_SERVICE_INTROSPECTION_INTERFACE_HPP_ -#define RCLCPP__NODE_INTERFACES__NODE_SERVICE_INTROSPECTION_INTERFACE_HPP_ - -#include "rclcpp/macros.hpp" -#include "rclcpp/visibility_control.hpp" -#include "rclcpp/service.hpp" -#include "rclcpp/client.hpp" - -namespace rclcpp -{ -namespace node_interfaces -{ -class NodeServiceIntrospectionInterface -{ -public: - RCLCPP_SMART_PTR_ALIASES_ONLY(NodeServiceIntrospectionInterface) - - RCLCPP_PUBLIC - size_t - virtual register_service(rclcpp::ServiceBase::SharedPtr service) = 0; - - RCLCPP_PUBLIC - size_t - virtual register_client(rclcpp::ClientBase::SharedPtr client) = 0; - - RCLCPP_PUBLIC - virtual - ~NodeServiceIntrospectionInterface() = default; -}; - -} // namespace node_interfaces -} // namespace rclcpp -#endif // RCLCPP__NODE_INTERFACES__NODE_SERVICE_INTROSPECTION_INTERFACE_HPP_ diff --git a/rclcpp/include/rclcpp/node_options.hpp b/rclcpp/include/rclcpp/node_options.hpp index 59dcd53c5a..47a97fd3c4 100644 --- a/rclcpp/include/rclcpp/node_options.hpp +++ b/rclcpp/include/rclcpp/node_options.hpp @@ -45,7 +45,6 @@ class NodeOptions * - use_global_arguments = true * - use_intra_process_comms = false * - enable_topic_statistics = false - * - enable_service_introspection = false * - start_parameter_services = true * - start_parameter_event_publisher = true * - clock_type = RCL_ROS_TIME @@ -202,16 +201,6 @@ class NodeOptions bool enable_topic_statistics() const; - /// Return the enable_service_introspection flag - RCLCPP_PUBLIC - bool - enable_service_introspection() const; - - /// Set the enable_service_introspection flag - RCLCPP_PUBLIC - NodeOptions & - enable_service_introspection(bool enable_service_introspection); - /// Set the enable_topic_statistics flag, return this for parameter idiom. /** * If true, topic statistics collection and publication will be enabled @@ -422,8 +411,6 @@ class NodeOptions bool enable_topic_statistics_ {false}; - bool enable_service_introspection_ {false}; - bool start_parameter_services_ {true}; bool start_parameter_event_publisher_ {true}; diff --git a/rclcpp/include/rclcpp/parameter_service.hpp b/rclcpp/include/rclcpp/parameter_service.hpp index 9fd1d65b28..a952939aca 100644 --- a/rclcpp/include/rclcpp/parameter_service.hpp +++ b/rclcpp/include/rclcpp/parameter_service.hpp @@ -45,15 +45,12 @@ class ParameterService ParameterService( const std::shared_ptr node_base, const std::shared_ptr node_services, - const std::shared_ptr node_clock, rclcpp::node_interfaces::NodeParametersInterface * node_params, const rmw_qos_profile_t & qos_profile) : ParameterService( node_base, node_services, - node_clock, node_params, - false, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos_profile))) {} @@ -61,9 +58,7 @@ class ParameterService ParameterService( const std::shared_ptr node_base, const std::shared_ptr node_services, - const std::shared_ptr node_clock, rclcpp::node_interfaces::NodeParametersInterface * node_params, - bool enable_service_introspection = false, const rclcpp::QoS & qos_profile = rclcpp::ParametersQoS()); private: diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index 9e258a0223..7ba47e8473 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -26,6 +26,7 @@ #include "rcl/error_handling.h" #include "rcl/event_callback.h" #include "rcl/service.h" +#include "rcl/service_introspection.h" #include "rmw/error_handling.h" #include "rmw/impl/cpp/demangle.hpp" @@ -34,6 +35,7 @@ #include "tracetools/tracetools.h" #include "rclcpp/any_service_callback.hpp" +#include "rclcpp/clock.hpp" #include "rclcpp/detail/cpp_callback_trampoline.hpp" #include "rclcpp/exceptions.hpp" #include "rclcpp/expand_topic_or_service_name.hpp" @@ -311,7 +313,7 @@ class Service : ServiceBase(node_handle), any_callback_(any_callback) { using rosidl_typesupport_cpp::get_service_type_support_handle; - auto service_type_support_handle = get_service_type_support_handle(); + service_type_support_handle_ = get_service_type_support_handle(); // rcl does the static memory allocation here service_handle_ = std::shared_ptr( @@ -331,7 +333,7 @@ class Service rcl_ret_t ret = rcl_service_init( service_handle_.get(), node_handle.get(), - service_type_support_handle, + service_type_support_handle_, service_name.c_str(), &service_options); if (ret != RCL_RET_OK) { @@ -487,10 +489,39 @@ class Service } } + /// Configure client introspection. + /** + * \param[in] clock clock to use to generate introspection timestamps + * \param[in] qos_service_event_pub QoS settings to use when creating the introspection publisher + * \param[in] introspection_state the state to set introspection to + */ + void + configure_introspection( + Clock::SharedPtr clock, const QoS & qos_service_event_pub, + rcl_service_introspection_state_t introspection_state) + { + rcl_publisher_options_t pub_opts = rcl_publisher_get_default_options(); + pub_opts.qos = qos_service_event_pub.get_rmw_qos_profile(); + + rcl_ret_t ret = rcl_service_configure_service_introspection( + service_handle_.get(), + node_handle_.get(), + clock->get_clock_handle(), + service_type_support_handle_, + pub_opts, + introspection_state); + + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "failed to configure service introspection"); + } + } + private: RCLCPP_DISABLE_COPY(Service) AnyServiceCallback any_callback_; + + const rosidl_service_type_support_t * service_type_support_handle_; }; } // namespace rclcpp diff --git a/rclcpp/src/rclcpp/client.cpp b/rclcpp/src/rclcpp/client.cpp index 4adc6d4a96..e33db71ce2 100644 --- a/rclcpp/src/rclcpp/client.cpp +++ b/rclcpp/src/rclcpp/client.cpp @@ -23,9 +23,11 @@ #include "rcl/graph.h" #include "rcl/node.h" #include "rcl/wait.h" + #include "rclcpp/exceptions.hpp" #include "rclcpp/node_interfaces/node_base_interface.hpp" #include "rclcpp/node_interfaces/node_graph_interface.hpp" +#include "rclcpp/qos.hpp" #include "rclcpp/utilities.hpp" #include "rclcpp/logging.hpp" @@ -241,7 +243,6 @@ ClientBase::set_on_new_response_callback(rcl_event_callback_t callback, const vo user_data); if (RCL_RET_OK != ret) { - using rclcpp::exceptions::throw_from_rcl_error; throw_from_rcl_error(ret, "failed to set the on new response callback for client"); } } diff --git a/rclcpp/src/rclcpp/node.cpp b/rclcpp/src/rclcpp/node.cpp index d473534786..c1fbdb1f98 100644 --- a/rclcpp/src/rclcpp/node.cpp +++ b/rclcpp/src/rclcpp/node.cpp @@ -32,7 +32,6 @@ #include "rclcpp/node_interfaces/node_graph.hpp" #include "rclcpp/node_interfaces/node_logging.hpp" #include "rclcpp/node_interfaces/node_parameters.hpp" -#include "rclcpp/node_interfaces/node_service_introspection.hpp" #include "rclcpp/node_interfaces/node_services.hpp" #include "rclcpp/node_interfaces/node_time_source.hpp" #include "rclcpp/node_interfaces/node_timers.hpp" @@ -194,8 +193,7 @@ Node::Node( get_parameter_events_qos(*node_base_, options), options.parameter_event_publisher_options(), options.allow_undeclared_parameters(), - options.automatically_declare_parameters_from_overrides(), - options.enable_service_introspection() + options.automatically_declare_parameters_from_overrides() )), node_time_source_(new rclcpp::node_interfaces::NodeTimeSource( node_base_, @@ -209,9 +207,6 @@ Node::Node( options.use_clock_thread() )), node_waitables_(new rclcpp::node_interfaces::NodeWaitables(node_base_.get())), - node_service_introspection_(new rclcpp::node_interfaces::NodeServiceIntrospection( - node_base_, - node_parameters_)), node_options_(options), sub_namespace_(""), effective_namespace_(create_effective_namespace(this->get_namespace(), sub_namespace_)) @@ -598,12 +593,6 @@ Node::get_node_services_interface() return node_services_; } -rclcpp::node_interfaces::NodeServiceIntrospectionInterface::SharedPtr -Node::get_node_service_introspection_interface() -{ - return node_service_introspection_; -} - rclcpp::node_interfaces::NodeParametersInterface::SharedPtr Node::get_node_parameters_interface() { diff --git a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp index f3e5ae46ad..9dafcba381 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp @@ -76,8 +76,7 @@ NodeParameters::NodeParameters( const rclcpp::QoS & parameter_event_qos, const rclcpp::PublisherOptionsBase & parameter_event_publisher_options, bool allow_undeclared_parameters, - bool automatically_declare_parameters_from_overrides, - bool enable_service_introspection_for_parameter_service) + bool automatically_declare_parameters_from_overrides) : allow_undeclared_(allow_undeclared_parameters), events_publisher_(nullptr), node_logging_(node_logging), @@ -92,9 +91,7 @@ NodeParameters::NodeParameters( publisher_options.allocator = std::make_shared(); if (start_parameter_services) { - parameter_service_ = std::make_shared( - node_base, node_services, node_clock, - this, enable_service_introspection_for_parameter_service); + parameter_service_ = std::make_shared(node_base, node_services, this); } if (start_parameter_event_publisher) { diff --git a/rclcpp/src/rclcpp/node_interfaces/node_service_introspection.cpp b/rclcpp/src/rclcpp/node_interfaces/node_service_introspection.cpp deleted file mode 100644 index 0c958b1658..0000000000 --- a/rclcpp/src/rclcpp/node_interfaces/node_service_introspection.cpp +++ /dev/null @@ -1,134 +0,0 @@ -// Copyright 2022 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 -#include "rclcpp/node_interfaces/node_service_introspection.hpp" -#include "rcl/service_introspection.h" -#include "rcl/client.h" -#include "rcl/service.h" - -using rclcpp::node_interfaces::NodeServiceIntrospection; - - -NodeServiceIntrospection::NodeServiceIntrospection( - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr & node_base, - const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & node_parameters) -: node_base_(node_base) -{ - // declare service introspection parameters - if (!node_parameters->has_parameter("publish_service_events")) { - node_parameters->declare_parameter("publish_service_events", rclcpp::ParameterValue(true)); - } - if (!node_parameters->has_parameter("publish_service_content")) { - node_parameters->declare_parameter("publish_service_content", rclcpp::ParameterValue(true)); - } - if (!node_parameters->has_parameter("publish_client_events")) { - node_parameters->declare_parameter("publish_client_events", rclcpp::ParameterValue(true)); - } - if (!node_parameters->has_parameter("publish_client_content")) { - node_parameters->declare_parameter("publish_client_content", rclcpp::ParameterValue(true)); - } - - std::function &)> - configure_service_introspection_callback = - [this](const std::vector & parameters) { - rcl_ret_t ret; - for (const auto & param : parameters) { - if (param.get_name() == "publish_service_events") { - for (auto srv = services_.begin(); srv != services_.end(); ++srv) { - if (srv->expired()) { - srv = services_.erase(srv); - } else { - ret = rcl_service_introspection_configure_server_service_events( - srv->lock()->get_service_handle().get(), - this->node_base_->get_rcl_node_handle(), - param.get_value()); - if (RCL_RET_OK != ret) { - throw std::runtime_error( - std::string( - "Failed to configure service introspection events with error ") + - std::to_string(ret)); - } - } - } - } else if (param.get_name() == "publish_client_events") { - for (auto clt = clients_.begin(); clt != clients_.end(); ++clt) { - if (clt->expired()) { - clt = clients_.erase(clt); - } else { - ret = rcl_service_introspection_configure_client_service_events( - clt->lock()->get_client_handle().get(), - this->node_base_->get_rcl_node_handle(), - param.get_value()); - if (RCL_RET_OK != ret) { - throw std::runtime_error( - std::string( - "Failed to configure service introspection events with error ") + - std::to_string(ret)); - } - } - } - } else if (param.get_name() == "publish_service_content") { - for (auto srv = services_.begin(); srv != services_.end(); ++srv) { - if (srv->expired()) { - srv = services_.erase(srv); - } else { - ret = rcl_service_introspection_configure_server_service_event_message_payload( - srv->lock()->get_service_handle().get(), param.get_value()); - if (RCL_RET_OK != ret) { - throw std::runtime_error( - std::string( - "Failed to configure service introspection message payload with error ") + - std::to_string(ret)); - } - } - } - } else if (param.get_name() == "publish_client_content") { - for (auto clt = clients_.begin(); clt != clients_.end(); ++clt) { - if (clt->expired()) { - clt = clients_.erase(clt); - } else { - ret = rcl_service_introspection_configure_client_service_event_message_payload( - clt->lock()->get_client_handle().get(), param.get_value()); - if (RCL_RET_OK != ret) { - throw std::runtime_error( - std::string( - "Failed to configure service introspection message payload with error ") + - std::to_string(ret)); - } - } - } - } - } - }; - // register callbacks - post_set_parameters_callback_handle_ = node_parameters->add_post_set_parameters_callback( - configure_service_introspection_callback); -} - -size_t -NodeServiceIntrospection::register_client(rclcpp::ClientBase::SharedPtr client) -{ - std::weak_ptr weak_client = client; - clients_.push_back(weak_client); - return clients_.size(); -} - -size_t -NodeServiceIntrospection::register_service(rclcpp::ServiceBase::SharedPtr service) -{ - std::weak_ptr weak_service = service; - services_.push_back(weak_service); - return services_.size(); -} diff --git a/rclcpp/src/rclcpp/node_options.cpp b/rclcpp/src/rclcpp/node_options.cpp index 5d58decfe2..b90a4b27e7 100644 --- a/rclcpp/src/rclcpp/node_options.cpp +++ b/rclcpp/src/rclcpp/node_options.cpp @@ -75,7 +75,6 @@ NodeOptions::operator=(const NodeOptions & other) this->enable_rosout_ = other.enable_rosout_; this->use_intra_process_comms_ = other.use_intra_process_comms_; this->enable_topic_statistics_ = other.enable_topic_statistics_; - this->enable_service_introspection_ = other.enable_service_introspection_; this->start_parameter_services_ = other.start_parameter_services_; this->start_parameter_event_publisher_ = other.start_parameter_event_publisher_; this->clock_type_ = other.clock_type_; @@ -103,7 +102,6 @@ NodeOptions::get_rcl_node_options() const node_options_->use_global_arguments = this->use_global_arguments_; node_options_->enable_rosout = this->enable_rosout_; node_options_->rosout_qos = this->rosout_qos_.get_rmw_qos_profile(); - node_options_->enable_service_introspection = this->enable_service_introspection_; int c_argc = 0; std::unique_ptr c_argv; @@ -230,20 +228,6 @@ NodeOptions::enable_topic_statistics() const return this->enable_topic_statistics_; } -bool -NodeOptions::enable_service_introspection() const -{ - return this->enable_service_introspection_; -} - -NodeOptions & -NodeOptions::enable_service_introspection(bool enable_service_introspection) -{ - this->node_options_.reset(); // reset node options to make it be recreated on next access. - this->enable_service_introspection_ = enable_service_introspection; - return *this; -} - NodeOptions & NodeOptions::enable_topic_statistics(bool enable_topic_statistics) { diff --git a/rclcpp/src/rclcpp/parameter_service.cpp b/rclcpp/src/rclcpp/parameter_service.cpp index 829cd01fbb..0923798339 100644 --- a/rclcpp/src/rclcpp/parameter_service.cpp +++ b/rclcpp/src/rclcpp/parameter_service.cpp @@ -20,25 +20,21 @@ #include #include "rclcpp/logging.hpp" -#include "rclcpp/create_service.hpp" #include "./parameter_service_names.hpp" -#include "rclcpp/node_interfaces/node_clock_interface.hpp" using rclcpp::ParameterService; ParameterService::ParameterService( const std::shared_ptr node_base, const std::shared_ptr node_services, - const std::shared_ptr node_clock, rclcpp::node_interfaces::NodeParametersInterface * node_params, - bool enable_service_introspection, const rclcpp::QoS & qos_profile) { const std::string node_name = node_base->get_name(); get_parameters_service_ = create_service( - node_base, node_services, node_clock, + node_base, node_services, node_name + "/" + parameter_service_names::get_parameters, [node_params]( const std::shared_ptr, @@ -56,10 +52,10 @@ ParameterService::ParameterService( RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "Failed to get parameters: %s", ex.what()); } }, - qos_profile, nullptr, enable_service_introspection); + qos_profile, nullptr); get_parameter_types_service_ = create_service( - node_base, node_services, node_clock, + node_base, node_services, node_name + "/" + parameter_service_names::get_parameter_types, [node_params]( const std::shared_ptr, @@ -77,10 +73,10 @@ ParameterService::ParameterService( RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "Failed to get parameter types: %s", ex.what()); } }, - qos_profile, nullptr, enable_service_introspection); + qos_profile, nullptr); set_parameters_service_ = create_service( - node_base, node_services, node_clock, + node_base, node_services, node_name + "/" + parameter_service_names::set_parameters, [node_params]( const std::shared_ptr, @@ -102,10 +98,10 @@ ParameterService::ParameterService( response->results.push_back(result); } }, - qos_profile, nullptr, enable_service_introspection); + qos_profile, nullptr); set_parameters_atomically_service_ = create_service( - node_base, node_services, node_clock, + node_base, node_services, node_name + "/" + parameter_service_names::set_parameters_atomically, [node_params]( const std::shared_ptr, @@ -129,10 +125,10 @@ ParameterService::ParameterService( response->result.reason = "One or more parameters were not declared before setting"; } }, - qos_profile, nullptr, enable_service_introspection); + qos_profile, nullptr); describe_parameters_service_ = create_service( - node_base, node_services, node_clock, + node_base, node_services, node_name + "/" + parameter_service_names::describe_parameters, [node_params]( const std::shared_ptr, @@ -146,10 +142,10 @@ ParameterService::ParameterService( RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "Failed to describe parameters: %s", ex.what()); } }, - qos_profile, nullptr, enable_service_introspection); + qos_profile, nullptr); list_parameters_service_ = create_service( - node_base, node_services, node_clock, + node_base, node_services, node_name + "/" + parameter_service_names::list_parameters, [node_params]( const std::shared_ptr, @@ -159,5 +155,5 @@ ParameterService::ParameterService( auto result = node_params->list_parameters(request->prefixes, request->depth); response->result = result; }, - qos_profile, nullptr, enable_service_introspection); + qos_profile, nullptr); } diff --git a/rclcpp/src/rclcpp/service.cpp b/rclcpp/src/rclcpp/service.cpp index ea1a83a2a6..9c246e4b6b 100644 --- a/rclcpp/src/rclcpp/service.cpp +++ b/rclcpp/src/rclcpp/service.cpp @@ -22,6 +22,7 @@ #include "rclcpp/any_service_callback.hpp" #include "rclcpp/macros.hpp" +#include "rclcpp/qos.hpp" #include "rmw/error_handling.h" #include "rmw/rmw.h" @@ -131,7 +132,7 @@ ServiceBase::set_on_new_request_callback(rcl_event_callback_t callback, const vo user_data); if (RCL_RET_OK != ret) { - using rclcpp::exceptions::throw_from_rcl_error; - throw_from_rcl_error(ret, "failed to set the on new request callback for service"); + rclcpp::exceptions::throw_from_rcl_error( + ret, "failed to set the on new request callback for service"); } } diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index bdf7ef82d9..0399f3ae11 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -508,7 +508,6 @@ if(TARGET test_service) ) target_link_libraries(test_service ${PROJECT_NAME} mimick) endif() -# ament_add_gtest(test_service_introspection test_service_introspection.cpp) ament_add_gmock(test_service_introspection test_service_introspection.cpp) if(TARGET test_service) ament_target_dependencies(test_service_introspection diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_service_introspection.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_service_introspection.cpp deleted file mode 100644 index c588d0b20d..0000000000 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_service_introspection.cpp +++ /dev/null @@ -1,99 +0,0 @@ -// Copyright 2022 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 - -#include - -#include "rclcpp/node_interfaces/node_service_introspection.hpp" -#include "rclcpp/node_interfaces/node_services_interface.hpp" -#include "rclcpp/node.hpp" -#include "rclcpp/node_options.hpp" -#include "rcl/service_introspection.h" - -class TestService : public rclcpp::ServiceBase -{ -public: - explicit TestService(rclcpp::Node * node) - : rclcpp::ServiceBase(node->get_node_base_interface()->get_shared_rcl_node_handle()) {} - - std::shared_ptr create_request() override {return nullptr;} - std::shared_ptr create_request_header() override {return nullptr;} - void handle_request(std::shared_ptr, std::shared_ptr) override {} -}; - -class TestClient : public rclcpp::ClientBase -{ -public: - explicit TestClient(rclcpp::Node * node) - : rclcpp::ClientBase(node->get_node_base_interface().get(), node->get_node_graph_interface()) {} - - std::shared_ptr create_response() override {return nullptr;} - std::shared_ptr create_request_header() override {return nullptr;} - void handle_response( - std::shared_ptr, std::shared_ptr) override {} -}; -class TestNodeServiceIntrospection : public ::testing::Test -{ -public: - void SetUp() - { - rclcpp::init(0, nullptr); - } - - void TearDown() - { - rclcpp::shutdown(); - } -}; - -TEST_F(TestNodeServiceIntrospection, construct_from_node) -{ - std::shared_ptr node = std::make_shared( - "node", "ns", - rclcpp::NodeOptions().enable_service_introspection(true)); - - // This dynamic cast is not necessary for the unittest itself, but instead is used to ensure - // the proper type is being tested and covered. - auto * node_service_introspection = - dynamic_cast( - node->get_node_service_introspection_interface().get()); - - ASSERT_NE(nullptr, node_service_introspection); - ASSERT_TRUE(node->get_parameter(RCL_SERVICE_INTROSPECTION_PUBLISH_CLIENT_PARAMETER)); - ASSERT_TRUE(node->get_parameter(RCL_SERVICE_INTROSPECTION_PUBLISH_SERVICE_PARAMETER)); - ASSERT_TRUE( - node->get_parameter(RCL_SERVICE_INTROSPECTION_PUBLISH_CLIENT_EVENT_CONTENT_PARAMETER)); - ASSERT_TRUE( - node->get_parameter(RCL_SERVICE_INTROSPECTION_PUBLISH_SERVICE_EVENT_CONTENT_PARAMETER)); -} - -TEST_F(TestNodeServiceIntrospection, register_services_and_clients) -{ - std::shared_ptr node = std::make_shared( - "node", "ns", - rclcpp::NodeOptions().enable_service_introspection(true)); - - // This dynamic cast is not necessary for the unittest itself, but instead is used to ensure - // the proper type is being tested and covered. - auto * node_service_introspection = - dynamic_cast( - node->get_node_service_introspection_interface().get()); - - auto service = std::make_shared(node.get()); - auto client = std::make_shared(node.get()); - - ASSERT_EQ(1, node_service_introspection->register_service(service)); - ASSERT_EQ(1, node_service_introspection->register_client(client)); -} diff --git a/rclcpp/test/rclcpp/test_client.cpp b/rclcpp/test/rclcpp/test_client.cpp index ead263cc98..9070e1caa9 100644 --- a/rclcpp/test/rclcpp/test_client.cpp +++ b/rclcpp/test/rclcpp/test_client.cpp @@ -131,11 +131,9 @@ TEST_F(TestClient, construction_with_free_function) { node->get_node_base_interface(), node->get_node_graph_interface(), node->get_node_services_interface(), - node->get_node_clock_interface(), "service", rmw_qos_profile_services_default, - nullptr, - true); + nullptr); } { ASSERT_THROW( @@ -144,11 +142,9 @@ TEST_F(TestClient, construction_with_free_function) { node->get_node_base_interface(), node->get_node_graph_interface(), node->get_node_services_interface(), - node->get_node_clock_interface(), "invalid_?service", rmw_qos_profile_services_default, - nullptr, - true); + nullptr); }, rclcpp::exceptions::InvalidServiceNameError); } { @@ -156,11 +152,9 @@ TEST_F(TestClient, construction_with_free_function) { node->get_node_base_interface(), node->get_node_graph_interface(), node->get_node_services_interface(), - node->get_node_clock_interface(), "service", rclcpp::ServicesQoS(), - nullptr, - true); + nullptr); } { ASSERT_THROW( @@ -169,11 +163,9 @@ TEST_F(TestClient, construction_with_free_function) { node->get_node_base_interface(), node->get_node_graph_interface(), node->get_node_services_interface(), - node->get_node_clock_interface(), "invalid_?service", rclcpp::ServicesQoS(), - nullptr, - true); + nullptr); }, rclcpp::exceptions::InvalidServiceNameError); } } diff --git a/rclcpp/test/rclcpp/test_service.cpp b/rclcpp/test/rclcpp/test_service.cpp index dd4d49aca6..a3c361cfde 100644 --- a/rclcpp/test/rclcpp/test_service.cpp +++ b/rclcpp/test/rclcpp/test_service.cpp @@ -30,7 +30,7 @@ using namespace std::chrono_literals; -class TestServiceIntrospection : public ::testing::Test +class TestService : public ::testing::Test { protected: static void SetUpTestCase() @@ -87,7 +87,7 @@ class TestServiceSub : public ::testing::Test /* Testing service construction and destruction. */ -TEST_F(TestServiceIntrospection, construction_and_destruction) { +TEST_F(TestService, construction_and_destruction) { using rcl_interfaces::srv::ListParameters; auto callback = [](const ListParameters::Request::SharedPtr, ListParameters::Response::SharedPtr) { @@ -128,7 +128,7 @@ TEST_F(TestServiceSub, construction_and_destruction) { } } -TEST_F(TestServiceIntrospection, construction_and_destruction_rcl_errors) { +TEST_F(TestService, construction_and_destruction_rcl_errors) { auto callback = [](const test_msgs::srv::Empty::Request::SharedPtr, test_msgs::srv::Empty::Response::SharedPtr) {}; @@ -148,7 +148,7 @@ TEST_F(TestServiceIntrospection, construction_and_destruction_rcl_errors) { } /* Testing basic getters */ -TEST_F(TestServiceIntrospection, basic_public_getters) { +TEST_F(TestService, basic_public_getters) { using rcl_interfaces::srv::ListParameters; auto callback = [](const ListParameters::Request::SharedPtr, ListParameters::Response::SharedPtr) { @@ -188,7 +188,7 @@ TEST_F(TestServiceIntrospection, basic_public_getters) { } } -TEST_F(TestServiceIntrospection, take_request) { +TEST_F(TestService, take_request) { auto callback = [](const test_msgs::srv::Empty::Request::SharedPtr, test_msgs::srv::Empty::Response::SharedPtr) {}; @@ -216,7 +216,7 @@ TEST_F(TestServiceIntrospection, take_request) { } } -TEST_F(TestServiceIntrospection, send_response) { +TEST_F(TestService, send_response) { auto callback = [](const test_msgs::srv::Empty::Request::SharedPtr, test_msgs::srv::Empty::Response::SharedPtr) {}; @@ -242,7 +242,7 @@ TEST_F(TestServiceIntrospection, send_response) { /* Testing on_new_request callbacks. */ -TEST_F(TestServiceIntrospection, on_new_request_callback) { +TEST_F(TestService, on_new_request_callback) { auto server_callback = [](const test_msgs::srv::Empty::Request::SharedPtr, test_msgs::srv::Empty::Response::SharedPtr) {FAIL();}; @@ -312,7 +312,7 @@ TEST_F(TestServiceIntrospection, on_new_request_callback) { EXPECT_THROW(server->set_on_new_request_callback(invalid_cb), std::invalid_argument); } -TEST_F(TestServiceIntrospection, rcl_service_response_publisher_get_actual_qos_error) { +TEST_F(TestService, rcl_service_response_publisher_get_actual_qos_error) { auto mock = mocking_utils::patch_and_return( "lib:rclcpp", rcl_service_response_publisher_get_actual_qos, nullptr); auto callback = @@ -324,7 +324,7 @@ TEST_F(TestServiceIntrospection, rcl_service_response_publisher_get_actual_qos_e std::runtime_error("failed to get service's response publisher qos settings: error not set")); } -TEST_F(TestServiceIntrospection, rcl_service_request_subscription_get_actual_qos_error) { +TEST_F(TestService, rcl_service_request_subscription_get_actual_qos_error) { auto mock = mocking_utils::patch_and_return( "lib:rclcpp", rcl_service_request_subscription_get_actual_qos, nullptr); auto callback = @@ -337,7 +337,7 @@ TEST_F(TestServiceIntrospection, rcl_service_request_subscription_get_actual_qos } -TEST_F(TestServiceIntrospection, server_qos) { +TEST_F(TestService, server_qos) { rclcpp::ServicesQoS qos_profile; qos_profile.liveliness(rclcpp::LivelinessPolicy::Automatic); rclcpp::Duration duration(std::chrono::nanoseconds(1)); @@ -359,7 +359,7 @@ TEST_F(TestServiceIntrospection, server_qos) { EXPECT_EQ(qos_profile, rs_qos); } -TEST_F(TestServiceIntrospection, server_qos_depth) { +TEST_F(TestService, server_qos_depth) { using namespace std::literals::chrono_literals; uint64_t server_cb_count_ = 0; diff --git a/rclcpp/test/rclcpp/test_service_introspection.cpp b/rclcpp/test/rclcpp/test_service_introspection.cpp index 4ffb8c6253..6dbe94715e 100644 --- a/rclcpp/test/rclcpp/test_service_introspection.cpp +++ b/rclcpp/test/rclcpp/test_service_introspection.cpp @@ -52,8 +52,7 @@ class TestServiceIntrospection : public ::testing::Test void SetUp() { node = std::make_shared( - "my_node", "/ns", - rclcpp::NodeOptions().enable_service_introspection(true)); + "my_node", "/ns"); auto srv_callback = [](const BasicTypes::Request::SharedPtr & req, const BasicTypes::Response::SharedPtr & resp) { @@ -86,34 +85,17 @@ class TestServiceIntrospection : public ::testing::Test std::chrono::milliseconds timeout = std::chrono::milliseconds(1000); }; -/* - Testing service construction and destruction with service introspection enabled - */ -TEST_F(TestServiceIntrospection, construction_and_destruction) -{ - auto callback = - [](const BasicTypes::Request::SharedPtr &, const BasicTypes::Response::SharedPtr &) {}; - { - auto service = node->create_service("test_create_service", callback); - EXPECT_NE(nullptr, service->get_service_handle()); - const rclcpp::ServiceBase * const_service_base = service.get(); - EXPECT_NE(nullptr, const_service_base->get_service_handle()); - } - - { - ASSERT_THROW( - { - auto service = node->create_service("invalid_service?", callback); - }, rclcpp::exceptions::InvalidServiceNameError); - } -} - TEST_F(TestServiceIntrospection, service_introspection_nominal) { auto request = std::make_shared(); request->set__bool_value(true); request->set__int64_value(42); + client->configure_introspection( + node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_CONTENTS); + service->configure_introspection( + node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_CONTENTS); + auto future = client->async_send_request(request); ASSERT_EQ( rclcpp::FutureReturnCode::SUCCESS, @@ -166,8 +148,10 @@ TEST_F(TestServiceIntrospection, service_introspection_nominal) TEST_F(TestServiceIntrospection, service_introspection_enable_disable_events) { - node->set_parameter(rclcpp::Parameter("publish_service_events", false)); - node->set_parameter(rclcpp::Parameter("publish_client_events", false)); + client->configure_introspection( + node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_OFF); + service->configure_introspection( + node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_OFF); auto request = std::make_shared(); request->set__bool_value(true); @@ -183,35 +167,46 @@ TEST_F(TestServiceIntrospection, service_introspection_enable_disable_events) EXPECT_EQ(events.size(), 0U); events.clear(); - node->set_parameter(rclcpp::Parameter("publish_service_events", false)); - node->set_parameter(rclcpp::Parameter("publish_client_events", true)); + + client->configure_introspection( + node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_METADATA); + service->configure_introspection( + node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_OFF); + future = client->async_send_request(request); ASSERT_EQ( rclcpp::FutureReturnCode::SUCCESS, rclcpp::spin_until_future_complete(node, future, timeout)); start = std::chrono::steady_clock::now(); - while ((std::chrono::steady_clock::now() - start) < timeout) { + while (events.size() < 2 && (std::chrono::steady_clock::now() - start) < timeout) { rclcpp::spin_some(node); } EXPECT_EQ(events.size(), 2U); - events.clear(); - node->set_parameter(rclcpp::Parameter("publish_service_events", true)); - node->set_parameter(rclcpp::Parameter("publish_client_events", false)); + + client->configure_introspection( + node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_OFF); + service->configure_introspection( + node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_METADATA); + future = client->async_send_request(request); ASSERT_EQ( rclcpp::FutureReturnCode::SUCCESS, rclcpp::spin_until_future_complete(node, future, timeout)); start = std::chrono::steady_clock::now(); - while ((std::chrono::steady_clock::now() - start) < timeout) { + while (events.size() < 2 && (std::chrono::steady_clock::now() - start) < timeout) { rclcpp::spin_some(node); } EXPECT_EQ(events.size(), 2U); events.clear(); - node->set_parameter(rclcpp::Parameter("publish_service_events", true)); - node->set_parameter(rclcpp::Parameter("publish_client_events", true)); + + client->configure_introspection( + node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_METADATA); + service->configure_introspection( + node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_METADATA); + future = client->async_send_request(request); ASSERT_EQ( rclcpp::FutureReturnCode::SUCCESS, @@ -225,8 +220,10 @@ TEST_F(TestServiceIntrospection, service_introspection_enable_disable_events) TEST_F(TestServiceIntrospection, service_introspection_enable_disable_event_content) { - node->set_parameter(rclcpp::Parameter("publish_service_content", false)); - node->set_parameter(rclcpp::Parameter("publish_client_content", false)); + client->configure_introspection( + node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_METADATA); + service->configure_introspection( + node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_METADATA); auto request = std::make_shared(); request->set__bool_value(true); @@ -245,10 +242,13 @@ TEST_F(TestServiceIntrospection, service_introspection_enable_disable_event_cont EXPECT_EQ(event->response.size(), 0U); } - events.clear(); - node->set_parameter(rclcpp::Parameter("publish_service_content", false)); - node->set_parameter(rclcpp::Parameter("publish_client_content", true)); + + client->configure_introspection( + node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_CONTENTS); + service->configure_introspection( + node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_METADATA); + future = client->async_send_request(request); ASSERT_EQ( rclcpp::FutureReturnCode::SUCCESS, @@ -276,8 +276,12 @@ TEST_F(TestServiceIntrospection, service_introspection_enable_disable_event_cont } events.clear(); - node->set_parameter(rclcpp::Parameter("publish_service_content", true)); - node->set_parameter(rclcpp::Parameter("publish_client_content", false)); + + client->configure_introspection( + node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_METADATA); + service->configure_introspection( + node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_CONTENTS); + future = client->async_send_request(request); ASSERT_EQ( rclcpp::FutureReturnCode::SUCCESS, @@ -305,8 +309,12 @@ TEST_F(TestServiceIntrospection, service_introspection_enable_disable_event_cont } events.clear(); - node->set_parameter(rclcpp::Parameter("publish_service_content", true)); - node->set_parameter(rclcpp::Parameter("publish_client_content", true)); + + client->configure_introspection( + node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_CONTENTS); + service->configure_introspection( + node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_CONTENTS); + future = client->async_send_request(request); ASSERT_EQ( rclcpp::FutureReturnCode::SUCCESS, diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp index 184b1c667c..a9bf2a5008 100644 --- a/rclcpp_action/include/rclcpp_action/client.hpp +++ b/rclcpp_action/include/rclcpp_action/client.hpp @@ -31,7 +31,6 @@ #include "rclcpp/exceptions.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/node_interfaces/node_base_interface.hpp" -#include #include "rclcpp/node_interfaces/node_logging_interface.hpp" #include "rclcpp/node_interfaces/node_graph_interface.hpp" #include "rclcpp/logger.hpp" @@ -186,7 +185,6 @@ class ClientBase : public rclcpp::Waitable ClientBase( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph, - rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock, rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, const std::string & action_name, const rosidl_action_type_support_t * type_support, @@ -394,13 +392,12 @@ class Client : public ClientBase Client( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph, - rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock, rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, const std::string & action_name, const rcl_action_client_options_t & client_options = rcl_action_client_get_default_options() ) : ClientBase( - node_base, node_graph, node_clock, node_logging, action_name, + node_base, node_graph, node_logging, action_name, rosidl_typesupport_cpp::get_action_type_support_handle(), client_options) { diff --git a/rclcpp_action/include/rclcpp_action/create_client.hpp b/rclcpp_action/include/rclcpp_action/create_client.hpp index f262117624..f594bca78d 100644 --- a/rclcpp_action/include/rclcpp_action/create_client.hpp +++ b/rclcpp_action/include/rclcpp_action/create_client.hpp @@ -44,7 +44,6 @@ typename Client::SharedPtr create_client( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface, rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_interface, - rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_interface, rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_interface, rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr node_waitables_interface, const std::string & name, @@ -84,7 +83,6 @@ create_client( new Client( node_base_interface, node_graph_interface, - node_clock_interface, node_logging_interface, name, options), @@ -113,7 +111,6 @@ create_client( return rclcpp_action::create_client( node->get_node_base_interface(), node->get_node_graph_interface(), - node->get_node_clock_interface(), node->get_node_logging_interface(), node->get_node_waitables_interface(), name, diff --git a/rclcpp_action/src/client.cpp b/rclcpp_action/src/client.cpp index 69042a525f..e687ab3400 100644 --- a/rclcpp_action/src/client.cpp +++ b/rclcpp_action/src/client.cpp @@ -23,7 +23,6 @@ #include "rcl_action/action_client.h" #include "rcl_action/wait.h" #include "rclcpp/node_interfaces/node_base_interface.hpp" -#include #include "rclcpp/node_interfaces/node_logging_interface.hpp" #include "rclcpp_action/client.hpp" @@ -38,14 +37,12 @@ class ClientBaseImpl ClientBaseImpl( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph, - rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock, rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, const std::string & action_name, const rosidl_action_type_support_t * type_support, const rcl_action_client_options_t & client_options) : node_graph_(node_graph), node_handle(node_base->get_shared_rcl_node_handle()), - node_clock_(std::move(node_clock)), logger(node_logging->get_logger().get_child("rclcpp_action")), random_bytes_generator(std::random_device{}()) { @@ -71,8 +68,8 @@ class ClientBaseImpl }); *client_handle = rcl_action_get_zero_initialized_client(); rcl_ret_t ret = rcl_action_client_init( - client_handle.get(), node_handle.get(), node_clock_->get_clock()->get_clock_handle(), - type_support, action_name.c_str(), &client_options); + client_handle.get(), node_handle.get(), type_support, + action_name.c_str(), &client_options); if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error( ret, "could not initialize rcl action client"); @@ -108,7 +105,6 @@ class ClientBaseImpl // node_handle must be destroyed after client_handle to prevent memory leak std::shared_ptr node_handle{nullptr}; std::shared_ptr client_handle{nullptr}; - rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_{nullptr}; rclcpp::Logger logger; using ResponseCallback = std::function response)>; @@ -129,13 +125,12 @@ class ClientBaseImpl ClientBase::ClientBase( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph, - rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock, rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, const std::string & action_name, const rosidl_action_type_support_t * type_support, const rcl_action_client_options_t & client_options) : pimpl_(new ClientBaseImpl( - node_base, node_graph, node_clock, node_logging, action_name, type_support, client_options)) + node_base, node_graph, node_logging, action_name, type_support, client_options)) { } diff --git a/rclcpp_action/test/test_client.cpp b/rclcpp_action/test/test_client.cpp index 7d03ec0870..b94a82d500 100644 --- a/rclcpp_action/test/test_client.cpp +++ b/rclcpp_action/test/test_client.cpp @@ -321,7 +321,6 @@ TEST_F(TestClient, construction_and_destruction_callback_group) rclcpp_action::create_client( client_node->get_node_base_interface(), client_node->get_node_graph_interface(), - client_node->get_node_clock_interface(), client_node->get_node_logging_interface(), client_node->get_node_waitables_interface(), action_name, diff --git a/rclcpp_lifecycle/src/lifecycle_node.cpp b/rclcpp_lifecycle/src/lifecycle_node.cpp index 835d250200..723c7bc8c1 100644 --- a/rclcpp_lifecycle/src/lifecycle_node.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node.cpp @@ -100,8 +100,7 @@ LifecycleNode::LifecycleNode( options.parameter_event_qos(), options.parameter_event_publisher_options(), options.allow_undeclared_parameters(), - options.automatically_declare_parameters_from_overrides(), - options.enable_service_introspection() + options.automatically_declare_parameters_from_overrides() )), node_time_source_(new rclcpp::node_interfaces::NodeTimeSource( node_base_, From 1279cf8dd248d79686d70c8bdbbbd709e002bed4 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Fri, 24 Feb 2023 17:18:25 +0000 Subject: [PATCH 3/3] Fixes from review. Signed-off-by: Chris Lalancette --- rclcpp/include/rclcpp/client.hpp | 12 +++++------ rclcpp/include/rclcpp/service.hpp | 20 +++++++++---------- .../rclcpp/test_service_introspection.cpp | 2 +- 3 files changed, 15 insertions(+), 19 deletions(-) diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index ef1b08bfca..f3346a067c 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -470,15 +470,13 @@ class Client : public ClientBase rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph, const std::string & service_name, rcl_client_options_t & client_options) - : ClientBase(node_base, node_graph) + : ClientBase(node_base, node_graph), + srv_type_support_handle_(rosidl_typesupport_cpp::get_service_type_support_handle()) { - using rosidl_typesupport_cpp::get_service_type_support_handle; - service_type_support_handle_ = - get_service_type_support_handle(); rcl_ret_t ret = rcl_client_init( this->get_client_handle().get(), this->get_rcl_node_handle(), - service_type_support_handle_, + srv_type_support_handle_, service_name.c_str(), &client_options); if (ret != RCL_RET_OK) { @@ -802,7 +800,7 @@ class Client : public ClientBase client_handle_.get(), node_handle_.get(), clock->get_clock_handle(), - service_type_support_handle_, + srv_type_support_handle_, pub_opts, introspection_state); @@ -863,7 +861,7 @@ class Client : public ClientBase std::mutex pending_requests_mutex_; private: - const rosidl_service_type_support_t * service_type_support_handle_; + const rosidl_service_type_support_t * srv_type_support_handle_; }; } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index 7ba47e8473..3cfc11e9ca 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -310,11 +310,9 @@ class Service const std::string & service_name, AnyServiceCallback any_callback, rcl_service_options_t & service_options) - : ServiceBase(node_handle), any_callback_(any_callback) + : ServiceBase(node_handle), any_callback_(any_callback), + srv_type_support_handle_(rosidl_typesupport_cpp::get_service_type_support_handle()) { - using rosidl_typesupport_cpp::get_service_type_support_handle; - service_type_support_handle_ = get_service_type_support_handle(); - // rcl does the static memory allocation here service_handle_ = std::shared_ptr( new rcl_service_t, [handle = node_handle_, service_name](rcl_service_t * service) @@ -333,7 +331,7 @@ class Service rcl_ret_t ret = rcl_service_init( service_handle_.get(), node_handle.get(), - service_type_support_handle_, + srv_type_support_handle_, service_name.c_str(), &service_options); if (ret != RCL_RET_OK) { @@ -373,8 +371,8 @@ class Service std::shared_ptr node_handle, std::shared_ptr service_handle, AnyServiceCallback any_callback) - : ServiceBase(node_handle), - any_callback_(any_callback) + : ServiceBase(node_handle), any_callback_(any_callback), + srv_type_support_handle_(rosidl_typesupport_cpp::get_service_type_support_handle()) { // check if service handle was initialized if (!rcl_service_is_valid(service_handle.get())) { @@ -408,8 +406,8 @@ class Service std::shared_ptr node_handle, rcl_service_t * service_handle, AnyServiceCallback any_callback) - : ServiceBase(node_handle), - any_callback_(any_callback) + : ServiceBase(node_handle), any_callback_(any_callback), + srv_type_support_handle_(rosidl_typesupport_cpp::get_service_type_support_handle()) { // check if service handle was initialized if (!rcl_service_is_valid(service_handle)) { @@ -507,7 +505,7 @@ class Service service_handle_.get(), node_handle_.get(), clock->get_clock_handle(), - service_type_support_handle_, + srv_type_support_handle_, pub_opts, introspection_state); @@ -521,7 +519,7 @@ class Service AnyServiceCallback any_callback_; - const rosidl_service_type_support_t * service_type_support_handle_; + const rosidl_service_type_support_t * srv_type_support_handle_; }; } // namespace rclcpp diff --git a/rclcpp/test/rclcpp/test_service_introspection.cpp b/rclcpp/test/rclcpp/test_service_introspection.cpp index 6dbe94715e..94c94a58ce 100644 --- a/rclcpp/test/rclcpp/test_service_introspection.cpp +++ b/rclcpp/test/rclcpp/test_service_introspection.cpp @@ -1,4 +1,4 @@ -// Copyright 2017 Open Source Robotics Foundation, Inc. +// Copyright 2022 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.