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;