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/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/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 719e9a87d..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 @@ -44,6 +45,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" @@ -119,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); diff --git a/rclpy/src/rclpy/client.cpp b/rclpy/src/rclpy/client.cpp index af22bb720..1376ec91a 100644 --- a/rclpy/src/rclpy/client.cpp +++ b/rclpy/src/rclpy/client.cpp @@ -17,6 +17,7 @@ #include #include #include +#include #include #include @@ -24,6 +25,7 @@ #include #include "client.hpp" +#include "clock.hpp" #include "exceptions.hpp" #include "node.hpp" #include "python_allocator.hpp" @@ -40,12 +42,11 @@ 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( - common_get_type_support(pysrv_type)); - if (!srv_type) { + srv_type_ = static_cast(common_get_type_support(pysrv_type)); + if (nullptr == srv_type_) { throw py::error_already_set(); } @@ -55,7 +56,6 @@ Client::Client( client_ops.qos = pyqos_profile.cast(); } - // Create a client rcl_client_ = std::shared_ptr( PythonAllocator().allocate(1), @@ -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 '"}; @@ -146,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()); @@ -164,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 ff1f29d59..90088afce 100644 --- a/rclpy/src/rclpy/client.hpp +++ b/rclpy/src/rclpy/client.hpp @@ -18,9 +18,13 @@ #include #include +#include +#include #include +#include +#include "clock.hpp" #include "destroyable.hpp" #include "node.hpp" @@ -43,9 +47,9 @@ 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 */ - 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; @@ -86,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; @@ -93,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/service.cpp b/rclpy/src/rclpy/service.cpp index 02b7ba88d..ad50dbd51 100644 --- a/rclpy/src/rclpy/service.cpp +++ b/rclpy/src/rclpy/service.cpp @@ -16,12 +16,14 @@ #include #include +#include #include #include #include #include +#include "clock.hpp" #include "exceptions.hpp" #include "node.hpp" #include "service.hpp" @@ -38,13 +40,13 @@ 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) { - auto srv_type = static_cast( + srv_type_ = static_cast( common_get_type_support(pysrv_type)); - if (!srv_type) { + if (nullptr == srv_type_) { throw py::error_already_set(); } @@ -54,7 +56,7 @@ Service::Service( service_ops.qos = pyqos_profile.cast(); } - // Create a client + // Create a service rcl_service_ = std::shared_ptr( new rcl_service_t, [node](rcl_service_t * service) @@ -75,7 +77,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, + 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) { @@ -144,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()); @@ -165,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 95c2dd05c..278cad097 100644 --- a/rclpy/src/rclpy/service.hpp +++ b/rclpy/src/rclpy/service.hpp @@ -18,11 +18,13 @@ #include #include +#include #include #include #include +#include "clock.hpp" #include "destroyable.hpp" #include "node.hpp" #include "utils.hpp" @@ -48,10 +50,9 @@ class Service : public Destroyable, public std::enable_shared_from_this * \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 */ 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( @@ -98,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; @@ -105,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 new file mode 100644 index 000000000..8ac62bed2 --- /dev/null +++ b/rclpy/src/rclpy/service_introspection.cpp @@ -0,0 +1,33 @@ +// 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 "service_introspection.hpp" +#include "rcl/service_introspection.h" + +namespace rclpy +{ + +void +define_service_introspection(py::module_ module) +{ + py::module_ m2 = module.def_submodule( + "service_introspection", + "utilities for introspecting services"); + + 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 new file mode 100644 index 000000000..1f6173104 --- /dev/null +++ b/rclpy/src/rclpy/service_introspection.hpp @@ -0,0 +1,30 @@ +// 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..909a5e776 --- /dev/null +++ b/rclpy/test/test_service_introspection.py @@ -0,0 +1,172 @@ +# 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.qos import qos_profile_system_default +from rclpy.service_introspection import ServiceIntrospectionState +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) + 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.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) + 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.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) + 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()