Merge pull request ros-navigation#1354 from SteveMacenski/vorpal

fix clearing logic for near-max_obj_height instances

nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp

@@ -152,6 +152,11 @@ class VoxelLayer : public ObstacleLayer
   {
     return sqrt((x1 - x0) * (x1 - x0) + (y1 - y0) * (y1 - y0) + (z1 - z0) * (z1 - z0));
   }
+
+  double getSizeInMetersZ() const
+  {
+    return (size_z_ - 1 + 0.5) * z_resolution_;
+  }
 };
 
 }  // namespace nav2_costmap_2d

nav2_costmap_2d/plugins/voxel_layer.cpp

@@ -314,6 +314,7 @@ void VoxelLayer::raytraceFreespace(
   // we can pre-compute the enpoints of the map outside of the inner loop... we'll need these later
   double map_end_x = origin_x_ + getSizeInMetersX();
   double map_end_y = origin_y_ + getSizeInMetersY();
+  double map_end_z = origin_z_ + getSizeInMetersZ();
 
   sensor_msgs::PointCloud2ConstIterator<float> iter_x(*(clearing_observation.cloud_), "x");
   sensor_msgs::PointCloud2ConstIterator<float> iter_y(*(clearing_observation.cloud_), "y");
@@ -337,9 +338,9 @@ void VoxelLayer::raytraceFreespace(
     double t = 1.0;
 
     // we can only raytrace to a maximum z height
-    if (wpz > max_obstacle_height_) {
+    if (wpz > map_end_z) {
       // we know we want the vector's z value to be max_z
-      t = std::max(0.0, std::min(t, (max_obstacle_height_ - 0.01 - oz) / c));
+      t = std::max(0.0, std::min(t, (map_end_z - 0.01 - oz) / c));
     } else if (wpz < origin_z_) {
       // and we can only raytrace down to the floor
       // we know we want the vector's z value to be 0.0