Some possible bugs in voxel_layer

[coderbaibai]

Bug report

Required Info:

• Operating System: Ubuntu 24.04
• Computer:12th Gen Interl(R) Core(TM) i5-1240P
• ROS2 Version: jazzy
• Version or commit hash:0aee198
• DDS implementation:fastdds

Steps to reproduce issue

1.Use voxel layer on local_costmap and start a simulation or a real robot.
for example

plugins: ["voxel_layer", "inflation_layer"]
inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
        inflation_radius: 0.5
      voxel_layer:
        plugin: "nav2_costmap_2d::VoxelLayer"
        enabled: True
        publish_voxel_map: True
        origin_z: 0.0
        z_resolution: 0.05
        z_voxels: 16
        max_obstacle_height: 2.0
        mark_threshold: 0
        observation_sources: scan
        scan:
          topic: /scan
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "LaserScan"
          raytrace_max_range: 3.0
          raytrace_min_range: 0.0
          obstacle_max_range: 2.5
          obstacle_min_range: 0.0

2. Subscribing /local_costmap/clearing_endpoints in rviz2.
3. Send some goal to the robot, drive it around.

Expected behavior

The points of /local_costmap/clearing_endpoints are stable in some ways.I mean we would not abandon points easily.

Actual behavior

Many points are abandon.I print the infomation of the points, I found they are all on the boundary of the local_costmap.

Reproduction instructions

Add some tolerant for the precision.As a result it works.

inline bool worldToMap3DFloat(
    double wx, double wy, double wz, double & mx, double & my,
    double & mz)
  {
    if (wx < origin_x_-0.001 || wy < origin_y_-0.001 || wz < origin_z_-0.001) {
      RCLCPP_WARN(logger_,"drop point (%.3lf,%.3lf,%.3lf) below (%.3lf,%.3lf,%.3lf)",wx,wy,wz,origin_x_,origin_y_,origin_z_);
      return false;
    }
    if(wx < origin_x_) wx = origin_x_;
    if(wy < origin_y_) wy = origin_y_;
    if(wz < origin_z_) wz = origin_z_;
    mx = ((wx - origin_x_) / resolution_);
    my = ((wy - origin_y_) / resolution_);
    mz = ((wz - origin_z_) / z_resolution_);
    if (mx < size_x_ && my < size_y_ && mz < size_z_) {
      return true;
    }
    RCLCPP_WARN(logger_,"drop point (%.3lf,%.3lf,%.3lf) over (%.3lf,%.3lf,%.3lf)",wx,wy,wz,size_x_*resolution_+origin_x_,size_y_*resolution_+origin_y_,size_z_*z_resolution_+origin_z_);
    return false;
  }

[SteveMacenski]

Can you provide a video so I can see what you mean?

[coderbaibai]

Sure.Please pay attention to the green boxs in the video.They are flashing.

vl_videomp4.mp4

And I print the information of the abandoned points.

[SteveMacenski]

I'm failing to see the issue. val >= origin in the 3 axes to be considered. If its lower then its out of bounds of the costmap. Adding that tolerance would breach the bounds of the costmap and shouldn't be considered.

worldToMap3DFloat is used in 2x places:

• Finding the sensor's location in the costmap
• Finding the sensor data point in the costmap

If the value is less than the origin, then the translation would be negative. When converting back to the world frame, we add +0.5 of the costmap size to account for the binning down when converting worldToMap, but worldToMap itself should not have that tolerance added in.

I think what you're describing would slightly overclear. Are you seeing when you have the jumping in the clearing that there's noise in the sensor data at those points where sometimes they're in bounds and sometimes they're not? It seems to me like there's some edge aliasing of the laser scanner so there are noisy points along the edge from the obstacle dropping back into the larger scheme, so the 'dancing' are an actual effect of sometimes there's data there and sometimes there's not based on random noise.

[coderbaibai]

From the perspective of software architecture and function definition, adding tolerance is indeed not a good solution. But I still have doubts about this issue. Anyway, shouldn't we project all the points scanned by the laser onto our cost map and eliminate obstacles through ray tracing
In fact, the original point cloud in the video I uploaded before was always outside the local cost map. They are connected to the sensor origin and intersect with the local cost map boundary. The problem is that the intersection points we obtain may not be within the local cost map. This doesn't seem to be the result we wanted. @SteveMacenski

[SteveMacenski]

The point has to be valid in the costmap in order to raytrace or we're going to be accessing bogus or out of bounds data. By checking if the end points are valid, we know that the ray that it comprises is also validly in the costmap in a convex shape (the square costmap).

I don't disagree with you that we could add in checks for this and raytrace to the extent the costmap allows by its bounds, the maximum raytracing range, as long as that point is below the maximum obstacle range. Would you be interested in contributing that?

[coderbaibai]

Sure!I will show more details and code of this issue soon.

[SteveMacenski]

Hi! Any update when this might be ready?

[coderbaibai]

I'm sorry for not replying for a long time due to work. Maybe the following code is a good way to solve this problem.

diff --git a/nav2_costmap_2d/plugins/voxel_layer.cpp b/nav2_costmap_2d/plugins/voxel_layer.cpp
index 3c1e069f..d7605271 100644
--- a/nav2_costmap_2d/plugins/voxel_layer.cpp
+++ b/nav2_costmap_2d/plugins/voxel_layer.cpp
@@ -352,31 +352,48 @@ void VoxelLayer::raytraceFreespace(
     double b = wpy - oy;
     double c = wpz - oz;
     double t = 1.0;
+    bool wp_outside = false;
 
     // we can only raytrace to a maximum z 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, (map_end_z - 0.01 - oz) / c));
+      wp_outside = true;
     } 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
       t = std::min(t, (origin_z_ - oz) / c);
+      wp_outside = true;
     }
 
     // the minimum value to raytrace from is the origin
     if (wpx < origin_x_) {
       t = std::min(t, (origin_x_ - ox) / a);
+      wp_outside = true;
     }
     if (wpy < origin_y_) {
       t = std::min(t, (origin_y_ - oy) / b);
+      wp_outside = true;
     }
 
     // the maximum value to raytrace to is the end of the map
     if (wpx > map_end_x) {
       t = std::min(t, (map_end_x - ox) / a);
+      wp_outside = true;
     }
     if (wpy > map_end_y) {
       t = std::min(t, (map_end_y - oy) / b);
+      wp_outside = true;
+    }
+
+    constexpr double wp_epsilon = 1e-5;
+    if(wp_outside){
+      if(t>0.0){
+        t-=wp_epsilon;
+      }
+      else if(t<0.0){
+        t+=wp_epsilon;
+      }
     }
 
     wpx = ox + a * t;

And then the points become stable without drop out like this.

2025-06-25.00-15-50.webm

This is just one of the solutions and more tests are needed to be done. @SteveMacenski

[SteveMacenski]

That seems sane to me! Please go ahead and test or let me know if you prefer another solution. Open a PR once you're ready! :-)

[coderbaibai]

It's OK

[SteveMacenski]

Sorry - I don't understand what you mean by that in this context. Can you clarify?

[coderbaibai]

This solution seems to be effective.I will open a PR later.