This repository has been archived by the owner on Aug 2, 2022. It is now read-only.
-
Notifications
You must be signed in to change notification settings - Fork 2
/
demo.py
102 lines (84 loc) · 2.86 KB
/
demo.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
"""
follow the race loop
usage:
demo.py <track XML file> [<input log file>]
"""
import math
import os
import struct
import sys
from iolog import IOLog, Timeout, IOFromFile
from track import Track
def segment_turn(segment):
"""Return turn angle in degrees (expected car structure input)"""
# for Robocar with following definition
# <section name="Front Axle">
# <attnum name="xpos" min="0.1" max="5" val="1.104"/>
# <section name="Rear Axle">
# <attnum name="xpos" min="-5" max="-0.1" val="-1.469"/>
axis_dist = 1.104 + 1.469
if segment.arc is None:
return 0.0
angle = math.atan2(axis_dist, segment.radius)
if segment.arc < 0:
# right left
angle = -angle
return math.degrees(angle)
def drive(io, track):
port = 4001
io.bind(('', port))
io.settimeout(1.0)
ctr = 0
gas = 0.0
brake = 0.0
turn = 0.0
prev_segment = None
while True:
data = struct.pack('fffiBB', turn, gas, brake, 1, 11, ctr & 0xFF)
io.sendto(data, ('127.0.0.1', 3001))
try:
status = io.recv(1024)
assert len(status) == 794, len(status)
sim_status = struct.unpack_from('B', status, 792)[0]
if sim_status == 5: # simulation stopped
break
absPosX, absPosY, absPosZ = struct.unpack_from('fff', status, 44)
angX, angY, angZ = struct.unpack_from('fff', status, 56)
heading = angZ # in radiands +/- PI
segment, rel_pose = track.nearest_segment((absPosX, absPosY, heading))
if segment is not None:
signed_dist, heading_offset = segment.get_offset(rel_pose)
if signed_dist < 5.0:
gas = 0.2
else:
gas = 0.1
turn = segment_turn(segment)
if heading_offset is not None:
turn -= math.degrees(heading_offset)
dead_band = 0.1
max_dist_turn_deg = 10
if signed_dist < -dead_band:
# turn left
turn += min(max_dist_turn_deg, -dead_band - signed_dist)
elif signed_dist > dead_band:
# turn right
turn += max(-max_dist_turn_deg, dead_band - signed_dist)
if prev_segment != segment:
print(segment, rel_pose)
prev_segment = segment
except Timeout:
pass
ctr += 1
if __name__ == "__main__":
if len(sys.argv) < 2:
print(__doc__)
sys.exit(2)
filename = sys.argv[1]
track = Track.from_xml_file(filename)
if len(sys.argv) == 2:
prefix = os.path.splitext(os.path.basename(filename))[0]
io = IOLog(prefix=prefix)
else:
io = IOFromFile(filename=sys.argv[2])
drive(io, track)
# vim: expandtab sw=4 ts=4