sitl: add Gazebo plant and smoke harness

tools/sitl/test_kaptan_sim.py

@@ -0,0 +1,100 @@
+"""Regression tests for host-SITL command-path truth assertions."""
+
+import importlib.machinery
+import importlib.util
+from pathlib import Path
+import unittest
+from unittest import mock
+
+
+def load_client_module():
+    path = Path(__file__).with_name("kaptan-sim")
+    loader = importlib.machinery.SourceFileLoader("kaptan_sim", str(path))
+    spec = importlib.util.spec_from_loader(loader.name, loader)
+    assert spec is not None
+    module = importlib.util.module_from_spec(spec)
+    loader.exec_module(module)
+    return module
+
+
+class TruthAcceptanceTests(unittest.TestCase):
+    @classmethod
+    def setUpClass(cls):
+        cls.client = load_client_module()
+
+    def acceptable(self, *, motors_stopped=True, ground_plane_error=0.0, landing=True):
+        return self.client.truth_sample_within_tolerance(
+            position_error=0.01,
+            velocity=0.01,
+            attitude_error=0.01,
+            motors_stopped=motors_stopped,
+            ground_plane_error=ground_plane_error,
+            position_tolerance=0.75,
+            velocity_tolerance=0.25,
+            attitude_tolerance=0.35,
+            require_stopped_motors=landing,
+            require_ground_plane=landing,
+        )
+
+    def test_move_truth_contract_allows_an_armed_hover(self):
+        self.assertTrue(self.acceptable(motors_stopped=False, ground_plane_error=1.0, landing=False))
+
+    def test_landing_truth_contract_requires_zero_motors_and_ground_plane(self):
+        self.assertFalse(self.acceptable(motors_stopped=False))
+        self.assertFalse(
+            self.acceptable(
+                ground_plane_error=self.client.GROUND_PLANE_TOLERANCE_METRES + 0.001
+            )
+        )
+        self.assertTrue(
+            self.acceptable(
+                ground_plane_error=self.client.GROUND_PLANE_TOLERANCE_METRES
+            )
+        )
+
+    def test_land_builds_the_terminal_ground_plane_command(self):
+        control_socket = object()
+        clock_socket = object()
+        with (
+            mock.patch.object(self.client, "wait_ready") as wait_ready,
+            mock.patch.object(self.client, "command_goal") as command_goal,
+        ):
+            self.client.land(
+                control_socket,
+                clock_socket,
+                1.0,
+                -2.0,
+                60.0,
+                True,
+                True,
+                0.75,
+                0.25,
+                0.35,
+                0.75,
+            )
+
+        wait_ready.assert_called_once_with(control_socket, 60.0)
+        arguments = command_goal.call_args.args
+        self.assertEqual(arguments[0:2], (control_socket, clock_socket))
+        magic, version, goal_id, north, east, up = self.client.GOAL.unpack(arguments[2])
+        self.assertEqual((magic, version, north, east, up), (b"KSD1", 1, 1.0, -2.0, 0.0))
+        self.assertEqual(arguments[3], goal_id)
+        self.assertEqual(arguments[4:], (
+            1.0,
+            -2.0,
+            0.0,
+            60.0,
+            True,
+            True,
+            0.75,
+            0.25,
+            0.35,
+            0.75,
+            self.client.GOAL_LANDED,
+            True,
+            "land",
+        ))
+
+
+if __name__ == "__main__":
+    unittest.main()