otimal deep coppy obj

plugins/inflation_layer.cpp

@@ -58,7 +58,6 @@ InflationLayer::InflationLayer()
   , inflate_unknown_(false)
   , cell_inflation_radius_(0)
   , cached_cell_inflation_radius_(0)
-  , seen_(NULL)
   , cached_costs_(NULL)
   , cached_distances_(NULL)
   , last_min_x_(-std::numeric_limits<float>::max())
@@ -76,10 +75,8 @@ void InflationLayer::onInitialize()
 
   boost::unique_lock < boost::recursive_mutex > lock(*inflation_access_);
   current_ = true;
-  if (seen_)
-    delete[] seen_;
-  seen_ = NULL;
-  seen_size_ = 0;
+  seen_.clear();
+  seen_generation_ = 0;
   need_reinflation_ = false;
   std::string config_file_name = "inflation_layer_params.yaml";
   // std::cout << "InflationLayer: " << config_file_name << std::endl;
@@ -144,10 +141,8 @@ void InflationLayer::matchSize()
   computeCaches();
 
   unsigned int size_x = costmap->getSizeInCellsX(), size_y = costmap->getSizeInCellsY();
-  if (seen_)
-    delete[] seen_;
-  seen_size_ = size_x * size_y;
-  seen_ = new bool[seen_size_];
+  seen_.assign(static_cast<std::size_t>(size_x) * size_y, 0);
+  seen_generation_ = 0;
 }
 
 void InflationLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x,
@@ -203,26 +198,28 @@ void InflationLayer::updateCosts(robot_costmap_2d::Costmap2D& master_grid, int m
   if (cell_inflation_radius_ == 0)
     return;
 
-  // make sure the inflation list is empty at the beginning of the cycle (should always be true)
-  if(!inflation_cells_.empty())
-    robot::log_error("The inflation list must be empty at the beginning of inflation\n");
+  for (std::vector<CellData>& cells : inflation_cells_)
+    cells.clear();
 
   unsigned char* master_array = master_grid.getCharMap();
   unsigned int size_x = master_grid.getSizeInCellsX(), size_y = master_grid.getSizeInCellsY();
 
-  if (seen_ == NULL) {
-    robot::log_error("InflationLayer::updateCosts(): seen_ array is NULL\n");
-    seen_size_ = size_x * size_y;
-    seen_ = new bool[seen_size_];
-  }
-  else if (seen_size_ != size_x * size_y)
+  const std::size_t map_size = static_cast<std::size_t>(size_x) * size_y;
+  if (seen_.size() != map_size)
   {
-    robot::log_error("InflationLayer::updateCosts(): seen_ array size is wrong\n");
-    delete[] seen_;
-    seen_size_ = size_x * size_y;
-    seen_ = new bool[seen_size_];
+    seen_.assign(map_size, 0);
+    seen_generation_ = 0;
+  }
+
+  if (seen_generation_ == std::numeric_limits<std::uint32_t>::max())
+  {
+    std::fill(seen_.begin(), seen_.end(), 0);
+    seen_generation_ = 1;
+  }
+  else
+  {
+    ++seen_generation_;
   }
-  memset(seen_, false, size_x * size_y * sizeof(bool));
 
   // We need to include in the inflation cells outside the bounding
   // box min_i...max_j, by the amount cell_inflation_radius_.  Cells
@@ -238,11 +235,13 @@ void InflationLayer::updateCosts(robot_costmap_2d::Costmap2D& master_grid, int m
   max_i = std::min(int(size_x), max_i);
   max_j = std::min(int(size_y), max_j);
 
-  // Inflation list; we append cells to visit in a list associated with its distance to the nearest obstacle
-  // We use a map<distance, list> to emulate the priority queue used before, with a notable performance boost
+  // Precomputed distance buckets preserve priority ordering without a tree lookup
+  // for every enqueued cell.
 
   // Start with lethal obstacles: by definition distance is 0.0
-  std::vector<CellData>& obs_bin = inflation_cells_[0.0];
+  if (inflation_cells_.empty())
+    return;
+  std::vector<CellData>& obs_bin = inflation_cells_.front();
   for (int j = min_j; j < max_j; j++)
   {
     for (int i = min_i; i < max_i; i++)
@@ -258,23 +257,22 @@ void InflationLayer::updateCosts(robot_costmap_2d::Costmap2D& master_grid, int m
 
   // Process cells by increasing distance; new cells are appended to the corresponding distance bin, so they
   // can overtake previously inserted but farther away cells
-  std::map<double, std::vector<CellData> >::iterator bin;
-  for (bin = inflation_cells_.begin(); bin != inflation_cells_.end(); ++bin)
+  for (std::vector<CellData>& bin : inflation_cells_)
   {
-    for (int i = 0; i < bin->second.size(); ++i)
+    for (std::size_t i = 0; i < bin.size(); ++i)
     {
       // process all cells at distance dist_bin.first
-      const CellData& cell = bin->second[i];
+      const CellData& cell = bin[i];
 
       unsigned int index = cell.index_;
 
       // ignore if already visited
-      if (seen_[index])
+      if (seen_[index] == seen_generation_)
       {
         continue;
       }
 
-      seen_[index] = true;
+      seen_[index] = seen_generation_;
 
       unsigned int mx = cell.x_;
       unsigned int my = cell.y_;
@@ -301,7 +299,6 @@ void InflationLayer::updateCosts(robot_costmap_2d::Costmap2D& master_grid, int m
     }
   }
 
-  inflation_cells_.clear();
 }
 
 /**
@@ -316,7 +313,7 @@ void InflationLayer::updateCosts(robot_costmap_2d::Costmap2D& master_grid, int m
 inline void InflationLayer::enqueue(unsigned int index, unsigned int mx, unsigned int my,
                                     unsigned int src_x, unsigned int src_y)
 {
-  if (!seen_[index])
+  if (seen_[index] != seen_generation_)
   {
     // we compute our distance table one cell further than the inflation radius dictates so we can make the check below
     double distance = distanceLookup(mx, my, src_x, src_y);
@@ -325,8 +322,10 @@ inline void InflationLayer::enqueue(unsigned int index, unsigned int mx, unsigne
     if (distance > cell_inflation_radius_)
       return;
 
-    // push the cell data onto the inflation list and mark
-    inflation_cells_[distance].push_back(CellData(index, mx, my, src_x, src_y));
+    const unsigned int dx = std::abs(static_cast<int>(mx) - static_cast<int>(src_x));
+    const unsigned int dy = std::abs(static_cast<int>(my) - static_cast<int>(src_y));
+    const unsigned int bin_index = distance_bin_lookup_[dx * distance_lookup_size_ + dy];
+    inflation_cells_[bin_index].push_back(CellData(index, mx, my, src_x, src_y));
   }
 }
 
@@ -354,6 +353,38 @@ void InflationLayer::computeCaches()
     }
 
     cached_cell_inflation_radius_ = cell_inflation_radius_;
+
+    distance_lookup_size_ = cell_inflation_radius_ + 2;
+    distance_levels_.clear();
+    for (unsigned int i = 0; i < distance_lookup_size_; ++i)
+    {
+      for (unsigned int j = 0; j < distance_lookup_size_; ++j)
+      {
+        if (cached_distances_[i][j] <= cell_inflation_radius_)
+          distance_levels_.push_back(cached_distances_[i][j]);
+      }
+    }
+    std::sort(distance_levels_.begin(), distance_levels_.end());
+    distance_levels_.erase(
+        std::unique(distance_levels_.begin(), distance_levels_.end()), distance_levels_.end());
+
+    inflation_cells_.clear();
+    inflation_cells_.resize(distance_levels_.size());
+    distance_bin_lookup_.assign(
+        static_cast<std::size_t>(distance_lookup_size_) * distance_lookup_size_, 0);
+    for (unsigned int i = 0; i < distance_lookup_size_; ++i)
+    {
+      for (unsigned int j = 0; j < distance_lookup_size_; ++j)
+      {
+        const double distance = cached_distances_[i][j];
+        if (distance > cell_inflation_radius_)
+          continue;
+        distance_bin_lookup_[i * distance_lookup_size_ + j] =
+            static_cast<unsigned int>(
+                std::lower_bound(distance_levels_.begin(), distance_levels_.end(), distance) -
+                distance_levels_.begin());
+      }
+    }
   }
 
   for (unsigned int i = 0; i <= cell_inflation_radius_ + 1; ++i)
@@ -367,6 +398,10 @@ void InflationLayer::computeCaches()
 
 void InflationLayer::deleteKernels()
 {
+  inflation_cells_.clear();
+  distance_levels_.clear();
+  distance_bin_lookup_.clear();
+  distance_lookup_size_ = 0;
   if (cached_distances_ != NULL)
   {
     for (unsigned int i = 0; i <= cached_cell_inflation_radius_ + 1; ++i)