Overview

The following code examples are available as Jupyter Notebooks in tutorials/collision_checker.ipynb and tutorials/commonroad_io_interface.ipynb.

Python Wrapper pycrcc

The collision checker provides you the functionality to check if basic geometric shapes and groups of shapes collide. Currently several basic shapes are available: axis-aligned rectangles (pycrcc.RectAABB), oriented rectangles (pycrcc.RectOBB), triangles (pycrcc.Triangle), circles (pycrcc.Circle), and polygons (pycrcc.Polygon). The most basic intersection test can be performed between these primitive shapes.

Creating Basic Geometric Shapes

We start with creating a set of basic shapes. We therefore need to import the Python wrapper ‘pycrcc’.

import matplotlib.pyplot as plt
import pycrcc
from commonroad_cc.visualization.draw_dispatch import draw_object

# Axis-aligned rectangle with width/2, height/2, x-position , y-position
aabb = pycrcc.RectAABB(2.0, 3.0, 3.0, 2.0)

# Oriented rectangle with width/2, height/2, orientation, x-position , y-position
obb = pycrcc.RectOBB(1.0, 2.0, 0.3, 8.0, 10.0)

# Circle with radius, x-position , y-position
circ = pycrcc.Circle(2.5, 6.0, 7.0)

# Triangle with vertices (x1, y1), (x2, y2), and (x3, y3)
tri = pycrcc.Triangle(0.0, 0.0, 4.0, 0.0, 2.0, 2.0)

plt.figure(figsize=(10, 10))
draw_object(aabb, draw_params={'collision': {'facecolor': 'green'}})
draw_object(obb, draw_params={'collision': {'facecolor': 'red'}})
draw_object(circ, draw_params={'collision': {'facecolor': 'yellow'}})
draw_object(tri, draw_params={'collision': {'facecolor': 'blue'}})
plt.autoscale()
plt.axis('equal')
plt.show()

In order to create a polygon, we need to define several components: the vertices of the outer boundary, the vertices of holes, and a triangle mesh which is used for collision checks. We can therefore use Python Triangle, which is a python wrapper around Jonathan Richard Shewchuk’s Triangle library.

import triangle

# define the vertices of the outer boundary, we assume that we have no holes
vertices = [[2.0, 0.0], [3.0, 0.0], [3.5, 1.5], [5.0, 2.0], [4.5, 2.5], [1.5, 1.5]]
# triangulate the polygon
number_of_vertices = len(vertices)
segments = list(zip(range(0, number_of_vertices-1), range(1, number_of_vertices)))
segments.append((0, number_of_vertices-1))
triangles = triangle.triangulate({'vertices': vertices, 'segments': segments}, opts='pqS2.4')
# convert all triangles to pycrcc.Triangle
mesh = list()
for t in triangles['triangles']:
    v0 = triangles['vertices'][t[0]]
    v1 = triangles['vertices'][t[1]]
    v2 = triangles['vertices'][t[2]]
    mesh.append(pycrcc.Triangle(v0[0], v0[1],
                                v1[0], v1[1],
                                v2[0], v2[1]))
# create the polygon with the vertices of the outer boundary, the holes, and the triangle mesh
polygon = pycrcc.Polygon(vertices, list(), mesh)

# draw the polygon and its triangle mesh
plt.figure(figsize=(10, 10))
plt.subplot(211)
draw_object(aabb, draw_params={'collision': {'facecolor': 'green'}})
draw_object(obb, draw_params={'collision': {'facecolor': 'red'}})
draw_object(circ, draw_params={'collision': {'facecolor': 'yellow'}})
draw_object(tri, draw_params={'collision': {'facecolor': 'blue'}})
draw_object(polygon, draw_params={'collision': {'facecolor': 'orange'}})
plt.autoscale()
plt.axis('equal')

plt.subplot(212)
draw_object(aabb, draw_params={'collision': {'facecolor': 'green'}})
draw_object(obb, draw_params={'collision': {'facecolor': 'red'}})
draw_object(circ, draw_params={'collision': {'facecolor': 'yellow'}})
draw_object(tri, draw_params={'collision': {'facecolor': 'blue'}})
draw_object(mesh, draw_params={'collision': {'facecolor': 'orange'}})
plt.autoscale()
plt.axis('equal')
plt.show()

Performing Collision Checks Between Basic Shapes

Collision queries can be conducted using the function ‘collide’:

print('Collision between OBB and AABB: ', obb.collide(aabb))
print('Collision between AABB and Circle: ', aabb.collide(circ))
print('Collision between Circle and OBB:  ', circ.collide(obb))
print('Collision between Triangle and AABB:  ', tri.collide(aabb))
print('Collision between Polygon and Triangle: ', polygon.collide(tri))
print('Collision between Polygon and Circle: ', polygon.collide(circ))

Creating Groups of Shapes

Several basic shapes can be grouped into one collision object using the class ShapeGroup. Collision checks can be performed similiar to basic shapes.

# create two shape groups
sg_1 = pycrcc.ShapeGroup()
sg_1.add_shape(obb)
sg_1.add_shape(aabb)

sg_2 = pycrcc.ShapeGroup()
sg_2.add_shape(circ)
sg_2.add_shape(tri)

print('Collision between Circle and Shapegroup 1: ', circ.collide(sg_1))
print('Collision between Shapegroup 1 and Shapegroup 2: ', sg_1.collide(sg_2))

plt.figure(figsize=(10, 10))
draw_object(sg_1, draw_params={'collision': {'facecolor': 'green'}})
draw_object(sg_2, draw_params={'collision': {'facecolor': 'red'}})
plt.autoscale()
plt.axis('equal')
plt.show()

Time-variant Obstacles

So far, we have only considered static objects. We can also create time-varying obstacles. Note that the collision checks are only performed at discrete points in time k.

# create a new time-variant collision objects which starts at time step 1
tvo_1 = pycrcc.TimeVariantCollisionObject(1)
# we need to add the shape of the object for each time step
tvo_1.append_obstacle(pycrcc.RectOBB(2, 1, 0.0, 2.0, 5)) # time step 1
tvo_1.append_obstacle(pycrcc.RectOBB(2, 1, 0.0, 2.5, 5)) # time step 2
tvo_1.append_obstacle(pycrcc.RectOBB(2, 1, 0.0, 3, 5))   # time step 3
tvo_1.append_obstacle(pycrcc.RectOBB(2, 1, 0.0, 3.5, 5)) # time step 4
tvo_1.append_obstacle(pycrcc.RectOBB(2, 1, 0.0, 4, 5))   # time step 5
tvo_1.append_obstacle(pycrcc.RectOBB(2, 1, 0.0, 4.5, 5)) # time step 6
tvo_1.append_obstacle(pycrcc.RectOBB(2, 1, 0.0, 5, 5))   # time step 7
tvo_1.append_obstacle(pycrcc.RectOBB(2, 1, 0.0, 5.5, 5)) # time step 8

# create a second time-variant collision objects which starts at time step 4
tvo_2 = pycrcc.TimeVariantCollisionObject(4)
tvo_2.append_obstacle(pycrcc.RectOBB(2, 1, 1.5, 6.0, 0)) # time step 4
tvo_2.append_obstacle(pycrcc.RectOBB(2, 1, 1.5, 6.0, 2)) # time step 5
tvo_2.append_obstacle(pycrcc.RectOBB(2, 1, 1.5, 6.0, 3)) # time step 6
tvo_2.append_obstacle(pycrcc.RectOBB(2, 1, 1.5, 6.0, 4)) # time step 7
tvo_2.append_obstacle(pycrcc.RectOBB(2, 1, 1.5, 6.0, 5)) # time step 8
tvo_2.append_obstacle(pycrcc.RectOBB(2, 1, 1.5, 6.0, 6)) # time step 9
tvo_2.append_obstacle(pycrcc.RectOBB(2, 1, 1.5, 6.0, 7)) # time step 10

# Check if both objects collide
print('Collision between time-varying obstacle tvo_1 and tvo_2: ', tvo_1.collide(tvo_2))

plt.figure(figsize=(10, 10))
draw_object(tvo_1, draw_params={'collision': {'facecolor': 'red'}})
draw_object(tvo_2, draw_params={'collision': {'facecolor': 'green'}})
plt.autoscale()
plt.axis('equal')
plt.show()

Creating a Collision Checker

Several planning algorithms test a large number of candidate trajectories for collisions. These checks must be executed between each trajectory and all obstacles in the environment. The pycrcc.CollisionChecker provides the functionality to manage the set of all obstacles in the environment. After all obstacles are added to pycrcc.CollisionChecker, a collision check query for a trajectory can be called:

# create a new collision checker
cc = pycrcc.CollisionChecker()
# add all obstacles in the environment
cc.add_collision_object(tvo_1)
cc.add_collision_object(sg_1)
cc.add_collision_object(pycrcc.RectOBB(2, 1, 1.5, 6.0, 0))

print('Collision with trajectory tvo_2: ', cc.collide(tvo_2))

plt.figure(figsize=(10, 10))
draw_object(cc, draw_params={'collision': {'facecolor': 'red'}})
draw_object(tvo_2, draw_params={'collision': {'facecolor': 'green'}})
plt.autoscale()
plt.axis('equal')
plt.show()

Sometimes, it might be necessary to get all obstacles within the collision checker at a specific point in time. This can be done with the function ‘time_slice’:

# plot all obstacles at time step 4
plt.figure(figsize=(10, 10))
draw_object(cc.time_slice(4), draw_params={'collision': {'facecolor': 'red'}})
plt.autoscale()
plt.axis('equal')
plt.show()

Intersection Between Segments and Obstacles

We can also create a segment and test it for intersections with objects in the environment.

# Axis-aligned rectangle with width/2, height/2, x-position , y-position
aabb_2 = pycrcc.RectAABB(2,3,12,8)

# create collision checker and add obstacles
cc = pycrcc.CollisionChecker()
cc.add_collision_object(obb)
cc.add_collision_object(aabb)
cc.add_collision_object(aabb_2)
cc.add_collision_object(circ)

# segment with start and end point
start = [0, 0]
end = [9.5, 9.5]

print("Raytrace, with join")
intervals = cc.raytrace(start[0], start[1], end[0], end[1], True)
print(intervals)

print("Raytrace, no join")
intervals = cc.raytrace(start[0], start[1], end[0], end[1], False)
print(intervals)

plt.figure(figsize=(10, 10))
draw_object(cc)
plt.axis('equal')
plt.ylim([-5, 20])
plt.xlim([-5, 20])

# plot start and end point of segment
plt.plot((start[0], end[0]), (start[1], end[1]), '*g', zorder=50)

# plot all intersecting intervals of the segment
for i in intervals:
    plt.plot((i[0], i[2]), (i[1], i[3]), zorder=50)

plt.show()

commonroad-io Interface

The collision checker library provides a convenient interface to commonroad-io. In order to illustrate the functionality, we load a CommonRoad scenario.

import os
import matplotlib.pyplot as plt

from commonroad.common.file_reader import CommonRoadFileReader
from commonroad_cc.visualization.draw_dispatch import draw_object

# load the exemplary CommonRoad scenario using the CommonRoad file reader
scenario, planning_problem_set = CommonRoadFileReader('ZAM_Tutorial-1_2_T-1.xml').open()

# plot the scenario
plt.figure(figsize=(25, 10))
draw_object(scenario)
draw_object(planning_problem_set)
plt.autoscale()
plt.gca().set_aspect('equal')
plt.show()

Converting CommonRoad Objects to Collision Objects

All shapes and obstacles can be converted to collision objects in order to perform intersection tests. Therefore, we need to call the function ‘create_collision_object()’. The supported CommonRoad objects are:

  • commonroad.geometry.shape.ShapeGroup

  • commonroad.geometry.shape.Polygon

  • commonroad.geometry.shape.Circle

  • commonroad.geometry.shape.Rectangle

  • commonroad.scenario.obstacle.StaticObstacle

  • commonroad.scenario.obstacle.DynamicObstacle

  • commonroad.prediction.prediction.SetBasedPrediction

  • commonroad.prediction.prediction.TrajectoryPrediction

Note that the trajectories of dynamic obstacles are not interpolated. So collisions are only considered at discrete points in time.

from commonroad_cc.collision_detection.pycrcc_collision_dispatch import create_collision_object

plt.figure(figsize=(25, 10))

draw_object(scenario.lanelet_network)

# convert each static obstacle in the scenario to a collision object and plot it
for obs in scenario.static_obstacles:
    draw_object(create_collision_object(obs), draw_params={'collision': {'facecolor': 'red'}})

# convert each dynamic obstacle in the scenario to a collision object and plot it
for obs in scenario.dynamic_obstacles:
    draw_object(create_collision_object(obs), draw_params={'collision': {'facecolor': 'blue'}})

plt.autoscale()
plt.axis('equal')
plt.show()

Creating a Collision Checker from the Scenario

A pycrcc.CollisionChecker object can be directly generated from a CommonRoad scenario:

from commonroad_cc.collision_detection.pycrcc_collision_dispatch import create_collision_checker

cc = create_collision_checker(scenario)

plt.figure(figsize=(25, 10))
draw_object(scenario.lanelet_network)
draw_object(cc, draw_params={'collision': {'facecolor': 'blue'}})
plt.autoscale()
plt.axis('equal')
plt.show()

Given the collision checker for the scenario, it can be easily checked if a trajectory of a ego vehicle collides with an object in the environment.

import numpy as np
from commonroad.scenario.trajectory import State, Trajectory
from commonroad.prediction.prediction import TrajectoryPrediction
from commonroad.geometry.shape import Rectangle

# create a trajectory for the ego vehicle starting at time step 0
position = np.array([[2.5, 0.0], [4.5, 0.0], [6.5, 0.0], [8.5, 0.0], [10.5, 0.0], [12.5, 0.0], [14.5, 0.0]])
state_list = list()
for k in range(0, len(position)):
    state_list.append(State(**{'position': position[k], 'orientation': 0.0}))
trajectory = Trajectory(0, state_list)

# create the shape of the ego vehicle
shape = Rectangle(length=4.5, width=2.0)
# create a TrajectoryPrediction object consisting of the trajectory and the shape of the ego vehicle
traj_pred = TrajectoryPrediction(trajectory=trajectory, shape=shape)

# create a collision object using the trajectory prediction of the ego vehicle
co = create_collision_object(traj_pred)

# test the trajectory of the ego vehicle for collisions
print('Collision between the trajectory of the ego vehicle and objects in the environment: ', cc.collide(co))

plt.figure(figsize=(25, 10))
draw_object(scenario.lanelet_network)
draw_object(cc, draw_params={'collision': {'facecolor': 'blue'}})
draw_object(co, draw_params={'collision': {'facecolor': 'green'}})
plt.autoscale()
plt.axis('equal')
plt.show()

Minkowski Sum

The commonroad-io interface offers the possibility to compute the minkowski sum with a circle and an arbitrary commonroad-io shape before adding the shape to the collision checker.

from commonroad_cc.collision_detection.pycrcc_collision_dispatch import create_collision_checker

cc = create_collision_checker(scenario, params={'minkowski_sum_circle': True,
                                                'minkowski_sum_circle_radius': 2.0,
                                                'resolution': 4})

plt.figure(figsize=(25, 10))
draw_object(scenario.lanelet_network)
draw_object(cc, draw_params={'collision': {'facecolor': 'blue'}})
plt.autoscale()
plt.axis('equal')
plt.show()