get_type_description service (#1139)

* Add get_type_description service

Signed-off-by: Emerson Knapp <[email protected]>

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
 )

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()
 

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)

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(

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 <pybind11/pybind11.h>
+
+#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<rcl_service_t> 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<Service>(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<type_description_interfaces__srv__GetTypeDescription_Request *>(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_<TypeDescriptionService, Destroyable, std::shared_ptr<TypeDescriptionService>
+  >(module, "TypeDescriptionService")
+  .def(py::init<Node &>())
+  .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

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 <pybind11/pybind11.h>
+
+#include <rmw/types.h>
+
+#include <memory>
+
+#include "destroyable.hpp"
+#include "service.hpp"
+
+namespace py = pybind11;
+
+namespace rclpy
+{
+
+class TypeDescriptionService
+  : public Destroyable, public std::enable_shared_from_this<TypeDescriptionService>
+{
+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> 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_

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),
             }
         )