Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034 #ifndef TRAJECTORY_ROLLOUT_MAP_GRID_H_
00035 #define TRAJECTORY_ROLLOUT_MAP_GRID_H_
00036
00037 #include <vector>
00038 #include <iostream>
00039 #include <base_local_planner/trajectory_inc.h>
00040 #include <ros/console.h>
00041 #include <ros/ros.h>
00042
00043 #include <base_local_planner/map_cell.h>
00044 #include <costmap_2d/costmap_2d.h>
00045 #include <geometry_msgs/PoseStamped.h>
00046
00047 namespace base_local_planner{
00052 class MapGrid{
00053 public:
00057 MapGrid();
00058
00064 MapGrid(unsigned int size_x, unsigned int size_y);
00065
00074 MapGrid(unsigned int size_x, unsigned int size_y, double scale, double x, double y);
00075
00082 inline MapCell& operator() (unsigned int x, unsigned int y){
00083 return map_[size_x_ * y + x];
00084 }
00085
00092 inline MapCell operator() (unsigned int x, unsigned int y) const {
00093 return map_[size_x_ * y + x];
00094 }
00095
00096 inline MapCell& getCell(unsigned int x, unsigned int y){
00097 return map_[size_x_ * y + x];
00098 }
00099
00103 ~MapGrid(){}
00104
00109 MapGrid(const MapGrid& mg);
00110
00115 MapGrid& operator= (const MapGrid& mg);
00116
00120 void resetPathDist();
00121
00129 void sizeCheck(unsigned int size_x, unsigned int size_y, double o_x, double o_y);
00130
00134 void commonInit();
00135
00142 size_t getIndex(int x, int y);
00143
00149 inline void updatePathCell(MapCell* current_cell, MapCell* check_cell,
00150 std::queue<MapCell*>& dist_queue, const costmap_2d::Costmap2D& costmap){
00151
00152 check_cell->path_mark = true;
00153
00154
00155 unsigned char cost = costmap.getCost(check_cell->cx, check_cell->cy);
00156 if(!getCell(check_cell->cx, check_cell->cy).within_robot && (cost == costmap_2d::LETHAL_OBSTACLE || cost == costmap_2d::INSCRIBED_INFLATED_OBSTACLE || cost == costmap_2d::NO_INFORMATION)){
00157 check_cell->path_dist = map_.size();
00158 return;
00159 }
00160
00161 double new_path_dist = current_cell->path_dist + 1;
00162 if(new_path_dist < check_cell->path_dist)
00163 check_cell->path_dist = new_path_dist;
00164
00165 dist_queue.push(check_cell);
00166 }
00167
00173 inline void updateGoalCell(MapCell* current_cell, MapCell* check_cell,
00174 std::queue<MapCell*>& dist_queue, const costmap_2d::Costmap2D& costmap ){
00176 check_cell->goal_mark = true;
00177
00178
00179 unsigned char cost = costmap.getCost(check_cell->cx, check_cell->cy);
00180 if(!getCell(check_cell->cx, check_cell->cy).within_robot && (cost == costmap_2d::LETHAL_OBSTACLE || cost == costmap_2d::INSCRIBED_INFLATED_OBSTACLE || cost == costmap_2d::NO_INFORMATION)){
00181 check_cell->goal_dist = map_.size();
00182 return;
00183 }
00184
00185 double new_goal_dist = current_cell->goal_dist + 1;
00186 if(new_goal_dist < check_cell->goal_dist)
00187 check_cell->goal_dist = new_goal_dist;
00188
00189 dist_queue.push(check_cell);
00190 }
00191
00196 void computePathDistance(std::queue<MapCell*>& dist_queue, const costmap_2d::Costmap2D& costmap);
00197
00202 void computeGoalDistance(std::queue<MapCell*>& dist_queue, const costmap_2d::Costmap2D& costmap);
00203
00207 void setPathCells(const costmap_2d::Costmap2D& costmap, const std::vector<geometry_msgs::PoseStamped>& global_plan);
00208
00209 unsigned int size_x_, size_y_;
00210 std::vector<MapCell> map_;
00211
00212 double scale;
00213
00214 double goal_x_, goal_y_;
00216 double origin_x, origin_y;
00217 };
00218 };
00219
00220 #endif