diff --git a/examples/MODULE.bazel b/examples/MODULE.bazel index c92b4b7d..e5e6b83a 100644 --- a/examples/MODULE.bazel +++ b/examples/MODULE.bazel @@ -32,9 +32,11 @@ use_repo( # Required for bazel test: "ros2_common_interfaces", "ros2_geometry2", + "ros2_image_common", "ros2_launch", "ros2_launch_ros", "ros2_message_filters", + "ros2_pluginlib", "ros2_rcl_interfaces", "ros2_rclcpp", "ros2_rcutils", diff --git a/examples/image_pub_sub/BUILD.bazel b/examples/image_pub_sub/BUILD.bazel new file mode 100644 index 00000000..d98b4e54 --- /dev/null +++ b/examples/image_pub_sub/BUILD.bazel @@ -0,0 +1,81 @@ +load("@com_github_mvukov_rules_ros2//ros2:cc_defs.bzl", "ros2_cpp_binary") +load("@com_github_mvukov_rules_ros2//ros2:launch.bzl", "ros2_launch") +load("@com_github_mvukov_rules_ros2//ros2:plugin.bzl", "ros2_plugin") +load("@com_github_mvukov_rules_ros2//ros2:test.bzl", "ros2_test") +load("@com_github_mvukov_rules_ros2//ros2:topic.bzl", "ros2_topic") + +ros2_cpp_binary( + name = "image_publisher", + srcs = ["image_publisher.cc"], + data = [ + ":rotated_plugin", + ], + set_up_ament = True, + deps = [ + "@ros2_common_interfaces//:cpp_sensor_msgs", + "@ros2_image_common//:image_transport", + "@ros2_rclcpp//:rclcpp", + ], +) + +ros2_cpp_binary( + name = "image_resizer", + srcs = ["image_resizer.cc"], + data = [ + ":rotated_plugin", + ], + set_up_ament = True, + deps = [ + "@ros2_common_interfaces//:cpp_sensor_msgs", + "@ros2_image_common//:image_transport", + "@ros2_rclcpp//:rclcpp", + ], +) + +ros2_plugin( + name = "rotated_plugin", + srcs = [ + "rotated_publisher.cc", + "rotated_publisher.h", + "rotated_subscriber.cc", + "rotated_subscriber.h", + ], + plugin_specs = [ + { + "base_class_type": "image_transport::PublisherPlugin", + "class_name": "image_transport/rotated_pub", + "class_type": "image_pub_sub::RotatedPublisher", + }, + { + "base_class_type": "image_transport::SubscriberPlugin", + "class_name": "image_transport/rotated_sub", + "class_type": "image_pub_sub::RotatedSubscriber", + }, + ], + deps = [ + "@ros2_common_interfaces//:cpp_sensor_msgs", + "@ros2_image_common//:image_transport", + "@ros2_pluginlib//:pluginlib", + "@ros2_rclcpp//:rclcpp", + ], +) + +ros2_test( + name = "tests", + size = "small", + launch_file = "tests.py", + nodes = [ + ":image_publisher", + ":image_resizer", + ], + deps = [ + "@ros2_common_interfaces//:py_sensor_msgs", + ], +) + +ros2_topic( + name = "topic", + deps = [ + "@ros2_common_interfaces//:py_sensor_msgs", + ], +) diff --git a/examples/image_pub_sub/image_publisher.cc b/examples/image_pub_sub/image_publisher.cc new file mode 100644 index 00000000..a843b0c9 --- /dev/null +++ b/examples/image_pub_sub/image_publisher.cc @@ -0,0 +1,92 @@ +// Copyright 2026 Yuki Furuta +// +// 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 +#include + +#include "image_transport/image_transport.hpp" +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/image_encodings.hpp" +#include "sensor_msgs/msg/image.hpp" + +class ImagePublisher final : public rclcpp::Node { + public: + ImagePublisher() + : Node("image_publisher"), pub_(), test_image_(), timer_(nullptr) { + declare_parameter("width", 1280); + declare_parameter("height", 720); + declare_parameter("frequency", 5.0); + + const auto width = get_parameter("width").as_int(); + const auto height = get_parameter("height").as_int(); + const auto frequency = get_parameter("frequency").as_double(); + + pub_ = image_transport::create_publisher(this, "~/output"); + + test_image_.header.frame_id = "camera"; + test_image_.height = height; + test_image_.width = width; + test_image_.encoding = sensor_msgs::image_encodings::BGR8; + test_image_.is_bigendian = false; + test_image_.step = width * 3; + test_image_.data.resize(height * width * 3); + + const std::vector> colors = { + {128, 128, 128}, // Gray + {0, 255, 255}, // Yellow + {255, 255, 0}, // Cyan + {0, 255, 0}, // Green + {255, 0, 255}, // Magenta + {0, 0, 255}, // Red + {255, 0, 0}, // Blue + }; + + for (size_t y = 0; y < static_cast(height); ++y) { + for (size_t x = 0; x < static_cast(width); ++x) { + size_t bar_index = x / (width / colors.size()); + if (bar_index >= colors.size()) { + bar_index = colors.size() - 1; + } + const size_t pixel_index = (y * width + x) * 3; + test_image_.data[pixel_index] = colors[bar_index][0]; // B + test_image_.data[pixel_index + 1] = colors[bar_index][1]; // G + test_image_.data[pixel_index + 2] = colors[bar_index][2]; // R + } + } + + const auto timer_callback = [this]() -> void { + test_image_.header.stamp = now(); + RCLCPP_INFO(get_logger(), "Publishing image %dx%d", test_image_.width, + test_image_.height); + pub_.publish(test_image_); + }; + + const auto period_ms = static_cast(1000.0 / frequency); + timer_ = create_wall_timer(std::chrono::milliseconds(period_ms), + timer_callback); + } + + private: + image_transport::Publisher pub_; + sensor_msgs::msg::Image test_image_; + rclcpp::TimerBase::SharedPtr timer_; +}; + +int main(int argc, char* argv[]) { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/examples/image_pub_sub/image_resizer.cc b/examples/image_pub_sub/image_resizer.cc new file mode 100644 index 00000000..c3e92219 --- /dev/null +++ b/examples/image_pub_sub/image_resizer.cc @@ -0,0 +1,99 @@ +// Copyright 2026 Yuki Furuta +// +// 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 "image_transport/image_transport.hpp" +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/image_encodings.hpp" +#include "sensor_msgs/msg/image.hpp" + +class ImageResizer final : public rclcpp::Node { + public: + ImageResizer() + : Node("image_resizer"), sub_(), pub_(), scale_(1.0) { + declare_parameter("scale", 1.0); + scale_ = get_parameter("scale").as_double(); + + declare_parameter("input.image_transport", "raw"); + const auto transport_hint = get_parameter("input.image_transport").as_string(); + + sub_ = image_transport::create_subscription( + this, "~/input", + std::bind(&ImageResizer::imageCallback, this, std::placeholders::_1), + transport_hint); + pub_ = image_transport::create_publisher(this, "~/output"); + + RCLCPP_INFO(get_logger(), "Image resizer started with scale: %.2f, transport_hint: %s", + scale_, transport_hint.c_str()); + } + + private: + void imageCallback(const sensor_msgs::msg::Image::ConstSharedPtr& msg) { + RCLCPP_INFO(get_logger(), "Received image %dx%d, resizing to scale %.2f", + msg->width, msg->height, scale_); + + if (msg->encoding != sensor_msgs::image_encodings::BGR8) { + RCLCPP_ERROR(get_logger(), + "Unsupported encoding '%s'. Only '%s' is supported.", + msg->encoding.c_str(), + sensor_msgs::image_encodings::BGR8); + return; + } + + auto resized_image = sensor_msgs::msg::Image(); + resized_image.header = msg->header; + resized_image.encoding = msg->encoding; + resized_image.is_bigendian = msg->is_bigendian; + + const size_t resized_width = static_cast(msg->width * scale_); + const size_t resized_height = static_cast(msg->height * scale_); + resized_image.width = resized_width; + resized_image.height = resized_height; + resized_image.step = resized_width * 3; + resized_image.data.resize(resized_height * resized_width * 3); + + for (size_t y = 0; y < resized_height; ++y) { + for (size_t x = 0; x < resized_width; ++x) { + size_t src_x = static_cast(x / scale_); + size_t src_y = static_cast(y / scale_); + + if (src_x >= msg->width) src_x = msg->width - 1; + if (src_y >= msg->height) src_y = msg->height - 1; + + const size_t src_index = (src_y * msg->width + src_x) * 3; + const size_t dst_index = (y * resized_width + x) * 3; + + resized_image.data[dst_index] = msg->data[src_index]; + resized_image.data[dst_index + 1] = msg->data[src_index + 1]; + resized_image.data[dst_index + 2] = msg->data[src_index + 2]; + } + } + + pub_.publish(resized_image); + RCLCPP_INFO(get_logger(), "Published resized image %dx%d", + resized_image.width, resized_image.height); + } + + image_transport::Subscriber sub_; + image_transport::Publisher pub_; + double scale_; +}; + +int main(int argc, char* argv[]) { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/examples/image_pub_sub/rotated_publisher.cc b/examples/image_pub_sub/rotated_publisher.cc new file mode 100644 index 00000000..9672f943 --- /dev/null +++ b/examples/image_pub_sub/rotated_publisher.cc @@ -0,0 +1,71 @@ +// Copyright 2026 Yuki Furuta +// +// 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 "rotated_publisher.h" + +#include + +#include "rclcpp/logging.hpp" +#include "sensor_msgs/image_encodings.hpp" + +namespace image_pub_sub { + +void RotatedPublisher::publish(const sensor_msgs::msg::Image& message, + const PublishFn& publish_fn) const { + + if (message.encoding != sensor_msgs::image_encodings::BGR8) { + auto logger = rclcpp::get_logger("rotated_publisher"); + RCLCPP_ERROR(logger, + "RotatedPublisher only supports '%s' encoding, got '%s'", + sensor_msgs::image_encodings::BGR8, + message.encoding.c_str()); + return; + } + + // Create rotated image by rotating +90 degrees (clockwise). + // Rotate +90 deg clockwise: pixel at (x, y) moves to (height - 1 - y, x). + sensor_msgs::msg::Image rotated_image = message; + + const size_t src_width = message.width; + const size_t src_height = message.height; + const size_t bytes_per_pixel = 3; + + rotated_image.width = src_height; + rotated_image.height = src_width; + rotated_image.step = rotated_image.width * bytes_per_pixel; + + std::vector rotated_data(rotated_image.height * rotated_image.step); + + for (size_t src_y = 0; src_y < src_height; ++src_y) { + for (size_t src_x = 0; src_x < src_width; ++src_x) { + const size_t dst_x = src_height - 1 - src_y; + const size_t dst_y = src_x; + + const size_t src_offset = (src_y * message.step) + (src_x * bytes_per_pixel); + const size_t dst_offset = (dst_y * rotated_image.step) + (dst_x * bytes_per_pixel); + + rotated_data[dst_offset + 0] = message.data[src_offset + 0]; + rotated_data[dst_offset + 1] = message.data[src_offset + 1]; + rotated_data[dst_offset + 2] = message.data[src_offset + 2]; + } + } + + rotated_image.data = rotated_data; + publish_fn(rotated_image); +} + +} // namespace image_pub_sub + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(image_pub_sub::RotatedPublisher, image_transport::PublisherPlugin) diff --git a/examples/image_pub_sub/rotated_publisher.h b/examples/image_pub_sub/rotated_publisher.h new file mode 100644 index 00000000..26861005 --- /dev/null +++ b/examples/image_pub_sub/rotated_publisher.h @@ -0,0 +1,37 @@ +// Copyright 2026 Yuki Furuta +// +// 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 ROTATED_PUBLISHER_H +#define ROTATED_PUBLISHER_H + +#include + +#include "image_transport/simple_publisher_plugin.hpp" +#include "sensor_msgs/msg/image.hpp" + +namespace image_pub_sub { + +class RotatedPublisher final + : public image_transport::SimplePublisherPlugin { + public: + virtual std::string getTransportName() const final { return "rotated"; } + + protected: + virtual void publish(const sensor_msgs::msg::Image& message, + const PublishFn& publish_fn) const final; +}; + +} // namespace image_pub_sub + +#endif // ROTATED_PUBLISHER_H diff --git a/examples/image_pub_sub/rotated_subscriber.cc b/examples/image_pub_sub/rotated_subscriber.cc new file mode 100644 index 00000000..d14e20ed --- /dev/null +++ b/examples/image_pub_sub/rotated_subscriber.cc @@ -0,0 +1,74 @@ +// Copyright 2026 Yuki Furuta +// +// 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 "rotated_subscriber.h" + +#include +#include + +#include "sensor_msgs/image_encodings.hpp" + +namespace image_pub_sub { + +void RotatedSubscriber::internalCallback( + const sensor_msgs::msg::Image::ConstSharedPtr& message, + const Callback& user_cb) { + + if (message->encoding != sensor_msgs::image_encodings::BGR8) { + auto logger = rclcpp::get_logger("rotated_subscriber"); + RCLCPP_ERROR(logger, + "RotatedSubscriber only supports '%s' encoding, got '%s'", + sensor_msgs::image_encodings::BGR8, + message->encoding.c_str()); + return; + } + + // The rotated image was already processed by the publisher (+90 deg). + // For the subscriber, we need to rotate it -90 deg to restore the original: + // pixel at (x, y) moves to (y, width - 1 - x). + auto restored_image = std::make_shared(*message); + + const size_t src_width = message->width; + const size_t src_height = message->height; + const size_t bytes_per_pixel = 3; + + restored_image->width = src_height; + restored_image->height = src_width; + restored_image->step = restored_image->width * bytes_per_pixel; + + std::vector restored_data(restored_image->height * restored_image->step); + + for (size_t src_y = 0; src_y < src_height; ++src_y) { + for (size_t src_x = 0; src_x < src_width; ++src_x) { + const size_t dst_x = src_y; + const size_t dst_y = src_width - 1 - src_x; + + const size_t src_offset = (src_y * message->step) + (src_x * bytes_per_pixel); + const size_t dst_offset = (dst_y * restored_image->step) + (dst_x * bytes_per_pixel); + + restored_data[dst_offset + 0] = message->data[src_offset + 0]; + restored_data[dst_offset + 1] = message->data[src_offset + 1]; + restored_data[dst_offset + 2] = message->data[src_offset + 2]; + } + } + + restored_image->data = restored_data; + + user_cb(restored_image); +} + +} // namespace image_pub_sub + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(image_pub_sub::RotatedSubscriber, image_transport::SubscriberPlugin) diff --git a/examples/image_pub_sub/rotated_subscriber.h b/examples/image_pub_sub/rotated_subscriber.h new file mode 100644 index 00000000..aca08e61 --- /dev/null +++ b/examples/image_pub_sub/rotated_subscriber.h @@ -0,0 +1,51 @@ +// Copyright 2026 Yuki Furuta +// +// 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 ROTATED_SUBSCRIBER_H +#define ROTATED_SUBSCRIBER_H + +#include + +#include "image_transport/simple_subscriber_plugin.hpp" +#include "rclcpp/subscription_options.hpp" +#include "sensor_msgs/msg/image.hpp" + +namespace image_pub_sub { + +class RotatedSubscriber final + : public image_transport::SimpleSubscriberPlugin { + public: + virtual ~RotatedSubscriber() {} + + virtual std::string getTransportName() const final { return "rotated"; } + + protected: + virtual void internalCallback( + const sensor_msgs::msg::Image::ConstSharedPtr& message, + const Callback& user_cb) final; + + // Override 5-argument subscribeImpl to use SimpleSubscriberPlugin's implementation + virtual void subscribeImpl( + rclcpp::Node* node, + const std::string& base_topic, + const Callback& callback, + rmw_qos_profile_t custom_qos, + rclcpp::SubscriptionOptions options) override final { + subscribeImplWithOptions(node, base_topic, callback, custom_qos, options); + } +}; + +} // namespace image_pub_sub + +#endif // ROTATED_SUBSCRIBER_H diff --git a/examples/image_pub_sub/tests.py b/examples/image_pub_sub/tests.py new file mode 100755 index 00000000..0625dbb4 --- /dev/null +++ b/examples/image_pub_sub/tests.py @@ -0,0 +1,163 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +# +# Copyright 2026 Yuki Furuta +# +# 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. + +import time +import unittest + +import launch +import launch_ros.actions +import launch_testing.actions +import launch_testing.markers +import rclpy +import sensor_msgs.msg + + +@launch_testing.markers.keep_alive +def generate_test_description(): + publisher_node = launch_ros.actions.Node( + name='image_publisher', + executable='image_pub_sub/image_publisher', + parameters=[ + { + 'width': 640, + 'height': 480, + 'frequency': 10.0, + }, + ], + ) + + resizer_node = launch_ros.actions.Node( + name='image_resizer', + executable='image_pub_sub/image_resizer', + parameters=[ + { + 'scale': 0.5, + 'input.image_transport': 'rotated', + }, + ], + remappings=[ + ('~/input/rotated', 'image_publisher/output/rotated'), + ], + ) + + return ( + launch.LaunchDescription([ + publisher_node, + resizer_node, + launch_testing.actions.ReadyToTest(), + ]), + { + 'image_publisher': publisher_node, + 'image_resizer': resizer_node, + }) + + +class TestImagePubSub(unittest.TestCase): + """Tests image_transport functionality with rotated plugin. + + This test verifies that: + 1. The image_publisher node publishes images using rotated transport + 2. The image_resizer node receives images via rotated transport and publishes resized versions + 3. The rotated image transport plugin correctly rotates and restores images + 4. Image transport works correctly with custom transport plugins + """ + + @classmethod + def setUpClass(cls): + rclpy.init() + + @classmethod + def tearDownClass(cls): + rclpy.shutdown() + + def setUp(self): + self.node = rclpy.create_node('image_pub_sub_test') + + def tearDown(self): + self.node.destroy_node() + + def test_topic_advertised(self, launch_service, image_publisher, image_resizer, proc_output): + end_time = time.time() + 10.0 + topic_info = [] + while time.time() < end_time: + rclpy.spin_once(self.node, timeout_sec=1.0) + topic_info = self.node.get_topic_names_and_types() + if topic_info: + break + advertised_topic_names = {name for name, type in topic_info} + required_topic_names = { + '/image_publisher/output', + '/image_publisher/output/rotated', + '/image_resizer/output', + '/image_resizer/output/rotated', + } + self.assertTrue(required_topic_names.issubset(advertised_topic_names), + f'Required topics not advertised. Missing: {required_topic_names - advertised_topic_names}') + + + def test_publisher_transmits(self, launch_service, image_publisher, proc_output): + """Test that the image_publisher publishes images on the expected topic.""" + msgs_rx = [] + + sub = self.node.create_subscription( + sensor_msgs.msg.Image, + 'image_publisher/output/rotated', + lambda msg: msgs_rx.append(msg), + 10) + + try: + end_time = time.time() + 10 + while time.time() < end_time: + rclpy.spin_once(self.node, timeout_sec=0.1) + if len(msgs_rx) > 2: + break + + self.assertGreater(len(msgs_rx), 2, + 'image_publisher should have transmitted multiple images') + # Verify image properties (width and height are swapped due to +90 deg rotation). + self.assertEqual(msgs_rx[0].width, 480) + self.assertEqual(msgs_rx[0].height, 640) + self.assertEqual(msgs_rx[0].encoding, 'bgr8') + finally: + self.node.destroy_subscription(sub) + + def test_resizer_transmits(self, launch_service, image_resizer, proc_output): + """Test that the image_resizer receives and publishes resized images.""" + msgs_rx = [] + + sub = self.node.create_subscription( + sensor_msgs.msg.Image, + 'image_resizer/output', + lambda msg: msgs_rx.append(msg), + 10) + + try: + end_time = time.time() + 10 + while time.time() < end_time: + rclpy.spin_once(self.node, timeout_sec=0.1) + if len(msgs_rx) > 2: + break + + self.assertGreater(len(msgs_rx), 2, + 'image_resizer should have transmitted multiple images') + # Verify resized image properties (should be 50% of original 640x480). + # After restoration from rotated transport, dimensions are back to original ratio. + self.assertEqual(msgs_rx[0].width, 320) + self.assertEqual(msgs_rx[0].height, 240) + self.assertEqual(msgs_rx[0].encoding, 'bgr8') + finally: + self.node.destroy_subscription(sub)