How Costmap works

Question

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 http://imageshack.com/a/img46/971/wf7w.png

---

How Costmap really looks like(openCV):

How Costmap really looks like(openCV) http://imageshack.com/a/img571/8012/n7mm.png

(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.

2. 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)

3. 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

---

Originally posted by BP on ROS Answers with karma: 176 on 2014-04-02

Post score: 1

---

Original comments

Comment by Marcus on 2015-02-25:
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

Answer 1

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.

---

Originally posted by ahendrix with karma: 47576 on 2014-04-04

This answer was ACCEPTED on the original site

Post score: 1

---

Original comments

Comment by BP on 2014-04-04:
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)

Comment by BP on 2014-04-04:
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?

Comment by paulbovbel on 2014-04-04:
For reference, http://docs.ros.org/hydro/api/costmap_2d/html/cost__values_8h.html

255 are unknown/no information cells

Comment by BP on 2014-04-04:
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

Comment by paulbovbel on 2014-04-04:
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.

Comment by paulbovbel on 2014-04-04:
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.

Comment by paulbovbel on 2014-04-04:
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

Comment by BP on 2014-04-04:
thank's, that surely will help

Comment by BP on 2014-04-05:
I set unknown_threshold to sth like 10000 and there's no more unknown space