sick_ldmrs_object_marker.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2016, DFKI GmbH
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
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 DFKI GmbH nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * 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 THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  *
29  * Authors:
30  * Martin Günther <martin.guenther@dfki.de>
31  * Jochen Sprickerhof <ros@jochen.sprickerhof.de>
32  *
33  */
34 
35 #include <ros/ros.h>
36 
37 #include <visualization_msgs/MarkerArray.h>
38 #include <sick_ldmrs_msgs/ObjectArray.h>
39 
40 static const unsigned char GLASBEY_LUT[] =
41 {
42  255, 255, 255,
43  0, 0, 255,
44  255, 0, 0,
45  0, 255, 0,
46  0, 0, 51,
47  255, 0, 182,
48  0, 83, 0,
49  255, 211, 0,
50  0, 159, 255,
51  154, 77, 66,
52  0, 255, 190,
53  120, 63, 193,
54  31, 150, 152,
55  255, 172, 253,
56  177, 204, 113,
57  241, 8, 92,
58  254, 143, 66,
59  221, 0, 255,
60  32, 26, 1,
61  114, 0, 85,
62  118, 108, 149,
63  2, 173, 36,
64  200, 255, 0,
65  136, 108, 0,
66  255, 183, 159,
67  133, 133, 103,
68  161, 3, 0,
69  20, 249, 255,
70  0, 71, 158,
71  220, 94, 147,
72  147, 212, 255,
73  0, 76, 255,
74  0, 66, 80,
75  57, 167, 106,
76  238, 112, 254,
77  0, 0, 100,
78  171, 245, 204,
79  161, 146, 255,
80  164, 255, 115,
81  255, 206, 113,
82  71, 0, 21,
83  212, 173, 197,
84  251, 118, 111,
85  171, 188, 0,
86  117, 0, 215,
87  166, 0, 154,
88  0, 115, 254,
89  165, 93, 174,
90  98, 132, 2,
91  0, 121, 168,
92  0, 255, 131,
93  86, 53, 0,
94  159, 0, 63,
95  66, 45, 66,
96  255, 242, 187,
97  0, 93, 67,
98  252, 255, 124,
99  159, 191, 186,
100  167, 84, 19,
101  74, 39, 108,
102  0, 16, 166,
103  145, 78, 109,
104  207, 149, 0,
105  195, 187, 255,
106  253, 68, 64,
107  66, 78, 32,
108  106, 1, 0,
109  181, 131, 84,
110  132, 233, 147,
111  96, 217, 0,
112  255, 111, 211,
113  102, 75, 63,
114  254, 100, 0,
115  228, 3, 127,
116  17, 199, 174,
117  210, 129, 139,
118  91, 118, 124,
119  32, 59, 106,
120  180, 84, 255,
121  226, 8, 210,
122  0, 1, 20,
123  93, 132, 68,
124  166, 250, 255,
125  97, 123, 201,
126  98, 0, 122,
127  126, 190, 58,
128  0, 60, 183,
129  255, 253, 0,
130  7, 197, 226,
131  180, 167, 57,
132  148, 186, 138,
133  204, 187, 160,
134  55, 0, 49,
135  0, 40, 1,
136  150, 122, 129,
137  39, 136, 38,
138  206, 130, 180,
139  150, 164, 196,
140  180, 32, 128,
141  110, 86, 180,
142  147, 0, 185,
143  199, 48, 61,
144  115, 102, 255,
145  15, 187, 253,
146  172, 164, 100,
147  182, 117, 250,
148  216, 220, 254,
149  87, 141, 113,
150  216, 85, 34,
151  0, 196, 103,
152  243, 165, 105,
153  216, 255, 182,
154  1, 24, 219,
155  52, 66, 54,
156  255, 154, 0,
157  87, 95, 1,
158  198, 241, 79,
159  255, 95, 133,
160  123, 172, 240,
161  120, 100, 49,
162  162, 133, 204,
163  105, 255, 220,
164  198, 82, 100,
165  121, 26, 64,
166  0, 238, 70,
167  231, 207, 69,
168  217, 128, 233,
169  255, 211, 209,
170  209, 255, 141,
171  36, 0, 3,
172  87, 163, 193,
173  211, 231, 201,
174  203, 111, 79,
175  62, 24, 0,
176  0, 117, 223,
177  112, 176, 88,
178  209, 24, 0,
179  0, 30, 107,
180  105, 200, 197,
181  255, 203, 255,
182  233, 194, 137,
183  191, 129, 46,
184  69, 42, 145,
185  171, 76, 194,
186  14, 117, 61,
187  0, 30, 25,
188  118, 73, 127,
189  255, 169, 200,
190  94, 55, 217,
191  238, 230, 138,
192  159, 54, 33,
193  80, 0, 148,
194  189, 144, 128,
195  0, 109, 126,
196  88, 223, 96,
197  71, 80, 103,
198  1, 93, 159,
199  99, 48, 60,
200  2, 206, 148,
201  139, 83, 37,
202  171, 0, 255,
203  141, 42, 135,
204  85, 83, 148,
205  150, 255, 0,
206  0, 152, 123,
207  255, 138, 203,
208  222, 69, 200,
209  107, 109, 230,
210  30, 0, 68,
211  173, 76, 138,
212  255, 134, 161,
213  0, 35, 60,
214  138, 205, 0,
215  111, 202, 157,
216  225, 75, 253,
217  255, 176, 77,
218  229, 232, 57,
219  114, 16, 255,
220  111, 82, 101,
221  134, 137, 48,
222  99, 38, 80,
223  105, 38, 32,
224  200, 110, 0,
225  209, 164, 255,
226  198, 210, 86,
227  79, 103, 77,
228  174, 165, 166,
229  170, 45, 101,
230  199, 81, 175,
231  255, 89, 172,
232  146, 102, 78,
233  102, 134, 184,
234  111, 152, 255,
235  92, 255, 159,
236  172, 137, 178,
237  210, 34, 98,
238  199, 207, 147,
239  255, 185, 30,
240  250, 148, 141,
241  49, 34, 78,
242  254, 81, 97,
243  254, 141, 100,
244  68, 54, 23,
245  201, 162, 84,
246  199, 232, 240,
247  68, 152, 0,
248  147, 172, 58,
249  22, 75, 28,
250  8, 84, 121,
251  116, 45, 0,
252  104, 60, 255,
253  64, 41, 38,
254  164, 113, 215,
255  207, 0, 155,
256  118, 1, 35,
257  83, 0, 88,
258  0, 82, 232,
259  43, 92, 87,
260  160, 217, 146,
261  176, 26, 229,
262  29, 3, 36,
263  122, 58, 159,
264  214, 209, 207,
265  160, 100, 105,
266  106, 157, 160,
267  153, 219, 113,
268  192, 56, 207,
269  125, 255, 89,
270  149, 0, 34,
271  213, 162, 223,
272  22, 131, 204,
273  166, 249, 69,
274  109, 105, 97,
275  86, 188, 78,
276  255, 109, 81,
277  255, 3, 248,
278  255, 0, 73,
279  202, 0, 35,
280  67, 109, 18,
281  234, 170, 173,
282  191, 165, 0,
283  38, 44, 51,
284  85, 185, 2,
285  121, 182, 158,
286  254, 236, 212,
287  139, 165, 89,
288  141, 254, 193,
289  0, 60, 43,
290  63, 17, 40,
291  255, 221, 246,
292  17, 26, 146,
293  154, 66, 84,
294  149, 157, 238,
295  126, 130, 72,
296  58, 6, 101,
297  189, 117, 101,
298 };
299 
300 ros::Publisher pub;
301 
302 void callback(const sick_ldmrs_msgs::ObjectArray::ConstPtr& oa)
303 {
304  visualization_msgs::MarkerArray velocity;
305  velocity.markers.resize(oa->objects.size());
306 
307  for (size_t i = 0; i < oa->objects.size(); i++)
308  {
309  velocity.markers[i].header = oa->header;
310  velocity.markers[i].ns = "velocities";
311  velocity.markers[i].id = oa->objects[i].id;
312  velocity.markers[i].type = visualization_msgs::Marker::ARROW;
313  velocity.markers[i].action = visualization_msgs::Marker::ADD;
314  velocity.markers[i].scale.x = 0.1;
315  velocity.markers[i].scale.y = 0.2;
316  velocity.markers[i].color.a = 0.75;
317  velocity.markers[i].color.r = GLASBEY_LUT[oa->objects[i].id * 3] / 255.0;
318  velocity.markers[i].color.g = GLASBEY_LUT[oa->objects[i].id * 3 + 1] / 255.0;
319  velocity.markers[i].color.b = GLASBEY_LUT[oa->objects[i].id * 3 + 2] / 255.0;
320  velocity.markers[i].lifetime = ros::Duration(0.5);
321 
322  velocity.markers[i].points.resize(2);
323  velocity.markers[i].points[0] = oa->objects[i].object_box_center.pose.position;
324  velocity.markers[i].points[1].x = oa->objects[i].object_box_center.pose.position.x + oa->objects[i].velocity.twist.linear.x;
325  velocity.markers[i].points[1].y = oa->objects[i].object_box_center.pose.position.y + oa->objects[i].velocity.twist.linear.y;
326  }
327 
328  pub.publish(velocity);
329 
330  visualization_msgs::MarkerArray bounding_box;
331  bounding_box.markers.resize(oa->objects.size());
332 
333  for (size_t i = 0; i < oa->objects.size(); i++)
334  {
335  bounding_box.markers[i].header = oa->header;
336  bounding_box.markers[i].ns = "bounding_boxes";
337  bounding_box.markers[i].id = oa->objects[i].id;
338  bounding_box.markers[i].type = visualization_msgs::Marker::CUBE;
339  bounding_box.markers[i].action = visualization_msgs::Marker::ADD;
340  bounding_box.markers[i].color.a = 0.75;
341  bounding_box.markers[i].color.r = GLASBEY_LUT[oa->objects[i].id * 3] / 255.0;
342  bounding_box.markers[i].color.g = GLASBEY_LUT[oa->objects[i].id * 3 + 1] / 255.0;
343  bounding_box.markers[i].color.b = GLASBEY_LUT[oa->objects[i].id * 3 + 2] / 255.0;
344  bounding_box.markers[i].lifetime = ros::Duration(0.5);
345 
346  bounding_box.markers[i].pose = oa->objects[i].bounding_box_center;
347  bounding_box.markers[i].scale = oa->objects[i].bounding_box_size;
348 
349  if (bounding_box.markers[i].scale.x == 0.0)
350  {
351  bounding_box.markers[i].scale.x = 0.01;
352  }
353  if (bounding_box.markers[i].scale.y == 0.0)
354  {
355  bounding_box.markers[i].scale.y = 0.01;
356  }
357  bounding_box.markers[i].scale.z = 0.2;
358  }
359 
360  pub.publish(bounding_box);
361 
362  visualization_msgs::MarkerArray object_boxes;
363  object_boxes.markers.resize(oa->objects.size());
364 
365  for (size_t i = 0; i < oa->objects.size(); i++)
366  {
367  object_boxes.markers[i].header = oa->header;
368  object_boxes.markers[i].ns = "object_boxes";
369  object_boxes.markers[i].id = oa->objects[i].id;
370  object_boxes.markers[i].type = visualization_msgs::Marker::CUBE;
371  object_boxes.markers[i].action = visualization_msgs::Marker::ADD;
372  object_boxes.markers[i].color.a = 0.75;
373  object_boxes.markers[i].color.r = GLASBEY_LUT[oa->objects[i].id * 3] / 255.0;
374  object_boxes.markers[i].color.g = GLASBEY_LUT[oa->objects[i].id * 3 + 1] / 255.0;
375  object_boxes.markers[i].color.b = GLASBEY_LUT[oa->objects[i].id * 3 + 2] / 255.0;
376  object_boxes.markers[i].lifetime = ros::Duration(0.5);
377 
378  object_boxes.markers[i].pose = oa->objects[i].object_box_center.pose;
379  object_boxes.markers[i].scale = oa->objects[i].object_box_size;
380 
381  if (object_boxes.markers[i].scale.x == 0.0)
382  {
383  object_boxes.markers[i].scale.x = 0.01;
384  }
385  if (object_boxes.markers[i].scale.y == 0.0)
386  {
387  object_boxes.markers[i].scale.y = 0.01;
388  }
389  object_boxes.markers[i].scale.z = 0.2;
390  }
391 
392  pub.publish(object_boxes);
393 
394  visualization_msgs::MarkerArray contour_lines;
395  contour_lines.markers.resize(oa->objects.size());
396 
397  for (size_t i = 0; i < oa->objects.size(); i++)
398  {
399  contour_lines.markers[i].header = oa->header;
400  contour_lines.markers[i].ns = "contour_lines";
401  contour_lines.markers[i].id = oa->objects[i].id;
402  contour_lines.markers[i].type = visualization_msgs::Marker::LINE_STRIP;
403  contour_lines.markers[i].action = visualization_msgs::Marker::ADD;
404  contour_lines.markers[i].scale.x = 0.1;
405  contour_lines.markers[i].color.a = 0.75;
406  contour_lines.markers[i].color.r = GLASBEY_LUT[oa->objects[i].id * 3] / 255.0;
407  contour_lines.markers[i].color.g = GLASBEY_LUT[oa->objects[i].id * 3 + 1] / 255.0;
408  contour_lines.markers[i].color.b = GLASBEY_LUT[oa->objects[i].id * 3 + 2] / 255.0;
409  contour_lines.markers[i].lifetime = ros::Duration(0.5);
410 
411  contour_lines.markers[i].points = oa->objects[i].contour_points;
412  }
413 
414  pub.publish(contour_lines);
415 }
416 
417 
418 int main(int argc, char **argv)
419 {
420  ros::init(argc, argv, "sick_ldmrs_object_marker");
421  ros::NodeHandle nh;
422 
423  ros::Subscriber sub = nh.subscribe("objects", 1, callback);
424  pub = nh.advertise<visualization_msgs::MarkerArray>("object_markers", 1);
425 
426  ros::spin();
427 
428  return 0;
429 }
ros::NodeHandle
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
ros::NodeHandle::subscribe
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
main
int main(int argc, char **argv)
Definition: sick_ldmrs_object_marker.cpp:418
ros::init
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
pub
ros::Publisher pub
Definition: sick_ldmrs_object_marker.cpp:300
ros::spin
ROSCPP_DECL void spin(Spinner &spinner)
GLASBEY_LUT
static const unsigned char GLASBEY_LUT[]
Definition: sick_ldmrs_object_marker.cpp:40
callback
void callback(const sick_ldmrs_msgs::ObjectArray::ConstPtr &oa)
Definition: sick_ldmrs_object_marker.cpp:302
ros::NodeHandle::advertise
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)

sick_ldmrs_tools
Author(s): Martin Günther , Jochen Sprickerhof
autogenerated on Sun Oct 25 2020 03:33:58