Fawkes API  Fawkes Development Version
visualization_thread.cpp
1 
2 /***************************************************************************
3  * visualization_thread.cpp - Visualization for colli
4  *
5  * Created: Fri Oct 18 15:16:23 2013
6  * Copyright 2013 Bahram Maleki-Fard
7  ****************************************************************************/
8 
9 /* This program is free software; you can redistribute it and/or modify
10  * it under the terms of the GNU General Public License as published by
11  * the Free Software Foundation; either version 2 of the License, or
12  * (at your option) any later version.
13  *
14  * This program is distributed in the hope that it will be useful,
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17  * GNU Library General Public License for more details.
18  *
19  * Read the full text in the LICENSE.GPL file in the doc directory.
20  */
21 
22 #include "visualization_thread.h"
23 
24 #ifdef HAVE_VISUAL_DEBUGGING
25 
26 #include "utils/rob/roboshape_colli.h"
27 #include "search/og_laser.h"
28 #include "search/astar_search.h"
29 
30 #include <core/threading/mutex_locker.h>
31 #include <utils/math/angle.h>
32 #include <utils/math/types.h>
33 
34 #include <ros/ros.h>
35 #include <nav_msgs/GridCells.h>
36 #include <visualization_msgs/MarkerArray.h>
37 
38 using namespace fawkes;
39 
40 /** @class ColliVisualizationThread "visualization_thread.h"
41  * @author Bahram Maleki-Fard
42  */
43 
44 /** Constructor. */
45 ColliVisualizationThread::ColliVisualizationThread()
46  : fawkes::Thread("ColliVisualizationThread", Thread::OPMODE_WAITFORWAKEUP),
47  occ_grid_( 0 ),
48  search_( 0 )
49 {
50 }
51 
52 void
53 ColliVisualizationThread::init()
54 {
55  pub_roboshape_ = new ros::Publisher();
56  *pub_roboshape_ = rosnode->advertise< nav_msgs::GridCells >("colli_roboshape", 1);
57 
58  pub_cells_occ_ = new ros::Publisher();
59  *pub_cells_occ_ = rosnode->advertise< nav_msgs::GridCells >("colli_cells_occupied", 1);
60 
61  pub_cells_near_ = new ros::Publisher();
62  *pub_cells_near_ = rosnode->advertise< nav_msgs::GridCells >("colli_cells_near", 1);
63 
64  pub_cells_mid_ = new ros::Publisher();
65  *pub_cells_mid_ = rosnode->advertise< nav_msgs::GridCells >("colli_cells_mid", 1);
66 
67  pub_cells_far_ = new ros::Publisher();
68  *pub_cells_far_ = rosnode->advertise< nav_msgs::GridCells >("colli_cells_far", 1);
69 
70  pub_cells_free_ = new ros::Publisher();
71  *pub_cells_free_ = rosnode->advertise< nav_msgs::GridCells >("colli_cells_free", 1);
72 
73  pub_search_path_ = new ros::Publisher();
74  *pub_search_path_ = rosnode->advertise< nav_msgs::GridCells >("colli_search_path", 1);
75 
76  std::string cfg_prefix = "/plugins/colli/";
77  roboshape_ = new RoboShapeColli( (cfg_prefix + "roboshape/").c_str(), logger, config );
78  frame_base_ = config->get_string((cfg_prefix + "frame/base").c_str());
79  frame_laser_ = config->get_string((cfg_prefix + "frame/laser").c_str());
80 }
81 
82 void
83 ColliVisualizationThread::finalize()
84 {
85  pub_roboshape_->shutdown();
86  delete pub_roboshape_;
87 
88  pub_cells_occ_->shutdown();
89  delete pub_cells_occ_;
90  pub_cells_near_->shutdown();
91  delete pub_cells_near_;
92  pub_cells_mid_->shutdown();
93  delete pub_cells_mid_;
94  pub_cells_far_->shutdown();
95  delete pub_cells_far_;
96  pub_cells_free_->shutdown();
97  delete pub_cells_free_;
98 
99  pub_search_path_->shutdown();
100  delete pub_search_path_;
101 
102  delete roboshape_;
103 }
104 
105 
106 void
107 ColliVisualizationThread::loop()
108 {
109  if( (occ_grid_ == NULL) || (search_ == NULL) )
110  return;
111 
112  MutexLocker lock(&mutex_);
113 
114  // define grid settings
115  nav_msgs::GridCells grid;
116  grid.header.frame_id = frame_laser_;
117  grid.cell_width = 0.05;
118  grid.cell_height = 0.05;
119 
120  // publish roboshape
121  grid.cells.clear();
122  float rad = 0;
123  float radinc = M_PI/180.f;
124  for( unsigned int i=0; i<360; ++i ) {
125  float len = roboshape_->get_robot_length_for_rad( rad );
126  geometry_msgs::Point p;
127  p.x = len * cos(rad);
128  p.y = len * sin(rad);
129  p.z = 0;
130  grid.cells.push_back(p);
131  rad += radinc;
132  }
133  grid.header.stamp = ros::Time::now();
134  pub_roboshape_->publish(grid);
135 
136  // publish grid cells
137  grid.cells.clear();
138  nav_msgs::GridCells grid_cells_occ(grid);
139  nav_msgs::GridCells grid_cells_near(grid);
140  nav_msgs::GridCells grid_cells_mid(grid);
141  nav_msgs::GridCells grid_cells_far(grid);
142  nav_msgs::GridCells grid_cells_free(grid);
143  Probability prob;
144  point_t gridpos_laser = occ_grid_->get_laser_position();
145  for( int y=0; y < occ_grid_->get_height(); ++y ) {
146  for( int x=0; x < occ_grid_->get_width(); ++x ) {
147  geometry_msgs::Point p;
148  p.x = (x - gridpos_laser.x) * grid.cell_width;
149  p.y = (y - gridpos_laser.y) * grid.cell_height;
150  p.z = 0;
151 
152  prob = occ_grid_->get_prob(x,y);
153  if( prob == cell_costs_.occ) {
154  grid_cells_occ.cells.push_back( p );
155 
156  } else if( prob == cell_costs_.near ) {
157  grid_cells_near.cells.push_back( p );
158 
159  } else if( prob == cell_costs_.mid ) {
160  grid_cells_mid.cells.push_back( p );
161 
162  } else if( prob == cell_costs_.far ) {
163  grid_cells_far.cells.push_back( p );
164 
165  } else if( prob == cell_costs_.free ) {
166  grid_cells_free.cells.push_back( p );
167  }
168  }
169  }
170  pub_cells_occ_->publish( grid_cells_occ );
171  pub_cells_near_->publish( grid_cells_near );
172  pub_cells_mid_->publish( grid_cells_mid );
173  pub_cells_far_->publish( grid_cells_far );
174  pub_cells_free_->publish( grid_cells_free );
175 
176  // publish path
177  grid.cells.clear();
178  grid.header.frame_id = frame_base_;
179  std::vector< point_t >* plan = search_->get_plan();
180  point_t gridpos_robo = search_->get_robot_position();
181  for( std::vector<point_t>::iterator it=plan->begin(); it!=plan->end(); ++it ) {
182  geometry_msgs::Point p;
183  p.x = ((*it).x - gridpos_robo.x) * grid.cell_width;
184  p.y = ((*it).y - gridpos_robo.y) * grid.cell_height;
185  p.z = 0;
186  grid.cells.push_back( p );
187  }
188  grid.header.stamp = ros::Time::now();
189  pub_search_path_->publish( grid );
190 
191 }
192 
193 void
194 ColliVisualizationThread::setup(LaserOccupancyGrid* occ_grid,
195  Search* search)
196 {
197  MutexLocker lock(&mutex_);
198  search_ = search;
199  occ_grid_ = occ_grid;
200  cell_costs_ = occ_grid_->get_cell_costs();
201 }
202 
203 #endif
float Probability
A probability type.
Definition: probability.h:33
This class tries to translate the found plan to interpreteable data for the rest of the program...
Fawkes library namespace.
Mutex locking helper.
Definition: mutex_locker.h:33
RoboShapeColli(const char *cfg_prefix, Logger *logger, Configuration *config, int readings_per_degree=1)
Constructor.
Thread class encapsulation of pthreads.
Definition: thread.h:42
colli_cell_cost_t get_cell_costs() const
Get cell costs.
Definition: og_laser.cpp:442
This OccGrid is derived by the Occupancy Grid originally from Andreas Strack, but modified for speed ...
Definition: og_laser.h:49
int y
y coordinate
Definition: types.h:42
This is the plan class.
Definition: astar_search.h:47
Point with cartesian coordinates as signed integers.
Definition: types.h:40
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
int x
x coordinate
Definition: types.h:41