void MapGrid::computeTargetDistance(queue<MapCell*>& dist_queue, const costmap_2d::Costmap2D& costmap){
MapCell* current_cell;
MapCell* check_cell;
unsigned int last_col = size_x_ - 1;
unsigned int last_row = size_y_ - 1;
while(!dist_queue.empty()){
current_cell = dist_queue.front();
dist_queue.pop();
if(current_cell->cx > 0){
check_cell = current_cell - 1;
if(!check_cell->target_mark){
//mark the cell as visisted
check_cell->target_mark = true;
if(updatePathCell(current_cell, check_cell, costmap)) {
dist_queue.push(check_cell);
}
}
}
if(current_cell->cx < last_col){
check_cell = current_cell + 1;
if(!check_cell->target_mark){
check_cell->target_mark = true;
if(updatePathCell(current_cell, check_cell, costmap)) {
dist_queue.push(check_cell);
}
}
}
if(current_cell->cy > 0){
check_cell = current_cell - size_x_;
if(!check_cell->target_mark){
check_cell->target_mark = true;
if(updatePathCell(current_cell, check_cell, costmap)) {
dist_queue.push(check_cell);
}
}
}
if(current_cell->cy < last_row){
check_cell = current_cell + size_x_;
if(!check_cell->target_mark){
check_cell->target_mark = true;
if(updatePathCell(current_cell, check_cell, costmap)) {
dist_queue.push(check_cell);
}
}
}
}
}
void MapGrid::computePathDistance(queue<MapCell*>& dist_queue, const costmap_2d::Costmap2D& costmap){
MapCell* current_cell;
MapCell* check_cell;
unsigned int last_col = size_x_ - 1;
unsigned int last_row = size_y_ - 1;
while(!dist_queue.empty()){
current_cell = dist_queue.front();
check_cell = current_cell;
dist_queue.pop();
if(current_cell->cx > 0){
check_cell = current_cell - 1;
if(!check_cell->path_mark){
updatePathCell(current_cell, check_cell, dist_queue, costmap);
}
}
if(current_cell->cx < last_col){
check_cell = current_cell + 1;
if(!check_cell->path_mark){
updatePathCell(current_cell, check_cell, dist_queue, costmap);
}
}
if(current_cell->cy > 0){
check_cell = current_cell - size_x_;
if(!check_cell->path_mark){
updatePathCell(current_cell, check_cell, dist_queue, costmap);
}
}
if(current_cell->cy < last_row){
check_cell = current_cell + size_x_;
if(!check_cell->path_mark){
updatePathCell(current_cell, check_cell, dist_queue, costmap);
}
}
}
}
