22 #include "visualization_thread.h" 24 #ifdef HAVE_VISUAL_DEBUGGING 26 #include "utils/rob/roboshape_colli.h" 27 #include "search/og_laser.h" 28 #include "search/astar_search.h" 30 #include <core/threading/mutex_locker.h> 31 #include <utils/math/angle.h> 32 #include <utils/math/types.h> 35 #include <nav_msgs/GridCells.h> 36 #include <visualization_msgs/MarkerArray.h> 45 ColliVisualizationThread::ColliVisualizationThread()
53 ColliVisualizationThread::init()
55 pub_roboshape_ =
new ros::Publisher();
56 *pub_roboshape_ = rosnode->advertise< nav_msgs::GridCells >(
"colli_roboshape", 1);
58 pub_cells_occ_ =
new ros::Publisher();
59 *pub_cells_occ_ = rosnode->advertise< nav_msgs::GridCells >(
"colli_cells_occupied", 1);
61 pub_cells_near_ =
new ros::Publisher();
62 *pub_cells_near_ = rosnode->advertise< nav_msgs::GridCells >(
"colli_cells_near", 1);
64 pub_cells_mid_ =
new ros::Publisher();
65 *pub_cells_mid_ = rosnode->advertise< nav_msgs::GridCells >(
"colli_cells_mid", 1);
67 pub_cells_far_ =
new ros::Publisher();
68 *pub_cells_far_ = rosnode->advertise< nav_msgs::GridCells >(
"colli_cells_far", 1);
70 pub_cells_free_ =
new ros::Publisher();
71 *pub_cells_free_ = rosnode->advertise< nav_msgs::GridCells >(
"colli_cells_free", 1);
73 pub_search_path_ =
new ros::Publisher();
74 *pub_search_path_ = rosnode->advertise< nav_msgs::GridCells >(
"colli_search_path", 1);
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());
83 ColliVisualizationThread::finalize()
85 pub_roboshape_->shutdown();
86 delete pub_roboshape_;
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_;
99 pub_search_path_->shutdown();
100 delete pub_search_path_;
107 ColliVisualizationThread::loop()
109 if( (occ_grid_ == NULL) || (search_ == NULL) )
115 nav_msgs::GridCells grid;
116 grid.header.frame_id = frame_laser_;
117 grid.cell_width = 0.05;
118 grid.cell_height = 0.05;
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);
130 grid.cells.push_back(p);
133 grid.header.stamp = ros::Time::now();
134 pub_roboshape_->publish(grid);
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);
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;
152 prob = occ_grid_->get_prob(x,y);
153 if( prob == cell_costs_.occ) {
154 grid_cells_occ.cells.push_back( p );
156 }
else if( prob == cell_costs_.near ) {
157 grid_cells_near.cells.push_back( p );
159 }
else if( prob == cell_costs_.mid ) {
160 grid_cells_mid.cells.push_back( p );
162 }
else if( prob == cell_costs_.far ) {
163 grid_cells_far.cells.push_back( p );
165 }
else if( prob == cell_costs_.free ) {
166 grid_cells_free.cells.push_back( p );
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 );
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;
186 grid.cells.push_back( p );
188 grid.header.stamp = ros::Time::now();
189 pub_search_path_->publish( grid );
199 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.