From 060e540635f68954fe25e0875dcfd1168f164d65 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Onur=20=C3=96zkan?= Date: Sun, 9 Aug 2026 13:58:52 +0300 Subject: [PATCH] sitl: add Gazebo plant and smoke harness --- .dockerignore | 3 + tools/sitl/CMakeLists.txt | 13 + tools/sitl/Dockerfile | 43 ++ tools/sitl/compose.yaml | 33 ++ tools/sitl/e2e-smoke.sh | 39 ++ tools/sitl/gazebo-entrypoint.sh | 4 + tools/sitl/kaptan-sim | 564 +++++++++++++++++++ tools/sitl/src/kaptan_sitl_quad.cc | 650 ++++++++++++++++++++++ tools/sitl/test_kaptan_sim.py | 100 ++++ tools/sitl/world-structure-test.py | 143 +++++ tools/sitl/worlds/kaptan-sitl-quad.sdf.in | 204 +++++++ 11 files changed, 1796 insertions(+) create mode 100644 .dockerignore create mode 100644 tools/sitl/CMakeLists.txt create mode 100644 tools/sitl/Dockerfile create mode 100644 tools/sitl/compose.yaml create mode 100755 tools/sitl/e2e-smoke.sh create mode 100755 tools/sitl/gazebo-entrypoint.sh create mode 100755 tools/sitl/kaptan-sim create mode 100644 tools/sitl/src/kaptan_sitl_quad.cc create mode 100644 tools/sitl/test_kaptan_sim.py create mode 100644 tools/sitl/world-structure-test.py create mode 100644 tools/sitl/worlds/kaptan-sitl-quad.sdf.in diff --git a/.dockerignore b/.dockerignore new file mode 100644 index 0000000..a309aca --- /dev/null +++ b/.dockerignore @@ -0,0 +1,3 @@ +target +.git +tools/hitl diff --git a/tools/sitl/CMakeLists.txt b/tools/sitl/CMakeLists.txt new file mode 100644 index 0000000..60487a6 --- /dev/null +++ b/tools/sitl/CMakeLists.txt @@ -0,0 +1,13 @@ +cmake_minimum_required(VERSION 3.22 FATAL_ERROR) +project(kaptan_sitl LANGUAGES CXX) + +# Gazebo Jetty deliberately supplies unversioned CMake package targets. The +# image installs only the Jetty (gz-sim 10) development ABI, so an accidental +# build against another Gazebo major fails at configuration time. +find_package(gz-cmake REQUIRED) +gz_find_package(gz-sim REQUIRED) + +add_library(kaptan_sitl_quad SHARED src/kaptan_sitl_quad.cc) +target_compile_features(kaptan_sitl_quad PRIVATE cxx_std_20) +target_compile_options(kaptan_sitl_quad PRIVATE -Wall -Wextra -Wpedantic -Werror) +target_link_libraries(kaptan_sitl_quad PRIVATE gz-sim::gz-sim) diff --git a/tools/sitl/Dockerfile b/tools/sitl/Dockerfile new file mode 100644 index 0000000..3016d3e --- /dev/null +++ b/tools/sitl/Dockerfile @@ -0,0 +1,43 @@ +# syntax=docker/dockerfile:1.7 + +FROM rust@sha256:3914072ca0c3b8aad871db9169a651ccfce30cf58303e5d6f2db16d1d8a7e58f AS sitl-build +WORKDIR /opt/kaptan +COPY . . +# Select the pinned host compiler explicitly rather than inheriting the +# repository's embedded-target toolchain components. `--locked` makes the +# resolved source graph an explicit reviewable input to the image build. +RUN cargo +1.90.0 build --locked --release --package kaptan-sitl + +# Gazebo Jetty's supported amd64 runtime. The image is intentionally split so +# the C++ plugin and host control process share no source-time build hack. +FROM ubuntu@sha256:561618e2c15bf2397621dd04f96926663a3b5616c189cf7e38db7e82f5c538ea AS gazebo-runtime +SHELL ["/bin/bash", "-o", "pipefail", "-c"] +ENV DEBIAN_FRONTEND=noninteractive +RUN apt-get update \ + && apt-get install --no-install-recommends -y ca-certificates curl gnupg \ + && curl --fail --location --silent --show-error \ + https://packages.osrfoundation.org/gazebo.gpg \ + --output /usr/share/keyrings/pkgs-osrf-archive-keyring.gpg \ + && echo "deb [arch=amd64 signed-by=/usr/share/keyrings/pkgs-osrf-archive-keyring.gpg] https://packages.osrfoundation.org/gazebo/ubuntu-stable noble main" \ + > /etc/apt/sources.list.d/gazebo-stable.list \ + && apt-get update \ + && apt-get install --no-install-recommends -y gz-sim10-cli \ + && rm -rf /var/lib/apt/lists/* + +FROM gazebo-runtime AS gazebo-build +RUN apt-get update \ + && apt-get install --no-install-recommends -y cmake g++ ninja-build pkg-config libgz-sim10-dev \ + && rm -rf /var/lib/apt/lists/* +COPY tools/sitl /opt/kaptan/sitl +RUN cmake -G Ninja -S /opt/kaptan/sitl -B /opt/kaptan/sitl/plugin/build \ + && cmake --build /opt/kaptan/sitl/plugin/build --parallel + +FROM gazebo-runtime +ENV HOME=/tmp \ + GZ_SIM_SYSTEM_PLUGIN_PATH=/opt/kaptan/sitl/plugin/build \ + GZ_SIM_RESOURCE_PATH=/opt/kaptan/sitl/worlds +COPY tools/sitl /opt/kaptan/sitl +COPY --from=gazebo-build /opt/kaptan/sitl/plugin/build/libkaptan_sitl_quad.so \ + /opt/kaptan/sitl/plugin/build/libkaptan_sitl_quad.so +COPY --from=sitl-build /opt/kaptan/target/release/kaptan-sitl /usr/local/bin/kaptan-sitl +RUN chmod 0555 /opt/kaptan/sitl/gazebo-entrypoint.sh /opt/kaptan/sitl/kaptan-sim /usr/local/bin/kaptan-sitl diff --git a/tools/sitl/compose.yaml b/tools/sitl/compose.yaml new file mode 100644 index 0000000..09e743c --- /dev/null +++ b/tools/sitl/compose.yaml @@ -0,0 +1,33 @@ +services: + gazebo-sitl: + build: + context: ../.. + dockerfile: tools/sitl/Dockerfile + command: ["/opt/kaptan/sitl/gazebo-entrypoint.sh"] + depends_on: + sitl: + condition: service_started + environment: + HOME: /tmp + network_mode: host + read_only: true + tmpfs: + - /tmp:mode=1777,size=256m + cap_drop: ["ALL"] + security_opt: ["no-new-privileges:true"] + init: true + stop_grace_period: 5s + + sitl: + build: + context: ../.. + dockerfile: tools/sitl/Dockerfile + command: ["/usr/local/bin/kaptan-sitl", "serve"] + network_mode: host + read_only: true + tmpfs: + - /tmp:mode=1777,size=32m + cap_drop: ["ALL"] + security_opt: ["no-new-privileges:true"] + init: true + stop_grace_period: 2s diff --git a/tools/sitl/e2e-smoke.sh b/tools/sitl/e2e-smoke.sh new file mode 100755 index 0000000..0718e64 --- /dev/null +++ b/tools/sitl/e2e-smoke.sh @@ -0,0 +1,39 @@ +#!/usr/bin/env sh +# Host-only closed-loop acceptance scenario. It verifies the actual UDP lease, +# estimator, safety, trajectory, controller, allocator, and Gazebo loop across +# ascent, cross-axis translation, descent, and terminal landing. Every terminal +# phase holds independent, noise-free Gazebo truth; that recorder cannot feed +# the controller. +set -eu + +project_root=$(CDPATH= cd -- "$(dirname -- "$0")/../.." && pwd) +compose_file="$project_root/tools/sitl/compose.yaml" + +python3 "$project_root/tools/sitl/world-structure-test.py" + +cleanup() { + status=$? + trap - EXIT + # Preserve the first failing command's result, but retain enough evidence + # to distinguish a client/lease failure from a Gazebo or container start + # failure in CI. `down` otherwise removes the only useful diagnostics. + if [ "$status" -ne 0 ]; then + echo "SITL smoke failed (exit $status); compose state and recent logs follow:" >&2 + docker compose -f "$compose_file" ps >&2 || true + docker compose -f "$compose_file" logs --no-color --tail 250 >&2 || true + fi + docker compose -f "$compose_file" down --remove-orphans || true + exit "$status" +} +trap cleanup EXIT +trap 'exit 130' INT +trap 'exit 143' TERM + +docker compose -f "$compose_file" up -d --build +"$project_root/tools/sitl/kaptan-sim" ready --timeout 30 +"$project_root/tools/sitl/kaptan-sim" move 0 0 1.5 --wait --verify-truth --timeout 60 +"$project_root/tools/sitl/kaptan-sim" move 5 3 5 --wait --verify-truth --timeout 90 +"$project_root/tools/sitl/kaptan-sim" move -4 -2 2 --wait --verify-truth --timeout 90 +# Start the terminal operation while airborne and off the landing point. A +# `move ... 1` is an armed hover, not evidence of landing or zero motors. +"$project_root/tools/sitl/kaptan-sim" land 0 0 --wait --verify-truth --timeout 60 diff --git a/tools/sitl/gazebo-entrypoint.sh b/tools/sitl/gazebo-entrypoint.sh new file mode 100755 index 0000000..26d83ad --- /dev/null +++ b/tools/sitl/gazebo-entrypoint.sh @@ -0,0 +1,4 @@ +#!/bin/sh +set -eu + +exec gz sim --force-version 10 -s -r -v 3 /opt/kaptan/sitl/worlds/kaptan-sitl-quad.sdf.in diff --git a/tools/sitl/kaptan-sim b/tools/sitl/kaptan-sim new file mode 100755 index 0000000..3e4c6d6 --- /dev/null +++ b/tools/sitl/kaptan-sim @@ -0,0 +1,564 @@ +#!/usr/bin/env python3 +"""Small operator client for the local Kaptan host-SITL process.""" + +from __future__ import annotations + +import argparse +import math +import secrets +import socket +import struct +import sys +import time + + +CONTROL = ("127.0.0.1", 14662) +TRUTH = ("127.0.0.1", 14663) +# The simulator lease identifies its one local operator by UDP source address. +# Keep this source stable across separate CLI invocations so a completed move +# can be followed by the next command without a second, competing operator. +OPERATOR_CONTROL = ("127.0.0.1", 14665) +OPERATOR_CLOCK = ("127.0.0.1", 14664) +VERSION = 1 +STATUS_QUERY = struct.Struct("<4sB3x") +STATUS = struct.Struct("<4sB3xQH2x") +GOAL = struct.Struct("<4sB3xI3f") +GOAL_KEEPALIVE = struct.Struct("<4sB3xI") +GOAL_STATUS = struct.Struct("<4sB3xIB3x") +CLOCK_PACKET = struct.Struct("<4sB3xQ") +# KST2 carries pose/velocity, launch-relative attitude, and applied motor +# fractions. It is a test-only stream; the Rust controller never binds TRUTH. +TRUTH_PACKET = struct.Struct("<4sB3xQI14f") + +ESTIMATOR_READY = 1 << 0 +IMU_FRESH = 1 << 1 +LOCAL_AIDING_FRESH = 1 << 2 +OUTPUT_PERMITTED = 1 << 3 +ACTUATOR_TRANSPORT = 1 << 4 +READY = ESTIMATOR_READY | IMU_FRESH | LOCAL_AIDING_FRESH | ACTUATOR_TRANSPORT +GOAL_KEEPALIVE_MICROSECONDS = 100_000 +GOAL_REJECTED = 0 +GOAL_ACTIVE = 1 +GOAL_SETTLED = 2 +GOAL_LANDED = 3 +# Independent truth-recorder assertion for the single, flat ground plane +# declared by the supplied world. This is not a physical contact detector and +# must not be carried into a flight target. +GROUND_PLANE_TOLERANCE_METRES = 0.10 + + +def arguments() -> argparse.Namespace: + parser = argparse.ArgumentParser(description=__doc__) + subcommands = parser.add_subparsers(dest="command", required=True) + ready = subcommands.add_parser("ready", help="wait for estimator and simulated sensors") + ready.add_argument("--timeout", type=float, default=20.0) + move = subcommands.add_parser("move", help="command absolute local north/east/up metres") + move.add_argument("north", type=float) + move.add_argument("east", type=float) + move.add_argument("up", type=float) + move.add_argument("--wait", action="store_true", help="wait for the settled result") + move.add_argument( + "--verify-truth", + action="store_true", + help="verify noise-free Gazebo truth on the test-only recorder port after settling", + ) + move.add_argument("--truth-position-tolerance", type=float, default=0.75) + move.add_argument("--truth-velocity-tolerance", type=float, default=0.50) + move.add_argument("--truth-attitude-tolerance-rad", type=float, default=0.35) + move.add_argument( + "--truth-hold-seconds", + type=float, + default=0.75, + help="require every independent truth sample to remain within tolerance for this interval", + ) + # A distant, bounded goal is intentionally slow: the default must not turn + # a valid long trajectory into a spurious client-side test failure. + move.add_argument("--timeout", type=float, default=180.0) + land = subcommands.add_parser( + "land", + help="descend to an explicit local landing point and revoke virtual motor authority", + ) + land.add_argument("north", type=float, help="absolute local north landing coordinate in metres") + land.add_argument("east", type=float, help="absolute local east landing coordinate in metres") + land.add_argument("--wait", action="store_true", help="wait for the terminal landed result") + land.add_argument( + "--verify-truth", + action="store_true", + help="verify independent Gazebo ground truth and zero applied motor fractions after landing", + ) + land.add_argument("--truth-position-tolerance", type=float, default=0.75) + land.add_argument("--truth-velocity-tolerance", type=float, default=0.25) + land.add_argument("--truth-attitude-tolerance-rad", type=float, default=0.35) + land.add_argument("--truth-hold-seconds", type=float, default=0.75) + land.add_argument("--timeout", type=float, default=180.0) + return parser.parse_args() + + +def recv(socket_: socket.socket, deadline: float) -> bytes | None: + remaining = deadline - time.monotonic() + if remaining <= 0: + return None + socket_.settimeout(remaining) + try: + payload, sender = socket_.recvfrom(256) + except TimeoutError: + return None + return payload if sender[0] == "127.0.0.1" else None + + +def latest_simulation_time( + clock_socket: socket.socket, previous: int | None +) -> int | None: + """Drains Gazebo's test-operator clock without trusting host elapsed time.""" + latest = previous + while True: + try: + payload, sender = clock_socket.recvfrom(64) + except BlockingIOError: + return latest + if sender[0] != "127.0.0.1" or len(payload) != CLOCK_PACKET.size: + continue + magic, version, simulation_time = CLOCK_PACKET.unpack(payload) + if magic != b"KSC1" or version != VERSION: + continue + if latest is not None and simulation_time < latest: + raise RuntimeError("Gazebo simulation clock reset; restart the command in the new epoch") + latest = simulation_time + + +def status(socket_: socket.socket, deadline: float) -> int | None: + socket_.sendto(STATUS_QUERY.pack(b"KSQ1", VERSION), CONTROL) + payload = recv(socket_, deadline) + if payload is None or len(payload) != STATUS.size: + return None + magic, version, _, flags = STATUS.unpack(payload) + if magic != b"KSS1" or version != VERSION: + return None + return flags + + +def wait_ready(socket_: socket.socket, timeout: float) -> None: + deadline = time.monotonic() + timeout + latest = None + while time.monotonic() < deadline: + latest = status(socket_, min(deadline, time.monotonic() + 0.5)) + if latest is not None and latest & READY == READY: + return + if latest is None: + raise RuntimeError("host SITL is not running; run docker compose -f tools/sitl/compose.yaml up -d --build") + names = ( + (ESTIMATOR_READY, "estimator initialization"), + (IMU_FRESH, "fresh simulator IMU"), + (LOCAL_AIDING_FRESH, "accepted simulator local-state aiding"), + (ACTUATOR_TRANSPORT, "virtual actuator transport"), + ) + missing = [name for bit, name in names if latest & bit != bit] + raise RuntimeError(f"SITL is waiting for: {', '.join(missing)}") + + +def truth_sample_within_tolerance( + position_error: float, + velocity: float, + attitude_error: float, + motors_stopped: bool, + ground_plane_error: float, + position_tolerance: float, + velocity_tolerance: float, + attitude_tolerance: float, + require_stopped_motors: bool, + require_ground_plane: bool, +) -> bool: + """Checks one independent truth sample against its declared contract.""" + return not ( + position_error > position_tolerance + or velocity > velocity_tolerance + or attitude_error > attitude_tolerance + or (require_stopped_motors and not motors_stopped) + or ( + require_ground_plane + and ground_plane_error > GROUND_PLANE_TOLERANCE_METRES + ) + ) + + +def verify_truth( + north: float, + east: float, + up: float, + timeout: float, + position_tolerance: float, + velocity_tolerance: float, + attitude_tolerance: float, + hold_seconds: float, + require_stopped_motors: bool, + require_ground_plane: bool, + control_socket: socket.socket | None = None, + clock_socket: socket.socket | None = None, + keepalive: bytes | None = None, +) -> None: + with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as truth_socket: + truth_socket.bind(TRUTH) + acquisition_deadline = time.monotonic() + timeout + hold_deadline: int | None = None + samples = 0 + worst_position_error = 0.0 + worst_velocity = 0.0 + worst_attitude_error = 0.0 + worst_ground_plane_error = 0.0 + worst_motor_fraction = 0.0 + last_out_of_tolerance: str | None = None + simulation_time: int | None = None + next_keepalive_time: int | None = None + last_truth_time: int | None = None + while time.monotonic() < acquisition_deadline: + if clock_socket is not None: + simulation_time = latest_simulation_time(clock_socket, simulation_time) + if ( + keepalive is not None + and control_socket is not None + and simulation_time is not None + and (next_keepalive_time is None or simulation_time >= next_keepalive_time) + ): + control_socket.sendto(keepalive, CONTROL) + next_keepalive_time = simulation_time + GOAL_KEEPALIVE_MICROSECONDS + payload = recv(truth_socket, min(acquisition_deadline, time.monotonic() + 0.01)) + if payload is None or len(payload) != TRUTH_PACKET.size: + continue + ( + magic, + version, + truth_time, + _, + north_m, + east_m, + down_m, + north_v, + east_v, + down_v, + attitude_w, + attitude_x, + attitude_y, + attitude_z, + motor_1, + motor_2, + motor_3, + motor_4, + ) = TRUTH_PACKET.unpack(payload) + if magic != b"KST2" or version != VERSION: + continue + if last_truth_time is not None and truth_time < last_truth_time: + raise RuntimeError("Gazebo truth time regressed") + last_truth_time = truth_time + position_error = math.sqrt( + (north_m - north) ** 2 + (east_m - east) ** 2 + (down_m + up) ** 2 + ) + # `up` is converted to local-NED down above. A terminal land + # request explicitly sets it to zero, so this distinguishes the + # declared ground plane from a low-velocity armed hover. + ground_plane_error = abs(down_m + up) + velocity = math.sqrt(north_v**2 + east_v**2 + down_v**2) + quaternion_norm = math.sqrt( + attitude_w**2 + attitude_x**2 + attitude_y**2 + attitude_z**2 + ) + if not math.isfinite(quaternion_norm) or abs(quaternion_norm - 1.0) > 1e-3: + raise RuntimeError("Gazebo truth contained a non-unit attitude quaternion") + attitude_error = 2.0 * math.acos(min(1.0, abs(attitude_w) / quaternion_norm)) + motors = (motor_1, motor_2, motor_3, motor_4) + maximum_motor_fraction = max(abs(motor) for motor in motors) + motors_stopped = all(abs(motor) <= 0.01 for motor in motors) + if not truth_sample_within_tolerance( + position_error, + velocity, + attitude_error, + motors_stopped, + ground_plane_error, + position_tolerance, + velocity_tolerance, + attitude_tolerance, + require_stopped_motors, + require_ground_plane, + ): + # A lifecycle result says the controller's own settle rule was + # met. The independent plant may still be damping. Require one + # uninterrupted truth hold, rather than rejecting the first + # transient sample after that result. + hold_deadline = None + samples = 0 + last_out_of_tolerance = ( + "position_error=" + f"{position_error:.3f} m, velocity={velocity:.3f} m/s, " + f"attitude_error={attitude_error:.3f} rad, " + f"ground_plane_error={ground_plane_error:.3f} m, motors={motors}" + ) + continue + if hold_deadline is None: + # Use the truth record's own simulated timestamp. A separately + # drained clock record can be ahead of buffered truth packets, + # so it cannot prove an uninterrupted physical hold. + hold_deadline = truth_time + int(hold_seconds * 1_000_000) + worst_position_error = position_error + worst_velocity = velocity + worst_attitude_error = attitude_error + worst_ground_plane_error = ground_plane_error + worst_motor_fraction = maximum_motor_fraction + else: + worst_position_error = max(worst_position_error, position_error) + worst_velocity = max(worst_velocity, velocity) + worst_attitude_error = max(worst_attitude_error, attitude_error) + worst_ground_plane_error = max( + worst_ground_plane_error, ground_plane_error + ) + worst_motor_fraction = max(worst_motor_fraction, maximum_motor_fraction) + samples += 1 + if truth_time >= hold_deadline: + summary = ( + "truth verified: " + f"samples={samples}, position_error<={worst_position_error:.3f} m, " + f"velocity<={worst_velocity:.3f} m/s, " + f"attitude_error<={worst_attitude_error:.3f} rad" + ) + if require_ground_plane: + summary += f", ground_plane_error<={worst_ground_plane_error:.3f} m" + if require_stopped_motors: + summary += f", motor_fraction<={worst_motor_fraction:.3f}" + print(summary) + return + if last_out_of_tolerance is not None: + raise RuntimeError( + "Gazebo truth never held within tolerance before timeout: " + + last_out_of_tolerance + ) + raise RuntimeError("did not receive a Gazebo truth record") + + +def move( + socket_: socket.socket, + clock_socket: socket.socket, + north: float, + east: float, + up: float, + timeout: float, + wait: bool, + verify: bool, + position_tolerance: float, + velocity_tolerance: float, + attitude_tolerance: float, + hold_seconds: float, +) -> None: + if not all(math.isfinite(value) for value in (north, east, up)): + raise ValueError("goal coordinates must be finite") + wait_ready(socket_, timeout) + goal_id = secrets.randbelow(0xFFFF_FFFF) + 1 + goal = GOAL.pack(b"KSG1", VERSION, goal_id, north, east, up) + command_goal( + socket_, + clock_socket, + goal, + goal_id, + north, + east, + up, + timeout, + wait, + verify, + position_tolerance, + velocity_tolerance, + attitude_tolerance, + hold_seconds, + GOAL_SETTLED, + False, + "move", + ) + + +def land( + socket_: socket.socket, + clock_socket: socket.socket, + north: float, + east: float, + timeout: float, + wait: bool, + verify: bool, + position_tolerance: float, + velocity_tolerance: float, + attitude_tolerance: float, + hold_seconds: float, +) -> None: + if not all(math.isfinite(value) for value in (north, east)): + raise ValueError("landing coordinates must be finite") + wait_ready(socket_, timeout) + goal_id = secrets.randbelow(0xFFFF_FFFF) + 1 + landing = GOAL.pack(b"KSD1", VERSION, goal_id, north, east, 0.0) + command_goal( + socket_, + clock_socket, + landing, + goal_id, + north, + east, + 0.0, + timeout, + wait, + verify, + position_tolerance, + velocity_tolerance, + attitude_tolerance, + hold_seconds, + GOAL_LANDED, + True, + "land", + ) + + +def command_goal( + socket_: socket.socket, + clock_socket: socket.socket, + goal: bytes, + goal_id: int, + north: float, + east: float, + up: float, + timeout: float, + wait: bool, + verify: bool, + position_tolerance: float, + velocity_tolerance: float, + attitude_tolerance: float, + hold_seconds: float, + terminal_lifecycle: int, + require_stopped_motors: bool, + action: str, +) -> None: + keepalive = GOAL_KEEPALIVE.pack(b"KSK1", VERSION, goal_id) + deadline = time.monotonic() + timeout + simulation_time: int | None = None + next_send_time: int | None = None + active = False + while time.monotonic() < deadline: + simulation_time = latest_simulation_time(clock_socket, simulation_time) + if next_send_time is None or ( + simulation_time is not None and simulation_time >= next_send_time + ): + socket_.sendto(keepalive if active else goal, CONTROL) + if simulation_time is not None: + next_send_time = simulation_time + GOAL_KEEPALIVE_MICROSECONDS + payload = recv(socket_, min(deadline, time.monotonic() + 0.01)) + if payload is None or len(payload) != GOAL_STATUS.size: + continue + magic, version, received_id, lifecycle = GOAL_STATUS.unpack(payload) + if magic != b"KSL1" or version != VERSION or received_id != goal_id: + continue + if lifecycle == GOAL_REJECTED: + raise RuntimeError(f"{action} was rejected because SITL was not estimator-ready") + if lifecycle == GOAL_ACTIVE: + active = True + print(f"{action} active: north={north:g} m, east={east:g} m, up={up:g} m") + break + if not active: + raise RuntimeError("goal was not activated") + if not wait: + return + while time.monotonic() < deadline: + simulation_time = latest_simulation_time(clock_socket, simulation_time) + if simulation_time is not None and ( + next_send_time is None or simulation_time >= next_send_time + ): + socket_.sendto(keepalive, CONTROL) + next_send_time = simulation_time + GOAL_KEEPALIVE_MICROSECONDS + payload = recv(socket_, min(deadline, time.monotonic() + 0.01)) + if payload is None or len(payload) != GOAL_STATUS.size: + continue + magic, version, received_id, lifecycle = GOAL_STATUS.unpack(payload) + if ( + magic == b"KSL1" + and version == VERSION + and received_id == goal_id + and lifecycle == terminal_lifecycle + ): + print("landed" if terminal_lifecycle == GOAL_LANDED else "settled") + if verify: + verify_truth( + north, + east, + up, + min(5.0, timeout), + position_tolerance, + velocity_tolerance, + attitude_tolerance, + hold_seconds, + require_stopped_motors, + terminal_lifecycle == GOAL_LANDED, + None if require_stopped_motors else socket_, + clock_socket, + None if require_stopped_motors else keepalive, + ) + return + raise RuntimeError(f"{action} did not reach its terminal state before timeout") + + +def main() -> int: + values = arguments() + if values.timeout <= 0: + raise ValueError("timeout must be positive") + if values.command in ("move", "land"): + tolerances = ( + values.truth_position_tolerance <= 0 + or values.truth_velocity_tolerance <= 0 + or values.truth_attitude_tolerance_rad <= 0 + or values.truth_hold_seconds <= 0 + or not math.isfinite(values.truth_position_tolerance) + or not math.isfinite(values.truth_velocity_tolerance) + or not math.isfinite(values.truth_attitude_tolerance_rad) + or not math.isfinite(values.truth_hold_seconds) + ) + if tolerances: + raise ValueError("truth tolerances and hold duration must be finite and positive") + with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as client: + client.bind(OPERATOR_CONTROL) + if values.command == "ready": + wait_ready(client, values.timeout) + print("ready") + elif values.command == "move": + with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as clock: + clock.bind(OPERATOR_CLOCK) + clock.setblocking(False) + move( + client, + clock, + values.north, + values.east, + values.up, + values.timeout, + values.wait, + values.verify_truth, + values.truth_position_tolerance, + values.truth_velocity_tolerance, + values.truth_attitude_tolerance_rad, + values.truth_hold_seconds, + ) + else: + with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as clock: + clock.bind(OPERATOR_CLOCK) + clock.setblocking(False) + land( + client, + clock, + values.north, + values.east, + values.timeout, + values.wait, + values.verify_truth, + values.truth_position_tolerance, + values.truth_velocity_tolerance, + values.truth_attitude_tolerance_rad, + values.truth_hold_seconds, + ) + return 0 + + +if __name__ == "__main__": + try: + raise SystemExit(main()) + except (OSError, RuntimeError, ValueError, struct.error) as error: + print(f"kaptan-sim: {error}", file=sys.stderr) + raise SystemExit(2) from error diff --git a/tools/sitl/src/kaptan_sitl_quad.cc b/tools/sitl/src/kaptan_sitl_quad.cc new file mode 100644 index 0000000..f5ee2e6 --- /dev/null +++ b/tools/sitl/src/kaptan_sitl_quad.cc @@ -0,0 +1,650 @@ +// Deterministic, deliberately narrow Gazebo-side adapter for the Kaptan +// quad-X host-SITL transport contract. It is simulator tooling, not flight code. + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace kaptan::sitl +{ +namespace +{ +constexpr std::uint8_t kPacketVersion = 1; +constexpr std::array kImuMagic{'K', 'S', 'I', '1'}; +constexpr std::array kLocalStateMagic{'K', 'S', 'V', '1'}; +constexpr std::array kClockMagic{'K', 'S', 'C', '1'}; +constexpr std::array kTruthMagic{'K', 'S', 'T', '2'}; +constexpr std::array kOutputMagic{'K', 'S', 'O', '1'}; +constexpr std::size_t kImuPacketSize = 44; +constexpr std::size_t kClockPacketSize = 16; +constexpr std::size_t kTruthPacketSize = 76; +constexpr std::size_t kOutputPacketSize = 40; +constexpr std::size_t kMaximumPacketsPerStep = 4; +constexpr std::size_t kQuadMotorCount = 4; + +struct QuadMotorGeometry +{ + gz::math::Vector3d offset_flu; + double reaction_torque_sign; +}; + +// This test model is FLU / ENU. It is not an electrical Kakute timer map: +// M1 front-right CW, M2 rear-right CCW, M3 rear-left CW, M4 front-left CCW. +constexpr std::array kQuadXGeometry{{ + {{0.17, -0.17, 0.0}, -1.0}, + {{-0.17, -0.17, 0.0}, 1.0}, + {{-0.17, 0.17, 0.0}, -1.0}, + {{0.17, 0.17, 0.0}, 1.0}, +}}; + +bool IsFiniteFraction(const float value) +{ + return std::isfinite(value) && value >= 0.0F && value <= 1.0F; +} + +bool IsZero(const float value) +{ + return value == 0.0F; +} + +void WriteU32(std::uint8_t *destination, const std::uint32_t value) +{ + for (std::size_t index = 0; index < sizeof(value); ++index) + { + destination[index] = static_cast(value >> (index * 8)); + } +} + +void WriteU64(std::uint8_t *destination, const std::uint64_t value) +{ + for (std::size_t index = 0; index < sizeof(value); ++index) + { + destination[index] = static_cast(value >> (index * 8)); + } +} + +std::uint32_t ReadU32(const std::uint8_t *source) +{ + std::uint32_t value = 0; + for (std::size_t index = 0; index < sizeof(value); ++index) + { + value |= static_cast(source[index]) << (index * 8); + } + return value; +} + +std::uint64_t ReadU64(const std::uint8_t *source) +{ + std::uint64_t value = 0; + for (std::size_t index = 0; index < sizeof(value); ++index) + { + value |= static_cast(source[index]) << (index * 8); + } + return value; +} + +void WriteF32(std::uint8_t *destination, const float value) +{ + WriteU32(destination, std::bit_cast(value)); +} + +float ReadF32(const std::uint8_t *source) +{ + return std::bit_cast(ReadU32(source)); +} + +bool SequenceAfter(const std::uint32_t next, const std::uint32_t previous) +{ + const std::uint32_t distance = next - previous; + return distance != 0 && distance <= UINT32_MAX / 2; +} + +class UdpSocket +{ + public: + UdpSocket() = default; + ~UdpSocket() + { + if (this->descriptor_ >= 0) + { + close(this->descriptor_); + } + } + + UdpSocket(const UdpSocket &) = delete; + UdpSocket &operator=(const UdpSocket &) = delete; + + bool BindLoopback(const std::uint16_t port) + { + this->descriptor_ = socket(AF_INET, SOCK_DGRAM, 0); + if (this->descriptor_ < 0) + { + return false; + } + const int flags = fcntl(this->descriptor_, F_GETFL, 0); + if (flags < 0 || fcntl(this->descriptor_, F_SETFL, flags | O_NONBLOCK) < 0) + { + return false; + } + sockaddr_in address{}; + address.sin_family = AF_INET; + address.sin_port = htons(port); + address.sin_addr.s_addr = htonl(INADDR_LOOPBACK); + return bind( + this->descriptor_, + reinterpret_cast(&address), + sizeof(address)) == 0; + } + + bool SendToLoopback( + const std::uint16_t port, const std::uint8_t *data, const std::size_t size) const + { + sockaddr_in address{}; + address.sin_family = AF_INET; + address.sin_port = htons(port); + address.sin_addr.s_addr = htonl(INADDR_LOOPBACK); + return sendto( + this->descriptor_, data, size, MSG_DONTWAIT, + reinterpret_cast(&address), sizeof(address)) == + static_cast(size); + } + + ssize_t Receive(std::uint8_t *data, const std::size_t size) const + { + sockaddr_in sender{}; + socklen_t sender_size = sizeof(sender); + const ssize_t received = recvfrom( + this->descriptor_, data, size, MSG_DONTWAIT, + reinterpret_cast(&sender), &sender_size); + if (received < 0 || sender.sin_addr.s_addr != htonl(INADDR_LOOPBACK)) + { + return -1; + } + return received; + } + + private: + int descriptor_{-1}; +}; + +struct OutputCommand +{ + std::uint64_t controller_time_usec{}; + std::uint32_t control_sequence{}; + bool permitted{}; + std::array motors{}; +}; + +bool DecodeOutput(const std::array &packet, + OutputCommand &output) +{ + if (!std::equal(kOutputMagic.begin(), kOutputMagic.end(), packet.begin()) || + packet[4] != kPacketVersion || packet[5] != 0 || packet[6] != 0 || packet[7] != 0) + { + return false; + } + output.controller_time_usec = ReadU64(packet.data() + 8); + output.control_sequence = ReadU32(packet.data() + 16); + if (packet[20] > 1 || packet[21] != 0 || packet[22] != 0 || packet[23] != 0) + { + return false; + } + output.permitted = packet[20] == 1; + for (std::size_t index = 0; index < output.motors.size(); ++index) + { + output.motors[index] = ReadF32(packet.data() + 24 + index * sizeof(float)); + if (!IsFiniteFraction(output.motors[index])) + { + return false; + } + } + return output.permitted || std::all_of(output.motors.begin(), output.motors.end(), IsZero); +} +} // namespace + +class Quad final : public gz::sim::System, + public gz::sim::ISystemConfigure, + public gz::sim::ISystemPreUpdate, + public gz::sim::ISystemPostUpdate, + public gz::sim::ISystemReset +{ + public: + Quad() : data_(std::make_unique()) {} + + void Configure(const gz::sim::Entity &entity, + const std::shared_ptr &sdf, + gz::sim::EntityComponentManager &ecm, + gz::sim::EventManager &) override; + + void PreUpdate(const gz::sim::UpdateInfo &info, + gz::sim::EntityComponentManager &ecm) override; + + void PostUpdate(const gz::sim::UpdateInfo &info, + const gz::sim::EntityComponentManager &) override; + + void Reset(const gz::sim::UpdateInfo &, + gz::sim::EntityComponentManager &) override; + + private: + struct Data + { + void OnImu(const gz::msgs::IMU &message) + { + std::lock_guard lock(this->imu_mutex); + this->latest_imu = message; + ++this->imu_generation; + if (!this->received_first_imu) + { + gzmsg << "Kaptan SITL quad received its first Gazebo IMU sample.\n"; + this->received_first_imu = true; + } + } + + gz::sim::Link body; + gz::transport::Node node; + UdpSocket socket; + std::uint16_t sensor_port{}; + std::uint16_t actuator_port{}; + std::uint16_t truth_port{}; + std::uint16_t operator_clock_port{}; + std::chrono::microseconds actuator_timeout{}; + double maximum_motor_thrust_newtons{}; + double motor_time_constant_seconds{}; + std::normal_distribution local_position_noise{0.0, 0.0}; + std::normal_distribution local_velocity_noise{0.0, 0.0}; + std::mt19937 local_state_random{}; + double reaction_torque_per_newton_metre{}; + std::optional launch_origin_enu; + std::optional launch_attitude_enu; + std::mutex imu_mutex; + gz::msgs::IMU latest_imu; + std::uint64_t imu_generation{}; + std::uint64_t published_imu_generation{}; + bool received_first_imu{}; + std::optional output; + std::array applied_motor_fractions{}; + std::optional> previous_output_order; + // Actuator freshness belongs to the simulated vehicle timeline, never + // host scheduling time. That keeps timeout behavior deterministic when + // Gazebo is paused, stepped, or runs at a non-real-time factor. + std::optional last_output_sim_time; + }; + + std::unique_ptr data_; +}; + +void Quad::Configure(const gz::sim::Entity &entity, + const std::shared_ptr &sdf, + gz::sim::EntityComponentManager &ecm, + gz::sim::EventManager &) +{ + const gz::sim::Model model(entity); + if (!model.Valid(ecm)) + { + gzerr << "Kaptan SITL quad must be attached to a model.\n"; + return; + } + const auto body_entity = model.LinkByName(ecm, "base_link"); + if (body_entity == gz::sim::kNullEntity) + { + gzerr << "Kaptan SITL quad requires a base_link.\n"; + return; + } + this->data_->body = gz::sim::Link(body_entity); + this->data_->body.EnableVelocityChecks(ecm); + + const std::string imu_topic = sdf->Get("imu_topic", "").first; + const unsigned int sensor_port = sdf->Get("sensor_udp_port", 0U).first; + const unsigned int actuator_port = + sdf->Get("actuator_udp_port", 0U).first; + const unsigned int truth_port = sdf->Get("truth_udp_port", 0U).first; + const unsigned int operator_clock_port = + sdf->Get("operator_clock_udp_port", 0U).first; + const std::uint64_t timeout_usec = + sdf->Get("actuator_timeout_usec", 0).first; + this->data_->maximum_motor_thrust_newtons = + sdf->Get("maximum_motor_thrust_newtons", 0.0).first; + this->data_->motor_time_constant_seconds = + sdf->Get("motor_time_constant_seconds", 0.0).first; + const double local_position_noise_stddev = + sdf->Get("local_position_noise_stddev_m", -1.0).first; + const double local_velocity_noise_stddev = + sdf->Get("local_velocity_noise_stddev_mps", -1.0).first; + const unsigned int local_state_noise_seed = + sdf->Get("local_state_noise_seed", 0U).first; + this->data_->reaction_torque_per_newton_metre = + sdf->Get("reaction_torque_per_newton_metre", 0.0).first; + if (imu_topic.empty() || sensor_port == 0 || actuator_port == 0 || truth_port == 0 || + operator_clock_port == 0 || + sensor_port > std::numeric_limits::max() || + actuator_port > std::numeric_limits::max() || + truth_port > std::numeric_limits::max() || + operator_clock_port > std::numeric_limits::max() || + timeout_usec == 0 || !std::isfinite(this->data_->maximum_motor_thrust_newtons) || + this->data_->maximum_motor_thrust_newtons <= 0.0 || + !std::isfinite(this->data_->motor_time_constant_seconds) || + this->data_->motor_time_constant_seconds <= 0.0 || + !std::isfinite(local_position_noise_stddev) || local_position_noise_stddev < 0.0 || + !std::isfinite(local_velocity_noise_stddev) || local_velocity_noise_stddev < 0.0 || + !std::isfinite(this->data_->reaction_torque_per_newton_metre) || + this->data_->reaction_torque_per_newton_metre < 0.0) + { + gzerr << "Kaptan SITL quad has an invalid static transport or vehicle configuration.\n"; + return; + } + this->data_->sensor_port = static_cast(sensor_port); + this->data_->actuator_port = static_cast(actuator_port); + this->data_->truth_port = static_cast(truth_port); + this->data_->operator_clock_port = static_cast(operator_clock_port); + this->data_->actuator_timeout = std::chrono::microseconds(timeout_usec); + this->data_->local_position_noise = + std::normal_distribution(0.0, local_position_noise_stddev); + this->data_->local_velocity_noise = + std::normal_distribution(0.0, local_velocity_noise_stddev); + this->data_->local_state_random.seed(local_state_noise_seed); + if (!this->data_->socket.BindLoopback(this->data_->actuator_port)) + { + gzerr << "Kaptan SITL quad could not bind its loopback actuator socket.\n"; + return; + } + if (!this->data_->node.Subscribe(imu_topic, &Data::OnImu, this->data_.get())) + { + gzerr << "Kaptan SITL quad could not subscribe to its configured IMU topic.\n"; + } + else + { + gzmsg << "Kaptan SITL quad subscribed to Gazebo IMU topic [" << imu_topic << "].\n"; + } +} + +void Quad::PreUpdate(const gz::sim::UpdateInfo &info, + gz::sim::EntityComponentManager &ecm) +{ + if (info.paused || info.simTime.count() < 0 || !this->data_->body.Valid(ecm)) + { + return; + } + const auto simulation_time = + std::chrono::duration_cast(info.simTime); + for (std::size_t attempt = 0; attempt < kMaximumPacketsPerStep; ++attempt) + { + std::array packet{}; + if (this->data_->socket.Receive(packet.data(), packet.size()) != + static_cast(packet.size())) + { + break; + } + OutputCommand command{}; + if (!DecodeOutput(packet, command)) + { + continue; + } + if (!command.permitted) + { + this->data_->output = command; + continue; + } + if (this->data_->previous_output_order.has_value() && + (command.controller_time_usec <= this->data_->previous_output_order->first || + !SequenceAfter(command.control_sequence, + this->data_->previous_output_order->second))) + { + continue; + } + this->data_->previous_output_order = + std::pair{command.controller_time_usec, command.control_sequence}; + this->data_->last_output_sim_time = simulation_time; + this->data_->output = command; + } + + const bool command_is_fresh = + this->data_->output.has_value() && this->data_->output->permitted && + this->data_->last_output_sim_time.has_value() && + simulation_time >= *this->data_->last_output_sim_time && + simulation_time - *this->data_->last_output_sim_time <= this->data_->actuator_timeout; + const auto pose = this->data_->body.WorldPose(ecm); + if (!pose.has_value()) + { + return; + } + const double dt_seconds = std::chrono::duration(info.dt).count(); + if (!std::isfinite(dt_seconds) || dt_seconds <= 0.0) + { + return; + } + const double alpha = 1.0 - std::exp(-dt_seconds / this->data_->motor_time_constant_seconds); + const auto desired_motors = command_is_fresh + ? this->data_->output->motors + : std::array{}; + const gz::math::Vector3d thrust_axis_world = pose->Rot().RotateVector({0.0, 0.0, 1.0}); + for (std::size_t index = 0; index < kQuadMotorCount; ++index) + { + this->data_->applied_motor_fractions[index] += + alpha * (static_cast(desired_motors[index]) - + this->data_->applied_motor_fractions[index]); + const double thrust = + this->data_->applied_motor_fractions[index] * + this->data_->maximum_motor_thrust_newtons; + this->data_->body.AddWorldWrench( + ecm, thrust_axis_world * thrust, + thrust_axis_world * + (kQuadXGeometry[index].reaction_torque_sign * thrust * + this->data_->reaction_torque_per_newton_metre), + kQuadXGeometry[index].offset_flu); + } +} + +void Quad::PostUpdate(const gz::sim::UpdateInfo &info, + const gz::sim::EntityComponentManager &_ecm) +{ + if (info.paused || info.simTime.count() < 0) + { + return; + } + // This is deliberately emitted for every unpaused Gazebo step, even when + // an IMU sample is absent. The host runtime uses it as its sole freshness + // and lease time base; host scheduling cannot advance safety state. + const auto send_clock = [this, &info]() { + std::array clock_packet{}; + std::copy(kClockMagic.begin(), kClockMagic.end(), clock_packet.begin()); + clock_packet[4] = kPacketVersion; + WriteU64(clock_packet.data() + 8, static_cast( + std::chrono::duration_cast( + info.simTime) + .count())); + (void)this->data_->socket.SendToLoopback( + this->data_->sensor_port, clock_packet.data(), clock_packet.size()); + (void)this->data_->socket.SendToLoopback( + this->data_->operator_clock_port, clock_packet.data(), clock_packet.size()); + }; + gz::msgs::IMU imu; + { + std::lock_guard lock(this->data_->imu_mutex); + if (this->data_->imu_generation == this->data_->published_imu_generation) + { + send_clock(); + return; + } + imu = this->data_->latest_imu; + this->data_->published_imu_generation = this->data_->imu_generation; + } + + // Gazebo's standard body sensor convention is FLU. Kaptan's is FRD, so the + // single explicit boundary conversion is x, -y, -z for both vectors. + std::array packet{}; + std::copy(kImuMagic.begin(), kImuMagic.end(), packet.begin()); + packet[4] = kPacketVersion; + WriteU64(packet.data() + 8, static_cast( + std::chrono::duration_cast( + info.simTime) + .count())); + WriteU32(packet.data() + 16, static_cast(this->data_->imu_generation)); + const std::array frd{ + static_cast(imu.linear_acceleration().x()), + -static_cast(imu.linear_acceleration().y()), + -static_cast(imu.linear_acceleration().z()), + static_cast(imu.angular_velocity().x()), + -static_cast(imu.angular_velocity().y()), + -static_cast(imu.angular_velocity().z()), + }; + if (!std::all_of(frd.begin(), frd.end(), [](const float value) { return std::isfinite(value); })) + { + send_clock(); + return; + } + for (std::size_t index = 0; index < frd.size(); ++index) + { + WriteF32(packet.data() + 20 + index * sizeof(float), frd[index]); + } + (void)this->data_->socket.SendToLoopback( + this->data_->sensor_port, packet.data(), packet.size()); + + const auto pose = this->data_->body.WorldPose(_ecm); + const auto velocity = this->data_->body.WorldLinearVelocity(_ecm); + if (!pose.has_value() || !velocity.has_value()) + { + send_clock(); + return; + } + if (!this->data_->launch_origin_enu.has_value()) + { + this->data_->launch_origin_enu = pose->Pos(); + this->data_->launch_attitude_enu = pose->Rot(); + } + // This fixed test vehicle is yawed so ENU world +Y/+X/-Z are respectively + // its launch-local NED north/east/down axes. The conversion happens once at + // the Gazebo boundary; all flight code receives only explicit NED SI data. + const auto &origin = *this->data_->launch_origin_enu; + const std::array local_ned{ + static_cast(pose->Pos().Y() - origin.Y() + + this->data_->local_position_noise(this->data_->local_state_random)), + static_cast(pose->Pos().X() - origin.X() + + this->data_->local_position_noise(this->data_->local_state_random)), + static_cast(origin.Z() - pose->Pos().Z() + + this->data_->local_position_noise(this->data_->local_state_random)), + static_cast(velocity->Y() + + this->data_->local_velocity_noise(this->data_->local_state_random)), + static_cast(velocity->X() + + this->data_->local_velocity_noise(this->data_->local_state_random)), + static_cast(-velocity->Z() + + this->data_->local_velocity_noise(this->data_->local_state_random)), + }; + if (!std::all_of( + local_ned.begin(), local_ned.end(), + [](const float value) { return std::isfinite(value); })) + { + send_clock(); + return; + } + std::array state_packet{}; + std::copy(kLocalStateMagic.begin(), kLocalStateMagic.end(), state_packet.begin()); + state_packet[4] = kPacketVersion; + WriteU64(state_packet.data() + 8, static_cast( + std::chrono::duration_cast( + info.simTime) + .count())); + WriteU32(state_packet.data() + 16, static_cast(this->data_->imu_generation)); + for (std::size_t index = 0; index < local_ned.size(); ++index) + { + WriteF32(state_packet.data() + 20 + index * sizeof(float), local_ned[index]); + } + (void)this->data_->socket.SendToLoopback( + this->data_->sensor_port, state_packet.data(), state_packet.size()); + + // This packet is intentionally sent to a separate test-only port. The + // controller never binds that port, so these noise-free values cannot form a + // parallel control path or mask estimator behavior. The quaternion is the + // body attitude relative to launch; its rotation magnitude is frame + // invariant, which permits the test client to verify attitude without + // duplicating the ENU/FLU-to-NED/FRD conversion path. + const auto launch_attitude = *this->data_->launch_attitude_enu; + const auto relative_attitude = launch_attitude.Inverse() * pose->Rot(); + const std::array truth_ned{ + static_cast(pose->Pos().Y() - origin.Y()), + static_cast(pose->Pos().X() - origin.X()), + static_cast(origin.Z() - pose->Pos().Z()), + static_cast(velocity->Y()), + static_cast(velocity->X()), + static_cast(-velocity->Z()), + static_cast(relative_attitude.W()), + static_cast(relative_attitude.X()), + static_cast(relative_attitude.Y()), + static_cast(relative_attitude.Z()), + static_cast(this->data_->applied_motor_fractions[0]), + static_cast(this->data_->applied_motor_fractions[1]), + static_cast(this->data_->applied_motor_fractions[2]), + static_cast(this->data_->applied_motor_fractions[3]), + }; + if (!std::all_of( + truth_ned.begin(), truth_ned.end(), + [](const float value) { return std::isfinite(value); })) + { + send_clock(); + return; + } + std::array truth_packet{}; + std::copy(kTruthMagic.begin(), kTruthMagic.end(), truth_packet.begin()); + truth_packet[4] = kPacketVersion; + WriteU64(truth_packet.data() + 8, static_cast( + std::chrono::duration_cast( + info.simTime) + .count())); + WriteU32(truth_packet.data() + 16, static_cast(this->data_->imu_generation)); + for (std::size_t index = 0; index < truth_ned.size(); ++index) + { + WriteF32(truth_packet.data() + 20 + index * sizeof(float), truth_ned[index]); + } + (void)this->data_->socket.SendToLoopback( + this->data_->truth_port, truth_packet.data(), truth_packet.size()); + send_clock(); +} + +void Quad::Reset(const gz::sim::UpdateInfo &, + gz::sim::EntityComponentManager &) +{ + this->data_->output.reset(); + this->data_->applied_motor_fractions = {}; + this->data_->previous_output_order.reset(); + this->data_->last_output_sim_time.reset(); + this->data_->launch_origin_enu.reset(); + this->data_->launch_attitude_enu.reset(); + std::lock_guard lock(this->data_->imu_mutex); + this->data_->published_imu_generation = this->data_->imu_generation; +} + +} // namespace kaptan::sitl + +GZ_ADD_PLUGIN(kaptan::sitl::Quad, + gz::sim::System, + kaptan::sitl::Quad::ISystemConfigure, + kaptan::sitl::Quad::ISystemPreUpdate, + kaptan::sitl::Quad::ISystemPostUpdate, + kaptan::sitl::Quad::ISystemReset) +GZ_ADD_PLUGIN_ALIAS(kaptan::sitl::Quad, "kaptan::sitl::Quad") diff --git a/tools/sitl/test_kaptan_sim.py b/tools/sitl/test_kaptan_sim.py new file mode 100644 index 0000000..99af61a --- /dev/null +++ b/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() diff --git a/tools/sitl/world-structure-test.py b/tools/sitl/world-structure-test.py new file mode 100644 index 0000000..8b0429f --- /dev/null +++ b/tools/sitl/world-structure-test.py @@ -0,0 +1,143 @@ +#!/usr/bin/env python3 +"""Structural regression checks for Kaptan's deterministic Gazebo SITL world. + +This is deliberately an SDF/model-contract check, not a dynamics substitute. +It protects the reviewed boundary that the map is visual-only while the flat +ground and four landing feet provide the sole touchdown geometry. +""" + +from __future__ import annotations + +from pathlib import Path +import sys +import xml.etree.ElementTree as xml + + +WORLD = Path(__file__).with_name("worlds") / "kaptan-sitl-quad.sdf.in" + + +def fail(message: str) -> None: + raise AssertionError(message) + + +def model(root: xml.Element, name: str) -> xml.Element: + found = root.find(f"./world/model[@name='{name}']") + if found is None: + fail(f"missing model {name!r}") + return found + + +def named(parent: xml.Element, tag: str, name: str) -> xml.Element: + found = parent.find(f"./{tag}[@name='{name}']") + if found is None: + fail(f"missing {tag} {name!r}") + return found + + +def pose(element: xml.Element) -> tuple[float, ...]: + text = element.findtext("pose") + if text is None: + fail(f"{element.tag} {element.get('name')!r} has no pose") + values = tuple(float(value) for value in text.split()) + if len(values) != 6: + fail(f"{element.tag} {element.get('name')!r} pose does not have six fields") + return values + + +def cylinder(element: xml.Element) -> tuple[float, float]: + radius = element.findtext("geometry/cylinder/radius") + length = element.findtext("geometry/cylinder/length") + if radius is None or length is None: + fail(f"{element.tag} {element.get('name')!r} is not a cylinder") + return float(radius), float(length) + + +def vector(element: xml.Element, path: str, size: int) -> tuple[float, ...]: + text = element.findtext(path) + if text is None: + fail(f"{element.tag} {element.get('name')!r} has no {path!r}") + values = tuple(float(value) for value in text.split()) + if len(values) != size: + fail(f"{element.tag} {element.get('name')!r} {path!r} has {len(values)} fields") + return values + + +def check_world() -> None: + root = xml.parse(WORLD).getroot() + world = root.find("./world") + if world is None: + fail("missing world") + model_names = {model_element.get("name") for model_element in world.findall("model")} + if model_names != {"ground", "sitl_reference_map", "kaptan_sitl_quad"}: + fail( + "the declared flat-world model may contain only ground, the visual reference map, " + f"and the quad; found {sorted(model_names)!r}" + ) + + ground = model(root, "ground") + ground_collisions = ground.findall(".//collision") + if [collision.get("name") for collision in ground_collisions] != ["ground_collision"]: + fail("ground must retain exactly one flat collision plane") + if ground_collisions[0].find("geometry/plane") is None: + fail("ground collision must remain a flat plane") + if vector(ground_collisions[0], "geometry/plane/normal", 3) != (0.0, 0.0, 1.0): + fail("ground plane must remain horizontal with world +Z normal") + if vector(ground_collisions[0], "geometry/plane/size", 2) != (100.0, 100.0): + fail("ground plane must retain its declared 100 m square contact area") + + reference_map = model(root, "sitl_reference_map") + if reference_map.findall(".//collision"): + fail("reference map must remain visual-only and cannot add obstacle physics") + if reference_map.findall(".//transparency"): + fail("reference map must not use unsupported SDF transparency material children") + reference_link = named(reference_map, "link", "reference_link") + actual_marks = {visual.get("name") for visual in reference_link.findall("visual")} + if actual_marks != {"landing_pad"}: + fail(f"reference map may contain only the landing pad; found {sorted(actual_marks)!r}") + + quad = model(root, "kaptan_sitl_quad") + if pose(quad)[:3] != (0.0, 0.0, 0.14): + fail("quad base_link must start in its reviewed landing-foot rest pose") + base_link = named(quad, "link", "base_link") + body_collision = named(base_link, "collision", "body_collision") + if vector(body_collision, "geometry/box/size", 3) != (0.30, 0.30, 0.08): + fail("quad body collision must retain its reviewed clearance geometry") + foot_positions = { + "front_right": (0.12, -0.12), + "rear_right": (-0.12, -0.12), + "rear_left": (-0.12, 0.12), + "front_left": (0.12, 0.12), + } + for foot, expected_xy in foot_positions.items(): + collision = named(base_link, "collision", f"landing_foot_{foot}_collision") + visual = named(base_link, "visual", f"landing_foot_{foot}_visual") + if pose(collision)[:3] != (*expected_xy, -0.10): + fail(f"landing foot {foot!r} has an unexpected contact pose") + if pose(visual) != pose(collision): + fail(f"landing foot {foot!r} visual must match its collision geometry") + if cylinder(collision) != (0.022, 0.08) or cylinder(visual) != (0.022, 0.08): + fail(f"landing foot {foot!r} must retain its reviewed cylinder dimensions") + + expected_quad_collisions = { + "body_collision", + *(f"landing_foot_{foot}_collision" for foot in foot_positions), + } + actual_quad_collisions = {collision.get("name") for collision in base_link.findall("collision")} + if actual_quad_collisions != expected_quad_collisions: + fail( + "quad collision geometry must remain its body and four declared landing feet; " + f"found {sorted(actual_quad_collisions)!r}" + ) + + sensors = quad.findall(".//sensor") + if [(sensor.get("name"), sensor.get("type")) for sensor in sensors] != [("imu", "imu")]: + fail("this local-state SITL world must not imply unmodeled visual/range/flow sensors") + + +if __name__ == "__main__": + try: + check_world() + except (AssertionError, OSError, xml.ParseError, ValueError) as error: + print(f"SITL world structure check failed: {error}", file=sys.stderr) + sys.exit(1) + print("SITL world structure check passed") diff --git a/tools/sitl/worlds/kaptan-sitl-quad.sdf.in b/tools/sitl/worlds/kaptan-sitl-quad.sdf.in new file mode 100644 index 0000000..10d076d --- /dev/null +++ b/tools/sitl/worlds/kaptan-sitl-quad.sdf.in @@ -0,0 +1,204 @@ + + + + 0 0 -9.80665 + + 0.0025 + 1.0 + 400 + + + + + + + ogre2 + + + + 9002 + 30 + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + -0.5 0.1 -0.9 + + + 0.35 0.38 0.42 1 + 0.08 0.11 0.16 1 + true + + + + + true + + + 0 0 0.002 0 0 0 + 1.250.004 + + 0.03 0.20 0.36 10.05 0.36 0.65 1 + 0.01 0.08 0.14 1 + + + + + + + true + + + 0 0 1100 100 + + + 0 0 1100 100 + 0.2 0.2 0.2 10.4 0.4 0.4 1 + + + + + + + + + 0 0 0.14 0 0 1.5707963268 + + + 1.2 + + 0.0210.0210.039 + 000 + + + + 0.30 0.30 0.08 + + + 0.30 0.30 0.08 + 0.05 0.1 0.2 10.1 0.2 0.5 1 + + + 0.12 -0.12 -0.10 0 0 0 + 0.0220.08 + + + 0.12 -0.12 -0.10 0 0 0 + 0.0220.08 + 0.07 0.07 0.08 10.12 0.12 0.14 1 + + + -0.12 -0.12 -0.10 0 0 0 + 0.0220.08 + + + -0.12 -0.12 -0.10 0 0 0 + 0.0220.08 + 0.07 0.07 0.08 10.12 0.12 0.14 1 + + + -0.12 0.12 -0.10 0 0 0 + 0.0220.08 + + + -0.12 0.12 -0.10 0 0 0 + 0.0220.08 + 0.07 0.07 0.08 10.12 0.12 0.14 1 + + + 0.12 0.12 -0.10 0 0 0 + 0.0220.08 + + + 0.12 0.12 -0.10 0 0 0 + 0.0220.08 + 0.07 0.07 0.08 10.12 0.12 0.14 1 + + + 0.085 -0.085 0 0 0 -0.7853981634 + 0.30 0.025 0.02 + + + -0.085 -0.085 0 0 0 0.7853981634 + 0.30 0.025 0.02 + + + -0.085 0.085 0 0 0 -0.7853981634 + 0.30 0.025 0.02 + + + 0.085 0.085 0 0 0 0.7853981634 + 0.30 0.025 0.02 + + + 0.17 -0.17 0.05 0 0 0 + 0.100.01 + 0.7 0.1 0.1 10.7 0.1 0.1 1 + + + -0.17 -0.17 0.05 0 0 0 + 0.100.01 + 0.1 0.7 0.1 10.1 0.7 0.1 1 + + + -0.17 0.17 0.05 0 0 0 + 0.100.01 + 0.1 0.1 0.7 10.1 0.1 0.7 1 + + + 0.17 0.17 0.05 0 0 0 + 0.100.01 + 0.7 0.7 0.1 10.7 0.7 0.1 1 + + + true + 100 + /kaptan/sitl/imu + + + 00.002 + 00.002 + 00.002 + + + 00.03 + 00.03 + 00.03 + + + + + + /kaptan/sitl/imu + 14660 + 14661 + 14663 + 14664 + 25000 + 4.5 + 0.03 + 0.10 + 0.05 + 18181 + 0.012 + + + +