Rosanswers logo

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"/>


transform_tolerance: 0.5,
robot_base_frame: base_link,

width: 200,
height: 200,
origin_x: -150,
origin_y: -50,

resolution: 0.2,
- [-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,

    - {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}

Originally posted by teonik on ROS Answers with karma: 48 on 2018-02-07

Post score: 0


1 Answer 1


Rosanswers logo

I guess we figured out the mechanics, and, to my opinion, it's not very intuitive.

After voxel marking occurs, the mark_threshold parameter counts occupied voxels in each single vertical stack, and marks the corresponding 2d cell as occupied if the total number of occupied voxels exceeds the parameter.

For our case, this changes almost nothing, since many obstacles are only marked by a single horizontal scanning plane of our lidar. Manipulating the observation_persistence parameter does not change things much; apparently, it only affects marking of single voxels.

I think the functionality I asked for in the question would actually be a valuable addition to costmap_2d. When working with dense clouds (which happens more and more often) this could be a basis of effective dynamic filtering and proofing of obstacles.

We will have to use another way of handling obstacles, possibly octomap.

Originally posted by teonik with karma: 48 on 2018-02-09

This answer was ACCEPTED on the original site

Post score: 1

Original comments

Comment by David Lu on 2018-02-20:
The other option you could pursue is to write your own custom layer and use it as a plugin.


Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.