From ab0827e1077094c8ec30e6b5098a4dfd45c94fa3 Mon Sep 17 00:00:00 2001 From: Matheus Sousa Date: Thu, 5 Feb 2026 16:53:04 -0300 Subject: [PATCH] Fixing remaps and namespaces Uses the main python node and parse the data for the internal image_transport node. --- image_transport_py/README.md | 10 ++-- .../pybind_image_transport.cpp | 56 ++++++++++++------- 2 files changed, 42 insertions(+), 24 deletions(-) diff --git a/image_transport_py/README.md b/image_transport_py/README.md index 97c8259e..658dc2dc 100644 --- a/image_transport_py/README.md +++ b/image_transport_py/README.md @@ -61,25 +61,25 @@ An object for image transport operations. #### Constructor -- `__init__(node_name, image_transport="", launch_params_filepath="")` +- `__init__(node, node_name, image_transport="", launch_params_filepath="")` Initialize an ImageTransport object with its node name, `image_transport` and launch params file path. If no `image_transport` specified, the default `raw` plugin will be initialized. #### Methods -- `advertise(base_topic, queue_size, latch=False)` +- `advertise(node, base_topic, queue_size, latch=False)` Advertise an image topic. -- `advertise_camera(base_topic, queue_size, latch=False)` +- `advertise_camera(node, base_topic, queue_size, latch=False)` Advertise an image topic with camera info. -- `subscribe(base_topic, queue_size, callback)` +- `subscribe(node, base_topic, queue_size, callback)` Subscribe to an image topic. -- `subscribe_camera(base_topic, queue_size, callback)` +- `subscribe_camera(node, base_topic, queue_size, callback)` Subscribe to an image topic with camera info. diff --git a/image_transport_py/src/image_transport_py/pybind_image_transport.cpp b/image_transport_py/src/image_transport_py/pybind_image_transport.cpp index a9cdab47..2a630341 100644 --- a/image_transport_py/src/image_transport_py/pybind_image_transport.cpp +++ b/image_transport_py/src/image_transport_py/pybind_image_transport.cpp @@ -78,21 +78,23 @@ PYBIND11_MODULE(_image_transport, m) pybind11::class_(m, "ImageTransport") .def( pybind11::init( - [](const std::string & node_name, const std::string & image_transport, - const std::string & launch_params_filepath) { + [](pybind11::object py_node, std::string & node_name, + const std::string & image_transport, const std::string & launch_params_filepath) { if (!rclcpp::ok()) { rclcpp::init(0, nullptr); } rclcpp::NodeOptions node_options; + std::string ros_ns = "__ns:=" + py_node.attr("get_namespace")().cast(); if (!launch_params_filepath.empty()) { node_options.allow_undeclared_parameters(true) .automatically_declare_parameters_from_overrides(true) - .arguments({"--ros-args", "--params-file", launch_params_filepath}); + .arguments({"--ros-args", "--params-file", launch_params_filepath, "-r", ros_ns}); } else if (!image_transport.empty()) { node_options.allow_undeclared_parameters(true) .automatically_declare_parameters_from_overrides(true) - .append_parameter_override("image_transport", image_transport); + .append_parameter_override("image_transport", image_transport) + .arguments({"--ros-args", "-r", ros_ns}); } rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared(node_name, "", node_options); @@ -109,62 +111,78 @@ PYBIND11_MODULE(_image_transport, m) return image_transport::ImageTransport(*node); }), - pybind11::arg("node_name"), pybind11::arg("image_transport") = "", + pybind11::arg("node"), pybind11::arg("node_name"), pybind11::arg("image_transport") = "", pybind11::arg("launch_params_filepath") = "", "Initialize an ImageTransport object with a node name, image_transport" " and launch params file path.") .def( "advertise", - pybind11::overload_cast( - &image_transport::ImageTransport::advertise), - pybind11::arg("base_topic"), pybind11::arg("queue_size"), pybind11::arg("latch") = false, - "Advertise an image topic.") + [](image_transport::ImageTransport & image_transport, pybind11::object py_node, + const std::string & base_topic, uint32_t queue_size, bool latch){ + + const std::string topic = py_node.attr("resolve_topic_name")(base_topic).cast(); + auto publisher = image_transport.advertise(topic, queue_size, latch); + return publisher; + }, + pybind11::arg("node"), pybind11::arg("base_topic"), pybind11::arg("queue_size"), + pybind11::arg("latch") = false, "Advertise an image topic.") .def( "advertise_camera", - pybind11::overload_cast( - &image_transport::ImageTransport::advertiseCamera), - pybind11::arg("base_topic"), pybind11::arg("queue_size"), pybind11::arg("latch") = false, - "Advertise an image topic with camera info.") + [](image_transport::ImageTransport & image_transport, pybind11::object py_node, + const std::string & base_topic, uint32_t queue_size, bool latch){ + + const std::string topic = py_node.attr("resolve_topic_name")(base_topic).cast(); + auto publisher = image_transport.advertise(topic, queue_size, latch); + return publisher; + }, + pybind11::arg("node"), pybind11::arg("base_topic"), pybind11::arg("queue_size"), + pybind11::arg("latch") = false, "Advertise an image topic with camera info.") .def( "subscribe", - [](image_transport::ImageTransport & image_transport, + [](image_transport::ImageTransport & image_transport, pybind11::object py_node, const std::string & base_topic, uint32_t queue_size, const image_transport::Subscriber::Callback & callback) { + const std::string topic = py_node.attr("resolve_topic_name")(base_topic).cast(); + // Static vector to keep the subscriptions alive thread_local auto vec = std::vector>(); auto subscription = std::make_shared( image_transport.subscribe( - base_topic, + topic, queue_size, callback)); vec.push_back(subscription); return subscription; }, - pybind11::arg("base_topic"), pybind11::arg("queue_size"), pybind11::arg("callback"), + pybind11::arg("node"), pybind11::arg("base_topic"), pybind11::arg("queue_size"), + pybind11::arg("callback"), "Subscribe to an image topic.") .def( "subscribe_camera", - [](image_transport::ImageTransport & image_transport, + [](image_transport::ImageTransport & image_transport, pybind11::object py_node, const std::string & base_topic, uint32_t queue_size, const image_transport::CameraSubscriber::Callback & callback) { + const std::string topic = py_node.attr("resolve_topic_name")(base_topic).cast(); + // Static vector to keep the subscriptions alive thread_local auto vec = std::vector>(); auto subscription = std::make_shared( image_transport.subscribeCamera( - base_topic, + topic, queue_size, callback)); vec.push_back(subscription); return subscription; }, - pybind11::arg("base_topic"), pybind11::arg("queue_size"), pybind11::arg("callback"), + pybind11::arg("node"), pybind11::arg("base_topic"), pybind11::arg("queue_size"), + pybind11::arg("callback"), "Subscribe to an image topic with camera info."); pybind11::class_>(