From e704e31beaf79af4d874098591338d48cfaa23af Mon Sep 17 00:00:00 2001 From: Zbynek Winkler Date: Sat, 22 Feb 2020 12:09:19 -0800 Subject: [PATCH 1/2] subt.main: add TimeoutMonitor --- subt/main.py | 2 ++ subt/monitors.py | 35 +++++++++++++++++++++++++++++ subt/test_monitors.py | 52 +++++++++++++++++++++++++++++++++++++++++++ 3 files changed, 89 insertions(+) create mode 100644 subt/monitors.py create mode 100644 subt/test_monitors.py diff --git a/subt/main.py b/subt/main.py index 43f65d2ce..a773e7375 100644 --- a/subt/main.py +++ b/subt/main.py @@ -10,6 +10,7 @@ from datetime import timedelta from collections import defaultdict from io import StringIO +from random import Random import numpy as np @@ -146,6 +147,7 @@ class SubTChallenge: def __init__(self, config, bus): self.bus = bus bus.register("desired_speed", "pose2d", "artf_xyz", "pose3d", "stdout", "request_origin") + self.random = Random(0) self.start_pose = None self.traveled_dist = 0.0 self.time = None diff --git a/subt/monitors.py b/subt/monitors.py new file mode 100644 index 000000000..6278cb26f --- /dev/null +++ b/subt/monitors.py @@ -0,0 +1,35 @@ + +from datetime import timedelta + +class TimeoutReached(Exception): + pass + + +class Timeout: + def __init__(self, robot, timeout): + self.uuid = robot.random.getrandbits(64) + self.robot = robot + self.timeout = timeout + self.was_timeout = False + + def update(self): + self.timeout -= self.robot.time - self.time + self.time = self.robot.time + if self.timeout < timedelta(): + raise TimeoutReached(self.uuid) + + def __enter__(self): + self.time = self.robot.time + self.handle = self.robot.register(self.update) + + def __exit__(self, exc_type, exc_val, exc_tb): + self.robot.unregister(self.handle) + if exc_val is not None and isinstance(exc_val, TimeoutReached): + if exc_val.args[0] == self.uuid: + self.was_timeout = True + return True # don't reraise + + def __bool__(self): + return self.was_timeout + + diff --git a/subt/test_monitors.py b/subt/test_monitors.py new file mode 100644 index 000000000..977b9cb8a --- /dev/null +++ b/subt/test_monitors.py @@ -0,0 +1,52 @@ +import unittest +from datetime import timedelta +from random import Random +from subt import monitors + +class Robot: + def __init__(self): + self.callbacks = [] + self.time = timedelta() + self.random = Random(0) + + def update(self, dt=timedelta(milliseconds=101)): + self.time += dt + for f in self.callbacks: + f() + + def register(self, callback): + self.callbacks.append(callback) + return callback + + def unregister(self, handle): + assert handle in self.callbacks + self.callbacks.remove(handle) + + +class TimeoutTest(unittest.TestCase): + def test_timeout(self): + robot = Robot() + timeout = monitors.Timeout(robot, timedelta(seconds=1)) + with timeout: + while True: + robot.update() + self.assertGreater(robot.time, timedelta(seconds=1)) + self.assertTrue(timeout) + + def test_stacked_timeout(self): + robot = Robot() + timeout_outside = monitors.Timeout(robot, timedelta(seconds=1)) + timeout_inside = monitors.Timeout(robot, timedelta(seconds=0.5)) + with timeout_outside: + with timeout_inside: + while True: + robot.update() + self.assertGreater(robot.time, timedelta(seconds=0.5)) + self.assertLess(robot.time, timedelta(seconds=1)) + self.assertTrue(timeout_inside) + self.assertFalse(timeout_outside) + while True: + robot.update() + self.assertGreater(robot.time, timedelta(seconds=1)) + self.assertTrue(timeout_outside) + From 476bbc67254e2e3e92c9671144b62d705ee07a07 Mon Sep 17 00:00:00 2001 From: Zbynek Winkler Date: Sun, 23 Feb 2020 09:24:32 -0800 Subject: [PATCH 2/2] Add pitch and roll monitor. --- subt/monitors.py | 85 +++++++++++++++++++++++++++++++++++++------ subt/test_monitors.py | 74 +++++++++++++++++++++++++++++++++---- 2 files changed, 139 insertions(+), 20 deletions(-) diff --git a/subt/monitors.py b/subt/monitors.py index 6278cb26f..a93387c61 100644 --- a/subt/monitors.py +++ b/subt/monitors.py @@ -1,35 +1,96 @@ +import logging + +g_logger = logging.getLogger(__name__) -from datetime import timedelta class TimeoutReached(Exception): pass -class Timeout: +class TimeoutMonitor: def __init__(self, robot, timeout): - self.uuid = robot.random.getrandbits(64) self.robot = robot self.timeout = timeout - self.was_timeout = False + self.fired = False def update(self): - self.timeout -= self.robot.time - self.time - self.time = self.robot.time - if self.timeout < timedelta(): - raise TimeoutReached(self.uuid) + if (self.robot.time - self.start_time) > self.timeout: + raise TimeoutReached(self) def __enter__(self): - self.time = self.robot.time + self.start_time = self.robot.time + self.fired = False self.handle = self.robot.register(self.update) def __exit__(self, exc_type, exc_val, exc_tb): self.robot.unregister(self.handle) if exc_val is not None and isinstance(exc_val, TimeoutReached): - if exc_val.args[0] == self.uuid: - self.was_timeout = True + if exc_val.args[0] == self: + self.fired = True + g_logger.info("Timeout {self.timeout} reached.") return True # don't reraise def __bool__(self): - return self.was_timeout + return self.fired + + +class PitchError(Exception): + pass + +class RollError(Exception): + pass + + +class PitchMonitor: + def __init__(self, robot, pitch_limit): + self.robot = robot + self.pitch_limit = pitch_limit + self.fired = False + def update(self): + if self.pitch_limit is not None and self.robot.pitch is not None: + if abs(self.robot.pitch) > self.pitch_limit: + raise PitchError(self) + + def __enter__(self): + self.fired = False + self.handle = self.robot.register(self.update) + + def __exit__(self, exc_type, exc_val, exc_tb): + self.robot.unregister(self.handle) + if exc_val is not None and isinstance(exc_val, PitchError): + if exc_val.args[0] == self: + self.fired = True + g_logger.info("Pitch limit {math.degrees(self.pitch_limit)} reached.") + return True # don't reraise + + def __bool__(self): + return self.fired + + +class RollMonitor: + def __init__(self, robot, roll_limit): + self.robot = robot + self.roll_limit = roll_limit + self.fired = False + + def update(self): + if self.roll_limit is not None and self.robot.roll is not None: + if abs(self.robot.roll) > self.roll_limit: + raise RollError(self) + + def __enter__(self): + self.fired = False + self.handle = self.robot.register(self.update) + + def __exit__(self, exc_type, exc_val, exc_tb): + self.robot.unregister(self.handle) + if exc_val is not None and isinstance(exc_val, RollError): + if exc_val.args[0] == self: + self.fired = True + g_logger.info("Pitch limit {math.degrees(self.roll_limit)} reached.") + return True # don't reraise + + def __bool__(self): + return self.fired diff --git a/subt/test_monitors.py b/subt/test_monitors.py index 977b9cb8a..bdbb8e395 100644 --- a/subt/test_monitors.py +++ b/subt/test_monitors.py @@ -1,16 +1,21 @@ import unittest +import math +from unittest import mock from datetime import timedelta -from random import Random from subt import monitors class Robot: + def __init__(self): self.callbacks = [] self.time = timedelta() - self.random = Random(0) + self.pitch = 0 + self.roll = 0 def update(self, dt=timedelta(milliseconds=101)): self.time += dt + self.pitch += 0.1 + self.roll += 0.15 for f in self.callbacks: f() @@ -24,29 +29,82 @@ def unregister(self, handle): class TimeoutTest(unittest.TestCase): + def test_timeout(self): robot = Robot() - timeout = monitors.Timeout(robot, timedelta(seconds=1)) + timeout = monitors.TimeoutMonitor(robot, timedelta(seconds=1)) with timeout: - while True: + for _ in range(100): robot.update() + else: + self.assertTrue(False) self.assertGreater(robot.time, timedelta(seconds=1)) self.assertTrue(timeout) def test_stacked_timeout(self): robot = Robot() - timeout_outside = monitors.Timeout(robot, timedelta(seconds=1)) - timeout_inside = monitors.Timeout(robot, timedelta(seconds=0.5)) + timeout_outside = monitors.TimeoutMonitor(robot, timedelta(seconds=1)) + timeout_inside = monitors.TimeoutMonitor(robot, timedelta(seconds=0.5)) with timeout_outside: with timeout_inside: - while True: + for _ in range(100): robot.update() self.assertGreater(robot.time, timedelta(seconds=0.5)) self.assertLess(robot.time, timedelta(seconds=1)) self.assertTrue(timeout_inside) self.assertFalse(timeout_outside) - while True: + for _ in range(100): robot.update() self.assertGreater(robot.time, timedelta(seconds=1)) self.assertTrue(timeout_outside) + +class PitchRollTest(unittest.TestCase): + + def test_pitch(self): + robot = Robot() + pitch_limit = monitors.PitchMonitor(robot, math.radians(80)) + with pitch_limit: + for _ in range(100): + robot.update() + else: + self.assertTrue(False) + self.assertTrue(pitch_limit) + + def test_roll(self): + robot = Robot() + roll_limit = monitors.RollMonitor(robot, math.radians(80)) + with roll_limit: + for _ in range(100): + robot.update() + else: + self.assertTrue(False) + self.assertTrue(roll_limit) + + def test_stacked_roll(self): + robot = Robot() + max_roll_limit = monitors.RollMonitor(robot, math.radians(80)) + with max_roll_limit: + mid_roll_limit = monitors.RollMonitor(robot, math.radians(20)) + with mid_roll_limit: + for _ in range(100): + robot.update() + else: + self.assertTrue(False) + self.assertTrue(mid_roll_limit) + self.assertFalse(max_roll_limit) + for _ in range(100): + robot.update() + else: + self.assertTrue(False) + self.assertTrue(mid_roll_limit) + self.assertTrue(max_roll_limit) + + def test_roll_logging(self): + with mock.patch("subt.monitors.g_logger") as logger: + robot = Robot() + roll = monitors.RollMonitor(robot, math.radians(20)) + with roll: + robot.roll = 2 + robot.update() + self.assertTrue(logger.info.called)