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
37using namespace fawkes;
38
39/** @class ColliVisualizationThread "visualization_thread.h"
40 * @author Bahram Maleki-Fard
41 */
42
43/** Constructor. */
44ColliVisualizationThread::ColliVisualizationThread()
45: fawkes::Thread("ColliVisualizationThread", Thread::OPMODE_WAITFORWAKEUP), occ_grid_(0), search_(0)
46{
47}
48
49void
50ColliVisualizationThread::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
79void
80ColliVisualizationThread::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
102void
103ColliVisualizationThread::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
188void
189ColliVisualizationThread::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
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
This OccGrid is derived by the Occupancy Grid originally from Andreas Strack, but modified for speed ...
Definition: og_laser.h:47
colli_cell_cost_t get_cell_costs() const
Get cell costs.
Definition: og_laser.cpp:471
Mutex locking helper.
Definition: mutex_locker.h:34
This class is mainly the same as the basic class with the difference that all data is precalculated o...
This is the plan class.
Definition: astar_search.h:45
Thread class encapsulation of pthreads.
Definition: thread.h:46
This class tries to translate the found plan to interpreteable data for the rest of the program.
Fawkes library namespace.
float Probability
A probability type.
Definition: probability.h:29
Point with cartesian coordinates as signed integers.
Definition: types.h:42
int x
x coordinate
Definition: types.h:43
int y
y coordinate
Definition: types.h:44