From 67fdcaa08bf0ca66fafb6b25104c3d8877b9d317 Mon Sep 17 00:00:00 2001 From: Brian Chen Date: Fri, 26 Aug 2022 02:00:02 -0700 Subject: [PATCH] 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 | 155 ++++++++++++++++++++++ 13 files changed, 415 insertions(+), 27 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 b01509cf6..96021c04c 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 @@ -189,6 +190,7 @@ if(BUILD_TESTING) test/test_qos_overriding_options.py test/test_rate.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..e206c4cf6 --- /dev/null +++ b/rclpy/test/test_service_introspection.py @@ -0,0 +1,155 @@ +# 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. + +from typing import List +import time +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()