Ask Your Question

How Costmap works

asked 2014-04-02 12:47:09 -0600

BP gravatar image

updated 2016-10-24 09:01:16 -0600

ngrennan gravatar image

I am trying to understand how Costmaps Works. I have Costmap with params:

costmap params

base_scan: {clearing: true, data_type: LaserScan, expected_update_rate: 0.4, marking: true,
max_obstacle_height: 2.4, min_obstacle_height: 0.08, observation_persistence: 5.0}
cost_scaling_factor: 13.0
footprint: '[[-0.375,-0.375],[-0.375,0.375],[0.375,0.375],[0.375,-0.375]]'
footprint_padding: 0.05
global_frame: /odom
height: 6
inflation_radius: 0.5
lethal_cost_threshold: 100
map_topic: map
map_type: voxel
mark_threshold: 0
max_obstacle_height: 2.0
max_obstacle_range: 2.5
observation_sources: base_scan
obstacle_range: 2.5
origin_x: 0.0
origin_y: 0.0
origin_z: 0.0
publish_frequency: 5.0
publish_voxel_map: false
raytrace_range: 3.0
resolution: 0.05
restore_defaults: false
robot_base_frame: /base_link
robot_radius: 0.46
rolling_window: true
static_map: false
track_unknown_space: false
transform_tolerance: 0.3
unknown_cost_value: 0
unknown_threshold: 9
update_frequency: 5.0
width: 6
z_resolution: 0.05
z_voxels: 10

rviz(OcupancyGrid with topic: /test_node/local_costmap/infalted_obstacles: rviz(OcupancyGrid with topic: /test_node/local_costmap/infalted_obstacles

How Costmap really looks like(openCV):

How Costmap really looks like(openCV)

(I assume that black is obstacle and white is free because when I put robot in free space I get white costmap with black square(robot footprint)).

this assumption was wrong
This can be not true as @ahendrix mentioned. The Costmap picture is flipped vertically to OccupancyGrid

and my questions:

  1. Why the costmap is consisted only of 0 or 255 values ( I thought there should be a gradient 0-255)?
I printed values in console and there are values between 0-255 (but not a lot of them - I suppose it depends on inflation radius). But still I don't get it why costmap looks like this and sends correct OccupancyGrid topics.
  1. Why when obstacle spotted whole "beam" on map is 255 (not for instance part of beam from the point on distance that obstacle was spotted and further)?
i was wrong with the values(black is 0-free)
  1. Why the place of robot (footprint?) is spoted as obstacle
i was wrong with the values- footprint is free, but why when i put robot in free space, costmap has only footprint spotted as free?

Thanks in advance

edit retag flag offensive close merge delete


Hey BP, I am trying to get the costmap_2d into any kind of image format: see here It seems like you were able to do this. Could you give me a short explanation? Thanks, Marcus

Marcus gravatar image Marcus  ( 2015-02-25 07:56:41 -0600 )edit

1 Answer

Sort by ยป oldest newest most voted

answered 2014-04-04 07:00:25 -0600

ahendrix gravatar image

updated 2014-04-04 10:42:45 -0600

You have the values backward; In the costmap, 255 is occupied, and 0 is free space. Values in between capture the uncertainty about whether a particular cell is occupied or not.

It looks like your costmap rendering is flipped vertically compared to the display in rviz. I suspect this is due to differences in how the axes are aligned in rviz vs images.

The black areas you see correspond to 0 values in the costmap; these areas are free space. This includes all of the space between your laser and the object it's seeing (minus inflation). This also includes the footprint of the robot, because in order for the planner to operate properly, the robot cannot be in a start state that is colliding with anything.

The white areas that you see in the costmap are either occupied or unknown space. Occupied space is marked explicitly by the laser, and unknown space is space where there are no sensor readings, such as behind the robot, and on the far side of obstacles.

The obstacle layer in the costmap computes free space by using raytracing to mark the space between the sensor and the obstacle as free. If there are no obstacles in the environment, there's nothing to raytrace to, so all of the space around the robot remains unknown.

edit flag offensive delete link more


so why when i put robot in free space costmap looks like I've said? Also- pictures I posted are for the same place (costmap is rotated by -90 deg to the OcupancyGrid) So why there is a hudge distance ( I thought the costmap should look like infaltedObstacles but with bigger range and gradient)

BP gravatar image BP  ( 2014-04-04 08:41:01 -0600 )edit

But I still can't see it on costmap- it should have about 135 degrees of fully free grids) because there is no obstacles and there is measurement from laserscans. and there is only about 45 deg. area of that. And why when I put robot in free space there are obstacles everywhere?

BP gravatar image BP  ( 2014-04-04 10:10:42 -0600 )edit

For reference, 255 are unknown/no information cells

paulbovbel gravatar image paulbovbel  ( 2014-04-04 11:26:46 -0600 )edit

thanks. It's a bit strange for me that area with no obstacles that were found is considered as unknown, but I think I can live with it

BP gravatar image BP  ( 2014-04-04 11:30:31 -0600 )edit

by default, areas are raytraced 'clear' for only for an area inbetween the robot and an obstacle. This leads to the long-hallway problem where if you can't see the end of the room, that entire area remains unknown and you can't navigate through it.

paulbovbel gravatar image paulbovbel  ( 2014-04-04 12:10:36 -0600 )edit

you should check the echo of /scan for areas where there's no obstacle. It should be either inf or maxrange + 1. In hydro, there are parameters which will enable raytracing of free space for those cases. Not sure about prior distributions.

paulbovbel gravatar image paulbovbel  ( 2014-04-04 12:11:58 -0600 )edit

you could also enable unknown_space for navfn global planner, which will let your robot plan paths through unknown space, though this may be unsafe for general use

paulbovbel gravatar image paulbovbel  ( 2014-04-04 12:12:41 -0600 )edit

thank's, that surely will help

BP gravatar image BP  ( 2014-04-04 12:48:20 -0600 )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



Asked: 2014-04-02 12:47:09 -0600

Seen: 2,642 times

Last updated: Apr 04 '14