Tutorial 02: 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.

%matplotlib inline
import os
import matplotlib.pyplot as plt

from commonroad.common.file_reader import CommonRoadFileReader
from commonroad.visualization.mp_renderer import MPRenderer

# 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
rnd = MPRenderer(figsize=(25, 10))
scenario.draw(rnd)
planning_problem_set.draw(rnd)
rnd.render()

[<matplotlib.collections.PathCollection at 0x7ff4a7b4b350>,
 <matplotlib.collections.PathCollection at 0x7ff4a7aa9610>,
 <matplotlib.collections.PathCollection at 0x7ff46a0aafd0>,
 <matplotlib.collections.PathCollection at 0x7ff4a7d11f10>,
 <matplotlib.collections.PolyCollection at 0x7ff4a7d02c90>,
 Text(16.0, 0.0, ''),
 <matplotlib.collections.EllipseCollection at 0x7ff4c8f8f410>,
 <matplotlib.collections.EllipseCollection at 0x7ff4a7b4bf90>,
 <matplotlib.collections.PatchCollection at 0x7ff4a7a7be10>]

1. 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_dc.collision.collision_detection.pycrcc_collision_dispatch import create_collision_object

rnd = MPRenderer(figsize=(25, 10))
scenario.lanelet_network.draw(rnd)

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

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

rnd.render()

[<matplotlib.collections.PathCollection at 0x7ff45c78bed0>,
 <matplotlib.collections.PathCollection at 0x7ff4a7ab54d0>,
 <matplotlib.collections.PathCollection at 0x7ff4a7bda110>,
 <matplotlib.collections.PathCollection at 0x7ff4a7bdac50>,
 <matplotlib.collections.PolyCollection at 0x7ff45c79a590>,
 <matplotlib.collections.PatchCollection at 0x7ff4a7bc3510>]

2. Creating a Collision Checker from the Scenario

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

from commonroad_dc.collision.collision_detection.pycrcc_collision_dispatch import create_collision_checker

cc = create_collision_checker(scenario)

rnd = MPRenderer(figsize=(25, 10))
scenario.lanelet_network.draw(rnd)
cc.draw(rnd, draw_params={'facecolor': 'blue'})
rnd.render()

[<matplotlib.collections.PathCollection at 0x7ff45c6cabd0>,
 <matplotlib.collections.PathCollection at 0x7ff45c727690>,
 <matplotlib.collections.PathCollection at 0x7ff4a7b59650>,
 <matplotlib.collections.PathCollection at 0x7ff45c79a790>,
 <matplotlib.collections.PolyCollection at 0x7ff45c6caf90>,
 <matplotlib.collections.PatchCollection at 0x7ff45c727510>]

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(**{'time_step':k,'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))

rnd = MPRenderer(figsize=(25, 10))
scenario.lanelet_network.draw(rnd)
cc.draw(rnd, draw_params={'facecolor': 'blue'})
co.draw(rnd, draw_params={'facecolor': 'green'})
rnd.render()

Collision between the trajectory of the ego vehicle and objects in the environment:  False

[<matplotlib.collections.PathCollection at 0x7ff45c60a550>,
 <matplotlib.collections.PathCollection at 0x7ff45c66f990>,
 <matplotlib.collections.PathCollection at 0x7ff45c79a310>,
 <matplotlib.collections.PathCollection at 0x7ff45c66f410>,
 <matplotlib.collections.PolyCollection at 0x7ff45c6d93d0>,
 <matplotlib.collections.PatchCollection at 0x7ff45c655a50>]

3. 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_dc.collision.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,
                                                'triangulation_method':'gpc'})

rnd = MPRenderer(figsize=(25, 10))
scenario.lanelet_network.draw(rnd)
cc.draw(rnd, draw_params={'facecolor': 'blue', 'draw_mesh': True})
rnd.render()

[<matplotlib.collections.PathCollection at 0x7ff45c5bf150>,
 <matplotlib.collections.PathCollection at 0x7ff45c5a7350>,
 <matplotlib.collections.PathCollection at 0x7ff45c6caf10>,
 <matplotlib.collections.PathCollection at 0x7ff45c67d310>,
 <matplotlib.collections.PolyCollection at 0x7ff45c5bf7d0>,
 <matplotlib.collections.PatchCollection at 0x7ff45c591c90>]