From 05c170d6ce263c6617fa0948797029434fdc5048 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 21 Feb 2023 16:24:05 +0000 Subject: [PATCH 1/3] Switch Client constructor to use std::string. This make it more similar to the Service constructor. Also make both of them use const std::string & to avoid a copy. Signed-off-by: Chris Lalancette --- rclpy/src/rclpy/client.cpp | 6 +++--- rclpy/src/rclpy/client.hpp | 3 ++- rclpy/src/rclpy/service.cpp | 7 +++---- rclpy/src/rclpy/service.hpp | 2 +- 4 files changed, 9 insertions(+), 9 deletions(-) diff --git a/rclpy/src/rclpy/client.cpp b/rclpy/src/rclpy/client.cpp index af22bb720..d4b986c41 100644 --- a/rclpy/src/rclpy/client.cpp +++ b/rclpy/src/rclpy/client.cpp @@ -40,7 +40,7 @@ Client::destroy() } Client::Client( - Node & node, py::object pysrv_type, const char * service_name, py::object pyqos_profile) + Node & node, py::object pysrv_type, const std::string & service_name, py::object pyqos_profile) : node_(node) { auto srv_type = static_cast( @@ -77,7 +77,7 @@ Client::Client( *rcl_client_ = rcl_get_zero_initialized_client(); rcl_ret_t ret = rcl_client_init( - rcl_client_.get(), node_.rcl_ptr(), srv_type, service_name, &client_ops); + rcl_client_.get(), node_.rcl_ptr(), srv_type, service_name.c_str(), &client_ops); if (RCL_RET_OK != ret) { if (RCL_RET_SERVICE_NAME_INVALID == ret) { std::string error_text{"failed to create client due to invalid service name '"}; @@ -150,7 +150,7 @@ void define_client(py::object module) { py::class_>(module, "Client") - .def(py::init()) + .def(py::init()) .def_property_readonly( "pointer", [](const Client & client) { return reinterpret_cast(client.rcl_ptr()); diff --git a/rclpy/src/rclpy/client.hpp b/rclpy/src/rclpy/client.hpp index ff1f29d59..73c60ed1f 100644 --- a/rclpy/src/rclpy/client.hpp +++ b/rclpy/src/rclpy/client.hpp @@ -20,6 +20,7 @@ #include #include +#include #include "destroyable.hpp" #include "node.hpp" @@ -45,7 +46,7 @@ class Client : public Destroyable, public std::enable_shared_from_this * \param[in] service_name The service name * \param[in] pyqos rmw_qos_profile_t object for this client */ - Client(Node & node, py::object pysrv_type, const char * service_name, py::object pyqos); + Client(Node & node, py::object pysrv_type, const std::string & service_name, py::object pyqos); ~Client() = default; diff --git a/rclpy/src/rclpy/service.cpp b/rclpy/src/rclpy/service.cpp index 02b7ba88d..b9ec0b2ae 100644 --- a/rclpy/src/rclpy/service.cpp +++ b/rclpy/src/rclpy/service.cpp @@ -38,7 +38,7 @@ Service::destroy() } Service::Service( - Node & node, py::object pysrv_type, std::string service_name, + Node & node, py::object pysrv_type, const std::string & service_name, py::object pyqos_profile) : node_(node) { @@ -75,8 +75,7 @@ Service::Service( *rcl_service_ = rcl_get_zero_initialized_service(); rcl_ret_t ret = rcl_service_init( - rcl_service_.get(), node_.rcl_ptr(), srv_type, - service_name.c_str(), &service_ops); + rcl_service_.get(), node_.rcl_ptr(), srv_type, service_name.c_str(), &service_ops); if (RCL_RET_OK != ret) { if (ret == RCL_RET_SERVICE_NAME_INVALID) { std::string error_text{"failed to create service due to invalid topic name '"}; @@ -148,7 +147,7 @@ void define_service(py::object module) { py::class_>(module, "Service") - .def(py::init()) + .def(py::init()) .def_property_readonly( "pointer", [](const Service & service) { return reinterpret_cast(service.rcl_ptr()); diff --git a/rclpy/src/rclpy/service.hpp b/rclpy/src/rclpy/service.hpp index 95c2dd05c..db1b96f60 100644 --- a/rclpy/src/rclpy/service.hpp +++ b/rclpy/src/rclpy/service.hpp @@ -51,7 +51,7 @@ class Service : public Destroyable, public std::enable_shared_from_this * \return capsule containing the rcl_service_t */ Service( - Node & node, py::object pysrv_type, std::string service_name, + Node & node, py::object pysrv_type, const std::string & service_name, py::object pyqos_profile); Service( From f23403b2bc66a3123284b06da62077de177ab4b4 Mon Sep 17 00:00:00 2001 From: Brian Chen Date: Fri, 26 Aug 2022 02:00:02 -0700 Subject: [PATCH 2/3] Implement service introspection in rclpy. Add in the parameters to control it, as well as the correct calls into the underlying rcl implementation. Also add in tests. Signed-off-by: Brian Chen Signed-off-by: Chris Lalancette --- rclpy/CMakeLists.txt | 2 + rclpy/rclpy/__init__.py | 7 +- rclpy/rclpy/node.py | 118 +++++++++++++++- rclpy/src/rclpy/_rclpy_pybind11.cpp | 2 + rclpy/src/rclpy/client.cpp | 17 ++- rclpy/src/rclpy/client.hpp | 10 +- rclpy/src/rclpy/node.cpp | 8 +- rclpy/src/rclpy/node.hpp | 3 +- rclpy/src/rclpy/service.cpp | 23 ++- rclpy/src/rclpy/service.hpp | 8 +- rclpy/src/rclpy/service_introspection.cpp | 60 ++++++++ rclpy/src/rclpy/service_introspection.hpp | 29 ++++ rclpy/test/test_service_introspection.py | 165 ++++++++++++++++++++++ rclpy/test/test_waitable.py | 7 +- 14 files changed, 430 insertions(+), 29 deletions(-) create mode 100644 rclpy/src/rclpy/service_introspection.cpp create mode 100644 rclpy/src/rclpy/service_introspection.hpp create mode 100644 rclpy/test/test_service_introspection.py diff --git a/rclpy/CMakeLists.txt b/rclpy/CMakeLists.txt index 1b1c12f58..c5e843f8e 100644 --- a/rclpy/CMakeLists.txt +++ b/rclpy/CMakeLists.txt @@ -96,6 +96,7 @@ pybind11_add_module(_rclpy_pybind11 SHARED src/rclpy/serialization.cpp src/rclpy/service.cpp src/rclpy/service_info.cpp + src/rclpy/service_introspection.cpp src/rclpy/signal_handler.cpp src/rclpy/subscription.cpp src/rclpy/time_point.cpp @@ -190,6 +191,7 @@ if(BUILD_TESTING) test/test_rate.py test/test_rosout_subscription.py test/test_serialization.py + test/test_service_introspection.py test/test_subscription.py test/test_task.py test/test_time_source.py diff --git a/rclpy/rclpy/__init__.py b/rclpy/rclpy/__init__.py index cea1f9163..33c2a56f0 100644 --- a/rclpy/rclpy/__init__.py +++ b/rclpy/rclpy/__init__.py @@ -143,7 +143,8 @@ def create_node( start_parameter_services: bool = True, parameter_overrides: List[Parameter] = None, allow_undeclared_parameters: bool = False, - automatically_declare_parameters_from_overrides: bool = False + automatically_declare_parameters_from_overrides: bool = False, + enable_service_introspection: bool = False, ) -> 'Node': """ Create an instance of :class:`.Node`. @@ -165,6 +166,7 @@ def create_node( This option doesn't affect `parameter_overrides`. :param automatically_declare_parameters_from_overrides: If True, the "parameter overrides" will be used to implicitly declare parameters on the node during creation, default False. + :param enable_service_introspection: Flag to enable service introspection, default False. :return: An instance of the newly created node. """ # imported locally to avoid loading extensions on module import @@ -178,7 +180,8 @@ def create_node( allow_undeclared_parameters=allow_undeclared_parameters, automatically_declare_parameters_from_overrides=( automatically_declare_parameters_from_overrides - )) + ), + enable_service_introspection=enable_service_introspection) def spin_once(node: 'Node', *, executor: 'Executor' = None, timeout_sec: float = None) -> None: diff --git a/rclpy/rclpy/node.py b/rclpy/rclpy/node.py index 95042e2ca..04fee9fc7 100644 --- a/rclpy/rclpy/node.py +++ b/rclpy/rclpy/node.py @@ -66,6 +66,7 @@ from rclpy.publisher import Publisher from rclpy.qos import qos_profile_parameter_events from rclpy.qos import qos_profile_services_default +from rclpy.qos import qos_profile_system_default from rclpy.qos import QoSProfile from rclpy.qos_event import PublisherEventCallbacks from rclpy.qos_event import SubscriptionEventCallbacks @@ -126,7 +127,8 @@ def __init__( start_parameter_services: bool = True, parameter_overrides: List[Parameter] = None, allow_undeclared_parameters: bool = False, - automatically_declare_parameters_from_overrides: bool = False + automatically_declare_parameters_from_overrides: bool = False, + enable_service_introspection: bool = False, ) -> None: """ Create a Node. @@ -150,6 +152,8 @@ def __init__( This flag affects the behavior of parameter-related operations. :param automatically_declare_parameters_from_overrides: If True, the "parameter overrides" will be used to implicitly declare parameters on the node during creation. + :param enable_service_introspection: If True, the node will enable introspection of + services. """ self.__handle = None self._context = get_default_context() if context is None else context @@ -182,7 +186,8 @@ def __init__( self._context.handle, cli_args, use_global_arguments, - enable_rosout + enable_rosout, + enable_service_introspection ) except ValueError: # these will raise more specific errors if the name or namespace is bad @@ -231,6 +236,18 @@ def __init__( if start_parameter_services: self._parameter_service = ParameterService(self) + if enable_service_introspection: + self.declare_parameters( + namespace='', + parameters=[ + (_rclpy.service_introspection.RCL_SERVICE_INTROSPECTION_PUBLISH_CLIENT_PARAMETER, # noqa E501 + 'off', ParameterDescriptor()), + (_rclpy.service_introspection.RCL_SERVICE_INTROSPECTION_PUBLISH_SERVICE_PARAMETER, # noqa E501 + 'off', ParameterDescriptor()), + ]) + self.add_on_set_parameters_callback(self._check_service_introspection_parameters) + self.add_post_set_parameters_callback(self._configure_service_introspection) + @property def publishers(self) -> Iterator[Publisher]: """Get publishers that have been created on this node.""" @@ -1576,13 +1593,89 @@ def create_subscription( return subscription + def _check_service_introspection_parameters( + self, parameters: List[Parameter] + ) -> SetParametersResult: + result = SetParametersResult(successful=True) + for param in parameters: + if param.name not in ( + _rclpy.service_introspection.RCL_SERVICE_INTROSPECTION_PUBLISH_CLIENT_PARAMETER, # noqa: E501 + _rclpy.service_introspection.RCL_SERVICE_INTROSPECTION_PUBLISH_SERVICE_PARAMETER): # noqa: E501 + continue + + if param.type_ != Parameter.Type.STRING: + result.successful = False + result.reason = 'Parameter type must be string' + break + + if param.value not in ('off', 'metadata', 'contents'): + result.successful = False + result.reason = "Value must be one of 'off', 'metadata', or 'contents'" + break + + return result + + def _configure_service_introspection(self, parameters: List[Parameter]): + for param in parameters: + if param.name == \ + _rclpy.service_introspection.RCL_SERVICE_INTROSPECTION_PUBLISH_CLIENT_PARAMETER: # noqa: E501 + + value = param.value + + for cli in self.clients: + should_enable_service_events = False + should_enable_contents = False + if value == 'off': + should_enable_service_events = False + should_enable_contents = False + elif value == 'metadata': + should_enable_service_events = True + should_enable_contents = False + elif value == 'contents': + should_enable_service_events = True + should_enable_contents = True + + _rclpy.service_introspection.configure_client_events( + cli.handle.pointer, + self.handle.pointer, + should_enable_service_events) + _rclpy.service_introspection.configure_client_message_payload( + cli.handle.pointer, + should_enable_contents) + elif param.name == \ + _rclpy.service_introspection.RCL_SERVICE_INTROSPECTION_PUBLISH_SERVICE_PARAMETER: # noqa: E501 + + value = param.value + + for srv in self.services: + should_enable_service_events = False + should_enable_contents = False + if value == 'off': + should_enable_service_events = False + should_enable_contents = False + elif value == 'metadata': + should_enable_service_events = True + should_enable_contents = False + elif value == 'contents': + should_enable_service_events = True + should_enable_contents = True + + _rclpy.service_introspection.configure_service_events( + srv.handle.pointer, + self.handle.pointer, + should_enable_service_events) + _rclpy.service_introspection.configure_service_message_payload( + srv.handle.pointer, + should_enable_contents) + def create_client( self, srv_type, srv_name: str, *, qos_profile: QoSProfile = qos_profile_services_default, - callback_group: CallbackGroup = None + callback_group: CallbackGroup = None, + service_event_qos_profile: QoSProfile = qos_profile_system_default ) -> Client: """ Create a new service client. @@ -1590,8 +1683,12 @@ def create_client( :param srv_type: The service type. :param srv_name: The name of the service. :param qos_profile: The quality of service profile to apply the service client. + :param service_event_publisher_qos_profile: The quality of service + profile to apply the service event publisher. :param callback_group: The callback group for the service client. If ``None``, then the default callback group for the node is used. + :param service_event_qos_profile: The quality of service profile to apply to service + introspection (if enabled). """ if callback_group is None: callback_group = self.default_callback_group @@ -1603,7 +1700,9 @@ def create_client( self.handle, srv_type, srv_name, - qos_profile.get_c_qos_profile()) + qos_profile.get_c_qos_profile(), + service_event_qos_profile.get_c_qos_profile(), + self._clock.handle) except ValueError: failed = True if failed: @@ -1625,7 +1724,8 @@ def create_service( callback: Callable[[SrvTypeRequest, SrvTypeResponse], SrvTypeResponse], *, qos_profile: QoSProfile = qos_profile_services_default, - callback_group: CallbackGroup = None + callback_group: CallbackGroup = None, + service_event_qos_profile: QoSProfile = qos_profile_system_default ) -> Service: """ Create a new service server. @@ -1635,8 +1735,12 @@ def create_service( :param callback: A user-defined callback function that is called when a service request received by the server. :param qos_profile: The quality of service profile to apply the service server. + :param service_event_publisher_qos_profile: The quality of service + profile to apply the service event publisher. :param callback_group: The callback group for the service server. If ``None``, then the default callback group for the node is used. + :param service_event_qos_profile: The quality of service profile to apply to service + introspection (if enabled). """ if callback_group is None: callback_group = self.default_callback_group @@ -1648,7 +1752,9 @@ def create_service( self.handle, srv_type, srv_name, - qos_profile.get_c_qos_profile()) + qos_profile.get_c_qos_profile(), + service_event_qos_profile.get_c_qos_profile(), + self._clock.handle) except ValueError: failed = True if failed: diff --git a/rclpy/src/rclpy/_rclpy_pybind11.cpp b/rclpy/src/rclpy/_rclpy_pybind11.cpp index 719e9a87d..2e66a00da 100644 --- a/rclpy/src/rclpy/_rclpy_pybind11.cpp +++ b/rclpy/src/rclpy/_rclpy_pybind11.cpp @@ -44,6 +44,7 @@ #include "serialization.hpp" #include "service.hpp" #include "service_info.hpp" +#include "service_introspection.hpp" #include "signal_handler.hpp" #include "subscription.hpp" #include "time_point.hpp" @@ -240,4 +241,5 @@ PYBIND11_MODULE(_rclpy_pybind11, m) { rclpy::define_signal_handler_api(m); rclpy::define_clock_event(m); rclpy::define_lifecycle_api(m); + rclpy::define_service_introspection(m); } diff --git a/rclpy/src/rclpy/client.cpp b/rclpy/src/rclpy/client.cpp index d4b986c41..65a3c4f72 100644 --- a/rclpy/src/rclpy/client.cpp +++ b/rclpy/src/rclpy/client.cpp @@ -22,8 +22,10 @@ #include #include +#include #include "client.hpp" +#include "clock.hpp" #include "exceptions.hpp" #include "node.hpp" #include "python_allocator.hpp" @@ -40,22 +42,29 @@ Client::destroy() } Client::Client( - Node & node, py::object pysrv_type, const std::string & service_name, py::object pyqos_profile) + Node & node, py::object pysrv_type, const std::string & service_name, py::object pyqos_profile, + py::object pyqos_service_event_pub, Clock & clock) : node_(node) { auto srv_type = static_cast( common_get_type_support(pysrv_type)); - if (!srv_type) { + if (nullptr == srv_type) { throw py::error_already_set(); } rcl_client_options_t client_ops = rcl_client_get_default_options(); + if (rcl_node_get_options(node.rcl_ptr())->enable_service_introspection) { + client_ops.clock = clock.rcl_ptr(); + client_ops.enable_service_introspection = true; + client_ops.event_publisher_options.qos = pyqos_service_event_pub.is_none() ? + rcl_publisher_get_default_options().qos : pyqos_service_event_pub.cast(); + } + if (!pyqos_profile.is_none()) { client_ops.qos = pyqos_profile.cast(); } - // Create a client rcl_client_ = std::shared_ptr( PythonAllocator().allocate(1), @@ -150,7 +159,7 @@ void define_client(py::object module) { py::class_>(module, "Client") - .def(py::init()) + .def(py::init()) .def_property_readonly( "pointer", [](const Client & client) { return reinterpret_cast(client.rcl_ptr()); diff --git a/rclpy/src/rclpy/client.hpp b/rclpy/src/rclpy/client.hpp index 73c60ed1f..4872d74c5 100644 --- a/rclpy/src/rclpy/client.hpp +++ b/rclpy/src/rclpy/client.hpp @@ -18,10 +18,12 @@ #include #include +#include #include #include +#include "clock.hpp" #include "destroyable.hpp" #include "node.hpp" @@ -44,9 +46,13 @@ class Client : public Destroyable, public std::enable_shared_from_this * \param[in] node Node to add the client to * \param[in] pysrv_type Service module associated with the client * \param[in] service_name The service name - * \param[in] pyqos rmw_qos_profile_t object for this client + * \param[in] pyqos QoSProfile python object for this client + * \param[in] pyqos_service_event_pub QoSProfile python object for the service event publisher + * \param[in] clock Clock to use for service event timestamps */ - Client(Node & node, py::object pysrv_type, const std::string & service_name, py::object pyqos); + Client( + Node & node, py::object pysrv_type, const std::string & service_name, py::object pyqos, + py::object pyqos_service_event_pub, Clock & clock); ~Client() = default; diff --git a/rclpy/src/rclpy/node.cpp b/rclpy/src/rclpy/node.cpp index 50a00e000..444db36de 100644 --- a/rclpy/src/rclpy/node.cpp +++ b/rclpy/src/rclpy/node.cpp @@ -16,6 +16,7 @@ #include #include +#include #include #include #include @@ -382,7 +383,8 @@ Node::Node( Context & context, py::object pycli_args, bool use_global_arguments, - bool enable_rosout) + bool enable_rosout, + bool enable_service_introspection) : context_(context) { rcl_ret_t ret; @@ -458,6 +460,7 @@ Node::Node( options.use_global_arguments = use_global_arguments; options.arguments = arguments; options.enable_rosout = enable_rosout; + options.enable_service_introspection = enable_service_introspection; { rclpy::LoggingGuard scoped_logging_guard; @@ -531,11 +534,12 @@ Node::get_action_names_and_types() return convert_to_py_names_and_types(&names_and_types); } + void define_node(py::object module) { py::class_>(module, "Node") - .def(py::init()) + .def(py::init()) .def_property_readonly( "pointer", [](const Node & node) { return reinterpret_cast(node.rcl_ptr()); diff --git a/rclpy/src/rclpy/node.hpp b/rclpy/src/rclpy/node.hpp index bd17c516e..7bc0c4978 100644 --- a/rclpy/src/rclpy/node.hpp +++ b/rclpy/src/rclpy/node.hpp @@ -52,7 +52,8 @@ class Node : public Destroyable, public std::enable_shared_from_this Context & context, py::object pycli_args, bool use_global_arguments, - bool enable_rosout); + bool enable_rosout, + bool enable_service_introspection); /// Get the fully qualified name of the node. /** diff --git a/rclpy/src/rclpy/service.cpp b/rclpy/src/rclpy/service.cpp index b9ec0b2ae..5aec77ed4 100644 --- a/rclpy/src/rclpy/service.cpp +++ b/rclpy/src/rclpy/service.cpp @@ -22,6 +22,7 @@ #include #include +#include "clock.hpp" #include "exceptions.hpp" #include "node.hpp" #include "service.hpp" @@ -37,24 +38,32 @@ Service::destroy() node_.destroy(); } +// TODO(clalancette): Why is this a std::string, while Client is a const char *? Service::Service( Node & node, py::object pysrv_type, const std::string & service_name, - py::object pyqos_profile) + py::object pyqos_srv_profile, py::object pyqos_service_event_pub, Clock & clock) : node_(node) { - auto srv_type = static_cast( + auto * srv_type = static_cast( common_get_type_support(pysrv_type)); - if (!srv_type) { + if (nullptr == srv_type) { throw py::error_already_set(); } rcl_service_options_t service_ops = rcl_service_get_default_options(); - if (!pyqos_profile.is_none()) { - service_ops.qos = pyqos_profile.cast(); + if (rcl_node_get_options(node.rcl_ptr())->enable_service_introspection) { + service_ops.clock = clock.rcl_ptr(); + service_ops.enable_service_introspection = true; + service_ops.event_publisher_options.qos = pyqos_service_event_pub.is_none() ? + rcl_publisher_get_default_options().qos : pyqos_service_event_pub.cast(); } - // Create a client + if (!pyqos_srv_profile.is_none()) { + service_ops.qos = pyqos_srv_profile.cast(); + } + + // Create a service rcl_service_ = std::shared_ptr( new rcl_service_t, [node](rcl_service_t * service) @@ -147,7 +156,7 @@ void define_service(py::object module) { py::class_>(module, "Service") - .def(py::init()) + .def(py::init()) .def_property_readonly( "pointer", [](const Service & service) { return reinterpret_cast(service.rcl_ptr()); diff --git a/rclpy/src/rclpy/service.hpp b/rclpy/src/rclpy/service.hpp index db1b96f60..4b2e0fa3b 100644 --- a/rclpy/src/rclpy/service.hpp +++ b/rclpy/src/rclpy/service.hpp @@ -23,6 +23,7 @@ #include #include +#include "clock.hpp" #include "destroyable.hpp" #include "node.hpp" #include "utils.hpp" @@ -47,12 +48,13 @@ class Service : public Destroyable, public std::enable_shared_from_this * \param[in] node Node to add the service to * \param[in] pysrv_type Service module associated with the service * \param[in] service_name Python object for the service name - * \param[in] pyqos_profile QoSProfile Python object for this service - * \return capsule containing the rcl_service_t + * \param[in] pyqos_srv_profile QoSProfile Python object for this service + * \param[in] pyqos_service_event_pub QoSProfile Python object for the service event publisher + * \param[in] clock Clock to use for service event timestamps */ Service( Node & node, py::object pysrv_type, const std::string & service_name, - py::object pyqos_profile); + py::object pyqos_srv_profile, py::object pyqos_service_event_pub, Clock & clock); Service( Node & node, std::shared_ptr rcl_service); diff --git a/rclpy/src/rclpy/service_introspection.cpp b/rclpy/src/rclpy/service_introspection.cpp new file mode 100644 index 000000000..a73ad68ab --- /dev/null +++ b/rclpy/src/rclpy/service_introspection.cpp @@ -0,0 +1,60 @@ +// 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 "service_introspection.hpp" +#include "rcl/service_introspection.h" +#include "rcl/client.h" +#include "rcl/node.h" +#include "rcl/service.h" + +namespace rclpy +{ + +void +define_service_introspection(py::module_ module) +{ + py::module_ m2 = module.def_submodule( + "service_introspection", + "utilities for introspecting services"); + m2.def( + "configure_service_events", + [](size_t srv, size_t node, bool opt) { + return rcl_service_introspection_configure_server_service_events( + reinterpret_cast< + rcl_service_t *>(srv), reinterpret_cast(node), opt); + }); + m2.def( + "configure_client_events", + [](size_t clt, size_t node, bool opt) { + return rcl_service_introspection_configure_client_service_events( + reinterpret_cast(clt), reinterpret_cast(node), opt); + }); + m2.def( + "configure_service_message_payload", [](size_t srv, bool opt) { + return rcl_service_introspection_configure_server_service_event_message_payload( + reinterpret_cast(srv), opt); + }); + m2.def( + "configure_client_message_payload", [](size_t clt, bool opt) { + return rcl_service_introspection_configure_client_service_event_message_payload( + reinterpret_cast(clt), opt); + }); + m2.attr("RCL_SERVICE_INTROSPECTION_PUBLISH_CLIENT_PARAMETER") = "publish_client_events"; + m2.attr("RCL_SERVICE_INTROSPECTION_PUBLISH_SERVICE_PARAMETER") = "publish_service_events"; + m2.attr("RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX") = RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX; +} +} // namespace rclpy diff --git a/rclpy/src/rclpy/service_introspection.hpp b/rclpy/src/rclpy/service_introspection.hpp new file mode 100644 index 000000000..03ee33a1d --- /dev/null +++ b/rclpy/src/rclpy/service_introspection.hpp @@ -0,0 +1,29 @@ +// 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 RCLPY__SERVICE_INTROSPECTION_HPP_ +#define RCLPY__SERVICE_INTROSPECTION_HPP_ + +#include "pybind11/pybind11.h" + +namespace py = pybind11; + +namespace rclpy +{ + +void +define_service_introspection(py::module_ module); + +} // namespace rclpy +#endif // RCLPY__SERVICE_INTROSPECTION_HPP_ diff --git a/rclpy/test/test_service_introspection.py b/rclpy/test/test_service_introspection.py new file mode 100644 index 000000000..1d8889571 --- /dev/null +++ b/rclpy/test/test_service_introspection.py @@ -0,0 +1,165 @@ +# 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. + +import time +from typing import List +import unittest + +import rclpy +import rclpy.executors +from rclpy.parameter import Parameter +from service_msgs.msg import ServiceEventInfo +from test_msgs.srv import BasicTypes + + +class TestServiceEvents(unittest.TestCase): + + @classmethod + def setUpClass(cls): + cls.context = rclpy.context.Context() + rclpy.init(context=cls.context) + + def setUp(self): + self.node = rclpy.create_node( + 'TestServiceIntrospection', context=self.context, enable_service_introspection=True) + self.executor = rclpy.executors.SingleThreadedExecutor(context=self.context) + self.executor.add_node(self.node) + self.srv = self.node.create_service(BasicTypes, 'test_service', self.srv_callback) + self.cli = self.node.create_client(BasicTypes, 'test_service') + self.sub = self.node.create_subscription(BasicTypes.Event, 'test_service/_service_event', + self.sub_callback, 10) + self.event_messages: List[BasicTypes.Event] = [] + + def tearDown(self): + self.node.destroy_node() + + @classmethod + def tearDownClass(cls): + rclpy.shutdown(context=cls.context) + + def sub_callback(self, msg): + self.event_messages.append(msg) + + def srv_callback(self, req, resp): + resp.bool_value = not req.bool_value + resp.int64_value = req.int64_value + return resp + + def test_service_introspection_metadata(self): + req = BasicTypes.Request() + req.bool_value = False + req.int64_value = 12345 + + self.node.set_parameters([ + Parameter('publish_service_events', Parameter.Type.STRING, 'metadata'), + Parameter('publish_client_events', Parameter.Type.STRING, 'metadata')]) + + future = self.cli.call_async(req) + self.executor.spin_until_future_complete(future, timeout_sec=5.0) + self.assertTrue(future.done()) + + start = time.monotonic() + end = start + 5.0 + while len(self.event_messages) < 4: + self.executor.spin_once(timeout_sec=0.1) + + now = time.monotonic() + self.assertTrue(now < end) + + self.assertEqual(len(self.event_messages), 4) + + result_dict = {} + for msg in self.event_messages: + result_dict[msg.info.event_type] = msg + self.assertEqual( + set(result_dict.keys()), + {ServiceEventInfo.REQUEST_SENT, ServiceEventInfo.REQUEST_RECEIVED, + ServiceEventInfo.RESPONSE_SENT, ServiceEventInfo.RESPONSE_RECEIVED}) + + self.assertEqual(len(result_dict[ServiceEventInfo.REQUEST_SENT].request), 0) + self.assertEqual(len(result_dict[ServiceEventInfo.REQUEST_SENT].response), 0) + + self.assertEqual(len(result_dict[ServiceEventInfo.RESPONSE_SENT].request), 0) + self.assertEqual(len(result_dict[ServiceEventInfo.RESPONSE_SENT].response), 0) + + self.assertEqual(len(result_dict[ServiceEventInfo.REQUEST_RECEIVED].request), 0) + self.assertEqual(len(result_dict[ServiceEventInfo.REQUEST_RECEIVED].response), 0) + + self.assertEqual(len(result_dict[ServiceEventInfo.RESPONSE_RECEIVED].request), 0) + self.assertEqual(len(result_dict[ServiceEventInfo.RESPONSE_RECEIVED].response), 0) + + def test_service_introspection_contents(self): + req = BasicTypes.Request() + req.bool_value = False + req.int64_value = 12345 + + self.node.set_parameters([ + Parameter('publish_service_events', Parameter.Type.STRING, 'contents'), + Parameter('publish_client_events', Parameter.Type.STRING, 'contents')]) + + future = self.cli.call_async(req) + self.executor.spin_until_future_complete(future, timeout_sec=5.0) + self.assertTrue(future.done()) + + start = time.monotonic() + end = start + 5.0 + while len(self.event_messages) < 4: + self.executor.spin_once(timeout_sec=0.1) + + now = time.monotonic() + self.assertTrue(now < end) + + self.assertEqual(len(self.event_messages), 4) + + result_dict = {} + for msg in self.event_messages: + result_dict[msg.info.event_type] = msg + self.assertEqual( + set(result_dict.keys()), + {ServiceEventInfo.REQUEST_SENT, ServiceEventInfo.REQUEST_RECEIVED, + ServiceEventInfo.RESPONSE_SENT, ServiceEventInfo.RESPONSE_RECEIVED}) + + self.assertEqual(len(result_dict[ServiceEventInfo.REQUEST_SENT].request), 1) + self.assertEqual(result_dict[ServiceEventInfo.REQUEST_SENT].request[0].bool_value, False) + self.assertEqual(result_dict[ServiceEventInfo.REQUEST_SENT].request[0].int64_value, 12345) + self.assertEqual(len(result_dict[ServiceEventInfo.REQUEST_SENT].response), 0) + + self.assertEqual(len(result_dict[ServiceEventInfo.RESPONSE_SENT].request), 0) + self.assertEqual(len(result_dict[ServiceEventInfo.RESPONSE_SENT].response), 1) + self.assertEqual(result_dict[ServiceEventInfo.RESPONSE_SENT].response[0].bool_value, True) + self.assertEqual( + result_dict[ServiceEventInfo.RESPONSE_SENT].response[0].int64_value, + 12345) + + self.assertEqual(len(result_dict[ServiceEventInfo.REQUEST_RECEIVED].request), 1) + self.assertEqual( + result_dict[ServiceEventInfo.REQUEST_RECEIVED].request[0].bool_value, + False) + self.assertEqual( + result_dict[ServiceEventInfo.REQUEST_RECEIVED].request[0].int64_value, + 12345) + self.assertEqual(len(result_dict[ServiceEventInfo.REQUEST_RECEIVED].response), 0) + + self.assertEqual(len(result_dict[ServiceEventInfo.RESPONSE_RECEIVED].request), 0) + self.assertEqual(len(result_dict[ServiceEventInfo.RESPONSE_RECEIVED].response), 1) + self.assertEqual( + result_dict[ServiceEventInfo.RESPONSE_RECEIVED].response[0].bool_value, + True) + self.assertEqual( + result_dict[ServiceEventInfo.RESPONSE_RECEIVED].response[0].int64_value, + 12345) + + +if __name__ == '__main__': + unittest.main() diff --git a/rclpy/test/test_waitable.py b/rclpy/test/test_waitable.py index f79841c51..1eca9d156 100644 --- a/rclpy/test/test_waitable.py +++ b/rclpy/test/test_waitable.py @@ -22,6 +22,7 @@ from rclpy.clock import ClockType from rclpy.executors import SingleThreadedExecutor from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy +from rclpy.qos import qos_profile_system_default from rclpy.qos import QoSProfile from rclpy.task import Future from rclpy.type_support import check_for_type_support @@ -43,7 +44,8 @@ def __init__(self, node): with node.handle: self.client = _rclpy.Client( - node.handle, EmptySrv, 'test_client', QoSProfile(depth=10).get_c_qos_profile()) + node.handle, EmptySrv, 'test_client', QoSProfile(depth=10).get_c_qos_profile(), + qos_profile_system_default.get_c_qos_profile(), node.get_clock().handle) self.client_index = None self.client_is_ready = False @@ -86,7 +88,8 @@ def __init__(self, node): with node.handle: self.server = _rclpy.Service( - node.handle, EmptySrv, 'test_server', QoSProfile(depth=10).get_c_qos_profile()) + node.handle, EmptySrv, 'test_server', QoSProfile(depth=10).get_c_qos_profile(), + qos_profile_system_default.get_c_qos_profile(), node.get_clock().handle) self.server_index = None self.server_is_ready = False From 963b548d33f9d467d8bf1fb86abfa73349fb705f Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 21 Feb 2023 20:12:15 +0000 Subject: [PATCH 3/3] Implement 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 --- rclpy/rclpy/__init__.py | 7 +- rclpy/rclpy/client.py | 19 ++++ rclpy/rclpy/node.py | 118 ++-------------------- rclpy/rclpy/service.py | 19 ++++ rclpy/rclpy/service_introspection.py | 17 ++++ rclpy/src/rclpy/_rclpy_pybind11.cpp | 4 +- rclpy/src/rclpy/client.cpp | 44 +++++--- rclpy/src/rclpy/client.hpp | 19 +++- rclpy/src/rclpy/node.cpp | 8 +- rclpy/src/rclpy/node.hpp | 3 +- rclpy/src/rclpy/service.cpp | 47 ++++++--- rclpy/src/rclpy/service.hpp | 19 +++- rclpy/src/rclpy/service_introspection.cpp | 37 +------ rclpy/src/rclpy/service_introspection.hpp | 1 + rclpy/test/test_service_introspection.py | 23 +++-- rclpy/test/test_waitable.py | 7 +- 16 files changed, 180 insertions(+), 212 deletions(-) create mode 100644 rclpy/rclpy/service_introspection.py diff --git a/rclpy/rclpy/__init__.py b/rclpy/rclpy/__init__.py index 33c2a56f0..cea1f9163 100644 --- a/rclpy/rclpy/__init__.py +++ b/rclpy/rclpy/__init__.py @@ -143,8 +143,7 @@ def create_node( start_parameter_services: bool = True, parameter_overrides: List[Parameter] = None, allow_undeclared_parameters: bool = False, - automatically_declare_parameters_from_overrides: bool = False, - enable_service_introspection: bool = False, + automatically_declare_parameters_from_overrides: bool = False ) -> 'Node': """ Create an instance of :class:`.Node`. @@ -166,7 +165,6 @@ def create_node( This option doesn't affect `parameter_overrides`. :param automatically_declare_parameters_from_overrides: If True, the "parameter overrides" will be used to implicitly declare parameters on the node during creation, default False. - :param enable_service_introspection: Flag to enable service introspection, default False. :return: An instance of the newly created node. """ # imported locally to avoid loading extensions on module import @@ -180,8 +178,7 @@ def create_node( allow_undeclared_parameters=allow_undeclared_parameters, automatically_declare_parameters_from_overrides=( automatically_declare_parameters_from_overrides - ), - enable_service_introspection=enable_service_introspection) + )) def spin_once(node: 'Node', *, executor: 'Executor' = None, timeout_sec: float = None) -> None: diff --git a/rclpy/rclpy/client.py b/rclpy/rclpy/client.py index 92e1b4782..d46256bc5 100644 --- a/rclpy/rclpy/client.py +++ b/rclpy/rclpy/client.py @@ -18,9 +18,11 @@ from typing import TypeVar from rclpy.callback_groups import CallbackGroup +from rclpy.clock import Clock from rclpy.context import Context from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy from rclpy.qos import QoSProfile +from rclpy.service_introspection import ServiceIntrospectionState from rclpy.task import Future # Used for documentation purposes only @@ -180,6 +182,23 @@ def wait_for_service(self, timeout_sec: float = None) -> bool: return self.service_is_ready() + def configure_introspection( + self, clock: Clock, + service_event_qos_profile: QoSProfile, + introspection_state: ServiceIntrospectionState + ) -> None: + """ + Configure client introspection. + + :param clock: Clock to use for generating timestamps. + :param service_event_qos_profile: QoSProfile to use when creating service event publisher. + :param introspection_state: ServiceIntrospectionState to set introspection. + """ + with self.handle: + self.__client.configure_introspection(clock.handle, + service_event_qos_profile.get_c_qos_profile(), + introspection_state) + @property def handle(self): return self.__client diff --git a/rclpy/rclpy/node.py b/rclpy/rclpy/node.py index 04fee9fc7..95042e2ca 100644 --- a/rclpy/rclpy/node.py +++ b/rclpy/rclpy/node.py @@ -66,7 +66,6 @@ from rclpy.publisher import Publisher from rclpy.qos import qos_profile_parameter_events from rclpy.qos import qos_profile_services_default -from rclpy.qos import qos_profile_system_default from rclpy.qos import QoSProfile from rclpy.qos_event import PublisherEventCallbacks from rclpy.qos_event import SubscriptionEventCallbacks @@ -127,8 +126,7 @@ def __init__( start_parameter_services: bool = True, parameter_overrides: List[Parameter] = None, allow_undeclared_parameters: bool = False, - automatically_declare_parameters_from_overrides: bool = False, - enable_service_introspection: bool = False, + automatically_declare_parameters_from_overrides: bool = False ) -> None: """ Create a Node. @@ -152,8 +150,6 @@ def __init__( This flag affects the behavior of parameter-related operations. :param automatically_declare_parameters_from_overrides: If True, the "parameter overrides" will be used to implicitly declare parameters on the node during creation. - :param enable_service_introspection: If True, the node will enable introspection of - services. """ self.__handle = None self._context = get_default_context() if context is None else context @@ -186,8 +182,7 @@ def __init__( self._context.handle, cli_args, use_global_arguments, - enable_rosout, - enable_service_introspection + enable_rosout ) except ValueError: # these will raise more specific errors if the name or namespace is bad @@ -236,18 +231,6 @@ def __init__( if start_parameter_services: self._parameter_service = ParameterService(self) - if enable_service_introspection: - self.declare_parameters( - namespace='', - parameters=[ - (_rclpy.service_introspection.RCL_SERVICE_INTROSPECTION_PUBLISH_CLIENT_PARAMETER, # noqa E501 - 'off', ParameterDescriptor()), - (_rclpy.service_introspection.RCL_SERVICE_INTROSPECTION_PUBLISH_SERVICE_PARAMETER, # noqa E501 - 'off', ParameterDescriptor()), - ]) - self.add_on_set_parameters_callback(self._check_service_introspection_parameters) - self.add_post_set_parameters_callback(self._configure_service_introspection) - @property def publishers(self) -> Iterator[Publisher]: """Get publishers that have been created on this node.""" @@ -1593,89 +1576,13 @@ def create_subscription( return subscription - def _check_service_introspection_parameters( - self, parameters: List[Parameter] - ) -> SetParametersResult: - result = SetParametersResult(successful=True) - for param in parameters: - if param.name not in ( - _rclpy.service_introspection.RCL_SERVICE_INTROSPECTION_PUBLISH_CLIENT_PARAMETER, # noqa: E501 - _rclpy.service_introspection.RCL_SERVICE_INTROSPECTION_PUBLISH_SERVICE_PARAMETER): # noqa: E501 - continue - - if param.type_ != Parameter.Type.STRING: - result.successful = False - result.reason = 'Parameter type must be string' - break - - if param.value not in ('off', 'metadata', 'contents'): - result.successful = False - result.reason = "Value must be one of 'off', 'metadata', or 'contents'" - break - - return result - - def _configure_service_introspection(self, parameters: List[Parameter]): - for param in parameters: - if param.name == \ - _rclpy.service_introspection.RCL_SERVICE_INTROSPECTION_PUBLISH_CLIENT_PARAMETER: # noqa: E501 - - value = param.value - - for cli in self.clients: - should_enable_service_events = False - should_enable_contents = False - if value == 'off': - should_enable_service_events = False - should_enable_contents = False - elif value == 'metadata': - should_enable_service_events = True - should_enable_contents = False - elif value == 'contents': - should_enable_service_events = True - should_enable_contents = True - - _rclpy.service_introspection.configure_client_events( - cli.handle.pointer, - self.handle.pointer, - should_enable_service_events) - _rclpy.service_introspection.configure_client_message_payload( - cli.handle.pointer, - should_enable_contents) - elif param.name == \ - _rclpy.service_introspection.RCL_SERVICE_INTROSPECTION_PUBLISH_SERVICE_PARAMETER: # noqa: E501 - - value = param.value - - for srv in self.services: - should_enable_service_events = False - should_enable_contents = False - if value == 'off': - should_enable_service_events = False - should_enable_contents = False - elif value == 'metadata': - should_enable_service_events = True - should_enable_contents = False - elif value == 'contents': - should_enable_service_events = True - should_enable_contents = True - - _rclpy.service_introspection.configure_service_events( - srv.handle.pointer, - self.handle.pointer, - should_enable_service_events) - _rclpy.service_introspection.configure_service_message_payload( - srv.handle.pointer, - should_enable_contents) - def create_client( self, srv_type, srv_name: str, *, qos_profile: QoSProfile = qos_profile_services_default, - callback_group: CallbackGroup = None, - service_event_qos_profile: QoSProfile = qos_profile_system_default + callback_group: CallbackGroup = None ) -> Client: """ Create a new service client. @@ -1683,12 +1590,8 @@ def create_client( :param srv_type: The service type. :param srv_name: The name of the service. :param qos_profile: The quality of service profile to apply the service client. - :param service_event_publisher_qos_profile: The quality of service - profile to apply the service event publisher. :param callback_group: The callback group for the service client. If ``None``, then the default callback group for the node is used. - :param service_event_qos_profile: The quality of service profile to apply to service - introspection (if enabled). """ if callback_group is None: callback_group = self.default_callback_group @@ -1700,9 +1603,7 @@ def create_client( self.handle, srv_type, srv_name, - qos_profile.get_c_qos_profile(), - service_event_qos_profile.get_c_qos_profile(), - self._clock.handle) + qos_profile.get_c_qos_profile()) except ValueError: failed = True if failed: @@ -1724,8 +1625,7 @@ def create_service( callback: Callable[[SrvTypeRequest, SrvTypeResponse], SrvTypeResponse], *, qos_profile: QoSProfile = qos_profile_services_default, - callback_group: CallbackGroup = None, - service_event_qos_profile: QoSProfile = qos_profile_system_default + callback_group: CallbackGroup = None ) -> Service: """ Create a new service server. @@ -1735,12 +1635,8 @@ def create_service( :param callback: A user-defined callback function that is called when a service request received by the server. :param qos_profile: The quality of service profile to apply the service server. - :param service_event_publisher_qos_profile: The quality of service - profile to apply the service event publisher. :param callback_group: The callback group for the service server. If ``None``, then the default callback group for the node is used. - :param service_event_qos_profile: The quality of service profile to apply to service - introspection (if enabled). """ if callback_group is None: callback_group = self.default_callback_group @@ -1752,9 +1648,7 @@ def create_service( self.handle, srv_type, srv_name, - qos_profile.get_c_qos_profile(), - service_event_qos_profile.get_c_qos_profile(), - self._clock.handle) + qos_profile.get_c_qos_profile()) except ValueError: failed = True if failed: diff --git a/rclpy/rclpy/service.py b/rclpy/rclpy/service.py index 3fe278792..adb55056a 100644 --- a/rclpy/rclpy/service.py +++ b/rclpy/rclpy/service.py @@ -16,8 +16,10 @@ from typing import TypeVar from rclpy.callback_groups import CallbackGroup +from rclpy.clock import Clock from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy from rclpy.qos import QoSProfile +from rclpy.service_introspection import ServiceIntrospectionState # Used for documentation purposes only SrvType = TypeVar('SrvType') @@ -79,6 +81,23 @@ def send_response(self, response: SrvTypeResponse, header) -> None: else: raise TypeError() + def configure_introspection( + self, clock: Clock, + service_event_qos_profile: QoSProfile, + introspection_state: ServiceIntrospectionState + ) -> None: + """ + Configure service introspection. + + :param clock: Clock to use for generating timestamps. + :param service_event_qos_profile: QoSProfile to use when creating service event publisher. + :param introspection_state: ServiceIntrospectionState to set introspection. + """ + with self.handle: + self.__service.configure_introspection(clock.handle, + service_event_qos_profile.get_c_qos_profile(), + introspection_state) + @property def handle(self): return self.__service diff --git a/rclpy/rclpy/service_introspection.py b/rclpy/rclpy/service_introspection.py new file mode 100644 index 000000000..77fcfd793 --- /dev/null +++ b/rclpy/rclpy/service_introspection.py @@ -0,0 +1,17 @@ +# Copyright 2023 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. + +from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy + +ServiceIntrospectionState = _rclpy.service_introspection.ServiceIntrospectionState diff --git a/rclpy/src/rclpy/_rclpy_pybind11.cpp b/rclpy/src/rclpy/_rclpy_pybind11.cpp index 2e66a00da..307ff9afd 100644 --- a/rclpy/src/rclpy/_rclpy_pybind11.cpp +++ b/rclpy/src/rclpy/_rclpy_pybind11.cpp @@ -15,6 +15,7 @@ #include #include +#include #include #include @@ -120,6 +121,8 @@ PYBIND11_MODULE(_rclpy_pybind11, m) { py::register_exception( m, "InvalidHandle", PyExc_RuntimeError); + rclpy::define_service_introspection(m); + rclpy::define_client(m); rclpy::define_context(m); @@ -241,5 +244,4 @@ PYBIND11_MODULE(_rclpy_pybind11, m) { rclpy::define_signal_handler_api(m); rclpy::define_clock_event(m); rclpy::define_lifecycle_api(m); - rclpy::define_service_introspection(m); } diff --git a/rclpy/src/rclpy/client.cpp b/rclpy/src/rclpy/client.cpp index 65a3c4f72..1376ec91a 100644 --- a/rclpy/src/rclpy/client.cpp +++ b/rclpy/src/rclpy/client.cpp @@ -17,12 +17,12 @@ #include #include #include +#include #include #include #include #include -#include #include "client.hpp" #include "clock.hpp" @@ -42,25 +42,16 @@ Client::destroy() } Client::Client( - Node & node, py::object pysrv_type, const std::string & service_name, py::object pyqos_profile, - py::object pyqos_service_event_pub, Clock & clock) + Node & node, py::object pysrv_type, const std::string & service_name, py::object pyqos_profile) : node_(node) { - auto srv_type = static_cast( - common_get_type_support(pysrv_type)); - if (nullptr == srv_type) { + srv_type_ = static_cast(common_get_type_support(pysrv_type)); + if (nullptr == srv_type_) { throw py::error_already_set(); } rcl_client_options_t client_ops = rcl_client_get_default_options(); - if (rcl_node_get_options(node.rcl_ptr())->enable_service_introspection) { - client_ops.clock = clock.rcl_ptr(); - client_ops.enable_service_introspection = true; - client_ops.event_publisher_options.qos = pyqos_service_event_pub.is_none() ? - rcl_publisher_get_default_options().qos : pyqos_service_event_pub.cast(); - } - if (!pyqos_profile.is_none()) { client_ops.qos = pyqos_profile.cast(); } @@ -86,7 +77,7 @@ Client::Client( *rcl_client_ = rcl_get_zero_initialized_client(); rcl_ret_t ret = rcl_client_init( - rcl_client_.get(), node_.rcl_ptr(), srv_type, service_name.c_str(), &client_ops); + rcl_client_.get(), node_.rcl_ptr(), srv_type_, service_name.c_str(), &client_ops); if (RCL_RET_OK != ret) { if (RCL_RET_SERVICE_NAME_INVALID == ret) { std::string error_text{"failed to create client due to invalid service name '"}; @@ -155,11 +146,29 @@ Client::take_response(py::object pyresponse_type) return result_tuple; } +void +Client::configure_introspection( + Clock & clock, py::object pyqos_service_event_pub, + rcl_service_introspection_state_t introspection_state) +{ + rcl_publisher_options_t pub_opts = rcl_publisher_get_default_options(); + pub_opts.qos = + pyqos_service_event_pub.is_none() ? rcl_publisher_get_default_options().qos : + pyqos_service_event_pub.cast(); + + rcl_ret_t ret = rcl_client_configure_service_introspection( + rcl_client_.get(), node_.rcl_ptr(), clock.rcl_ptr(), srv_type_, pub_opts, introspection_state); + + if (RCL_RET_OK != ret) { + throw RCLError("failed to configure client introspection"); + } +} + void define_client(py::object module) { py::class_>(module, "Client") - .def(py::init()) + .def(py::init()) .def_property_readonly( "pointer", [](const Client & client) { return reinterpret_cast(client.rcl_ptr()); @@ -173,6 +182,9 @@ define_client(py::object module) "Return true if the service server is available") .def( "take_response", &Client::take_response, - "Take a received response from an earlier request"); + "Take a received response from an earlier request") + .def( + "configure_introspection", &Client::configure_introspection, + "Configure whether introspection is enabled"); } } // namespace rclpy diff --git a/rclpy/src/rclpy/client.hpp b/rclpy/src/rclpy/client.hpp index 4872d74c5..90088afce 100644 --- a/rclpy/src/rclpy/client.hpp +++ b/rclpy/src/rclpy/client.hpp @@ -18,6 +18,7 @@ #include #include +#include #include #include @@ -47,12 +48,8 @@ class Client : public Destroyable, public std::enable_shared_from_this * \param[in] pysrv_type Service module associated with the client * \param[in] service_name The service name * \param[in] pyqos QoSProfile python object for this client - * \param[in] pyqos_service_event_pub QoSProfile python object for the service event publisher - * \param[in] clock Clock to use for service event timestamps */ - Client( - Node & node, py::object pysrv_type, const std::string & service_name, py::object pyqos, - py::object pyqos_service_event_pub, Clock & clock); + Client(Node & node, py::object pysrv_type, const std::string & service_name, py::object pyqos); ~Client() = default; @@ -93,6 +90,17 @@ class Client : public Destroyable, public std::enable_shared_from_this return rcl_client_.get(); } + /// Configure introspection. + /** + * \param[in] clock clock to use for service event timestamps + * \param[in] pyqos_service_event_pub QoSProfile python object for the service event publisher + * \param[in] introspection_state which state to set introspection to + */ + void + configure_introspection( + Clock & clock, py::object pyqos_service_event_pub, + rcl_service_introspection_state_t introspection_state); + /// Force an early destruction of this object void destroy() override; @@ -100,6 +108,7 @@ class Client : public Destroyable, public std::enable_shared_from_this private: Node node_; std::shared_ptr rcl_client_; + rosidl_service_type_support_t * srv_type_; }; /// Define a pybind11 wrapper for an rclpy::Client diff --git a/rclpy/src/rclpy/node.cpp b/rclpy/src/rclpy/node.cpp index 444db36de..50a00e000 100644 --- a/rclpy/src/rclpy/node.cpp +++ b/rclpy/src/rclpy/node.cpp @@ -16,7 +16,6 @@ #include #include -#include #include #include #include @@ -383,8 +382,7 @@ Node::Node( Context & context, py::object pycli_args, bool use_global_arguments, - bool enable_rosout, - bool enable_service_introspection) + bool enable_rosout) : context_(context) { rcl_ret_t ret; @@ -460,7 +458,6 @@ Node::Node( options.use_global_arguments = use_global_arguments; options.arguments = arguments; options.enable_rosout = enable_rosout; - options.enable_service_introspection = enable_service_introspection; { rclpy::LoggingGuard scoped_logging_guard; @@ -534,12 +531,11 @@ Node::get_action_names_and_types() return convert_to_py_names_and_types(&names_and_types); } - void define_node(py::object module) { py::class_>(module, "Node") - .def(py::init()) + .def(py::init()) .def_property_readonly( "pointer", [](const Node & node) { return reinterpret_cast(node.rcl_ptr()); diff --git a/rclpy/src/rclpy/node.hpp b/rclpy/src/rclpy/node.hpp index 7bc0c4978..bd17c516e 100644 --- a/rclpy/src/rclpy/node.hpp +++ b/rclpy/src/rclpy/node.hpp @@ -52,8 +52,7 @@ class Node : public Destroyable, public std::enable_shared_from_this Context & context, py::object pycli_args, bool use_global_arguments, - bool enable_rosout, - bool enable_service_introspection); + bool enable_rosout); /// Get the fully qualified name of the node. /** diff --git a/rclpy/src/rclpy/service.cpp b/rclpy/src/rclpy/service.cpp index 5aec77ed4..ad50dbd51 100644 --- a/rclpy/src/rclpy/service.cpp +++ b/rclpy/src/rclpy/service.cpp @@ -16,6 +16,7 @@ #include #include +#include #include #include @@ -38,29 +39,21 @@ Service::destroy() node_.destroy(); } -// TODO(clalancette): Why is this a std::string, while Client is a const char *? Service::Service( Node & node, py::object pysrv_type, const std::string & service_name, - py::object pyqos_srv_profile, py::object pyqos_service_event_pub, Clock & clock) + py::object pyqos_profile) : node_(node) { - auto * srv_type = static_cast( + srv_type_ = static_cast( common_get_type_support(pysrv_type)); - if (nullptr == srv_type) { + if (nullptr == srv_type_) { throw py::error_already_set(); } rcl_service_options_t service_ops = rcl_service_get_default_options(); - if (rcl_node_get_options(node.rcl_ptr())->enable_service_introspection) { - service_ops.clock = clock.rcl_ptr(); - service_ops.enable_service_introspection = true; - service_ops.event_publisher_options.qos = pyqos_service_event_pub.is_none() ? - rcl_publisher_get_default_options().qos : pyqos_service_event_pub.cast(); - } - - if (!pyqos_srv_profile.is_none()) { - service_ops.qos = pyqos_srv_profile.cast(); + if (!pyqos_profile.is_none()) { + service_ops.qos = pyqos_profile.cast(); } // Create a service @@ -84,7 +77,8 @@ Service::Service( *rcl_service_ = rcl_get_zero_initialized_service(); rcl_ret_t ret = rcl_service_init( - rcl_service_.get(), node_.rcl_ptr(), srv_type, service_name.c_str(), &service_ops); + rcl_service_.get(), node_.rcl_ptr(), srv_type_, + service_name.c_str(), &service_ops); if (RCL_RET_OK != ret) { if (ret == RCL_RET_SERVICE_NAME_INVALID) { std::string error_text{"failed to create service due to invalid topic name '"}; @@ -152,11 +146,29 @@ Service::get_qos_profile() return rclpy::convert_to_qos_dict(&options->qos); } +void +Service::configure_introspection( + Clock & clock, py::object pyqos_service_event_pub, + rcl_service_introspection_state_t introspection_state) +{ + rcl_publisher_options_t pub_opts = rcl_publisher_get_default_options(); + pub_opts.qos = + pyqos_service_event_pub.is_none() ? rcl_publisher_get_default_options().qos : + pyqos_service_event_pub.cast(); + + rcl_ret_t ret = rcl_service_configure_service_introspection( + rcl_service_.get(), node_.rcl_ptr(), clock.rcl_ptr(), srv_type_, pub_opts, introspection_state); + + if (RCL_RET_OK != ret) { + throw RCLError("failed to configure service introspection"); + } +} + void define_service(py::object module) { py::class_>(module, "Service") - .def(py::init()) + .def(py::init()) .def_property_readonly( "pointer", [](const Service & service) { return reinterpret_cast(service.rcl_ptr()); @@ -173,6 +185,9 @@ define_service(py::object module) "Send a response") .def( "service_take_request", &Service::service_take_request, - "Take a request from a given service"); + "Take a request from a given service") + .def( + "configure_introspection", &Service::configure_introspection, + "Configure whether introspection is enabled"); } } // namespace rclpy diff --git a/rclpy/src/rclpy/service.hpp b/rclpy/src/rclpy/service.hpp index 4b2e0fa3b..278cad097 100644 --- a/rclpy/src/rclpy/service.hpp +++ b/rclpy/src/rclpy/service.hpp @@ -18,6 +18,7 @@ #include #include +#include #include #include @@ -48,13 +49,11 @@ class Service : public Destroyable, public std::enable_shared_from_this * \param[in] node Node to add the service to * \param[in] pysrv_type Service module associated with the service * \param[in] service_name Python object for the service name - * \param[in] pyqos_srv_profile QoSProfile Python object for this service - * \param[in] pyqos_service_event_pub QoSProfile Python object for the service event publisher - * \param[in] clock Clock to use for service event timestamps + * \param[in] pyqos_profile QoSProfile Python object for this service */ Service( Node & node, py::object pysrv_type, const std::string & service_name, - py::object pyqos_srv_profile, py::object pyqos_service_event_pub, Clock & clock); + py::object pyqos_profile); Service( Node & node, std::shared_ptr rcl_service); @@ -100,6 +99,17 @@ class Service : public Destroyable, public std::enable_shared_from_this py::dict get_qos_profile(); + /// Configure introspection. + /** + * \param[in] clock clock to use for service event timestamps + * \param[in] pyqos_service_event_pub QoSProfile python object for the service event publisher + * \param[in] introspection_state which state to set introspection to + */ + void + configure_introspection( + Clock & clock, py::object pyqos_service_event_pub, + rcl_service_introspection_state_t introspection_state); + /// Force an early destruction of this object void destroy() override; @@ -107,6 +117,7 @@ class Service : public Destroyable, public std::enable_shared_from_this private: Node node_; std::shared_ptr rcl_service_; + rosidl_service_type_support_t * srv_type_; }; /// Define a pybind11 wrapper for an rclpy::Service diff --git a/rclpy/src/rclpy/service_introspection.cpp b/rclpy/src/rclpy/service_introspection.cpp index a73ad68ab..8ac62bed2 100644 --- a/rclpy/src/rclpy/service_introspection.cpp +++ b/rclpy/src/rclpy/service_introspection.cpp @@ -12,13 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - #include "service_introspection.hpp" #include "rcl/service_introspection.h" -#include "rcl/client.h" -#include "rcl/node.h" -#include "rcl/service.h" namespace rclpy { @@ -29,32 +24,10 @@ define_service_introspection(py::module_ module) py::module_ m2 = module.def_submodule( "service_introspection", "utilities for introspecting services"); - m2.def( - "configure_service_events", - [](size_t srv, size_t node, bool opt) { - return rcl_service_introspection_configure_server_service_events( - reinterpret_cast< - rcl_service_t *>(srv), reinterpret_cast(node), opt); - }); - m2.def( - "configure_client_events", - [](size_t clt, size_t node, bool opt) { - return rcl_service_introspection_configure_client_service_events( - reinterpret_cast(clt), reinterpret_cast(node), opt); - }); - m2.def( - "configure_service_message_payload", [](size_t srv, bool opt) { - return rcl_service_introspection_configure_server_service_event_message_payload( - reinterpret_cast(srv), opt); - }); - m2.def( - "configure_client_message_payload", [](size_t clt, bool opt) { - return rcl_service_introspection_configure_client_service_event_message_payload( - reinterpret_cast(clt), opt); - }); - m2.attr("RCL_SERVICE_INTROSPECTION_PUBLISH_CLIENT_PARAMETER") = "publish_client_events"; - m2.attr("RCL_SERVICE_INTROSPECTION_PUBLISH_SERVICE_PARAMETER") = "publish_service_events"; - m2.attr("RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX") = RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX; + + py::enum_(m2, "ServiceIntrospectionState") + .value("OFF", RCL_SERVICE_INTROSPECTION_OFF) + .value("METADATA", RCL_SERVICE_INTROSPECTION_METADATA) + .value("CONTENTS", RCL_SERVICE_INTROSPECTION_CONTENTS); } } // namespace rclpy diff --git a/rclpy/src/rclpy/service_introspection.hpp b/rclpy/src/rclpy/service_introspection.hpp index 03ee33a1d..1f6173104 100644 --- a/rclpy/src/rclpy/service_introspection.hpp +++ b/rclpy/src/rclpy/service_introspection.hpp @@ -26,4 +26,5 @@ void define_service_introspection(py::module_ module); } // namespace rclpy + #endif // RCLPY__SERVICE_INTROSPECTION_HPP_ diff --git a/rclpy/test/test_service_introspection.py b/rclpy/test/test_service_introspection.py index 1d8889571..909a5e776 100644 --- a/rclpy/test/test_service_introspection.py +++ b/rclpy/test/test_service_introspection.py @@ -18,7 +18,8 @@ import rclpy import rclpy.executors -from rclpy.parameter import Parameter +from rclpy.qos import qos_profile_system_default +from rclpy.service_introspection import ServiceIntrospectionState from service_msgs.msg import ServiceEventInfo from test_msgs.srv import BasicTypes @@ -32,7 +33,7 @@ def setUpClass(cls): def setUp(self): self.node = rclpy.create_node( - 'TestServiceIntrospection', context=self.context, enable_service_introspection=True) + 'TestServiceIntrospection', context=self.context) self.executor = rclpy.executors.SingleThreadedExecutor(context=self.context) self.executor.add_node(self.node) self.srv = self.node.create_service(BasicTypes, 'test_service', self.srv_callback) @@ -61,9 +62,12 @@ def test_service_introspection_metadata(self): req.bool_value = False req.int64_value = 12345 - self.node.set_parameters([ - Parameter('publish_service_events', Parameter.Type.STRING, 'metadata'), - Parameter('publish_client_events', Parameter.Type.STRING, 'metadata')]) + self.cli.configure_introspection(self.node.get_clock(), + qos_profile_system_default, + ServiceIntrospectionState.METADATA) + self.srv.configure_introspection(self.node.get_clock(), + qos_profile_system_default, + ServiceIntrospectionState.METADATA) future = self.cli.call_async(req) self.executor.spin_until_future_complete(future, timeout_sec=5.0) @@ -104,9 +108,12 @@ def test_service_introspection_contents(self): req.bool_value = False req.int64_value = 12345 - self.node.set_parameters([ - Parameter('publish_service_events', Parameter.Type.STRING, 'contents'), - Parameter('publish_client_events', Parameter.Type.STRING, 'contents')]) + self.cli.configure_introspection(self.node.get_clock(), + qos_profile_system_default, + ServiceIntrospectionState.CONTENTS) + self.srv.configure_introspection(self.node.get_clock(), + qos_profile_system_default, + ServiceIntrospectionState.CONTENTS) future = self.cli.call_async(req) self.executor.spin_until_future_complete(future, timeout_sec=5.0) diff --git a/rclpy/test/test_waitable.py b/rclpy/test/test_waitable.py index 1eca9d156..f79841c51 100644 --- a/rclpy/test/test_waitable.py +++ b/rclpy/test/test_waitable.py @@ -22,7 +22,6 @@ from rclpy.clock import ClockType from rclpy.executors import SingleThreadedExecutor from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy -from rclpy.qos import qos_profile_system_default from rclpy.qos import QoSProfile from rclpy.task import Future from rclpy.type_support import check_for_type_support @@ -44,8 +43,7 @@ def __init__(self, node): with node.handle: self.client = _rclpy.Client( - node.handle, EmptySrv, 'test_client', QoSProfile(depth=10).get_c_qos_profile(), - qos_profile_system_default.get_c_qos_profile(), node.get_clock().handle) + node.handle, EmptySrv, 'test_client', QoSProfile(depth=10).get_c_qos_profile()) self.client_index = None self.client_is_ready = False @@ -88,8 +86,7 @@ def __init__(self, node): with node.handle: self.server = _rclpy.Service( - node.handle, EmptySrv, 'test_server', QoSProfile(depth=10).get_c_qos_profile(), - qos_profile_system_default.get_c_qos_profile(), node.get_clock().handle) + node.handle, EmptySrv, 'test_server', QoSProfile(depth=10).get_c_qos_profile()) self.server_index = None self.server_is_ready = False