Fix localization for marsupial robots

osgar/lib/quaternion.py

@@ -31,6 +31,7 @@ def identity():
     return [0, 0, 0, 1]
 
 def rotate_vector(vector, quaternion):
+    # https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation
     qvector = vector + [0]
     con = conjugate(quaternion)
     part1 = multiply(quaternion, qvector)

subt/offseter.py

@@ -1,5 +1,9 @@
 from threading import Thread
 
+from osgar.bus import BusShutdownException
+from osgar.lib import quaternion
+
+
 class Offseter:
 
     def __init__(self, config, bus):
@@ -8,14 +12,15 @@ def __init__(self, config, bus):
         #self.thread.name = bus.name
         self.bus = bus
         self.is_alive = self.thread.is_alive
+        self.init_quat = None  # unknown rotation
 
     def start(self):
         self.thread.start()
 
     def join(self, timeout=None):
         self.thread.join(timeout=timeout)
 
-    def run(self):
+    def _run(self):
         origin = None
         xyz = None
         while None in (origin, xyz):
@@ -24,6 +29,8 @@ def run(self):
                 if len(data) == 8:
                     robot_name, x, y, z, qa, qb, qc, qd = data
                     origin = [x, y, z]
+                    if self.init_quat is None:
+                        self.init_quat = (qa, qb, qc, qd)
             elif channel == "pose3d":
                 xyz, orientation = data
             else:
@@ -34,17 +41,21 @@ def run(self):
         while True:
             dt, channel, data = self.bus.listen()
             if channel == "origin":
-                if len(data) == 8:
-                    robot_name, x, y, z, qa, qb, qc, qd = data
-                    origin = [x, y, z]
-                    offset = [o - p for o,p in zip(origin, xyz)]
+                pass  # do not update offset estimate
             elif channel == "pose3d":
                 xyz, orientation = data
-                xyz_offset = [o + p for o,p in zip(offset, xyz)]
-                self.bus.publish("pose3d", [xyz_offset, orientation])
+                xyz2 = quaternion.rotate_vector(xyz, self.init_quat)
+                xyz_offset = [o + p for o,p in zip(offset, xyz2)]
+                orientation_offset = quaternion.multiply(orientation, self.init_quat)
+                self.bus.publish("pose3d", [xyz_offset, orientation_offset])
             else:
                 raise RuntimeError(f"unknown channel '{channel}'")
 
+    def run(self):
+        try:
+            self._run()
+        except BusShutdownException:
+            pass
 
     def request_stop(self):
         self.bus.shutdown()

subt/test_offseter.py

@@ -0,0 +1,38 @@
+import unittest
+from unittest.mock import MagicMock
+import datetime
+
+from subt.offseter import Offseter
+from osgar.bus import Bus
+
+
+class OffseterTest(unittest.TestCase):
+
+    def test_usage(self):
+        logger = MagicMock()
+        bus = Bus(logger)
+        logger.write = MagicMock(return_value=datetime.timedelta(microseconds=9721))
+        c = Offseter(bus=bus.handle('offseter'), config={})
+        tester = bus.handle('tester')
+        tester.register('origin', 'pose3d')
+        bus.connect('tester.origin', 'offseter.origin')
+        bus.connect('tester.pose3d', 'offseter.pose3d')
+        bus.connect('offseter.pose3d', 'tester.pose3d')
+        c.start()
+        tester.publish('origin', [b'A300W600R', -6.4, 5.0, 0.8, 0.0, 0.0, 1.0, 0])
+        tester.publish('pose3d', [[0, 0, 0], [0, 0, 0, 1]])  # first pose is "consumed"
+        tester.publish('pose3d', [[-2, -1, 0], [0, 0, 0, 1]])
+        tester.publish('pose3d', [[-2, -3, 1], [0, 0, 0.7071068, 0.7071068]])  # 90 deg
+        c.request_stop()
+        c.join()
+        self.assertIsNotNone(c.init_quat)
+        xyz, ori = tester.listen()[2]
+        self.assertEqual(xyz, [-6.4 + 2, 5.0 + 1, 0.8])
+        self.assertEqual(ori, [0.0, 0.0, 1.0, 0])
+
+        xyz, ori = tester.listen()[2]
+        self.assertEqual(xyz, [-6.4 + 2, 5.0 + 3, 0.8 + 1])
+        self.assertEqual(ori, [0, 0, 0.7071068, -0.7071068])  # 270 deg
+
+# vim: expandtab sw=4 ts=4
+