41 : size_x_(0), size_y_(0)
64 for(
unsigned int i = 0; i <
size_y_; ++i){
65 for(
unsigned int j = 0; j <
size_x_; ++j){
66 unsigned int id = size_x_ * i + j;
85 if(
map_.size() != size_x * size_y)
86 map_.resize(size_x * size_y);
92 for(
unsigned int i = 0; i <
size_y_; ++i){
93 for(
unsigned int j = 0; j <
size_x_; ++j){
94 int index = size_x_ * i + j;
107 unsigned char cost = costmap.
getCost(check_cell->
cx, check_cell->
cy);
116 double new_target_dist = current_cell->
target_dist + 1;
117 if (new_target_dist < check_cell->target_dist) {
126 for(
unsigned int i = 0; i <
map_.size(); ++i) {
128 map_[i].target_mark =
false;
129 map_[i].within_robot =
false;
134 std::vector<geometry_msgs::PoseStamped>& global_plan_out,
double resolution) {
135 if (global_plan_in.size() == 0) {
138 double last_x = global_plan_in[0].pose.position.x;
139 double last_y = global_plan_in[0].pose.position.y;
140 global_plan_out.push_back(global_plan_in[0]);
142 double min_sq_resolution = resolution * resolution;
144 for (
unsigned int i = 1; i < global_plan_in.size(); ++i) {
145 double loop_x = global_plan_in[i].pose.position.x;
146 double loop_y = global_plan_in[i].pose.position.y;
147 double sqdist = (loop_x - last_x) * (loop_x - last_x) + (loop_y - last_y) * (loop_y - last_y);
148 if (sqdist > min_sq_resolution) {
149 int steps = ceil((
sqrt(sqdist)) / resolution);
151 double deltax = (loop_x - last_x) / steps;
152 double deltay = (loop_y - last_y) / steps;
154 for (
int j = 1; j < steps; ++j) {
155 geometry_msgs::PoseStamped pose;
156 pose.pose.position.x = last_x + j * deltax;
157 pose.pose.position.y = last_y + j * deltay;
158 pose.pose.position.z = global_plan_in[i].pose.position.z;
159 pose.pose.orientation = global_plan_in[i].pose.orientation;
160 pose.header = global_plan_in[i].header;
161 global_plan_out.push_back(pose);
164 global_plan_out.push_back(global_plan_in[i]);
172 const std::vector<geometry_msgs::PoseStamped>& global_plan) {
175 bool started_path =
false;
177 queue<MapCell*> path_dist_queue;
179 std::vector<geometry_msgs::PoseStamped> adjusted_global_plan;
181 if (adjusted_global_plan.size() != global_plan.size()) {
182 ROS_DEBUG(
"Adjusted global plan resolution, added %zu points", adjusted_global_plan.size() - global_plan.size());
186 for (i = 0; i < adjusted_global_plan.size(); ++i) {
187 double g_x = adjusted_global_plan[i].pose.position.x;
188 double g_y = adjusted_global_plan[i].pose.position.y;
189 unsigned int map_x, map_y;
194 path_dist_queue.push(¤t);
196 }
else if (started_path) {
201 ROS_ERROR(
"None of the %d first of %zu (%zu) points of the global plan were in the local costmap and free",
202 i, adjusted_global_plan.size(), global_plan.size());
211 const std::vector<geometry_msgs::PoseStamped>& global_plan) {
214 int local_goal_x = -1;
215 int local_goal_y = -1;
216 bool started_path =
false;
218 std::vector<geometry_msgs::PoseStamped> adjusted_global_plan;
222 for (
unsigned int i = 0; i < adjusted_global_plan.size(); ++i) {
223 double g_x = adjusted_global_plan[i].pose.position.x;
224 double g_y = adjusted_global_plan[i].pose.position.y;
225 unsigned int map_x, map_y;
227 local_goal_x = map_x;
228 local_goal_y = map_y;
237 ROS_ERROR(
"None of the points of the global plan were in the local costmap, global plan points too far from robot");
241 queue<MapCell*> path_dist_queue;
242 if (local_goal_x >= 0 && local_goal_y >= 0) {
247 path_dist_queue.push(¤t);
258 unsigned int last_col =
size_x_ - 1;
259 unsigned int last_row =
size_y_ - 1;
260 while(!dist_queue.empty()){
261 current_cell = dist_queue.front();
266 if(current_cell->
cx > 0){
267 check_cell = current_cell - 1;
272 dist_queue.push(check_cell);
277 if(current_cell->
cx < last_col){
278 check_cell = current_cell + 1;
282 dist_queue.push(check_cell);
287 if(current_cell->
cy > 0){
288 check_cell = current_cell -
size_x_;
292 dist_queue.push(check_cell);
297 if(current_cell->
cy < last_row){
298 check_cell = current_cell +
size_x_;
302 dist_queue.push(check_cell);
double unreachableCellCosts()
unsigned int cy
Cell index in the grid map.
void mapToWorld(unsigned int mx, unsigned int my, double &wx, double &wy) const
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.
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.
bool within_robot
Mark for cells within the robot footprint.
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...
unsigned char getCost(unsigned int mx, unsigned int my) const
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.
static const unsigned char INSCRIBED_INFLATED_OBSTACLE
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.
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.
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
unsigned int getSizeInCellsY() const
bool target_mark
Marks for computing path/goal distances.
double target_dist
Distance to planner's path.
unsigned int getSizeInCellsX() const
static const unsigned char LETHAL_OBSTACLE
static const unsigned char NO_INFORMATION
Stores path distance and goal distance information used for scoring trajectories. ...
double getResolution() const
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
bool worldToMap(double wx, double wy, unsigned int &mx, unsigned int &my) const