Updates to raytrace clearing, edge case bugfix

costmap_2d/plugins/voxel_layer.cpp

@@ -295,14 +295,6 @@ void VoxelLayer::clear(std::vector<Observation>& observations, double* min_x, do
   {
     unsigned int max_raytrace_range_in_cells = std::floor(max_raytrace_range_ / resolution_);
 
-    if (max_raytrace_range_in_cells < size_y_ || max_raytrace_range_in_cells < size_x_)
-    {
-      ROS_WARN_STREAM_ONCE(
-          "The raytrace_range is set to something smaller than the costmap size, max_raytrace_range_in_cells: " << max_raytrace_range_in_cells << ", costmap size: " << size_x_ << "x" << size_y_);
-      ROS_WARN_ONCE(
-          "Make sure that the measurements stay inside the raytrace_range, else some nice random memory over-writing happens.");
-    }
-
     update_area_center = max_raytrace_range_in_cells + 1; // +1 to have a buffer
     unsigned int cached_update_area_width = update_area_center * 2 + 1; //+1 to have a center point
 
@@ -445,7 +437,13 @@ void VoxelLayer::raytraceFreespace(const Observation& clearing_observation, boos
     //In the case we go outside, we want to go all the way to the border (and also clear the cell at the border).
     //Because of this we crop at 0.0 - epsilon and size (which is max index +1), "setting the obstacle cell just outside of the border".
     double cropped_distance = 0.0;
-    double scaling = 1.0;
+
+    // Initialize scale based on raytrace range.
+    // When c++17 is supported, use hypot(abs_dx, abs_dy, abs_dz) to compute observation length
+    double obs_len = std::sqrt((abs_dx * resolution_) * (abs_dx * resolution_) +
+                               (abs_dy * resolution_) * (abs_dy * resolution_) +
+                               (abs_dz * z_resolution_) * (abs_dz * z_resolution_));
+    double scaling = std::min(1.0, max_raytrace_range_/obs_len);
 
     //Check if we go outside, and set the scaling factor accordingly
     if (point_x < 0.0)

voxel_grid/include/voxel_grid/cached_clearer.h

@@ -27,106 +27,120 @@ class CachedClearer : public AbstractGridUpdater
     costmap_size_y_ = costmap_size_y;
 
     //should be overwritten later
-    offset_from_costmap_x_ = costmap_size_x * 0.5;
-    offset_from_costmap_y_ = costmap_size_y * 0.5;
+    current_offset_from_costmap_x_ = costmap_size_x * 0.5;
+    current_offset_from_costmap_y_ = costmap_size_y * 0.5;
 
     cache_width_ = cache_width;
     z_is_offset_ = z_is_offset;
 
-    unsigned int cache_size = cache_width_ * cache_width_;
-    clearing_masks_cache_ = boost::shared_ptr<uint32_t[]>(new uint32_t[cache_size]);
     updated_cells_indices_ = boost::shared_ptr < std::list<std::pair<unsigned int, unsigned int> >
         > (new std::list<std::pair<unsigned int, unsigned int> >);
+  }
 
+  virtual boost::shared_ptr<uint32_t[]> getNewCache()
+  {
+    unsigned int cache_size = cache_width_ * cache_width_;
+    boost::shared_ptr<uint32_t[]> cache(new uint32_t[cache_size]);
     uint32_t empty_mask = (uint32_t)0;
     for (int i = 0; i < cache_size; ++i)
-      clearing_masks_cache_[i] = empty_mask;
+      cache[i] = empty_mask;
+    return cache;
   }
-  ;
 
   virtual void setCostmapOffsets(int offset_from_costmap_x, int offset_from_costmap_y)
   {
-    offset_from_costmap_x_ = offset_from_costmap_x;
-    offset_from_costmap_y_ = offset_from_costmap_y;
+    std::pair<int, int> costmap_offset(offset_from_costmap_x, offset_from_costmap_y);
+    if (sensor_clearing_caches_.find(costmap_offset) == sensor_clearing_caches_.end())
+    {
+      sensor_clearing_caches_[costmap_offset] = getNewCache();
+    }
+    current_offset_from_costmap_x_ = offset_from_costmap_x;
+    current_offset_from_costmap_y_ = offset_from_costmap_y;
   }
 
   inline virtual void operator()(unsigned int offset, uint32_t z_mask)
   {
-    clearing_masks_cache_[offset] |= z_mask;
+    sensor_clearing_caches_[std::make_pair(current_offset_from_costmap_x_,
+                                           current_offset_from_costmap_y_)][offset] |= z_mask;
   }
 
   virtual void update()
   {
     updated_cells_indices_->clear();
     cleared_voxels_ = sensor_msgs::PointCloud();
-
-    unsigned int cache_size_x = cache_width_; //for readability
-    unsigned int cache_size_y = cache_width_; //for readability
-    unsigned int linear_cache_index = 0;
-    unsigned int linear_costmap_index = 0;
-    int costmap_x = 0;
-    int costmap_y = 0;
-
-    for (unsigned int y_index = 0; y_index < cache_size_y; ++y_index)
+    std::map<std::pair<int, int>, boost::shared_ptr<uint32_t[]> >::iterator it;
+    for (it=sensor_clearing_caches_.begin(); it!=sensor_clearing_caches_.end(); ++it)
     {
-      linear_cache_index = y_index * cache_size_x;
-      costmap_y = y_index - offset_from_costmap_y_;
+      unsigned int cache_size_x = cache_width_; //for readability
+      unsigned int cache_size_y = cache_width_; //for readability
+      unsigned int linear_cache_index = 0;
+      unsigned int linear_costmap_index = 0;
+      int costmap_x = 0;
+      int costmap_y = 0;
+      int sensor_offset_x = it->first.first;
+      int sensor_offset_y = it->first.second;
+
+      for (unsigned int y_index = 0; y_index < cache_size_y; ++y_index)
+      {
+        linear_cache_index = y_index * cache_size_x;
+        costmap_y = y_index - sensor_offset_y;
 
-      if (costmap_y < 0)
-        continue;
+        if (costmap_y < 0)
+          continue;
 
-      if (costmap_y >= costmap_size_y_)
-        break;
+        if (costmap_y >= costmap_size_y_)
+          break;
 
-      costmap_x = -offset_from_costmap_x_;
+        costmap_x = -sensor_offset_x;
 
-      linear_costmap_index = costmap_y * costmap_size_x_ + costmap_x;
+        linear_costmap_index = costmap_y * costmap_size_x_ + costmap_x;
 
-      for (unsigned int x_index = 0; x_index < cache_size_x; ++x_index)
-      {
-        uint32_t clearing_mask = clearing_masks_cache_[linear_cache_index];
+        for (unsigned int x_index = 0; x_index < cache_size_x; ++x_index)
+        {
+          uint32_t clearing_mask = it->second[linear_cache_index];
 
-        if (clearing_mask == (uint32_t)0)
-        { //not updated
-          costmap_x++;
-          linear_costmap_index++;
-          linear_cache_index++;
-          continue;
-        }
+          if (clearing_mask == (uint32_t)0)
+          { //not updated
+            costmap_x++;
+            linear_costmap_index++;
+            linear_cache_index++;
+            continue;
+          }
 
-        if (z_is_offset_)
-          clearing_mask = undo_clearing_mask_offset(clearing_mask);
+          if (z_is_offset_)
+            clearing_mask = undoClearingMaskOffset(clearing_mask);
 
-        if (costmap_x < 0)
-        { //In case of underflow because of corner cases
-          costmap_x++;
-          linear_costmap_index++;
-          linear_cache_index++;
-          continue;
-        }
+          if (costmap_x < 0)
+          { //In case of underflow because of corner cases
+            costmap_x++;
+            linear_costmap_index++;
+            linear_cache_index++;
+            continue;
+          }
 
-        if (costmap_x >= costmap_size_x_)
-        { //In case of overflow because of corner cases
-          break;
-        }
+          if (costmap_x >= costmap_size_x_)
+          { //In case of overflow because of corner cases
+            break;
+          }
 
-        uint32_t* voxel_column = &voxel_grid_data_[linear_costmap_index];
+          uint32_t* voxel_column = &voxel_grid_data_[linear_costmap_index];
 
-        *voxel_column &= ~(clearing_mask); //clear unknown and clear cell
+          *voxel_column &= ~(clearing_mask); //clear unknown and clear cell
 
-        updateCostmap(*voxel_column, linear_costmap_index);
-        updated_cells_indices_->push_back(std::make_pair(costmap_x, costmap_y));
+          updateCostmap(*voxel_column, linear_costmap_index);
+          updated_cells_indices_->push_back(std::make_pair(costmap_x, costmap_y));
 
-        updateClearedVoxels(costmap_x, costmap_y, clearing_mask);
+          updateClearedVoxels(costmap_x, costmap_y, clearing_mask);
 
-        costmap_x++;
-        linear_costmap_index++;
-        linear_cache_index++;
+          costmap_x++;
+          linear_costmap_index++;
+          linear_cache_index++;
+        }
       }
     }
   }
 
-  inline virtual uint32_t undo_clearing_mask_offset(uint32_t clearing_mask)
+  inline virtual uint32_t undoClearingMaskOffset(uint32_t clearing_mask)
   {
     clearing_mask >>= 1;
     uint32_t lower_bits_mask = ~((uint32_t)0) >> 16;
@@ -183,12 +197,12 @@ class CachedClearer : public AbstractGridUpdater
   unsigned int costmap_size_x_;
   unsigned int costmap_size_y_;
 
-  int offset_from_costmap_x_;
-  int offset_from_costmap_y_;
+  int current_offset_from_costmap_x_;
+  int current_offset_from_costmap_y_;
   unsigned int cache_width_;
   bool z_is_offset_;
 
-  boost::shared_ptr<uint32_t[]> clearing_masks_cache_;
+  std::map<std::pair<int, int>, boost::shared_ptr<uint32_t[]> > sensor_clearing_caches_; // a cache per sensor location
   boost::shared_ptr<std::list<std::pair<unsigned int, unsigned int> > > updated_cells_indices_;
   sensor_msgs::PointCloud cleared_voxels_;
 };

voxel_grid/include/voxel_grid/voxel_grid.h

@@ -383,11 +383,11 @@ class VoxelGrid
                                           OffC off_c, unsigned int abs_da, unsigned int abs_db, unsigned int abs_dc,
                                           int error_b, int error_c, int offset_a, int offset_b, int offset_c,
                                           unsigned int &offset, unsigned int &z_mask,
-                                          unsigned int number_of_steps)
+                                          int number_of_steps)
   {
     //-1 because we don't want to clear the corners _after_ the last cell
     //last cell is cleared after the loop
-    for (unsigned int i = 0; i < number_of_steps - 1; ++i)
+    for (int i = 0; i < number_of_steps - 1; ++i)
     {
       (*clearer)(offset, z_mask);
       off_a(offset_a);