def center_point1():
outdir = get_comptests_output_dir()
templates = load_tile_types()
for k, v in templates.items():
if isinstance(v, LaneSegment):
area = RectangularArea((-2, -2), (3, 3))
dest = os.path.join(outdir, k)
N = len(v.control_points)
betas = list(np.linspace(-2, N + 1, 20))
betas.extend(range(N))
betas = sorted(betas)
transforms = []
for timestamp in betas:
beta = timestamp
p = v.center_point(beta)
# print('%s: %s' % (beta, geo.SE2.friendly(p)))
transform = SE2Transform.from_SE2(p)
transforms.append(transform)
c = SampledSequence[SE2Transform](betas, transforms)
v.set_object("a", PlacedObject(), ground_truth=c)
draw_static(v, dest, area=area)
def m1():
outdir = get_comptests_output_dir()
gm = load_map("udem1")
# dw = DuckietownWorld()
# for map_name, tm in gym_maps.items():
# DW.root.set_object(map_name, tm)
root = PlacedObject()
world = PlacedObject()
root.set_object("world", world)
origin = SE2Transform([1, 10], np.deg2rad(10))
world.set_object("tile_map",
gm,
ground_truth=Constant[SE2Transform](origin))
# d = dw.as_json_dict()
# print(json.dumps(d, indent=4))
# print(yaml.safe_dump(d, default_flow_style=False))
#
G = get_meausurements_graph(root)
fn = os.path.join(outdir, "out1.pdf")
plot_measurement_graph(root, G, fn)
def svg2():
outdir = get_comptests_output_dir()
templates = load_tile_types()
for k, v in templates.items():
area = RectangularArea((-1, -1), (1, 1))
dest = os.path.join(outdir, k)
draw_static(v, dest, area=area)
def lane_pose_test1():
outdir = get_comptests_output_dir()
# load one of the maps (see the list using dt-world-draw-maps)
dw = load_map("udem1")
v = 5
# define a SampledSequence with timestamp, command
commands_sequence = SampledSequence.from_iterator(
[
# we set these velocities at 1.0
(1.0, WheelVelocityCommands(0.1 * v, 0.1 * v)),
# at 2.0 we switch and so on
(2.0, WheelVelocityCommands(0.1 * v, 0.4 * v)),
(4.0, WheelVelocityCommands(0.1 * v, 0.4 * v)),
(5.0, WheelVelocityCommands(0.1 * v, 0.2 * v)),
(6.0, WheelVelocityCommands(0.1 * v, 0.1 * v)),
]
)
# we upsample the sequence by 5
commands_sequence = commands_sequence.upsample(5)
## Simulate the dynamics of the vehicle
# start from q0
q0 = geo.SE2_from_translation_angle([1.8, 0.7], 0)
# instantiate the class that represents the dynamics
dynamics = reasonable_duckiebot()
# this function integrates the dynamics
poses_sequence = get_robot_trajectory(dynamics, q0, commands_sequence)
#################
# Visualization and rule evaluation
# Creates an object 'duckiebot'
ego_name = "duckiebot"
db = DB18() # class that gives the appearance
# convert from SE2 to SE2Transform representation
transforms_sequence = poses_sequence.transform_values(SE2Transform.from_SE2)
# puts the object in the world with a certain "ground_truth" constraint
dw.set_object(ego_name, db, ground_truth=transforms_sequence)
# Rule evaluation (do not touch)
interval = SampledSequence.from_iterator(enumerate(commands_sequence.timestamps))
evaluated = evaluate_rules(
poses_sequence=transforms_sequence,
interval=interval,
world=dw,
ego_name=ego_name,
)
timeseries = make_timeseries(evaluated)
# Drawing
area = RectangularArea((0, 0), (3, 3))
draw_static(dw, outdir, area=area, timeseries=timeseries)
def maps():
outdir = get_comptests_output_dir()
map_names = list_maps()
print(map_names)
for map_name in map_names:
duckietown_map = load_map(map_name)
out = os.path.join(outdir, map_name)
draw_map(out, duckietown_map)
def test_pose_sampling_1():
m = dw.load_map("4way")
along_lane = 0.2
only_straight = True
for i in range(45):
q = sample_good_starting_pose(m,
only_straight=only_straight,
along_lane=along_lane)
ground_truth = dw.SE2Transform.from_SE2(q)
m.set_object(f"sampled-{i}", dw.DB18(), ground_truth=ground_truth)
outdir = get_comptests_output_dir()
draw_static(m, outdir)
def svg1():
outdir = get_comptests_output_dir()
control_points = [
SE2Transform.from_SE2(geo.SE2_from_translation_angle([0, 0], 0)),
SE2Transform.from_SE2(geo.SE2_from_translation_angle([1, 0], 0)),
SE2Transform.from_SE2(geo.SE2_from_translation_angle([2, -1], np.deg2rad(-90))),
]
width = 0.3
ls = LaneSegment(width, control_points)
area = RectangularArea((-1, -2), (3, 2))
draw_static(ls, outdir, area=area)
def lane_pose_test1():
outdir = get_comptests_output_dir()
dm = load_map('udem1')
print(get_object_tree(dm, attributes=True))
res = get_skeleton_graph(dm)
# area = RectangularArea((0, 0), (3, 3))
draw_static(res.root2, outdir + '/root2')
# draw_static(dm, outdir + '/orig')
print(get_object_tree(res.root2, attributes=True))
def lane_pose_segment1():
outdir = get_comptests_output_dir()
dm = load_map("udem1")
_ = get_object_tree(dm, attributes=True)
res = get_skeleton_graph(dm)
# area = RectangularArea((0, 0), (3, 3))
draw_static(res.root2, outdir + "/root2")
# draw_static(dm, outdir + '/orig')
_ = get_object_tree(res.root2, attributes=True)
def assert_seq(s: Union[str, Language], seq: List[Event], expect: Sequence[type], final: type):
if isinstance(s, str):
s = s.replace('\n', ' ').strip()
while ' ' in s:
s = s.replace(' ', ' ')
l = parse_language(s)
else:
l = s
s2 = language_to_str(l)
print(s)
print(s2)
l2 = parse_language(s2)
assert l == l2, (s, s2)
pc = LanguageChecker(l)
logger.info(f'Active start: {pc.get_active_states_names()}')
dn = get_comptests_output_dir()
fn = os.path.join(dn, 'language.dot')
make_sure_dir_exists(fn)
write_dot(pc.g, fn)
logger.info(f'Written to {fn}')
# all except last
for i, (e, r) in enumerate(zip(seq, expect)):
logger.info(f'Active before: {pc.get_active_states_names()}')
logger.info(f'Event {e}')
res = pc.push(e)
logger.info(f'Active after: {pc.get_active_states_names()}')
if not isinstance(res, r):
msg = f'Input {i} ({e}) response was {type(res).__name__} instead of {r.__name__}'
msg += f'\n entire sequence: {seq}'
msg += f'\n language: {l}'
msg += f'\n language string: {s2}'
raise Exception(msg)
res = pc.finish()
if not isinstance(res, final):
msg = f'finish response was {type(res).__name__} instead of {final.__name__}'
msg += f'\n entire sequence: {seq}'
msg += f'\n language: {l}'
msg += f'\n language string: {s2}'
raise Exception(msg)
def wb1():
outdir = get_comptests_output_dir()
root = PlacedObject()
tile_map = create_map(H=3, W=3)
world = PlacedObject()
root.set_object('world', world)
placement = Constant[SE2Transform](SE2Transform.identity())
world.set_object('map1', tile_map, ground_truth=placement)
ego = PlacedObject()
world_coordinates = Constant[SE2Transform](SE2Transform([0, 0], 0))
world.set_object('ego', ego, ground_truth=world_coordinates)
d = root.as_json_dict()
# print(json.dumps(DW.root.as_json_dict(), indent=4))
# print(yaml.safe_dump(d, default_flow_style=False))
# print('------')
r1 = Serializable.from_json_dict(d)
# print('read: %s' % r1)
d1 = r1.as_json_dict()
def tag_positions():
map_yaml_data = yaml.load(map_yaml, Loader=yaml.Loader)
m = construct_map(map_yaml_data)
print(get_object_tree(m, attributes=True))
outdir = get_comptests_output_dir()
draw_static(m, outdir)
def test_pwm1():
parameters = get_DB18_nominal(delay=0)
# initial configuration
init_pose = np.array([0, 0.8])
init_vel = np.array([0, 0])
q0 = geo.SE2_from_R2(init_pose)
v0 = geo.se2_from_linear_angular(init_vel, 0)
tries = {
"straight_50": (PWMCommands(+0.5, 0.5)),
"straight_max": (PWMCommands(+1.0, +1.0)),
"straight_over_max": (PWMCommands(+1.5, +1.5)),
"pure_left": (PWMCommands(motor_left=-0.5, motor_right=+0.5)),
"pure_right": (PWMCommands(motor_left=+0.5, motor_right=-0.5)),
"slight-forward-left": (PWMCommands(motor_left=0, motor_right=0.25)),
"faster-forward-right": (PWMCommands(motor_left=0.5, motor_right=0)),
# 'slight-right': (PWMCommands(-0.1, 0.1)),
}
dt = 0.03
t_max = 10
map_data_yaml = """
tiles:
- [floor/W,floor/W, floor/W, floor/W, floor/W]
- [straight/W , straight/W , straight/W, straight/W, straight/W]
- [floor/W,floor/W, floor/W, floor/W, floor/W]
tile_size: 0.61
"""
map_data = yaml.load(map_data_yaml)
root = construct_map(map_data)
timeseries = {}
for id_try, commands in tries.items():
seq = integrate_dynamics(parameters, q0, v0, dt, t_max, commands)
ground_truth = seq.transform_values(
lambda t: SE2Transform.from_SE2(t[0]))
poses = seq.transform_values(lambda t: t[0])
velocities = get_velocities_from_sequence(poses)
linear = velocities.transform_values(linear_from_se2)
angular = velocities.transform_values(angular_from_se2)
# print(linear.values)
# print(angular.values)
root.set_object(id_try, DB18(), ground_truth=ground_truth)
sequences = {}
sequences["motor_left"] = seq.transform_values(
lambda _: commands.motor_left)
sequences["motor_right"] = seq.transform_values(
lambda _: commands.motor_right)
plots = TimeseriesPlot(f"{id_try} - PWM commands", "pwm_commands",
sequences)
timeseries[f"{id_try} - commands"] = plots
sequences = {}
sequences["linear_velocity"] = linear
sequences["angular_velocity"] = angular
plots = TimeseriesPlot(f"{id_try} - Velocities", "velocities",
sequences)
timeseries[f"{id_try} - velocities"] = plots
outdir = os.path.join(get_comptests_output_dir(), "together")
draw_static(root, outdir, timeseries=timeseries)
def check_car_dynamics_correct(klass, CarCommands, CarParams):
"""
:param klass: the implementation
:return:
"""
# load one of the maps (see the list using dt-world-draw-maps)
outdir = get_comptests_output_dir()
dw = load_map('udem1')
v = 5
# define a SampledSequence with timestamp, command
commands_sequence = SampledSequence.from_iterator([
# we set these velocities at 1.0
(1.0, CarCommands(0.1 * v, 0.1 * v)),
# at 2.0 we switch and so on
(2.0, CarCommands(0.1 * v, 0.4 * v)),
(4.0, CarCommands(0.1 * v, 0.4 * v)),
(5.0, CarCommands(0.1 * v, 0.2 * v)),
(6.0, CarCommands(0.1 * v, 0.1 * v)),
])
# we upsample the sequence by 5
commands_sequence = commands_sequence.upsample(5)
## Simulate the dynamics of the vehicle
# start from q0 and v0
q0 = geo.SE2_from_translation_angle([1.8, 0.7], 0)
v0 = geo.se2.zero()
c0 = q0, v0
# instantiate the class that represents the dynamics
dynamics = CarParams(10.0)
# this function integrates the dynamics
poses_sequence = get_robot_trajectory(dynamics, q0, commands_sequence)
#################
# Visualization and rule evaluation
# Creates an object 'duckiebot'
ego_name = 'duckiebot'
db = DB18() # class that gives the appearance
# convert from SE2 to SE2Transform representation
transforms_sequence = poses_sequence.transform_values(
SE2Transform.from_SE2)
# puts the object in the world with a certain "ground_truth" constraint
dw.set_object(ego_name, db, ground_truth=transforms_sequence)
# Rule evaluation (do not touch)
interval = SampledSequence.from_iterator(
enumerate(commands_sequence.timestamps))
evaluated = evaluate_rules(poses_sequence=transforms_sequence,
interval=interval,
world=dw,
ego_name=ego_name)
timeseries = make_timeseries(evaluated)
# Drawing
area = RectangularArea((0, 0), (3, 3))
draw_static(dw, outdir, area=area, timeseries=timeseries)
expected = {
1.0:
geo.SE2_from_translation_angle([1.8, 0.7], 0.0),
1.6:
geo.SE2_from_translation_angle([2.09998657, 0.70245831],
0.016389074695313716),
2.0:
geo.SE2_from_translation_angle([2.29993783, 0.70682836],
0.027315124492189525),
2.4:
geo.SE2_from_translation_angle([2.49991893, 0.70792121],
-0.016385672773040847),
2.8:
geo.SE2_from_translation_angle([2.69975684, 0.70027647],
-0.060086470038271216),
3.2:
geo.SE2_from_translation_angle([2.89906999, 0.68390873],
-0.10378726730350164),
3.6:
geo.SE2_from_translation_angle([3.09747779, 0.65884924],
-0.14748806456873198),
4.0:
geo.SE2_from_translation_angle([3.2946014, 0.62514586],
-0.19118886183396236),
4.6:
geo.SE2_from_translation_angle([3.58705642, 0.55852875],
-0.25674005773180786),
5.0:
geo.SE2_from_translation_angle([3.77932992, 0.50353298],
-0.3004408549970382),
6.0:
geo.SE2_from_translation_angle([4.2622088, 0.37429798],
-0.22257046876429318),
}
for t, expected_pose in expected.items():
assert np.allclose(poses_sequence.at(t=t), expected_pose), t
print('All tests passed successfully!')
