image_publisher.cc
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2017 Roboception GmbH
3  * All rights reserved
4  *
5  * Author: Heiko Hirschmueller
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions are met:
9  *
10  * 1. Redistributions of source code must retain the above copyright notice,
11  * this list of conditions and the following disclaimer.
12  *
13  * 2. Redistributions in binary form must reproduce the above copyright notice,
14  * this list of conditions and the following disclaimer in the documentation
15  * and/or other materials provided with the distribution.
16  *
17  * 3. Neither the name of the copyright holder nor the names of its contributors
18  * may be used to endorse or promote products derived from this software without
19  * specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
22  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
23  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
24  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
25  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
26  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
27  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
28  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
29  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
30  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31  * POSSIBILITY OF SUCH DAMAGE.
32  */
33 
34 #include "image_publisher.h"
35 
36 #include <rc_genicam_api/image.h>
38 
40 
41 namespace rc
42 {
43 ImagePublisher::ImagePublisher(image_transport::ImageTransport& it, const std::string& frame_id_prefix, bool _left,
44  bool _color, bool out1_filter)
45  : GenICam2RosPublisher(frame_id_prefix)
46 {
47  left = _left;
48  color = _color;
49  seq = 0;
50 
51  std::string name;
52 
53  if (left)
54  {
55  name = "left/image_rect";
56  }
57  else
58  {
59  name = "right/image_rect";
60  }
61 
62  if (color)
63  {
64  name = name + "_color";
65  }
66 
67  pub = it.advertise(name, 1);
68 
69  if (out1_filter)
70  {
71  pub_out1_low = it.advertise(name + "_out1_low", 1);
72  pub_out1_high = it.advertise(name + "_out1_high", 1);
73  }
74 }
75 
77 {
79 }
80 
81 void ImagePublisher::publish(const rcg::Buffer* buffer, uint32_t part, uint64_t pixelformat)
82 {
83  publish(buffer, part, pixelformat, false);
84 }
85 
86 void ImagePublisher::publish(const rcg::Buffer* buffer, uint32_t part, uint64_t pixelformat, bool out1)
87 {
88  bool sub = (pub.getNumSubscribers() > 0 && (!out1_only_low || !out1));
89 
90  if (!out1 && pub_out1_low.getNumSubscribers() > 0)
91  sub = true;
92  if (out1 && pub_out1_high.getNumSubscribers() > 0)
93  sub = true;
94 
95  if (sub && (pixelformat == Mono8 || pixelformat == YCbCr411_8))
96  {
97  // create image and initialize header
98 
99  sensor_msgs::ImagePtr im = boost::make_shared<sensor_msgs::Image>();
100 
101  const uint64_t freq = 1000000000ul;
102  uint64_t time = buffer->getTimestampNS();
103 
104  im->header.seq = seq++;
105  im->header.stamp.sec = time / freq;
106  im->header.stamp.nsec = time - freq * im->header.stamp.sec;
107  im->header.frame_id = frame_id;
108 
109  // set image size
110 
111  im->width = static_cast<uint32_t>(buffer->getWidth(part));
112  im->height = static_cast<uint32_t>(buffer->getHeight(part));
113  im->is_bigendian = false;
114 
115  bool stacked = false;
116 
117  if (im->height > im->width)
118  {
119  stacked = true;
120  im->height >>= 1;
121  }
122 
123  // get pointer to image data in buffer
124 
125  const uint8_t* ps = static_cast<const uint8_t*>(buffer->getBase(part));
126  size_t pstep = im->width + buffer->getXPadding(part);
127 
128  if (pixelformat == YCbCr411_8)
129  {
130  pstep = (im->width >> 2) * 6 + buffer->getXPadding(part);
131  }
132 
133  if (!left)
134  {
135  if (stacked)
136  {
137  ps += pstep * im->height;
138  }
139  else
140  {
141  return; // buffer does not contain a right image
142  }
143  }
144 
145  // convert image data
146 
147  if (color) // convert to color
148  {
149  im->encoding = sensor_msgs::image_encodings::RGB8;
150  im->step = 3 * im->width * sizeof(uint8_t);
151 
152  im->data.resize(im->step * im->height);
153  uint8_t* pt = reinterpret_cast<uint8_t*>(&im->data[0]);
154 
155  if (pixelformat == Mono8) // convert from monochrome
156  {
157  return; // do not convert from monochrome, skip instead
158  }
159  else if (pixelformat == YCbCr411_8) // convert from YUV 411
160  {
161  for (uint32_t k = 0; k < im->height; k++)
162  {
163  for (uint32_t i = 0; i < im->width; i += 4)
164  {
165  rcg::convYCbCr411toQuadRGB(pt, ps, i);
166  pt += 12;
167  }
168 
169  ps += pstep;
170  }
171  }
172  }
173  else // convert to monochrome
174  {
176  im->step = im->width * sizeof(uint8_t);
177 
178  im->data.resize(im->step * im->height);
179  uint8_t* pt = reinterpret_cast<uint8_t*>(&im->data[0]);
180 
181  if (pixelformat == Mono8) // copy monochrome image
182  {
183  for (uint32_t k = 0; k < im->height; k++)
184  {
185  for (uint32_t i = 0; i < im->width; i++)
186  {
187  *pt++ = ps[i];
188  }
189 
190  ps += pstep;
191  }
192  }
193  else if (pixelformat == YCbCr411_8) // copy monochrome part of YUV 411 image
194  {
195  for (uint32_t k = 0; k < im->height; k++)
196  {
197  int j = 0;
198 
199  for (uint32_t i = 0; i < im->width; i += 4)
200  {
201  *pt++ = ps[j];
202  *pt++ = ps[j + 1];
203  *pt++ = ps[j + 3];
204  *pt++ = ps[j + 4];
205  j += 6;
206  }
207 
208  ps += pstep;
209  }
210  }
211  }
212 
213  // publish message
214 
215  if (!out1_only_low || !out1)
216  pub.publish(im);
217  if (!out1)
218  pub_out1_low.publish(im);
219  if (out1)
221  }
222 }
223 }
Interface for all publishers relating to images, point clouds or other stereo-camera data...
bool used() override
Returns true if there are subscribers to the topic.
Mono8
image_transport::Publisher pub_out1_low
Publisher advertise(const std::string &base_topic, uint32_t queue_size, bool latch=false)
size_t getXPadding(std::uint32_t part) const
void publish(const rcg::Buffer *buffer, uint32_t part, uint64_t pixelformat) override
Offers a buffer for publication.
YCbCr411_8
uint32_t getNumSubscribers() const
size_t getWidth(std::uint32_t part) const
image_transport::Publisher pub
void convYCbCr411toQuadRGB(uint8_t rgb[12], const uint8_t *row, int i)
void * getBase(std::uint32_t part) const
void publish(const sensor_msgs::Image &message) const
uint64_t getTimestampNS() const
image_transport::Publisher pub_out1_high
size_t getHeight(std::uint32_t part) const
ImagePublisher(image_transport::ImageTransport &it, const std::string &frame_id_prefix, bool left, bool color, bool out1_filter)
Initialization of publisher.


rc_visard_driver
Author(s): Heiko Hirschmueller , Christian Emmerich , Felix Ruess
autogenerated on Sat Feb 13 2021 03:42:55