Permalink
Browse files

Merge pull request #2 from robotika/feature/turn180deg

Logging annotations and turn 180deg
  • Loading branch information...
m3d committed Nov 27, 2017
2 parents 99de89a + b370745 commit 302119755bc435eddf89acb1218da7dbcbb78ee0
Showing with 99 additions and 32 deletions.
  1. +1 −0 logger.py
  2. +63 −27 myr2017.py
  3. +22 −2 robot.py
  4. +3 −2 test_logger.py
  5. +10 −1 test_robot.py
@@ -33,6 +33,7 @@ def write(self, stream_id, data):
stream_id, len(data)))
self.f.write(data)
self.f.flush()
return dt
def close(self):
self.f.close()
@@ -5,13 +5,15 @@
import sys
import struct
import itertools
from datetime import timedelta
from logger import LogWriter, LogReader, LogEnd
from robot import Robot
DEFAULT_HOST = '127.0.0.1' # The remote host
DEFAULT_PORT = 5559 # The same port as used by the server
ANNOT_STREAM = 0 # the same as debug/info
INPUT_STREAM = 1
OUTPUT_STREAM = 2
@@ -22,22 +24,23 @@ def __init__(self, soc, log, ignore_ref_output=False):
self.log = log
self.ignore_ref_output = ignore_ref_output
self.buf = b''
self.time = None
def get(self):
if len(self.buf) < 1024:
if self.soc is None:
data = self.log.read(INPUT_STREAM)[2]
self.time, __, data = self.log.read(INPUT_STREAM)
else:
data = self.soc.recv(1024)
self.log.write(INPUT_STREAM, data)
self.time = self.log.write(INPUT_STREAM, data)
self.buf += data
assert len(self.buf) > 7 and self.buf[:6] == b'NAIO01', self.buf
msg_id = self.buf[6]
size = struct.unpack('>I', self.buf[7:7+4])[0]
data = self.buf[11:11+size]
self.buf = self.buf[11+size+4:] # cut CRC32
return msg_id, data
return self.time, msg_id, data
def put(self, cmd):
msg_id, data = cmd
@@ -50,6 +53,10 @@ def put(self, cmd):
self.log.write(OUTPUT_STREAM, naio_msg)
self.soc.sendall(naio_msg)
def annot(self, annotation):
if self.soc is not None:
self.log.write(ANNOT_STREAM, annotation)
def laser2ascii(scan):
"Eduro ASCII art"
@@ -74,15 +81,56 @@ def laser2ascii(scan):
return s, mid-left, right-mid
def move_one_meter(robot):
def move_straight(robot, how_far):
robot.annot(b'TAG:move_one_meter:BEGIN')
odo_start = robot.odometry_left_raw + robot.odometry_right_raw
robot.move_forward()
dist = 0.0
while dist < 1.0:
while dist < how_far:
robot.update()
odo = robot.odometry_left_raw + robot.odometry_right_raw - odo_start
dist = 0.06465 * odo / 4.0
robot.stop()
robot.annot(b'TAG:move_one_meter:END')
def turn_right_90deg(robot):
robot.annot(b'TAG:turn_right_90deg:BEGIN')
robot.turn_right()
gyro_sum = 0
start_time = robot.time
num_updates = 0
while robot.time - start_time < timedelta(minutes=1):
robot.update()
gyro_sum += robot.gyro_raw[2] # time is required!
num_updates += 1
# the updates are 10Hz (based on laser measurements)
angle = (gyro_sum * 0.1) * 30.5/1000.0
# also it looks the rotation (in Simulatoz) is clockwise
if angle > 90.0: # TODO lower threshold for minor corrections
break
robot.stop()
print('gyro_sum', gyro_sum, robot.time - start_time, num_updates)
robot.annot(b'TAG:turn_right_90deg:END')
def navigate_row(robot, verbose):
robot.move_forward()
while True:
robot.update()
max_dist = max(robot.laser)
triplet = laser2ascii(robot.laser)
s, left, right = triplet
if left < right:
robot.move_right()
elif left > right:
robot.move_left()
else:
robot.move_forward()
if verbose:
print('%4d' % max_dist, triplet)
if max_dist == 0:
break
def main(host, port):
@@ -108,7 +156,7 @@ def main(host, port):
with s, LogWriter(note=str(sys.argv)) as log:
print(log.filename)
io = WrapperIO(s, log)
yield Robot(io.get, io.put)
yield Robot(io.get, io.put, io.annot)
print(log.filename)
@@ -118,32 +166,20 @@ def main_replay(filename, force):
with LogReader(filename) as log:
print('REPLAY', log.filename)
io = WrapperIO(None, log, ignore_ref_output=force)
yield Robot(io.get, io.put)
yield Robot(io.get, io.put, io.annot)
print('REPLAY', log.filename)
def play_game(robot, verbose):
robot.move_forward()
while True:
robot.update()
max_dist = max(robot.laser)
triplet = laser2ascii(robot.laser)
s, left, right = triplet
if left < right:
robot.move_right()
elif left > right:
robot.move_left()
else:
robot.move_forward()
if verbose:
print('%4d' % max_dist, triplet)
if max_dist == 0:
break
for i in range(10):
navigate_row(robot, verbose)
move_straight(robot, how_far=1.2)
turn_right_90deg(robot)
move_straight(robot, how_far=0.7)
turn_right_90deg(robot)
move_one_meter(robot)
robot.stop()
robot.update()
robot.stop()
robot.update()
if __name__ == '__main__':
@@ -6,35 +6,45 @@
MOTOR_ID = 0x01
ODOMETRY_ID = 0x06
LASER_ID = 0x07
GYRO_ID = 0x0A
class Robot:
def __init__(self, get, put, term=LASER_ID):
def __init__(self, get, put, annot=None, term=LASER_ID):
"provide input and output methods"
self.get = get
self.put = put
self.term = term
self._annot = annot
self.laser = None
self.odometry_left_raw = 0
self.odometry_right_raw = 0
self.gyro_raw = None
self.motor_pwm = [0, 0]
self.prev_odo = None
self.time = None
def update(self):
while True:
msg_type, data = self.get()
self.time, msg_type, data = self.get()
if msg_type == LASER_ID:
self.update_laser(data)
elif msg_type == ODOMETRY_ID:
self.update_odometry(data)
elif msg_type == GYRO_ID:
self.update_gyro(data)
if msg_type == self.term:
break
self.put((MOTOR_ID, self.get_motor_cmd()))
def annot(self, annotation):
'note, that annotation is expected binary bytes'
self._annot(annotation)
# Motion Commands
def move_forward(self):
self.motor_pwm = [0x70, 0x70]
@@ -45,6 +55,12 @@ def move_left(self):
def move_right(self):
self.motor_pwm = [0x70, 0x40]
def turn_left(self):
self.motor_pwm = [0x100-0x70, 0x70]
def turn_right(self):
self.motor_pwm = [0x70, 0x100-0x70]
def stop(self):
self.motor_pwm = [0, 0]
@@ -61,6 +77,10 @@ def update_odometry(self, data):
self.odometry_left_raw += diff[2] + diff[3]
self.prev_odo = data
def update_gyro(self, data):
assert len(data) == 6, len(data)
# X, Y, Z (gain factor 30.5)
self.gyro_raw = struct.unpack('>hhh', data)
def get_motor_cmd(self):
return bytes(self.motor_pwm)
@@ -18,9 +18,10 @@ def test_context_manager(self):
self.assertTrue(log.filename.startswith('tmpp'))
filename = log.filename
start_time = log.start_time
log.write(10, b'\x01\x02\x02\x04')
t1 = log.write(10, b'\x01\x02\x02\x04')
time.sleep(0.01)
log.write(10, b'\x05\x06\x07\x08')
t2 = log.write(10, b'\x05\x06\x07\x08')
self.assertLess(t1, t2)
with LogReader(filename) as log:
self.assertEqual(start_time, log.start_time)
@@ -8,7 +8,7 @@ class RoboTest(unittest.TestCase):
def test_odometry(self):
q_in = Queue()
q_out = Queue()
robot = Robot(q_in.get, q_out.put, ODOMETRY_ID)
robot = Robot(q_in.get, q_out.put, term=ODOMETRY_ID)
q_in.put((ODOMETRY_ID, b'\x00\x00\x00\x00'))
robot.update()
@@ -24,4 +24,13 @@ def test_odometry(self):
self.assertEqual(robot.odometry_right_raw, 2)
self.assertEqual(q_out.get(), (MOTOR_ID, b'\x00\x00'))
def test_annotations(self):
q_in = Queue()
q_out = Queue()
q_annot = Queue()
robot = Robot(q_in.get, q_out.put, annot=q_annot.put)
robot.annot(b'TAG:turn_right_90deg:BEGIN')
self.assertEqual(q_annot.get_nowait(), b'TAG:turn_right_90deg:BEGIN')
# vim: expandtab sw=4 ts=4

0 comments on commit 3021197

Please sign in to comment.