diff --git a/avif_image_transport/CMakeLists.txt b/avif_image_transport/CMakeLists.txt new file mode 100644 index 0000000..0a625ab --- /dev/null +++ b/avif_image_transport/CMakeLists.txt @@ -0,0 +1,55 @@ +cmake_minimum_required(VERSION 3.5) + +project(avif_image_transport) + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +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) + +include_directories(include) + +add_library( + ${PROJECT_NAME} SHARED + src/avif_publisher.cpp + src/avif_subscriber.cpp + src/manifest.cpp +) + +target_link_libraries(${PROJECT_NAME} avif) + +ament_target_dependencies(${PROJECT_NAME} + "image_transport" + "rclcpp" + "pluginlib" + "sensor_msgs" +) + +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 avif_plugins.xml) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/avif_image_transport/avif_plugins.xml b/avif_image_transport/avif_plugins.xml new file mode 100644 index 0000000..6b33653 --- /dev/null +++ b/avif_image_transport/avif_plugins.xml @@ -0,0 +1,19 @@ + + + + This plugin publishes a AVIF image + + + + + + This plugin decompresses a AVIF Image. + + + diff --git a/avif_image_transport/include/avif_image_transport/avif_common.hpp b/avif_image_transport/include/avif_image_transport/avif_common.hpp new file mode 100644 index 0000000..d4c4ae6 --- /dev/null +++ b/avif_image_transport/include/avif_image_transport/avif_common.hpp @@ -0,0 +1,51 @@ +// Copyright (c) 2023, Open Source Robotics Foundation, Inc. +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// 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 OWNER 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 AVIF_IMAGE_TRANSPORT__AVIF_COMMON_HPP_ +#define AVIF_IMAGE_TRANSPORT__AVIF_COMMON_HPP_ + +#include +#include + +namespace avif_image_transport +{ +using ParameterDescriptor = rcl_interfaces::msg::ParameterDescriptor; +using ParameterValue = rclcpp::ParameterValue; + +struct ParameterDefinition +{ + const ParameterValue defaultValue; + const ParameterDescriptor descriptor; +}; +} // namespace avif_image_transport + +#endif // AVIF_IMAGE_TRANSPORT__AVIF_COMMON_HPP_ diff --git a/avif_image_transport/include/avif_image_transport/avif_publisher.hpp b/avif_image_transport/include/avif_image_transport/avif_publisher.hpp new file mode 100644 index 0000000..9b16684 --- /dev/null +++ b/avif_image_transport/include/avif_image_transport/avif_publisher.hpp @@ -0,0 +1,101 @@ +// Copyright (c) 2023, Open Source Robotics Foundation, Inc. +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// 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 OWNER 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 AVIF_IMAGE_TRANSPORT__AVIF_PUBLISHER_HPP_ +#define AVIF_IMAGE_TRANSPORT__AVIF_PUBLISHER_HPP_ + +#include + +#include +#include +#include + +#include +#include +#include + +#include + +#include "avif_image_transport/avif_common.hpp" + +namespace avif_image_transport +{ +using CompressedImage = sensor_msgs::msg::CompressedImage; +using ParameterEvent = rcl_interfaces::msg::ParameterEvent; + +struct AvifImageDeleter +{ + void operator()(avifImage * image) {avifImageDestroy(image);} +}; + +using AvifImageUniquePtr = std::unique_ptr; + +class AVIFPublisher : public image_transport::SimplePublisherPlugin +{ +public: + AVIFPublisher(); + ~AVIFPublisher() override; + std::string getTransportName() const override; + +protected: + // Overridden to set up reconfigure server + void advertiseImpl( + rclcpp::Node * node, + const std::string & base_topic, + rmw_qos_profile_t custom_qos, + rclcpp::PublisherOptions options) override; + + void publish( + const sensor_msgs::msg::Image & message, + const PublishFn & publish_fn) const override; + + rclcpp::Logger logger_; + rclcpp::Node * node_; + +private: + AvifImageUniquePtr ConvertToAvif( + const sensor_msgs::msg::Image & message, + bool lossless, + int bit_depth) const; + + std::vector parameters_; + + avifEncoder * encoder_ = NULL; + + void declareParameter( + const std::string & base_name, + const ParameterDefinition & definition); +}; + +} // namespace avif_image_transport + +#endif // AVIF_IMAGE_TRANSPORT__AVIF_PUBLISHER_HPP_ diff --git a/avif_image_transport/include/avif_image_transport/avif_subscriber.hpp b/avif_image_transport/include/avif_image_transport/avif_subscriber.hpp new file mode 100644 index 0000000..330c076 --- /dev/null +++ b/avif_image_transport/include/avif_image_transport/avif_subscriber.hpp @@ -0,0 +1,90 @@ +// Copyright (c) 2023, Open Source Robotics Foundation, Inc. +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// 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 OWNER 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 AVIF_IMAGE_TRANSPORT__AVIF_SUBSCRIBER_HPP_ +#define AVIF_IMAGE_TRANSPORT__AVIF_SUBSCRIBER_HPP_ + +#include + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +#include "avif_image_transport/avif_common.hpp" + + +namespace avif_image_transport +{ +using CompressedImage = sensor_msgs::msg::CompressedImage; +using ParameterEvent = rcl_interfaces::msg::ParameterEvent; + +class AVIFSubscriber final + : public image_transport::SimpleSubscriberPlugin +{ +public: + AVIFSubscriber(); + virtual ~AVIFSubscriber() = default; + + std::string getTransportName() const override; + +protected: + void subscribeImpl( + rclcpp::Node *, + const std::string & base_topic, + const Callback & callback, + rmw_qos_profile_t custom_qos, + rclcpp::SubscriptionOptions options) override; + + void internalCallback( + const sensor_msgs::msg::CompressedImage::ConstSharedPtr & message, + const Callback & user_cb) override; + + rclcpp::Logger logger_; + rclcpp::Node * node_; + +private: + avifDecoder * decoder_; + std::mutex mutex; +}; + +} // namespace avif_image_transport + +#endif // AVIF_IMAGE_TRANSPORT__AVIF_SUBSCRIBER_HPP_ diff --git a/avif_image_transport/package.xml b/avif_image_transport/package.xml new file mode 100644 index 0000000..8c269d3 --- /dev/null +++ b/avif_image_transport/package.xml @@ -0,0 +1,25 @@ + + avif_image_transport + 3.2.0 + + avif_image_transport provides a plugin to image_transport for transparently sending images + encoded as avif + + Alejandro Hernandez Cordero + BSD + + http://www.ros.org/wiki/image_transport_plugins + Alejandro Hernandez Cordero + + ament_cmake + + image_transport + + ament_lint_auto + ament_lint_common + + + ament_cmake + + + diff --git a/avif_image_transport/src/avif_publisher.cpp b/avif_image_transport/src/avif_publisher.cpp new file mode 100644 index 0000000..6797f58 --- /dev/null +++ b/avif_image_transport/src/avif_publisher.cpp @@ -0,0 +1,250 @@ +// Copyright (c) 2023, Open Source Robotics Foundation, Inc. +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// 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 OWNER 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 "avif_image_transport/avif_publisher.hpp" + +#include + +#include + +namespace avif_image_transport +{ + +enum avifParameters +{ + AVIF_QUALITY = 0, + AVIF_SPEED = 1, +}; + +const struct ParameterDefinition kParameters[] = +{ + { + // Quality - AVIF Compression Level from 0 to 100. A higher value means a smaller size. + ParameterValue(static_cast(75)), + ParameterDescriptor() + .set__name("quality") + .set__type(rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER) + .set__description("Quality") + .set__read_only(false) + .set__integer_range( + {rcl_interfaces::msg::IntegerRange() + .set__from_value(0) + .set__to_value(100) + .set__step(1)}) + }, + { + // Speed - 10 - Fastest, 0 - slowest + ParameterValue(static_cast(5)), + ParameterDescriptor() + .set__name("speed") + .set__type(rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER) + .set__description("Speed") + .set__read_only(false) + .set__integer_range( + {rcl_interfaces::msg::IntegerRange() + .set__from_value(0) + .set__to_value(10) + .set__step(1)}) + }, +}; + +AVIFPublisher::AVIFPublisher() +: logger_(rclcpp::get_logger("AVIFPublisher")) +{ + this->encoder_ = avifEncoderCreate(); +} + +AVIFPublisher::~AVIFPublisher() +{ + if (this->encoder_) {avifEncoderDestroy(this->encoder_);} +} + +std::string AVIFPublisher::getTransportName() const +{ + return "avif"; +} + +void AVIFPublisher::advertiseImpl( + rclcpp::Node * node, + const std::string & base_topic, + rmw_qos_profile_t custom_qos, + rclcpp::PublisherOptions options) +{ + node_ = node; + typedef image_transport::SimplePublisherPlugin Base; + Base::advertiseImpl(node, base_topic, custom_qos, options); + + // Declare Parameters + uint ns_len = node->get_effective_namespace().length(); + std::string param_base_name = base_topic.substr(ns_len); + std::replace(param_base_name.begin(), param_base_name.end(), '/', '.'); + + for (const ParameterDefinition & pd : kParameters) { + declareParameter(param_base_name, pd); + } +} + +AvifImageUniquePtr AVIFPublisher::ConvertToAvif( + const sensor_msgs::msg::Image & message, + bool lossless, + int bit_depth) const +{ + avifImage * result; + + const int width = message.width; + const int height = message.height; + if (message.encoding == "mono8") { + result = avifImageCreateEmpty(); + if (result == nullptr) { + RCLCPP_DEBUG(logger_, "Not able to create an empty image"); + return nullptr; + } + + result->width = width; + result->height = height; + result->depth = bit_depth; + result->yuvFormat = AVIF_PIXEL_FORMAT_YUV400; + result->colorPrimaries = AVIF_COLOR_PRIMARIES_UNSPECIFIED; + result->transferCharacteristics = AVIF_TRANSFER_CHARACTERISTICS_UNSPECIFIED; + result->matrixCoefficients = AVIF_MATRIX_COEFFICIENTS_IDENTITY; + result->yuvRange = AVIF_RANGE_FULL; + memcpy(&result->yuvPlanes[0], &message.data[0], message.data.size()); + + // result->yuvPlanes[0] = message.data; + result->yuvRowBytes[0] = message.step; + result->imageOwnsYUVPlanes = AVIF_FALSE; + return AvifImageUniquePtr(result); + } + + if (lossless) { + result = + avifImageCreate(width, height, bit_depth, AVIF_PIXEL_FORMAT_YUV444); + if (result == nullptr) {return nullptr;} + result->colorPrimaries = AVIF_COLOR_PRIMARIES_UNSPECIFIED; + result->transferCharacteristics = AVIF_TRANSFER_CHARACTERISTICS_UNSPECIFIED; + result->matrixCoefficients = AVIF_MATRIX_COEFFICIENTS_IDENTITY; + result->yuvRange = AVIF_RANGE_FULL; + } else { + result = + avifImageCreate(width, height, bit_depth, AVIF_PIXEL_FORMAT_YUV420); + if (result == nullptr) {return nullptr;} + result->colorPrimaries = AVIF_COLOR_PRIMARIES_BT709; + result->transferCharacteristics = AVIF_TRANSFER_CHARACTERISTICS_SRGB; + result->matrixCoefficients = AVIF_MATRIX_COEFFICIENTS_BT601; + result->yuvRange = AVIF_RANGE_FULL; + } + + avifRGBImage rgba; + avifRGBImageSetDefaults(&rgba, result); + if (message.encoding == "bgr8") { + rgba.format = AVIF_RGB_FORMAT_BGR; + } else if (message.encoding == "bgra8") { + rgba.format = AVIF_RGB_FORMAT_BGRA; + } + + rgba.rowBytes = message.step; + rgba.depth = bit_depth; + + rgba.pixels = reinterpret_cast(const_cast(&message.data[0])); + + if (avifImageRGBToYUV(result, &rgba) != AVIF_RESULT_OK) { + avifImageDestroy(result); + return nullptr; + } + return AvifImageUniquePtr(result); +} + +void AVIFPublisher::publish( + const sensor_msgs::msg::Image & message, + const PublishFn & publish_fn) const +{ + int bit_depth = 8; + int speed = node_->get_parameter(parameters_[AVIF_SPEED]).get_value(); + this->encoder_->speed = speed; + + int quality = node_->get_parameter(parameters_[AVIF_QUALITY]).get_value(); + this->encoder_->minQuantizer = encoder_->maxQuantizer = + (AVIF_QUANTIZER_BEST_QUALITY - AVIF_QUANTIZER_WORST_QUALITY) * + quality / (100 - 0) + + AVIF_QUANTIZER_WORST_QUALITY; + + auto avif_image = ConvertToAvif(message, false, bit_depth); + if (avif_image == nullptr) { + return; + } + avifEncoderAddImage( + this->encoder_, avif_image.get(), /*durationInTimescale=*/ 1, + AVIF_ADD_IMAGE_FLAG_SINGLE); + + avifRWData encoded_data = AVIF_DATA_EMPTY; + + avifResult addImageResult = avifEncoderAddImage( + encoder_, + avif_image.get(), 1, AVIF_ADD_IMAGE_FLAG_SINGLE); + if (addImageResult != AVIF_RESULT_OK) { + // std::cout << "error encoding addImageResult: " << addImageResult << std::endl; + } + + avifResult finishResult = avifEncoderFinish(encoder_, &encoded_data); + if (finishResult != AVIF_RESULT_OK) { + // std::cout << "error encoding finishResult: " << finishResult << std::endl; + } + + sensor_msgs::msg::CompressedImage compressed; + compressed.data.resize(encoded_data.size); + memcpy(&compressed.data[0], encoded_data.data, encoded_data.size); + + publish_fn(compressed); +} + +void AVIFPublisher::declareParameter( + const std::string & base_name, + const ParameterDefinition & definition) +{ + // transport scoped parameter (e.g. image_raw.compressed.format) + 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_->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_->get_parameter(param_name).get_parameter_value(); + } +} +} // namespace avif_image_transport diff --git a/avif_image_transport/src/avif_subscriber.cpp b/avif_image_transport/src/avif_subscriber.cpp new file mode 100644 index 0000000..c5b0ac3 --- /dev/null +++ b/avif_image_transport/src/avif_subscriber.cpp @@ -0,0 +1,107 @@ +// Copyright (c) 2023, Open Source Robotics Foundation, Inc. +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// 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 OWNER 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 "avif_image_transport/avif_subscriber.hpp" + +#include + +#include +#include + +#include + +#include +#include +#include + + +namespace avif_image_transport +{ +AVIFSubscriber::AVIFSubscriber() +: logger_(rclcpp::get_logger("AVIFSubscriber")) +{ + this->decoder_ = avifDecoderCreate(); + + RCLCPP_DEBUG(logger_, "AVIFSubscriber"); +} + +std::string AVIFSubscriber::getTransportName() const +{ + return "avif"; +} + +void AVIFSubscriber::subscribeImpl( + rclcpp::Node * node, + const std::string & base_topic, + const Callback & callback, + rmw_qos_profile_t custom_qos, + rclcpp::SubscriptionOptions options) +{ + node_ = node; + logger_ = node->get_logger(); + typedef image_transport::SimpleSubscriberPlugin Base; + Base::subscribeImpl(node, base_topic, callback, custom_qos, options); +} + +void AVIFSubscriber::internalCallback( + const sensor_msgs::msg::CompressedImage::ConstSharedPtr & msg, + const Callback & user_cb) +{ + // if the reentract callback is configure this mutex is required + std::unique_lock guard(this->mutex); + + avifImage * avif_image = avifImageCreateEmpty(); + avifDecoderReadMemory( + this->decoder_, avif_image, &msg->data[0], + msg->data.size()); + + auto result = std::make_shared(); + result->height = avif_image->height; + result->width = avif_image->width; + result->is_bigendian = false; + result->step = avif_image->yuvRowBytes[0] * 3; + result->encoding = "rgb8"; + result->data.resize(result->height * result->width * 3); + + avifRGBImage rgba; + avifRGBImageSetDefaults(&rgba, avif_image); + rgba.format = AVIF_RGB_FORMAT_BGR; + rgba.rowBytes = result->step; + rgba.depth = 8; + rgba.pixels = reinterpret_cast(const_cast(&result->data[0])); + avifImageYUVToRGB(avif_image, &rgba); + + user_cb(result); + avifImageDestroy(avif_image); +} +} // namespace avif_image_transport diff --git a/avif_image_transport/src/manifest.cpp b/avif_image_transport/src/manifest.cpp new file mode 100644 index 0000000..5940f40 --- /dev/null +++ b/avif_image_transport/src/manifest.cpp @@ -0,0 +1,38 @@ +// Copyright (c) 2023, Open Source Robotics Foundation, Inc. +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// 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 OWNER 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 "avif_image_transport/avif_publisher.hpp" +#include "avif_image_transport/avif_subscriber.hpp" + +PLUGINLIB_EXPORT_CLASS(avif_image_transport::AVIFPublisher, image_transport::PublisherPlugin) +PLUGINLIB_EXPORT_CLASS(avif_image_transport::AVIFSubscriber, image_transport::SubscriberPlugin)