diff --git a/rclpy/rclpy/node.py b/rclpy/rclpy/node.py index b57401514..2c28053d0 100644 --- a/rclpy/rclpy/node.py +++ b/rclpy/rclpy/node.py @@ -1091,6 +1091,32 @@ def remove_waitable(self, waitable: Waitable) -> None: self.__waitables.remove(waitable) self._wake_executor() + def resolve_topic_name(self, topic: str, *, only_expand: bool = False) -> str: + """ + Return a topic name expanded and remapped. + + :param topic: topic name to be expanded and remapped. + :param only_expand: if `True`, remapping rules won't be applied. + :return: a fully qualified topic name, + result of applying expansion and remapping to the given `topic`. + """ + with self.handle as capsule: + return _rclpy.rclpy_resolve_name(capsule, topic, only_expand, False) + + def resolve_service_name( + self, service: str, *, only_expand: bool = False + ) -> str: + """ + Return a service name expanded and remapped. + + :param service: service name to be expanded and remapped. + :param only_expand: if `True`, remapping rules won't be applied. + :return: a fully qualified service name, + result of applying expansion and remapping to the given `service`. + """ + with self.handle as capsule: + return _rclpy.rclpy_resolve_name(capsule, service, only_expand, True) + def create_publisher( self, msg_type, diff --git a/rclpy/src/rclpy/_rclpy.c b/rclpy/src/rclpy/_rclpy.c index 6102ec523..08b4fb489 100644 --- a/rclpy/src/rclpy/_rclpy.c +++ b/rclpy/src/rclpy/_rclpy.c @@ -1693,6 +1693,61 @@ rclpy_remap_topic_name(PyObject * Py_UNUSED(self), PyObject * args) return result; } + +/// Expand and remap a topic name +/** + * Raises ValueError if the capsule is not the correct type + * + * \param[in] pynode Capsule pointing to the node + * \param[in] topic_name topic string to be remapped + * \param[in] only_expand when `false`, remapping rules are ignored + * \param[in] is_service `true` for service names, `false` for topic names + * \return expanded and remapped topic name + */ +static PyObject * +rclpy_resolve_name(PyObject * Py_UNUSED(self), PyObject * args) +{ + PyObject * pynode = NULL; + const char * name = NULL; + int only_expand = false; + int is_service = false; + + + if (!PyArg_ParseTuple(args, "Ospp", &pynode, &name, &only_expand, &is_service)) { + return NULL; + } + + const rcl_node_t * node = rclpy_handle_get_pointer_from_capsule(pynode, "rcl_node_t"); + if (node == NULL) { + return NULL; + } + const rcl_node_options_t * node_options = rcl_node_get_options(node); + if (node_options == NULL) { + return NULL; + } + + char * output_cstr = NULL; + rcl_ret_t ret = rcl_node_resolve_name( + node, + name, + node_options->allocator, + is_service, + only_expand, + &output_cstr); + if (ret != RCL_RET_OK) { + PyErr_Format( + PyExc_RuntimeError, "Failed to resolve name %s: %s", name, rcl_get_error_string().str); + rcl_reset_error(); + return NULL; + } + + PyObject * result = PyUnicode_FromString(output_cstr); + node_options->allocator.deallocate(output_cstr, node_options->allocator.state); + + return result; +} + + /// Handle destructor for publisher static void _rclpy_destroy_publisher(void * p) @@ -5697,6 +5752,10 @@ static PyMethodDef rclpy_methods[] = { rclpy_get_validation_error_for_node_name, METH_VARARGS, "Get the error message and invalid index of a node name or None if valid." }, + { + "rclpy_resolve_name", rclpy_resolve_name, METH_VARARGS, + "Expand and remap a topic or service name." + }, { "rclpy_create_publisher", rclpy_create_publisher, METH_VARARGS, "Create a Publisher." diff --git a/rclpy/test/test_node.py b/rclpy/test/test_node.py index 3a766c02f..601750e0e 100644 --- a/rclpy/test/test_node.py +++ b/rclpy/test/test_node.py @@ -1920,5 +1920,21 @@ def test_node_get_fully_qualified_name(self): rclpy.shutdown(context=g_context) +def test_node_resolve_name(): + context = rclpy.Context() + rclpy.init( + args=['--ros-args', '-r', 'foo:=bar'], + context=context, + ) + node = rclpy.create_node('test_rclpy_node_resolve_name', namespace='/my_ns', context=context) + assert node.resolve_topic_name('foo') == '/my_ns/bar' + assert node.resolve_topic_name('/abs') == '/abs' + assert node.resolve_topic_name('foo', only_expand=True) == '/my_ns/foo' + assert node.resolve_service_name('foo') == '/my_ns/bar' + assert node.resolve_service_name('/abs') == '/abs' + assert node.resolve_service_name('foo', only_expand=True) == '/my_ns/foo' + rclpy.shutdown(context=context) + + if __name__ == '__main__': unittest.main()