radar_object_marker.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2018, SICK AG
3  * Copyright (C) 2018, Ing.-Buero Dr. Michael Lehning
4  * Copyright (C) 2016, DFKI GmbH
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 are met:
9  *
10  * * Redistributions of source code must retain the above copyright
11  * notice, this list of conditions and the following disclaimer.
12  * * Redistributions in binary form must reproduce the above copyright
13  * notice, this list of conditions and the following disclaimer in the
14  * documentation and/or other materials provided with the distribution.
15  * * Neither the name of DFKI GmbH nor the names of its
16  * contributors may be used to endorse or promote products derived from
17  * this software without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
20  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
21  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
22  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
23  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
24  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
25  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
26  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
27  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
28  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
29  * POSSIBILITY OF SUCH DAMAGE.
30  *
31  */
32 
33 #include <ros/ros.h>
34 
35 #include <visualization_msgs/MarkerArray.h>
36 #include <sick_scan/RadarScan.h>
37 
38 #include <ros/ros.h>
39 #include <ros/package.h>
40 #include <math.h>
41 #include <visualization_msgs/MarkerArray.h>
42 #include <sick_scan/RadarScan.h>
43 #include "radar_object_marker/radar_object_marker.h"
44 #include "pcl_converter/gnuplotPaletteReader.h"
45 #include <boost/serialization/singleton.hpp>
46 
47 float ballRadius = 0.1;
48 float objectArrowScale = 1.0;
49 GnuPlotPalette pal;
50 bool usingPal = false;
51 
52 ros::Publisher pub;
53 ros::Publisher pub_cloud;
54 
55 
56 
57 
58 void normalize( float& angle )
59 {
60  while ( angle < -M_PI ) angle += (2 * M_PI);
61  while ( angle > M_PI ) angle -= (2 * M_PI);
62 }
63 
64 bool isWithinRange( float testAngle, float a, float b )
65 {
66  a -= testAngle;
67  b -= testAngle;
68  normalize( a );
69  normalize( b );
70  if ( a * b >= 0 )
71  return false; // lying on the same side
72  return fabs( a - b ) < M_PI;
73 }
74 
75 
76 
77 /* check for both directions */
78 bool checkForAngleInterval(float orgAngle, float* dstAngle, float mainAngle, float mainAngleTol)
79 {
80  float tmpAngle;
81  bool bRet = isWithinRange(orgAngle, mainAngle - mainAngleTol, mainAngle + mainAngleTol);
82  if (bRet == true)
83  {
84  tmpAngle = mainAngle;
85  normalize(tmpAngle);
86  *dstAngle = tmpAngle;
87  return(bRet);
88  }
89  bRet = isWithinRange(orgAngle, M_PI + mainAngle - mainAngleTol, M_PI + mainAngle + mainAngleTol);
90  if (bRet == true)
91  {
92  tmpAngle = mainAngle + M_PI;
93  normalize(tmpAngle);
94  *dstAngle = tmpAngle;
95  return(bRet);
96 
97  }
98 
99 
100 
101  return(bRet);
102 
103 
104 
105 }
106 
107 bool checkForAngleIntervalTestbed()
108 {
109  bool bRet = false;
110  double testAngleArrDeg[] = {-161.0, -10.0, -20.0, -30.0, 330.0, 10.0, 20.0, 30.0, -160.0};
111 
112  std::vector<double> testAngleArr;
113 
114  for ( int i =0; i < sizeof(testAngleArrDeg)/sizeof(testAngleArrDeg[0]); i++)
115  {
116  double tmpAngle = testAngleArrDeg[i] / 180.0 * M_PI;
117  testAngleArr.push_back(tmpAngle);
118  }
119 
120 
121  double mainAngle = 20.0/180.0 * M_PI;
122  double mainAngleTol = 30.0/180.0 * M_PI;
123  for (int i = 0; i < testAngleArr.size(); i++)
124  {
125  float angle = testAngleArr[i];
126  float dstAngle;
127  bRet = checkForAngleInterval(angle, &dstAngle, mainAngle, mainAngleTol);
128 
129  printf("Ergebnis: %2d %6.2lf %6.2lf %s\n", i, angle, dstAngle, bRet ? "JA" : "NEIN");
130  }
131  return bRet;
132 }
133 
134 
135 
136 void callback(const sick_scan::RadarScan::ConstPtr &oa)
137  {
138  RadarObjectMarkerCfg *cfgPtr = &boost::serialization::singleton<RadarObjectMarkerCfg>::get_mutable_instance();
139 
140  enum TARGET_MARKER_TYPE {TARGET_MARKER_BALL, TARGET_MARKER_ARROW, TARGET_MARKER_NUM};
141  visualization_msgs::MarkerArray rawTargetArray[TARGET_MARKER_NUM]; // ball and arrow
142  sensor_msgs::PointCloud2 cloud_msg;
143 
144  for (int i = 0; i < TARGET_MARKER_NUM; i++)
145  {
146  rawTargetArray[i].markers.resize(oa->targets.width);
147  }
148 
149  int coordIdx[3] = {0};
150  int vradIdx = -1;
151  int amplitudeIdx = -1;
152 
153  for (int i = 0; i < 3; i++)
154  {
155  coordIdx[i] = -1;
156  }
157 
158 
159  bool useCurrTimeStamp = true;
160  ros::Time currTimeStamp = ros::Time::now(); // timestamp incoming package, will be overwritten by get_datagram
161  for (int i = 0; i < oa->targets.fields.size(); i++)
162  {
163  std::string fieldName = oa->targets.fields[i].name;
164  // x,y,z, vrad, amplitude
165  if (fieldName.compare("x") == 0)
166  {
167  coordIdx[0] = i;
168  }
169  if (fieldName.compare("y") == 0)
170  {
171  coordIdx[1] = i;
172  }
173  if (fieldName.compare("z") == 0)
174  {
175  coordIdx[2] = i;
176  }
177  if (fieldName.compare("vrad") == 0)
178  {
179  vradIdx = i;
180  }
181  if (fieldName.compare("amplitude") == 0)
182  {
183  amplitudeIdx = i;
184  }
185 
186  // printf("%s\n", fieldName.c_str());
187  }
188 
189  int numRawTargets = oa->targets.width;
190 
191  if (numRawTargets > 0)
192  {
193  cloud_msg.header = oa->header;
194 
195  if (useCurrTimeStamp)
196  {
197  cloud_msg.header.stamp = currTimeStamp;
198  }
199 
200  cloud_msg.height = oa->targets.height;
201  cloud_msg.width = oa->targets.width;
202  cloud_msg.row_step = oa->targets.row_step;
203  cloud_msg.point_step = oa->targets.point_step;
204  cloud_msg.data = oa->targets.data;
205  cloud_msg.fields = oa->targets.fields;
206  cloud_msg.is_dense = oa->targets.is_dense;
207  cloud_msg.is_bigendian = oa->targets.is_bigendian;
208  pub_cloud.publish(cloud_msg);
209  }
210  for (size_t i = 0; i < oa->targets.width; i++)
211  {
212  int tmpId = i;
213  float ampl = 0.0;
214  float xs, ys;
215  float pts3d[3];
216  float vrad = 0.0;
217  for (int ii = 0; ii < 3; ii++)
218  {
219  if (coordIdx[ii] != -1)
220  {
221  int tmpIdx = coordIdx[ii];
222  float *valPtr = (float *) (&(oa->targets.data[0]) + i * oa->targets.point_step +
223  oa->targets.fields[tmpIdx].offset);
224  pts3d[ii] = *valPtr;
225  }
226  }
227  if (vradIdx != -1)
228  {
229  int tmpIdx = vradIdx;
230  float *valPtr = (float *) (&(oa->targets.data[0]) + i * oa->targets.point_step +
231  oa->targets.fields[tmpIdx].offset);
232  vrad = *valPtr;
233  }
234  if (amplitudeIdx)
235  {
236  int tmpIdx = amplitudeIdx;
237  float *valPtr = (float *) (&(oa->targets.data[0]) + i * oa->targets.point_step +
238  oa->targets.fields[tmpIdx].offset);
239  ampl = *valPtr;
240 
241  }
242 
243  for (int iLoop = 0; iLoop < TARGET_MARKER_NUM; iLoop++)
244  {
245 
246  rawTargetArray[iLoop].markers[i].header = oa->header;
247 
248  if (useCurrTimeStamp)
249  {
250  rawTargetArray[iLoop].markers[i].header.stamp = currTimeStamp;
251  }
252 
253  rawTargetArray[iLoop].markers[i].ns = "rawtargets";
254  rawTargetArray[iLoop].markers[i].id = tmpId + iLoop * numRawTargets;
255  switch (iLoop)
256  {
257  case TARGET_MARKER_ARROW:
258  {
259  rawTargetArray[iLoop].markers[i].type = visualization_msgs::Marker::ARROW;
260  rawTargetArray[iLoop].markers[i].scale.x = 0.1;
261  rawTargetArray[iLoop].markers[i].scale.y = 0.2;
262 
263  rawTargetArray[iLoop].markers[i].points.resize(2);
264  rawTargetArray[iLoop].markers[i].points[0].x = pts3d[0];
265  rawTargetArray[iLoop].markers[i].points[0].y = pts3d[1];
266  rawTargetArray[iLoop].markers[i].points[0].z = pts3d[2];
267  float lineOfSight = atan2(pts3d[1], pts3d[0]);
268  rawTargetArray[iLoop].markers[i].points[1].x = pts3d[0] + cos(lineOfSight) * vrad * 0.1;
269  rawTargetArray[iLoop].markers[i].points[1].y = pts3d[1] + sin(lineOfSight) * vrad * 0.1;
270  rawTargetArray[iLoop].markers[i].points[1].z = pts3d[2];
271 
272  }
273  break;
274  case TARGET_MARKER_BALL:
275  rawTargetArray[iLoop].markers[i].type = visualization_msgs::Marker::SPHERE;
276  rawTargetArray[iLoop].markers[i].scale.x = ballRadius;
277  rawTargetArray[iLoop].markers[i].scale.y = ballRadius;
278  rawTargetArray[iLoop].markers[i].scale.z = ballRadius;
279 
280 
281  rawTargetArray[iLoop].markers[i].pose.position.x = pts3d[0];
282  rawTargetArray[iLoop].markers[i].pose.position.y = pts3d[1];
283  rawTargetArray[iLoop].markers[i].pose.position.z = pts3d[2];
284  rawTargetArray[iLoop].markers[i].pose.orientation.x = 0.0;
285  rawTargetArray[iLoop].markers[i].pose.orientation.y = 0.0;
286  rawTargetArray[iLoop].markers[i].pose.orientation.z = 0.0;
287  rawTargetArray[iLoop].markers[i].pose.orientation.w = 1.0;
288  break;
289 
290  }
291  rawTargetArray[iLoop].markers[i].action = visualization_msgs::Marker::ADD;
292  if (cfgPtr->isPaletteUsed())
293  {
294  GnuPlotPalette pal = cfgPtr->getGnuPlotPalette();
295  float idxRange = ampl - cfgPtr->getPaletteMinAmpl();
296  double rangeAmpl = cfgPtr->getPaletteMaxAmpl() - cfgPtr->getPaletteMinAmpl();
297  idxRange /= rangeAmpl;
298  if (idxRange < 0.0)
299  {
300  idxRange = 0.0;
301  }
302  if (idxRange > 1.0)
303  {
304  idxRange = 1.0;
305  }
306  unsigned char greyIdx = (unsigned char)(255.999 * idxRange);
307  unsigned char red = pal.getRbgValue(greyIdx, 0U);
308  unsigned char green = pal.getRbgValue(greyIdx, 1U);
309  unsigned char blue = pal.getRbgValue(greyIdx, 2U);
310 
311  float redF, greenF, blueF;
312  redF = red / 255.00;
313  greenF = green / 255.00;
314  blueF = blue / 255.00;
315  rawTargetArray[iLoop].markers[i].color.a = 0.75;
316  rawTargetArray[iLoop].markers[i].color.r = redF;
317  rawTargetArray[iLoop].markers[i].color.g = greenF;
318  rawTargetArray[iLoop].markers[i].color.b = blueF;
319  }
320  else{
321  rawTargetArray[iLoop].markers[i].color.a = 0.75;
322  rawTargetArray[iLoop].markers[i].color.r = GLASBEY_LUT[tmpId * 3] / 255.0;
323  rawTargetArray[iLoop].markers[i].color.g = GLASBEY_LUT[tmpId * 3 + 1] / 255.0;
324  rawTargetArray[iLoop].markers[i].color.b = GLASBEY_LUT[tmpId * 3 + 2] / 255.0;
325  }
326  rawTargetArray[iLoop].markers[i].lifetime = ros::Duration(0.5);
327 
328 
329  }
330  }
331 
332  for (int iLoop = 0; iLoop < TARGET_MARKER_NUM; iLoop++)
333  {
334  pub.publish(rawTargetArray[iLoop]);
335  }
336 
337  /***********************************************************************
338  *
339  *
340  * Drawing marker for objects
341  *
342  ***********************************************************************
343  */
344  visualization_msgs::MarkerArray object_boxes;
345  object_boxes.markers.resize(2 * oa->objects.size());
346 
347  visualization_msgs::MarkerArray object_labels;
348  object_labels.markers.resize(1 * oa->objects.size());
349 
350  float vp_dir = 0.0;
351  float vp_dir_deg = 0.0;
352 
353  double angleMainDirDeg = 20;
354  double angleMainDir = angleMainDirDeg / 180.0 * M_PI;
355  double angleMainDirTolDeg = 20.0;
356  double angleMainDirTol = angleMainDirTolDeg / 180.0 * M_PI;
357 
358  std::vector<float> vehiclePredefinedDir;
359  std::vector<float> vehicleAbsoluteSpeed;
360  std::vector<bool> vehicleIsInPredefinedDir;
361 
362  int objNum = oa->objects.size();
363 
364  vehiclePredefinedDir.resize(objNum);
365  vehicleIsInPredefinedDir.resize(objNum); // flag arraw whether the vehicle drives in tihs dir. or not
366  vehicleAbsoluteSpeed.resize(objNum);
367 
368  for (int iLoop = 0; iLoop < 2; iLoop++)
369  {
370  for (size_t i = 0; i < oa->objects.size(); i++)
371  {
372  int colorId = oa->objects[i].id % 256;
373  float vp[3];
374  vp[0] = oa->objects[i].velocity.twist.linear.x;
375  vp[1] = oa->objects[i].velocity.twist.linear.y;
376  vp[2] = oa->objects[i].velocity.twist.linear.z;
377  float vabs = sqrt(vp[0] * vp[0] + vp[1]*vp[1] + vp[2]*vp[2]);
378  vehicleAbsoluteSpeed[i] = vabs;
379  vp_dir = atan2(vp[1], vp[0]);
380  vp_dir_deg = vp_dir / 3.141592 * 180.0;
381  float dstAngle;
382 
383  bool bRet = checkForAngleInterval(vp_dir, &dstAngle, angleMainDir, angleMainDirTol);
384  vehiclePredefinedDir[i] = dstAngle;
385  vehicleIsInPredefinedDir[i] = bRet;
386 
387  int idx = i + iLoop * oa->objects.size();
388  int tmpId = i; // better: oa->objects[i].id;
389  object_boxes.markers[idx].header = oa->header;
390 
391  if (useCurrTimeStamp)
392  {
393  object_boxes.markers[idx].header.stamp = currTimeStamp;
394  }
395  std::string nameSpaceBoxes = "object_boxes_slow";
396  if (vabs > 5.0)
397  {
398  nameSpaceBoxes = "object_boxes_fast";
399  }
400  object_boxes.markers[idx].ns = nameSpaceBoxes;
401  object_boxes.markers[idx].id = tmpId + iLoop * oa->objects.size(); // i; // oa->objects[i].id;
402  object_boxes.markers[idx].action = visualization_msgs::Marker::ADD;
403  object_boxes.markers[idx].color.a = 0.75;
404  object_boxes.markers[idx].color.r = GLASBEY_LUT[colorId * 3] / 255.0;
405  object_boxes.markers[idx].color.g = GLASBEY_LUT[colorId * 3 + 1] / 255.0;
406  object_boxes.markers[idx].color.b = GLASBEY_LUT[colorId * 3 + 2] / 255.0;
407  object_boxes.markers[idx].lifetime = ros::Duration(1.0);
408 
409  if (iLoop == 0)
410  {
411  object_labels.markers[i].header = oa->header;
412  if (useCurrTimeStamp)
413  {
414  if (iLoop == 0) {
415  object_labels.markers[i].header.stamp = currTimeStamp;
416  }
417  }
418 
419  object_labels.markers[i].id = tmpId + iLoop * oa->objects.size(); // i; // oa->objects[i].id;
420  object_labels.markers[i].action = visualization_msgs::Marker::ADD;
421  object_labels.markers[i].color.a = 0.75;
422  object_labels.markers[i].color.r = GLASBEY_LUT[colorId * 3] / 255.0;
423  object_labels.markers[i].color.g = GLASBEY_LUT[colorId * 3 + 1] / 255.0;
424  object_labels.markers[i].color.b = GLASBEY_LUT[colorId * 3 + 2] / 255.0;
425  object_labels.markers[i].lifetime = ros::Duration(1.0);
426 
427  object_labels.markers[i].type = visualization_msgs::Marker::TEXT_VIEW_FACING;
428  object_labels.markers[i].pose = oa->objects[i].object_box_center.pose;
429  object_labels.markers[i].scale.z = 1.0;
430 
431  {
432 
433  char szLabel[255];
434  sprintf(szLabel,"%5.1lf [m/s]", vabs );
435 
436  if (vabs > 10.0)
437  {
438  // printf("Dir: %8.3lf [deg]\n", vp_dir_deg);
439  }
440  std::string object_label_ns = "???";
441  if (vabs > 5.0) // 18 km/h
442  {
443  object_label_ns = "object_label_fast";
444  }
445  else
446  {
447  object_label_ns = "object_label_slow";
448 
449  }
450  object_labels.markers[i].text = szLabel;
451  object_labels.markers[i].ns = object_label_ns;
452 
453  if (vehicleIsInPredefinedDir[i] == false)
454  {
455  object_labels.markers[i].scale.z = 0.00;
456  }
457  }
458 
459 
460  }
461 
462  /* object box center is the reference ...
463  * */
464  object_boxes.markers[idx].pose = oa->objects[i].object_box_center.pose; // Position and Orientation
465  object_boxes.markers[idx].scale = oa->objects[i].object_box_size;
466 
467  switch (iLoop)
468  {
469  case 0: // Drawing object cubes
470  object_boxes.markers[idx].type = visualization_msgs::Marker::CUBE;
471  object_boxes.markers[idx].pose.position.z += oa->objects[i].object_box_size.z * 0.5;
472 
473 
474  break;
475  case 1:
476  object_boxes.markers[idx].type = visualization_msgs::Marker::ARROW;
477 
478  break;
479  }
480 
481  switch (iLoop)
482  {
483  case 0:
484  if (object_boxes.markers[idx].scale.x == 0.0)
485  {
486  object_boxes.markers[idx].scale.x = 0.01;
487  }
488  if (object_boxes.markers[idx].scale.y == 0.0)
489  {
490  object_boxes.markers[idx].scale.y = 0.01;
491  }
492  if (object_boxes.markers[idx].scale.z == 0.0)
493  {
494  object_boxes.markers[idx].scale.z = 0.01;
495  }
496 
497  if (vehicleIsInPredefinedDir[i] == false) {
498  object_boxes.markers[idx].scale.x = 0.0;
499  object_boxes.markers[idx].scale.y = 0.0;
500  object_boxes.markers[idx].scale.z = 0.0;
501  object_boxes.markers[idx].lifetime = ros::Duration(0.001);
502  } else{
503 
504 
505  if (vehicleIsInPredefinedDir[i] == true)
506  {
507  float theta = vehiclePredefinedDir[i];
508 
509 
510  // Falls wir eine Zwangsrichtung einpraegen wollen, dann muessen wir es am besten hier tun.
511 
512  // (n_x, n_y, n_z) = (0, 0, 1), so (x, y, z, w) = (0, 0, sin(theta/2), cos(theta/2)).
513  // see https://answers.ros.org/question/9772/quaternions-orientation-representation/
514  float theta2 = 0.5 * theta;
515  object_boxes.markers[idx].pose.orientation.z = sin(theta2);
516  object_boxes.markers[idx].pose.orientation.w = cos(theta2);
517 
518  }
519 
520 
521  }
522  break;
523  case 1:
524  {
525  float xTmp;
526  float yTmp;
527 
528  object_boxes.markers[idx].scale.x = 0.5;
529  object_boxes.markers[idx].scale.y = 0.5;
530  object_boxes.markers[idx].scale.z = 0.5;
531 
532 
533 
534 /* float cosVal = oa->objects[i].object_box_center.pose.orientation.x;
535  float sinVal = oa->objects[i].object_box_center.pose.orientation.y;
536 */
537  xTmp = oa->objects[i].object_box_center.pose.position.x;
538  yTmp = oa->objects[i].object_box_center.pose.position.y;
539 
540  double vehLen = oa->objects[i].object_box_size.x;
541 
542 
543  float quaternion[4] = {0};
544  // rotation around z-axis: [cos(theta/2.0), 0, 0, sin(theta/2.0) ]
545  quaternion[0] = oa->objects[i].object_box_center.pose.orientation.x;
546  quaternion[1] = oa->objects[i].object_box_center.pose.orientation.y;
547  quaternion[2] = oa->objects[i].object_box_center.pose.orientation.z;
548  quaternion[3] = oa->objects[i].object_box_center.pose.orientation.w;
549 
550 
551  // Falls wir eine Zwangsrichtung einpraegen wollen, dann muessen wir es am besten hier tun.
552 
553  // (n_x, n_y, n_z) = (0, 0, 1), so (x, y, z, w) = (0, 0, sin(theta/2), cos(theta/2)).
554  // see https://answers.ros.org/question/9772/quaternions-orientation-representation/
555  float theta2 = atan2(quaternion[2], quaternion[3]);
556  float theta = 2.0f * theta2;
557  if (vehicleIsInPredefinedDir[i] == true)
558  {
559  theta = vehiclePredefinedDir[i];
560  }
561 
562  float cosVal = cos(theta);
563  float sinVal = sin(theta);
564  xTmp += cosVal * vehLen * 0.5;
565  yTmp += sinVal * vehLen * 0.5;
566  object_boxes.markers[idx].pose.position.x = xTmp;
567  object_boxes.markers[idx].pose.position.y = yTmp;
568  object_boxes.markers[idx].pose.position.z = 0.0;
569 
570  // oa->objects[i].object_box_center.pose.orientation;
571  // Arrow orientation is already rotated
572  object_boxes.markers[idx].pose.orientation.x = 0.0;
573  object_boxes.markers[idx].pose.orientation.y = 0.0;
574  object_boxes.markers[idx].pose.orientation.z = 0.0;
575  object_boxes.markers[idx].pose.orientation.w = 0.0;
576 
577  object_boxes.markers[idx].points.resize(2);
578  object_boxes.markers[idx].points[0].x = 0.0;
579  object_boxes.markers[idx].points[0].y = 0.0;
580  object_boxes.markers[idx].points[0].z = 0.0;
581 
582  float absSpeed = 0.0;
583  for (int j = 0; j < 3; j++)
584  {
585  float speedPartial = 0.0;
586  switch(j)
587  {
588  case 0: speedPartial = oa->objects[i].velocity.twist.linear.x; break;
589  case 1: speedPartial = oa->objects[i].velocity.twist.linear.y; break;
590  case 2: speedPartial = oa->objects[i].velocity.twist.linear.z; break;
591  }
592  absSpeed += speedPartial * speedPartial;
593  }
594  absSpeed = sqrt(absSpeed);
595  double lengthOfArrow = objectArrowScale * absSpeed;
596  if (objectArrowScale < 0.0)
597  {
598  lengthOfArrow = -objectArrowScale; // if scale is negative take its absolute value as length of arrow in [m]
599  }
600  // lengthOfArrow = 5.0;
601 
602  if (vehicleIsInPredefinedDir[i] == false) {
603  object_boxes.markers[idx].lifetime = ros::Duration(0.001);
604  lengthOfArrow = 0.0;
605  }
606  object_boxes.markers[idx].points[1].x = 0.0 + lengthOfArrow * cosVal;
607  object_boxes.markers[idx].points[1].y = 0.0 + lengthOfArrow * sinVal;
608  object_boxes.markers[idx].points[1].z = 0.0;
609  // pub.publish(object_boxes);
610 
611  }
612  break;
613  }
614 
615  // printf("Idx: %2d X: %8.3lf Y: %8.3lf\n", idx, object_boxes.markers[idx].pose.position.x, object_boxes.markers[idx].pose.position.y);
616  }
617  }
618  pub.publish(object_boxes);
619  pub.publish(object_labels);
620 
621  }
622 
623 
624  int main(int argc, char **argv)
625  {
626  // checkForAngleIntervalTestbed();
627  ROS_INFO("radar_object_marker, compiled at [%s] [%s]", __DATE__, __TIME__);
628  RadarObjectMarkerCfg *cfgPtr = &boost::serialization::singleton<RadarObjectMarkerCfg>::get_mutable_instance();
629  ros::init(argc, argv, "radar_object_marker");
630  ros::NodeHandle nh;
631  ros::NodeHandle nhPriv("~");
632 
633  std::string heatMap;
634  double minAmpl = 10.0;
635  double maxAmpl = 90.0;
636  bool paramRes = nhPriv.getParam("rawtarget_sphere_radius", ballRadius);
637  paramRes = nhPriv.getParam("rawtarget_palette_name", heatMap);
638  paramRes = nhPriv.getParam("rawtarget_palette_min_ampl", minAmpl);
639  paramRes = nhPriv.getParam("rawtarget_palette_max_ampl", maxAmpl);
640 
641  paramRes = nhPriv.getParam("object_arrow_scale", objectArrowScale);
642 
643  cfgPtr->setPaletteName(heatMap);
644  cfgPtr->setPaletteMinAmpl(minAmpl);
645  cfgPtr->setPaletteMaxAmpl(maxAmpl);
646 
647 
648 
649  std::string path = ros::package::getPath("sick_scan");
650 
651  if (cfgPtr->getPaletteName().length() > 0)
652  {
653  std::string heatMapFileName = path + "/config/" + cfgPtr->getPaletteName();
654  pal.load(heatMapFileName);
655  usingPal = true;
656  cfgPtr->setGnuPlotPalette(pal);
657  cfgPtr->setPaletteUsed(true);
658  }
659  else
660  {
661  cfgPtr->setPaletteUsed(false);
662  }
663 
664  ROS_INFO("Subscribing to radar and pulishing to radar_markers");
665 
666  ros::Subscriber sub = nh.subscribe("radar", 1, callback);
667  pub = nh.advertise<visualization_msgs::MarkerArray>("radar_markers", 1);
668 
669  pub_cloud = nh.advertise<sensor_msgs::PointCloud2>("radar_cloud", 1);
670 
671  ros::spin();
672 
673  return 0;
674  }
pub_cloud
ros::Publisher pub_cloud
Definition: radar_object_marker.cpp:53
RadarObjectMarkerCfg::setGnuPlotPalette
void setGnuPlotPalette(const GnuPlotPalette &gnuPlotPalette)
Definition: radar_object_marker.h:287
ros::NodeHandle
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
red
static std_msgs::ColorRGBA red(void)
Definition: sick_scan_marker.cpp:54
main
int main(int argc, char **argv)
Definition: radar_object_marker.cpp:624
ros::NodeHandle::subscribe
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
GLASBEY_LUT
static const unsigned char GLASBEY_LUT[]
Definition: radar_object_marker.h:10
ros::init
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
checkForAngleIntervalTestbed
bool checkForAngleIntervalTestbed()
Definition: radar_object_marker.cpp:107
RadarObjectMarkerCfg::getPaletteMinAmpl
double getPaletteMinAmpl() const
Definition: radar_object_marker.h:311
usingPal
bool usingPal
Definition: radar_object_marker.cpp:50
RadarObjectMarkerCfg::setPaletteUsed
void setPaletteUsed(bool paletteUsed)
Definition: radar_object_marker.h:277
GnuPlotPalette::load
int load(std::string palettName)
Definition: gnuplotPaletteReader.cpp:48
normalize
void normalize(float &angle)
Definition: radar_object_marker.cpp:58
pub
ros::Publisher pub
Definition: radar_object_marker.cpp:52
angle
TFSIMD_FORCE_INLINE tfScalar angle(const Quaternion &q1, const Quaternion &q2)
RadarObjectMarkerCfg::getGnuPlotPalette
const GnuPlotPalette & getGnuPlotPalette() const
Definition: radar_object_marker.h:282
ros::spin
ROSCPP_DECL void spin(Spinner &spinner)
GnuPlotPalette::getRbgValue
unsigned char getRbgValue(unsigned char greyIdx, unsigned int channelIdx)
Definition: gnuplotPaletteReader.cpp:163
checkForAngleInterval
bool checkForAngleInterval(float orgAngle, float *dstAngle, float mainAngle, float mainAngleTol)
Definition: radar_object_marker.cpp:78
ROS_INFO
#define ROS_INFO(...)
RadarObjectMarkerCfg::getPaletteMaxAmpl
double getPaletteMaxAmpl() const
Definition: radar_object_marker.h:301
blue
static std_msgs::ColorRGBA blue(void)
Definition: sick_scan_marker.cpp:64
ros::NodeHandle::advertise
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
RadarObjectMarkerCfg::getPaletteName
const std::string & getPaletteName() const
Definition: radar_object_marker.h:292
ros::package::getPath
ROSLIB_DECL std::string getPath(const std::string &package_name)
objectArrowScale
float objectArrowScale
Definition: radar_object_marker.cpp:48
green
static std_msgs::ColorRGBA green(void)
Definition: sick_scan_marker.cpp:59
RadarObjectMarkerCfg::setPaletteName
void setPaletteName(const std::string &paletteName)
Definition: radar_object_marker.h:297
ros::NodeHandle::getParam
bool getParam(const std::string &key, std::string &s) const
callback
void callback(const sick_scan::RadarScan::ConstPtr &oa)
Definition: radar_object_marker.cpp:136
ros::Time::now
static Time now()
RadarObjectMarkerCfg::setPaletteMinAmpl
void setPaletteMinAmpl(double paletteMinAmpl)
Definition: radar_object_marker.h:316
pal
GnuPlotPalette pal
Definition: radar_object_marker.cpp:49
radar_object_marker.h
ballRadius
float ballRadius
Definition: radar_object_marker.cpp:47
RadarObjectMarkerCfg::setPaletteMaxAmpl
void setPaletteMaxAmpl(double paletteMaxAmpl)
Definition: radar_object_marker.h:306
isWithinRange
bool isWithinRange(float testAngle, float a, float b)
Definition: radar_object_marker.cpp:64

sick_scan
Author(s): Michael Lehning , Jochen Sprickerhof , Martin Günther
autogenerated on Wed May 5 2021 03:05:48