costmap_layer.cpp
Go to the documentation of this file.
00001 #include<costmap_2d/costmap_layer.h>
00002 
00003 namespace costmap_2d
00004 {
00005 
00006 void CostmapLayer::touch(double x, double y, double* min_x, double* min_y, double* max_x, double* max_y)
00007 {
00008     *min_x = std::min(x, *min_x);
00009     *min_y = std::min(y, *min_y);
00010     *max_x = std::max(x, *max_x);
00011     *max_y = std::max(y, *max_y);
00012 }
00013 
00014 void CostmapLayer::matchSize()
00015 {
00016     Costmap2D* master = layered_costmap_->getCostmap();
00017     resizeMap(master->getSizeInCellsX(), master->getSizeInCellsY(), master->getResolution(),
00018             master->getOriginX(), master->getOriginY());
00019 }
00020 
00021 void CostmapLayer::addExtraBounds(double mx0, double my0, double mx1, double my1)
00022 {
00023     extra_min_x_ = std::min(mx0, extra_min_x_);
00024     extra_max_x_ = std::max(mx1, extra_max_x_);
00025     extra_min_y_ = std::min(my0, extra_min_y_);
00026     extra_max_y_ = std::max(my1, extra_max_y_);
00027     has_extra_bounds_ = true;
00028 }
00029 
00030 void CostmapLayer::useExtraBounds(double* min_x, double* min_y, double* max_x, double* max_y)
00031 {
00032     if (!has_extra_bounds_)
00033         return;
00034 
00035     *min_x = std::min(extra_min_x_, *min_x);
00036     *min_y = std::min(extra_min_y_, *min_y);
00037     *max_x = std::max(extra_max_x_, *max_x);
00038     *max_y = std::max(extra_max_y_, *max_y);
00039     extra_min_x_ = 1e6;
00040     extra_min_y_ = 1e6;
00041     extra_max_x_ = -1e6;
00042     extra_max_y_ = -1e6;
00043     has_extra_bounds_ = false;
00044 }
00045 
00046 void CostmapLayer::updateWithMax(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
00047 {
00048   if (!enabled_)
00049     return;
00050 
00051   unsigned char* master_array = master_grid.getCharMap();
00052   unsigned int span = master_grid.getSizeInCellsX();
00053 
00054   for (int j = min_j; j < max_j; j++)
00055   {
00056     unsigned int it = j * span + min_i;
00057     for (int i = min_i; i < max_i; i++)
00058     {
00059       if (costmap_[it] == NO_INFORMATION){
00060         it++;
00061         continue;
00062       }
00063 
00064       unsigned char old_cost = master_array[it];
00065       if (old_cost == NO_INFORMATION || old_cost < costmap_[it])
00066         master_array[it] = costmap_[it];
00067       it++;
00068     }
00069   }
00070 }
00071 
00072 void CostmapLayer::updateWithTrueOverwrite(costmap_2d::Costmap2D& master_grid, int min_i, int min_j,
00073                                            int max_i, int max_j)
00074 {
00075   if (!enabled_)
00076     return;
00077   unsigned char* master = master_grid.getCharMap();
00078   unsigned int span = master_grid.getSizeInCellsX();
00079 
00080   for (int j = min_j; j < max_j; j++)
00081   {
00082     unsigned int it = span*j+min_i;
00083     for (int i = min_i; i < max_i; i++)
00084     {
00085       master[it] = costmap_[it];
00086       it++;
00087     }
00088   }
00089 }
00090 
00091 void CostmapLayer::updateWithOverwrite(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
00092 {
00093   if (!enabled_)
00094     return;
00095   unsigned char* master = master_grid.getCharMap();
00096   unsigned int span = master_grid.getSizeInCellsX();
00097 
00098   for (int j = min_j; j < max_j; j++)
00099   {
00100     unsigned int it = span*j+min_i;
00101     for (int i = min_i; i < max_i; i++)
00102     {
00103       if (costmap_[it] != NO_INFORMATION)
00104         master[it] = costmap_[it];
00105       it++;
00106     }
00107   }
00108 }
00109 
00110 void CostmapLayer::updateWithAddition(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
00111 {
00112   if (!enabled_)
00113     return;
00114   unsigned char* master_array = master_grid.getCharMap();
00115   unsigned int span = master_grid.getSizeInCellsX();
00116 
00117   for (int j = min_j; j < max_j; j++)
00118   {
00119     unsigned int it = j * span + min_i;
00120     for (int i = min_i; i < max_i; i++)
00121     {
00122       if (costmap_[it] == NO_INFORMATION){
00123         it++;
00124         continue;
00125       }
00126 
00127       unsigned char old_cost = master_array[it];
00128       if (old_cost == NO_INFORMATION)
00129         master_array[it] = costmap_[it];
00130       else
00131       {
00132         int sum = old_cost + costmap_[it];
00133         if (sum >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
00134             master_array[it] = costmap_2d::INSCRIBED_INFLATED_OBSTACLE - 1;
00135         else
00136             master_array[it] = sum;
00137       }
00138       it++;
00139     }
00140   }
00141 }
00142 }  // namespace costmap_2d


costmap_2d
Author(s): Eitan Marder-Eppstein, David V. Lu!!, Dave Hershberger, contradict@gmail.com
autogenerated on Wed Aug 2 2017 03:12:21