visualization_rviz.cpp
Go to the documentation of this file.
1 /**************************************************************************************************
2  Software License Agreement (BSD License)
3 
4  Copyright (c) 2011-2013, LAR toolkit developers - University of Aveiro - http://lars.mec.ua.pt
5  All rights reserved.
6 
7  Redistribution and use in source and binary forms, with or without modification, are permitted
8  provided that the following conditions are met:
9 
10  *Redistributions of source code must retain the above copyright notice, this list of
11  conditions and the following disclaimer.
12  *Redistributions in binary form must reproduce the above copyright notice, this list of
13  conditions and the following disclaimer in the documentation and/or other materials provided
14  with the distribution.
15  *Neither the name of the University of Aveiro nor the names of its contributors may be used to
16  endorse or promote products derived from this software without specific prior written permission.
17 
18  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR
19  IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
20  FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
21  CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
22  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
23  DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
24  IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
25  OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 ***************************************************************************************************/
33 #include "lidar_segmentation.h"
34 #include "clustering.h"
35 #include "visualization_rviz.h"
36 
37 vector<visualization_msgs::Marker> createTargetMarkers( vector<ClusterPtr>& clusters , vector<ClusterPtr>& clusters_Premebida , vector<ClusterPtr>& clusters_Dietmayer, vector<ClusterPtr>& clusters_Santos, vector<ClusterPtr>& clusters_ABD ,vector<ClusterPtr>& clusters_nn , vector<ClusterPtr>& clusters_GT )
38 {
39 
40  static Markers marker_list;
41 
42  //Reduce the elements status, ADD to REMOVE and REMOVE to delete
43  marker_list.decrement();
44 
45  // Create a colormap
46  class_colormap colormap("hsv",10, 1, false);
47 
48  visualization_msgs::Marker marker_ids;
49  visualization_msgs::Marker marker_ids_prem;
50  visualization_msgs::Marker marker_ids_diet;
51  visualization_msgs::Marker marker_ids_santos;
52  visualization_msgs::Marker marker_ids_abd;
53  visualization_msgs::Marker marker_ids_nn;
54  visualization_msgs::Marker marker_ids_gt;
55 
56  visualization_msgs::Marker marker_clusters;
57  visualization_msgs::Marker marker_clusters_prem;
58  visualization_msgs::Marker marker_clusters_diet;
59  visualization_msgs::Marker marker_clusters_santos;
60  visualization_msgs::Marker marker_clusters_abd;
61  visualization_msgs::Marker marker_clusters_nn;
62  visualization_msgs::Marker marker_clusters_gt;
63 
64  marker_ids.header.frame_id = "/my_frame";
65  marker_ids.header.stamp = ros::Time::now();
66  marker_ids_prem.header.frame_id = "/my_frame";
67  marker_ids_prem.header.stamp = ros::Time::now();
68  marker_ids_diet.header.frame_id = "/my_frame";
69  marker_ids_diet.header.stamp = ros::Time::now();
70  marker_ids_santos.header.frame_id = "/my_frame";
71  marker_ids_santos.header.stamp = ros::Time::now();
72  marker_ids_abd.header.frame_id = "/my_frame";
73  marker_ids_abd.header.stamp = ros::Time::now();
74  marker_ids_nn.header.frame_id = "/my_frame";
75  marker_ids_nn.header.stamp = ros::Time::now();
76  marker_ids_gt.header.frame_id = "/my_frame";
77  marker_ids_gt.header.stamp = ros::Time::now();
78 
79  marker_clusters.header.frame_id = "/my_frame";
80  marker_clusters.header.stamp = marker_ids.header.stamp;
81  marker_clusters_prem.header.frame_id = "/my_frame";
82  marker_clusters_prem.header.stamp = marker_ids_prem.header.stamp;
83  marker_clusters_diet.header.frame_id = "/my_frame";
84  marker_clusters_diet.header.stamp = marker_ids_diet.header.stamp;
85  marker_clusters_santos.header.frame_id = "/my_frame";
86  marker_clusters_santos.header.stamp = marker_ids_santos.header.stamp;
87  marker_clusters_abd.header.frame_id = "/my_frame";
88  marker_clusters_abd.header.stamp = marker_ids_abd.header.stamp;
89  marker_clusters_nn.header.frame_id = "/my_frame";
90  marker_clusters_nn.header.stamp = marker_ids_nn.header.stamp;
91  marker_clusters_gt.header.frame_id = "/my_frame";
92  marker_clusters_gt.header.stamp = marker_ids_gt.header.stamp;
93 
94  marker_ids.ns = "ids";
95  marker_ids.action = visualization_msgs::Marker::ADD;
96  marker_ids_prem.ns = "ids_prem";
97  marker_ids_prem.action = visualization_msgs::Marker::ADD;
98  marker_ids_diet.ns = "ids_diet";
99  marker_ids_diet.action = visualization_msgs::Marker::ADD;
100  marker_ids_santos.ns = "ids_santos";
101  marker_ids_santos.action = visualization_msgs::Marker::ADD;
102  marker_ids_abd.ns = "ids_abd";
103  marker_ids_abd.action = visualization_msgs::Marker::ADD;
104  marker_ids_nn.ns = "ids_nn";
105  marker_ids_nn.action = visualization_msgs::Marker::ADD;
106  marker_ids_gt.ns = "ids_gt";
107  marker_ids_gt.action = visualization_msgs::Marker::ADD;
108 
109  marker_clusters.ns = "clusters";
110  marker_clusters.action = visualization_msgs::Marker::ADD;
111  marker_clusters_prem.ns = "clusters_prem";
112  marker_clusters_prem.action = visualization_msgs::Marker::ADD;
113  marker_clusters_diet.ns = "clusters_diet";
114  marker_clusters_diet.action = visualization_msgs::Marker::ADD;
115  marker_clusters_santos.ns = "clusters_santos";
116  marker_clusters_santos.action = visualization_msgs::Marker::ADD;
117  marker_clusters_abd.ns = "clusters_abd";
118  marker_clusters_abd.action = visualization_msgs::Marker::ADD;
119  marker_clusters_nn.ns = "clusters_nn";
120  marker_clusters_nn.action = visualization_msgs::Marker::ADD;
121  marker_clusters_gt.ns = "clusters_gt";
122  marker_clusters_gt.action = visualization_msgs::Marker::ADD;
123 
124  marker_ids.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
125  marker_ids_prem.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
126  marker_ids_diet.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
127  marker_ids_santos.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
128  marker_ids_abd.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
129  marker_ids_nn.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
130  marker_ids_gt.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
131 
132  marker_clusters.type = visualization_msgs::Marker::SPHERE_LIST;
133  marker_clusters_prem.type = visualization_msgs::Marker::SPHERE_LIST;
134  marker_clusters_diet.type = visualization_msgs::Marker::SPHERE_LIST;
135  marker_clusters_santos.type = visualization_msgs::Marker::SPHERE_LIST;
136  marker_clusters_abd.type = visualization_msgs::Marker::SPHERE_LIST;
137  marker_clusters_nn.type = visualization_msgs::Marker::SPHERE_LIST;
138  marker_clusters_gt.type = visualization_msgs::Marker::SPHERE_LIST;
139 
140  marker_ids.scale.x = 0.5;
141  marker_ids.scale.y = 0.5;
142  marker_ids.scale.z = 0.5;
143  marker_ids_prem.scale.x = 0.5;
144  marker_ids_prem.scale.y = 0.5;
145  marker_ids_prem.scale.z = 0.5;
146  marker_ids_diet.scale.x = 0.5;
147  marker_ids_diet.scale.y = 0.5;
148  marker_ids_diet.scale.z = 0.5;
149  marker_ids_santos.scale.x = 0.5;
150  marker_ids_santos.scale.y = 0.5;
151  marker_ids_santos.scale.z = 0.5;
152  marker_ids_abd.scale.x = 0.5;
153  marker_ids_abd.scale.y = 0.5;
154  marker_ids_abd.scale.z = 0.5;
155  marker_ids_nn.scale.x = 0.5;
156  marker_ids_nn.scale.y = 0.5;
157  marker_ids_nn.scale.z = 0.5;
158  marker_ids_gt.scale.x = 0.5;
159  marker_ids_gt.scale.y = 0.5;
160  marker_ids_gt.scale.z = 0.5;
161 
162  marker_clusters.scale.x = 0.2;
163  marker_clusters.scale.y = 0.2;
164  marker_clusters.scale.z = 0.2;
165  marker_clusters_prem.scale.x = 0.2;
166  marker_clusters_prem.scale.y = 0.2;
167  marker_clusters_prem.scale.z = 0.2;
168  marker_clusters_diet.scale.x = 0.2;
169  marker_clusters_diet.scale.y = 0.2;
170  marker_clusters_diet.scale.z = 0.2;
171  marker_clusters_santos.scale.x = 0.2;
172  marker_clusters_santos.scale.y = 0.2;
173  marker_clusters_santos.scale.z = 0.2;
174  marker_clusters_abd.scale.x = 0.2;
175  marker_clusters_abd.scale.y = 0.2;
176  marker_clusters_abd.scale.z = 0.2;
177  marker_clusters_nn.scale.x = 0.2;
178  marker_clusters_nn.scale.y = 0.2;
179  marker_clusters_nn.scale.z = 0.2;
180  marker_clusters_gt.scale.x = 0.2;
181  marker_clusters_gt.scale.y = 0.2;
182  marker_clusters_gt.scale.z = 0.2;
183 
184  marker_ids.color.a = 1.0;
185  marker_ids.color.r = 0.0;
186  marker_ids.color.g = 0.0;
187  marker_ids.color.b = 0.0;
188  marker_ids_prem.color.a = 1.0;
189  marker_ids_prem.color.r = 0.0;
190  marker_ids_prem.color.g = 0.0;
191  marker_ids_prem.color.b = 0.0;
192  marker_ids_diet.color.a = 1.0;
193  marker_ids_diet.color.r = 0.0;
194  marker_ids_diet.color.g = 0.0;
195  marker_ids_diet.color.b = 0.0;
196  marker_ids_santos.color.a = 1.0;
197  marker_ids_santos.color.r = 0.0;
198  marker_ids_santos.color.g = 0.0;
199  marker_ids_santos.color.b = 0.0;
200  marker_ids_abd.color.a = 1.0;
201  marker_ids_abd.color.r = 0.0;
202  marker_ids_abd.color.g = 0.0;
203  marker_ids_abd.color.b = 0.0;
204  marker_ids_nn.color.a = 1.0;
205  marker_ids_nn.color.r = 0.0;
206  marker_ids_nn.color.g = 0.0;
207  marker_ids_nn.color.b = 0.0;
208  marker_ids_gt.color.a = 1.0;
209  marker_ids_gt.color.r = 0.0;
210  marker_ids_gt.color.g = 0.0;
211  marker_ids_gt.color.b = 0.0;
212 
213 
214  for ( uint i = 0 ; i< clusters.size() ; i++) //search all clusters
215  {
216  ClusterPtr cluster = clusters[i];
217 
218  std_msgs::ColorRGBA color = colormap.color(i);
219 
220  //Place in the marker every point belonging to the cluster "i"
221  for(uint h=0;h<cluster->support_points.size();h++)
222  {
223  geometry_msgs::Point pt;
224  pt.x=cluster->support_points[h]->x;
225  pt.y=cluster->support_points[h]->y;
226  pt.z=0;
227 
228  marker_clusters.points.push_back(pt);
229  marker_clusters.colors.push_back(color);
230  }
231 
232  marker_ids.pose.position.x = cluster->centroid->x;
233  marker_ids.pose.position.y = cluster->centroid->y;
234  marker_ids.pose.position.z = 0.3;
235 
236  //texto
237  boost::format fm("%d");
238  fm % cluster->id;
239 
240  marker_ids.text = fm.str();
241  marker_ids.id = cluster->id;
242  marker_list.update(marker_ids);
243 
244  marker_list.update(marker_clusters);
245 
246  } //end for
247 
248 
249  for ( uint i = 0; i< clusters_Premebida.size() ; i++)
250  {
251 
252  ClusterPtr cluster_prem = clusters_Premebida[i];
253 
254  std_msgs::ColorRGBA color = colormap.color(i);
255 
256  //Place in the marker every point belonging to the cluster "i"
257  for(uint h=0;h<cluster_prem->support_points.size();h++)
258  {
259  geometry_msgs::Point pt;
260  pt.x=cluster_prem->support_points[h]->x;
261  pt.y=cluster_prem->support_points[h]->y;
262  pt.z= 1.0;
263 
264  marker_clusters_prem.points.push_back(pt);
265  marker_clusters_prem.colors.push_back(color);
266  }
267 
268  marker_ids_prem.pose.position.x = cluster_prem->centroid->x;
269  marker_ids_prem.pose.position.y = cluster_prem->centroid->y;
270  marker_ids_prem.pose.position.z = 1.3;
271 
272 // text
273  boost::format fm("%d");
274  fm % cluster_prem->id;
275 
276  marker_ids_prem.text = fm.str();
277  marker_ids_prem.id = cluster_prem->id;
278 
279  marker_list.update(marker_ids_prem);
280  marker_list.update(marker_clusters_prem);
281 
282  } //end for
283 
284  for ( uint i = 0; i< clusters_Dietmayer.size() ; i++)
285  {
286  ClusterPtr cluster_diet = clusters_Dietmayer[i];
287 
288  std_msgs::ColorRGBA color = colormap.color(i);
289 
290  //Place in the marker every point belonging to the cluster "i"
291  for(uint h=0;h<cluster_diet->support_points.size();h++)
292  {
293  geometry_msgs::Point pt;
294  pt.x=cluster_diet->support_points[h]->x;
295  pt.y=cluster_diet->support_points[h]->y;
296  pt.z= 2.0;
297 
298  marker_clusters_diet.points.push_back(pt);
299  marker_clusters_diet.colors.push_back(color);
300  }
301 
302  marker_ids_diet.pose.position.x = cluster_diet->centroid->x;
303  marker_ids_diet.pose.position.y = cluster_diet->centroid->y;
304  marker_ids_diet.pose.position.z = 2.3;
305 
306  //text
307  boost::format fm("%d");
308  fm % cluster_diet->id;
309 
310  marker_ids_diet.text = fm.str();
311  marker_ids_diet.id = cluster_diet->id;
312 
313  marker_list.update(marker_ids_diet);
314  marker_list.update(marker_clusters_diet);
315 
316  } //end for
317 
318  for ( uint i = 0; i< clusters_Santos.size() ; i++)
319  {
320  ClusterPtr cluster_santos = clusters_Santos[i];
321 
322  std_msgs::ColorRGBA color = colormap.color(i);
323 
324  //Place in the marker every point belonging to the cluster "i"
325  for(uint h=0;h<cluster_santos->support_points.size();h++)
326  {
327  geometry_msgs::Point pt;
328  pt.x=cluster_santos->support_points[h]->x;
329  pt.y=cluster_santos->support_points[h]->y;
330  pt.z= 3.0;
331 
332  marker_clusters_santos.points.push_back(pt);
333  marker_clusters_santos.colors.push_back(color);
334  }
335 
336  marker_ids_santos.pose.position.x = cluster_santos->centroid->x;
337  marker_ids_santos.pose.position.y = cluster_santos->centroid->y;
338  marker_ids_santos.pose.position.z = 3.3;
339 
340  //text
341  boost::format fm("%d");
342  fm % cluster_santos->id;
343 
344  marker_ids_santos.text = fm.str();
345  marker_ids_santos.id = cluster_santos->id;
346 
347  marker_list.update(marker_ids_santos);
348  marker_list.update(marker_clusters_santos);
349 
350  } //end for
351 
352  for ( uint i = 0; i< clusters_ABD.size() ; i++)
353  {
354  ClusterPtr cluster_abd = clusters_ABD[i];
355 
356  std_msgs::ColorRGBA color = colormap.color(i);
357 
358  //Place in the marker every point belonging to the cluster "i"
359  for(uint h=0;h<cluster_abd->support_points.size();h++)
360  {
361  geometry_msgs::Point pt;
362  pt.x=cluster_abd->support_points[h]->x;
363  pt.y=cluster_abd->support_points[h]->y;
364  pt.z= 4.0;
365 
366  marker_clusters_abd.points.push_back(pt);
367  marker_clusters_abd.colors.push_back(color);
368  }
369 
370  marker_ids_abd.pose.position.x = cluster_abd->centroid->x;
371  marker_ids_abd.pose.position.y = cluster_abd->centroid->y;
372  marker_ids_abd.pose.position.z = 4.3;
373 
374  //text
375  boost::format fm("%d");
376  fm % cluster_abd->id;
377 
378  marker_ids_abd.text = fm.str();
379  marker_ids_abd.id = cluster_abd->id;
380 
381  marker_list.update(marker_ids_abd);
382  marker_list.update(marker_clusters_abd);
383 
384  } //end for
385 
386  for ( uint i = 0; i< clusters_nn.size() ; i++)
387  {
388  ClusterPtr cluster_nn = clusters_nn[i];
389 
390  std_msgs::ColorRGBA color = colormap.color(i);
391 
392  //Place in the marker every point belonging to the cluster "i"
393  for(uint h=0;h<cluster_nn->support_points.size();h++)
394  {
395  geometry_msgs::Point pt;
396  pt.x=cluster_nn->support_points[h]->x;
397  pt.y=cluster_nn->support_points[h]->y;
398  pt.z= 5.0;
399 
400  marker_clusters_nn.points.push_back(pt);
401  marker_clusters_nn.colors.push_back(color);
402  }
403 
404  marker_ids_nn.pose.position.x = cluster_nn->centroid->x;
405  marker_ids_nn.pose.position.y = cluster_nn->centroid->y;
406  marker_ids_nn.pose.position.z = 5.3;
407 
408  //text
409  boost::format fm("%d");
410  fm % cluster_nn->id;
411 
412  marker_ids_nn.text = fm.str();
413  marker_ids_nn.id = cluster_nn->id;
414 
415  marker_list.update(marker_ids_nn);
416  marker_list.update(marker_clusters_nn);
417 
418  } //end for
419 
420  for ( uint i = 0; i< clusters_GT.size() ; i++)
421  {
422  ClusterPtr cluster_gt = clusters_GT[i];
423 
424  std_msgs::ColorRGBA color = colormap.color(i);
425 
426  //Place in the marker every point belonging to the cluster "i"
427  for(uint h=0;h<cluster_gt->support_points.size();h++)
428  {
429  geometry_msgs::Point pt;
430  pt.x=cluster_gt->support_points[h]->x;
431  pt.y=cluster_gt->support_points[h]->y;
432  pt.z= 6.0;
433 
434  marker_clusters_gt.points.push_back(pt);
435  marker_clusters_gt.colors.push_back(color);
436  }
437 
438  marker_ids_gt.pose.position.x = cluster_gt->centroid->x;
439  marker_ids_gt.pose.position.y = cluster_gt->centroid->y;
440  marker_ids_gt.pose.position.z = 6.3;
441 
442  //text
443  boost::format fm("%d");
444  fm % cluster_gt->id;
445 
446  marker_ids_gt.text = fm.str();
447  marker_ids_gt.id = cluster_gt->id;
448 
449  marker_list.update(marker_ids_gt);
450  marker_list.update(marker_clusters_gt);
451 
452  } //end for
453 
454 
455  //Remove markers that should not be transmitted
456  marker_list.clean();
457 
458  //Clean the marker_vector and put new markers in it;
459  return marker_list.getOutgoingMarkers();
460 
461 } //end function
Markers::update
void update(visualization_msgs::Marker &marker)
Definition: visualization_rviz.h:48
clustering.h
Clustering related functions header.
ClusterPtr
boost::shared_ptr< Cluster > ClusterPtr
Definition: clustering.h:70
createTargetMarkers
vector< visualization_msgs::Marker > createTargetMarkers(vector< ClusterPtr > &clusters, vector< ClusterPtr > &clusters_Premebida, vector< ClusterPtr > &clusters_Dietmayer, vector< ClusterPtr > &clusters_Santos, vector< ClusterPtr > &clusters_ABD, vector< ClusterPtr > &clusters_nn, vector< ClusterPtr > &clusters_GT)
Create marker form clusters This function creates a set of markers for the current clusters from the ...
Definition: visualization_rviz.cpp:37
Markers::decrement
void decrement(void)
Definition: visualization_rviz.h:60
Markers::getOutgoingMarkers
vector< visualization_msgs::Marker > getOutgoingMarkers(void)
Definition: visualization_rviz.h:87
Markers::clean
void clean(void)
Definition: visualization_rviz.h:76
lidar_segmentation.h
Global include file.
visualization_rviz.h
Visualization on rviz related functions header.

lidar_segmentation
Author(s): Daniel Coimbra
autogenerated on Mon Mar 2 2015 01:32:12