pointcloud_utils.cpp
Go to the documentation of this file.
1 /*
2  * @brief Pointcloud utilities for ROS simu
3  *
4  * Copyright (C) 2021, Ing.-Buero Dr. Michael Lehning, Hildesheim
5  * Copyright (C) 2021, SICK AG, Waldkirch
6  * All rights reserved.
7  *
8 * Licensed under the Apache License, Version 2.0 (the "License");
9 * you may not use this file except in compliance with the License.
10 * You may obtain a copy of the License at
11 *
12 * http://www.apache.org/licenses/LICENSE-2.0
13 *
14 * Unless required by applicable law or agreed to in writing, software
15 * distributed under the License is distributed on an "AS IS" BASIS,
16 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
17 * See the License for the specific language governing permissions and
18 * limitations under the License.
19 *
20 *
21 * All rights reserved.
22 *
23 * Redistribution and use in source and binary forms, with or without
24 * modification, are permitted provided that the following conditions are met:
25 *
26 * * Redistributions of source code must retain the above copyright
27 * notice, this list of conditions and the following disclaimer.
28 * * Redistributions in binary form must reproduce the above copyright
29 * notice, this list of conditions and the following disclaimer in the
30 * documentation and/or other materials provided with the distribution.
31 * * Neither the name of Osnabrueck University nor the names of its
32 * contributors may be used to endorse or promote products derived from
33 * this software without specific prior written permission.
34 * * Neither the name of SICK AG nor the names of its
35 * contributors may be used to endorse or promote products derived from
36 * this software without specific prior written permission
37 * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its
38 * contributors may be used to endorse or promote products derived from
39 * this software without specific prior written permission
40 *
41 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
42 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
43 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
44 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
45 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
46 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
47 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
48 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
49 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
50 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
51 * POSSIBILITY OF SUCH DAMAGE.
52  *
53  * Created on: 12.01.2021
54  *
55  * Authors:
56  * Michael Lehning <michael.lehning@lehning.de>
57  *
58  */
59 
60 #include <sick_scan/pointcloud_utils.h>
61 
62 #include "toojpeg.h"
63 
64 static FILE *foutJpg = 0;
65 
66 static std::vector<ros_visualization_msgs::Marker> s_visualization_marker_array;
67 
68 static unsigned char s_fontCharBlock8x12[] =
69 {
70  0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00 , //
71  0x00,0x18,0x18,0x18,0x18,0x18,0x18,0x00,0x18,0x18,0x00,0x00 , // !
72  0x00,0x36,0x36,0x36,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00 , // "
73  0x00,0x00,0x14,0x14,0x3E,0x14,0x14,0x3E,0x14,0x14,0x00,0x00 , // #
74  0x00,0x08,0x1C,0x32,0x30,0x1C,0x06,0x26,0x1C,0x08,0x08,0x00 , // $
75  0x00,0x13,0x2B,0x2B,0x16,0x0C,0x1A,0x35,0x35,0x32,0x00,0x00 , // %
76  0x00,0x1C,0x36,0x36,0x1C,0x1D,0x37,0x36,0x36,0x1B,0x00,0x00 , // &
77  0x00,0x0C,0x0C,0x18,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00 , // '
78  0x00,0x0C,0x18,0x30,0x30,0x30,0x30,0x30,0x18,0x0C,0x00,0x00 , // (
79  0x00,0x30,0x18,0x0C,0x0C,0x0C,0x0C,0x0C,0x18,0x30,0x00,0x00 , // )
80  0x00,0x00,0x00,0x36,0x1C,0x7F,0x1C,0x36,0x00,0x00,0x00,0x00 , // *
81  0x00,0x00,0x00,0x00,0x0C,0x0C,0x3F,0x0C,0x0C,0x00,0x00,0x00 , // +
82  0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x1C,0x1C,0x0C,0x18 , // ,
83  0x00,0x00,0x00,0x00,0x00,0x00,0x3E,0x00,0x00,0x00,0x00,0x00 , // -
84  0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x1C,0x1C,0x00,0x00 , // .
85  0x00,0x06,0x06,0x0C,0x0C,0x18,0x18,0x30,0x30,0x60,0x00,0x00 , // /
86  0x00,0x1E,0x36,0x36,0x36,0x36,0x36,0x36,0x36,0x3C,0x00,0x00 , // 0
87  0x00,0x0C,0x1C,0x3C,0x0C,0x0C,0x0C,0x0C,0x0C,0x0C,0x00,0x00 , // 1
88  0x00,0x1C,0x36,0x36,0x06,0x0C,0x0C,0x18,0x30,0x3E,0x00,0x00 , // 2
89  0x00,0x1C,0x36,0x06,0x06,0x1C,0x06,0x06,0x36,0x1C,0x00,0x00 , // 3
90  0x00,0x18,0x18,0x18,0x36,0x36,0x36,0x3F,0x06,0x06,0x00,0x00 , // 4
91  0x00,0x3E,0x30,0x30,0x30,0x3C,0x06,0x06,0x36,0x1C,0x00,0x00 , // 5
92  0x00,0x0C,0x18,0x18,0x30,0x3C,0x36,0x36,0x36,0x1C,0x00,0x00 , // 6
93  0x00,0x3E,0x06,0x06,0x0C,0x0C,0x0C,0x18,0x18,0x18,0x00,0x00 , // 7
94  0x00,0x1C,0x36,0x36,0x36,0x1C,0x36,0x36,0x36,0x1C,0x00,0x00 , // 8
95  0x00,0x1C,0x36,0x36,0x36,0x1E,0x0C,0x0C,0x18,0x30,0x00,0x00 , // 9
96  0x00,0x00,0x00,0x1C,0x1C,0x00,0x00,0x00,0x1C,0x1C,0x00,0x00 , // :
97  0x00,0x00,0x00,0x1C,0x1C,0x00,0x00,0x00,0x1C,0x1C,0x0C,0x18 , // , //
98  0x00,0x02,0x06,0x0C,0x18,0x30,0x18,0x0C,0x06,0x02,0x00,0x00 , // <
99  0x00,0x00,0x00,0x00,0x3E,0x00,0x00,0x3E,0x00,0x00,0x00,0x00 , // =
100  0x00,0x20,0x30,0x18,0x0C,0x06,0x0C,0x18,0x30,0x20,0x00,0x00 , // >
101  0x00,0x1C,0x36,0x36,0x06,0x0C,0x18,0x00,0x18,0x18,0x00,0x00 , // ?
102  0x00,0x0E,0x19,0x33,0x35,0x35,0x35,0x32,0x18,0x0F,0x00,0x00 , // @
103  0x00,0x08,0x1C,0x36,0x36,0x36,0x3E,0x36,0x36,0x36,0x00,0x00 , // A
104  0x00,0x3C,0x36,0x36,0x36,0x3C,0x36,0x36,0x36,0x3C,0x00,0x00 , // B
105  0x00,0x1C,0x36,0x36,0x30,0x30,0x30,0x36,0x36,0x1C,0x00,0x00 , // C
106  0x00,0x3C,0x36,0x36,0x36,0x36,0x36,0x36,0x36,0x3C,0x00,0x00 , // D
107  0x00,0x3E,0x30,0x30,0x30,0x3C,0x30,0x30,0x30,0x3E,0x00,0x00 , // E
108  0x00,0x3E,0x30,0x30,0x30,0x3C,0x30,0x30,0x30,0x30,0x00,0x00 , // F
109  0x00,0x1C,0x36,0x30,0x30,0x36,0x36,0x36,0x36,0x1C,0x00,0x00 , // G
110  0x00,0x36,0x36,0x36,0x36,0x3E,0x36,0x36,0x36,0x36,0x00,0x00 , // H
111  0x00,0x1E,0x0C,0x0C,0x0C,0x0C,0x0C,0x0C,0x0C,0x1E,0x00,0x00 , // I
112  0x00,0x06,0x06,0x06,0x06,0x06,0x06,0x36,0x36,0x1C,0x00,0x00 , // J
113  0x00,0x36,0x36,0x36,0x3C,0x38,0x3C,0x36,0x36,0x36,0x00,0x00 , // K
114  0x00,0x30,0x30,0x30,0x30,0x30,0x30,0x30,0x30,0x3E,0x00,0x00 , // L
115  0x00,0x63,0x63,0x77,0x77,0x7F,0x6B,0x6B,0x63,0x63,0x00,0x00 , // M
116  0x00,0x33,0x33,0x3B,0x3B,0x3F,0x37,0x33,0x33,0x33,0x00,0x00 , // N
117  0x00,0x1C,0x36,0x36,0x36,0x36,0x36,0x36,0x36,0x1C,0x00,0x00 , // O
118  0x00,0x3C,0x36,0x36,0x36,0x3C,0x30,0x30,0x30,0x30,0x00,0x00 , // P
119  0x00,0x1C,0x36,0x36,0x36,0x36,0x36,0x36,0x36,0x1C,0x0C,0x06 , // Q
120  0x00,0x3C,0x36,0x36,0x36,0x3C,0x36,0x36,0x36,0x36,0x00,0x00 , // R
121  0x00,0x1C,0x36,0x32,0x38,0x1C,0x0E,0x26,0x36,0x1C,0x00,0x00 , // S
122  0x00,0x3F,0x0C,0x0C,0x0C,0x0C,0x0C,0x0C,0x0C,0x0C,0x00,0x00 , // T
123  0x00,0x36,0x36,0x36,0x36,0x36,0x36,0x36,0x36,0x1C,0x00,0x00 , // U
124  0x00,0x36,0x36,0x36,0x36,0x36,0x36,0x36,0x1C,0x08,0x00,0x00 , // V
125  0x00,0x63,0x63,0x6B,0x6B,0x6B,0x6B,0x36,0x36,0x36,0x00,0x00 , // W
126  0x00,0x36,0x36,0x36,0x1C,0x08,0x1C,0x36,0x36,0x36,0x00,0x00 , // X
127  0x00,0x33,0x33,0x33,0x33,0x1E,0x0C,0x0C,0x0C,0x0C,0x00,0x00 , // Y
128  0x00,0x3E,0x06,0x0C,0x0C,0x18,0x18,0x30,0x30,0x3E,0x00,0x00 , // Z
129  0x00,0x1E,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x1E , // [
130  0x00,0x30,0x30,0x18,0x18,0x0C,0x0C,0x06,0x06,0x03,0x00,0x00 , // Backslash
131  0x00,0x3C,0x0C,0x0C,0x0C,0x0C,0x0C,0x0C,0x0C,0x0C,0x0C,0x3C , // ]
132  0x00,0x08,0x1C,0x36,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00 , // ^
133  0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x7F , // _
134  0x00,0x18,0x18,0x0C,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00 , // `
135  0x00,0x00,0x00,0x00,0x1C,0x06,0x1E,0x36,0x36,0x1E,0x00,0x00 , // a
136  0x00,0x30,0x30,0x30,0x3C,0x36,0x36,0x36,0x36,0x3C,0x00,0x00 , // b
137  0x00,0x00,0x00,0x00,0x1C,0x36,0x30,0x30,0x36,0x1C,0x00,0x00 , // c
138  0x00,0x06,0x06,0x06,0x1E,0x36,0x36,0x36,0x36,0x1E,0x00,0x00 , // d
139  0x00,0x00,0x00,0x00,0x1C,0x36,0x3E,0x30,0x32,0x1C,0x00,0x00 , // e
140  0x00,0x0E,0x18,0x18,0x3E,0x18,0x18,0x18,0x18,0x18,0x00,0x00 , // f
141  0x00,0x00,0x00,0x00,0x1E,0x36,0x36,0x36,0x1E,0x06,0x26,0x1C , // g
142  0x00,0x30,0x30,0x30,0x3C,0x36,0x36,0x36,0x36,0x36,0x00,0x00 , // h
143  0x00,0x0C,0x00,0x00,0x1C,0x0C,0x0C,0x0C,0x0C,0x1E,0x00,0x00 , // i
144  0x00,0x0C,0x00,0x00,0x1C,0x0C,0x0C,0x0C,0x0C,0x0C,0x0C,0x38 , // j
145  0x00,0x30,0x30,0x30,0x36,0x3C,0x38,0x3C,0x36,0x36,0x00,0x00 , // k
146  0x00,0x1C,0x0C,0x0C,0x0C,0x0C,0x0C,0x0C,0x0C,0x1E,0x00,0x00 , // l
147  0x00,0x00,0x00,0x00,0x76,0x7F,0x6B,0x6B,0x6B,0x63,0x00,0x00 , // m
148  0x00,0x00,0x00,0x00,0x3C,0x36,0x36,0x36,0x36,0x36,0x00,0x00 , // n
149  0x00,0x00,0x00,0x00,0x1C,0x36,0x36,0x36,0x36,0x1C,0x00,0x00 , // o
150  0x00,0x00,0x00,0x00,0x3C,0x36,0x36,0x36,0x36,0x3C,0x30,0x30 , // p
151  0x00,0x00,0x00,0x00,0x1E,0x36,0x36,0x36,0x36,0x1E,0x06,0x06 , // q
152  0x00,0x00,0x00,0x00,0x36,0x3E,0x30,0x30,0x30,0x30,0x00,0x00 , // r
153  0x00,0x00,0x00,0x00,0x1E,0x30,0x3C,0x1E,0x06,0x3C,0x00,0x00 , // s
154  0x00,0x00,0x18,0x18,0x3C,0x18,0x18,0x18,0x18,0x0E,0x00,0x00 , // t
155  0x00,0x00,0x00,0x00,0x36,0x36,0x36,0x36,0x36,0x1E,0x00,0x00 , // u
156  0x00,0x00,0x00,0x00,0x36,0x36,0x36,0x36,0x1C,0x08,0x00,0x00 , // v
157  0x00,0x00,0x00,0x00,0x63,0x6B,0x6B,0x6B,0x3E,0x36,0x00,0x00 , // w
158  0x00,0x00,0x00,0x00,0x36,0x36,0x1C,0x1C,0x36,0x36,0x00,0x00 , // x
159  0x00,0x00,0x00,0x00,0x36,0x36,0x36,0x36,0x1C,0x0C,0x18,0x30 , // y
160  0x00,0x00,0x00,0x00,0x3E,0x06,0x0C,0x18,0x30,0x3E,0x00,0x00 , // z
161  0x00,0x0E,0x18,0x18,0x18,0x30,0x18,0x18,0x18,0x0E,0x00,0x00 , // {
162  0x00,0x18,0x18,0x18,0x18,0x18,0x00,0x18,0x18,0x18,0x18,0x18 , // |
163  0x00,0x30,0x18,0x18,0x18,0x0C,0x18,0x18,0x18,0x30,0x00,0x00 , // }
164  0x00,0x1A,0x3E,0x2C,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00 , // ~
165  0x00,0x00,0x00,0x08,0x1C,0x36,0x22,0x22,0x3E,0x3E,0x00,0x00 , // 
166 };
167 
168 void jpegOutputCallback(unsigned char oneByte)
169 {
170  fwrite(&oneByte, 1, 1, foutJpg);
171 }
172 
173 void setVisualizationMarkerArray(const std::vector<ros_visualization_msgs::Marker> & marker_array)
174 {
175  s_visualization_marker_array = marker_array;
176 }
177 
178 static void setPixel(unsigned char *pixel, int wi, int hi, int numColorChannel, int x, int y, unsigned char rgb_color[3])
179 {
180  if( x >= 0 && x < wi && y >= 0 && y < hi)
181  {
182  int pixAdr = numColorChannel * (y * wi + x);
183  pixel[pixAdr + 0] = rgb_color[0];
184  pixel[pixAdr + 1] = rgb_color[1];
185  pixel[pixAdr + 2] = rgb_color[2];
186  }
187 }
188 
189 static void drawLine(unsigned char *pixel, int wi, int hi, int numColorChannel, int startpoint[2], int endpoint[2], float color[3])
190 {
191  int x0 = startpoint[0], y0 = startpoint[1], x1 = endpoint[0], y1 = endpoint[1];
192  unsigned char rgb_color[3] = { (unsigned char)((int)(255.999 * color[0])&0xFF), (unsigned char)((int)(255.999 * color[1])&0xFF), (unsigned char)((int)(255.999 * color[2])&0xFF) };
193  int dx = abs(x1 - x0), sx = x0 < x1 ? 1 : -1;
194  int dy = abs(y1 - y0), sy = y0 < y1 ? 1 : -1;
195  int err = (dx > dy ? dx : -dy) / 2, e2;
196  for (;;)
197  {
198  setPixel(pixel, wi, hi, numColorChannel, x0, y0, rgb_color);
199  if (x0 == x1 && y0 == y1) break;
200  e2 = err;
201  if (e2 > -dx) { err -= dy; x0 += sx; }
202  if (e2 < dy) { err += dx; y0 += sy; }
203  }
204 }
205 
206 static void drawText(unsigned char *pixel, int wi, int hi, int numColorChannel, const std::string& text, int pos[2], float color[3])
207 {
208  unsigned char rgb_color[3] = { (unsigned char)((int)(255.999 * color[0])&0xFF), (unsigned char)((int)(255.999 * color[1])&0xFF), (unsigned char)((int)(255.999 * color[2])&0xFF) };
209  int strLen = (int)text.length();
210  int xTmp = pos[0];
211  for (int i = 0; i < strLen; i++)
212  {
213  char ch = text[i];
214  if ((ch >= ' ') && (ch < 0x80))
215  {
216  int idx = ch - ' ';
217  int offset = idx * 12;
218  for (int ii = 0; ii < 12; ii++)
219  {
220  unsigned char ucBitMask = s_fontCharBlock8x12[offset + ii];
221  for (int jj = 0; jj < 8; jj++)
222  {
223  if (ucBitMask & (0x01 << (7 - jj)))
224  {
225  setPixel(pixel, wi, hi, numColorChannel, xTmp + jj, pos[1] + ii, rgb_color);
226  }
227  }
228  }
229  }
230  xTmp += 8;
231  }
232 }
233 
234 /*
235 ** Plots a pointcloud and saves to jpg and csv-file
236 */
237 void plotPointCloud(const ros_sensor_msgs::PointCloud2& cloud_, int intervall, const std::string & outputfolder)
238 {
239  static int cnt = 0;
240  cnt++;
241 
242  ROS_DEBUG_STREAM("plotPointCloud: outputfolder=\"" << outputfolder << "\"");
243  const unsigned char *cloudDataPtr = &(cloud_.data[0]);
244  int w = cloud_.width;
245  int h = cloud_.height;
246 
247  int numShots = w * h;
248 
249  float *ptr = (float *) cloudDataPtr;
250 
251  if (cnt == intervall)
252  {
253  std::string filename_tmpl = outputfolder + "/scan";
254 #ifdef _MSC_VER
255  std::replace( filename_tmpl.begin(), filename_tmpl.end(), '/', '\\');
256 #else
257  std::replace( filename_tmpl.begin(), filename_tmpl.end(), '\\', '/');
258 #endif
259  std::string jpgFileName_tmp = filename_tmpl + ".jpg_tmp";
260 
261  int xic = 400;
262  int yic = 400;
263  int w2i = 400;
264  int h2i = 400;
265  int hi = h2i * 2 + 1;
266  int wi = w2i * 2 + 1;
267  int pixNum = hi * wi;
268  int numColorChannel = 3;
269  unsigned char *pixel = (unsigned char *) malloc(numColorChannel * hi * wi);
270  memset(pixel, 0, numColorChannel * pixNum);
271  double scaleFac = 50.0;
272 
273  for (int i = 0; i < hi; i++)
274  {
275  int pixAdr = numColorChannel * (i * wi + xic);
276  pixel[pixAdr] = 0x40;
277  pixel[pixAdr + 1] = 0x40;
278  pixel[pixAdr + 2] = 0x40;
279  }
280  for (int i = 0; i < wi; i++)
281  {
282  int pixAdr = numColorChannel * (yic * wi + i);
283  pixel[pixAdr] = 0x40;
284  pixel[pixAdr + 1] = 0x40;
285  pixel[pixAdr + 2] = 0x40;
286  }
287 
288  scaleFac *= -1.0;
289  for (int i = 0; i < numShots; i++)
290  {
291  double x, y, z, intensity;
292  x = ptr[0];
293  y = ptr[1];
294  z = ptr[2];
295  intensity = ptr[3];
296  ptr += 4;
297  int xi = (x * scaleFac) + xic;
298  int yi = (y * scaleFac) + yic;
299  if ((xi >= 0) && (xi < wi))
300  {
301  if ((yi >= 0) && (xi < hi))
302  {
303  // yi shows left (due to neg. scaleFac)
304  // xi shows up (due to neg. scaleFac)
305  int pixAdr = numColorChannel * (xi * wi + yi);
306  int layer = i / w;
307  unsigned char color[3] = {0x00};
308  switch (layer)
309  {
310  case 0:
311  color[0] = 0xFF;
312  break;
313  case 1:
314  color[1] = 0xFF;
315  break;
316  case 2:
317  color[2] = 0xFF;
318  break;
319  case 3:
320  color[0] = 0xFF;
321  color[1] = 0xFF;
322  break;
323  }
324 
325  for (int kk = 0; kk < 3; kk++)
326  {
327  pixel[pixAdr + kk] = color[kk];
328 
329  }
330  }
331  }
332 
333  }
334 
335  // Draw marker array and fields (if any visualization markers are set)
336  // Convert ROS to image coordinates: ROS: x upward, y to the left, Image: x to the right, y downwards, Note: scaleFac is negative
337  for(int marker_idx = 0; marker_idx < s_visualization_marker_array.size(); marker_idx++)
338  {
339  const ros_visualization_msgs::Marker& marker = s_visualization_marker_array[marker_idx];
340  ros_std_msgs::ColorRGBA marker_rgba = marker.color;
341  float marker_pos[3] = { (float)marker.pose.position.x, (float)marker.pose.position.y, (float)marker.pose.position.z };
342  std::string marker_text = marker.text;
343  if(marker.type == ros_visualization_msgs::Marker::TRIANGLE_LIST)
344  {
345  for(int point_idx = 2; point_idx < marker.points.size(); point_idx+=3)
346  {
347  int triangle_p0[2] = { (int)(marker.points[point_idx - 2].y * scaleFac + xic), (int)(marker.points[point_idx - 2].x * scaleFac + yic) };
348  int triangle_p1[2] = { (int)(marker.points[point_idx - 1].y * scaleFac + xic), (int)(marker.points[point_idx - 1].x * scaleFac + yic) };
349  int triangle_p2[2] = { (int)(marker.points[point_idx - 0].y * scaleFac + xic), (int)(marker.points[point_idx - 0].x * scaleFac + yic) };
350  if(point_idx < marker.colors.size())
351  marker_rgba = marker.colors[point_idx];
352  float marker_color[3] = { (float)marker_rgba.r, (float)marker_rgba.g, (float)marker_rgba.b };
353  drawLine(pixel, wi, hi, numColorChannel, triangle_p0, triangle_p1, marker_color);
354  drawLine(pixel, wi, hi, numColorChannel, triangle_p1, triangle_p2, marker_color);
355  drawLine(pixel, wi, hi, numColorChannel, triangle_p2, triangle_p1, marker_color);
356  }
357  }
358  else if(marker.type == ros_visualization_msgs::Marker::TEXT_VIEW_FACING)
359  {
360  int text_pos[2] = { (int)(2 * marker_pos[1] * scaleFac + xic), (int)(3 * marker_pos[0] * scaleFac + yic) };
361  int text_bias = ((text_pos[0] < xic) ? -1 : 0) * 8 * marker_text.length(); // ROS: marker position is the center of the text area
362  text_pos[0] += text_bias;
363  float text_color[3] = { (float)marker_rgba.r, (float)marker_rgba.g, (float)marker_rgba.b };
364  drawText(pixel, wi, hi, numColorChannel, marker_text, text_pos, text_color);
365  }
366  else // currently supported types are ros_visualization_msgs::Marker::TEXT_VIEW_FACING and ros_visualization_msgs::Marker::TRIANGLE_LIST
367  {
368  ROS_INFO_STREAM("visualization_marker[" << marker_idx << "]: type=" << (int)marker.type << " NOT supported, text=" << marker_text << ", pos=(" << marker_pos[0] << "," << marker_pos[1] << "," << marker_pos[1] << "), "
369  << ", color_rgb=(" << marker_rgba.r << "," << marker_rgba.g << "," << marker_rgba.b << "), " << (marker.points.size()) << " points.");
370  }
371  }
372 
373  // Write JPEG Scan Top View
374  foutJpg = fopen(jpgFileName_tmp.c_str(), "wb");
375  if (foutJpg == NULL)
376  {
377  ROS_INFO_STREAM("Demo file " << jpgFileName_tmp << " not created. Check existience of demo dir. or patch code.\n");
378  }
379  else
380  {
381  TooJpeg::writeJpeg(jpegOutputCallback, pixel, wi, hi, true, 99);
382  fclose(foutJpg);
383 
384  free(pixel);
385  std::string jpgFileName_out = filename_tmpl + ".jpg";
386 #ifdef _MSC_VER
387  _unlink(jpgFileName_out.c_str());
388  rename(jpgFileName_tmp.c_str(), jpgFileName_out.c_str());
389 #else
390  rename(jpgFileName_tmp.c_str(), jpgFileName_out.c_str());
391 #endif
392 
393  }
394  // Write CSV-File
395  std::string csvFileNameTmp = filename_tmpl + ".csv_tmp";
396  FILE *foutCsv = fopen(csvFileNameTmp.c_str(), "w");
397  if (foutCsv)
398  {
399  // ZIEL: fprintf(foutCsv,"timestamp;range;elevation;azimuth;x;y;z;intensity\n");
400  fprintf(foutCsv,"timestamp_sec;timestamp_nanosec;range;azimuth_deg;elevation_deg;x;y;z;intensity\n");
401  const unsigned char *cloudDataPtr = &(cloud_.data[0]);
402 
403  int numShots = w * h;
404 
405  float *ptr = (float *) cloudDataPtr;
406 
407 
408  long timestamp_sec = cloud_.header.stamp.sec;
409  long timestamp_nanosec = cloud_.header.stamp.nsec;
410  for (int i = 0; i < numShots; i++)
411  {
412  double x, y, z, intensity;
413  x = ptr[0];
414  y = ptr[1];
415  z = ptr[2];
416  float range_xy = sqrt(x*x+y*y);
417  float range_xyz = sqrt(x*x+y*y+z*z);
418  float elevation = atan2(z, range_xy);
419  float azimuth = atan2(y,x);
420  float elevationDeg = elevation * 180.0 / M_PI;
421  float azimuthDeg = azimuth * 180.0 / M_PI;
422 
423  intensity = ptr[3];
424  ptr += 4;
425  fprintf(foutCsv,"%12ld;%12ld;%8.3lf;%8.3lf;%8.3lf;%8.3f;%8.3f;%8.3f;%8.0f\n", timestamp_sec, timestamp_nanosec, range_xyz, azimuthDeg, elevationDeg, x,y,z,intensity);
426  }
427  fclose(foutCsv);
428  std::string csvFileNameOut = filename_tmpl + ".csv";
429 #ifdef _MSC_VER
430  _unlink(csvFileNameOut.c_str());
431  rename(csvFileNameTmp.c_str(), csvFileNameOut.c_str());
432 #else
433  rename(csvFileNameTmp.c_str(), csvFileNameOut.c_str());
434 #endif
435  }
436  else
437  {
438  ROS_INFO_STREAM("Demo file " << csvFileNameTmp << " not created. Check existience of demo dir. or patch code.\n");
439  }
440  cnt = 0;
441  }
442 
443 }
sick_scan_xd_api_test.marker
marker
Definition: sick_scan_xd_api_test.py:445
NULL
#define NULL
pointcloud_utils.h
setPixel
static void setPixel(unsigned char *pixel, int wi, int hi, int numColorChannel, int x, int y, unsigned char rgb_color[3])
Definition: pointcloud_utils.cpp:178
sensor_msgs::PointCloud2
::sensor_msgs::PointCloud2_< std::allocator< void > > PointCloud2
Definition: PointCloud2.h:90
drawLine
static void drawLine(unsigned char *pixel, int wi, int hi, int numColorChannel, int startpoint[2], int endpoint[2], float color[3])
Definition: pointcloud_utils.cpp:189
s_fontCharBlock8x12
static unsigned char s_fontCharBlock8x12[]
Definition: pointcloud_utils.cpp:68
drawText
static void drawText(unsigned char *pixel, int wi, int hi, int numColorChannel, const std::string &text, int pos[2], float color[3])
Definition: pointcloud_utils.cpp:206
foutJpg
static FILE * foutJpg
Definition: pointcloud_utils.cpp:64
s_visualization_marker_array
static std::vector< ros_visualization_msgs::Marker > s_visualization_marker_array
Definition: pointcloud_utils.cpp:66
ROS_INFO_STREAM
#define ROS_INFO_STREAM(...)
Definition: sick_scan_ros2_example.cpp:71
TooJpeg::writeJpeg
bool writeJpeg(WRITE_ONE_BYTE output, const void *pixels_, unsigned short width, unsigned short height, bool isRGB, unsigned char quality_, bool downsample, const char *comment)
Definition: roswrap/src/toojpeg/toojpeg.cpp:352
visualization_msgs::Marker
::visualization_msgs::Marker_< std::allocator< void > > Marker
Definition: Marker.h:193
ROS_DEBUG_STREAM
#define ROS_DEBUG_STREAM(args)
Definition: sick_scan_logging.h:113
plotPointCloud
void plotPointCloud(const ros_sensor_msgs::PointCloud2 &cloud_, int intervall, const std::string &outputfolder)
Definition: pointcloud_utils.cpp:237
setVisualizationMarkerArray
void setVisualizationMarkerArray(const std::vector< ros_visualization_msgs::Marker > &marker_array)
Definition: pointcloud_utils.cpp:173
std_msgs::ColorRGBA
::std_msgs::ColorRGBA_< std::allocator< void > > ColorRGBA
Definition: ColorRGBA.h:63
jpegOutputCallback
void jpegOutputCallback(unsigned char oneByte)
Definition: pointcloud_utils.cpp:168
color
static ros_std_msgs::ColorRGBA color(float r, float g, float b, float a=0.5f)
Definition: sick_scan_marker.cpp:69

sick_scan_xd
Author(s): Michael Lehning , Jochen Sprickerhof , Martin Günther
autogenerated on Fri Oct 25 2024 02:47:09