There is an offset between the grid and the raw data
KavenYau opened this issue · comments
Hi, I am trying to use STVL, but I found a issue that there is an offset between the grid of costmap and the raw data:
Did you see this before? And Have you any ideas of that?
More informations:
ROS version: Foxy
built from source: 396b25b
config of local_costmap:
local_costmap:
local_costmap:
ros__parameters:
update_frequency: 5.0
publish_frequency: 2.0
global_frame: odom
robot_base_frame: base_link
use_sim_time: True
rolling_window: true
width: 8
height: 8
resolution: 0.02
footprint: "[[-0.32, -0.315], [-0.36, -0.275], [-0.36, 0.275], [-0.32, 0.315], [0.32, 0.315], [0.36, 0.275], [0.36, -0.275], [0.32, -0.315]]"
footprint_padding: 0.01
plugins: ["stvl_layer", "inflation_layer"]
stvl_layer:
plugin: "spatio_temporal_voxel_layer/SpatioTemporalVoxelLayer" # For Foxy and later
enabled: true
voxel_decay: 1.0 # seconds if linear, e^n if exponential
decay_model: 0 # 0=linear, 1=exponential, -1=persistent
voxel_size: 0.04 # meters
track_unknown_space: true # default space is known
max_obstacle_height: 2.0 # meters
unknown_threshold: 15 # voxel height
mark_threshold: 0 # voxel height
update_footprint_enabled: true
combination_method: 1 # 1=max, 0=override
origin_z: 0.0 # meters
publish_voxel_map: false # default off
transform_tolerance: 0.2 # seconds
mapping_mode: false # default off, saves map not for navigation
map_save_duration: 60.0 # default 60s, how often to autosave
observation_sources: scan
scan:
topic: /scan
max_obstacle_height: 2.0
clearing: True
marking: True
data_type: "LaserScan"
raytrace_max_range: 15.0
raytrace_min_range: 0.0
obstacle_max_range: 15.0
obstacle_min_range: 0.0
I thought I had this conversation with someone else years ago and we fixed it but I can't seem to find any reference to it in the issue tracker... how odd.
A couple of things to characterize the problem to make sure we understand what's going on
- If you do this in the global costmap, do you see the same thing?
- Can you show me a figure in this setting of 1) the data 2) the costmap and 3) the voxel grid from STVL on that data? Don't show any data not being inputted into STVL, just things representing it
- Does the current state of
ros2branch do this? It might require some API messing with to get it to work, I think between Foxy and Galactic we did some minor changes
My goal is to figure out 1) if this is aliasing related to local costmap. I don't think so but worth a look. 2) if the grid to costmap conversion is off or if the grid itself is off.
I thought when I had this discussion before we found that openVDB was centering the layer over cells while costmap 2D was using the corners for its coordinates (or vise versa) and we dealt with it by adding a +0.5 cell offset to handle it.
Marking into the grid:
spatio_temporal_voxel_layer/src/spatio_temporal_voxel_grid.cpp
Lines 289 to 296 in 7bf1e12
costmap update population from the grid:
spatio_temporal_voxel_layer/src/spatio_temporal_voxel_layer.cpp
Lines 692 to 694 in 01c0a8d
If you do this in the global costmap, do you see the same thing?
Yes, there is an offset too.
Can you show me a figure in this setting of 1) the data 2) the costmap and 3) the voxel grid from STVL on that data? Don't show any data not being inputted into STVL, just things representing it
purple grids: LETHAL_OBSTACLE value in the costmap
white points: voxel grid published from STVL
color points: scan data
openVDB was centering the layer over cells while costmap 2D was using the corners for its coordinates (or vise versa) and we dealt with it by adding a +0.5 cell offset to handle it.
Could this figure represent it?
Does the current state of ros2 branch do this? It might require some API messing with to get it to work, I think between Foxy and Galactic we did some minor changes
I found a commit(7bf1e12) to fix voxel visualization but not in foxy
got it, thanks for finding that, I'd be happy to merge that into Foxy, can you submit a PR?
I found something related to this issue.
I made a simple application for testing point convertion of openvdb.
{
x = y = 0.0;
std::cout << "===========================================" << std::endl;
std::cout << "x axis:" << std::endl;
for (int i = 0; i < end; i++) {
x = start + (step * i);
openvdb::Vec3d test_mark_grid(grid->worldToIndex(openvdb::Vec3d(x, y, 0.0)));
openvdb::Vec3d pose_world =
grid->indexToWorld(openvdb::Coord(test_mark_grid[0], test_mark_grid[1], test_mark_grid[2]));
std::cout << "points: " << x << " " << y << " " << 0.0 << " pose_world: " << pose_world[0] << " " << pose_world[1]
<< " " << pose_world[2] << std::endl;
}
}A part of the outputs:
points: -0.02 0 0 pose_world: -0.02 0 0
points: -0.019 0 0 pose_world: 0 0 0
points: -0.018 0 0 pose_world: 0 0 0
points: -0.017 0 0 pose_world: 0 0 0
points: -0.016 0 0 pose_world: 0 0 0
points: -0.015 0 0 pose_world: 0 0 0
points: -0.014 0 0 pose_world: 0 0 0
points: -0.013 0 0 pose_world: 0 0 0
points: -0.012 0 0 pose_world: 0 0 0
points: -0.011 0 0 pose_world: 0 0 0
points: -0.01 0 0 pose_world: 0 0 0
points: -0.009 0 0 pose_world: 0 0 0
points: -0.008 0 0 pose_world: 0 0 0
points: -0.007 0 0 pose_world: 0 0 0
points: -0.006 0 0 pose_world: 0 0 0
points: -0.005 0 0 pose_world: 0 0 0
points: -0.004 0 0 pose_world: 0 0 0
points: -0.003 0 0 pose_world: 0 0 0
points: -0.002 0 0 pose_world: 0 0 0
points: -0.001 0 0 pose_world: 0 0 0
points: 0 0 0 pose_world: 0 0 0
points: 0.001 0 0 pose_world: 0 0 0
points: 0.002 0 0 pose_world: 0 0 0
points: 0.003 0 0 pose_world: 0 0 0
points: 0.004 0 0 pose_world: 0 0 0
points: 0.005 0 0 pose_world: 0 0 0
points: 0.006 0 0 pose_world: 0 0 0
points: 0.007 0 0 pose_world: 0 0 0
points: 0.008 0 0 pose_world: 0 0 0
points: 0.009 0 0 pose_world: 0 0 0
points: 0.01 0 0 pose_world: 0 0 0
points: 0.011 0 0 pose_world: 0 0 0
points: 0.012 0 0 pose_world: 0 0 0
points: 0.013 0 0 pose_world: 0 0 0
points: 0.014 0 0 pose_world: 0 0 0
points: 0.015 0 0 pose_world: 0 0 0
points: 0.016 0 0 pose_world: 0 0 0
points: 0.017 0 0 pose_world: 0 0 0
points: 0.018 0 0 pose_world: 0 0 0
points: 0.019 0 0 pose_world: 0 0 0
points: 0.02 0 0 pose_world: 0.02 0 0
It seems that zero has two intervals that are (-0.02, 0.0] and (0.0, 0.02), and the voxel_size is 0.02. It means there is always an offset if the value is positive(or negative). But I only tested on my laptop(Ubuntu 20.04 LTS) and the openvdb installed by apt. Do you know something about that?
That is odd, I have no not noticed this, no. That's probably why we see the offset that we do, we assumed that it was due to definitions of coordinate systems being different (e.g. a point center is either the point where 4 box edges meet vs the center of a given box).
Luckily, everything we do is in positive coordinate space, so there shouldn't be an issue with positive and negative, its just positive.