costmap_2d marking threshold
I'm running costmap_2d with PointCloud2 feed from a 3D lidar as the only source. Lidar points are preliminary filtered, the ground is mostly segmented out.
It works fine with both ObstacleLayer and VoxelLayer, marking and clearing. The problem is: I want to set the marking threshold, so that a single lidar point does not immediately mark the cell as occupied. Ideally, I want the lidar observations to have some short but finite lifetime, and the cells to be marked only if they accumulate, say, 5 points at a single moment of time. And here the troubles begin.
1) as I understood, the mark_threshold parameter only works for voxel layer. Is that right? If yes, why? it makes the same perfect sense for 2d occupancy grid.
2) for the voxel layer I managed to employ the mark_threshold, but setting it to anything greater than 0.5 makes the costmap not marking anything, save for a few cells.
There is no clear and concise explanation of how this thresholding works for filling in the occupancy grid, and the documentation even contradicts itself on some occasions. Workable configs provided by people in questions puzzle me (like, mark_threshold:9 ?).
Could anyone please explain how do I achieve desired behaviour? The YAML config and launch file are below.
Xubuntu 16.04, ROS Kinetic.
in the launchfile:
<node name="costmap_node" pkg="costmap_2d" type="costmap_2d_node" output="screen" clear_params="true">
<rosparam file="$(find lidar_proc)/params/costmap/costmap_common_voxel_params.yaml" command="load" ns="costmap" />
<remap from="/cloud_in" to="/lidar/flt_out_cloud"/>
</node>
costmap_common_voxel_params.yaml:
transform_tolerance: 0.5,
robot_base_frame: base_link,
width: 200,
height: 200,
origin_x: -150,
origin_y: -50,
resolution: 0.2,
footprint:
- [-11.2, -3.2]
- [-11.2, 3.2]
- [0.2, 3.2]
- [0.2, -3.2],
update_frequency: 10.0,
static_map: false,
publish_frequency: 10.0,
max_obstacle_height: 3.0,
min_obstacle_height: -2,
obstacle_range: 70.0,
raytrace_range: 70.0,
always_send_full_costmap: true,
plugins:
- {name: obstacles, type: "costmap_2d::VoxelLayer"}
obstacles: {
z_resolution: 0.2,
z_voxels: 15,
origin_z: -2.0,
publish_voxel_map: true,
mark_threshold: 0.5,
enabled: true,
max_obstacle_height: 3.0,
min_obstacle_height: -2,
obstacle_range: 70.0,
raytrace_range: 70.0,
inflation_radius: 0.0,
combination_method: 1,
observation_sources: lidar_obstacles,
lidar_obstacles: {
sensor_frame: /Sensor,
data_type: PointCloud2,
topic: /cloud_in,
expected_update_rate: 5.0,
observation_persistence: 0.0,
max_obstacle_height: 3.0,
min_obstacle_height: -2,
marking: true,
clearing: false,
raytrace_range: 70.0,
obstacle_range: 70.0}
}