# How Costmap works

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]]'
global_frame: /odom
height: 6
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
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:

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 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?

edit retag 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

( 2015-02-25 07:56:41 -0500 )edit

Sort by » oldest newest most voted

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.

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)

( 2014-04-04 08:41:01 -0500 )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?

( 2014-04-04 10:10:42 -0500 )edit

For reference, http://docs.ros.org/hydro/api/costmap_2d/html/cost__values_8h.html 255 are unknown/no information cells

( 2014-04-04 11:26:46 -0500 )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

( 2014-04-04 11:30:31 -0500 )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.

( 2014-04-04 12:10:36 -0500 )edit
1

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.

( 2014-04-04 12:11:58 -0500 )edit
1

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

( 2014-04-04 12:12:41 -0500 )edit

thank's, that surely will help

( 2014-04-04 12:48:20 -0500 )edit