71 lines
2.7 KiB
Python
71 lines
2.7 KiB
Python
import ast
|
|
import math
|
|
from pathlib import Path
|
|
import threading
|
|
from types import SimpleNamespace
|
|
import unittest
|
|
|
|
|
|
class Twist:
|
|
def __init__(self):
|
|
self.linear = SimpleNamespace(x=0.0)
|
|
self.angular = SimpleNamespace(z=0.0)
|
|
|
|
|
|
class TeleopTest(unittest.TestCase):
|
|
def setUp(self):
|
|
self.now = 10.0
|
|
self.messages = []
|
|
source = ast.parse(Path(__file__).with_name('bridge.py').read_text())
|
|
node = next(item for item in source.body if isinstance(item, ast.ClassDef) and item.name == 'SlamNode')
|
|
methods = [item for item in node.body if isinstance(item, ast.FunctionDef)
|
|
and item.name in ('publish_cmd_vel', '_check_teleop_timeout')]
|
|
scope = {'math': math, 'Twist': Twist, 'time': SimpleNamespace(monotonic=lambda: self.now)}
|
|
exec(compile(ast.Module(body=methods, type_ignores=[]), 'bridge-teleop', 'exec'), scope)
|
|
cls = type('Teleop', (), {name: scope[name] for name in ('publish_cmd_vel', '_check_teleop_timeout')})
|
|
self.node = cls()
|
|
self.node.read_only = False
|
|
self.node._teleop_lock = threading.Lock()
|
|
self.node._teleop_deadline = None
|
|
self.node._cmd_vel_pub = SimpleNamespace(publish=lambda msg: self.messages.append((msg.linear.x, msg.angular.z)))
|
|
|
|
def test_low_speed_and_expiry(self):
|
|
self.node.publish_cmd_vel(5.0, -5.0, True)
|
|
self.assertEqual(self.messages[-1], (0.1, -0.5))
|
|
self.now += 0.6
|
|
self.node._check_teleop_timeout()
|
|
self.assertEqual(self.messages[-1], (0.0, 0.0))
|
|
count = len(self.messages)
|
|
self.node._check_teleop_timeout()
|
|
self.assertEqual(len(self.messages), count)
|
|
|
|
def test_heartbeat_and_release(self):
|
|
self.node.publish_cmd_vel(0.1, 0, True)
|
|
self.now += 0.4
|
|
self.node.publish_cmd_vel(0.1, 0, True)
|
|
self.now += 0.2
|
|
self.node._check_teleop_timeout()
|
|
self.assertEqual(len(self.messages), 2)
|
|
self.node.publish_cmd_vel(0, 0, True)
|
|
self.assertIsNone(self.node._teleop_deadline)
|
|
|
|
def test_simulation_and_idle_navigation_unchanged(self):
|
|
self.node._check_teleop_timeout()
|
|
self.assertEqual(self.messages, [])
|
|
self.node.publish_cmd_vel(0.22, 2.84)
|
|
self.now += 2
|
|
self.node._check_teleop_timeout()
|
|
self.assertEqual(self.messages, [(0.22, 2.84)])
|
|
|
|
def test_invalid_and_readonly_rejected(self):
|
|
with self.assertRaises(RuntimeError):
|
|
self.node.publish_cmd_vel(float('nan'), 0, True)
|
|
self.node.read_only = True
|
|
with self.assertRaises(RuntimeError):
|
|
self.node.publish_cmd_vel(0.1, 0, True)
|
|
self.assertEqual(self.messages, [])
|
|
|
|
|
|
if __name__ == '__main__':
|
|
unittest.main()
|