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
00066
00073 inline MapCell& operator() (unsigned int x, unsigned int y){
00074 return map_[size_x_ * y + x];
00075 }
00076
00083 inline MapCell operator() (unsigned int x, unsigned int y) const {
00084 return map_[size_x_ * y + x];
00085 }
00086
00087 inline MapCell& getCell(unsigned int x, unsigned int y){
00088 return map_[size_x_ * y + x];
00089 }
00090
00094 ~MapGrid(){}
00095
00100 MapGrid(const MapGrid& mg);
00101
00106 MapGrid& operator= (const MapGrid& mg);
00107
00111 void resetPathDist();
00112
00118 void sizeCheck(unsigned int size_x, unsigned int size_y);
00119
00123 void commonInit();
00124
00131 size_t getIndex(int x, int y);
00132
00136 inline double obstacleCosts() {
00137 return map_.size();
00138 }
00139
00144 inline double unreachableCellCosts() {
00145 return map_.size() + 1;
00146 }
00147
00153 inline bool updatePathCell(MapCell* current_cell, MapCell* check_cell,
00154 const costmap_2d::Costmap2D& costmap);
00155
00163 static void adjustPlanResolution(const std::vector<geometry_msgs::PoseStamped>& global_plan_in,
00164 std::vector<geometry_msgs::PoseStamped>& global_plan_out, double resolution);
00165
00170 void computeTargetDistance(std::queue<MapCell*>& dist_queue, const costmap_2d::Costmap2D& costmap);
00171
00176 void computeGoalDistance(std::queue<MapCell*>& dist_queue, const costmap_2d::Costmap2D& costmap);
00177
00181 void setTargetCells(const costmap_2d::Costmap2D& costmap, const std::vector<geometry_msgs::PoseStamped>& global_plan);
00182
00186 void setLocalGoal(const costmap_2d::Costmap2D& costmap,
00187 const std::vector<geometry_msgs::PoseStamped>& global_plan);
00188
00189 double goal_x_, goal_y_;
00191 unsigned int size_x_, size_y_;
00192
00193 private:
00194
00195 std::vector<MapCell> map_;
00196
00197 };
00198 };
00199
00200 #endif