compressed_publisher.cpp
Go to the documentation of this file.
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 20012, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Willow Garage nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
36 #include <cv_bridge/cv_bridge.h>
38 #include <opencv2/highgui/highgui.hpp>
39 #include <opencv2/imgcodecs.hpp>
40 #include <boost/make_shared.hpp>
41 
43 
44 #include <vector>
45 #include <sstream>
46 
47 // If OpenCV4
48 #if CV_VERSION_MAJOR > 3
49 #include <opencv2/imgcodecs/legacy/constants_c.h>
50 #endif
51 
52 using namespace cv;
53 using namespace std;
54 
56 
58 {
59 
60 void CompressedPublisher::advertiseImpl(ros::NodeHandle &nh, const std::string &base_topic, uint32_t queue_size,
61  const image_transport::SubscriberStatusCallback &user_connect_cb,
62  const image_transport::SubscriberStatusCallback &user_disconnect_cb,
63  const ros::VoidPtr &tracked_object, bool latch)
64 {
66  Base::advertiseImpl(nh, base_topic, queue_size, user_connect_cb, user_disconnect_cb, tracked_object, latch);
67 
68  // Set up reconfigure server for this topic
69  reconfigure_server_ = boost::make_shared<ReconfigureServer>(this->nh());
70  ReconfigureServer::CallbackType f = boost::bind(&CompressedPublisher::configCb, this, _1, _2);
71  reconfigure_server_->setCallback(f);
72 }
73 
74 void CompressedPublisher::configCb(Config& config, uint32_t level)
75 {
76  config_ = config;
77 }
78 
79 void CompressedPublisher::publish(const sensor_msgs::Image& message, const PublishFn& publish_fn) const
80 {
81  // Compressed image message
82  sensor_msgs::CompressedImage compressed;
83  compressed.header = message.header;
84  compressed.format = message.encoding;
85 
86  // Compression settings
87  std::vector<int> params;
88 
89  // Get codec configuration
90  compressionFormat encodingFormat = UNDEFINED;
91  if (config_.format == compressed_image_transport::CompressedPublisher_jpeg)
92  encodingFormat = JPEG;
93  if (config_.format == compressed_image_transport::CompressedPublisher_png)
94  encodingFormat = PNG;
95 
96  // Bit depth of image encoding
97  int bitDepth = enc::bitDepth(message.encoding);
98  int numChannels = enc::numChannels(message.encoding);
99 
100  switch (encodingFormat)
101  {
102  // JPEG Compression
103  case JPEG:
104  {
105  params.resize(9, 0);
106  params[0] = IMWRITE_JPEG_QUALITY;
107  params[1] = config_.jpeg_quality;
108  params[2] = IMWRITE_JPEG_PROGRESSIVE;
109  params[3] = config_.jpeg_progressive ? 1 : 0;
110  params[4] = IMWRITE_JPEG_OPTIMIZE;
111  params[5] = config_.jpeg_optimize ? 1 : 0;
112  params[6] = IMWRITE_JPEG_RST_INTERVAL;
113  params[7] = config_.jpeg_restart_interval;
114 
115  // Update ros message format header
116  compressed.format += "; jpeg compressed ";
117 
118  // Check input format
119  if ((bitDepth == 8) || (bitDepth == 16))
120  {
121  // Target image format
122  std::string targetFormat;
123  if (enc::isColor(message.encoding))
124  {
125  // convert color images to BGR8 format
126  targetFormat = "bgr8";
127  compressed.format += targetFormat;
128  }
129 
130  // OpenCV-ros bridge
131  try
132  {
134  cv_bridge::CvImageConstPtr cv_ptr = cv_bridge::toCvShare(message, tracked_object, targetFormat);
135 
136  // Compress image
137  if (cv::imencode(".jpg", cv_ptr->image, compressed.data, params))
138  {
139 
140  float cRatio = (float)(cv_ptr->image.rows * cv_ptr->image.cols * cv_ptr->image.elemSize())
141  / (float)compressed.data.size();
142  ROS_DEBUG("Compressed Image Transport - Codec: jpg, Compression Ratio: 1:%.2f (%lu bytes)", cRatio, compressed.data.size());
143  }
144  else
145  {
146  ROS_ERROR("cv::imencode (jpeg) failed on input image");
147  }
148  }
149  catch (cv_bridge::Exception& e)
150  {
151  ROS_ERROR("%s", e.what());
152  }
153  catch (cv::Exception& e)
154  {
155  ROS_ERROR("%s", e.what());
156  }
157 
158  // Publish message
159  publish_fn(compressed);
160  }
161  else
162  ROS_ERROR("Compressed Image Transport - JPEG compression requires 8/16-bit color format (input format is: %s)", message.encoding.c_str());
163 
164  break;
165  }
166  // PNG Compression
167  case PNG:
168  {
169  params.resize(3, 0);
170  params[0] = IMWRITE_PNG_COMPRESSION;
171  params[1] = config_.png_level;
172 
173  // Update ros message format header
174  compressed.format += "; png compressed ";
175 
176  // Check input format
177  if ((bitDepth == 8) || (bitDepth == 16))
178  {
179 
180  // Target image format
181  stringstream targetFormat;
182  if (enc::isColor(message.encoding))
183  {
184  // convert color images to RGB domain
185  targetFormat << "bgr" << bitDepth;
186  compressed.format += targetFormat.str();
187  }
188 
189  // OpenCV-ros bridge
190  try
191  {
193  cv_bridge::CvImageConstPtr cv_ptr = cv_bridge::toCvShare(message, tracked_object, targetFormat.str());
194 
195  // Compress image
196  if (cv::imencode(".png", cv_ptr->image, compressed.data, params))
197  {
198 
199  float cRatio = (float)(cv_ptr->image.rows * cv_ptr->image.cols * cv_ptr->image.elemSize())
200  / (float)compressed.data.size();
201  ROS_DEBUG("Compressed Image Transport - Codec: png, Compression Ratio: 1:%.2f (%lu bytes)", cRatio, compressed.data.size());
202  }
203  else
204  {
205  ROS_ERROR("cv::imencode (png) failed on input image");
206  }
207  }
208  catch (cv_bridge::Exception& e)
209  {
210  ROS_ERROR("%s", e.what());
211  return;
212  }
213  catch (cv::Exception& e)
214  {
215  ROS_ERROR("%s", e.what());
216  return;
217  }
218 
219  // Publish message
220  publish_fn(compressed);
221  }
222  else
223  ROS_ERROR("Compressed Image Transport - PNG compression requires 8/16-bit encoded color format (input format is: %s)", message.encoding.c_str());
224  break;
225  }
226 
227  default:
228  ROS_ERROR("Unknown compression type '%s', valid options are 'jpeg' and 'png'", config_.format.c_str());
229  break;
230  }
231 
232 }
233 
234 } //namespace compressed_image_transport
CvImageConstPtr toCvShare(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
f
boost::function< void(const SingleSubscriberPublisher &)> SubscriberStatusCallback
boost::function< void(const sensor_msgs::CompressedImage &)> PublishFn
compressed_image_transport::CompressedPublisherConfig Config
JPEG
#define ROS_ERROR(...)
#define ROS_DEBUG(...)
PNG


compressed_image_transport
Author(s): Patrick Mihelich, Julius Kammerl
autogenerated on Tue Jul 2 2019 19:10:13