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.
RoboShapeColli(const char *cfg_prefix, Logger *logger, Configuration *config, int readings_per_degree=1)
Constructor.
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.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.