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 "search/astar_search.h"
27 # include "search/og_laser.h"
28 # include "utils/rob/roboshape_colli.h"
29 
30 # include <core/threading/mutex_locker.h>
31 # include <nav_msgs/GridCells.h>
32 # include <ros/ros.h>
33 # include <utils/math/angle.h>
34 # include <utils/math/types.h>
35 # include <visualization_msgs/MarkerArray.h>
36 
37 using namespace fawkes;
38 
39 /** @class ColliVisualizationThread "visualization_thread.h"
40  * @author Bahram Maleki-Fard
41  */
42 
43 /** Constructor. */
44 ColliVisualizationThread::ColliVisualizationThread()
45 : fawkes::Thread("ColliVisualizationThread", Thread::OPMODE_WAITFORWAKEUP), occ_grid_(0), search_(0)
46 {
47 }
48 
49 void
50 ColliVisualizationThread::init()
51 {
52  pub_roboshape_ = new ros::Publisher();
53  *pub_roboshape_ = rosnode->advertise<nav_msgs::GridCells>("colli_roboshape", 1);
54 
55  pub_cells_occ_ = new ros::Publisher();
56  *pub_cells_occ_ = rosnode->advertise<nav_msgs::GridCells>("colli_cells_occupied", 1);
57 
58  pub_cells_near_ = new ros::Publisher();
59  *pub_cells_near_ = rosnode->advertise<nav_msgs::GridCells>("colli_cells_near", 1);
60 
61  pub_cells_mid_ = new ros::Publisher();
62  *pub_cells_mid_ = rosnode->advertise<nav_msgs::GridCells>("colli_cells_mid", 1);
63 
64  pub_cells_far_ = new ros::Publisher();
65  *pub_cells_far_ = rosnode->advertise<nav_msgs::GridCells>("colli_cells_far", 1);
66 
67  pub_cells_free_ = new ros::Publisher();
68  *pub_cells_free_ = rosnode->advertise<nav_msgs::GridCells>("colli_cells_free", 1);
69 
70  pub_search_path_ = new ros::Publisher();
71  *pub_search_path_ = rosnode->advertise<nav_msgs::GridCells>("colli_search_path", 1);
72 
73  std::string cfg_prefix = "/plugins/colli/";
74  roboshape_ = new RoboShapeColli((cfg_prefix + "roboshape/").c_str(), logger, config);
75  frame_base_ = config->get_string((cfg_prefix + "frame/base").c_str());
76  frame_laser_ = config->get_string((cfg_prefix + "frame/laser").c_str());
77 }
78 
79 void
80 ColliVisualizationThread::finalize()
81 {
82  pub_roboshape_->shutdown();
83  delete pub_roboshape_;
84 
85  pub_cells_occ_->shutdown();
86  delete pub_cells_occ_;
87  pub_cells_near_->shutdown();
88  delete pub_cells_near_;
89  pub_cells_mid_->shutdown();
90  delete pub_cells_mid_;
91  pub_cells_far_->shutdown();
92  delete pub_cells_far_;
93  pub_cells_free_->shutdown();
94  delete pub_cells_free_;
95 
96  pub_search_path_->shutdown();
97  delete pub_search_path_;
98 
99  delete roboshape_;
100 }
101 
102 void
103 ColliVisualizationThread::loop()
104 {
105  if ((occ_grid_ == NULL) || (search_ == NULL))
106  return;
107 
108  MutexLocker lock(&mutex_);
109 
110  // define grid settings
111  nav_msgs::GridCells grid;
112  grid.header.frame_id = frame_laser_;
113  grid.cell_width = 0.05;
114  grid.cell_height = 0.05;
115 
116  // publish roboshape
117  grid.cells.clear();
118  float rad = 0;
119  float radinc = M_PI / 180.f;
120  for (unsigned int i = 0; i < 360; ++i) {
121  float len = roboshape_->get_robot_length_for_rad(rad);
122  geometry_msgs::Point p;
123  p.x = (double)len * cosf(rad);
124  p.y = (double)len * sinf(rad);
125  p.z = 0;
126  grid.cells.push_back(p);
127  rad += radinc;
128  }
129  grid.header.stamp = ros::Time::now();
130  pub_roboshape_->publish(grid);
131 
132  // publish grid cells
133  grid.cells.clear();
134  nav_msgs::GridCells grid_cells_occ(grid);
135  nav_msgs::GridCells grid_cells_near(grid);
136  nav_msgs::GridCells grid_cells_mid(grid);
137  nav_msgs::GridCells grid_cells_far(grid);
138  nav_msgs::GridCells grid_cells_free(grid);
139  Probability prob;
140  point_t gridpos_laser = occ_grid_->get_laser_position();
141  for (int y = 0; y < occ_grid_->get_height(); ++y) {
142  for (int x = 0; x < occ_grid_->get_width(); ++x) {
143  geometry_msgs::Point p;
144  p.x = (double)(x - gridpos_laser.x) * grid.cell_width;
145  p.y = (double)(y - gridpos_laser.y) * grid.cell_height;
146  p.z = 0;
147 
148  prob = occ_grid_->get_prob(x, y);
149  if (prob == cell_costs_.occ) {
150  grid_cells_occ.cells.push_back(p);
151 
152  } else if (prob == cell_costs_.near) {
153  grid_cells_near.cells.push_back(p);
154 
155  } else if (prob == cell_costs_.mid) {
156  grid_cells_mid.cells.push_back(p);
157 
158  } else if (prob == cell_costs_.far) {
159  grid_cells_far.cells.push_back(p);
160 
161  } else if (prob == cell_costs_.free) {
162  grid_cells_free.cells.push_back(p);
163  }
164  }
165  }
166  pub_cells_occ_->publish(grid_cells_occ);
167  pub_cells_near_->publish(grid_cells_near);
168  pub_cells_mid_->publish(grid_cells_mid);
169  pub_cells_far_->publish(grid_cells_far);
170  pub_cells_free_->publish(grid_cells_free);
171 
172  // publish path
173  grid.cells.clear();
174  grid.header.frame_id = frame_base_;
175  std::vector<point_t> *plan = search_->get_plan();
176  point_t gridpos_robo = search_->get_robot_position();
177  for (std::vector<point_t>::iterator it = plan->begin(); it != plan->end(); ++it) {
178  geometry_msgs::Point p;
179  p.x = (double)((*it).x - gridpos_robo.x) * grid.cell_width;
180  p.y = (double)((*it).y - gridpos_robo.y) * grid.cell_height;
181  p.z = 0;
182  grid.cells.push_back(p);
183  }
184  grid.header.stamp = ros::Time::now();
185  pub_search_path_->publish(grid);
186 }
187 
188 void
189 ColliVisualizationThread::setup(LaserOccupancyGrid *occ_grid, Search *search)
190 {
191  MutexLocker lock(&mutex_);
192  search_ = search;
193  occ_grid_ = occ_grid;
194  cell_costs_ = occ_grid_->get_cell_costs();
195 }
196 
197 #endif
float Probability
A probability type.
Definition: probability.h:29
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
Thread class encapsulation of pthreads.
Definition: thread.h:45
colli_cell_cost_t get_cell_costs() const
Get cell costs.
Definition: og_laser.cpp:471
This OccGrid is derived by the Occupancy Grid originally from Andreas Strack, but modified for speed ...
Definition: og_laser.h:46
int y
y coordinate
Definition: types.h:44
This is the plan class.
Definition: astar_search.h:44
Point with cartesian coordinates as signed integers.
Definition: types.h:41
This class is mainly the same as the basic class with the difference that all data is precalculated o...
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:43