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); } } } }