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 6dffb543e9..37d98052a3 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_,