Ask Your Question

costmap_2d marking threshold

asked 2018-02-07 12:17:09 -0500

teonik gravatar image

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}
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted

answered 2018-02-09 07:27:15 -0500

teonik gravatar image

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.

edit flag offensive delete link more



The other option you could pursue is to write your own custom layer and use it as a plugin.

David Lu gravatar image David Lu  ( 2018-02-20 15:28:23 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower


Asked: 2018-02-07 12:17:09 -0500

Seen: 638 times

Last updated: Feb 09 '18