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 }
costmap_2d
Author(s): Eitan Marder-Eppstein, David V. Lu!!, Dave Hershberger, contradict@gmail.com
autogenerated on Sun Mar 3 2019 03:46:15