representation_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 ***************************************************************************************************/
34 #include "representation_rviz.h"
35 
36 vector<visualization_msgs::Marker> create_Markers_ElavationMap(Navigability_Map& nav_map, std::string grid_frame)
37 {
38  static Markers marker_list;
39  grid_nodePtr node_data;
40  double Zmin=nav_map.Zmin_filter;
41  double Zmax=nav_map.Zmax_filter;
42  int colorrange=nav_map.colorrange;
43 
44  //Reduce the elements status, ADD to REMOVE and REMOVE to delete
45  marker_list.decrement();
46 
47  // Create a colormap
48 
49  class_colormap colormap_positive("jet",colorrange, 1, false);
50  class_colormap colormap_negative("pink",colorrange, 1, false);
51 
52  /***** Elevation Cell *****/
53  visualization_msgs::Marker marker_cell;
54 
55  marker_cell.header.frame_id = grid_frame;
56  marker_cell.header.stamp = ros::Time::now();;
57  marker_cell.ns = "elvation_cell";
58  marker_cell.action = visualization_msgs::Marker::ADD;
59  marker_cell.type = visualization_msgs::Marker::CUBE;
60  marker_cell.pose.orientation.x = 0; marker_cell.pose.orientation.y = 0.0; marker_cell.pose.orientation.z = 0; marker_cell.pose.orientation.w = 1;
61 
62  marker_cell.scale.x = nav_map.Sx;
63  marker_cell.scale.y = nav_map.Sy;
64 
65  geometry_msgs::Point p;
66 
67  /***** Confidence Cell *****/
68  visualization_msgs::Marker marker_text;
69 
70  marker_text.header.frame_id = grid_frame;
71  marker_text.header.stamp = ros::Time::now();;
72  marker_text.ns = "confidence_cell";
73  marker_text.action = visualization_msgs::Marker::ADD;
74  marker_text.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
75  marker_text.scale.x = 0.07;
76  marker_text.scale.y = 0.07;
77  marker_text.scale.z = 0.07;
78  marker_text.color.a = 1.0;
79  marker_text.color.r = 0.0;
80  marker_text.color.g = 0.0;
81  marker_text.color.b = 0.0;
82 
83 
84  uint counter=0;
85  for(uint row=0;row<nav_map.grid.rows();row++)
86  {
87  //X posistion
88  p.x = row*nav_map.Sx+nav_map.Sx/2;
89 
90  for(uint col=0;col<nav_map.grid.cols();col++)
91  {
92 
93  node_data = nav_map.grid(row,col);
94 
95  if (node_data->num_points>0 || node_data->interpolate_z_data) //node is occupied
96  {
97  //id
98  marker_cell.id = counter;
99 
100  //Y position
101  p.y = col*nav_map.Sy-nav_map.CARaxis_col*nav_map.Sy;
102 
103  //Scale and Color
104 
105  p.z= node_data->Zmed/2;
106  marker_cell.scale.z = node_data->Zmed;
107 
108  //color
109  if (node_data->Zmed>=0 )
110  marker_cell.color = colormap_positive.color((colorrange*node_data->Zmed)/Zmax);
111  else
112  marker_cell.color =colormap_positive.color((colorrange*abs(node_data->Zmed))/abs(Zmin));
113 
114  //set cell pos
115  marker_cell.pose.position=p;
116 
117  //update marker
118  marker_list.update(marker_cell);
119 
120  /****** Text Confidence cell *****/
121 
122  //id
123  marker_text.id = counter;
124 
125  //position
126  marker_text.pose.position.x = p.x;
127  marker_text.pose.position.y = p.y;
128  if (p.z>=0)
129  marker_text.pose.position.z = node_data->Zmed+0.05;
130  else
131  marker_text.pose.position.z = 0.05;
132 
133  //text
134  boost::format fm("%d");
135  fm % node_data->Z_confidence;
136 // fm % node_data->angle_confidence_y;
137 // fm % node_data->Zmed;
138  marker_text.text = fm.str();
139 
140  //update marker
141 // marker_list.update(marker_text);
142 
143 
144 
145  }
146  counter++;
147  }
148  }
149 
150  //Remove markers that should not be transmitted
151  marker_list.clean();
152 
153  //Clean the marker_vector and put new markers in it;
154  return marker_list.getOutgoingMarkers();
155 }
156 
157 
158 
159 visualization_msgs::Marker create_Markers_Normals(Navigability_Map& nav_map, std::string grid_frame)
160 {
161  grid_nodePtr node_data;
162  double px_aux=0;
163  double py_aux=0;
164  double pz_aux=0;
165  //publish a marker to rviz with the normals
166  visualization_msgs::Marker mk; //declare the array
167 
168  geometry_msgs::Point p;
169  std_msgs::ColorRGBA color;
170 
171  mk.header.frame_id = grid_frame; mk.header.stamp = ros::Time::now(); mk.ns = "normals";
172  mk.action = visualization_msgs::Marker::ADD; mk.type = visualization_msgs::Marker::LINE_LIST; mk.id = 0;
173  mk.pose.position.x = 0; mk.pose.position.y = 0; mk.pose.position.z = 0;
174  mk.pose.orientation.x = 0; mk.pose.orientation.y = 0; mk.pose.orientation.z = 0; mk.pose.orientation.w = 1;
175  mk.scale.x = 0.01; mk.scale.y = 0.01; mk.scale.z = 0;
176  mk.color.r = 0.7; mk.color.g = 0; mk.color.b = 0; mk.color.a = 1;
177 
178  for(uint row=0;row<nav_map.grid.rows();row++)
179  {
180  for(uint col=0;col<nav_map.grid.cols();col++)
181  {
182  node_data = nav_map.grid(row,col);
183 
184  if (node_data->has_normal) //node has_normal
185  {
186  //X posistion
187  p.x = row*nav_map.Sx+nav_map.Sx/2;
188  px_aux = p.x;
189 
190  //Y position
191  p.y = col*nav_map.Sy-nav_map.CARaxis_col*nav_map.Sy;
192  py_aux = p.y;
193 
194  //Z posistion
195  p.z= node_data->Zmed;
196  if (p.z>=0)
197  pz_aux=p.z;
198  else
199  {
200  p.z =0;
201  pz_aux = 0;
202  }
203 
204  mk.points.push_back(p);
205 
206  double rat=0.5;
207 
208  p.x = px_aux + (cos(node_data->med_angle_X))*rat;
209  p.y = py_aux + (cos(node_data->med_angle_Y))*rat;
210  p.z = pz_aux + (cos(node_data->med_angle_Z))*rat;
211 
212  mk.points.push_back(p);
213  }
214  }
215  }
216 
217  return mk;
218 }
219 
220 vector<visualization_msgs::Marker> create_Markers_AccessibilityMap(Navigability_Map& nav_map, std::string grid_frame)
221 {
222  static Markers marker_list;
223  grid_nodePtr node_data;
224 
225  int colorrange=nav_map.colorrange;
226 
227  //Reduce the elements status, ADD to REMOVE and REMOVE to delete
228  marker_list.decrement();
229 
230  // Create a colormap
231 
232  class_colormap colormap_positive("jet",colorrange, 1, false);
233 
234  /***** _Accessibility Cell *****/
235  visualization_msgs::Marker marker_cell;
236 
237  marker_cell.header.frame_id = grid_frame;
238  marker_cell.header.stamp = ros::Time::now();;
239  marker_cell.ns = "accessibility_cell";
240  marker_cell.action = visualization_msgs::Marker::ADD;
241  marker_cell.type = visualization_msgs::Marker::CUBE;
242  marker_cell.pose.orientation.x = 0; marker_cell.pose.orientation.y = 0.0; marker_cell.pose.orientation.z = 0; marker_cell.pose.orientation.w = 1;
243 
244  marker_cell.scale.x = nav_map.Sx;
245  marker_cell.scale.y = nav_map.Sy;
246 
247  geometry_msgs::Point p;
248 
249 
250  /***** debug Cell *****/
251  visualization_msgs::Marker marker_text;
252 
253  marker_text.header.frame_id = grid_frame;
254  marker_text.header.stamp = ros::Time::now();;
255  marker_text.ns = "debug_cell";
256  marker_text.action = visualization_msgs::Marker::ADD;
257  marker_text.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
258  marker_text.scale.x = 0.07;
259  marker_text.scale.y = 0.07;
260  marker_text.scale.z = 0.07;
261  marker_text.color.a = 1.0;
262  marker_text.color.r = 0.0;
263  marker_text.color.g = 0.0;
264  marker_text.color.b = 0.0;
265 
266 
267  uint counter=0;
268  for(uint row=0;row<nav_map.grid.rows();row++)
269  {
270  //X posistion
271  p.x = row*nav_map.Sx+nav_map.Sx/2;
272 
273  for(uint col=0;col<nav_map.grid.cols();col++)
274  {
275 
276  node_data = nav_map.grid(row,col);
277 
278  if (node_data->num_points>0 || node_data->interpolate_z_data) //node is occupied
279  {
280  //id
281  marker_cell.id = counter;
282 
283  //Y position
284  p.y = col*nav_map.Sy-nav_map.CARaxis_col*nav_map.Sy;
285 
286 // Scale and Color
287  if (nav_map.debug_accessibility==1)
288  p.z=(1-node_data->z_accessibility)/2;
289  else if (nav_map.debug_accessibility==2)
290  p.z=(1-node_data->angleX_accessibility)/2;
291  else if (nav_map.debug_accessibility==3)
292  p.z=(1-node_data->angleY_accessibility)/2;
293  else if (nav_map.debug_accessibility==4)
294  p.z=(1-node_data->angleZ_accessibility)/2;
295  else if (nav_map.debug_accessibility==5)
296  p.z=(1-node_data->total_accessibility)/2;
297 
298  if (nav_map.debug_accessibility==1)
299  marker_cell.scale.z =1-node_data->z_accessibility;
300  else if (nav_map.debug_accessibility==2)
301  marker_cell.scale.z =1-node_data->angleX_accessibility;
302  else if (nav_map.debug_accessibility==3)
303  marker_cell.scale.z =1-node_data->angleY_accessibility;
304  else if (nav_map.debug_accessibility==4)
305  marker_cell.scale.z =1-node_data->angleZ_accessibility;
306  else if (nav_map.debug_accessibility==5)
307  marker_cell.scale.z =1-node_data->total_accessibility;
308 
309  //color
310  if (nav_map.debug_accessibility==1)
311  marker_cell.color = colormap_positive.color(colorrange*(1-node_data->z_accessibility)-1);
312  else if (nav_map.debug_accessibility==2)
313  marker_cell.color = colormap_positive.color(colorrange*(1-node_data->angleX_accessibility)-1);
314  else if (nav_map.debug_accessibility==3)
315  marker_cell.color = colormap_positive.color(colorrange*(1-node_data->angleY_accessibility)-1);
316  else if (nav_map.debug_accessibility==4)
317  marker_cell.color = colormap_positive.color(colorrange*(1-node_data->angleZ_accessibility)-1);
318  else if (nav_map.debug_accessibility==5)
319  marker_cell.color = colormap_positive.color(colorrange*(1-node_data->total_accessibility)-1);
320 
321 
322  //set cell pos
323  marker_cell.pose.position=p;
324 
325  //update marker
326  marker_list.update(marker_cell);
327 
328 
329  /****** debug Confidence cell *****/
330 // id
331  marker_text.id = counter;
332 
333  //position
334  marker_text.pose.position.x = p.x;
335  marker_text.pose.position.y = p.y;
336 
337  if (nav_map.debug_accessibility==1)
338  marker_text.pose.position.z = (1-node_data->z_accessibility)+0.1;
339  else if (nav_map.debug_accessibility==2)
340  marker_text.pose.position.z = (1-node_data->angleX_accessibility)+0.1;
341  else if (nav_map.debug_accessibility==3)
342  marker_text.pose.position.z = (1-node_data->angleY_accessibility)+0.1;
343  else if (nav_map.debug_accessibility==4)
344  marker_text.pose.position.z = (1-node_data->angleZ_accessibility)+0.1;
345  else if (nav_map.debug_accessibility==5)
346  marker_text.pose.position.z = (1-node_data->total_accessibility)+0.1;
347 
348  //text
349  boost::format fm("%d");
350  if (nav_map.debug_accessibility==1)
351  fm % node_data->z_accessibility;
352  else if (nav_map.debug_accessibility==2)
353  fm % node_data->angleX_accessibility;
354  else if (nav_map.debug_accessibility==3)
355  fm % node_data->angleY_accessibility;
356  else if (nav_map.debug_accessibility==4)
357  fm % node_data->angleZ_accessibility;
358  else if (nav_map.debug_accessibility==5)
359  fm % node_data->total_accessibility;
360 
361  marker_text.text = fm.str();
362  //update marker
363 // marker_list.update(marker_text);
364 
365  }
366  counter++;
367  }
368  }
369 
370  //Remove markers that should not be transmitted
371  marker_list.clean();
372 
373  //Clean the marker_vector and put new markers in it;
374  return marker_list.getOutgoingMarkers();
375 }
376 
377 
378 
379 visualization_msgs::Marker create_Markers_Polygon(Navigability_Map& nav_map, std::string grid_frame)
380 {
381  //publish a marker to rviz with the polygon
382  visualization_msgs::Marker mk; //declare the array
383 
384  std_msgs::ColorRGBA color;
385 
386  mk.header.frame_id = grid_frame; mk.header.stamp = ros::Time::now(); mk.ns = "polygon";
387  mk.action = visualization_msgs::Marker::ADD; mk.type = visualization_msgs::Marker::LINE_STRIP; mk.id = 0;
388  mk.pose.position.x = 0; mk.pose.position.y = 0; mk.pose.position.z = 0;
389  mk.pose.orientation.x = 0; mk.pose.orientation.y = 0; mk.pose.orientation.z = 0; mk.pose.orientation.w = 1;
390  mk.scale.x = 0.08; mk.scale.y = 0.01; mk.scale.z = 0;
391  mk.color.r = 0; mk.color.g = 1; mk.color.b = 0; mk.color.a = 1;
392 
393 
394  for (uint i=0; i<nav_map.polygon_points.size(); i++)
395  mk.points.push_back(nav_map.polygon_points[i]);
396 
397  nav_map.polygon_points.erase(nav_map.polygon_points.begin(),nav_map.polygon_points.end());
398  return mk;
399 }
400 
401 visualization_msgs::Marker create_Markers_Obstacle(Navigability_Map& nav_map, std::string grid_frame)
402 {
403  //publish a marker to rviz with the polygon
404  visualization_msgs::Marker mk; //declare the array
405 
406  std_msgs::ColorRGBA color;
407 
408  mk.header.frame_id = grid_frame; mk.header.stamp = ros::Time::now(); mk.ns = "polygon_obstacle";
409  mk.action = visualization_msgs::Marker::ADD; mk.type = visualization_msgs::Marker::LINE_STRIP; mk.id = 0;
410  mk.pose.position.x = 0; mk.pose.position.y = 0; mk.pose.position.z = 0;
411  mk.pose.orientation.x = 0; mk.pose.orientation.y = 0; mk.pose.orientation.z = 0; mk.pose.orientation.w = 1;
412  mk.scale.x = 0.08; mk.scale.y = 0.01; mk.scale.z = 0;
413  mk.color.r = 1; mk.color.g = 0; mk.color.b = 0; mk.color.a = 1;
414 
415 
416  for (uint i=0; i<nav_map.obstacle_points.size(); i++)
417  mk.points.push_back(nav_map.obstacle_points[i]);
418 
419  nav_map.obstacle_points.erase(nav_map.obstacle_points.begin(),nav_map.obstacle_points.end());
420  return mk;
421 }
Navigability_Map::debug_accessibility
int debug_accessibility
Which accessibility map should be draw.
Definition: navigability_map.h:307
Navigability_Map::obstacle_points
std::vector< geometry_msgs::Point > obstacle_points
Definition: navigability_map.h:315
Navigability_Map
Definition: navigability_map.h:221
create_Markers_Normals
visualization_msgs::Marker create_Markers_Normals(Navigability_Map &nav_map, std::string grid_frame)
Function to generate marker for the visualization of the Normals on the Class Navigability_Map.
Definition: representation_rviz.cpp:159
Navigability_Map::Sx
double Sx
The X dimension of the cell.
Definition: navigability_map.h:262
create_Markers_Polygon
visualization_msgs::Marker create_Markers_Polygon(Navigability_Map &nav_map, std::string grid_frame)
Definition: representation_rviz.cpp:379
Markers::update
void update(visualization_msgs::Marker &marker)
Definition: representation_rviz.h:51
Navigability_Map::Sy
double Sy
The Y dimension of the cell.
Definition: navigability_map.h:265
Navigability_Map::polygon_points
std::vector< geometry_msgs::Point > polygon_points
Definition: navigability_map.h:314
create_Markers_Obstacle
visualization_msgs::Marker create_Markers_Obstacle(Navigability_Map &nav_map, std::string grid_frame)
Definition: representation_rviz.cpp:401
create_Markers_AccessibilityMap
vector< visualization_msgs::Marker > create_Markers_AccessibilityMap(Navigability_Map &nav_map, std::string grid_frame)
Function to generate marker for the visualization of the Accessibility Map on the Class Navigability_...
Definition: representation_rviz.cpp:220
Navigability_Map::Zmin_filter
double Zmin_filter
Max distance to be consider in the Z direction of the PointCloud.
Definition: navigability_map.h:244
Navigability_Map::grid
Grid grid
The two-dimensional grid with the cells.
Definition: navigability_map.h:259
Markers::decrement
void decrement(void)
Definition: representation_rviz.h:63
Navigability_Map::Zmax_filter
double Zmax_filter
Min distance to be consider in the Z direction of the PointCloud.
Definition: navigability_map.h:241
Markers::getOutgoingMarkers
vector< visualization_msgs::Marker > getOutgoingMarkers(void)
Definition: representation_rviz.h:90
Markers::clean
void clean(void)
Definition: representation_rviz.h:79
grid_nodePtr
boost::shared_ptr< grid_node > grid_nodePtr
Definition: navigability_map.h:206
create_Markers_ElavationMap
vector< visualization_msgs::Marker > create_Markers_ElavationMap(Navigability_Map &nav_map, std::string grid_frame)
Function to generate marker for the visualization of the Elevation Map on the Class Navigability_Map...
Definition: representation_rviz.cpp:36
Navigability_Map::CARaxis_col
int CARaxis_col
Center col of the grid.
Definition: navigability_map.h:280
representation_rviz.h
Declarion functions and class to handle the visualization markers of the Class Navigability_Map.
Navigability_Map::colorrange
int colorrange
Color to represent the maps.
Definition: navigability_map.h:256

navigability_map
Author(s): Diogo Matos
autogenerated on Mon Mar 2 2015 01:32:29