Table of Contents

Costmaps

Costmaps are way to describe positions in a defined area around a point with respect to certain constrains. For example, there is a costmap which contains every position from which a certain object is visible.

In PyCRAM these costmaps are used to dynamically generate positions for certain criteria like visibility, reachability or occupancy. So if you, for example, want to find a position from where the robot can see a certain object you would generate a costmap for the visibility and one for occupancy. These costmaps can then be merged with one another and result in a costmap which contains every position from which the object is visible and where the robot can stand.

Currently there are three types of costmaps implemented:

Occupancy Costmap

Occupancy costmaps represent all positions that don't have any objects positioned above them. Meaning the robot can stand there without colliding with anything. The occupancy costmap can be generated in two ways, either by using a map provided by the ROS map_server or by directly generating it from the BulletWorld. Additionally user can specify a value by which obstacles should be inflated creating a boundary around obstacles to avoid colliding with them. Below you can see the constructor for the Occupancy Costmap if it is generated from the ROS map_server. 

occupancy = OccupancyCostmap(0.2, True)

In this case is it enough to specify the value by which obstacles should be inflated as well as the boolean which says that this costmap should be generated from the ROS map_server. The value is given in metre meaning 0.2 translates to 20 centimetre. Since the costmap that generated in this way is rather large and usually encompasses the whole environment the robot is operating in there is a method that can cut out a smaller map from the whole occupancy map. This is done to get a local costmap that is centered around the point of interest. This is also necessary since the merging of costmaps requires that all given costmaps have the same size. 

local_ocupancy = occupancy._create_sub_map([1, 0.3, 0], 200)

The paramter given to the method are the new origin around which the local costmap is centered and the size the costmap should have.

In the case where the costmap is generated from the BulletWorld the constructor gets more parameter, specifying the exact dimensions of the costmap. 

occupancy = OccupancyCostmap(0.2, False, 200, 0.02, [1, 0.3, 0])

The given parameter are:

The inflation radius was already explained above while the boolean just specifies if the cosmap should be created from the ROS map_server or the BulletWorld. The size specifies the height and width of the cotsmap, costmaps are usually created as a square. The resolution determines how many metre a pixel in the costmap represents in the real world. The origin is the position, in world coordinate frame, around which the costmap is centered.

You can see an image of the final Occupancy costamp with an inflation radius of 0.2 m below.

Visibility Costmap

Visibility costmaps show the visibility for a specific position in a restricted area. This means every position from which the robot can see the position given as the map origin. This is done by creating depth images from the origin position of the costmap and then checking which positions are occluded by other objects.

The constructor for the visibility costmap gets the following parameter 

visibility = VisibilityCostmap(1.27, 1.6, 200, 0.02, [1, 0.3, 0])

The parameter for the constructor are:

The minimal and maximal height of the camera specify the height of the camera from the ground. This is in case the camera can move either if the torso of the robot moves or the camera itself can move. Since the movement of the camera gives the robot more possibilities to see the position this is taken into account with these parameters. If the camera on the robot can not change its height just set both parameter to the height of the camera.

Size of the costmap is the height and width of the resulting costmap. Resolution is how many metre a pixel in the costmap represents in the real world. Origin is the position the costmap is centered around as well as the position for which the visibility is calculated.

A simple visibility costmap with two objects can be seen below.

Gaussian Costmap

A gaussian costmap is essentially a 2D gauss distribution with its peak at the centre of the the costmap. Gaussian costmaps are, for example, used to approximate reachability of objects. The idea being that to reach an object the robot has to be relatively close to the object to reach it. 

gauss = GaussianCostmap(200, 15, 0.02, [1, 0.3, 0])

The parameter given to the gaussian costmap are:

The mean is the mean value of the gaussian distribution as well as the size of the resulting cotsmap. Sigma is the sigma value of the gaussian distribution which is contained in the costmap. Resolution and origin are the same as for other costmaps.

A plot of the gaussian costmap can be seen below. This is a matplotlib plot of the costmap to better show the distribution.

Visualization of Costmaps

Costmaps can be visualized in the BulletWorld, for this every costmap contains the method “visualize” which can be directly called for the costmap. 

visibility.visualize()

To make the visualization of a costmap disappear from the BulletWorld you can simply call the “close_visualization” method for the same costmap. 

visibility.close_visualization()

The images for the occupancy and visibility costmap show the visualisation in the BulletWorld with this method.

Debugging Visualization

For a more comprehensive visualization of the costmao the is the method “plot_grid” in the costmap.py file. This creates a matplotlib plot of the 2D numpy array which represents the costmap. The “plot_grid” method can be called as follows: 

visibility = VisibilityCostmap(1.27, 1.6, 200, 0.02, [1, 0.3, 0])
plot_grid(visibility.map)

The image for the gaussian costmap shows the plot of matplotlib.

Merging of Costmaps

It is possible to merge different costmaps to create a costmaps that contains positions that adhere to more than one constraint. For example, if you merge a visibility and occupancy costmap you get a costmap that contains positions where the robot can stand and see a specific point.

To be able to merge different costmaps there are a few restrictions that you have to follow. The restrictions are:

These restrictions make it much easier to merge costmaps and also reduce the probability of errors occurring in the resulting costmap. Since all for all costmaps these parameter can be set when creating them, it shouldn't pose a problem to match these parameter for all created costmaps, making them able to be merged.

The one exception for this is the occupancy costmap when generated from the ROS map_server, in this case all parameter are already decided by the map_server. However, for this case there is the “_create_sub_map” method which creates a local costmap from the bigger occupancy costmap. The only thing that can not be fixed with this is the resolution of the occupancy map, which stays the same for the local costmap. But then again, since when creating the other costmaps a resolution can be specified the resolution given for the other costmaps can be adjusted to the one of the occupancy costmap.