diff --git a/rclpy/CMakeLists.txt b/rclpy/CMakeLists.txt index a6e2be9be..d91c6b091 100644 --- a/rclpy/CMakeLists.txt +++ b/rclpy/CMakeLists.txt @@ -102,6 +102,7 @@ pybind11_add_module(_rclpy_pybind11 SHARED src/rclpy/subscription.cpp src/rclpy/time_point.cpp src/rclpy/timer.cpp + src/rclpy/type_description_service.cpp src/rclpy/utils.cpp src/rclpy/wait_set.cpp ) diff --git a/rclpy/rclpy/node.py b/rclpy/rclpy/node.py index 2e4bc8745..83f37a59a 100644 --- a/rclpy/rclpy/node.py +++ b/rclpy/rclpy/node.py @@ -78,6 +78,7 @@ from rclpy.timer import Rate from rclpy.timer import Timer from rclpy.topic_endpoint_info import TopicEndpointInfo +from rclpy.type_description_service import TypeDescriptionService from rclpy.type_support import check_is_valid_msg_type from rclpy.type_support import check_is_valid_srv_type from rclpy.utilities import get_default_context @@ -239,6 +240,8 @@ def __init__( if enable_logger_service: self._logger_service = LoggingService(self) + self._type_description_service = TypeDescriptionService(self) + @property def publishers(self) -> Iterator[Publisher]: """Get publishers that have been created on this node.""" @@ -1887,6 +1890,7 @@ def destroy_node(self): self.destroy_timer(self._timers[0]) while self._guards: self.destroy_guard_condition(self._guards[0]) + self._type_description_service.destroy() self.__node.destroy_when_not_in_use() self._wake_executor() diff --git a/rclpy/rclpy/type_description_service.py b/rclpy/rclpy/type_description_service.py new file mode 100644 index 000000000..d6940b995 --- /dev/null +++ b/rclpy/rclpy/type_description_service.py @@ -0,0 +1,96 @@ +# 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 rcl_interfaces.msg import ParameterDescriptor +from rcl_interfaces.msg import ParameterType + +from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy +from rclpy.parameter import Parameter +from rclpy.qos import qos_profile_services_default +from rclpy.service import Service +from rclpy.type_support import check_is_valid_srv_type +from rclpy.validate_topic_name import TOPIC_SEPARATOR_STRING + +from type_description_interfaces.srv import GetTypeDescription + +START_TYPE_DESCRIPTION_SERVICE_PARAM = 'start_type_description_service' + + +class TypeDescriptionService: + """ + Optionally initializes and contaiins the ~/get_type_description service. + + The service is implemented in rcl, but should be enabled via parameter and have its + callbacks handled via end-client execution framework, such as callback groups and waitsets. + + This is not intended for use by end users, rather it is a component to be used by Node. + """ + + def __init__(self, node): + """Initialize the service, if the parameter is set to true.""" + node_name = node.get_name() + self.service_name = TOPIC_SEPARATOR_STRING.join((node_name, 'get_type_description')) + self._type_description_srv = None + + self.enabled = False + if not node.has_parameter(START_TYPE_DESCRIPTION_SERVICE_PARAM): + descriptor = ParameterDescriptor( + name=START_TYPE_DESCRIPTION_SERVICE_PARAM, + type=ParameterType.PARAMETER_BOOL, + description='If enabled, start the ~/get_type_description service.', + read_only=True) + node.declare_parameter( + START_TYPE_DESCRIPTION_SERVICE_PARAM, + True, + descriptor) + param = node.get_parameter(START_TYPE_DESCRIPTION_SERVICE_PARAM) + if param.type_ != Parameter.Type.NOT_SET: + if param.type_ == Parameter.Type.BOOL: + self.enabled = param.value + else: + node.get_logger().error( + "Invalid type for parameter '{}' {!r} should be bool" + .format(START_TYPE_DESCRIPTION_SERVICE_PARAM, param.type_)) + else: + node.get_logger().debug( + 'Parameter {} not set, defaulting to true.' + .format(START_TYPE_DESCRIPTION_SERVICE_PARAM)) + + if self.enabled: + self._start_service(node) + + def destroy(self): + if self._type_description_srv is not None: + self._type_description_srv.destroy_when_not_in_use() + self._type_description_srv = None + + def _start_service(self, node): + self._type_description_srv = _rclpy.TypeDescriptionService(node.handle) + # Because we are creating our own service wrapper, must manually add the service + # to the appropriate parts of Node because we cannot call create_service. + check_is_valid_srv_type(GetTypeDescription) + service = Service( + service_impl=self._type_description_srv.impl, + srv_type=GetTypeDescription, + srv_name=self.service_name, + callback=self._service_callback, + callback_group=node.default_callback_group, + qos_profile=qos_profile_services_default) + node.default_callback_group.add_entity(service) + node._services.append(service) + node._wake_executor() + + def _service_callback(self, request, response): + return self._type_description_srv.handle_request( + request, GetTypeDescription.Response, self._node) diff --git a/rclpy/src/rclpy/_rclpy_pybind11.cpp b/rclpy/src/rclpy/_rclpy_pybind11.cpp index dd9819e4c..4da96a5d5 100644 --- a/rclpy/src/rclpy/_rclpy_pybind11.cpp +++ b/rclpy/src/rclpy/_rclpy_pybind11.cpp @@ -50,6 +50,7 @@ #include "subscription.hpp" #include "time_point.hpp" #include "timer.hpp" +#include "type_description_service.hpp" #include "utils.hpp" #include "wait_set.hpp" @@ -133,6 +134,8 @@ PYBIND11_MODULE(_rclpy_pybind11, m) { rclpy::define_service(m); + rclpy::define_type_description_service(m); + rclpy::define_service_info(m); m.def( diff --git a/rclpy/src/rclpy/type_description_service.cpp b/rclpy/src/rclpy/type_description_service.cpp new file mode 100644 index 000000000..fbb4bb03d --- /dev/null +++ b/rclpy/src/rclpy/type_description_service.cpp @@ -0,0 +1,85 @@ +// 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. + +#include + +#include "type_description_service.hpp" +#include "utils.hpp" + +namespace rclpy +{ + +TypeDescriptionService::TypeDescriptionService(Node & node) +{ + rcl_ret_t ret = rcl_node_type_description_service_init(node.rcl_ptr()); + if (RCL_RET_OK != ret) { + throw RCLError("Failed to initialize type description service"); + } + rcl_service_t * srv_ptr = nullptr; + ret = rcl_node_get_type_description_service(node.rcl_ptr(), &srv_ptr); + if (RCL_RET_OK != ret) { + throw RCLError("Failed to retrieve type description service implementation"); + } + std::shared_ptr srv_shared( + srv_ptr, + [node](rcl_service_t * srv) { + (void)srv; + rcl_ret_t ret = rcl_node_type_description_service_fini(node.rcl_ptr()); + if (RCL_RET_OK != ret) { + throw RCLError("Failed to finalize type description service"); + } + }); + service_ = std::make_shared(node, srv_shared); +} + +Service TypeDescriptionService::get_impl() +{ + return *service_; +} + +py::object TypeDescriptionService::handle_request( + py::object pyrequest, + py::object pyresponse_type, + Node & node) +{ + // Header not used by handler, just needed as part of signature. + rmw_request_id_t header; + type_description_interfaces__srv__GetTypeDescription_Response response; + auto request = convert_from_py(pyrequest); + rcl_node_type_description_service_handle_request( + node.rcl_ptr(), + &header, + static_cast(request.get()), + &response); + return convert_to_py(&response, pyresponse_type); +} + +void TypeDescriptionService::destroy() +{ + service_.reset(); +} + +void +define_type_description_service(py::object module) +{ + py::class_ + >(module, "TypeDescriptionService") + .def(py::init()) + .def_property_readonly( + "impl", &TypeDescriptionService::get_impl, "Get the rcl service wrapper capsule.") + .def( + "handle_request", &TypeDescriptionService::handle_request, + "Handle an incoming request by calling RCL implementation"); +} +} // namespace rclpy diff --git a/rclpy/src/rclpy/type_description_service.hpp b/rclpy/src/rclpy/type_description_service.hpp new file mode 100644 index 000000000..272652d06 --- /dev/null +++ b/rclpy/src/rclpy/type_description_service.hpp @@ -0,0 +1,74 @@ +// 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. + +#ifndef RCLPY__TYPE_DESCRIPTION_SERVICE_HPP_ +#define RCLPY__TYPE_DESCRIPTION_SERVICE_HPP_ + +#include + +#include + +#include + +#include "destroyable.hpp" +#include "service.hpp" + +namespace py = pybind11; + +namespace rclpy +{ + +class TypeDescriptionService + : public Destroyable, public std::enable_shared_from_this +{ +public: + /// Initialize and contain the rcl implementation of ~/get_type_description + /** + * \param[in] node Node to add the service to + */ + explicit TypeDescriptionService(Node & node); + + ~TypeDescriptionService() = default; + + /// Return the wrapped rcl service, so that it can be added to the node waitsets + /** + * \return The capsule containing the Service + */ + Service + get_impl(); + + /// Handle an incoming request to the service + /** + * \param[in] pyrequest incoming request to handle + * \param[in] pyresponse_type Python type of the response object to wrap the C message in + * \param[in] node The node that this service belongs to + * \return response message to send + */ + py::object + handle_request(py::object pyrequest, py::object pyresponse_type, Node & node); + + /// Force early cleanup of object + void + destroy() override; + +private: + std::shared_ptr service_; +}; + +/// Define a pybind11 wrapper for an rclpy::TypeDescriptionService +void +define_type_description_service(py::object module); +} // namespace rclpy + +#endif // RCLPY__TYPE_DESCRIPTION_SERVICE_HPP_ diff --git a/rclpy/test/test_node.py b/rclpy/test/test_node.py index 249280eb9..cc05342b0 100644 --- a/rclpy/test/test_node.py +++ b/rclpy/test/test_node.py @@ -50,6 +50,7 @@ from rclpy.qos import QoSProfile from rclpy.qos import QoSReliabilityPolicy from rclpy.time_source import USE_SIM_TIME_NAME +from rclpy.type_description_service import START_TYPE_DESCRIPTION_SERVICE_PARAM from rclpy.utilities import get_rmw_implementation_identifier from test_msgs.msg import BasicTypes @@ -997,7 +998,9 @@ def test_node_get_parameters_by_prefix(self): 'bar_prefix.foo': self.node.get_parameter('bar_prefix.foo'), 'bar_prefix.bar': self.node.get_parameter('bar_prefix.bar'), 'bar_prefix.baz': self.node.get_parameter('bar_prefix.baz'), - USE_SIM_TIME_NAME: self.node.get_parameter(USE_SIM_TIME_NAME) + USE_SIM_TIME_NAME: self.node.get_parameter(USE_SIM_TIME_NAME), + START_TYPE_DESCRIPTION_SERVICE_PARAM: self.node.get_parameter( + START_TYPE_DESCRIPTION_SERVICE_PARAM), } )