diff --git a/svtav1_image_transport/CMakeLists.txt b/svtav1_image_transport/CMakeLists.txt new file mode 100644 index 0000000..edbed28 --- /dev/null +++ b/svtav1_image_transport/CMakeLists.txt @@ -0,0 +1,69 @@ +cmake_minimum_required(VERSION 3.16) + +project(svtav1_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() + +list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake/Modules") + +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(cv_bridge REQUIRED) +find_package(OpenCV REQUIRED COMPONENTS imgcodecs imgproc) +find_package(SVTENC REQUIRED) +find_package(SVTDEC REQUIRED) + +include_directories( + include + ${SVTENC_INCLUDE_DIR} + ${SVTDEC_INCLUDE_DIR} +) + +add_library( + ${PROJECT_NAME} SHARED + src/svtav1_publisher.cpp + src/svtav1_subscriber.cpp + src/manifest.cpp +) + +ament_target_dependencies(${PROJECT_NAME} + "cv_bridge" + "image_transport" + "rclcpp" + "pluginlib" + "sensor_msgs" +) + +target_link_libraries(${PROJECT_NAME} + ${SVTDEC_LIBRARY} + ${SVTENC_LIBRARY} +) + +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 svtav1_plugins.xml) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/svtav1_image_transport/cmake/Modules/FindSVTDEC.cmake b/svtav1_image_transport/cmake/Modules/FindSVTDEC.cmake new file mode 100644 index 0000000..2f8d157 --- /dev/null +++ b/svtav1_image_transport/cmake/Modules/FindSVTDEC.cmake @@ -0,0 +1,42 @@ +# Copyright 2019 Open Source Robotics Foundation, Inc. +# +# 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. + +find_package(PkgConfig QUIET) +if(PKG_CONFIG_FOUND) + pkg_check_modules(_SVT SvtAv1Dec) +endif() + +find_path(SVTDEC_INCLUDE_DIR + NAMES svt-av1/EbSvtAv1Dec.h + PATHS ${_SVTDEC_INCLUDEDIR} +) +message(_SVTDEC_INCLUDEDIR ${_SVTDEC_INCLUDEDIR}) + +find_library(SVTDEC_LIBRARY + NAMES SvtAv1Dec + PATHS ${_SVTDEC_LIBDIR}) + +if(SVTDEC_LIBRARY) + set(SVTDEC_LIBRARIES + ${SVTDEC_LIBRARIES} + ${SVTDEC_LIBRARY}) +endif() + +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(SVTDEC + FOUND_VAR SVTDEC_FOUND + REQUIRED_VARS SVTDEC_LIBRARY SVTDEC_LIBRARIES SVTDEC_INCLUDE_DIR + VERSION_VAR _SVTDEC_VERSION) + +mark_as_advanced(SVTDEC_INCLUDE_DIR SVTDEC_LIBRARY SVTDEC_LIBRARIES) diff --git a/svtav1_image_transport/cmake/Modules/FindSVTENC.cmake b/svtav1_image_transport/cmake/Modules/FindSVTENC.cmake new file mode 100644 index 0000000..c3bed9b --- /dev/null +++ b/svtav1_image_transport/cmake/Modules/FindSVTENC.cmake @@ -0,0 +1,42 @@ +# Copyright 2019 Open Source Robotics Foundation, Inc. +# +# 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. + +find_package(PkgConfig QUIET) +if(PKG_CONFIG_FOUND) + pkg_check_modules(_SVT SvtAv1Enc) +endif() + +find_path(SVTENC_INCLUDE_DIR + NAMES svt-av1/EbSvtAv1Enc.h + PATHS ${_SVTENC_INCLUDEDIR} +) +message(_SVTENC_INCLUDEDIR ${_SVTENC_INCLUDEDIR}) + +find_library(SVTENC_LIBRARY + NAMES SvtAv1Enc + PATHS ${_SVTENC_LIBDIR}) + +if(SVTENC_LIBRARY) + set(SVTENC_LIBRARIES + ${SVTENC_LIBRARIES} + ${SVTENC_LIBRARY}) +endif() + +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(SVTENC + FOUND_VAR SVTENC_FOUND + REQUIRED_VARS SVTENC_LIBRARY SVTENC_LIBRARIES SVTENC_INCLUDE_DIR + VERSION_VAR _SVTENC_VERSION) + +mark_as_advanced(SVTENC_INCLUDE_DIR SVTENC_LIBRARY SVTENC_LIBRARIES) diff --git a/svtav1_image_transport/include/svtav1_image_transport/svtav1_common.hpp b/svtav1_image_transport/include/svtav1_image_transport/svtav1_common.hpp new file mode 100644 index 0000000..48fa4c9 --- /dev/null +++ b/svtav1_image_transport/include/svtav1_image_transport/svtav1_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 SVTAV1_IMAGE_TRANSPORT__SVTAV1_COMMON_HPP_ +#define SVTAV1_IMAGE_TRANSPORT__SVTAV1_COMMON_HPP_ + +#include +#include + +namespace svtav1_image_transport +{ +using ParameterDescriptor = rcl_interfaces::msg::ParameterDescriptor; +using ParameterValue = rclcpp::ParameterValue; + +struct ParameterDefinition +{ + const ParameterValue defaultValue; + const ParameterDescriptor descriptor; +}; +} // namespace svtav1_image_transport + +#endif // SVTAV1_IMAGE_TRANSPORT__SVTAV1_COMMON_HPP_ diff --git a/svtav1_image_transport/include/svtav1_image_transport/svtav1_publisher.hpp b/svtav1_image_transport/include/svtav1_image_transport/svtav1_publisher.hpp new file mode 100644 index 0000000..ab202f2 --- /dev/null +++ b/svtav1_image_transport/include/svtav1_image_transport/svtav1_publisher.hpp @@ -0,0 +1,105 @@ +// 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 SVTAV1_IMAGE_TRANSPORT__SVTAV1_PUBLISHER_HPP_ +#define SVTAV1_IMAGE_TRANSPORT__SVTAV1_PUBLISHER_HPP_ + +#include + +#include +#include +#include +#include + +#include +#include +#include + +#include + +#include "svtav1_image_transport/svtav1_common.hpp" + +#include + +namespace svtav1_image_transport +{ +using CompressedImage = sensor_msgs::msg::CompressedImage; +using ParameterEvent = rcl_interfaces::msg::ParameterEvent; + +class SVTAV1Publisher : public image_transport::SimplePublisherPlugin +{ +public: + SVTAV1Publisher(); + ~SVTAV1Publisher() 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; + + void Initialize(int width, int height) const; + + rclcpp::Logger logger_; + rclcpp::Node * node_; + +private: + void set_default_svt_configuration(int width, int height) const; + + std::vector parameters_; + + void declareParameter( + const std::string & base_name, + const ParameterDefinition & definition); + + /* SVT-AV1 Encoder Handle */ + mutable EbComponentType * svt_encoder{nullptr}; + /* SVT-AV1 configuration */ + mutable EbSvtAv1EncConfiguration * svt_config{nullptr}; + mutable uint64_t frame_count; + // int dts_offset; + mutable EbBufferHeaderType * input_buf{nullptr}; + + mutable std::mutex mutex; + mutable cv::Mat ycrcb_planes[3]; +}; + +} // namespace svtav1_image_transport + +#endif // SVTAV1_IMAGE_TRANSPORT__SVTAV1_PUBLISHER_HPP_ diff --git a/svtav1_image_transport/include/svtav1_image_transport/svtav1_subscriber.hpp b/svtav1_image_transport/include/svtav1_image_transport/svtav1_subscriber.hpp new file mode 100644 index 0000000..ed3c268 --- /dev/null +++ b/svtav1_image_transport/include/svtav1_image_transport/svtav1_subscriber.hpp @@ -0,0 +1,96 @@ +// 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 SVTAV1_IMAGE_TRANSPORT__SVTAV1_SUBSCRIBER_HPP_ +#define SVTAV1_IMAGE_TRANSPORT__SVTAV1_SUBSCRIBER_HPP_ + +#include +#include +#include + +#include // NOLINT + +#include + +#include +#include + +#include +#include +#include + + +namespace svtav1_image_transport +{ +using CompressedImage = sensor_msgs::msg::CompressedImage; +using ParameterEvent = rcl_interfaces::msg::ParameterEvent; + +class SVTAV1Subscriber final + : public image_transport::SimpleSubscriberPlugin +{ +public: + SVTAV1Subscriber(); + virtual ~SVTAV1Subscriber() = 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; + +private: + rclcpp::Logger logger_; + rclcpp::Node * node_; + + EbSvtAv1DecConfiguration * svt_decoder_config{nullptr}; + EbComponentType * svt_decoder{nullptr}; + EbBufferHeaderType * output_buf{nullptr}; + mutable std::mutex mutex; + + mutable bool key_frame_received{false}; + + mutable EbBufferHeaderType * buffer; + mutable cv::Mat mat_BGR2YUV_I420_decode; +}; + +} // namespace svtav1_image_transport + +#endif // SVTAV1_IMAGE_TRANSPORT__SVTAV1_SUBSCRIBER_HPP_ diff --git a/svtav1_image_transport/package.xml b/svtav1_image_transport/package.xml new file mode 100644 index 0000000..d625fa3 --- /dev/null +++ b/svtav1_image_transport/package.xml @@ -0,0 +1,26 @@ + + svtav1_image_transport + 3.2.0 + + svtav1_image_transport provides a plugin to image_transport for transparently sending images + encoded as AVT-AV1 + + Alejandro Hernandez Cordero + BSD + + http://www.ros.org/wiki/image_transport_plugins + Alejandro Hernandez Cordero + + ament_cmake + + image_transport + cv_bridge + + ament_lint_auto + ament_lint_common + + + ament_cmake + + + diff --git a/svtav1_image_transport/src/manifest.cpp b/svtav1_image_transport/src/manifest.cpp new file mode 100644 index 0000000..b8b43b1 --- /dev/null +++ b/svtav1_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 "svtav1_image_transport/svtav1_publisher.hpp" +#include "svtav1_image_transport/svtav1_subscriber.hpp" + +PLUGINLIB_EXPORT_CLASS(svtav1_image_transport::SVTAV1Publisher, image_transport::PublisherPlugin) +PLUGINLIB_EXPORT_CLASS(svtav1_image_transport::SVTAV1Subscriber, image_transport::SubscriberPlugin) diff --git a/svtav1_image_transport/src/svtav1_publisher.cpp b/svtav1_image_transport/src/svtav1_publisher.cpp new file mode 100644 index 0000000..aaea3bd --- /dev/null +++ b/svtav1_image_transport/src/svtav1_publisher.cpp @@ -0,0 +1,379 @@ +// 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 "svtav1_image_transport/svtav1_publisher.hpp" + +#include + +#include +#include "cv_bridge/cv_bridge.hpp" + +namespace svtav1_image_transport +{ + +enum hParameters +{ + SVTAV1_ENCMODE = 0, +}; + +const struct ParameterDefinition kParameters[] = +{ + { + // ENC mode - SVTAV1 Compression Level from 1 to 13. A lower value means a smaller size. + ParameterValue(static_cast(13)), + ParameterDescriptor() + .set__name("enc_mode") + .set__type(rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER) + .set__description("Encoder mode") + .set__read_only(false) + .set__integer_range( + {rcl_interfaces::msg::IntegerRange() + .set__from_value(0) + .set__to_value(13) + .set__step(1)}) + }, +}; + +#define PROP_RC_MODE_CQP 0 +#define PROP_RC_MODE_VBR 1 + +#define PROP_HIERARCHICAL_LEVEL_DEFAULT 4 +#define PROP_P_FRAMES_DEFAULT 0 +#define PROP_PRED_STRUCTURE_DEFAULT 2 +#define PROP_GOP_SIZE_DEFAULT -1 +#define PROP_INTRA_REFRESH_DEFAULT 1 +#define PROP_QP_DEFAULT 35 +#define PROP_DEBLOCKING_DEFAULT 1 +#define PROP_RC_MODE_DEFAULT PROP_RC_MODE_CQP +#define PROP_BITRATE_DEFAULT 70000 +#define PROP_QP_MAX_DEFAULT 63 +#define PROP_QP_MIN_DEFAULT 0 +#define PROP_LOOKAHEAD_DEFAULT (unsigned int)-1 +#define PROP_SCD_DEFAULT 0 +#define PROP_AUD_DEFAULT 0 +#define PROP_CORES_DEFAULT 10 +#define PROP_SOCKET_DEFAULT -1 + +SVTAV1Publisher::SVTAV1Publisher() +: logger_(rclcpp::get_logger("SVTAV1Publisher")) +{ +} + +SVTAV1Publisher::~SVTAV1Publisher() +{ +} + +std::string SVTAV1Publisher::getTransportName() const +{ + return "svtav1"; +} + +void SVTAV1Publisher::set_default_svt_configuration(int width, int height) const +{ + int encmode_value = this->node_->get_parameter(parameters_[SVTAV1_ENCMODE]).get_value(); + + // Zero-initialize svt_config because svt_av1_enc_init_handle() does not set + // many fields of svt_config. + // See https://gitlab.com/AOMediaCodec/SVT-AV1/-/issues/1697. + memset(this->svt_config, 0, sizeof(EbSvtAv1EncConfiguration)); + this->svt_config->enc_mode = encmode_value; + this->svt_config->source_width = width; + this->svt_config->source_height = height; + this->svt_config->intra_period_length = PROP_GOP_SIZE_DEFAULT - 1; + this->svt_config->intra_refresh_type = + static_cast(PROP_INTRA_REFRESH_DEFAULT); + this->svt_config->frame_rate = 30; + this->svt_config->frame_rate_denominator = 0; + this->svt_config->frame_rate_numerator = 0; + this->svt_config->hierarchical_levels = PROP_HIERARCHICAL_LEVEL_DEFAULT; + this->svt_config->pred_structure = PROP_PRED_STRUCTURE_DEFAULT; + this->svt_config->scene_change_detection = PROP_SCD_DEFAULT; + this->svt_config->rate_control_mode = PROP_RC_MODE_DEFAULT; + this->svt_config->target_bit_rate = PROP_BITRATE_DEFAULT; + this->svt_config->max_qp_allowed = PROP_QP_MAX_DEFAULT; + this->svt_config->min_qp_allowed = PROP_QP_MIN_DEFAULT; + this->svt_config->screen_content_mode = 0; + this->svt_config->enable_adaptive_quantization = 0; + this->svt_config->qp = PROP_QP_DEFAULT; + this->svt_config->use_qp_file = 0; + this->svt_config->enable_dlf_flag = (PROP_DEBLOCKING_DEFAULT == 1); + this->svt_config->film_grain_denoise_strength = 0; + this->svt_config->cdef_level = -1; + this->svt_config->enable_restoration_filtering = -1; + this->svt_config->enable_mfmv = -1; + + // HME parameters + this->svt_config->channel_id = 0; + this->svt_config->active_channel_count = 1; + this->svt_config->recon_enabled = 0; + + // thread affinity + this->svt_config->logical_processors = PROP_CORES_DEFAULT; + this->svt_config->target_socket = PROP_SOCKET_DEFAULT; + this->svt_config->pin_threads = 0; + + // tile based encoding + this->svt_config->tile_columns = 2; + this->svt_config->tile_rows = 0; + this->svt_config->restricted_motion_vector = 0; + + // alt-ref + this->svt_config->enable_tf = 1; + this->svt_config->enable_overlays = 0; + + // super resolution + this->svt_config->superres_mode = SUPERRES_NONE; // SUPERRES_NONE + this->svt_config->superres_denom = 8; + this->svt_config->superres_kf_denom = 8; + this->svt_config->superres_qthres = 43; + + // latency + + // Annex A + this->svt_config->profile = MAIN_PROFILE; + this->svt_config->tier = 0; + this->svt_config->level = 0; + + this->svt_config->stat_report = 0; + this->svt_config->high_dynamic_range_input = 0; + this->svt_config->encoder_bit_depth = 8; + this->svt_config->encoder_color_format = EB_YUV420; + this->svt_config->compressed_ten_bit_format = 0; + this->svt_config->use_cpu_flags = CPU_FLAGS_ALL; + + // color description + this->svt_config->color_range = 0; + this->svt_config->color_primaries = 2; + this->svt_config->transfer_characteristics = 2; + this->svt_config->matrix_coefficients = 2; +} + +void SVTAV1Publisher::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); + } +} + +void SVTAV1Publisher::Initialize(int width, int height) const +{ + this->svt_config = static_cast(malloc( + sizeof(EbSvtAv1EncConfiguration))); + if (!this->svt_config) { + std::cerr << "Error malloc EbSvtAv1EncConfiguration" << std::endl; + return; + } + this->frame_count = 0; + + EbErrorType res = + svt_av1_enc_init_handle(&this->svt_encoder, NULL, this->svt_config); + if (res != EB_ErrorNone) { + std::cerr << "svt_av1_enc_init_handle failed with error " << res << std::endl; + free(this->svt_config); + return; + } + + // setting configuration here since svt_av1_enc_init_handle overrides it + this->set_default_svt_configuration(width, height); + + res = svt_av1_enc_set_parameter(this->svt_encoder, this->svt_config); + if (res != EB_ErrorNone) { + std::cerr << "svt_av1_enc_set_parameter failed with error " << res << std::endl; + return; + } + + res = svt_av1_enc_init(this->svt_encoder); + + if (res != EB_ErrorNone) { + std::cerr << "svt_av1_enc_init failed with error " << res << std::endl; + return; + } + + this->input_buf = static_cast(malloc(sizeof(EbBufferHeaderType))); + if (!this->input_buf) { + free(this->svt_config); + std::cerr << "insufficient resources" << std::endl; + return; + } + this->input_buf->p_buffer = static_cast(malloc(sizeof(EbSvtIOFormat))); + if (!this->input_buf->p_buffer) { + free(this->svt_config); + free(this->input_buf); + std::cerr << "insufficient resources" << std::endl; + return; + } + memset(this->input_buf->p_buffer, 0, sizeof(EbSvtIOFormat)); + this->input_buf->size = sizeof(EbBufferHeaderType); + this->input_buf->p_app_private = NULL; + this->input_buf->pic_type = EB_AV1_INVALID_PICTURE; +} + +void SVTAV1Publisher::publish( + const sensor_msgs::msg::Image & message, + const PublishFn & publish_fn) const +{ + if (this->svt_config == nullptr) { + this->Initialize(message.width, message.height); + } + + auto cv_ptr = cv_bridge::toCvCopy(message); + const cv::Mat & cv_image = cv_ptr->image; + cv::Mat mat_BGR2YUV_I420; + + cv::cvtColor(cv_image, mat_BGR2YUV_I420, cv::COLOR_RGB2YUV_I420); + + EbErrorType res = EB_ErrorNone; + EbBufferHeaderType * input_buffer = this->input_buf; + EbSvtIOFormat * input_picture_buffer = + reinterpret_cast(this->input_buf->p_buffer); + + ycrcb_planes[0] = mat_BGR2YUV_I420(cv::Rect(0, 0, cv_image.cols, cv_image.rows)); + ycrcb_planes[1] = mat_BGR2YUV_I420(cv::Rect(0, cv_image.rows, cv_image.cols, cv_image.rows / 4)); + ycrcb_planes[2] = + mat_BGR2YUV_I420( + cv::Rect( + 0, cv_image.rows + cv_image.rows / 4, cv_image.cols, + cv_image.rows / 4)); + + input_picture_buffer->width = cv_image.cols; + input_picture_buffer->height = cv_image.rows; + input_picture_buffer->bit_depth = EB_EIGHT_BIT; + input_picture_buffer->color_fmt = EB_YUV420; + + input_picture_buffer->y_stride = cv_image.cols; + input_picture_buffer->cb_stride = cv_image.cols / 2; + input_picture_buffer->cr_stride = cv_image.cols / 2; + + input_picture_buffer->luma = ycrcb_planes[0].data; + input_picture_buffer->cb = ycrcb_planes[1].data; + input_picture_buffer->cr = ycrcb_planes[2].data; + + input_picture_buffer->luma_ext = NULL; + input_picture_buffer->cb_ext = NULL; + input_picture_buffer->cr_ext = NULL; + + input_picture_buffer->origin_x = 0; + input_picture_buffer->origin_y = 0; + + input_buffer->n_filled_len = mat_BGR2YUV_I420.cols * mat_BGR2YUV_I420.rows; + + /* Fill in Buffers Header control data */ + input_buffer->flags = 0; + input_buffer->p_app_private = NULL; + input_buffer->n_alloc_len = 0; + input_buffer->pts = this->frame_count++; + input_buffer->pic_type = EB_AV1_INVALID_PICTURE; + input_buffer->flags = 0; + input_buffer->metadata = NULL; + + input_buffer->pic_type = EB_AV1_KEY_PICTURE; + + res = svt_av1_enc_send_picture(this->svt_encoder, input_buffer); + if (res != EB_ErrorNone) { + std::cerr << "error in sending picture to encoder" << std::endl; + return; + } + + EbBufferHeaderType * output_buf = NULL; + + res = svt_av1_enc_get_packet(this->svt_encoder, &output_buf, false); + + uint8_t encode_at_eos = 0; + + if (output_buf != NULL) { + encode_at_eos = + ((output_buf->flags & EB_BUFFERFLAG_EOS) == EB_BUFFERFLAG_EOS); + } + + if (encode_at_eos) { + std::cerr << "GST_FLOW_EOS" << std::endl; + return; + } else if (res == EB_ErrorMax) { + std::cerr << "encoded failed" << std::endl; + return; + } else if (res != EB_NoErrorEmptyQueue && output_buf && output_buf->p_buffer && // NOLINT + (output_buf->n_filled_len > 0)) + { + sensor_msgs::msg::CompressedImage compressed; + compressed.data.resize(output_buf->n_filled_len + (3 * sizeof(uint32_t)) + sizeof(uint8_t)); + + int message_size = 0; + memcpy(&compressed.data[message_size], &message.width, sizeof(uint32_t)); + message_size += sizeof(uint32_t); + memcpy(&compressed.data[message_size], &message.height, sizeof(uint32_t)); + message_size += sizeof(uint32_t); + memcpy(&compressed.data[message_size], &this->frame_count, sizeof(uint32_t)); + message_size += sizeof(uint32_t); + compressed.data[message_size] = (input_buffer->pic_type == EB_AV1_KEY_PICTURE); + message_size += sizeof(uint8_t); + memcpy(&compressed.data[message_size], output_buf->p_buffer, output_buf->n_filled_len); + + svt_av1_enc_release_out_buffer(&output_buf); + publish_fn(compressed); + } +} + +void SVTAV1Publisher::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 svtav1_image_transport diff --git a/svtav1_image_transport/src/svtav1_subscriber.cpp b/svtav1_image_transport/src/svtav1_subscriber.cpp new file mode 100644 index 0000000..dcf1f6c --- /dev/null +++ b/svtav1_image_transport/src/svtav1_subscriber.cpp @@ -0,0 +1,244 @@ +// 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 "svtav1_image_transport/svtav1_subscriber.hpp" + +#include +#include + +#include + +#include +#include +#include + +#include "opencv2/core.hpp" +#include "opencv2/imgproc.hpp" +#include + +namespace svtav1_image_transport +{ +SVTAV1Subscriber::SVTAV1Subscriber() +: logger_(rclcpp::get_logger("SVTAV1Subscriber")) +{ + this->svt_decoder_config = + static_cast(malloc(sizeof(EbSvtAv1DecConfiguration))); + if (!this->svt_decoder_config) { + std::cerr << "Error svt_decoder_config" << std::endl; + return; + } + + auto return_error = svt_av1_dec_init_handle(&this->svt_decoder, NULL, this->svt_decoder_config); + if (return_error != EB_ErrorNone) { + return_error = svt_av1_dec_deinit_handle(this->svt_decoder); + return; + } + + this->svt_decoder_config->operating_point = -1; + this->svt_decoder_config->output_all_layers = 0; + this->svt_decoder_config->skip_film_grain = 0; + this->svt_decoder_config->skip_frames = 0; + this->svt_decoder_config->frames_to_be_decoded = 0; + this->svt_decoder_config->compressed_ten_bit_format = 0; + this->svt_decoder_config->eight_bit_output = 0; + + /* Picture parameters */ + this->svt_decoder_config->max_picture_width = 0; + this->svt_decoder_config->max_picture_height = 0; + this->svt_decoder_config->max_bit_depth = EB_EIGHT_BIT; + this->svt_decoder_config->is_16bit_pipeline = 0; + this->svt_decoder_config->max_color_format = EB_YUV420; + + // Application Specific parameters + this->svt_decoder_config->channel_id = 0; + this->svt_decoder_config->active_channel_count = 1; + this->svt_decoder_config->stat_report = 0; + + /* Multi-thread parameters */ + this->svt_decoder_config->threads = 10; + this->svt_decoder_config->num_p_frames = 1; + + return_error = svt_av1_dec_set_parameter(this->svt_decoder, this->svt_decoder_config); + if (return_error != EB_ErrorNone) { + std::cerr << "svt_av1_dec_set_parameter failed with error " << return_error << std::endl; + return; + } + + return_error = svt_av1_dec_init(this->svt_decoder); + if (return_error != EB_ErrorNone) { + std::cerr << "svt_av1_dec_init failed with error " << return_error << std::endl; + return; + } + + this->output_buf = static_cast(malloc(sizeof(EbBufferHeaderType))); + if (!this->output_buf) { + std::cerr << "insufficient resources" << std::endl; + return; + } + + this->output_buf->p_buffer = static_cast(malloc(sizeof(EbSvtIOFormat))); + if (!this->output_buf->p_buffer) { + std::cerr << "insufficient resources" << std::endl; + return; + } + + buffer = static_cast(malloc(sizeof(*buffer))); + if (!buffer) { + std::cerr << "insufficient resources" << std::endl; + return; + } + + buffer->p_buffer = static_cast(malloc(sizeof(EbSvtIOFormat))); + if (!buffer->p_buffer) { + std::cerr << "insufficient resources" << std::endl; + return; + } + reinterpret_cast(buffer->p_buffer)->luma = nullptr; + reinterpret_cast(buffer->p_buffer)->cb = nullptr; + reinterpret_cast(buffer->p_buffer)->cr = nullptr; +} + +std::string SVTAV1Subscriber::getTransportName() const +{ + return "svtav1"; +} + +void SVTAV1Subscriber::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 SVTAV1Subscriber::internalCallback( + const sensor_msgs::msg::CompressedImage::ConstSharedPtr & msg, + const Callback & user_cb) +{ + std::unique_lock guard(this->mutex); + + int w = 0; + int h = 0; + int frame_count = 0; + int key_frame = 0; + int size_message = 0; + memcpy(&w, &msg->data[size_message], sizeof(uint32_t)); + size_message += sizeof(uint32_t); + memcpy(&h, &msg->data[size_message], sizeof(uint32_t)); + size_message += sizeof(uint32_t); + memcpy(&frame_count, &msg->data[size_message], sizeof(uint32_t)); + size_message += sizeof(uint32_t); + key_frame = msg->data[size_message]; + size_message += sizeof(uint8_t); + int size = + (this->svt_decoder_config->max_bit_depth == + EB_EIGHT_BIT) ? sizeof(uint8_t) : sizeof(uint16_t); + size = size * w * h; + + if (reinterpret_cast(buffer->p_buffer)->luma == NULL) { + reinterpret_cast(buffer->p_buffer)->luma = + reinterpret_cast(malloc(size)); + reinterpret_cast(buffer->p_buffer)->cb = + reinterpret_cast(malloc(size >> 2)); + reinterpret_cast(buffer->p_buffer)->cr = + reinterpret_cast(malloc(size >> 2)); + + mat_BGR2YUV_I420_decode = cv::Mat(h + h / 2, w, CV_8UC1); + } + + output_buf->n_filled_len = msg->data.size() - (3 * sizeof(uint32_t) + sizeof(uint8_t)); + const unsigned char * p = msg->data.data() + (3 * sizeof(uint32_t) + sizeof(uint8_t)); + output_buf->p_buffer = (uint8_t *)p; // NOLINT + + auto return_error = svt_av1_dec_frame( + this->svt_decoder, + output_buf->p_buffer, + output_buf->n_filled_len, + 0); + + if (return_error != EB_ErrorNone) { + std::cerr << "svt_av1_dec_frame error" << std::endl; + return; + } + + EbAV1StreamInfo stream_info; + EbAV1FrameInfo frame_info; + + /* FilmGrain module req. even dim. for internal operation */ + EbSvtIOFormat * pic_buffer = reinterpret_cast(buffer->p_buffer); + pic_buffer->y_stride = w; + pic_buffer->cb_stride = w / 2; + pic_buffer->cr_stride = w / 2; + pic_buffer->width = w; + pic_buffer->height = h; + pic_buffer->luma_ext = NULL; + pic_buffer->cb_ext = NULL; + pic_buffer->cr_ext = NULL; + pic_buffer->origin_x = 0; + pic_buffer->origin_y = 0; + pic_buffer->bit_depth = this->svt_decoder_config->max_bit_depth; + + try { + if (svt_av1_dec_get_picture(this->svt_decoder, buffer, &stream_info, &frame_info) != + EB_DecNoOutputPicture) + { + memcpy( + mat_BGR2YUV_I420_decode.data, + reinterpret_cast(buffer->p_buffer)->luma, size); + memcpy( + &mat_BGR2YUV_I420_decode.data[w * h], + reinterpret_cast(buffer->p_buffer)->cb, + (size >> 2)); + memcpy( + &mat_BGR2YUV_I420_decode.data[(w * h) + (size >> 2)], + reinterpret_cast(buffer->p_buffer)->cr, (size >> 2)); + + cv::Mat rgb; + cv::cvtColor(mat_BGR2YUV_I420_decode, rgb, cv::COLOR_YUV2RGB_I420); + + std_msgs::msg::Header header; + header.stamp = this->node_->now(); + + auto result = cv_bridge::CvImage(header, sensor_msgs::image_encodings::BGR8, rgb); + auto img_msg = result.toImageMsg(); + user_cb(img_msg); + } + } catch (...) { + } +} +} // namespace svtav1_image_transport diff --git a/svtav1_image_transport/svtav1_plugins.xml b/svtav1_image_transport/svtav1_plugins.xml new file mode 100644 index 0000000..9761ddc --- /dev/null +++ b/svtav1_image_transport/svtav1_plugins.xml @@ -0,0 +1,19 @@ + + + + This plugin publishes a SVT-AV1 image + + + + + + This plugin decompresses a SVT-AV1 Image. + + +