otimal

include/robot_costmap_2d/voxel_layer.h

@@ -51,6 +51,9 @@
 #include <robot_costmap_2d/obstacle_layer.h>
 #include <robot_voxel_grid/voxel_grid.h>
 
+#include <limits>
+#include <vector>
+
 namespace robot_costmap_2d
 {
 
@@ -102,11 +105,42 @@ private:
   bool clearVoxelRay(double ox, double oy, double oz, double wx, double wy, double wz,
                      double raytrace_range, unsigned int cell_raytrace_range,
                      double* min_x, double* min_y, double* max_x, double* max_y);
+  bool clearDepthColumns(double ox, double oy, double cover_distance, double far_distance,
+                         double min_range, double max_range, double skip_dist,
+                         double* min_x, double* min_y, double* max_x, double* max_y);
+  bool clipColumnSegment(double& sx, double& sy, double& ex, double& ey) const;
 
 
   bool publish_voxel_;
   robot_voxel_grid::VoxelGrid robot_voxel_grid_;
   double z_resolution_, origin_z_;
+  /// Clearing rays stop this far [m] before the measured surface.
+  /// Negative keeps the legacy 2 * resolution behaviour.
+  double frustum_skip_distance_ = -1.0;
+
+  /// Full-column clearing: per depth-image pixel column, the nearest return
+  /// inside the obstacle height band certifies "no obstacle in this direction
+  /// closer than d". Cells along that 2D beam get their whole voxel column
+  /// cleared, removing marked voxels the per-pixel 3D rays cannot reach
+  /// (above the vertical FOV at close range).
+  bool frustum_column_clearing_ = false;
+  /// Height band [m] used to detect the nearest in-band return. Points below
+  /// the band (floor) do not shorten the beam. max < 0: use max_obstacle_height.
+  double column_clear_min_height_ = 0.10;
+  double column_clear_max_height_ = -1.0;
+  /// Full columns are only cleared beyond this distance [m], where the
+  /// vertical FOV covers the whole height band. Negative: derive each frame
+  /// from the camera intrinsics and mounting pose.
+  double column_cover_distance_ = -1.0;
+
+  struct DepthColumnStat
+  {
+    double min_band_dist = -1.0;  ///< horizontal distance of nearest in-band return; < 0 = none
+    double azimuth = 0.0;         ///< beam direction in the global frame
+    double best_row_delta = std::numeric_limits<double>::infinity();
+    bool has_ray = false;         ///< column had at least one readable pixel
+  };
+  std::vector<DepthColumnStat> depth_column_stats_;
   unsigned int unknown_threshold_, mark_threshold_, size_z_;
   robot_sensor_msgs::PointCloud clearing_endpoints_;
   std::vector<unsigned char> rolling_costmap_scratch_;