# disabling contracts for speed
import contracts
contracts.disable_all()
import duckietown_world as dw
# reducing the verbosity to critical
dw.logger.setLevel(50)
Utility function to draw in IPython:
from duckietown_world.svg_drawing.ipython_utils import ipython_draw_svg, ipython_draw_html
import geometry as geo
import numpy as np
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
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)]
Create the interpolated poses:
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:
root = dw.PlacedObject()
Create a SampledSequence of the pose:
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])
Add the object me
to the root, saying it's a Person, and with the
sequence above given as ground truth for the pose.
root.set_object("me", Person(0.1), ground_truth=seq_me)
Finally, draw the animation:
area = dw.RectangularArea((-1, -3), (3, 1))
ipython_draw_html(root, area=area);
Let's now get the lane object:
from duckietown_world.world_duckietown.tile_template import load_tile_types
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.
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)
# 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)
ipython_draw_html(lane_segment);