pycram.costmaps

pycram.costmaps#

Attributes#

cmap

Classes#

`Rectangle`	A rectangle that is described by a lower and upper x and y value.
`Costmap`	The base class of all Costmaps which implements the visualization of costmaps
`OccupancyCostmap`	The occupancy Costmap represents a map of the environment where obstacles or
`VisibilityCostmap`	A costmap that represents the visibility of a specific point for every position around
`GaussianCostmap`	Gaussian Costmaps are 2D gaussian distributions around the origin with the given mean and sigma
`SemanticCostmap`	Semantic Costmaps represent a 2D distribution over a link of an Object. An example of this would be a Costmap for a
`AlgebraicSemanticCostmap`	Class for a semantic costmap that is based on an algebraic set-description of the valid area.

Functions#

plot_grid(→ None)

An auxiliary method only used for debugging, it will plot a 2D numpy array using MatplotLib.

Module Contents#

class pycram.costmaps.Rectangle#

A rectangle that is described by a lower and upper x and y value.

x_lower: float#

x_upper: float#

y_lower: float#

y_upper: float#

translate(x: float, y: float)#: Translate the rectangle by x and y

scale(x_factor: float, y_factor: float)#: Scale the rectangle by x_factor and y_factor

class pycram.costmaps.Costmap(resolution: float, height: int, width: int, origin: pycram.datastructures.pose.PoseStamped, map: numpy.ndarray, world: typing_extensions.Optional[pycram.datastructures.world.World] = None)#

The base class of all Costmaps which implements the visualization of costmaps in the World.

world#

resolution: float#

size: int#

height: int#

width: int#

origin: pycram.datastructures.pose.PoseStamped#

map: numpy.ndarray#

vis_ids: typing_extensions.List[int] = []#

visualize() → None#: Visualizes a costmap in the BulletWorld, the visualisation works by subdividing the costmap in rectangles which are then visualized as pybullet visual shapes.

_chunks(lst: typing_extensions.List, n: int) → typing_extensions.List#

Yield successive n-sized chunks from lst.

Parameters:

lst – The list from which chunks should be yielded
n – Size of the chunks

Returns:

A list of size n from lst

close_visualization() → None#: Removes the visualization from the World.

_find_consectuive_line(start: typing_extensions.Tuple[int, int], map: numpy.ndarray) → int#

Finds the number of consecutive entries in the costmap which are greater than zero.

Parameters:

start – The indices in the costmap from which the consecutive line should be found.
map – The costmap in which the line should be found.

Returns:

The length of the consecutive line of entries greater than zero.

_find_max_box_height(start: typing_extensions.Tuple[int, int], length: int, map: numpy.ndarray) → int#

Finds the maximal height for a rectangle with a given width in a costmap. The method traverses one row at a time and checks if all entries for the given width are greater than zero. If an entry is less or equal than zero the height is returned.

Parameters:

start – The indices in the costmap from which the method should start.
length – The given width for the rectangle
map – The costmap in which should be searched.

Returns:

The height of the rectangle.

merge(other_cm: Costmap) → Costmap#

Merges the values of two costmaps and returns a new costmap that has for every cell the merged values of both inputs. To merge two costmaps they need to fulfill 3 constrains:

They need to have the same size
They need to have the same x and y coordinates in the origin
They need to have the same resolution

If any of these constrains is not fulfilled a ValueError will be raised.

Parameters:: other_cm – The other costmap with which this costmap should be merged.
Returns:: A new costmap that contains the merged values

__add__(other: Costmap) → Costmap#

Overloading of the “+” operator for merging of Costmaps. Furthermore, checks if ‘other’ is actual a Costmap and raises a ValueError if this is not the case. Please check merge() for further information of merging.

Parameters:: other – Another Costmap
Returns:: A new Costmap that contains the merged values from this Costmap and the other Costmap

partitioning_rectangles() → typing_extensions.List[Rectangle]#

Partition the map attached to this costmap into rectangles. The rectangles are axis aligned, exhaustive and disjoint sets.

Returns:: A list containing the partitioning rectangles

class pycram.costmaps.OccupancyCostmap(distance_to_obstacle: float, from_ros: typing_extensions.Optional[bool] = False, size: typing_extensions.Optional[int] = 100, resolution: typing_extensions.Optional[float] = 0.02, origin: typing_extensions.Optional[pycram.datastructures.pose.PoseStamped] = None, world: typing_extensions.Optional[pycram.datastructures.world.World] = None)#

Bases: Costmap

The occupancy Costmap represents a map of the environment where obstacles or positions which are inaccessible for a robot have a value of -1.

world#

_calculate_diff_origin(height: int, width: int) → pycram.datastructures.pose.PoseStamped#

Calculates the difference between the origin of the costmap as stated by the meta-data and the actual middle of the costmap which is used by PyCRAM to visualize the costmap. The origin as stated by the meta-data refers to the position of the global coordinate frame with the bottom left corner as reference.

Parameters:

height – The height of the costmap
width – The width of the costmap

Returns:

The difference between the actual origin and center of the costmap

static _get_map() → numpy.ndarray#

Receives the map array from the map_server converts it and into a numpy array.

Returns:: The costmap as a numpy array.

static _get_map_metadata() → nav_msgs.msg.MapMetaData#

Receives the meta-data about the costmap from the map_server and returns it. The meta-data contains things like, height, width, origin and resolution.

Returns:: The meta-data for the costmap array.

_convert_map(map: numpy.ndarray) → numpy.ndarray#

Converts the Occupancy Map received from ROS to be more consistent with how PyCRAM handles its costmap. Every possible cell for a robot to stand is set to one while anything else is set to zero. Additionally, this method also takes into account the distance_to_obstacle parameter and sets cell values that are too close to an obstacle to 0.

Parameters:: map – The map that should be converted. Represented as 2d numpy array
Returns:: The converted map. Represented as 2d numpy array.

create_sub_map(sub_origin: pycram.datastructures.pose.PoseStamped, size: int) → Costmap#

Creates a smaller map from the overall occupancy map, the new map is centered around the point specified by “sub_origin” and has the size “size”. The resolution of the costmap stays the same for the sub costmap.

Parameters:

sub_origin – The point in global coordinate frame, around which the sub costmap should be centered.
size – The size the sub costmap should have.

Returns:

The sub costmap, represented as 2d numpy array.

_create_from_world(size: int, resolution: float) → numpy.ndarray#

Creates an Occupancy Costmap for the specified World. This map marks every position as valid that has no object above it. After creating the costmap the distance to obstacle parameter is applied.

Parameters:

size – The size of this costmap. The size specifies the length of one side of the costmap. The costmap is created as a square.
resolution – The resolution of this costmap. This determines how much meter a pixel in the costmap represents.

class pycram.costmaps.VisibilityCostmap(min_height: float, max_height: float, size: typing_extensions.Optional[int] = 100, resolution: typing_extensions.Optional[float] = 0.02, origin: typing_extensions.Optional[pycram.datastructures.pose.PoseStamped] = None, world: typing_extensions.Optional[pycram.datastructures.world.World] = None, target_object: typing_extensions.Optional[pycram.world_concepts.world_object.Object] = None, robot: typing_extensions.Optional[pycram.world_concepts.world_object.Object] = None)#

Bases: Costmap

A costmap that represents the visibility of a specific point for every position around this point. For a detailed explanation on how the creation of the costmap works please look here: PhD Thesis (page 173)

world#

map#

size = 100#

resolution = 0.02#

max_height: float#

min_height: float#

origin: pycram.datastructures.pose.PoseStamped#

property target_object: typing_extensions.Optional[pycram.world_concepts.world_object.Object]#

property robot: typing_extensions.Optional[pycram.world_concepts.world_object.Object]#

move_target_and_robot_far_away()#

return_target_and_robot_to_their_original_position()#

_create_images() → typing_extensions.List[numpy.ndarray]#

Creates four depth images in every direction around the point for which the costmap should be created. The depth images are converted to metre, meaning that every entry in the depth images represents the distance to the next object in metre.

Returns:: A list of four depth images, the images are represented as 2D arrays.

_depth_buffer_to_meter(buffer: numpy.ndarray) → numpy.ndarray#

Converts the depth images generated by the World to represent each position in metre.

Returns:: The depth image in metre

_generate_map()#: This method generates the resulting density map by using the algorithm explained in Lorenz Mösenlechners PhD Thesis (page 178) The resulting map is then saved to self.map

class pycram.costmaps.GaussianCostmap(mean: int, sigma: float, resolution: typing_extensions.Optional[float] = 0.02, origin: typing_extensions.Optional[pycram.datastructures.pose.PoseStamped] = None)#

Bases: Costmap

Gaussian Costmaps are 2D gaussian distributions around the origin with the given mean and sigma

gau: numpy.ndarray#

map: numpy.ndarray#

size: float#

origin: pycram.datastructures.pose.PoseStamped#

_gaussian_window(mean: int, std: float) → numpy.ndarray#: This method creates a window of values with a gaussian distribution of size “mean” and standart deviation “std”. Code from Scipy

class pycram.costmaps.SemanticCostmap(obj: pycram.world_concepts.world_object.Object, link_name: str, resolution: float = 0.02, world: typing_extensions.Optional[pycram.datastructures.world.World] = None)#

Bases: Costmap

Semantic Costmaps represent a 2D distribution over a link of an Object. An example of this would be a Costmap for a table surface.

world: pycram.datastructures.world.World#

object: pycram.world_concepts.world_object.Object#

link: pycram.description.Link#

resolution: float = 0.02#

origin: pycram.datastructures.pose.PoseStamped#

height: int = 0#

width: int = 0#

map: numpy.ndarray = []#

get_edges_map(margin_in_meters: float, horizontal_only: bool = False) → Costmap#

Return a Costmap with only the edges of the original Costmap marked as possible positions.

Parameters:

margin_in_meters – The edge thickness in meters that should be marked as possible positions.
horizontal_only – If True only the horizontal edges will be marked as possible positions.

Returns:

The modified Costmap.

generate_map() → None#: Generates the semantic costmap according to the provided parameters. To do this the axis aligned bounding box (AABB) for the link name will be used. Height and width of the final Costmap will be the x and y sizes of the AABB.

get_aabb_for_link() → pycram.datastructures.dataclasses.AxisAlignedBoundingBox#

Returns:: The original untransformed (doesn’t take the current pose of the link into consideration, since only

the size is important here not the pose) axis aligned bounding box (AABB) of the link provided when creating: this costmap.

class pycram.costmaps.AlgebraicSemanticCostmap(object, urdf_link_name, world=None, number_of_samples=1000)#

Bases: SemanticCostmap

Class for a semantic costmap that is based on an algebraic set-description of the valid area.

x: random_events.variable.Continuous#: The variable for height.

y: random_events.variable.Continuous#: The variable for width.

original_valid_area: typing_extensions.Optional[random_events.product_algebra.SimpleEvent]#: The original rectangle of the valid area.

valid_area: typing_extensions.Optional[random_events.product_algebra.Event]#: A description of the valid positions as set.

number_of_samples: int#: The number of samples to generate for the iter.

check_valid_area_exists()#

left(margin=0.0) → random_events.product_algebra.Event#: Create an event left of the origins Y-Coordinate. :param margin: The margin of the events left bound. :return: The left event.

right(margin=0.0) → random_events.product_algebra.Event#: Create an event right of the origins Y-Coordinate. :param margin: The margin of the events right bound. :return: The right event.

top(margin=0.0) → random_events.product_algebra.Event#: Create an event above the origins X-Coordinate. :param margin: The margin of the events upper bound. :return: The top event.

bottom(margin=0.0) → random_events.product_algebra.Event#: Create an event below the origins X-Coordinate. :param margin: The margin of the events lower bound. :return: The bottom event.

inner(margin=0.2)#

border(margin=0.2)#

generate_map() → None#: Generates the semantic costmap according to the provided parameters. To do this the axis aligned bounding box (AABB) for the link name will be used. Height and width of the final Costmap will be the x and y sizes of the AABB.

as_distribution() → probabilistic_model.probabilistic_circuit.nx.probabilistic_circuit.ProbabilisticCircuit#

sample_to_pose(sample: numpy.ndarray) → pycram.datastructures.pose.PoseStamped#

Convert a sample from the costmap to a pose.

Parameters:: sample – The sample to convert
Returns:: The pose corresponding to the sample

__iter__() → typing_extensions.Iterator[pycram.datastructures.pose.PoseStamped]#

pycram.costmaps.cmap#

pycram.costmaps.plot_grid(data: numpy.ndarray) → None#: An auxiliary method only used for debugging, it will plot a 2D numpy array using MatplotLib.