34 #ifndef TRAJECTORY_ROLLOUT_MAP_GRID_H_ 35 #define TRAJECTORY_ROLLOUT_MAP_GRID_H_ 45 #include <geometry_msgs/PoseStamped.h> 64 MapGrid(
unsigned int size_x,
unsigned int size_y);
118 void sizeCheck(
unsigned int size_x,
unsigned int size_y);
145 return map_.size() + 1;
164 std::vector<geometry_msgs::PoseStamped>& global_plan_out,
double resolution);
187 const std::vector<geometry_msgs::PoseStamped>& global_plan);
double unreachableCellCosts()
void setTargetCells(const costmap_2d::Costmap2D &costmap, const std::vector< geometry_msgs::PoseStamped > &global_plan)
Update what cells are considered path based on the global plan.
~MapGrid()
Destructor for a MapGrid.
unsigned int size_y_
The dimensions of the grid.
double goal_y_
The goal distance was last computed from.
bool updatePathCell(MapCell *current_cell, MapCell *check_cell, const costmap_2d::Costmap2D &costmap)
Used to update the distance of a cell in path distance computation.
A grid of MapCell cells that is used to propagate path and goal distances for the trajectory controll...
static void adjustPlanResolution(const std::vector< geometry_msgs::PoseStamped > &global_plan_in, std::vector< geometry_msgs::PoseStamped > &global_plan_out, double resolution)
void commonInit()
Utility to share initialization code across constructors.
size_t getIndex(int x, int y)
Returns a 1D index into the MapCell array for a 2D index.
MapGrid()
Creates a 0x0 map by default.
MapCell & operator()(unsigned int x, unsigned int y)
Returns a map cell accessed by (col, row)
void sizeCheck(unsigned int size_x, unsigned int size_y)
check if we need to resize
MapGrid & operator=(const MapGrid &mg)
Assignment operator for a MapGrid.
std::vector< MapCell > map_
Storage for the MapCells.
void computeGoalDistance(std::queue< MapCell *> &dist_queue, const costmap_2d::Costmap2D &costmap)
Compute the distance from each cell in the local map grid to the local goal point.
Stores path distance and goal distance information used for scoring trajectories. ...
void setLocalGoal(const costmap_2d::Costmap2D &costmap, const std::vector< geometry_msgs::PoseStamped > &global_plan)
Update what cell is considered the next local goal.
MapCell & getCell(unsigned int x, unsigned int y)
void resetPathDist()
reset path distance fields for all cells
void computeTargetDistance(std::queue< MapCell *> &dist_queue, const costmap_2d::Costmap2D &costmap)
Compute the distance from each cell in the local map grid to the planned path.