====== 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
* visibility Costmap
* Gaussian Costmap
==== 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
* The the costmap shoudl not be created from Ros
* The size
* The resolution
* The origin
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.
{{:doc:pycram:occupancy_costmap.png?400|}}
==== 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:
* Minimal height of the camera
* Maximal height of the camera
* Size of the costmap
* Resolution of the costmap
* Origin of the costmap
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.
{{:doc:pycram:visibility_costmap.png?400|}}
==== 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:
* Mean of gaussian distribution
* Sigma of the gaussian distribution
* Resolution of the costmap
* Origin of the costmap
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.
{{:doc:pycram:gaussian_costmap.png?400|}}
==== 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:
* The costmaps must have the same size
* The costmaps must have the same origin
* The costmaps must have the same resolution
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.