first commit

src/costmap_layer.cpp

@@ -0,0 +1,173 @@
+#include<costmap_2d/costmap_layer.h>
+
+namespace costmap_2d
+{
+
+void CostmapLayer::touch(double x, double y, double* min_x, double* min_y, double* max_x, double* max_y)
+{
+    *min_x = std::min(x, *min_x);
+    *min_y = std::min(y, *min_y);
+    *max_x = std::max(x, *max_x);
+    *max_y = std::max(y, *max_y);
+}
+
+void CostmapLayer::matchSize()
+{
+    Costmap2D* master = layered_costmap_->getCostmap();
+    resizeMap(master->getSizeInCellsX(), master->getSizeInCellsY(), master->getResolution(),
+            master->getOriginX(), master->getOriginY());
+}
+
+void CostmapLayer::clearArea(int start_x, int start_y, int end_x, int end_y, bool invert_area)
+{
+  unsigned char* grid = getCharMap();
+  for(int x=0; x<(int)getSizeInCellsX(); x++){
+    bool xrange = x>start_x && x<end_x;
+
+    for(int y=0; y<(int)getSizeInCellsY(); y++){
+      if((xrange && y>start_y && y<end_y)!=invert_area)
+        continue;
+      int index = getIndex(x,y);
+      if(grid[index]!=NO_INFORMATION){
+        grid[index] = NO_INFORMATION;
+      }
+    }
+  }
+}
+
+void CostmapLayer::addExtraBounds(double mx0, double my0, double mx1, double my1)
+{
+    extra_min_x_ = std::min(mx0, extra_min_x_);
+    extra_max_x_ = std::max(mx1, extra_max_x_);
+    extra_min_y_ = std::min(my0, extra_min_y_);
+    extra_max_y_ = std::max(my1, extra_max_y_);
+    has_extra_bounds_ = true;
+}
+
+void CostmapLayer::useExtraBounds(double* min_x, double* min_y, double* max_x, double* max_y)
+{
+    if (!has_extra_bounds_)
+        return;
+
+    *min_x = std::min(extra_min_x_, *min_x);
+    *min_y = std::min(extra_min_y_, *min_y);
+    *max_x = std::max(extra_max_x_, *max_x);
+    *max_y = std::max(extra_max_y_, *max_y);
+    extra_min_x_ = 1e6;
+    extra_min_y_ = 1e6;
+    extra_max_x_ = -1e6;
+    extra_max_y_ = -1e6;
+    has_extra_bounds_ = false;
+}
+
+void CostmapLayer::updateWithMax(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
+{
+  if (!enabled_)
+    return;
+
+  unsigned char* master_array = master_grid.getCharMap();
+  unsigned int span = master_grid.getSizeInCellsX();
+ 
+  for (int j = min_j; j < max_j; j++)
+  { 
+    unsigned int it = j * span + min_i;
+    for (int i = min_i; i < max_i; i++)
+    { 
+      if (costmap_[it] == NO_INFORMATION){
+        it++;
+        continue;
+      }
+      if (costmap_[it] == CRITICAL_SPACE){
+        master_array[it] = costmap_[it];
+        it++;
+        continue;
+      }
+      if (costmap_[it] == PREFERRED_SPACE && master_array[it] < INSCRIBED_INFLATED_OBSTACLE ){
+        master_array[it] = costmap_[it];
+        it++;
+        continue;
+      }
+      if (costmap_[it] == UNPREFERRED_SPACE && master_array[it] < INSCRIBED_INFLATED_OBSTACLE ){
+        master_array[it] = costmap_[it];
+        it++;
+        continue;
+      }
+      unsigned char old_cost = master_array[it];
+      if (old_cost == NO_INFORMATION || old_cost < costmap_[it])
+        master_array[it] = costmap_[it];
+      it++;
+    }
+  }
+}
+
+void CostmapLayer::updateWithTrueOverwrite(costmap_2d::Costmap2D& master_grid, int min_i, int min_j,
+                                           int max_i, int max_j)
+{
+  if (!enabled_)
+    return;
+  unsigned char* master = master_grid.getCharMap();
+  unsigned int span = master_grid.getSizeInCellsX();
+
+  for (int j = min_j; j < max_j; j++)
+  {
+    unsigned int it = span*j+min_i;
+    for (int i = min_i; i < max_i; i++)
+    {
+      master[it] = costmap_[it];
+      it++;
+    }
+  }
+}
+
+void CostmapLayer::updateWithOverwrite(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
+{
+  if (!enabled_)
+    return;
+  unsigned char* master = master_grid.getCharMap();
+  unsigned int span = master_grid.getSizeInCellsX();
+
+  for (int j = min_j; j < max_j; j++)
+  {
+    unsigned int it = span*j+min_i;
+    for (int i = min_i; i < max_i; i++)
+    {
+      if (costmap_[it] != NO_INFORMATION)
+        master[it] = costmap_[it];
+      it++;
+    }
+  }
+}
+
+void CostmapLayer::updateWithAddition(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
+{
+  if (!enabled_)
+    return;
+  unsigned char* master_array = master_grid.getCharMap();
+  unsigned int span = master_grid.getSizeInCellsX();
+
+  for (int j = min_j; j < max_j; j++)
+  {
+    unsigned int it = j * span + min_i;
+    for (int i = min_i; i < max_i; i++)
+    {
+      if (costmap_[it] == NO_INFORMATION){
+        it++;
+        continue;
+      }
+
+      unsigned char old_cost = master_array[it];
+      if (old_cost == NO_INFORMATION)
+        master_array[it] = costmap_[it];
+      else
+      {
+        int sum = old_cost + costmap_[it];
+        if (sum >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
+            master_array[it] = costmap_2d::INSCRIBED_INFLATED_OBSTACLE - 1;
+        else
+            master_array[it] = sum;
+      }
+      it++;
+    }
+  }
+}
+}  // namespace costmap_2d