diff --git a/image_transport_plugins/package.xml b/image_transport_plugins/package.xml index c4d20e8..ff59e52 100644 --- a/image_transport_plugins/package.xml +++ b/image_transport_plugins/package.xml @@ -26,6 +26,7 @@ compressed_depth_image_transport compressed_image_transport theora_image_transport + zlib_image_transport zstd_image_transport diff --git a/zlib_image_transport/CMakeLists.txt b/zlib_image_transport/CMakeLists.txt new file mode 100644 index 0000000..b6859d8 --- /dev/null +++ b/zlib_image_transport/CMakeLists.txt @@ -0,0 +1,57 @@ +cmake_minimum_required(VERSION 3.20) + +project(zlib_image_transport) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(image_transport REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(ZLIB REQUIRED) + +include_directories(include) + +add_library( + ${PROJECT_NAME} SHARED + src/zlib_wrapper.cpp + src/zlib_publisher.cpp + src/zlib_subscriber.cpp + src/manifest.cpp +) + +target_link_libraries(${PROJECT_NAME} + ZLIB::ZLIB + image_transport::image_transport + rclcpp::rclcpp + pluginlib::pluginlib + ${sensor_msgs_TARGETS} +) + +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install( + DIRECTORY "include/" + DESTINATION include +) + +pluginlib_export_plugin_description_file(image_transport zlib_plugins.xml) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/zlib_image_transport/include/zlib_image_transport/zlib_common.hpp b/zlib_image_transport/include/zlib_image_transport/zlib_common.hpp new file mode 100644 index 0000000..0204073 --- /dev/null +++ b/zlib_image_transport/include/zlib_image_transport/zlib_common.hpp @@ -0,0 +1,48 @@ +// Copyright (c) 2026, Open Source Robotics Foundation, Inc. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#ifndef ZLIB_IMAGE_TRANSPORT__ZLIB_COMMON_HPP_ +#define ZLIB_IMAGE_TRANSPORT__ZLIB_COMMON_HPP_ + +#include +#include + +namespace zlib_image_transport +{ +using ParameterDescriptor = rcl_interfaces::msg::ParameterDescriptor; +using ParameterValue = rclcpp::ParameterValue; + +struct ParameterDefinition +{ + const ParameterValue defaultValue; + const ParameterDescriptor descriptor; +}; +} // namespace zlib_image_transport + +#endif // ZLIB_IMAGE_TRANSPORT__ZLIB_COMMON_HPP_ diff --git a/zlib_image_transport/include/zlib_image_transport/zlib_publisher.hpp b/zlib_image_transport/include/zlib_image_transport/zlib_publisher.hpp new file mode 100644 index 0000000..f961573 --- /dev/null +++ b/zlib_image_transport/include/zlib_image_transport/zlib_publisher.hpp @@ -0,0 +1,91 @@ +// Copyright (c) 2026, Open Source Robotics Foundation, Inc. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#ifndef ZLIB_IMAGE_TRANSPORT__ZLIB_PUBLISHER_HPP_ +#define ZLIB_IMAGE_TRANSPORT__ZLIB_PUBLISHER_HPP_ + +#include +#include +#include + +#include +#include +#include +#include + +#include + +#include "zlib_image_transport/zlib_common.hpp" + +namespace zlib_image_transport +{ + +using CompressedImage = sensor_msgs::msg::CompressedImage; +using ParameterEvent = rcl_interfaces::msg::ParameterEvent; + +class ZlibPublisher : public image_transport::SimplePublisherPlugin +{ +public: + ZlibPublisher(); + ~ZlibPublisher() override = default; + + std::string getTransportName() const override; + +protected: + void advertiseImpl( + image_transport::RequiredInterfaces node_interfaces, + const std::string & base_topic, + rclcpp::QoS custom_qos, + rclcpp::PublisherOptions options) final; + + void publish( + const sensor_msgs::msg::Image & message, + const PublisherT & publisher) const override; + + rclcpp::Logger logger_; + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_param_interface_; + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface_; + +private: + std::vector parameters_; + std::unordered_set deprecated_parameters_; + + rclcpp::node_interfaces::PreSetParametersCallbackHandle::SharedPtr + pre_set_parameter_callback_handle_; + + void declareParameter( + const std::string & base_name, + const ParameterDefinition & definition); + + void preSetParametersCallback(std::vector & parameters); +}; + +} // namespace zlib_image_transport + +#endif // ZLIB_IMAGE_TRANSPORT__ZLIB_PUBLISHER_HPP_ diff --git a/zlib_image_transport/include/zlib_image_transport/zlib_subscriber.hpp b/zlib_image_transport/include/zlib_image_transport/zlib_subscriber.hpp new file mode 100644 index 0000000..d9cb4f7 --- /dev/null +++ b/zlib_image_transport/include/zlib_image_transport/zlib_subscriber.hpp @@ -0,0 +1,77 @@ +// Copyright (c) 2026, Open Source Robotics Foundation, Inc. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#ifndef ZLIB_IMAGE_TRANSPORT__ZLIB_SUBSCRIBER_HPP_ +#define ZLIB_IMAGE_TRANSPORT__ZLIB_SUBSCRIBER_HPP_ + +#include +#include + +#include +#include + +#include +#include +#include +#include + +#include "zlib_image_transport/zlib_common.hpp" + +namespace zlib_image_transport +{ + +using ParameterEvent = rcl_interfaces::msg::ParameterEvent; + +class ZlibSubscriber final + : public image_transport::SimpleSubscriberPlugin +{ +public: + ZlibSubscriber(); + virtual ~ZlibSubscriber() = default; + + std::string getTransportName() const override; + +protected: + void subscribeImpl( + image_transport::RequiredInterfaces node_interfaces, + const std::string & base_topic, + const Callback & callback, + rclcpp::QoS custom_qos, + rclcpp::SubscriptionOptions options) override; + + void internalCallback( + const sensor_msgs::msg::CompressedImage::ConstSharedPtr & message, + const Callback & user_cb) override; + + rclcpp::Logger logger_; +}; + +} // namespace zlib_image_transport + +#endif // ZLIB_IMAGE_TRANSPORT__ZLIB_SUBSCRIBER_HPP_ diff --git a/zlib_image_transport/package.xml b/zlib_image_transport/package.xml new file mode 100644 index 0000000..b70287a --- /dev/null +++ b/zlib_image_transport/package.xml @@ -0,0 +1,29 @@ + + zlib_image_transport + 6.2.2 + + zlib_image_transport provides a plugin to image_transport for transparently sending images + encoded as zlib (deflate) blobs. + + Alejandro Hernandez Cordero + BSD + + https://docs.ros.org/en/rolling/p/image_transport/ + Alejandro Hernandez Cordero + + ament_cmake + + image_transport + pluginlib + rclcpp + sensor_msgs + zlib + + ament_lint_auto + ament_lint_common + + + ament_cmake + + + diff --git a/zlib_image_transport/src/manifest.cpp b/zlib_image_transport/src/manifest.cpp new file mode 100644 index 0000000..2bd1417 --- /dev/null +++ b/zlib_image_transport/src/manifest.cpp @@ -0,0 +1,35 @@ +// Copyright (c) 2026, Open Source Robotics Foundation, Inc. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include +#include "zlib_image_transport/zlib_publisher.hpp" +#include "zlib_image_transport/zlib_subscriber.hpp" + +PLUGINLIB_EXPORT_CLASS(zlib_image_transport::ZlibPublisher, image_transport::PublisherPlugin) +PLUGINLIB_EXPORT_CLASS(zlib_image_transport::ZlibSubscriber, image_transport::SubscriberPlugin) diff --git a/zlib_image_transport/src/zlib_publisher.cpp b/zlib_image_transport/src/zlib_publisher.cpp new file mode 100644 index 0000000..2c113d5 --- /dev/null +++ b/zlib_image_transport/src/zlib_publisher.cpp @@ -0,0 +1,227 @@ +// Copyright (c) 2026, Open Source Robotics Foundation, Inc. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include "zlib_image_transport/zlib_publisher.hpp" + +#include +#include +#include + +#include + +#include "zlib_wrapper.hpp" + +namespace zlib_image_transport +{ + +enum zlibParameters +{ + ZLIB_LEVEL = 0, +}; + +const struct ParameterDefinition kParameters[] = +{ + { + // ZLIB_LEVEL - zlib Compression Level from 0 to 9. Higher value = smaller size. + ParameterValue(static_cast(6)), + ParameterDescriptor() + .set__name("zlib_level") + .set__type(rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER) + .set__description("Compression level for zlib format (0=none, 1=fastest, 9=best compression)") + .set__read_only(false) + .set__integer_range( + {rcl_interfaces::msg::IntegerRange() + .set__from_value(0) + .set__to_value(9) + .set__step(1)}) + }, +}; + +ZlibPublisher::ZlibPublisher() +: logger_(rclcpp::get_logger("ZlibPublisher")) +{ +} + +std::string ZlibPublisher::getTransportName() const +{ + return "zlib"; +} + +void ZlibPublisher::advertiseImpl( + image_transport::RequiredInterfaces node_interfaces, + const std::string & base_topic, + rclcpp::QoS custom_qos, + rclcpp::PublisherOptions options) +{ + node_param_interface_ = node_interfaces.get_node_parameters_interface(); + node_base_interface_ = node_interfaces.get_node_base_interface(); + typedef image_transport::SimplePublisherPlugin Base; + Base::advertiseImpl(node_interfaces, base_topic, custom_qos, options); + + unsigned int ns_len = + std::string(node_interfaces.get_node_base_interface()->get_namespace()).length(); + std::string param_base_name = base_topic.substr(ns_len); + std::replace(param_base_name.begin(), param_base_name.end(), '/', '.'); + + if (ns_len > 1) { + pre_set_parameter_callback_handle_ = + node_param_interface_->add_pre_set_parameters_callback(std::bind( + &ZlibPublisher::preSetParametersCallback, + this, std::placeholders::_1)); + } + + for (const ParameterDefinition & pd : kParameters) { + declareParameter(param_base_name, pd); + } +} + +void ZlibPublisher::publish( + const sensor_msgs::msg::Image & message, + const PublisherT & publisher) const +{ + const int cfg_level = + node_param_interface_->get_parameter(parameters_[ZLIB_LEVEL]).as_int(); + + // Pre-allocate output buffer to the zlib worst-case bound. + const std::size_t bound = zlib_wrapper::compressBound(message.data.size()); + const std::size_t metadata = + 4 + // height + 4 + // width + 1 + // is_bigendian + 4 + // step + 4 + // encoding string length + message.encoding.size(); + + sensor_msgs::msg::CompressedImage compressed; + compressed.data.resize(metadata + bound); + + const std::size_t compressed_size = zlib_wrapper::compress( + &compressed.data[metadata], bound, + message.data.data(), message.data.size(), + cfg_level); + + if (compressed_size == 0) { + RCLCPP_ERROR(logger_, "zlib compression failed"); + return; + } + + compressed.data.resize(metadata + compressed_size); + + // ---- Metadata header (little-endian) ---- + compressed.data[0] = static_cast(message.height & 0xFF); + compressed.data[1] = static_cast((message.height >> 8) & 0xFF); + compressed.data[2] = static_cast((message.height >> 16) & 0xFF); + compressed.data[3] = static_cast((message.height >> 24) & 0xFF); + + compressed.data[4] = static_cast(message.width & 0xFF); + compressed.data[5] = static_cast((message.width >> 8) & 0xFF); + compressed.data[6] = static_cast((message.width >> 16) & 0xFF); + compressed.data[7] = static_cast((message.width >> 24) & 0xFF); + + compressed.data[8] = message.is_bigendian; + + compressed.data[9] = static_cast(message.step & 0xFF); + compressed.data[10] = static_cast((message.step >> 8) & 0xFF); + compressed.data[11] = static_cast((message.step >> 16) & 0xFF); + compressed.data[12] = static_cast((message.step >> 24) & 0xFF); + + compressed.data[13] = static_cast(message.encoding.size() & 0xFF); + compressed.data[14] = static_cast((message.encoding.size() >> 8) & 0xFF); + compressed.data[15] = static_cast((message.encoding.size() >> 16) & 0xFF); + compressed.data[16] = static_cast((message.encoding.size() >> 24) & 0xFF); + + memcpy(&compressed.data[17], message.encoding.data(), message.encoding.size()); + // ----------------------------------------- + + compressed.header = message.header; + compressed.format = "zlib"; + publisher->publish(compressed); +} + +void ZlibPublisher::declareParameter( + const std::string & base_name, + const ParameterDefinition & definition) +{ + const std::string transport_name = getTransportName(); + const std::string param_name = base_name + "." + transport_name + "." + + definition.descriptor.name; + parameters_.push_back(param_name); + + rclcpp::ParameterValue param_value; + + try { + param_value = node_param_interface_->declare_parameter( + param_name, definition.defaultValue, + definition.descriptor); + } catch (const rclcpp::exceptions::ParameterAlreadyDeclaredException &) { + RCLCPP_DEBUG(logger_, "%s was previously declared", definition.descriptor.name.c_str()); + param_value = node_param_interface_->get_parameter(param_name).get_parameter_value(); + } + + if (std::string(node_base_interface_->get_namespace()).length() > 1) { + const std::string deprecated_dot_name = "." + base_name + "." + transport_name + "." + + definition.descriptor.name; + deprecated_parameters_.insert(deprecated_dot_name); + + try { + node_param_interface_->declare_parameter( + deprecated_dot_name, param_value, definition.descriptor); + } catch (const rclcpp::exceptions::ParameterAlreadyDeclaredException &) { + RCLCPP_DEBUG(logger_, "%s was previously declared", definition.descriptor.name.c_str()); + } + } +} + +void ZlibPublisher::preSetParametersCallback(std::vector & parameters) +{ + std::vector new_parameters; + + for (auto & param : parameters) { + const auto & param_name = param.get_name(); + + if (deprecated_parameters_.find(param_name) != deprecated_parameters_.end()) { + auto non_dot_prefixed_name = param_name.substr(1); + RCLCPP_WARN_STREAM( + logger_, + "parameter `" << param_name << + "` with leading dot character is deprecated; use: `" << + non_dot_prefixed_name << "` instead"); + new_parameters.push_back( + rclcpp::Parameter(non_dot_prefixed_name, param.get_parameter_value())); + } + + if (std::find(parameters_.begin(), parameters_.end(), param_name) != parameters_.end()) { + new_parameters.emplace_back("." + param_name, param.get_parameter_value()); + } + } + + parameters.insert(parameters.end(), new_parameters.begin(), new_parameters.end()); +} + +} // namespace zlib_image_transport diff --git a/zlib_image_transport/src/zlib_subscriber.cpp b/zlib_image_transport/src/zlib_subscriber.cpp new file mode 100644 index 0000000..c9feb25 --- /dev/null +++ b/zlib_image_transport/src/zlib_subscriber.cpp @@ -0,0 +1,148 @@ +// Copyright (c) 2026, Open Source Robotics Foundation, Inc. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include "zlib_image_transport/zlib_subscriber.hpp" + +#include + +#include + +#include + +#include "zlib_wrapper.hpp" + +namespace zlib_image_transport +{ + +ZlibSubscriber::ZlibSubscriber() +: logger_(rclcpp::get_logger("ZlibSubscriber")) +{ +} + +std::string ZlibSubscriber::getTransportName() const +{ + return "zlib"; +} + +void ZlibSubscriber::subscribeImpl( + image_transport::RequiredInterfaces node_interfaces, + const std::string & base_topic, + const Callback & callback, + rclcpp::QoS custom_qos, + rclcpp::SubscriptionOptions options) +{ + logger_ = node_interfaces.get_node_logging_interface()->get_logger(); + typedef image_transport::SimpleSubscriberPlugin Base; + Base::subscribeImpl(node_interfaces, base_topic, callback, custom_qos, options); +} + +void ZlibSubscriber::internalCallback( + const sensor_msgs::msg::CompressedImage::ConstSharedPtr & msg, + const Callback & user_cb) +{ + constexpr std::size_t kFixedHeaderSize = 4 + 4 + 1 + 4 + 4; // = 17 bytes + + if (msg->data.size() < kFixedHeaderSize) { + RCLCPP_ERROR(logger_, "Compressed image too small to contain header"); + return; + } + + auto result = std::make_shared(); + + // ---- Decode fixed metadata header (little-endian) ---- + result->height = + (static_cast(msg->data[3]) << 24) | + (static_cast(msg->data[2]) << 16) | + (static_cast(msg->data[1]) << 8) | + static_cast(msg->data[0]); + + result->width = + (static_cast(msg->data[7]) << 24) | + (static_cast(msg->data[6]) << 16) | + (static_cast(msg->data[5]) << 8) | + static_cast(msg->data[4]); + + result->is_bigendian = msg->data[8]; + + result->step = + (static_cast(msg->data[12]) << 24) | + (static_cast(msg->data[11]) << 16) | + (static_cast(msg->data[10]) << 8) | + static_cast(msg->data[9]); + + const uint32_t encoding_size = + (static_cast(msg->data[16]) << 24) | + (static_cast(msg->data[15]) << 16) | + (static_cast(msg->data[14]) << 8) | + static_cast(msg->data[13]); + // ------------------------------------------------------ + + const std::size_t metadata = kFixedHeaderSize + encoding_size; + if (msg->data.size() < metadata) { + RCLCPP_ERROR(logger_, "Compressed image data truncated (encoding string missing)"); + return; + } + + result->encoding.resize(encoding_size); + memcpy(&result->encoding[0], &msg->data[17], encoding_size); + + const uint8_t * compressed_ptr = &msg->data[metadata]; + const std::size_t compressed_size = msg->data.size() - metadata; + + if (compressed_size == 0) { + RCLCPP_ERROR(logger_, "Compressed image payload is empty"); + return; + } + + // The uncompressed size is step * height (bytes per row × number of rows). + const std::size_t uncompressed_size = + static_cast(result->step) * static_cast(result->height); + + if (uncompressed_size == 0) { + RCLCPP_ERROR(logger_, "Decoded step or height is zero; cannot decompress"); + return; + } + + result->data.resize(uncompressed_size); + const std::size_t actual_size = zlib_wrapper::decompress( + result->data.data(), uncompressed_size, + compressed_ptr, compressed_size); + + if (actual_size == 0) { + RCLCPP_ERROR(logger_, "zlib decompression failed"); + return; + } + + result->data.resize(actual_size); + result->header = msg->header; + + user_cb(result); +} + +} // namespace zlib_image_transport diff --git a/zlib_image_transport/src/zlib_wrapper.cpp b/zlib_image_transport/src/zlib_wrapper.cpp new file mode 100644 index 0000000..8e795e5 --- /dev/null +++ b/zlib_image_transport/src/zlib_wrapper.cpp @@ -0,0 +1,70 @@ +// Copyright (c) 2026, Open Source Robotics Foundation, Inc. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include "zlib_wrapper.hpp" + +namespace zlib_wrapper +{ + +std::size_t compressBound(std::size_t src_size) +{ + return ::compressBound(static_cast(src_size)); +} + +std::size_t compress( + uint8_t * dst, std::size_t dst_capacity, + const uint8_t * src, std::size_t src_size, + int level) +{ + uLongf out_len = static_cast(dst_capacity); + int ret = ::compress2( + reinterpret_cast(dst), &out_len, + reinterpret_cast(src), static_cast(src_size), + level); + if (ret != Z_OK) { + return 0; + } + return static_cast(out_len); +} + +std::size_t decompress( + uint8_t * dst, std::size_t dst_capacity, + const uint8_t * src, std::size_t src_size) +{ + uLongf out_len = static_cast(dst_capacity); + int ret = ::uncompress( + reinterpret_cast(dst), &out_len, + reinterpret_cast(src), static_cast(src_size)); + if (ret != Z_OK) { + return 0; + } + return static_cast(out_len); +} + +} // namespace zlib_wrapper diff --git a/zlib_image_transport/src/zlib_wrapper.hpp b/zlib_image_transport/src/zlib_wrapper.hpp new file mode 100644 index 0000000..22448fd --- /dev/null +++ b/zlib_image_transport/src/zlib_wrapper.hpp @@ -0,0 +1,61 @@ +// Copyright (c) 2026, Open Source Robotics Foundation, Inc. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#ifndef ZLIB_WRAPPER_HPP_ +#define ZLIB_WRAPPER_HPP_ + +#include + +#include +#include + +namespace zlib_wrapper +{ + +/// Returns the maximum compressed output size for src_size bytes of input. +std::size_t compressBound(std::size_t src_size); + +/// Compress src into dst using zlib deflate at the given level (0–9). +/// dst must have at least compressBound(src_size) bytes. +/// Returns the actual compressed size, or 0 on error. +std::size_t compress( + uint8_t * dst, std::size_t dst_capacity, + const uint8_t * src, std::size_t src_size, + int level); + +/// Decompress src into dst. +/// dst_capacity must be at least the original uncompressed size. +/// Returns the actual decompressed size, or 0 on error. +std::size_t decompress( + uint8_t * dst, std::size_t dst_capacity, + const uint8_t * src, std::size_t src_size); + +} // namespace zlib_wrapper + +#endif // ZLIB_WRAPPER_HPP_ diff --git a/zlib_image_transport/zlib_plugins.xml b/zlib_image_transport/zlib_plugins.xml new file mode 100644 index 0000000..db6922d --- /dev/null +++ b/zlib_image_transport/zlib_plugins.xml @@ -0,0 +1,19 @@ + + + + This plugin publishes an image compressed with zlib (deflate). + + + + + + This plugin decompresses a zlib-compressed image. + + +