pcl_converter.cpp
Go to the documentation of this file.
1 
45 #include <ros/ros.h>
46 // PCL specific includes
47 #include <ros/package.h>
48 #include <sensor_msgs/PointCloud2.h>
50 #include <pcl/point_cloud.h>
51 #include <pcl/point_types.h>
52 #include <math.h>
53 
54 using namespace ros;
55 
57 
59 
61 bool usingPal = false;
62 
64 
65 double minRange = 255.99 * 0.25;
66 double maxRange = 0.00;
67 float minRangeGreyVal = 0.0;
68 float maxRangeGreyVal = 255.99;
69 
70 int numEchoOutput = 1;
71 
73 {
74 private:
75  bool dump2csv;
76 public:
77  bool isDump2csv() const
78  {
79  return dump2csv;
80  }
81 
82  void setDump2csv(bool dump2csv)
83  {
85  }
86 
87 };
88 
90 
91 
92 void dump2csv(const sensor_msgs::PointCloud2ConstPtr &cloud, int x_offset, int y_offset, int z_offset)
93 {
94  static int cnt = 0;
95  char szFileName[255] = {0};
96  sprintf(szFileName,"/tmp/cloud_dump_%04d.csv", cnt);
97  FILE *fout;
98  fout = fopen(szFileName,"w");
99  if (fout != NULL)
100  {
101  int i,j;
102  fprintf(fout,"%-8s;%-8s;%-8s;%-8s;%-8s;%-8s\n","X(col)","Y(Col)","X","Y","Z","Range");
103  for (i = 0; i < cloud->height; i++)
104  {
105  for (j = 0; j < cloud->width; j++)
106  {
107  float xRange,yRange,zRange;
108  float range;
109  memcpy(&xRange, &(cloud->data[x_offset]), sizeof(float));
110  memcpy(&yRange, &(cloud->data[y_offset]), sizeof(float));
111  memcpy(&zRange, &(cloud->data[z_offset]), sizeof(float));
112  range = sqrt(xRange*xRange + yRange*yRange + zRange*zRange);
113  fprintf(fout,"%5d;%5d;", i, j);
114  fprintf(fout,"%8.3f;%8.3f;%8.3f;%8.3f", xRange, yRange, zRange, range);
115  fprintf(fout,"\n");
116 
117  x_offset += cloud->point_step;
118  y_offset += cloud->point_step;
119  z_offset += cloud->point_step;
120  }
121  }
122  }
123  fclose(fout);
124  cnt++;
125 }
126 
127 
128 
129 
130 
138 void
139 cloud2image_cb(const sensor_msgs::PointCloud2ConstPtr &cloud)
140 {
141  sensor_msgs::Image msg;
142  sensor_msgs::Image msgVGA;
143  int rgb_index = -1;
144  int x_index = -1;
145  int y_index = -1;
146  int z_index = -1;
147 
148  int conversionTask = 3;
149  float range_eps = 0.1;
150 
151 
152  int heightUsed = cloud->height;
153  if (numEchoOutput == 1)
154  {
155  if (heightUsed > 24)
156  {
157  heightUsed = 24;
158  }
159  }
160  // Get the index we need
161  for (size_t d = 0; d < cloud->fields.size(); ++d)
162  {
163  if (cloud->fields[d].name == "intensity")
164  {
165  rgb_index = static_cast<int>(d); // relative index for each field entry
166 
167  }
168  if (cloud->fields[d].name == "x")
169  {
170  x_index = static_cast<int>(d);
171 
172  }
173  if (cloud->fields[d].name == "y")
174  {
175  y_index = static_cast<int>(d);
176 
177  }
178  if (cloud->fields[d].name == "z")
179  {
180  z_index = static_cast<int>(d);
181 
182  }
183  }
184  if (rgb_index == -1)
185  {
187  {
188  return;
189  // intensity flag is missing - ignore the scan
190  }
191  else
192  {
193  throw std::runtime_error("No intensity entry!!");
194  }
195  }
196 
197 
198 
199  int heightStretch = 5;
200 
201  int imgChannelCnt = 0;
202 
203  const int maxAllowedEncodings = 2;
204  bool encodeFlagArray[maxAllowedEncodings] = {0};
205  bool useHDFormat = false;
206  if (conversionTask & 1)
207  {
208  encodeFlagArray[0] = true;
209  imgChannelCnt++;
210  }
211  if (conversionTask & 2)
212  {
213  encodeFlagArray[1] = true;
214  imgChannelCnt++;
215  }
216 
217  if (cloud->width == 0 && heightUsed == 0)
218  {
219  throw std::runtime_error("Needs to be a dense like cloud!!");
220  }
221  else
222  {
223  msg.height = heightStretch * heightUsed * imgChannelCnt;
224  msg.width = cloud->width;
225  if (useHDFormat)
226  {
227  if (1080 > msg.height)
228  {
229  msg.height = 1080;
230  }
231  if (1920 > msg.width)
232  {
233  msg.width = 1920;
234  }
235  }
236  }
237  int rgb_offset = cloud->fields[rgb_index].offset;
238  int x_offset = cloud->fields[x_index].offset;
239  int y_offset = cloud->fields[y_index].offset;
240  int z_offset = cloud->fields[z_index].offset;
241 
242 
243 
244  int point_step = cloud->point_step;
245 
246  /*
247  * point_step: size of each element
248  * x_offset etc: relative index
249  *
250  */
252  {
253  dump2csv(cloud, x_offset, y_offset, z_offset);
254  }
255  // pcl::image_encodings::BGR8;
256  msg.encoding = "bgr8";
257  msg.step = static_cast<uint32_t>(msg.width * sizeof(uint8_t) * 3);
258  msg.data.resize(msg.step * msg.height);
259 
260 
261  int maxY = imgChannelCnt * heightStretch * heightUsed;
262 
263  for (size_t y = 0; y < heightUsed; y++)
264  {
265  for (size_t x = 0; x < cloud->width; x++)
266  {
267  for (int chIdx = 0; chIdx < imgChannelCnt; chIdx++)
268  {
269  int encodingCnt = 0;
270  int encodeIdx = -1; // Kanal, fuer den encodiert werden soll....
271  for (int j = 0; j < maxAllowedEncodings; j++)
272  {
273  if (encodeFlagArray[j])
274  {
275  if (chIdx == encodingCnt)
276  {
277  encodeIdx = j;
278  break;
279  }
280  encodingCnt++;
281  }
282  }
283 
284  for (int rowInner = 0; rowInner < heightStretch; rowInner++)
285  {
286  int rowTmp = (y * heightStretch + rowInner);
287  rowTmp += encodingCnt * heightStretch * heightUsed;
288 
289  int rowInnerIdx = maxY - (rowTmp) - 1;
290  int xTmp = x;
291  xTmp = cloud->width - 1 - x;
292  uint8_t *pixel = &(msg.data[rowInnerIdx * msg.step + xTmp * 3]);
293  uint8_t *pixelVGA = NULL;
294 
295  float intensity;
296  float xRange;
297  float yRange;
298  float zRange;
299  float range = 0.0;
300  memcpy(&xRange, &(cloud->data[x_offset]), sizeof(float));
301  memcpy(&yRange, &(cloud->data[y_offset]), sizeof(float));
302  memcpy(&zRange, &(cloud->data[z_offset]), sizeof(float));
303  range = sqrt(xRange * xRange + yRange * yRange + zRange * zRange);
304  memcpy(&intensity, &(cloud->data[rgb_offset]), sizeof(float));
305  if (intensity > 255.0)
306  {
307  intensity = 255.0;
308  }
309  if (intensity < 0.0)
310  {
311  intensity = 0.0;
312  }
313  unsigned char r, g, b;
314  unsigned char grey = 0;
315  if (encodeIdx == 0)
316  {
317  grey = (unsigned char) intensity;
318  }
319 
320  float minRealRange = (minRange < maxRange) ? minRange : maxRange;
321  float maxRealRange = (minRange < maxRange) ? maxRange : minRange;
322 
323  if (encodeIdx == 1)
324  {
325  bool setBlack = false;
326  if (range < range_eps)
327  {
328  setBlack = true;
329  }
330  if (range < minRealRange)
331  {
332  range = minRealRange;
333 
334  }
335  if (range > maxRealRange)
336  {
337  range = maxRealRange;
338  }
339  // calculate from range to grey value
340  range = (range - minRange) / (maxRange - minRange) * (maxRangeGreyVal - minRangeGreyVal);
341  range += minRangeGreyVal;
342 
343  grey = (unsigned char) range;
344  if (setBlack)
345  {
346  grey = 0;
347  }
348  }
349 
350  // grey = 0xFF;
351  r = grey;
352  g = grey;
353  b = grey;
354 
355 
356  if (usingPal)
357  {
358  r = pal.getRbgValue(grey, 0);
359  g = pal.getRbgValue(grey, 1);
360  b = pal.getRbgValue(grey, 2);
361  }
362  pixel[2] = r;
363  pixel[1] = g;
364  pixel[0] = b;
365 
366  //memcpy(pixel, &(cloud->data[rgb_offset]), 3 * sizeof (uint8_t));
367  }
368  }
369  rgb_offset += point_step;
370  x_offset += point_step;
371  y_offset += point_step;
372  z_offset += point_step;
373 
374  }
375 
376  }
377 
378 // Publish the data.
379  pub.publish(msg);
380 }
381 
382 void argUsage()
383 {
384  printf("sick_pcl_converter <input_pointcloud2-topic> <output_image-topic>");
385 }
386 
387 int
388 main(int argc, char **argv)
389 {
390  bool dumpCsvFlag = false;
391  // Initialize ROS
392  if (argc < 3)
393  {
394  argUsage();
395  exit(-1);
396  }
397 
398  // sensor_msgs::PointCloud2 pclTmp;
399 
400  std::string inputTopic = argv[1];
401  std::string outputTopic = argv[2];
402  ros::init(argc, argv, "sick_pcl_converter");
403 
404 
405  ros::NodeHandle nh("sick_pcl_converter");
406  std::string heatMap;
407  nh.getParam("heat_map", heatMap);
408 
409  nh.getParam("dump_csv", dumpCsvFlag);
410 
411  paramSettings.setDump2csv(dumpCsvFlag);
412 
414  {
415  ROS_INFO("Dump to csv file for debugging enabled.\n Files will be dumped at /tmp ");
416  }
417 
418  std::string path = ros::package::getPath("sick_scan");
419 
420  if (heatMap.length() > 0)
421  {
422  std::string heatMapFileName = path + "/config/" + heatMap;
423  pal.load(heatMapFileName);
424  usingPal = true;
425  }
426  ROS_INFO("Subscribing to %s and publishing images to %s\n", inputTopic.c_str(), outputTopic.c_str());
427  // Create a ROS subscriber for the input point cloud
428 // ros::Subscriber sub = nh.subscribe (inputTopic, 1, cloud_cb);
429  ros::Subscriber sub = nh.subscribe(inputTopic, 1, cloud2image_cb);
430 
431 
432  // Create a ROS publisher for the output point cloud
433 // pub = nh.advertise<sensor_msgs::PointCloud2> (outputTopic, 1);
434  pub = nh.advertise<sensor_msgs::Image>(outputTopic, 1);
435 
436  // Spin
437  ros::spin();
438 }
ParamSettings::dump2csv
bool dump2csv
Definition: pcl_converter.cpp:75
msg
msg
ros::Publisher
argUsage
void argUsage()
Definition: pcl_converter.cpp:382
ros::package::getPath
ROSLIB_DECL std::string getPath(const std::string &package_name)
ros::init
ROSCPP_DECL void init(const M_string &remappings, const std::string &name, uint32_t options=0)
ros
ros::NodeHandle::getParam
bool getParam(const std::string &key, bool &b) const
ros.h
GnuPlotPalette
Definition: gnuplotPaletteReader.h:4
maxRange
double maxRange
Definition: pcl_converter.cpp:66
pub
ros::Publisher pub
Definition: pcl_converter.cpp:58
ParamSettings::setDump2csv
void setDump2csv(bool dump2csv)
Definition: pcl_converter.cpp:82
paramSettings
ParamSettings paramSettings
Definition: pcl_converter.cpp:89
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
ros::NodeHandle::advertise
Publisher advertise(AdvertiseOptions &ops)
GnuPlotPalette::load
int load(std::string palettName)
Definition: gnuplotPaletteReader.cpp:48
maxRangeGreyVal
float maxRangeGreyVal
Definition: pcl_converter.cpp:68
d
d
minRange
double minRange
Definition: pcl_converter.cpp:65
ros::NodeHandle::subscribe
Subscriber subscribe(const std::string &topic, uint32_t queue_size, const boost::function< void(C)> &callback, const VoidConstPtr &tracked_object=VoidConstPtr(), const TransportHints &transport_hints=TransportHints())
usingPal
bool usingPal
Definition: pcl_converter.cpp:61
pal
GnuPlotPalette pal
Definition: pcl_converter.cpp:60
package.h
main
int main(int argc, char **argv)
Definition: pcl_converter.cpp:388
ParamSettings::isDump2csv
bool isDump2csv() const
Definition: pcl_converter.cpp:77
ignoreIntensityMissing
bool ignoreIntensityMissing
Definition: pcl_converter.cpp:63
gnuplotPaletteReader.h
dump2csv
void dump2csv(const sensor_msgs::PointCloud2ConstPtr &cloud, int x_offset, int y_offset, int z_offset)
Definition: pcl_converter.cpp:92
minRangeGreyVal
float minRangeGreyVal
Definition: pcl_converter.cpp:67
ros::spin
ROSCPP_DECL void spin()
ParamSettings
Definition: pcl_converter.cpp:72
numEchoOutput
int numEchoOutput
Definition: pcl_converter.cpp:70
ROS_INFO
#define ROS_INFO(...)
GnuPlotPalette::getRbgValue
unsigned char getRbgValue(unsigned char greyIdx, unsigned int channelIdx)
Definition: gnuplotPaletteReader.cpp:163
cloud2image_cb
void cloud2image_cb(const sensor_msgs::PointCloud2ConstPtr &cloud)
Copy the RGB fields of a PCLPointCloud2 msg into pcl::PCLImage format.
Definition: pcl_converter.cpp:139
ros::NodeHandle
ros::Subscriber
pcl_conversions.h


sick_scan
Author(s): Michael Lehning , Jochen Sprickerhof , Martin Günther
autogenerated on Thu Sep 8 2022 02:30:19