Setup

In [1]:
import contracts
contracts.disable_all()
In [2]:
import duckietown_world as dw
from duckietown_world.svg_drawing.ipython_utils import ipython_draw_html
INFO:dt-world:duckietown-world 1.0.15
In [3]:
dw.logger.setLevel(50)

Better visualization of output

In [4]:
%%html
<style>
pre {line-height: 90%}
</style>

Map representation

Let's load a map and see how data is represented inside:

In [5]:
m = dw.load_map('4way')
ipython_draw_html(m);

The map is a DuckietownMap which is an instance of PlacedObject. All spatially situated objects are instances of this class.

In [6]:
type(m).mro() # see all superclasses of the object
Out[6]:
[duckietown_world.world_duckietown.duckietown_map.DuckietownMap,
 duckietown_world.geo.placed_object.PlacedObject,
 duckietown_serialization_ds1.serialization1.Serializable,
 duckietown_serialization_ds1.serialization1.Serializable0,
 object]

Children

The data is arranged in a hierarchy. We can visualize using the function get_object_tree. The hierarchy is more deep, but we clip it to 2 levels:

In [7]:
print(dw.get_object_tree(m, levels=4))
DuckietownMap

├ ob00-trafficlight ┐ TrafficLight
│                   └───────────────
└ tilemap ┐ TileMap
          │ 
          │ ├ tile-0-4 ┐ Tile
          │ │          │
          │ │          │ └ curve_left ┐ PlacedObject
          │ │          │              │
          │ │          │              │ └ curve ┐ PlacedObject
          │ │          │              │         └───────────────
          │ │          │              └───────────────────────────
          │ │          └────────────────────────────────────────────
          │ ├ tile-0-0 ┐ Tile
          │ │          │
          │ │          │ └ curve_left ┐ PlacedObject
          │ │          │              │
          │ │          │              │ └ curve ┐ PlacedObject
          │ │          │              │         └───────────────
          │ │          │              └───────────────────────────
          │ │          └────────────────────────────────────────────
          │ ├ tile-0-1 ┐ Tile
          │ │          │
          │ │          │ └ straight ┐ PlacedObject
          │ │          │            │
          │ │          │            │ ├ lane2 ┐ LaneSegment
          │ │          │            │ │       └──────────────
          │ │          │            │ └ lane1 ┐ LaneSegment
          │ │          │            │         └──────────────
          │ │          │            └──────────────────────────
          │ │          └─────────────────────────────────────────
          │ ├ tile-0-2 ┐ Tile
          │ │          │
          │ │          │ └ 3way_left ┐ PlacedObject
          │ │          │             │
          │ │          │             │ ├ west_go_straight ┐ LaneSegment
          │ │          │             │ │                  └──────────────
          │ │          │             │ ├ east_go_straight ┐ LaneSegment
          │ │          │             │ │                  └──────────────
          │ │          │             │ ├ east_go_right ┐ LaneSegment
          │ │          │             │ │               └──────────────
          │ │          │             │ ├ north_go_left ┐ LaneSegment
          │ │          │             │ │               └──────────────
          │ │          │             │ ├ west_go_left ┐ LaneSegment
          │ │          │             │ │              └──────────────
          │ │          │             │ └ north_go_right ┐ LaneSegment
          │ │          │             │                  └──────────────
          │ │          │             └─────────────────────────────────────
          │ │          └─────────────────────────────────────────────────────
          │ ├ tile-0-3 ┐ Tile
          │ │          │
          │ │          │ └ straight ┐ PlacedObject
          │ │          │            │
          │ │          │            │ ├ lane2 ┐ LaneSegment
          │ │          │            │ │       └──────────────
          │ │          │            │ └ lane1 ┐ LaneSegment
          │ │          │            │         └──────────────
          │ │          │            └──────────────────────────
          │ │          └─────────────────────────────────────────
          │ ├ tile-3-4 ┐ Tile
          │ │          │
          │ │          │ └ straight ┐ PlacedObject
          │ │          │            │
          │ │          │            │ ├ lane2 ┐ LaneSegment
          │ │          │            │ │       └──────────────
          │ │          │            │ └ lane1 ┐ LaneSegment
          │ │          │            │         └──────────────
          │ │          │            └──────────────────────────
          │ │          └─────────────────────────────────────────
          │ ├ tile-3-2 ┐ Tile
          │ │          │
          │ │          │ └ straight ┐ PlacedObject
          │ │          │            │
          │ │          │            │ ├ lane2 ┐ LaneSegment
          │ │          │            │ │       └──────────────
          │ │          │            │ └ lane1 ┐ LaneSegment
          │ │          │            │         └──────────────
          │ │          │            └──────────────────────────
          │ │          └─────────────────────────────────────────
          │ ├ tile-4-0 ┐ Tile
          │ │          │
          │ │          │ └ curve_left ┐ PlacedObject
          │ │          │              │
          │ │          │              │ └ curve ┐ PlacedObject
          │ │          │              │         └───────────────
          │ │          │              └───────────────────────────
          │ │          └────────────────────────────────────────────
          │ ├ tile-4-1 ┐ Tile
          │ │          │
          │ │          │ └ straight ┐ PlacedObject
          │ │          │            │
          │ │          │            │ ├ lane2 ┐ LaneSegment
          │ │          │            │ │       └──────────────
          │ │          │            │ └ lane1 ┐ LaneSegment
          │ │          │            │         └──────────────
          │ │          │            └──────────────────────────
          │ │          └─────────────────────────────────────────
          │ ├ tile-4-2 ┐ Tile
          │ │          │
          │ │          │ └ 3way_left ┐ PlacedObject
          │ │          │             │
          │ │          │             │ ├ west_go_straight ┐ LaneSegment
          │ │          │             │ │                  └──────────────
          │ │          │             │ ├ east_go_straight ┐ LaneSegment
          │ │          │             │ │                  └──────────────
          │ │          │             │ ├ east_go_right ┐ LaneSegment
          │ │          │             │ │               └──────────────
          │ │          │             │ ├ north_go_left ┐ LaneSegment
          │ │          │             │ │               └──────────────
          │ │          │             │ ├ west_go_left ┐ LaneSegment
          │ │          │             │ │              └──────────────
          │ │          │             │ └ north_go_right ┐ LaneSegment
          │ │          │             │                  └──────────────
          │ │          │             └─────────────────────────────────────
          │ │          └─────────────────────────────────────────────────────
          │ ├ tile-4-3 ┐ Tile
          │ │          │
          │ │          │ └ straight ┐ PlacedObject
          │ │          │            │
          │ │          │            │ ├ lane2 ┐ LaneSegment
          │ │          │            │ │       └──────────────
          │ │          │            │ └ lane1 ┐ LaneSegment
          │ │          │            │         └──────────────
          │ │          │            └──────────────────────────
          │ │          └─────────────────────────────────────────
          │ ├ tile-4-4 ┐ Tile
          │ │          │
          │ │          │ └ curve_left ┐ PlacedObject
          │ │          │              │
          │ │          │              │ └ curve ┐ PlacedObject
          │ │          │              │         └───────────────
          │ │          │              └───────────────────────────
          │ │          └────────────────────────────────────────────
          │ ├ tile-2-2 ┐ Tile
          │ │          │
          │ │          │ └ 4way ┐ PlacedObject
          │ │          │        │
          │ │          │        │ ├ a ┐ PlacedObject
          │ │          │        │ │   └───────────────
          │ │          │        │ ├ c ┐ PlacedObject
          │ │          │        │ │   └───────────────
          │ │          │        │ ├ b ┐ PlacedObject
          │ │          │        │ │   └───────────────
          │ │          │        │ └ d ┐ PlacedObject
          │ │          │        │     └───────────────
          │ │          │        └───────────────────────
          │ │          └──────────────────────────────────
          │ ├ tile-2-3 ┐ Tile
          │ │          │
          │ │          │ └ straight ┐ PlacedObject
          │ │          │            │
          │ │          │            │ ├ lane2 ┐ LaneSegment
          │ │          │            │ │       └──────────────
          │ │          │            │ └ lane1 ┐ LaneSegment
          │ │          │            │         └──────────────
          │ │          │            └──────────────────────────
          │ │          └─────────────────────────────────────────
          │ ├ tile-2-0 ┐ Tile
          │ │          │
          │ │          │ └ 3way_left ┐ PlacedObject
          │ │          │             │
          │ │          │             │ ├ west_go_straight ┐ LaneSegment
          │ │          │             │ │                  └──────────────
          │ │          │             │ ├ east_go_straight ┐ LaneSegment
          │ │          │             │ │                  └──────────────
          │ │          │             │ ├ east_go_right ┐ LaneSegment
          │ │          │             │ │               └──────────────
          │ │          │             │ ├ north_go_left ┐ LaneSegment
          │ │          │             │ │               └──────────────
          │ │          │             │ ├ west_go_left ┐ LaneSegment
          │ │          │             │ │              └──────────────
          │ │          │             │ └ north_go_right ┐ LaneSegment
          │ │          │             │                  └──────────────
          │ │          │             └─────────────────────────────────────
          │ │          └─────────────────────────────────────────────────────
          │ ├ tile-2-1 ┐ Tile
          │ │          │
          │ │          │ └ straight ┐ PlacedObject
          │ │          │            │
          │ │          │            │ ├ lane2 ┐ LaneSegment
          │ │          │            │ │       └──────────────
          │ │          │            │ └ lane1 ┐ LaneSegment
          │ │          │            │         └──────────────
          │ │          │            └──────────────────────────
          │ │          └─────────────────────────────────────────
          │ ├ tile-2-4 ┐ Tile
          │ │          │
          │ │          │ └ 3way_left ┐ PlacedObject
          │ │          │             │
          │ │          │             │ ├ west_go_straight ┐ LaneSegment
          │ │          │             │ │                  └──────────────
          │ │          │             │ ├ east_go_straight ┐ LaneSegment
          │ │          │             │ │                  └──────────────
          │ │          │             │ ├ east_go_right ┐ LaneSegment
          │ │          │             │ │               └──────────────
          │ │          │             │ ├ north_go_left ┐ LaneSegment
          │ │          │             │ │               └──────────────
          │ │          │             │ ├ west_go_left ┐ LaneSegment
          │ │          │             │ │              └──────────────
          │ │          │             │ └ north_go_right ┐ LaneSegment
          │ │          │             │                  └──────────────
          │ │          │             └─────────────────────────────────────
          │ │          └─────────────────────────────────────────────────────
          │ ├ tile-1-1 ┐ Tile
          │ │          └───────
          │ ├ tile-1-0 ┐ Tile
          │ │          │
          │ │          │ └ straight ┐ PlacedObject
          │ │          │            │
          │ │          │            │ ├ lane2 ┐ LaneSegment
          │ │          │            │ │       └──────────────
          │ │          │            │ └ lane1 ┐ LaneSegment
          │ │          │            │         └──────────────
          │ │          │            └──────────────────────────
          │ │          └─────────────────────────────────────────
          │ ├ tile-1-3 ┐ Tile
          │ │          └───────
          │ ├ tile-1-2 ┐ Tile
          │ │          │
          │ │          │ └ straight ┐ PlacedObject
          │ │          │            │
          │ │          │            │ ├ lane2 ┐ LaneSegment
          │ │          │            │ │       └──────────────
          │ │          │            │ └ lane1 ┐ LaneSegment
          │ │          │            │         └──────────────
          │ │          │            └──────────────────────────
          │ │          └─────────────────────────────────────────
          │ ├ tile-3-3 ┐ Tile
          │ │          └───────
          │ ├ tile-1-4 ┐ Tile
          │ │          │
          │ │          │ └ straight ┐ PlacedObject
          │ │          │            │
          │ │          │            │ ├ lane2 ┐ LaneSegment
          │ │          │            │ │       └──────────────
          │ │          │            │ └ lane1 ┐ LaneSegment
          │ │          │            │         └──────────────
          │ │          │            └──────────────────────────
          │ │          └─────────────────────────────────────────
          │ ├ tile-3-1 ┐ Tile
          │ │          └───────
          │ └ tile-3-0 ┐ Tile
          │            │
          │            │ └ straight ┐ PlacedObject
          │            │            │
          │            │            │ ├ lane2 ┐ LaneSegment
          │            │            │ │       └──────────────
          │            │            │ └ lane1 ┐ LaneSegment
          │            │            │         └──────────────
          │            │            └──────────────────────────
          │            └─────────────────────────────────────────
          └────────────────────────────────────────────────────────────────────

The children are available in the children variable:

In [8]:
m.children
Out[8]:
{'ob00-trafficlight': TrafficLight(status={u'~Sequence': {'always': 'off'}, u'~Constant': {'always': 'off'}}),
 'tilemap': TileMap(H=5,W=5)}
In [9]:
m.children['tilemap'].children
Out[9]:
{'tile-0-0': Tile(kind=curve_left,drivable=True),
 'tile-0-1': Tile(kind=straight,drivable=True),
 'tile-0-2': Tile(kind=3way_left,drivable=True),
 'tile-0-3': Tile(kind=straight,drivable=True),
 'tile-0-4': Tile(kind=curve_left,drivable=True),
 'tile-1-0': Tile(kind=straight,drivable=True),
 'tile-1-1': Tile(kind=asphalt,drivable=False),
 'tile-1-2': Tile(kind=straight,drivable=True),
 'tile-1-3': Tile(kind=asphalt,drivable=False),
 'tile-1-4': Tile(kind=straight,drivable=True),
 'tile-2-0': Tile(kind=3way_left,drivable=True),
 'tile-2-1': Tile(kind=straight,drivable=True),
 'tile-2-2': Tile(kind=4way,drivable=True),
 'tile-2-3': Tile(kind=straight,drivable=True),
 'tile-2-4': Tile(kind=3way_left,drivable=True),
 'tile-3-0': Tile(kind=straight,drivable=True),
 'tile-3-1': Tile(kind=asphalt,drivable=False),
 'tile-3-2': Tile(kind=straight,drivable=True),
 'tile-3-3': Tile(kind=asphalt,drivable=False),
 'tile-3-4': Tile(kind=straight,drivable=True),
 'tile-4-0': Tile(kind=curve_left,drivable=True),
 'tile-4-1': Tile(kind=straight,drivable=True),
 'tile-4-2': Tile(kind=3way_left,drivable=True),
 'tile-4-3': Tile(kind=straight,drivable=True),
 'tile-4-4': Tile(kind=curve_left,drivable=True)}

You can use the notation below to get a child in a compact way:

In [10]:
lane_segment = m['tilemap/tile-0-1/straight/lane1']

In this case we can see how a Tile has a child curve_left with a child curve with two children lane1 and lane2:

In [11]:
tile = m.children['tilemap'].children['tile-0-0']
print(dw.get_object_tree(tile, attributes=False, levels=10))
Tile

└ curve_left ┐ PlacedObject
             │ 
             │ └ curve ┐ PlacedObject
             │         │
             │         │ ├ lane2 ┐ LaneSegment
             │         │ │       └──────────────
             │         │ └ lane1 ┐ LaneSegment
             │         │         └──────────────
             │         └──────────────────────────
             └──────────────────────────────────────
In [12]:
print(dw.get_object_tree(tile, attributes=True, levels=10))
Tile

 kind: curve_left
 drivable: true
 

└ curve_left ┐ PlacedObject
             │ 
             │ └ curve ┐ PlacedObject
             │         │
             │         │ ├ lane2 ┐ LaneSegment
             │         │ │       │
             │         │ │       │  width: 0.376
             │         │ │       │  control_points:
             │         │ │       │  -   ~SE2Transform:
             │         │ │       │          p:
             │         │ │       │          - -0.5
             │         │ │       │          - -0.22
             │         │ │       │  -   ~SE2Transform:
             │         │ │       │          theta_deg: 45
             │         │ │       │  -   ~SE2Transform:
             │         │ │       │          theta_deg: 90
             │         │ │       │          p:
             │         │ │       │          - 0.22
             │         │ │       │          - 0.5
             │         │ │       │
             │         │ │       └─────────────────────────
             │         │ └ lane1 ┐ LaneSegment
             │         │         │
             │         │         │  width: 0.376
             │         │         │  control_points:
             │         │         │  -   ~SE2Transform:
             │         │         │          p:
             │         │         │          - -0.5
             │         │         │          - -0.22
             │         │         │  -   ~SE2Transform:
             │         │         │          theta_deg: -45
             │         │         │          p:
             │         │         │          - -0.3
             │         │         │          - -0.3
             │         │         │  -   ~SE2Transform:
             │         │         │          theta_deg: -90
             │         │         │          p:
             │         │         │          - -0.22
             │         │         │          - -0.5
             │         │         │
             │         │         └──────────────────────────
             │         └──────────────────────────────────────
             └──────────────────────────────────────────────────
In [13]:
lane1 = tile['curve_left/curve/lane1']
lane2 = tile['curve_left/curve/lane2']
ipython_draw_html(lane1);
ipython_draw_html(lane2);
In [14]:
curve = tile['curve_left/curve']
print(dw.get_object_tree(curve, attributes=True, spatial_relations=True, levels=10))
PlacedObject

├ lane2 ┐ LaneSegment
│       │ 
│       │  width: 0.376
│       │  control_points:
│       │  -   ~SE2Transform:
│       │          p:
│       │          - -0.5
│       │          - -0.22
│       │  -   ~SE2Transform:
│       │          theta_deg: 45
│       │  -   ~SE2Transform:
│       │          theta_deg: 90
│       │          p:
│       │          - 0.22
│       │          - 0.5
│       │ 
│       └─────────────────────────
└ lane1 ┐ LaneSegment
        │ 
        │  width: 0.376
        │  control_points:
        │  -   ~SE2Transform:
        │          p:
        │          - -0.5
        │          - -0.22
        │  -   ~SE2Transform:
        │          theta_deg: -45
        │          p:
        │          - -0.3
        │          - -0.3
        │  -   ~SE2Transform:
        │          theta_deg: -90
        │          p:
        │          - -0.22
        │          - -0.5
        │ 
        └──────────────────────────

- from "." to "lane2"  SE2Transform([0.0, 0.0],1.57079632679) 
- from "." to "lane1"  SE2Transform([0.0, 0.0],0.0) 
In [15]:
ipython_draw_html(curve);
In [16]:
lane = tile['curve_left/curve/lane2']._copy()

ipython_draw_html(lane);
In [17]:
lane.width
Out[17]:
0.376
In [18]:
lane.control_points
Out[18]:
[SE2Transform([-0.5, -0.22],0.0),
 SE2Transform([0.0, 0.0],0.785398163397),
 SE2Transform([0.22, 0.5],1.57079632679)]

Parametrization of lanes

The lane is parametrized with a parameter beta that interpolates among the control points.

Here we create an animation of the center point for different betas.

In [19]:
import numpy as np
npoints = len(lane.control_points)
betas = list(np.linspace(-1, npoints + 1, 20))
print(betas)
[-1.0, -0.73684210526315796, -0.47368421052631582, -0.21052631578947367, 0.052631578947368363, 0.3157894736842104, 0.57894736842105265, 0.84210526315789469, 1.1052631578947367, 1.3684210526315788, 1.6315789473684208, 1.8947368421052628, 2.1578947368421053, 2.4210526315789473, 2.6842105263157894, 2.9473684210526314, 3.2105263157894735, 3.4736842105263159, 3.7368421052631575, 4.0]
In [20]:
transforms = []
for beta in betas:
    # call the function `center_point` to get the center point (in SE(2))
    p = lane.center_point(beta)
    transform = dw.SE2Transform.from_SE2(p)
    transforms.append(transform)

ground_truth = dw.SampledSequence(betas, transforms)
lane.set_object('traveling-point', dw.PlacedObject(), ground_truth=ground_truth)
ipython_draw_html(lane);

Scrub the timeline to see the animation.

In [21]:
import geometry as geo
q = geo.SE2_from_translation_angle([+0.05, -0.05], np.deg2rad(20))
In [22]:
lane.set_object('db18-4', dw.DB18(), ground_truth=dw.SE2Transform.from_SE2(q))
lane_pose = lane.lane_pose_from_SE2(q)

lane.set_object('marker3', dw.PlacedObject(), ground_truth=lane_pose.center_point)
In [23]:
lane.children
Out[23]:
{'db18-4': DB18(),
 'marker3': PlacedObject(),
 'traveling-point': PlacedObject()}
In [24]:
lane.spatial_relations
Out[24]:
{0: GroundTruth(() -> ('traveling-point',)  SampledSequence(timestamps=[-1.0, -0.736842105263158, -0.4736842105263158, -0.21052631578947367, 0.05263157894736836, 0.3157894736842104, 0.5789473684210527, 0.8421052631578947, 1.1052631578947367, 1.3684210526315788, 1.6315789473684208, 1.8947368421052628, 2.1578947368421053, 2.4210526315789473, 2.6842105263157894, 2.9473684210526314, 3.2105263157894735, 3.473684210526316, 3.7368421052631575, 4.0],values=[SE2Transform([-0.6, -0.2199999988079071],0.0), SE2Transform([-0.5736842105263158, -0.2199999988079071],0.0), SE2Transform([-0.5473684210526316, -0.2199999988079071],0.0), SE2Transform([-0.5210526315789473, -0.2199999988079071],0.0), SE2Transform([-0.470525719458915, -0.21874729127170855],0.0413367465923), SE2Transform([-0.32530923670416145, -0.19434484508076877],0.248020479554), SE2Transform([-0.18819117546714098, -0.1406611089369041],0.454704212515), SE2Transform([-0.0650081458829751, -0.059981201486350694],0.661387945476), SE2Transform([0.04088916109922749, 0.042518225576477324],0.868071656582), SE2Transform([0.12659068460015985, 0.16226192997345126],1.07475538954), SE2Transform([0.18589496327194147, 0.29704437840168835],1.2814391225), SE2Transform([0.21627763270313685, 0.44112837914891023],1.48812285547), SE2Transform([0.2199999988079071, 0.5157894736842106],1.57079632679), SE2Transform([0.2199999988079071, 0.5421052631578948],1.57079632679), SE2Transform([0.21999999880790708, 0.5684210526315789],1.57079632679), SE2Transform([0.21999999880790708, 0.5947368421052631],1.57079632679), SE2Transform([0.21999999880790708, 0.6210526315789473],1.57079632679), SE2Transform([0.21999999880790708, 0.6473684210526316],1.57079632679), SE2Transform([0.21999999880790708, 0.6736842105263157],1.57079632679), SE2Transform([0.21999999880790705, 0.7],1.57079632679)])),
 1: GroundTruth(() -> ('db18-4',)  SE2Transform([0.05, -0.05],0.349065850399)),
 2: GroundTruth(() -> ('marker3',)  SE2Transform([-1.528914340132026e-07, -1.5970960803857182e-07],0.785397875476))}
In [25]:
ipython_draw_html(lane);
In [26]:
assert lane_pose.correct_direction
In [27]:
assert lane_pose.inside
In [28]:
assert lane