38 #include <opencv2/highgui/highgui.hpp>
39 #include <opencv2/imgcodecs.hpp>
40 #include <boost/make_shared.hpp>
48 #if CV_VERSION_MAJOR > 3
49 #include <opencv2/imgcodecs/legacy/constants_c.h>
60 void CompressedPublisher::advertiseImpl(
ros::NodeHandle &nh,
const std::string &base_topic, uint32_t queue_size,
66 Base::advertiseImpl(nh, base_topic, queue_size, user_connect_cb, user_disconnect_cb, tracked_object, latch);
69 reconfigure_server_ = boost::make_shared<ReconfigureServer>(this->nh());
70 ReconfigureServer::CallbackType
f = boost::bind(&CompressedPublisher::configCb,
this, boost::placeholders::_1, boost::placeholders::_2);
71 reconfigure_server_->setCallback(
f);
74 void CompressedPublisher::configCb(
Config& config, uint32_t level)
79 void CompressedPublisher::publish(
const sensor_msgs::Image& message,
const PublishFn& publish_fn)
const
81 if (!config_.enable) {
86 sensor_msgs::CompressedImage compressed;
87 compressed.header = message.header;
88 compressed.format = message.encoding;
91 std::vector<int> params;
95 if (config_.format == compressed_image_transport::CompressedPublisher_jpeg)
96 encodingFormat =
JPEG;
97 if (config_.format == compressed_image_transport::CompressedPublisher_png)
101 int bitDepth = enc::bitDepth(message.encoding);
102 int numChannels = enc::numChannels(message.encoding);
104 switch (encodingFormat)
110 params.emplace_back(IMWRITE_JPEG_QUALITY);
111 params.emplace_back(config_.jpeg_quality);
112 params.emplace_back(IMWRITE_JPEG_PROGRESSIVE);
113 params.emplace_back(config_.jpeg_progressive ? 1 : 0);
114 params.emplace_back(IMWRITE_JPEG_OPTIMIZE);
115 params.emplace_back(config_.jpeg_optimize ? 1 : 0);
116 params.emplace_back(IMWRITE_JPEG_RST_INTERVAL);
117 params.emplace_back(config_.jpeg_restart_interval);
120 compressed.format +=
"; jpeg compressed ";
123 if ((bitDepth == 8) || (bitDepth == 16))
126 std::string targetFormat;
127 if (enc::isColor(message.encoding))
130 targetFormat =
"bgr8";
131 compressed.format += targetFormat;
141 if (cv::imencode(
".jpg", cv_ptr->image, compressed.data, params))
144 float cRatio = (float)(cv_ptr->image.rows * cv_ptr->image.cols * cv_ptr->image.elemSize())
145 / (
float)compressed.data.size();
146 ROS_DEBUG(
"Compressed Image Transport - Codec: jpg, Compression Ratio: 1:%.2f (%lu bytes)", cRatio, compressed.data.size());
150 ROS_ERROR(
"cv::imencode (jpeg) failed on input image");
157 catch (cv::Exception& e)
163 publish_fn(compressed);
166 ROS_ERROR(
"Compressed Image Transport - JPEG compression requires 8/16-bit color format (input format is: %s)", message.encoding.c_str());
174 params.emplace_back(IMWRITE_PNG_COMPRESSION);
175 params.emplace_back(config_.png_level);
178 compressed.format +=
"; png compressed ";
181 if ((bitDepth == 8) || (bitDepth == 16))
185 stringstream targetFormat;
186 if (enc::isColor(message.encoding))
189 targetFormat <<
"bgr" << bitDepth;
190 compressed.format += targetFormat.str();
200 if (cv::imencode(
".png", cv_ptr->image, compressed.data, params))
203 float cRatio = (float)(cv_ptr->image.rows * cv_ptr->image.cols * cv_ptr->image.elemSize())
204 / (
float)compressed.data.size();
205 ROS_DEBUG(
"Compressed Image Transport - Codec: png, Compression Ratio: 1:%.2f (%lu bytes)", cRatio, compressed.data.size());
209 ROS_ERROR(
"cv::imencode (png) failed on input image");
217 catch (cv::Exception& e)
224 publish_fn(compressed);
227 ROS_ERROR(
"Compressed Image Transport - PNG compression requires 8/16-bit encoded color format (input format is: %s)", message.encoding.c_str());
232 ROS_ERROR(
"Unknown compression type '%s', valid options are 'jpeg' and 'png'", config_.format.c_str());