ros_support.cpp
Go to the documentation of this file.
1 // *****************************************************************************
2 //
3 // Copyright (c) 2016-2017, Southwest Research Institute® (SwRI®)
4 // All rights reserved.
5 //
6 // Redistribution and use in source and binary forms, with or without
7 // modification, are permitted provided that the following conditions are met:
8 // * Redistributions of source code must retain the above copyright
9 // notice, this list of conditions and the following disclaimer.
10 // * Redistributions in binary form must reproduce the above copyright
11 // notice, this list of conditions and the following disclaimer in the
12 // documentation and/or other materials provided with the distribution.
13 // * Neither the name of Southwest Research Institute® (SwRI®) nor the
14 // names of its contributors may be used to endorse or promote products
15 // derived from this software without specific prior written permission.
16 //
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20 // ARE DISCLAIMED. IN NO EVENT SHALL Southwest Research Institute® BE LIABLE
21 // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
22 // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
23 // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
24 // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
25 // LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
26 // OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
27 // DAMAGE.
28 //
29 // *****************************************************************************
30 
31 #include <cv_bridge/cv_bridge.h>
32 #include <opencv2/highgui/highgui.hpp>
33 #include <boost/make_shared.hpp>
35 #include <ros/ros.h>
37 
38 using namespace cv;
39 using namespace std;
40 
52 
53 namespace IZ
54 {
55  sensor_msgs::CompressedImage compressImage(const sensor_msgs::Image& image)
56  {
60  }
61 
62  // Compressed image message
63  sensor_msgs::CompressedImage compressed;
64  compressed.header = image.header;
65  compressed.format = image.encoding;
66 
67  // Compression settings
68  std::vector<int> params;
69 
70  // Bit depth of image encoding
71  int bitDepth = enc::bitDepth(image.encoding);
72 
73  params.push_back(cv::IMWRITE_PXM_BINARY);
74  params.push_back(1);
75 
76  // Update ros message format header
77  compressed.format += "; iz compressed ";
78 
79  // Check input format
80  if ((bitDepth == 8) || (bitDepth == 16))
81  {
82 
83  // Target image format
84  stringstream targetFormat;
85  if (enc::isColor(image.encoding))
86  {
87  // convert color images to RGB domain
88  targetFormat << "bgr" << bitDepth;
89  compressed.format += targetFormat.str();
90  }
91 
92  // OpenCV-ros bridge
93  try
94  {
96  cv_bridge::CvImageConstPtr cv_ptr = cv_bridge::toCvShare(image, tracked_object, targetFormat.str());
97 
99 
100  // Compress image
101  std::vector<unsigned char> ppm_buffer;
102  ros::Time now = ros::Time::now();
103  if (cv::imencode(".ppm", cv_ptr->image, ppm_buffer, params))
104  {
105  if (!pi.readHeader((const unsigned char*) &ppm_buffer[0]))
106  {
107  std::stringstream ss;
108  for (int i = 0; i < 20; i++)
109  {
110  ss << ppm_buffer[i];
111  }
112  ROS_ERROR("Unable to read PPM produced by OpenCV. First few bytes: %s", ss.str().c_str());
113  return sensor_msgs::CompressedImage();
114  }
115  if (pi.components() != 3)
116  {
117  ROS_ERROR("Can only handle 24-bit PPM files.");
118  return sensor_msgs::CompressedImage();
119  }
120  unsigned char *dest = static_cast<unsigned char*>(malloc(pi.height() * pi.width() * 4 + 33));
121  //IZ::initEncodeTable();
122  unsigned char *destEnd = IZ::encodeImage(pi, dest);
123  size_t size = destEnd - dest;
124  compressed.data.resize(size);
125  memcpy(&compressed.data[0], dest, size);
126  free(dest);
127  ros::Time iz_time = ros::Time::now();
128  ROS_DEBUG_THROTTLE(1, "Took %lums to compress %lu bytes to %lu bytes (%f ratio)",
129  (iz_time - now).toNSec()/1000000L,
130  image.data.size(),
131  compressed.data.size(),
132  (double)image.data.size() / (double)image.data.size());
133  }
134  else
135  {
136  ROS_ERROR("cv::imencode (ppm) failed on input image");
137  return sensor_msgs::CompressedImage();
138  }
139  }
140  catch (cv_bridge::Exception& e)
141  {
142  ROS_ERROR("%s", e.what());
143  return sensor_msgs::CompressedImage();
144  }
145  catch (cv::Exception& e)
146  {
147  ROS_ERROR("%s", e.what());
148  return sensor_msgs::CompressedImage();
149  }
150 
151  return compressed;
152  }
153  else
154  {
155  ROS_ERROR(
156  "Compressed Image Transport - ImageZero compression requires 8/16-bit encoded color format (input format is: %s)",
157  image.encoding.c_str());
158  }
159 
160  return sensor_msgs::CompressedImage();
161  }
162 
163  sensor_msgs::Image decompressImage(const sensor_msgs::CompressedImageConstPtr& compressed)
164  {
168  }
169 
171  //IZ::initDecodeTable();
172  IZ::decodeImageSize(pi, &compressed->data[0]);
173  pi.setComponents(3);
174  const unsigned int dataSize = pi.width() * pi.height() * pi.components();
175  unsigned char* dest = (unsigned char*) malloc(dataSize + 33);
176  pi.writeHeader(dest);
177  IZ::decodeImage(pi, &compressed->data[0]);
178 
179  std::vector<unsigned char> ppm_data;
180  size_t size = pi.data() - dest + dataSize;
181  ppm_data.resize(size);
182  memcpy(&ppm_data[0], dest, size);
183  free(dest);
184 
186 
187  // Copy compressed header
188  cv_ptr->header = compressed->header;
189 
190  // Decode color/mono image
191  try
192  {
193  cv_ptr->image = cv::imdecode(cv::Mat(ppm_data), cv::IMREAD_COLOR);
194 
195  // Assign image encoding string
196  const size_t split_pos = compressed->format.find(';');
197  if (split_pos == std::string::npos)
198  {
199  switch (cv_ptr->image.channels())
200  {
201  case 1:
202  cv_ptr->encoding = enc::MONO8;
203  break;
204  case 3:
205  cv_ptr->encoding = enc::BGR8;
206  break;
207  default:
208  ROS_ERROR("Unsupported number of channels: %i", cv_ptr->image.channels());
209  break;
210  }
211  }
212  else
213  {
214  std::string image_encoding = compressed->format.substr(0, split_pos);
215 
216  cv_ptr->encoding = image_encoding;
217 
218  if (enc::isColor(image_encoding))
219  {
220  std::string compressed_encoding = compressed->format.substr(split_pos);
221  bool compressed_bgr_image = (compressed_encoding.find("compressed bgr") != std::string::npos);
222 
223  // Revert color transformation
224  if (compressed_bgr_image)
225  {
226  // if necessary convert colors from bgr to rgb
227  if ((image_encoding == enc::RGB8) || (image_encoding == enc::RGB16))
228  {
229  cv::cvtColor(cv_ptr->image, cv_ptr->image, CV_BGR2RGB);
230  }
231 
232  if ((image_encoding == enc::RGBA8) || (image_encoding == enc::RGBA16))
233  {
234  cv::cvtColor(cv_ptr->image, cv_ptr->image, CV_BGR2RGBA);
235  }
236 
237  if ((image_encoding == enc::BGRA8) || (image_encoding == enc::BGRA16))
238  {
239  cv::cvtColor(cv_ptr->image, cv_ptr->image, CV_BGR2BGRA);
240  }
241  }
242  else
243  {
244  // if necessary convert colors from rgb to bgr
245  if ((image_encoding == enc::BGR8) || (image_encoding == enc::BGR16))
246  {
247  cv::cvtColor(cv_ptr->image, cv_ptr->image, CV_RGB2BGR);
248  }
249 
250  if ((image_encoding == enc::BGRA8) || (image_encoding == enc::BGRA16))
251  {
252  cv::cvtColor(cv_ptr->image, cv_ptr->image, CV_RGB2BGRA);
253  }
254 
255  if ((image_encoding == enc::RGBA8) || (image_encoding == enc::RGBA16))
256  {
257  cv::cvtColor(cv_ptr->image, cv_ptr->image, CV_RGB2RGBA);
258  }
259  }
260  }
261  }
262  }
263  catch (cv::Exception& e)
264  {
265  ROS_ERROR("%s", e.what());
266  }
267 
268  size_t rows = cv_ptr->image.rows;
269  size_t cols = cv_ptr->image.cols;
270 
271  if ((rows > 0) && (cols > 0))
272  {
273  // Publish message to user callback
274  sensor_msgs::Image msg;
275  cv_ptr->toImageMsg(msg);
276  return msg;
277  }
278 
279  return sensor_msgs::Image();
280  }
281 }
CvImageConstPtr toCvShare(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
void setComponents(int components)
int height() const
int components() const
sensor_msgs::Image decompressImage(const sensor_msgs::CompressedImageConstPtr &compressed)
unsigned char * writeHeader(unsigned char *data)
unsigned char * data() const
static bool decode_tables_initialized
Definition: ros_support.h:42
unsigned char * encodeImage(const Image<> &im, unsigned char *dest)
static bool encode_tables_initialized
Definition: ros_support.h:41
int width() const
const unsigned char * decodeImage(Image<> &im, const unsigned char *src)
void initEncodeTable()
static Time now()
void decodeImageSize(Image<> &im, const unsigned char *src)
sensor_msgs::CompressedImage compressImage(const sensor_msgs::Image &image)
Definition: ros_support.cpp:55
#define ROS_ERROR(...)
#define ROS_DEBUG_THROTTLE(period,...)
void initDecodeTable()


imagezero_ros
Author(s): P. J. Reed
autogenerated on Mon Feb 28 2022 22:33:14