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