From fcccb841095fbcb0f5343efaa7269f142322a937 Mon Sep 17 00:00:00 2001 From: Georg Bartels Date: Thu, 27 Feb 2020 14:34:36 +0100 Subject: [PATCH 01/10] Refactored image decompression into stand-alone function. --- compressed_image_transport/CMakeLists.txt | 10 ++- .../compression_common.h | 6 ++ .../src/compressed_subscriber.cpp | 81 ++----------------- 3 files changed, 21 insertions(+), 76 deletions(-) diff --git a/compressed_image_transport/CMakeLists.txt b/compressed_image_transport/CMakeLists.txt index 7fe17ed..2d57f6c 100644 --- a/compressed_image_transport/CMakeLists.txt +++ b/compressed_image_transport/CMakeLists.txt @@ -16,13 +16,17 @@ catkin_package( include_directories(include ${catkin_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS}) +add_library(${PROJECT_NAME}_compression src/compression_common.cpp) +add_dependencies(${PROJECT_NAME}_compression ${catkin_EXPORTED_TARGETS}) +target_link_libraries(${PROJECT_NAME}_compression ${catkin_LIBRARIES} ${OpenCV_LIBRARIES}) + add_library(${PROJECT_NAME} src/compressed_publisher.cpp src/compressed_subscriber.cpp src/manifest.cpp) -add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_gencfg) -target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${OpenCV_LIBRARIES}) +add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_gencfg ${PROJECT_NAME}_compression) +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${OpenCV_LIBRARIES} ${PROJECT_NAME}_compression) class_loader_hide_library_symbols(${PROJECT_NAME}) -install(TARGETS ${PROJECT_NAME} +install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_compression ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} diff --git a/compressed_image_transport/include/compressed_image_transport/compression_common.h b/compressed_image_transport/include/compressed_image_transport/compression_common.h index a680a09..e1d3a13 100644 --- a/compressed_image_transport/include/compressed_image_transport/compression_common.h +++ b/compressed_image_transport/include/compressed_image_transport/compression_common.h @@ -35,6 +35,9 @@ #ifndef COMPRESSED_IMAGE_TRANSPORT_COMPRESSION_COMMON #define COMPRESSED_IMAGE_TRANSPORT_COMPRESSION_COMMON +#include +#include + namespace compressed_image_transport { @@ -44,6 +47,9 @@ enum compressionFormat UNDEFINED = -1, JPEG, PNG }; +// standadlone decoding function +sensor_msgs::ImagePtr decodeCompressedImage(const sensor_msgs::CompressedImageConstPtr& image, int decode_flag); + } //namespace compressed_image_transport #endif diff --git a/compressed_image_transport/src/compressed_subscriber.cpp b/compressed_image_transport/src/compressed_subscriber.cpp index a84a51f..529e46b 100644 --- a/compressed_image_transport/src/compressed_subscriber.cpp +++ b/compressed_image_transport/src/compressed_subscriber.cpp @@ -84,85 +84,20 @@ void CompressedSubscriber::shutdown() void CompressedSubscriber::internalCallback(const sensor_msgs::CompressedImageConstPtr& message, const Callback& user_cb) - { - - cv_bridge::CvImagePtr cv_ptr(new cv_bridge::CvImage); - - // Copy message header - cv_ptr->header = message->header; - - // Decode color/mono image - try + try { - cv_ptr->image = cv::imdecode(cv::Mat(message->data), imdecode_flag_); - - // Assign image encoding string - const size_t split_pos = message->format.find(';'); - if (split_pos==std::string::npos) - { - // Older version of compressed_image_transport does not signal image format - switch (cv_ptr->image.channels()) - { - case 1: - cv_ptr->encoding = enc::MONO8; - break; - case 3: - cv_ptr->encoding = enc::BGR8; - break; - default: - ROS_ERROR("Unsupported number of channels: %i", cv_ptr->image.channels()); - break; - } - } else - { - std::string image_encoding = message->format.substr(0, split_pos); - - cv_ptr->encoding = image_encoding; - - if ( enc::isColor(image_encoding)) - { - std::string compressed_encoding = message->format.substr(split_pos); - bool compressed_bgr_image = (compressed_encoding.find("compressed bgr") != std::string::npos); - - // Revert color transformation - if (compressed_bgr_image) - { - // if necessary convert colors from bgr to rgb - if ((image_encoding == enc::RGB8) || (image_encoding == enc::RGB16)) - cv::cvtColor(cv_ptr->image, cv_ptr->image, CV_BGR2RGB); - - if ((image_encoding == enc::RGBA8) || (image_encoding == enc::RGBA16)) - cv::cvtColor(cv_ptr->image, cv_ptr->image, CV_BGR2RGBA); - - if ((image_encoding == enc::BGRA8) || (image_encoding == enc::BGRA16)) - cv::cvtColor(cv_ptr->image, cv_ptr->image, CV_BGR2BGRA); - } else - { - // if necessary convert colors from rgb to bgr - if ((image_encoding == enc::BGR8) || (image_encoding == enc::BGR16)) - cv::cvtColor(cv_ptr->image, cv_ptr->image, CV_RGB2BGR); - - if ((image_encoding == enc::BGRA8) || (image_encoding == enc::BGRA16)) - cv::cvtColor(cv_ptr->image, cv_ptr->image, CV_RGB2BGRA); - - if ((image_encoding == enc::RGBA8) || (image_encoding == enc::RGBA16)) - cv::cvtColor(cv_ptr->image, cv_ptr->image, CV_RGB2RGBA); - } - } - } + // Decode color/mono image, and call user callback + user_cb(decodeCompressedImage(message, imdecode_flag_)); } - catch (cv::Exception& e) + catch (cv::Exception& e) + { + ROS_ERROR("%s", e.what()); + } + catch (std::runtime_error& e) { ROS_ERROR("%s", e.what()); } - - size_t rows = cv_ptr->image.rows; - size_t cols = cv_ptr->image.cols; - - if ((rows > 0) && (cols > 0)) - // Publish message to user callback - user_cb(cv_ptr->toImageMsg()); } } //namespace compressed_image_transport From 88a4be26506be9380f82ec153ca7e8cf39f25a13 Mon Sep 17 00:00:00 2001 From: Georg Bartels Date: Thu, 27 Feb 2020 14:39:48 +0100 Subject: [PATCH 02/10] Added missing file. --- .../src/compression_common.cpp | 111 ++++++++++++++++++ 1 file changed, 111 insertions(+) create mode 100644 compressed_image_transport/src/compression_common.cpp diff --git a/compressed_image_transport/src/compression_common.cpp b/compressed_image_transport/src/compression_common.cpp new file mode 100644 index 0000000..3c85bd4 --- /dev/null +++ b/compressed_image_transport/src/compression_common.cpp @@ -0,0 +1,111 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 20012, Willow Garage, 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 Willow Garage 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 +#include +#include +#include + +namespace enc = sensor_msgs::image_encodings; + +namespace compressed_image_transport { + sensor_msgs::ImagePtr decodeCompressedImage(const sensor_msgs::CompressedImageConstPtr &image, int decode_flag) { + if (!image) + throw std::runtime_error("Call to decode a compressed image received a NULL pointer."); + + cv_bridge::CvImagePtr cv_ptr(new cv_bridge::CvImage); + + // Copy message header + cv_ptr->header = image->header; + + // Decode color/mono image + cv_ptr->image = cv::imdecode(cv::Mat(image->data), decode_flag); + + // Assign image encoding string + const size_t split_pos = image->format.find(';'); + if (split_pos == std::string::npos) { + // Older version of compressed_image_transport does not signal image format + switch (cv_ptr->image.channels()) { + case 1: + cv_ptr->encoding = enc::MONO8; + break; + case 3: + cv_ptr->encoding = enc::BGR8; + break; + default: + throw std::runtime_error( + "Unsupported number of channels: " + std::to_string(cv_ptr->image.channels())); + } + } else { + std::string image_encoding = image->format.substr(0, split_pos); + + cv_ptr->encoding = image_encoding; + + if (enc::isColor(image_encoding)) { + std::string compressed_encoding = image->format.substr(split_pos); + bool compressed_bgr_image = (compressed_encoding.find("compressed bgr") != std::string::npos); + + // Revert color transformation + if (compressed_bgr_image) { + // if necessary convert colors from bgr to rgb + if ((image_encoding == enc::RGB8) || (image_encoding == enc::RGB16)) + cv::cvtColor(cv_ptr->image, cv_ptr->image, CV_BGR2RGB); + + if ((image_encoding == enc::RGBA8) || (image_encoding == enc::RGBA16)) + cv::cvtColor(cv_ptr->image, cv_ptr->image, CV_BGR2RGBA); + + if ((image_encoding == enc::BGRA8) || (image_encoding == enc::BGRA16)) + cv::cvtColor(cv_ptr->image, cv_ptr->image, CV_BGR2BGRA); + } else { + // if necessary convert colors from rgb to bgr + if ((image_encoding == enc::BGR8) || (image_encoding == enc::BGR16)) + cv::cvtColor(cv_ptr->image, cv_ptr->image, CV_RGB2BGR); + + if ((image_encoding == enc::BGRA8) || (image_encoding == enc::BGRA16)) + cv::cvtColor(cv_ptr->image, cv_ptr->image, CV_RGB2BGRA); + + if ((image_encoding == enc::RGBA8) || (image_encoding == enc::RGBA16)) + cv::cvtColor(cv_ptr->image, cv_ptr->image, CV_RGB2RGBA); + } + } + } + + if ((cv_ptr->image.rows > 0) && (cv_ptr->image.cols > 0)) + return cv_ptr->toImageMsg(); + else + throw std::runtime_error("Could not extract meaningful image. One of the dimensions was 0. Rows: " + + std::to_string(cv_ptr->image.rows) + ", columns: " + std::to_string(cv_ptr->image.cols) + "."); + } +} \ No newline at end of file From 73f2b1b6809ce9e3e30bc9d4840e04b8ce813448 Mon Sep 17 00:00:00 2001 From: Georg Bartels Date: Thu, 12 Mar 2020 13:00:35 +0100 Subject: [PATCH 03/10] Removed usage of std::to_string because it is a C++11 feature, and that is not available for ROS Kinetic. --- .../src/compression_common.cpp | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) diff --git a/compressed_image_transport/src/compression_common.cpp b/compressed_image_transport/src/compression_common.cpp index 3c85bd4..45a9c62 100644 --- a/compressed_image_transport/src/compression_common.cpp +++ b/compressed_image_transport/src/compression_common.cpp @@ -64,9 +64,11 @@ namespace compressed_image_transport { case 3: cv_ptr->encoding = enc::BGR8; break; - default: - throw std::runtime_error( - "Unsupported number of channels: " + std::to_string(cv_ptr->image.channels())); + default: { + std::stringstream ss; + ss << "Unsupported number of channels: " << cv_ptr->image.channels(); + throw std::runtime_error(ss.str()); + } } } else { std::string image_encoding = image->format.substr(0, split_pos); @@ -104,8 +106,11 @@ namespace compressed_image_transport { if ((cv_ptr->image.rows > 0) && (cv_ptr->image.cols > 0)) return cv_ptr->toImageMsg(); - else - throw std::runtime_error("Could not extract meaningful image. One of the dimensions was 0. Rows: " + - std::to_string(cv_ptr->image.rows) + ", columns: " + std::to_string(cv_ptr->image.cols) + "."); + else { + std::stringstream ss; + ss << "Could not extract meaningful image. One of the dimensions was 0. Rows: " + << cv_ptr->image.rows << ", columns: " << cv_ptr->image.cols << "."; + throw std::runtime_error(ss.str()); + } } } \ No newline at end of file From 69989cce0a84a0e9a6ad04648899297369c4d96f Mon Sep 17 00:00:00 2001 From: Ferenc Balint-Benczedi Date: Tue, 31 Mar 2020 14:37:06 +0200 Subject: [PATCH 04/10] image encoding as a standalone function --- .../compression_common.h | 4 + .../src/compressed_publisher.cpp | 135 +++--------------- .../src/compression_common.cpp | 132 ++++++++++++++++- 3 files changed, 152 insertions(+), 119 deletions(-) diff --git a/compressed_image_transport/include/compressed_image_transport/compression_common.h b/compressed_image_transport/include/compressed_image_transport/compression_common.h index e1d3a13..343d06f 100644 --- a/compressed_image_transport/include/compressed_image_transport/compression_common.h +++ b/compressed_image_transport/include/compressed_image_transport/compression_common.h @@ -50,6 +50,10 @@ enum compressionFormat // standadlone decoding function sensor_msgs::ImagePtr decodeCompressedImage(const sensor_msgs::CompressedImageConstPtr& image, int decode_flag); +// standadlone encoding function +// for setting the correct parameters take look at compressed_publisher.cpp +sensor_msgs::CompressedImageConstPtr encodeImage(const sensor_msgs::Image &message, compressionFormat encode_flag, std::vector params); + } //namespace compressed_image_transport #endif diff --git a/compressed_image_transport/src/compressed_publisher.cpp b/compressed_image_transport/src/compressed_publisher.cpp index ef01e35..8460d3a 100644 --- a/compressed_image_transport/src/compressed_publisher.cpp +++ b/compressed_image_transport/src/compressed_publisher.cpp @@ -78,14 +78,6 @@ void CompressedPublisher::configCb(Config& config, uint32_t level) void CompressedPublisher::publish(const sensor_msgs::Image& message, const PublishFn& publish_fn) const { - // Compressed image message - sensor_msgs::CompressedImage compressed; - compressed.header = message.header; - compressed.format = message.encoding; - - // Compression settings - std::vector params; - // Get codec configuration compressionFormat encodingFormat = UNDEFINED; if (config_.format == compressed_image_transport::CompressedPublisher_jpeg) @@ -93,16 +85,15 @@ void CompressedPublisher::publish(const sensor_msgs::Image& message, const Publi if (config_.format == compressed_image_transport::CompressedPublisher_png) encodingFormat = PNG; - // Bit depth of image encoding - int bitDepth = enc::bitDepth(message.encoding); - int numChannels = enc::numChannels(message.encoding); - + // Compression settings + std::vector params; + sensor_msgs::CompressedImageConstPtr compressed; switch (encodingFormat) { // JPEG Compression case JPEG: { - params.resize(9, 0); + params.resize(8, 0); params[0] = IMWRITE_JPEG_QUALITY; params[1] = config_.jpeg_quality; params[2] = IMWRITE_JPEG_PROGRESSIVE; @@ -112,123 +103,31 @@ void CompressedPublisher::publish(const sensor_msgs::Image& message, const Publi params[6] = IMWRITE_JPEG_RST_INTERVAL; params[7] = config_.jpeg_restart_interval; - // Update ros message format header - compressed.format += "; jpeg compressed "; - - // Check input format - if ((bitDepth == 8) || (bitDepth == 16)) - { - // Target image format - std::string targetFormat; - if (enc::isColor(message.encoding)) - { - // convert color images to BGR8 format - targetFormat = "bgr8"; - compressed.format += targetFormat; - } - - // OpenCV-ros bridge - try - { - boost::shared_ptr tracked_object; - cv_bridge::CvImageConstPtr cv_ptr = cv_bridge::toCvShare(message, tracked_object, targetFormat); - - // Compress image - if (cv::imencode(".jpg", cv_ptr->image, compressed.data, params)) - { - - float cRatio = (float)(cv_ptr->image.rows * cv_ptr->image.cols * cv_ptr->image.elemSize()) - / (float)compressed.data.size(); - ROS_DEBUG("Compressed Image Transport - Codec: jpg, Compression Ratio: 1:%.2f (%lu bytes)", cRatio, compressed.data.size()); - } - else - { - ROS_ERROR("cv::imencode (jpeg) failed on input image"); - } - } - catch (cv_bridge::Exception& e) - { - ROS_ERROR("%s", e.what()); - } - catch (cv::Exception& e) - { - ROS_ERROR("%s", e.what()); - } - - // Publish message - publish_fn(compressed); - } - else - ROS_ERROR("Compressed Image Transport - JPEG compression requires 8/16-bit color format (input format is: %s)", message.encoding.c_str()); - + // Publish message + ROS_INFO("Encoding as JPEG"); + compressed = encodeImage(message, encodingFormat, params); break; } - // PNG Compression + + // PNG Compression case PNG: { - params.resize(3, 0); + params.resize(2, 0); params[0] = IMWRITE_PNG_COMPRESSION; params[1] = config_.png_level; - - // Update ros message format header - compressed.format += "; png compressed "; - - // Check input format - if ((bitDepth == 8) || (bitDepth == 16)) - { - - // Target image format - stringstream targetFormat; - if (enc::isColor(message.encoding)) - { - // convert color images to RGB domain - targetFormat << "bgr" << bitDepth; - compressed.format += targetFormat.str(); - } - - // OpenCV-ros bridge - try - { - boost::shared_ptr tracked_object; - cv_bridge::CvImageConstPtr cv_ptr = cv_bridge::toCvShare(message, tracked_object, targetFormat.str()); - - // Compress image - if (cv::imencode(".png", cv_ptr->image, compressed.data, params)) - { - - float cRatio = (float)(cv_ptr->image.rows * cv_ptr->image.cols * cv_ptr->image.elemSize()) - / (float)compressed.data.size(); - ROS_DEBUG("Compressed Image Transport - Codec: png, Compression Ratio: 1:%.2f (%lu bytes)", cRatio, compressed.data.size()); - } - else - { - ROS_ERROR("cv::imencode (png) failed on input image"); - } - } - catch (cv_bridge::Exception& e) - { - ROS_ERROR("%s", e.what()); - return; - } - catch (cv::Exception& e) - { - ROS_ERROR("%s", e.what()); - return; - } - - // Publish message - publish_fn(compressed); - } - else - ROS_ERROR("Compressed Image Transport - PNG compression requires 8/16-bit encoded color format (input format is: %s)", message.encoding.c_str()); + ROS_INFO("Encoding as PNG"); + compressed = encodeImage(message, encodingFormat, params); break; } - default: ROS_ERROR("Unknown compression type '%s', valid options are 'jpeg' and 'png'", config_.format.c_str()); break; } - + // Publish message + if (compressed != NULL) + publish_fn(*compressed); + else + ROS_ERROR("Compressing image failed. Check your configuration."); } } //namespace compressed_image_transport diff --git a/compressed_image_transport/src/compression_common.cpp b/compressed_image_transport/src/compression_common.cpp index 45a9c62..89fa727 100644 --- a/compressed_image_transport/src/compression_common.cpp +++ b/compressed_image_transport/src/compression_common.cpp @@ -37,6 +37,7 @@ #include #include #include +#include namespace enc = sensor_msgs::image_encodings; @@ -113,4 +114,133 @@ namespace compressed_image_transport { throw std::runtime_error(ss.str()); } } -} \ No newline at end of file + + sensor_msgs::CompressedImageConstPtr encodeImage(const sensor_msgs::Image &message, compressionFormat encode_flag, std::vector params) { + + boost::shared_ptr compressed(new sensor_msgs::CompressedImage); + compressed->header = message.header; + compressed->format = message.encoding; + + // Bit depth of image encoding + int bitDepth = enc::bitDepth(message.encoding); + int numChannels = enc::numChannels(message.encoding); + + switch (encode_flag) { + case JPEG: + { + if( params.size()!= 8){ + std::ostringstream err; + err<<"Encoding image using JPEG called with wrong number of parameters." + <<" Got "<format += "; jpeg compressed "; + + // Check input format + if ((bitDepth == 8) || (bitDepth == 16)) { + // Target image format + std::string targetFormat; + if (enc::isColor(message.encoding)) { + // convert color images to BGR8 format + targetFormat = "bgr8"; + compressed->format += targetFormat; + } + + // OpenCV-ros bridge + try { + boost::shared_ptr tracked_object; + cv_bridge::CvImageConstPtr cv_ptr = cv_bridge::toCvShare(message, tracked_object, targetFormat); + + // Compress image + if (cv::imencode(".jpg", cv_ptr->image, compressed->data, params)) { + + float cRatio = (float) (cv_ptr->image.rows * cv_ptr->image.cols * cv_ptr->image.elemSize()) + / (float) compressed->data.size(); + ROS_DEBUG("Compressed Image Transport - Codec: jpg, Compression Ratio: 1:%.2f (%lu bytes)", cRatio, + compressed->data.size()); + } else { + ROS_ERROR("cv::imencode (jpeg) failed on input image"); + } + } + catch (cv_bridge::Exception &e) { + ROS_ERROR("%s", e.what()); + return NULL; + } + catch (cv::Exception &e) { + ROS_ERROR("%s", e.what()); + return NULL; + } + return compressed; + } + else + ROS_ERROR("Compressed Image Transport - JPEG compression requires 8/16-bit color format (input format is: %s)", message.encoding.c_str()); + break; + } + case PNG: + { + if( params.size() != 2){ + std::ostringstream err; + err<<"Encoding image using PNG called with wrong number of parameters." + <<" Got "<format += "; png compressed "; + + // Check input format + if ((bitDepth == 8) || (bitDepth == 16)) + { + + // Target image format + std::ostringstream targetFormat; + if (enc::isColor(message.encoding)) + { + // convert color images to RGB domain + targetFormat << "bgr" << bitDepth; + compressed->format += targetFormat.str(); + } + + // OpenCV-ros bridge + try + { + boost::shared_ptr tracked_object; + cv_bridge::CvImageConstPtr cv_ptr = cv_bridge::toCvShare(message, tracked_object, targetFormat.str()); + + // Compress image + if (cv::imencode(".png", cv_ptr->image, compressed->data, params)) + { + + float cRatio = (float)(cv_ptr->image.rows * cv_ptr->image.cols * cv_ptr->image.elemSize()) + / (float)compressed->data.size(); + ROS_DEBUG("Compressed Image Transport - Codec: png, Compression Ratio: 1:%.2f (%lu bytes)", cRatio, compressed->data.size()); + } + else + { + ROS_ERROR("cv::imencode (png) failed on input image"); + } + } + catch (cv_bridge::Exception& e) + { + ROS_ERROR("%s", e.what()); + return NULL; + } + catch (cv::Exception& e) + { + ROS_ERROR("%s", e.what()); + return NULL; + } + + // Publish message + return compressed; + } + else + ROS_ERROR("Compressed Image Transport - PNG compression requires 8/16-bit encoded color format (input format is: %s)", message.encoding.c_str()); + break; + } + default: + ROS_ERROR("Unknown compression type, valid options are 'jpeg(0)' and 'png(1)'"); + break; + } + } + +} From 8eb6d935b096601589ab142d261b50c99dc1753a Mon Sep 17 00:00:00 2001 From: Ferenc Balint-Benczedi Date: Tue, 31 Mar 2020 14:41:39 +0200 Subject: [PATCH 05/10] remove output --- compressed_image_transport/src/compressed_publisher.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/compressed_image_transport/src/compressed_publisher.cpp b/compressed_image_transport/src/compressed_publisher.cpp index 8460d3a..a37a88c 100644 --- a/compressed_image_transport/src/compressed_publisher.cpp +++ b/compressed_image_transport/src/compressed_publisher.cpp @@ -104,7 +104,6 @@ void CompressedPublisher::publish(const sensor_msgs::Image& message, const Publi params[7] = config_.jpeg_restart_interval; // Publish message - ROS_INFO("Encoding as JPEG"); compressed = encodeImage(message, encodingFormat, params); break; } @@ -115,7 +114,6 @@ void CompressedPublisher::publish(const sensor_msgs::Image& message, const Publi params.resize(2, 0); params[0] = IMWRITE_PNG_COMPRESSION; params[1] = config_.png_level; - ROS_INFO("Encoding as PNG"); compressed = encodeImage(message, encodingFormat, params); break; } From ab5085265e7d6c23379ffd66c59d1a38ec750336 Mon Sep 17 00:00:00 2001 From: Ferenc Balint-Benczedi Date: Tue, 31 Mar 2020 15:27:24 +0200 Subject: [PATCH 06/10] export compression library for other packages to see it --- compressed_image_transport/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/compressed_image_transport/CMakeLists.txt b/compressed_image_transport/CMakeLists.txt index 2d57f6c..672add8 100644 --- a/compressed_image_transport/CMakeLists.txt +++ b/compressed_image_transport/CMakeLists.txt @@ -9,7 +9,7 @@ generate_dynamic_reconfigure_options(cfg/CompressedPublisher.cfg cfg/CompressedS catkin_package( INCLUDE_DIRS include - LIBRARIES ${PROJECT_NAME} + LIBRARIES ${PROJECT_NAME} ${PROJECT_NAME}_compression CATKIN_DEPENDS cv_bridge dynamic_reconfigure image_transport DEPENDS OpenCV ) From af3279a1c350f0d214651bd7666e012d586b0847 Mon Sep 17 00:00:00 2001 From: Ferenc Balint-Benczedi Date: Tue, 31 Mar 2020 16:15:20 +0200 Subject: [PATCH 07/10] no const and don't force number of params for encoding, leave option for using default values --- .../compression_common.h | 12 +++++++++--- .../src/compressed_publisher.cpp | 2 +- .../src/compression_common.cpp | 14 +------------- 3 files changed, 11 insertions(+), 17 deletions(-) diff --git a/compressed_image_transport/include/compressed_image_transport/compression_common.h b/compressed_image_transport/include/compressed_image_transport/compression_common.h index 343d06f..42bdb59 100644 --- a/compressed_image_transport/include/compressed_image_transport/compression_common.h +++ b/compressed_image_transport/include/compressed_image_transport/compression_common.h @@ -50,9 +50,15 @@ enum compressionFormat // standadlone decoding function sensor_msgs::ImagePtr decodeCompressedImage(const sensor_msgs::CompressedImageConstPtr& image, int decode_flag); -// standadlone encoding function -// for setting the correct parameters take look at compressed_publisher.cpp -sensor_msgs::CompressedImageConstPtr encodeImage(const sensor_msgs::Image &message, compressionFormat encode_flag, std::vector params); + +/** + * @brief encodeImage standadlone encoding function wrapping around cv::imencode for compressin sensor_msgs::Image messages + * @param iamge the image message to encode + * @param encode_flag one of compressionFormat::JPEG or compressionFormat::PNG + * @param params Format-specific parameters. See cv::imwrite and cv::ImwriteFlags. + * @return + */ +sensor_msgs::CompressedImagePtr encodeImage(const sensor_msgs::Image &image, compressionFormat encode_flag, std::vector params = std::vector()); } //namespace compressed_image_transport diff --git a/compressed_image_transport/src/compressed_publisher.cpp b/compressed_image_transport/src/compressed_publisher.cpp index a37a88c..b410c28 100644 --- a/compressed_image_transport/src/compressed_publisher.cpp +++ b/compressed_image_transport/src/compressed_publisher.cpp @@ -87,7 +87,7 @@ void CompressedPublisher::publish(const sensor_msgs::Image& message, const Publi // Compression settings std::vector params; - sensor_msgs::CompressedImageConstPtr compressed; + sensor_msgs::CompressedImagePtr compressed; switch (encodingFormat) { // JPEG Compression diff --git a/compressed_image_transport/src/compression_common.cpp b/compressed_image_transport/src/compression_common.cpp index 89fa727..04cfcc4 100644 --- a/compressed_image_transport/src/compression_common.cpp +++ b/compressed_image_transport/src/compression_common.cpp @@ -115,7 +115,7 @@ namespace compressed_image_transport { } } - sensor_msgs::CompressedImageConstPtr encodeImage(const sensor_msgs::Image &message, compressionFormat encode_flag, std::vector params) { + sensor_msgs::CompressedImagePtr encodeImage(const sensor_msgs::Image &message, compressionFormat encode_flag, std::vector params) { boost::shared_ptr compressed(new sensor_msgs::CompressedImage); compressed->header = message.header; @@ -128,12 +128,6 @@ namespace compressed_image_transport { switch (encode_flag) { case JPEG: { - if( params.size()!= 8){ - std::ostringstream err; - err<<"Encoding image using JPEG called with wrong number of parameters." - <<" Got "<format += "; jpeg compressed "; // Check input format @@ -178,12 +172,6 @@ namespace compressed_image_transport { } case PNG: { - if( params.size() != 2){ - std::ostringstream err; - err<<"Encoding image using PNG called with wrong number of parameters." - <<" Got "<format += "; png compressed "; From 6ebb3b6908973ddbca466899435d6d7b62576646 Mon Sep 17 00:00:00 2001 From: Ferenc Balint-Benczedi Date: Tue, 31 Mar 2020 20:47:17 +0200 Subject: [PATCH 08/10] fix for 16.04 --- compressed_image_transport/src/compressed_publisher.cpp | 6 ++---- compressed_image_transport/src/compression_common.cpp | 5 +---- 2 files changed, 3 insertions(+), 8 deletions(-) diff --git a/compressed_image_transport/src/compressed_publisher.cpp b/compressed_image_transport/src/compressed_publisher.cpp index b410c28..ec0816e 100644 --- a/compressed_image_transport/src/compressed_publisher.cpp +++ b/compressed_image_transport/src/compressed_publisher.cpp @@ -122,10 +122,8 @@ void CompressedPublisher::publish(const sensor_msgs::Image& message, const Publi break; } // Publish message - if (compressed != NULL) - publish_fn(*compressed); - else - ROS_ERROR("Compressing image failed. Check your configuration."); + publish_fn(*compressed); + } } //namespace compressed_image_transport diff --git a/compressed_image_transport/src/compression_common.cpp b/compressed_image_transport/src/compression_common.cpp index 04cfcc4..526918d 100644 --- a/compressed_image_transport/src/compression_common.cpp +++ b/compressed_image_transport/src/compression_common.cpp @@ -158,11 +158,9 @@ namespace compressed_image_transport { } catch (cv_bridge::Exception &e) { ROS_ERROR("%s", e.what()); - return NULL; } catch (cv::Exception &e) { ROS_ERROR("%s", e.what()); - return NULL; } return compressed; } @@ -210,12 +208,10 @@ namespace compressed_image_transport { catch (cv_bridge::Exception& e) { ROS_ERROR("%s", e.what()); - return NULL; } catch (cv::Exception& e) { ROS_ERROR("%s", e.what()); - return NULL; } // Publish message @@ -229,6 +225,7 @@ namespace compressed_image_transport { ROS_ERROR("Unknown compression type, valid options are 'jpeg(0)' and 'png(1)'"); break; } + return compressed; } } From 49e2b79e5fc91d8ab4f4fb194edda01a91adb078 Mon Sep 17 00:00:00 2001 From: Ferenc Balint-Benczedi Date: Tue, 31 Mar 2020 21:07:19 +0200 Subject: [PATCH 09/10] change indent --- .../src/compression_common.cpp | 51 +++++++------------ 1 file changed, 19 insertions(+), 32 deletions(-) diff --git a/compressed_image_transport/src/compression_common.cpp b/compressed_image_transport/src/compression_common.cpp index 526918d..65b8f32 100644 --- a/compressed_image_transport/src/compression_common.cpp +++ b/compressed_image_transport/src/compression_common.cpp @@ -120,7 +120,6 @@ namespace compressed_image_transport { boost::shared_ptr compressed(new sensor_msgs::CompressedImage); compressed->header = message.header; compressed->format = message.encoding; - // Bit depth of image encoding int bitDepth = enc::bitDepth(message.encoding); int numChannels = enc::numChannels(message.encoding); @@ -129,7 +128,6 @@ namespace compressed_image_transport { case JPEG: { compressed->format += "; jpeg compressed "; - // Check input format if ((bitDepth == 8) || (bitDepth == 16)) { // Target image format @@ -155,75 +153,64 @@ namespace compressed_image_transport { } else { ROS_ERROR("cv::imencode (jpeg) failed on input image"); } - } - catch (cv_bridge::Exception &e) { + } catch (cv_bridge::Exception &e) { ROS_ERROR("%s", e.what()); - } - catch (cv::Exception &e) { + } catch (cv::Exception &e) { ROS_ERROR("%s", e.what()); } return compressed; - } - else + } else { ROS_ERROR("Compressed Image Transport - JPEG compression requires 8/16-bit color format (input format is: %s)", message.encoding.c_str()); - break; + break; + } } case PNG: { // Update ros message format header compressed->format += "; png compressed "; - // Check input format - if ((bitDepth == 8) || (bitDepth == 16)) - { + if ((bitDepth == 8) || (bitDepth == 16)) { // Target image format std::ostringstream targetFormat; - if (enc::isColor(message.encoding)) - { + if (enc::isColor(message.encoding)) { // convert color images to RGB domain targetFormat << "bgr" << bitDepth; compressed->format += targetFormat.str(); } // OpenCV-ros bridge - try - { + try { boost::shared_ptr tracked_object; cv_bridge::CvImageConstPtr cv_ptr = cv_bridge::toCvShare(message, tracked_object, targetFormat.str()); // Compress image - if (cv::imencode(".png", cv_ptr->image, compressed->data, params)) - { + if (cv::imencode(".png", cv_ptr->image, compressed->data, params)) { float cRatio = (float)(cv_ptr->image.rows * cv_ptr->image.cols * cv_ptr->image.elemSize()) / (float)compressed->data.size(); ROS_DEBUG("Compressed Image Transport - Codec: png, Compression Ratio: 1:%.2f (%lu bytes)", cRatio, compressed->data.size()); - } - else - { + } else { ROS_ERROR("cv::imencode (png) failed on input image"); } - } - catch (cv_bridge::Exception& e) - { + } catch (cv_bridge::Exception& e) { ROS_ERROR("%s", e.what()); - } - catch (cv::Exception& e) - { + } catch (cv::Exception& e) { ROS_ERROR("%s", e.what()); } // Publish message return compressed; - } - else + } else { ROS_ERROR("Compressed Image Transport - PNG compression requires 8/16-bit encoded color format (input format is: %s)", message.encoding.c_str()); - break; + break; + } } default: - ROS_ERROR("Unknown compression type, valid options are 'jpeg(0)' and 'png(1)'"); - break; + { + ROS_ERROR("Unknown compression type, valid options are 'jpeg(0)' and 'png(1)'"); + break; + } } return compressed; } From a91806b13e1d6d8c21b6dc3b7479a8512e2bfad6 Mon Sep 17 00:00:00 2001 From: Ferenc Balint-Benczedi Date: Mon, 20 Apr 2020 13:51:13 +0200 Subject: [PATCH 10/10] throw exceptions in compression_common instead of ROS_ERRORS and catch them in publisher --- .../src/compressed_publisher.cpp | 26 +++++++- .../src/compression_common.cpp | 63 +++++++------------ 2 files changed, 48 insertions(+), 41 deletions(-) diff --git a/compressed_image_transport/src/compressed_publisher.cpp b/compressed_image_transport/src/compressed_publisher.cpp index ec0816e..69ace21 100644 --- a/compressed_image_transport/src/compressed_publisher.cpp +++ b/compressed_image_transport/src/compressed_publisher.cpp @@ -104,7 +104,18 @@ void CompressedPublisher::publish(const sensor_msgs::Image& message, const Publi params[7] = config_.jpeg_restart_interval; // Publish message - compressed = encodeImage(message, encodingFormat, params); + try { + compressed = encodeImage(message, encodingFormat, params); + } + catch (cv_bridge::Exception& e) { + ROS_ERROR("%s", e.what()); + } + catch (cv::Exception& e) { + ROS_ERROR("%s", e.what()); + } + catch (std::runtime_error& e) { + ROS_ERROR("%s", e.what()); + } break; } @@ -114,7 +125,18 @@ void CompressedPublisher::publish(const sensor_msgs::Image& message, const Publi params.resize(2, 0); params[0] = IMWRITE_PNG_COMPRESSION; params[1] = config_.png_level; - compressed = encodeImage(message, encodingFormat, params); + try { + compressed = encodeImage(message, encodingFormat, params); + } + catch (cv_bridge::Exception& e) { + ROS_ERROR("%s", e.what()); + } + catch (cv::Exception& e) { + ROS_ERROR("%s", e.what()); + } + catch (std::runtime_error& e) { + ROS_ERROR("%s", e.what()); + } break; } default: diff --git a/compressed_image_transport/src/compression_common.cpp b/compressed_image_transport/src/compression_common.cpp index 65b8f32..a2bcf4a 100644 --- a/compressed_image_transport/src/compression_common.cpp +++ b/compressed_image_transport/src/compression_common.cpp @@ -139,29 +139,23 @@ namespace compressed_image_transport { } // OpenCV-ros bridge - try { - boost::shared_ptr tracked_object; - cv_bridge::CvImageConstPtr cv_ptr = cv_bridge::toCvShare(message, tracked_object, targetFormat); - - // Compress image - if (cv::imencode(".jpg", cv_ptr->image, compressed->data, params)) { - - float cRatio = (float) (cv_ptr->image.rows * cv_ptr->image.cols * cv_ptr->image.elemSize()) - / (float) compressed->data.size(); - ROS_DEBUG("Compressed Image Transport - Codec: jpg, Compression Ratio: 1:%.2f (%lu bytes)", cRatio, - compressed->data.size()); - } else { - ROS_ERROR("cv::imencode (jpeg) failed on input image"); - } - } catch (cv_bridge::Exception &e) { - ROS_ERROR("%s", e.what()); - } catch (cv::Exception &e) { - ROS_ERROR("%s", e.what()); + boost::shared_ptr tracked_object; + cv_bridge::CvImageConstPtr cv_ptr = cv_bridge::toCvShare(message, tracked_object, targetFormat); + + // Compress image + if (cv::imencode(".jpg", cv_ptr->image, compressed->data, params)) { + + float cRatio = (float) (cv_ptr->image.rows * cv_ptr->image.cols * cv_ptr->image.elemSize()) + / (float) compressed->data.size(); + ROS_DEBUG("Compressed Image Transport - Codec: jpg, Compression Ratio: 1:%.2f (%lu bytes)", cRatio, + compressed->data.size()); + } else { + throw std::runtime_error("cv::imencode (jpeg) failed on input image"); } return compressed; } else { - ROS_ERROR("Compressed Image Transport - JPEG compression requires 8/16-bit color format (input format is: %s)", message.encoding.c_str()); - break; + throw std::runtime_error("Compressed Image Transport - JPEG compression requires 8/16-bit color format (input format is: "+ message.encoding + + ")"); } } case PNG: @@ -179,36 +173,27 @@ namespace compressed_image_transport { compressed->format += targetFormat.str(); } - // OpenCV-ros bridge - try { - boost::shared_ptr tracked_object; - cv_bridge::CvImageConstPtr cv_ptr = cv_bridge::toCvShare(message, tracked_object, targetFormat.str()); + boost::shared_ptr tracked_object; + cv_bridge::CvImageConstPtr cv_ptr = cv_bridge::toCvShare(message, tracked_object, targetFormat.str()); - // Compress image - if (cv::imencode(".png", cv_ptr->image, compressed->data, params)) { + // Compress image + if (cv::imencode(".png", cv_ptr->image, compressed->data, params)) { - float cRatio = (float)(cv_ptr->image.rows * cv_ptr->image.cols * cv_ptr->image.elemSize()) - / (float)compressed->data.size(); - ROS_DEBUG("Compressed Image Transport - Codec: png, Compression Ratio: 1:%.2f (%lu bytes)", cRatio, compressed->data.size()); - } else { - ROS_ERROR("cv::imencode (png) failed on input image"); - } - } catch (cv_bridge::Exception& e) { - ROS_ERROR("%s", e.what()); - } catch (cv::Exception& e) { - ROS_ERROR("%s", e.what()); + float cRatio = (float)(cv_ptr->image.rows * cv_ptr->image.cols * cv_ptr->image.elemSize())/ (float)compressed->data.size(); + ROS_DEBUG("Compressed Image Transport - Codec: png, Compression Ratio: 1:%.2f (%lu bytes)", cRatio, compressed->data.size()); + } else { + throw std::runtime_error("cv::imencode (png) failed on input image"); } - // Publish message return compressed; } else { - ROS_ERROR("Compressed Image Transport - PNG compression requires 8/16-bit encoded color format (input format is: %s)", message.encoding.c_str()); + throw std::runtime_error("Compressed Image Transport - PNG compression requires 8/16-bit encoded color format (input format is: "+ message.encoding +")"); break; } } default: { - ROS_ERROR("Unknown compression type, valid options are 'jpeg(0)' and 'png(1)'"); + throw std::runtime_error("Unknown compression type, valid options are 'jpeg(0)' and 'png(1)'"); break; } }