map_grid.h
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2008, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 #ifndef TRAJECTORY_ROLLOUT_MAP_GRID_H_
35 #define TRAJECTORY_ROLLOUT_MAP_GRID_H_
36 
37 #include <vector>
38 #include <iostream>
40 #include <ros/console.h>
41 #include <ros/ros.h>
42 
44 #include <costmap_2d/costmap_2d.h>
45 #include <geometry_msgs/PoseStamped.h>
46 
47 namespace base_local_planner{
52  class MapGrid{
53  public:
57  MapGrid();
58 
64  MapGrid(unsigned int size_x, unsigned int size_y);
65 
66 
73  inline MapCell& operator() (unsigned int x, unsigned int y){
74  return map_[size_x_ * y + x];
75  }
76 
83  inline MapCell operator() (unsigned int x, unsigned int y) const {
84  return map_[size_x_ * y + x];
85  }
86 
87  inline MapCell& getCell(unsigned int x, unsigned int y){
88  return map_[size_x_ * y + x];
89  }
90 
95 
100  MapGrid(const MapGrid& mg);
101 
106  MapGrid& operator= (const MapGrid& mg);
107 
111  void resetPathDist();
112 
118  void sizeCheck(unsigned int size_x, unsigned int size_y);
119 
123  void commonInit();
124 
131  size_t getIndex(int x, int y);
132 
136  inline double obstacleCosts() {
137  return map_.size();
138  }
139 
144  inline double unreachableCellCosts() {
145  return map_.size() + 1;
146  }
147 
153  inline bool updatePathCell(MapCell* current_cell, MapCell* check_cell,
154  const costmap_2d::Costmap2D& costmap);
155 
163  static void adjustPlanResolution(const std::vector<geometry_msgs::PoseStamped>& global_plan_in,
164  std::vector<geometry_msgs::PoseStamped>& global_plan_out, double resolution);
165 
170  void computeTargetDistance(std::queue<MapCell*>& dist_queue, const costmap_2d::Costmap2D& costmap);
171 
176  void computeGoalDistance(std::queue<MapCell*>& dist_queue, const costmap_2d::Costmap2D& costmap);
177 
181  void setTargetCells(const costmap_2d::Costmap2D& costmap, const std::vector<geometry_msgs::PoseStamped>& global_plan);
182 
186  void setLocalGoal(const costmap_2d::Costmap2D& costmap,
187  const std::vector<geometry_msgs::PoseStamped>& global_plan);
188 
189  double goal_x_, goal_y_;
191  unsigned int size_x_, size_y_;
192 
193  private:
194 
195  std::vector<MapCell> map_;
196 
197  };
198 };
199 
200 #endif
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.
Definition: map_grid.cpp:171
~MapGrid()
Destructor for a MapGrid.
Definition: map_grid.h:94
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.
Definition: map_grid.cpp:255
unsigned int size_y_
The dimensions of the grid.
Definition: map_grid.h:191
double goal_y_
The goal distance was last computed from.
Definition: map_grid.h:189
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.
TFSIMD_FORCE_INLINE const tfScalar & y() const
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.
Definition: map_grid.cpp:103
A grid of MapCell cells that is used to propagate path and goal distances for the trajectory controll...
Definition: map_grid.h:52
static void adjustPlanResolution(const std::vector< geometry_msgs::PoseStamped > &global_plan_in, std::vector< geometry_msgs::PoseStamped > &global_plan_out, double resolution)
Definition: map_grid.cpp:133
void commonInit()
Utility to share initialization code across constructors.
Definition: map_grid.cpp:57
size_t getIndex(int x, int y)
Returns a 1D index into the MapCell array for a 2D index.
Definition: map_grid.cpp:73
MapGrid()
Creates a 0x0 map by default.
Definition: map_grid.cpp:40
MapCell & operator()(unsigned int x, unsigned int y)
Returns a map cell accessed by (col, row)
Definition: map_grid.h:73
void sizeCheck(unsigned int size_x, unsigned int size_y)
check if we need to resize
Definition: map_grid.cpp:84
MapGrid & operator=(const MapGrid &mg)
Assignment operator for a MapGrid.
Definition: map_grid.cpp:77
TFSIMD_FORCE_INLINE const tfScalar & x() const
std::vector< MapCell > map_
Storage for the MapCells.
Definition: map_grid.h:195
Stores path distance and goal distance information used for scoring trajectories. ...
Definition: map_cell.h:44
void setLocalGoal(const costmap_2d::Costmap2D &costmap, const std::vector< geometry_msgs::PoseStamped > &global_plan)
Update what cell is considered the next local goal.
Definition: map_grid.cpp:210
MapCell & getCell(unsigned int x, unsigned int y)
Definition: map_grid.h:87
void resetPathDist()
reset path distance fields for all cells
Definition: map_grid.cpp:125


base_local_planner
Author(s): Eitan Marder-Eppstein, Eric Perko, contradict@gmail.com
autogenerated on Sun Mar 3 2019 03:44:25