Costmap - Voxel - Obstacle mismatch

[GeorgiosGkovo]

Hello, Using the STVL plugin I noticed a slight misalignment in Rviz between between the costmap cells classified as LETHAL, the voxel positioning and the actual points of the obstacle detected by the lidar sensor. I am attaching a photo showcasing the mismatch along with the parameters I am using for the layer. Note that, the issued is persistent in the entirety of the map, my photo showcases jst one location where the issue can be observed. The main issue is that the resulting costmap used for obstacle avoidance is wrong and can potentially cause problems. I managed to fix the issue by translating the underlying grid by half a voxel in all directions to make voxels and and costmap aligned. I also used rounding after the WorldToIndex function to align the observed points with the created voxels, which seems to work. I am not sure whether this is an issue experienced by others or if there is some issue with my setup. I am using ROS Noetic.

local_costmap:
  global_frame: odom
  robot_base_frame: base_link
  footprint: [[-1.5, -0.7], [-1.5, 0.7], [1.5, 0.7], [1.5, -0.7]] # should be same as footprint in teb config, used for feasibility checking 

  update_frequency: 5.0 # [Hz] The frequency in Hz for the map to be updated.
  publish_frequency: 2.0 # [Hz]  The frequency in Hz for the map to publish display information.

  rolling_window: true
  width: 25 # [m]
  height: 25 # [m]
  resolution: 0.25
  origin_x: 0.
  origin_y: 0.

  always_send_full_costmap: true

  plugins:
    - {name: spatio_temporal_voxel_obstacle_layer, type: "spatio_temporal_voxel_layer/SpatioTemporalVoxelLayer"}

  spatio_temporal_voxel_obstacle_layer:
    enabled: true
    voxel_decay: 10 
    decay_model: 0 
    voxel_size: 0.25 
    track_unknown_space: true 
    unknown_threshold: 15  
    mark_threshold: 0
    update_footprint_enabled: true
    combination_method: 1
    obstacle_range: 13.0 
    origin_z: 0.0 
    publish_voxel_map: true 
    transform_tolerance: 0.5 
    mapping_mode: false 
    map_save_duration: 60 
    observation_sources:  lidar_3D  

    lidar_3D:
      enabled: true #default true, can be toggled on/off with associated service call
      data_type: PointCloud2
      topic: /points3d
      marking: true
      clearing: true
      max_z: 15.0 
      min_z: .0 
      vertical_fov_angle: 1.5708 
      vertical_fov_padding: 0.05 
      horizontal_fov_angle: 6.29 
      model_type: 1 
      filter: "voxel" 
      min_obstacle_height: 0.2 
      max_obstacle_height: 2.8 
      voxel_min_points: 0 
      expected_update_rate: 0.0 
      observation_persistence: 0.0 
      inf_is_valid: false 
      clear_after_reading: true 
      decay_acceleration: 50 
      sensor_frame: os_sensor

[SteveMacenski]

@GeorgiosGkovo can you test the patch in https://github.com/SteveMacenski/spatio_temporal_voxel_layer/pull/226 that is applied to ROS 2? It looks like I didn't backport this to ROS 1. Admittedly, I don't think much about ROS 1 anymore so that's an understandable accidental omission :disappointed_relieved:

If that patch works for you (and submitting a PR with those changes would be nice!) I can run a release.

[GeorgiosGkovo]

@SteveMacenski I tried the suggested fix and it appears to solve my issue. I will submit a PR as soon as possible.