Setup

In [1]:
# disabling contracts for speed
import contracts
contracts.disable_all()
In [2]:
import duckietown_world as dw
INFO:dt-world:duckietown-world 1.0.15
In [3]:
# reducing the verbosity to critical
dw.logger.setLevel(50)

Utility function to draw in IPython:

In [4]:
from duckietown_world.svg_drawing.ipython_utils import ipython_draw_svg, ipython_draw_html

Pose interpolation code

In [5]:
import geometry as geo
import numpy as np
In [6]:
def interpolate(q0, q1, alpha):
    v = geo.SE2.algebra_from_group(geo.SE2.multiply(geo.SE2.inverse(q0), q1))
    vi = v * alpha
    q = np.dot(q0, geo.SE2.group_from_algebra(vi))
    return q

PlacedObject example

All objects in the map are instances of PlacedObject.

To create a new object, subclass PlacedObject and implement the drawing method draw_svg and the extent_points method.

The SVG drawing is done using the svgwrite library.

In [7]:
class Person(dw.PlacedObject):

    def __init__(self, radius, *args, **kwargs):
        self.radius = radius
        dw.PlacedObject.__init__(self, *args, **kwargs)
        
    def draw_svg(self, drawing, g):
        # drawing is done using the library svgwrite
        c = drawing.circle(center=(0, 0), r=self.radius, fill='pink')
        g.add(c)
        # draws x,y axes
        dw.draw_axes(drawing, g)

    def extent_points(self):
        # set of points describing the boundary 
        L = self.radius
        return [(-L, -L), (+L, +L)]

Animation example

Create the interpolated poses:

In [8]:
q0 = geo.SE2_from_translation_angle([0, 0], 0)
q1 = geo.SE2_from_translation_angle([2, -2], np.deg2rad(-90))

# create a sequence of poses 
n = 10
seqs = []
steps = np.linspace(0, 1, num=n)
for alpha in steps:
    q = interpolate(q0, q1, alpha)
    seqs.append(q)

Create a root PlacedObject:

In [9]:
root = dw.PlacedObject()

Create a SampledSequence of the pose:

In [10]:
timestamps = range(len(seqs)) # [0, 1, 2, ...]

# SE2Transform is the wrapper for SE2 used by Duckietown World 
transforms = [dw.SE2Transform.from_SE2(_) for _ in seqs]
seq_me = dw.SampledSequence(timestamps, transforms)

print(seq_me.timestamps)

print(seq_me.values[0])
[0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0]
SE2Transform([0.0, 0.0],0.0)

Add the object me to the root, saying it's a Person, and with the sequence above given as ground truth for the pose.

In [11]:
root.set_object("me", Person(0.1), ground_truth=seq_me)

Finally, draw the animation:

In [12]:
area = dw.RectangularArea((-1, -3), (3, 1))

ipython_draw_html(root, area=area);

Let's now get the lane object:

In [13]:
from duckietown_world.world_duckietown.tile_template import load_tile_types
In [14]:
template = load_tile_types()['curve_left']
from copy import deepcopy
lane_segment = deepcopy(template['curve/lane1'])

We can use the function lane_segment.lane_pose_from_SE2Transform to get the lane pose information (relative heading, etc.), including the projection to the midlane.

In [15]:
center_points = []

for timestamp, pose_object in seq_me:
    lane_pose = lane_segment.lane_pose_from_SE2Transform(pose_object)
    print(lane_pose.center_point)
    center_points.append(lane_pose.center_point)

sequence = dw.SampledSequence(seq_me.timestamps, center_points)
SE2Transform([-0.30000004477965014, -0.2999999798538675],-0.785398022119)
SE2Transform([-0.2535450059194215, -0.3632753862930096],-1.06522139796)
SE2Transform([-0.23237773765423692, -0.41395325931605537],-1.26067311111)
SE2Transform([-0.2228051546175041, -0.4567845254826781],-1.41677098033)
SE2Transform([-0.21998273997725254, -0.4951323784596772],-1.55350079696)
SE2Transform([-0.21999999880790716, -0.7143834631692813],-1.57079632679)
SE2Transform([-0.21999999880790722, -0.9999074253871845],-1.57079632679)
SE2Transform([-0.21999999880790727, -1.3161948252544462],-1.57079632679)
SE2Transform([-0.21999999880790735, -1.6519741583338639],-1.57079632679)
SE2Transform([-0.21999999880790744, -2.000500609632275],-1.57079632679)
In [16]:
# we now add a marker for projection in the center point

lane_segment.set_object("projection2", dw.PlacedObject(), ground_truth=sequence)
lane_segment.set_object("me", Person(0.2), ground_truth=seq_me)
In [17]:
ipython_draw_html(lane_segment);