Merge pull request #2 from robotika/feature/turn180deg

Logging annotations and turn 180deg

logger.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()

myr2017.py

@@ -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__':

robot.py

@@ -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)

test_logger.py

@@ -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)

test_robot.py

@@ -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