22 #include "visualization_thread.h" 24 #ifdef HAVE_VISUAL_DEBUGGING 26 # include "search/astar_search.h" 27 # include "search/og_laser.h" 28 # include "utils/rob/roboshape_colli.h" 30 # include <core/threading/mutex_locker.h> 31 # include <nav_msgs/GridCells.h> 33 # include <utils/math/angle.h> 34 # include <utils/math/types.h> 35 # include <visualization_msgs/MarkerArray.h> 44 ColliVisualizationThread::ColliVisualizationThread()
45 :
fawkes::
Thread(
"ColliVisualizationThread",
Thread::OPMODE_WAITFORWAKEUP), occ_grid_(0), search_(0)
50 ColliVisualizationThread::init()
52 pub_roboshape_ =
new ros::Publisher();
53 *pub_roboshape_ = rosnode->advertise<nav_msgs::GridCells>(
"colli_roboshape", 1);
55 pub_cells_occ_ =
new ros::Publisher();
56 *pub_cells_occ_ = rosnode->advertise<nav_msgs::GridCells>(
"colli_cells_occupied", 1);
58 pub_cells_near_ =
new ros::Publisher();
59 *pub_cells_near_ = rosnode->advertise<nav_msgs::GridCells>(
"colli_cells_near", 1);
61 pub_cells_mid_ =
new ros::Publisher();
62 *pub_cells_mid_ = rosnode->advertise<nav_msgs::GridCells>(
"colli_cells_mid", 1);
64 pub_cells_far_ =
new ros::Publisher();
65 *pub_cells_far_ = rosnode->advertise<nav_msgs::GridCells>(
"colli_cells_far", 1);
67 pub_cells_free_ =
new ros::Publisher();
68 *pub_cells_free_ = rosnode->advertise<nav_msgs::GridCells>(
"colli_cells_free", 1);
70 pub_search_path_ =
new ros::Publisher();
71 *pub_search_path_ = rosnode->advertise<nav_msgs::GridCells>(
"colli_search_path", 1);
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());
80 ColliVisualizationThread::finalize()
82 pub_roboshape_->shutdown();
83 delete pub_roboshape_;
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_;
96 pub_search_path_->shutdown();
97 delete pub_search_path_;
103 ColliVisualizationThread::loop()
105 if ((occ_grid_ == NULL) || (search_ == NULL))
111 nav_msgs::GridCells grid;
112 grid.header.frame_id = frame_laser_;
113 grid.cell_width = 0.05;
114 grid.cell_height = 0.05;
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);
126 grid.cells.push_back(p);
129 grid.header.stamp = ros::Time::now();
130 pub_roboshape_->publish(grid);
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);
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;
148 prob = occ_grid_->get_prob(x, y);
149 if (prob == cell_costs_.occ) {
150 grid_cells_occ.cells.push_back(p);
152 }
else if (prob == cell_costs_.near) {
153 grid_cells_near.cells.push_back(p);
155 }
else if (prob == cell_costs_.mid) {
156 grid_cells_mid.cells.push_back(p);
158 }
else if (prob == cell_costs_.far) {
159 grid_cells_far.cells.push_back(p);
161 }
else if (prob == cell_costs_.free) {
162 grid_cells_free.cells.push_back(p);
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);
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;
182 grid.cells.push_back(p);
184 grid.header.stamp = ros::Time::now();
185 pub_search_path_->publish(grid);
193 occ_grid_ = occ_grid;
float Probability
A probability type.
This class tries to translate the found plan to interpreteable data for the rest of the program.
Fawkes library namespace.
Thread class encapsulation of pthreads.
colli_cell_cost_t get_cell_costs() const
Get cell costs.
This OccGrid is derived by the Occupancy Grid originally from Andreas Strack, but modified for speed ...
Point with cartesian coordinates as signed integers.
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.