refactor: move firmware workspace under firmware
This commit is contained in:
Onur Özkan committed
1 parent
153b0bf262
commit
f03fc119a4
222 files changed
+56
-13
No files matched your search
@@ -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)
|
||||
@@ -0,0 +1,28 @@
|
||||
[package]
|
||||
name = "kaptan-sitl"
|
||||
edition.workspace = true
|
||||
license.workspace = true
|
||||
rust-version.workspace = true
|
||||
version.workspace = true
|
||||
|
||||
[lints]
|
||||
workspace = true
|
||||
|
||||
[dependencies]
|
||||
kaptan-control = { path = "../../libs/kaptan-control" }
|
||||
kaptan-config = { path = "../../libs/kaptan-config" }
|
||||
kaptan-config-mavlink = { path = "../../libs/kaptan-config-mavlink", features = ["host-authority"] }
|
||||
kaptan-flight-config = { path = "../../libs/kaptan-flight-config" }
|
||||
kaptan-flight-runtime = { path = "../../libs/kaptan-flight-runtime" }
|
||||
kaptan-log = { path = "../../libs/kaptan-log", features = ["facade"] }
|
||||
kaptan-log-mavlink = { path = "../../libs/kaptan-log-mavlink" }
|
||||
kaptan-mavlink = { path = "../../libs/kaptan-mavlink" }
|
||||
kaptan-math = { path = "../../libs/kaptan-math" }
|
||||
kaptan-navigation = { path = "../../libs/kaptan-navigation", features = ["sitl"] }
|
||||
kaptan-safety = { path = "../../libs/kaptan-safety" }
|
||||
kaptan-time = { path = "../../libs/kaptan-time" }
|
||||
minicbor = "0.25"
|
||||
log = { version = "0.4.33", default-features = false }
|
||||
|
||||
[dev-dependencies]
|
||||
mavlink = { version = "0.18", default-features = false, features = ["embedded"] }
|
||||
@@ -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
|
||||
@@ -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
|
||||
Executable
+39
@@ -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
|
||||
Executable
+4
@@ -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
|
||||
Executable
+564
@@ -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
|
||||
@@ -0,0 +1,37 @@
|
||||
//! Host-only standard-log facade composition.
|
||||
|
||||
use std::sync::Once;
|
||||
|
||||
use kaptan_flight_runtime::RuntimeLogHub;
|
||||
use kaptan_log::{FacadeIngress, GlobalLogger, LogFilter, Severity};
|
||||
|
||||
const SITL_LOG_INGRESS_CAPACITY: usize = 16;
|
||||
static SITL_LOG_INGRESS: FacadeIngress<SITL_LOG_INGRESS_CAPACITY> =
|
||||
FacadeIngress::new(LogFilter::new(Severity::Info));
|
||||
static SITL_LOGGER: GlobalLogger<SITL_LOG_INGRESS_CAPACITY> =
|
||||
GlobalLogger::new(&SITL_LOG_INGRESS, sitl_forbidden_log_context);
|
||||
static INSTALL_SITL_LOGGER: Once = Once::new();
|
||||
|
||||
const fn sitl_forbidden_log_context() -> bool {
|
||||
false
|
||||
}
|
||||
|
||||
/// Installs the host-only standard logging facade once for the SITL process.
|
||||
///
|
||||
/// SITL has no ISR, panic, or hard-fault context. Flight targets deliberately
|
||||
/// do not enable this host-only feature.
|
||||
pub fn install_log_facade() {
|
||||
INSTALL_SITL_LOGGER.call_once(|| {
|
||||
SITL_LOGGER
|
||||
.install()
|
||||
.expect("SITL owns the process-global log facade")
|
||||
});
|
||||
}
|
||||
|
||||
pub(super) fn set_filter(filter: LogFilter) {
|
||||
SITL_LOG_INGRESS.set_filter(filter);
|
||||
}
|
||||
|
||||
pub(super) fn drain_one_into(hub: &mut RuntimeLogHub) {
|
||||
let _ = SITL_LOG_INGRESS.drain_one_into(hub);
|
||||
}
|
||||
@@ -0,0 +1,18 @@
|
||||
//! Host-only configuration composition for SITL.
|
||||
//!
|
||||
//! This module is deliberately not a firmware storage backend. Flight targets
|
||||
//! need a reviewed A/B journal and authority path; SITL uses a same-directory
|
||||
//! file replacement to make startup and configuration tests reproducible.
|
||||
|
||||
mod logging;
|
||||
mod runtime;
|
||||
mod store;
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests;
|
||||
|
||||
pub use logging::install_log_facade;
|
||||
pub use runtime::{CAPABILITIES, SitlConfigRuntime, SitlSnapshotImporter, configuration_service};
|
||||
pub use store::{
|
||||
FileProfileStore, FileStoreError, default_profile, load_profile, write_default_profile,
|
||||
};
|
||||
@@ -0,0 +1,103 @@
|
||||
//! Central SITL configuration and protocol composition.
|
||||
|
||||
use kaptan_config::ConfigManager;
|
||||
use kaptan_config_mavlink::{
|
||||
Authorized, ConfigCommand, ConfigProtocol, ProtocolError, ServiceResponse, SnapshotImporter,
|
||||
Target,
|
||||
};
|
||||
use kaptan_flight_config::{
|
||||
CandidateProfile, CborError, FORMAT_VERSION, VehicleCapabilities, VehicleProfile,
|
||||
decode_candidate,
|
||||
};
|
||||
use kaptan_flight_runtime::FlightRuntime;
|
||||
use kaptan_log::TransportAdmission;
|
||||
use kaptan_log_mavlink::{DiagnosticPayload, encode_diagnostic};
|
||||
|
||||
use super::{FileProfileStore, logging};
|
||||
|
||||
/// SITL's immutable compiled capability contract.
|
||||
pub const CAPABILITIES: VehicleCapabilities = VehicleCapabilities::quad_x();
|
||||
|
||||
/// Host integration adapter from the schema's one bounded CBOR profile blob to
|
||||
/// the generic custom-MAVLink transaction service.
|
||||
#[derive(Clone, Copy, Debug, Default)]
|
||||
pub struct SitlSnapshotImporter;
|
||||
|
||||
impl SnapshotImporter<CandidateProfile> for SitlSnapshotImporter {
|
||||
const IMPORT_SUPPORTED: bool = true;
|
||||
const PROFILE_FORMAT: u16 = FORMAT_VERSION;
|
||||
|
||||
fn decode(&self, bytes: &[u8]) -> Result<CandidateProfile, ProtocolError> {
|
||||
decode_candidate(bytes, &CAPABILITIES).map_err(|error| match error {
|
||||
CborError::TooLarge => ProtocolError::CborTooLarge,
|
||||
CborError::Malformed | CborError::TrailingData => ProtocolError::CborMalformed,
|
||||
CborError::InvalidProfile(violation) => ProtocolError::Violation(violation),
|
||||
})
|
||||
}
|
||||
}
|
||||
|
||||
/// SITL composition root retaining the one profile manager and one protocol
|
||||
/// lease state machine. The MAVLink adapter never owns either resource.
|
||||
pub struct SitlConfigRuntime {
|
||||
runtime: FlightRuntime<FileProfileStore>,
|
||||
protocol: ConfigProtocol<SitlSnapshotImporter>,
|
||||
}
|
||||
|
||||
impl SitlConfigRuntime {
|
||||
/// Dispatches one host-authorized transaction command. Real target ingress
|
||||
/// remains disabled until signing/replay authority is implemented.
|
||||
pub fn dispatch(&mut self, command: Authorized<ConfigCommand>) -> ServiceResponse {
|
||||
let (manager, _) = self.runtime.configuration_and_logs_mut();
|
||||
self.protocol.dispatch(manager, command)
|
||||
}
|
||||
|
||||
/// Enqueues the high-level error companion only after a composition owner
|
||||
/// has retained the authoritative `KAPTAN_CONFIG_RESULT`. There is no
|
||||
/// request correlation in `KAPTAN_ERROR`; its purpose is human-facing
|
||||
/// observability, never replacement of the structured result.
|
||||
pub fn after_config_result(&mut self, response: ServiceResponse) {
|
||||
if let ServiceResponse::Result(reply) = response {
|
||||
let (_, logs) = self.runtime.configuration_and_logs_mut();
|
||||
let _ = reply.publish_rejection(logs);
|
||||
}
|
||||
}
|
||||
|
||||
/// Returns the central manager for SITL state inspection only.
|
||||
pub const fn manager(&self) -> &ConfigManager<CandidateProfile, FileProfileStore> {
|
||||
self.runtime.configuration()
|
||||
}
|
||||
|
||||
/// Returns the immutable profile selected by the central manager at boot.
|
||||
pub const fn active_profile(&self) -> VehicleProfile {
|
||||
*self.runtime.configuration().active()
|
||||
}
|
||||
|
||||
/// Indicates custom CBOR import support to the local GCS bridge.
|
||||
pub const fn capability_flags(&self) -> u8 {
|
||||
self.protocol.capability_flags_for::<CandidateProfile>()
|
||||
}
|
||||
|
||||
/// Drains one standard `log` facade record and exposes the generated
|
||||
/// diagnostic payload for the host's sole MAVLink link owner. This
|
||||
/// host-only harness accepts the bounded handoff synchronously.
|
||||
pub fn pending_diagnostic_payload(&mut self) -> Option<DiagnosticPayload> {
|
||||
let (_, logs) = self.runtime.configuration_and_logs_mut();
|
||||
logging::drain_one_into(logs);
|
||||
self.runtime
|
||||
.submit_next(|record| (TransportAdmission::Accepted, encode_diagnostic(record)))
|
||||
}
|
||||
}
|
||||
|
||||
/// Builds the host-only central configuration composition used by forthcoming
|
||||
/// SITL/GCS integration. Flight targets must supply authenticated authority
|
||||
/// and a reviewed journal/activation coordinator before constructing one.
|
||||
pub fn configuration_service(store: FileProfileStore) -> Result<SitlConfigRuntime, String> {
|
||||
logging::install_log_facade();
|
||||
let runtime = FlightRuntime::recover(CAPABILITIES, store)
|
||||
.map_err(|error| format!("configuration recovery failed: {error:?}"))?;
|
||||
logging::set_filter(runtime.log_filter());
|
||||
Ok(SitlConfigRuntime {
|
||||
runtime,
|
||||
protocol: ConfigProtocol::with_importer(Target::new(1, 1), SitlSnapshotImporter),
|
||||
})
|
||||
}
|
||||
@@ -0,0 +1,177 @@
|
||||
//! Host-only persistence for a validated SITL profile.
|
||||
|
||||
use std::{
|
||||
fs::{self, File},
|
||||
io::{Read, Write},
|
||||
path::Path,
|
||||
};
|
||||
|
||||
use kaptan_config::{Generation, ProfileStore};
|
||||
use kaptan_flight_config::{
|
||||
CandidateProfile, CborEncodeError, CborError, MAXIMUM_CANDIDATE_BYTES, VehicleProfile,
|
||||
decode_candidate, encode_candidate,
|
||||
};
|
||||
use kaptan_log::KaptanError;
|
||||
|
||||
use super::{CAPABILITIES, configuration_service};
|
||||
|
||||
/// Host file-store error. It intentionally makes no flash power-loss claim.
|
||||
#[derive(Debug, KaptanError)]
|
||||
pub enum FileStoreError {
|
||||
/// Host filesystem operation failed.
|
||||
Io(std::io::Error),
|
||||
/// The bounded file record did not decode and validate.
|
||||
Cbor(CborError),
|
||||
/// The fixed snapshot buffer could not encode the complete candidate.
|
||||
Encode,
|
||||
/// The host generation counter cannot advance.
|
||||
GenerationExhausted,
|
||||
/// Factory-only startup deliberately has no durable configuration path.
|
||||
FactoryOnly,
|
||||
/// The private host envelope was malformed or did not carry a valid
|
||||
/// nonzero committed generation.
|
||||
Envelope,
|
||||
}
|
||||
|
||||
/// Same-directory atomic-replacement store for SITL only.
|
||||
pub struct FileProfileStore {
|
||||
path: Option<std::path::PathBuf>,
|
||||
generation: Option<Generation>,
|
||||
}
|
||||
|
||||
impl FileProfileStore {
|
||||
/// Creates a host-only store at a fixed profile path.
|
||||
pub fn new(path: impl Into<std::path::PathBuf>) -> Self {
|
||||
Self {
|
||||
path: Some(path.into()),
|
||||
generation: None,
|
||||
}
|
||||
}
|
||||
|
||||
/// Creates a factory-only store for `kaptan-sitl serve` without
|
||||
/// `--config`. Commits are rejected rather than silently discarded.
|
||||
pub const fn factory_only() -> Self {
|
||||
Self {
|
||||
path: None,
|
||||
generation: None,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl ProfileStore<CandidateProfile> for FileProfileStore {
|
||||
type Error = FileStoreError;
|
||||
|
||||
fn load(&mut self) -> Result<Option<(Generation, CandidateProfile)>, Self::Error> {
|
||||
let Some(path) = self.path.as_deref() else {
|
||||
return Ok(None);
|
||||
};
|
||||
let file = match File::open(path) {
|
||||
Ok(file) => file,
|
||||
Err(error) if error.kind() == std::io::ErrorKind::NotFound => return Ok(None),
|
||||
Err(error) => return Err(FileStoreError::Io(error)),
|
||||
};
|
||||
const MAXIMUM_FILE_BYTES: u64 = (MAXIMUM_CANDIDATE_BYTES + 64) as u64;
|
||||
if file.metadata().map_err(FileStoreError::Io)?.len() > MAXIMUM_FILE_BYTES {
|
||||
return Err(FileStoreError::Cbor(CborError::TooLarge));
|
||||
}
|
||||
let mut bytes = Vec::new();
|
||||
file.take(MAXIMUM_FILE_BYTES + 1)
|
||||
.read_to_end(&mut bytes)
|
||||
.map_err(FileStoreError::Io)?;
|
||||
if bytes.len() as u64 > MAXIMUM_FILE_BYTES {
|
||||
return Err(FileStoreError::Cbor(CborError::TooLarge));
|
||||
}
|
||||
let (generation, candidate) = decode_envelope(&bytes)?;
|
||||
self.generation = Some(generation);
|
||||
Ok(Some((generation, candidate)))
|
||||
}
|
||||
|
||||
fn persist(&mut self, candidate: &CandidateProfile) -> Result<Generation, Self::Error> {
|
||||
let path = self.path.clone().ok_or(FileStoreError::FactoryOnly)?;
|
||||
if self.generation.is_none() {
|
||||
let _ = self.load()?;
|
||||
}
|
||||
let generation = match self.generation {
|
||||
Some(generation) => Generation::try_new(
|
||||
generation
|
||||
.get()
|
||||
.checked_add(1)
|
||||
.ok_or(FileStoreError::GenerationExhausted)?,
|
||||
)
|
||||
.ok_or(FileStoreError::GenerationExhausted)?,
|
||||
None => Generation::new(1),
|
||||
};
|
||||
write_candidate(&path, *candidate, generation)?;
|
||||
self.generation = Some(generation);
|
||||
Ok(generation)
|
||||
}
|
||||
}
|
||||
|
||||
/// Returns the fully validated factory-active profile.
|
||||
pub fn default_profile() -> Result<VehicleProfile, String> {
|
||||
Ok(configuration_service(FileProfileStore::factory_only())?.active_profile())
|
||||
}
|
||||
|
||||
/// Loads one bounded, complete CBOR candidate and returns its active view.
|
||||
pub fn load_profile(path: impl AsRef<Path>) -> Result<VehicleProfile, String> {
|
||||
if !path.as_ref().is_file() {
|
||||
return Err("configuration file does not exist".to_owned());
|
||||
}
|
||||
Ok(configuration_service(FileProfileStore::new(path.as_ref()))?.active_profile())
|
||||
}
|
||||
|
||||
/// Writes one validated factory candidate using a same-directory replacement.
|
||||
pub fn write_default_profile(path: impl AsRef<Path>) -> Result<(), String> {
|
||||
let mut store = FileProfileStore::new(path.as_ref());
|
||||
store
|
||||
.persist(&CandidateProfile::default())
|
||||
.map(|_| ())
|
||||
.map_err(|error| error.to_string())
|
||||
}
|
||||
|
||||
fn write_candidate(
|
||||
path: &Path,
|
||||
candidate: CandidateProfile,
|
||||
generation: Generation,
|
||||
) -> Result<(), FileStoreError> {
|
||||
let mut writer = minicbor::encode::write::Cursor::new([0_u8; MAXIMUM_CANDIDATE_BYTES]);
|
||||
encode_candidate(candidate, &CAPABILITIES, &mut writer).map_err(|error| match error {
|
||||
CborEncodeError::InvalidProfile(violation) => {
|
||||
FileStoreError::Cbor(CborError::InvalidProfile(violation))
|
||||
}
|
||||
CborEncodeError::Encode(_) => FileStoreError::Encode,
|
||||
})?;
|
||||
let length = writer.position();
|
||||
let bytes = writer.into_inner();
|
||||
let mut envelope = minicbor::encode::write::Cursor::new([0_u8; MAXIMUM_CANDIDATE_BYTES + 64]);
|
||||
let mut encoder = minicbor::Encoder::new(&mut envelope);
|
||||
encoder
|
||||
.array(2)
|
||||
.and_then(|encoder| encoder.u64(generation.get()))
|
||||
.and_then(|encoder| encoder.bytes(&bytes[..length]))
|
||||
.map_err(|_| FileStoreError::Encode)?;
|
||||
let envelope_length = encoder.writer_mut().position();
|
||||
let envelope = envelope.into_inner();
|
||||
let temporary = path.with_extension("cbor.tmp");
|
||||
let mut file = File::create(&temporary).map_err(FileStoreError::Io)?;
|
||||
file.write_all(&envelope[..envelope_length])
|
||||
.map_err(FileStoreError::Io)?;
|
||||
file.sync_all().map_err(FileStoreError::Io)?;
|
||||
fs::rename(&temporary, path).map_err(FileStoreError::Io)
|
||||
}
|
||||
|
||||
fn decode_envelope(bytes: &[u8]) -> Result<(Generation, CandidateProfile), FileStoreError> {
|
||||
let mut decoder = minicbor::Decoder::new(bytes);
|
||||
if decoder.array().map_err(|_| FileStoreError::Envelope)? != Some(2) {
|
||||
return Err(FileStoreError::Envelope);
|
||||
}
|
||||
let generation = Generation::try_new(decoder.u64().map_err(|_| FileStoreError::Envelope)?)
|
||||
.ok_or(FileStoreError::Envelope)?;
|
||||
let candidate_bytes = decoder.bytes().map_err(|_| FileStoreError::Envelope)?;
|
||||
if decoder.position() != bytes.len() {
|
||||
return Err(FileStoreError::Envelope);
|
||||
}
|
||||
let candidate =
|
||||
decode_candidate(candidate_bytes, &CAPABILITIES).map_err(FileStoreError::Cbor)?;
|
||||
Ok((generation, candidate))
|
||||
}
|
||||
@@ -0,0 +1,356 @@
|
||||
use std::fs;
|
||||
|
||||
use kaptan_config::{ConfigModel, Generation, ProfileStore};
|
||||
use kaptan_config_mavlink::{
|
||||
Authorized, ConfigCommand, ResultKind, ServiceResponse, SnapshotImporter,
|
||||
};
|
||||
use kaptan_flight_config::{
|
||||
CandidateProfile, CborError, MAXIMUM_CANDIDATE_BYTES, SCHEMA_ID as PROFILE_SCHEMA_ID,
|
||||
decode_candidate, encode_candidate,
|
||||
};
|
||||
use kaptan_log_mavlink::DiagnosticPayload;
|
||||
use kaptan_mavlink::{
|
||||
encoder::MavlinkEncoder,
|
||||
message::{Message, kaptan::MavMessage},
|
||||
};
|
||||
|
||||
use super::{CAPABILITIES, FileProfileStore, SitlSnapshotImporter, configuration_service};
|
||||
|
||||
// Independently authored current-format CBOR map. It is intentionally not
|
||||
// produced by `encode_candidate`: this is the GCS/upload compatibility vector
|
||||
// for schema KAPTANPROFILE-01, with a non-default 1.7 kg mass.
|
||||
const EXTERNAL_PROFILE_CBOR: &[u8] = &[
|
||||
0xb8, 0x1a, // definite map, 26 entries
|
||||
0x00, 0x01, // format key/value
|
||||
0x19, 0xff, 0xff, 0x50, b'K', b'A', b'P', b'T', b'A', b'N', b'P', b'R', b'O', b'F', b'I', b'L',
|
||||
b'E', b'-', b'0', b'1', // schema key/value
|
||||
0x01, 0x1a, 0x00, 0x01, 0x86, 0xa0, // prearm dwell: 100000
|
||||
0x02, 0x19, 0xc3, 0x50, // IMU interval: 50000
|
||||
0x03, 0x19, 0xc3, 0x50, // IMU freshness: 50000
|
||||
0x04, 0x1a, 0x00, 0x02, 0x49, 0xf0, // local freshness: 150000
|
||||
0x05, 0x1a, 0x00, 0x07, 0xa1, 0x20, // link freshness: 500000
|
||||
0x06, 0xfa, 0x3f, 0x80, 0x00, 0x00, 0x07, 0xfa, 0x3f, 0x80, 0x00, 0x00, 0x08, 0xfa, 0x3f, 0x80,
|
||||
0x00, 0x00, 0x09, 0xfa, 0x3f, 0x80, 0x00, 0x00, 0x0a, 0xfa, 0x3f, 0x80, 0x00, 0x00, 0x0b, 0xfa,
|
||||
0x3f, 0x80, 0x00, 0x00, 0x0c, 0xfa, 0x3f, 0x80, 0x00, 0x00, 0x0d, 0xfa, 0x3f, 0xd9, 0x99,
|
||||
0x9a, // mass: 1.7
|
||||
0x0e, 0xfa, 0x41, 0xa0, 0x00, 0x00, // collective thrust: 20
|
||||
0x0f, 0xfa, 0x3f, 0x80, 0x00, 0x00, 0x10, 0xfa, 0x3f, 0x80, 0x00, 0x00, 0x11, 0xfa, 0x3f, 0x80,
|
||||
0x00, 0x00, 0x12, 0xfa, 0x00, 0x00, 0x00, 0x00, 0x13, 0xfa, 0x3f, 0x80, 0x00, 0x00, 0x14, 0xfa,
|
||||
0x3f, 0x80, 0x00, 0x00, 0x15, 0xfa, 0x00, 0x00, 0x00, 0x00, 0x16, 0xfa, 0x3f, 0x80, 0x00, 0x00,
|
||||
0x17, 0xfa, 0x3f, 0x80, 0x00, 0x00, 0x18, 0x18, 0x06, // maximum log level: INFO
|
||||
];
|
||||
|
||||
fn result(response: ServiceResponse) -> kaptan_config_mavlink::ConfigReply {
|
||||
match response {
|
||||
ServiceResponse::Result(reply) => reply,
|
||||
ServiceResponse::Info(_) | ServiceResponse::Descriptors(_) | ServiceResponse::Value(_) => {
|
||||
panic!("expected a mutating configuration result")
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
fn crc32c(bytes: &[u8]) -> u32 {
|
||||
let mut crc = !0_u32;
|
||||
for byte in bytes {
|
||||
crc ^= u32::from(*byte);
|
||||
for _ in 0..8 {
|
||||
crc = if crc & 1 == 0 {
|
||||
crc >> 1
|
||||
} else {
|
||||
(crc >> 1) ^ 0x82f6_3b78
|
||||
};
|
||||
}
|
||||
}
|
||||
!crc
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn host_store_round_trips_a_complete_validated_snapshot() {
|
||||
let path = std::env::temp_dir().join(format!(
|
||||
"kaptan-config-store-{}-{}.cbor",
|
||||
std::process::id(),
|
||||
std::thread::current().name().unwrap_or("test")
|
||||
));
|
||||
let temporary = path.with_extension("cbor.tmp");
|
||||
let _ = fs::remove_file(&path);
|
||||
let _ = fs::remove_file(&temporary);
|
||||
|
||||
let mut store = FileProfileStore::new(&path);
|
||||
assert!(store.load().unwrap().is_none());
|
||||
assert_eq!(
|
||||
store.persist(&CandidateProfile::default()).unwrap(),
|
||||
Generation::new(1)
|
||||
);
|
||||
let envelope = fs::read(&path).unwrap();
|
||||
assert!(matches!(
|
||||
decode_candidate(&envelope, &CAPABILITIES),
|
||||
Err(CborError::Malformed)
|
||||
));
|
||||
let (_, candidate) = store.load().unwrap().expect("persisted candidate");
|
||||
assert!(candidate.validate(&CAPABILITIES).is_ok());
|
||||
|
||||
assert_eq!(
|
||||
store.persist(&CandidateProfile::default()).unwrap(),
|
||||
Generation::new(2)
|
||||
);
|
||||
let mut reopened = FileProfileStore::new(&path);
|
||||
assert_eq!(
|
||||
reopened.load().unwrap().map(|(generation, _)| generation),
|
||||
Some(Generation::new(2))
|
||||
);
|
||||
let mut fresh = FileProfileStore::new(&path);
|
||||
assert_eq!(
|
||||
fresh.persist(&CandidateProfile::default()).unwrap(),
|
||||
Generation::new(3)
|
||||
);
|
||||
|
||||
fs::remove_file(&path).unwrap();
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn sitl_snapshot_importer_uses_the_real_schema_and_capabilities() {
|
||||
let candidate = CandidateProfile::default();
|
||||
let mut writer = minicbor::encode::write::Cursor::new([0_u8; MAXIMUM_CANDIDATE_BYTES]);
|
||||
encode_candidate(candidate, &CAPABILITIES, &mut writer).unwrap();
|
||||
let length = writer.position();
|
||||
let bytes = writer.into_inner();
|
||||
let imported = SitlSnapshotImporter
|
||||
.decode(&bytes[..length])
|
||||
.expect("valid current candidate import");
|
||||
assert!(imported.validate(&CAPABILITIES).is_ok());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn sitl_service_imports_a_real_cbor_candidate_only_into_its_transaction() {
|
||||
let path = std::env::temp_dir().join(format!(
|
||||
"kaptan-config-import-{}-{}.cbor",
|
||||
std::process::id(),
|
||||
std::thread::current().name().unwrap_or("test")
|
||||
));
|
||||
let _ = fs::remove_file(&path);
|
||||
let mut service = configuration_service(FileProfileStore::new(&path)).unwrap();
|
||||
let begin = result(service.dispatch(Authorized::host(
|
||||
9,
|
||||
ConfigCommand::Begin {
|
||||
request_id: 1,
|
||||
expected_generation: 0,
|
||||
target: kaptan_config_mavlink::Target::new(1, 1),
|
||||
},
|
||||
)));
|
||||
let mut writer = minicbor::encode::write::Cursor::new([0_u8; MAXIMUM_CANDIDATE_BYTES]);
|
||||
encode_candidate(CandidateProfile::default(), &CAPABILITIES, &mut writer).unwrap();
|
||||
let length = writer.position();
|
||||
assert!(length <= 192);
|
||||
let bytes = writer.into_inner();
|
||||
let import_begin = result(service.dispatch(Authorized::host(
|
||||
9,
|
||||
ConfigCommand::ImportBegin {
|
||||
request_id: 2,
|
||||
session_id: begin.session_id,
|
||||
expected_revision: 0,
|
||||
length: length as u16,
|
||||
schema_id: PROFILE_SCHEMA_ID,
|
||||
crc32c: crc32c(&bytes[..length]),
|
||||
target: kaptan_config_mavlink::Target::new(1, 1),
|
||||
},
|
||||
)));
|
||||
assert_eq!(import_begin.result, ResultKind::Accepted);
|
||||
let mut chunk = [0; 192];
|
||||
chunk[..length].copy_from_slice(&bytes[..length]);
|
||||
assert_eq!(
|
||||
result(service.dispatch(Authorized::host(
|
||||
9,
|
||||
ConfigCommand::ImportChunk {
|
||||
request_id: 3,
|
||||
session_id: begin.session_id,
|
||||
transfer_id: import_begin.transfer_id,
|
||||
offset: 0,
|
||||
length: length as u8,
|
||||
data: chunk,
|
||||
target: kaptan_config_mavlink::Target::new(1, 1),
|
||||
},
|
||||
)))
|
||||
.result,
|
||||
ResultKind::Accepted
|
||||
);
|
||||
assert_eq!(
|
||||
result(service.dispatch(Authorized::host(
|
||||
9,
|
||||
ConfigCommand::ImportFinish {
|
||||
request_id: 4,
|
||||
session_id: begin.session_id,
|
||||
transfer_id: import_begin.transfer_id,
|
||||
expected_revision: 0,
|
||||
target: kaptan_config_mavlink::Target::new(1, 1),
|
||||
},
|
||||
)))
|
||||
.result,
|
||||
ResultKind::Accepted
|
||||
);
|
||||
assert!(
|
||||
service
|
||||
.manager()
|
||||
.transaction_value(kaptan_config::FieldId::new(1))
|
||||
.is_some()
|
||||
);
|
||||
assert!(
|
||||
service
|
||||
.manager()
|
||||
.active_value(kaptan_config::FieldId::new(1))
|
||||
.is_some()
|
||||
);
|
||||
let _ = fs::remove_file(&path);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn external_gcs_vector_imports_commits_and_reopens_as_the_boot_profile() {
|
||||
let path = std::env::temp_dir().join(format!(
|
||||
"kaptan-config-gcs-vector-{}-{}.cbor",
|
||||
std::process::id(),
|
||||
std::thread::current().name().unwrap_or("test")
|
||||
));
|
||||
let _ = fs::remove_file(&path);
|
||||
let mut service = configuration_service(FileProfileStore::new(&path)).unwrap();
|
||||
let target = kaptan_config_mavlink::Target::new(1, 1);
|
||||
let begin = result(service.dispatch(Authorized::host(
|
||||
9,
|
||||
ConfigCommand::Begin {
|
||||
request_id: 1,
|
||||
expected_generation: 0,
|
||||
target,
|
||||
},
|
||||
)));
|
||||
let import = result(service.dispatch(Authorized::host(
|
||||
9,
|
||||
ConfigCommand::ImportBegin {
|
||||
request_id: 2,
|
||||
session_id: begin.session_id,
|
||||
expected_revision: 0,
|
||||
length: EXTERNAL_PROFILE_CBOR.len() as u16,
|
||||
schema_id: PROFILE_SCHEMA_ID,
|
||||
crc32c: crc32c(EXTERNAL_PROFILE_CBOR),
|
||||
target,
|
||||
},
|
||||
)));
|
||||
let mut data = [0_u8; 192];
|
||||
data[..EXTERNAL_PROFILE_CBOR.len()].copy_from_slice(EXTERNAL_PROFILE_CBOR);
|
||||
assert_eq!(
|
||||
result(service.dispatch(Authorized::host(
|
||||
9,
|
||||
ConfigCommand::ImportChunk {
|
||||
request_id: 3,
|
||||
session_id: begin.session_id,
|
||||
transfer_id: import.transfer_id,
|
||||
offset: 0,
|
||||
length: EXTERNAL_PROFILE_CBOR.len() as u8,
|
||||
data,
|
||||
target,
|
||||
},
|
||||
)))
|
||||
.result,
|
||||
ResultKind::Accepted
|
||||
);
|
||||
assert_eq!(
|
||||
result(service.dispatch(Authorized::host(
|
||||
9,
|
||||
ConfigCommand::ImportFinish {
|
||||
request_id: 4,
|
||||
session_id: begin.session_id,
|
||||
transfer_id: import.transfer_id,
|
||||
expected_revision: 0,
|
||||
target,
|
||||
},
|
||||
)))
|
||||
.revision,
|
||||
1
|
||||
);
|
||||
assert_eq!(
|
||||
result(service.dispatch(Authorized::host(
|
||||
9,
|
||||
ConfigCommand::Commit {
|
||||
request_id: 5,
|
||||
session_id: begin.session_id,
|
||||
expected_generation: 0,
|
||||
expected_revision: 1,
|
||||
target,
|
||||
},
|
||||
)))
|
||||
.result,
|
||||
ResultKind::CommittedRestartRequired
|
||||
);
|
||||
drop(service);
|
||||
|
||||
let reopened = configuration_service(FileProfileStore::new(&path)).unwrap();
|
||||
assert_eq!(reopened.active_profile().thrust().mass_kg(), 1.7);
|
||||
let _ = fs::remove_file(&path);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn standard_log_macro_reaches_the_generated_mavlink_log_payload() {
|
||||
let path = std::env::temp_dir().join(format!(
|
||||
"kaptan-sitl-log-{}-{}.cbor",
|
||||
std::process::id(),
|
||||
std::thread::current().name().unwrap_or("test")
|
||||
));
|
||||
let _ = fs::remove_file(&path);
|
||||
let mut service = configuration_service(FileProfileStore::new(&path)).unwrap();
|
||||
|
||||
log::info!("SITL configuration service ready");
|
||||
let Some(DiagnosticPayload::Log(payload)) = service.pending_diagnostic_payload() else {
|
||||
panic!("info facade record must reach KAPTAN_LOG");
|
||||
};
|
||||
assert_eq!(payload.level, kaptan_log::Severity::Info as u8);
|
||||
let message = std::str::from_utf8(&payload.message[..usize::from(payload.message_length)])
|
||||
.expect("facade text remains UTF-8");
|
||||
assert!(message.contains("config::tests"));
|
||||
assert!(message.ends_with("SITL configuration service ready"));
|
||||
let mut encoder = MavlinkEncoder::new(1, 1);
|
||||
let frame = encoder.custom(&payload);
|
||||
assert_eq!(frame.message_id(), 51_020);
|
||||
assert!(matches!(
|
||||
MavMessage::parse(
|
||||
mavlink::MavlinkVersion::V2,
|
||||
frame.message_id(),
|
||||
frame.payload(),
|
||||
),
|
||||
Ok(MavMessage::KAPTAN_LOG(data))
|
||||
if data.level == payload.level
|
||||
&& data.message_length == payload.message_length
|
||||
&& data.message == payload.message
|
||||
));
|
||||
let _ = fs::remove_file(&path);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn rejected_config_result_enqueues_a_separate_high_level_error_payload() {
|
||||
let path = std::env::temp_dir().join(format!(
|
||||
"kaptan-sitl-config-error-{}-{}.cbor",
|
||||
std::process::id(),
|
||||
std::thread::current().name().unwrap_or("test")
|
||||
));
|
||||
let _ = fs::remove_file(&path);
|
||||
let mut service = configuration_service(FileProfileStore::new(&path)).unwrap();
|
||||
let response = service.dispatch(Authorized::host(
|
||||
9,
|
||||
ConfigCommand::Begin {
|
||||
request_id: 1,
|
||||
expected_generation: 1,
|
||||
target: kaptan_config_mavlink::Target::new(1, 1),
|
||||
},
|
||||
));
|
||||
let reply = result(response);
|
||||
assert_eq!(reply.result, ResultKind::Rejected);
|
||||
service.after_config_result(response);
|
||||
|
||||
let Some(DiagnosticPayload::Error(payload)) = service.pending_diagnostic_payload() else {
|
||||
panic!("rejected configuration result must enqueue KAPTAN_ERROR");
|
||||
};
|
||||
let error_path = std::str::from_utf8(&payload.path[..usize::from(payload.path_length)])
|
||||
.expect("derived error path remains UTF-8");
|
||||
assert_eq!(
|
||||
error_path,
|
||||
"kaptan_config_mavlink::protocol::ProtocolError::GenerationConflict"
|
||||
);
|
||||
assert_eq!(payload.additional_information_length, 0);
|
||||
let _ = fs::remove_file(&path);
|
||||
}
|
||||
@@ -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 <gz/common/Console.hh>
|
||||
#include <gz/math/Pose3.hh>
|
||||
#include <gz/math/Vector3.hh>
|
||||
#include <gz/msgs/imu.pb.h>
|
||||
#include <gz/plugin/Register.hh>
|
||||
#include <gz/sim/EntityComponentManager.hh>
|
||||
#include <gz/sim/Link.hh>
|
||||
#include <gz/sim/Model.hh>
|
||||
#include <gz/sim/System.hh>
|
||||
#include <gz/transport/Node.hh>
|
||||
|
||||
#include <algorithm>
|
||||
#include <array>
|
||||
#include <bit>
|
||||
#include <chrono>
|
||||
#include <cmath>
|
||||
#include <cstddef>
|
||||
#include <cstdint>
|
||||
#include <cstring>
|
||||
#include <memory>
|
||||
#include <mutex>
|
||||
#include <limits>
|
||||
#include <optional>
|
||||
#include <random>
|
||||
#include <string>
|
||||
|
||||
#include <arpa/inet.h>
|
||||
#include <fcntl.h>
|
||||
#include <sys/socket.h>
|
||||
#include <unistd.h>
|
||||
|
||||
namespace kaptan::sitl
|
||||
{
|
||||
namespace
|
||||
{
|
||||
constexpr std::uint8_t kPacketVersion = 1;
|
||||
constexpr std::array<std::uint8_t, 4> kImuMagic{'K', 'S', 'I', '1'};
|
||||
constexpr std::array<std::uint8_t, 4> kLocalStateMagic{'K', 'S', 'V', '1'};
|
||||
constexpr std::array<std::uint8_t, 4> kClockMagic{'K', 'S', 'C', '1'};
|
||||
constexpr std::array<std::uint8_t, 4> kTruthMagic{'K', 'S', 'T', '2'};
|
||||
constexpr std::array<std::uint8_t, 4> 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<QuadMotorGeometry, kQuadMotorCount> 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<std::uint8_t>(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<std::uint8_t>(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<std::uint32_t>(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<std::uint64_t>(source[index]) << (index * 8);
|
||||
}
|
||||
return value;
|
||||
}
|
||||
|
||||
void WriteF32(std::uint8_t *destination, const float value)
|
||||
{
|
||||
WriteU32(destination, std::bit_cast<std::uint32_t>(value));
|
||||
}
|
||||
|
||||
float ReadF32(const std::uint8_t *source)
|
||||
{
|
||||
return std::bit_cast<float>(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<const sockaddr *>(&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<const sockaddr *>(&address), sizeof(address)) ==
|
||||
static_cast<ssize_t>(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<sockaddr *>(&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<float, kQuadMotorCount> motors{};
|
||||
};
|
||||
|
||||
bool DecodeOutput(const std::array<std::uint8_t, kOutputPacketSize> &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<Data>()) {}
|
||||
|
||||
void Configure(const gz::sim::Entity &entity,
|
||||
const std::shared_ptr<const sdf::Element> &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<double> local_position_noise{0.0, 0.0};
|
||||
std::normal_distribution<double> local_velocity_noise{0.0, 0.0};
|
||||
std::mt19937 local_state_random{};
|
||||
double reaction_torque_per_newton_metre{};
|
||||
std::optional<gz::math::Vector3d> launch_origin_enu;
|
||||
std::optional<gz::math::Quaterniond> 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<OutputCommand> output;
|
||||
std::array<double, kQuadMotorCount> applied_motor_fractions{};
|
||||
std::optional<std::pair<std::uint64_t, std::uint32_t>> 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<std::chrono::microseconds> last_output_sim_time;
|
||||
};
|
||||
|
||||
std::unique_ptr<Data> data_;
|
||||
};
|
||||
|
||||
void Quad::Configure(const gz::sim::Entity &entity,
|
||||
const std::shared_ptr<const sdf::Element> &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<std::string>("imu_topic", "").first;
|
||||
const unsigned int sensor_port = sdf->Get<unsigned int>("sensor_udp_port", 0U).first;
|
||||
const unsigned int actuator_port =
|
||||
sdf->Get<unsigned int>("actuator_udp_port", 0U).first;
|
||||
const unsigned int truth_port = sdf->Get<unsigned int>("truth_udp_port", 0U).first;
|
||||
const unsigned int operator_clock_port =
|
||||
sdf->Get<unsigned int>("operator_clock_udp_port", 0U).first;
|
||||
const std::uint64_t timeout_usec =
|
||||
sdf->Get<std::uint64_t>("actuator_timeout_usec", 0).first;
|
||||
this->data_->maximum_motor_thrust_newtons =
|
||||
sdf->Get<double>("maximum_motor_thrust_newtons", 0.0).first;
|
||||
this->data_->motor_time_constant_seconds =
|
||||
sdf->Get<double>("motor_time_constant_seconds", 0.0).first;
|
||||
const double local_position_noise_stddev =
|
||||
sdf->Get<double>("local_position_noise_stddev_m", -1.0).first;
|
||||
const double local_velocity_noise_stddev =
|
||||
sdf->Get<double>("local_velocity_noise_stddev_mps", -1.0).first;
|
||||
const unsigned int local_state_noise_seed =
|
||||
sdf->Get<unsigned int>("local_state_noise_seed", 0U).first;
|
||||
this->data_->reaction_torque_per_newton_metre =
|
||||
sdf->Get<double>("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<std::uint16_t>::max() ||
|
||||
actuator_port > std::numeric_limits<std::uint16_t>::max() ||
|
||||
truth_port > std::numeric_limits<std::uint16_t>::max() ||
|
||||
operator_clock_port > std::numeric_limits<std::uint16_t>::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<std::uint16_t>(sensor_port);
|
||||
this->data_->actuator_port = static_cast<std::uint16_t>(actuator_port);
|
||||
this->data_->truth_port = static_cast<std::uint16_t>(truth_port);
|
||||
this->data_->operator_clock_port = static_cast<std::uint16_t>(operator_clock_port);
|
||||
this->data_->actuator_timeout = std::chrono::microseconds(timeout_usec);
|
||||
this->data_->local_position_noise =
|
||||
std::normal_distribution<double>(0.0, local_position_noise_stddev);
|
||||
this->data_->local_velocity_noise =
|
||||
std::normal_distribution<double>(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<std::chrono::microseconds>(info.simTime);
|
||||
for (std::size_t attempt = 0; attempt < kMaximumPacketsPerStep; ++attempt)
|
||||
{
|
||||
std::array<std::uint8_t, kOutputPacketSize> packet{};
|
||||
if (this->data_->socket.Receive(packet.data(), packet.size()) !=
|
||||
static_cast<ssize_t>(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<double>(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<float, kQuadMotorCount>{};
|
||||
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<double>(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<std::uint8_t, kClockPacketSize> clock_packet{};
|
||||
std::copy(kClockMagic.begin(), kClockMagic.end(), clock_packet.begin());
|
||||
clock_packet[4] = kPacketVersion;
|
||||
WriteU64(clock_packet.data() + 8, static_cast<std::uint64_t>(
|
||||
std::chrono::duration_cast<std::chrono::microseconds>(
|
||||
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<std::uint8_t, kImuPacketSize> packet{};
|
||||
std::copy(kImuMagic.begin(), kImuMagic.end(), packet.begin());
|
||||
packet[4] = kPacketVersion;
|
||||
WriteU64(packet.data() + 8, static_cast<std::uint64_t>(
|
||||
std::chrono::duration_cast<std::chrono::microseconds>(
|
||||
info.simTime)
|
||||
.count()));
|
||||
WriteU32(packet.data() + 16, static_cast<std::uint32_t>(this->data_->imu_generation));
|
||||
const std::array<float, 6> frd{
|
||||
static_cast<float>(imu.linear_acceleration().x()),
|
||||
-static_cast<float>(imu.linear_acceleration().y()),
|
||||
-static_cast<float>(imu.linear_acceleration().z()),
|
||||
static_cast<float>(imu.angular_velocity().x()),
|
||||
-static_cast<float>(imu.angular_velocity().y()),
|
||||
-static_cast<float>(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<float, 6> local_ned{
|
||||
static_cast<float>(pose->Pos().Y() - origin.Y() +
|
||||
this->data_->local_position_noise(this->data_->local_state_random)),
|
||||
static_cast<float>(pose->Pos().X() - origin.X() +
|
||||
this->data_->local_position_noise(this->data_->local_state_random)),
|
||||
static_cast<float>(origin.Z() - pose->Pos().Z() +
|
||||
this->data_->local_position_noise(this->data_->local_state_random)),
|
||||
static_cast<float>(velocity->Y() +
|
||||
this->data_->local_velocity_noise(this->data_->local_state_random)),
|
||||
static_cast<float>(velocity->X() +
|
||||
this->data_->local_velocity_noise(this->data_->local_state_random)),
|
||||
static_cast<float>(-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<std::uint8_t, kImuPacketSize> state_packet{};
|
||||
std::copy(kLocalStateMagic.begin(), kLocalStateMagic.end(), state_packet.begin());
|
||||
state_packet[4] = kPacketVersion;
|
||||
WriteU64(state_packet.data() + 8, static_cast<std::uint64_t>(
|
||||
std::chrono::duration_cast<std::chrono::microseconds>(
|
||||
info.simTime)
|
||||
.count()));
|
||||
WriteU32(state_packet.data() + 16, static_cast<std::uint32_t>(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<float, 14> truth_ned{
|
||||
static_cast<float>(pose->Pos().Y() - origin.Y()),
|
||||
static_cast<float>(pose->Pos().X() - origin.X()),
|
||||
static_cast<float>(origin.Z() - pose->Pos().Z()),
|
||||
static_cast<float>(velocity->Y()),
|
||||
static_cast<float>(velocity->X()),
|
||||
static_cast<float>(-velocity->Z()),
|
||||
static_cast<float>(relative_attitude.W()),
|
||||
static_cast<float>(relative_attitude.X()),
|
||||
static_cast<float>(relative_attitude.Y()),
|
||||
static_cast<float>(relative_attitude.Z()),
|
||||
static_cast<float>(this->data_->applied_motor_fractions[0]),
|
||||
static_cast<float>(this->data_->applied_motor_fractions[1]),
|
||||
static_cast<float>(this->data_->applied_motor_fractions[2]),
|
||||
static_cast<float>(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<std::uint8_t, kTruthPacketSize> truth_packet{};
|
||||
std::copy(kTruthMagic.begin(), kTruthMagic.end(), truth_packet.begin());
|
||||
truth_packet[4] = kPacketVersion;
|
||||
WriteU64(truth_packet.data() + 8, static_cast<std::uint64_t>(
|
||||
std::chrono::duration_cast<std::chrono::microseconds>(
|
||||
info.simTime)
|
||||
.count()));
|
||||
WriteU32(truth_packet.data() + 16, static_cast<std::uint32_t>(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")
|
||||
File diff suppressed because it is too large.
Load diff
@@ -0,0 +1,360 @@
|
||||
//! Fixed-size local UDP records for Kaptan's host-only simulator harness.
|
||||
//!
|
||||
//! This transport is deliberately not MAVLink and not part of Kaptan's flight
|
||||
//! dialect. It is confined to one host network namespace and is versioned so
|
||||
//! malformed simulator traffic never becomes estimator or motor authority.
|
||||
|
||||
use kaptan_log::KaptanError;
|
||||
use std::net::SocketAddr;
|
||||
|
||||
pub const SENSOR_PORT: u16 = 14_660;
|
||||
pub const ACTUATOR_PORT: u16 = 14_661;
|
||||
pub const CONTROL_PORT: u16 = 14_662;
|
||||
pub const VERSION: u8 = 1;
|
||||
pub const IMU_MAGIC: [u8; 4] = *b"KSI1";
|
||||
pub const LOCAL_STATE_MAGIC: [u8; 4] = *b"KSV1";
|
||||
/// Gazebo simulation time, sent once for every unpaused simulation step.
|
||||
/// This is the sole time base for host-SITL freshness and command leases.
|
||||
pub const CLOCK_MAGIC: [u8; 4] = *b"KSC1";
|
||||
pub const OUTPUT_MAGIC: [u8; 4] = *b"KSO1";
|
||||
pub const GOAL_MAGIC: [u8; 4] = *b"KSG1";
|
||||
/// A host-SITL-only request to descend to a declared local landing point and
|
||||
/// revoke motor authority once the landing point has settled.
|
||||
pub const LAND_MAGIC: [u8; 4] = *b"KSD1";
|
||||
pub const GOAL_KEEPALIVE_MAGIC: [u8; 4] = *b"KSK1";
|
||||
pub const STATUS_QUERY_MAGIC: [u8; 4] = *b"KSQ1";
|
||||
pub const STATUS_MAGIC: [u8; 4] = *b"KSS1";
|
||||
pub const GOAL_STATUS_MAGIC: [u8; 4] = *b"KSL1";
|
||||
|
||||
pub const SENSOR_PACKET_SIZE: usize = 44;
|
||||
pub const CLOCK_PACKET_SIZE: usize = 16;
|
||||
pub const OUTPUT_PACKET_SIZE: usize = 40;
|
||||
pub const GOAL_PACKET_SIZE: usize = 24;
|
||||
pub const GOAL_KEEPALIVE_PACKET_SIZE: usize = 12;
|
||||
pub const STATUS_QUERY_PACKET_SIZE: usize = 8;
|
||||
pub const STATUS_PACKET_SIZE: usize = 20;
|
||||
pub const GOAL_STATUS_PACKET_SIZE: usize = 16;
|
||||
|
||||
pub const STATUS_ESTIMATOR_READY: u16 = 1 << 0;
|
||||
pub const STATUS_IMU_FRESH: u16 = 1 << 1;
|
||||
pub const STATUS_LOCAL_AIDING_FRESH: u16 = 1 << 2;
|
||||
pub const STATUS_OUTPUT_PERMITTED: u16 = 1 << 3;
|
||||
pub const STATUS_ACTUATOR_TRANSPORT: u16 = 1 << 4;
|
||||
#[derive(Clone, Copy, Debug, Eq, PartialEq, KaptanError)]
|
||||
pub enum PacketError {
|
||||
Length,
|
||||
Version,
|
||||
NonFinite,
|
||||
}
|
||||
|
||||
#[derive(Clone, Copy, Debug, PartialEq)]
|
||||
pub struct ImuPacket {
|
||||
pub time_microseconds: u64,
|
||||
pub sequence: u32,
|
||||
pub acceleration_frd_mps2: [f32; 3],
|
||||
pub angular_rate_frd_rad_s: [f32; 3],
|
||||
}
|
||||
|
||||
#[derive(Clone, Copy, Debug, PartialEq)]
|
||||
pub struct LocalStatePacket {
|
||||
pub time_microseconds: u64,
|
||||
pub sequence: u32,
|
||||
pub position_ned_metres: [f32; 3],
|
||||
pub velocity_ned_mps: [f32; 3],
|
||||
}
|
||||
|
||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
||||
pub struct ClockPacket {
|
||||
pub time_microseconds: u64,
|
||||
}
|
||||
|
||||
#[derive(Clone, Copy, Debug, PartialEq)]
|
||||
pub struct GoalPacket {
|
||||
pub goal_id: u32,
|
||||
pub north_metres: f32,
|
||||
pub east_metres: f32,
|
||||
pub up_metres: f32,
|
||||
}
|
||||
|
||||
/// Renews the command lease for one accepted goal without modifying it.
|
||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
||||
pub struct GoalKeepalivePacket {
|
||||
pub goal_id: u32,
|
||||
}
|
||||
|
||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
||||
#[repr(u8)]
|
||||
pub enum GoalLifecycle {
|
||||
Rejected = 0,
|
||||
Active = 1,
|
||||
Settled = 2,
|
||||
Landed = 3,
|
||||
}
|
||||
|
||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
||||
pub struct GoalStatusPacket {
|
||||
pub goal_id: u32,
|
||||
pub lifecycle: GoalLifecycle,
|
||||
}
|
||||
|
||||
pub fn decode_imu(bytes: &[u8]) -> Result<ImuPacket, PacketError> {
|
||||
let (time_microseconds, sequence, values) = decode_sensor(bytes, IMU_MAGIC)?;
|
||||
Ok(ImuPacket {
|
||||
time_microseconds,
|
||||
sequence,
|
||||
acceleration_frd_mps2: [values[0], values[1], values[2]],
|
||||
angular_rate_frd_rad_s: [values[3], values[4], values[5]],
|
||||
})
|
||||
}
|
||||
|
||||
pub fn decode_local_state(bytes: &[u8]) -> Result<LocalStatePacket, PacketError> {
|
||||
let (time_microseconds, sequence, values) = decode_sensor(bytes, LOCAL_STATE_MAGIC)?;
|
||||
Ok(LocalStatePacket {
|
||||
time_microseconds,
|
||||
sequence,
|
||||
position_ned_metres: [values[0], values[1], values[2]],
|
||||
velocity_ned_mps: [values[3], values[4], values[5]],
|
||||
})
|
||||
}
|
||||
|
||||
pub fn decode_clock(bytes: &[u8]) -> Result<ClockPacket, PacketError> {
|
||||
if bytes.len() != CLOCK_PACKET_SIZE {
|
||||
return Err(PacketError::Length);
|
||||
}
|
||||
if bytes[0..4] != CLOCK_MAGIC || bytes[4] != VERSION || bytes[5..8] != [0; 3] {
|
||||
return Err(PacketError::Version);
|
||||
}
|
||||
Ok(ClockPacket {
|
||||
time_microseconds: read_u64(&bytes[8..16]).ok_or(PacketError::Length)?,
|
||||
})
|
||||
}
|
||||
|
||||
pub fn decode_goal(bytes: &[u8]) -> Result<GoalPacket, PacketError> {
|
||||
decode_goal_with_magic(bytes, GOAL_MAGIC, false)
|
||||
}
|
||||
|
||||
/// Decodes a host-SITL landing request. Landing always targets the declared
|
||||
/// local ground plane (`up == 0`), rather than silently treating an arbitrary
|
||||
/// move goal as a landing operation.
|
||||
pub fn decode_landing(bytes: &[u8]) -> Result<GoalPacket, PacketError> {
|
||||
decode_goal_with_magic(bytes, LAND_MAGIC, true)
|
||||
}
|
||||
|
||||
fn decode_goal_with_magic(
|
||||
bytes: &[u8],
|
||||
magic: [u8; 4],
|
||||
require_ground_plane: bool,
|
||||
) -> Result<GoalPacket, PacketError> {
|
||||
if bytes.len() != GOAL_PACKET_SIZE {
|
||||
return Err(PacketError::Length);
|
||||
}
|
||||
if bytes[0..4] != magic || bytes[4] != VERSION || bytes[5..8] != [0; 3] {
|
||||
return Err(PacketError::Version);
|
||||
}
|
||||
let goal = GoalPacket {
|
||||
goal_id: read_u32(&bytes[8..12]).ok_or(PacketError::Length)?,
|
||||
north_metres: read_f32(&bytes[12..16]).ok_or(PacketError::Length)?,
|
||||
east_metres: read_f32(&bytes[16..20]).ok_or(PacketError::Length)?,
|
||||
up_metres: read_f32(&bytes[20..24]).ok_or(PacketError::Length)?,
|
||||
};
|
||||
if goal.goal_id == 0
|
||||
|| !goal.north_metres.is_finite()
|
||||
|| !goal.east_metres.is_finite()
|
||||
|| !goal.up_metres.is_finite()
|
||||
|| (require_ground_plane && goal.up_metres != 0.0)
|
||||
{
|
||||
return Err(PacketError::NonFinite);
|
||||
}
|
||||
Ok(goal)
|
||||
}
|
||||
|
||||
pub fn decode_goal_keepalive(bytes: &[u8]) -> Result<GoalKeepalivePacket, PacketError> {
|
||||
if bytes.len() != GOAL_KEEPALIVE_PACKET_SIZE {
|
||||
return Err(PacketError::Length);
|
||||
}
|
||||
if bytes[0..4] != GOAL_KEEPALIVE_MAGIC || bytes[4] != VERSION || bytes[5..8] != [0; 3] {
|
||||
return Err(PacketError::Version);
|
||||
}
|
||||
let goal_id = read_u32(&bytes[8..12]).ok_or(PacketError::Length)?;
|
||||
if goal_id == 0 {
|
||||
return Err(PacketError::NonFinite);
|
||||
}
|
||||
Ok(GoalKeepalivePacket { goal_id })
|
||||
}
|
||||
|
||||
pub fn is_status_query(bytes: &[u8]) -> bool {
|
||||
bytes.len() == STATUS_QUERY_PACKET_SIZE
|
||||
&& bytes[0..4] == STATUS_QUERY_MAGIC
|
||||
&& bytes[4] == VERSION
|
||||
&& bytes[5..8] == [0; 3]
|
||||
}
|
||||
|
||||
pub fn encode_output(
|
||||
time_microseconds: u64,
|
||||
sequence: u32,
|
||||
permitted: bool,
|
||||
motors: [f32; 4],
|
||||
) -> Result<[u8; OUTPUT_PACKET_SIZE], PacketError> {
|
||||
if motors
|
||||
.iter()
|
||||
.any(|motor| !motor.is_finite() || !(0.0..=1.0).contains(motor))
|
||||
|| (!permitted && motors.iter().any(|motor| *motor != 0.0))
|
||||
{
|
||||
return Err(PacketError::NonFinite);
|
||||
}
|
||||
let mut bytes = [0_u8; OUTPUT_PACKET_SIZE];
|
||||
bytes[0..4].copy_from_slice(&OUTPUT_MAGIC);
|
||||
bytes[4] = VERSION;
|
||||
bytes[8..16].copy_from_slice(&time_microseconds.to_le_bytes());
|
||||
bytes[16..20].copy_from_slice(&sequence.to_le_bytes());
|
||||
bytes[20] = u8::from(permitted);
|
||||
for (index, motor) in motors.iter().enumerate() {
|
||||
bytes[24 + index * 4..28 + index * 4].copy_from_slice(&motor.to_le_bytes());
|
||||
}
|
||||
Ok(bytes)
|
||||
}
|
||||
|
||||
pub fn encode_status(time_microseconds: u64, flags: u16) -> [u8; STATUS_PACKET_SIZE] {
|
||||
let mut bytes = [0_u8; STATUS_PACKET_SIZE];
|
||||
bytes[0..4].copy_from_slice(&STATUS_MAGIC);
|
||||
bytes[4] = VERSION;
|
||||
bytes[8..16].copy_from_slice(&time_microseconds.to_le_bytes());
|
||||
bytes[16..18].copy_from_slice(&flags.to_le_bytes());
|
||||
bytes
|
||||
}
|
||||
|
||||
pub fn encode_goal_status(status: GoalStatusPacket) -> [u8; GOAL_STATUS_PACKET_SIZE] {
|
||||
let mut bytes = [0_u8; GOAL_STATUS_PACKET_SIZE];
|
||||
bytes[0..4].copy_from_slice(&GOAL_STATUS_MAGIC);
|
||||
bytes[4] = VERSION;
|
||||
bytes[8..12].copy_from_slice(&status.goal_id.to_le_bytes());
|
||||
bytes[12] = status.lifecycle as u8;
|
||||
bytes
|
||||
}
|
||||
|
||||
pub fn localhost(sender: SocketAddr) -> bool {
|
||||
sender.ip().is_loopback()
|
||||
}
|
||||
|
||||
fn decode_sensor(bytes: &[u8], magic: [u8; 4]) -> Result<(u64, u32, [f32; 6]), PacketError> {
|
||||
if bytes.len() != SENSOR_PACKET_SIZE {
|
||||
return Err(PacketError::Length);
|
||||
}
|
||||
if bytes[0..4] != magic || bytes[4] != VERSION || bytes[5..8] != [0; 3] {
|
||||
return Err(PacketError::Version);
|
||||
}
|
||||
let mut values = [0.0; 6];
|
||||
for (index, value) in values.iter_mut().enumerate() {
|
||||
*value = read_f32(&bytes[20 + index * 4..24 + index * 4]).ok_or(PacketError::Length)?;
|
||||
}
|
||||
if values.iter().any(|value| !value.is_finite()) {
|
||||
return Err(PacketError::NonFinite);
|
||||
}
|
||||
Ok((
|
||||
read_u64(&bytes[8..16]).ok_or(PacketError::Length)?,
|
||||
read_u32(&bytes[16..20]).ok_or(PacketError::Length)?,
|
||||
values,
|
||||
))
|
||||
}
|
||||
|
||||
fn read_u32(bytes: &[u8]) -> Option<u32> {
|
||||
Some(u32::from_le_bytes(bytes.try_into().ok()?))
|
||||
}
|
||||
|
||||
fn read_u64(bytes: &[u8]) -> Option<u64> {
|
||||
Some(u64::from_le_bytes(bytes.try_into().ok()?))
|
||||
}
|
||||
|
||||
fn read_f32(bytes: &[u8]) -> Option<f32> {
|
||||
Some(f32::from_le_bytes(bytes.try_into().ok()?))
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
|
||||
fn sensor(magic: [u8; 4]) -> [u8; SENSOR_PACKET_SIZE] {
|
||||
let mut bytes = [0_u8; SENSOR_PACKET_SIZE];
|
||||
bytes[..4].copy_from_slice(&magic);
|
||||
bytes[4] = VERSION;
|
||||
bytes[8..16].copy_from_slice(&123_u64.to_le_bytes());
|
||||
bytes[16..20].copy_from_slice(&9_u32.to_le_bytes());
|
||||
for index in 0..6 {
|
||||
bytes[20 + index * 4..24 + index * 4]
|
||||
.copy_from_slice(&(index as f32 + 0.5).to_le_bytes());
|
||||
}
|
||||
bytes
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn sensor_records_are_finite() {
|
||||
let packet = decode_imu(&sensor(IMU_MAGIC)).unwrap();
|
||||
assert_eq!(packet.time_microseconds, 123);
|
||||
assert_eq!(packet.acceleration_frd_mps2, [0.5, 1.5, 2.5]);
|
||||
assert_eq!(packet.angular_rate_frd_rad_s, [3.5, 4.5, 5.5]);
|
||||
assert_eq!(
|
||||
decode_imu(&sensor(LOCAL_STATE_MAGIC)),
|
||||
Err(PacketError::Version)
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn simulator_clock_has_no_host_receipt_time() {
|
||||
let mut packet = [0_u8; CLOCK_PACKET_SIZE];
|
||||
packet[..4].copy_from_slice(&CLOCK_MAGIC);
|
||||
packet[4] = VERSION;
|
||||
packet[8..16].copy_from_slice(&987_654_u64.to_le_bytes());
|
||||
assert_eq!(
|
||||
decode_clock(&packet),
|
||||
Ok(ClockPacket {
|
||||
time_microseconds: 987_654,
|
||||
})
|
||||
);
|
||||
packet[4] = VERSION + 1;
|
||||
assert_eq!(decode_clock(&packet), Err(PacketError::Version));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn inhibited_output() {
|
||||
assert!(encode_output(1, 2, true, [0.1, 0.2, 0.3, 0.4]).is_ok());
|
||||
assert_eq!(
|
||||
encode_output(1, 2, false, [0.1, 0.0, 0.0, 0.0]),
|
||||
Err(PacketError::NonFinite)
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn goal_keepalive_validation() {
|
||||
let mut packet = [0_u8; GOAL_KEEPALIVE_PACKET_SIZE];
|
||||
packet[..4].copy_from_slice(&GOAL_KEEPALIVE_MAGIC);
|
||||
packet[4] = VERSION;
|
||||
packet[8..12].copy_from_slice(&7_u32.to_le_bytes());
|
||||
assert_eq!(
|
||||
decode_goal_keepalive(&packet),
|
||||
Ok(GoalKeepalivePacket { goal_id: 7 })
|
||||
);
|
||||
packet[8..12].copy_from_slice(&0_u32.to_le_bytes());
|
||||
assert_eq!(decode_goal_keepalive(&packet), Err(PacketError::NonFinite));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn landing_is_an_explicit_ground_plane_command() {
|
||||
let mut packet = [0_u8; GOAL_PACKET_SIZE];
|
||||
packet[..4].copy_from_slice(&LAND_MAGIC);
|
||||
packet[4] = VERSION;
|
||||
packet[8..12].copy_from_slice(&7_u32.to_le_bytes());
|
||||
packet[12..16].copy_from_slice(&1.0_f32.to_le_bytes());
|
||||
packet[16..20].copy_from_slice(&(-2.0_f32).to_le_bytes());
|
||||
assert_eq!(
|
||||
decode_landing(&packet),
|
||||
Ok(GoalPacket {
|
||||
goal_id: 7,
|
||||
north_metres: 1.0,
|
||||
east_metres: -2.0,
|
||||
up_metres: 0.0,
|
||||
})
|
||||
);
|
||||
packet[20..24].copy_from_slice(&0.1_f32.to_le_bytes());
|
||||
assert_eq!(decode_landing(&packet), Err(PacketError::NonFinite));
|
||||
}
|
||||
}
|
||||
@@ -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()
|
||||
@@ -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")
|
||||
@@ -0,0 +1,204 @@
|
||||
<?xml version="1.0" ?>
|
||||
<sdf version="1.10">
|
||||
<world name="kaptan_sitl">
|
||||
<gravity>0 0 -9.80665</gravity>
|
||||
<physics name="dart" type="dart">
|
||||
<max_step_size>0.0025</max_step_size>
|
||||
<real_time_factor>1.0</real_time_factor>
|
||||
<real_time_update_rate>400</real_time_update_rate>
|
||||
</physics>
|
||||
|
||||
<plugin filename="gz-sim-physics-system" name="gz::sim::systems::Physics"/>
|
||||
<plugin filename="gz-sim-user-commands-system" name="gz::sim::systems::UserCommands"/>
|
||||
<plugin filename="gz-sim-scene-broadcaster-system" name="gz::sim::systems::SceneBroadcaster"/>
|
||||
<plugin filename="gz-sim-sensors-system" name="gz::sim::systems::Sensors">
|
||||
<render_engine>ogre2</render_engine>
|
||||
</plugin>
|
||||
<plugin filename="gz-sim-imu-system" name="gz::sim::systems::Imu"/>
|
||||
<plugin filename="gz-sim-websocket-server-system" name="gz::sim::systems::WebsocketServer">
|
||||
<port>9002</port>
|
||||
<publication_hz>30</publication_hz>
|
||||
</plugin>
|
||||
|
||||
<light type="directional" name="sun">
|
||||
<cast_shadows>true</cast_shadows>
|
||||
<pose>0 0 10 0 0 0</pose>
|
||||
<diffuse>0.8 0.8 0.8 1</diffuse>
|
||||
<specular>0.2 0.2 0.2 1</specular>
|
||||
<direction>-0.5 0.1 -0.9</direction>
|
||||
</light>
|
||||
<scene>
|
||||
<ambient>0.35 0.38 0.42 1</ambient>
|
||||
<background>0.08 0.11 0.16 1</background>
|
||||
<shadows>true</shadows>
|
||||
</scene>
|
||||
|
||||
<!-- Visual-only launch-local reference map. It deliberately has no
|
||||
collision geometry: the closed-loop model receives IMU plus a direct
|
||||
noisy local-state sensor, not vision, terrain, range, or flow. The
|
||||
pad makes the landing point legible without claiming a terrain or
|
||||
obstacle model that the estimator does not observe. -->
|
||||
<model name="sitl_reference_map">
|
||||
<static>true</static>
|
||||
<link name="reference_link">
|
||||
<visual name="landing_pad">
|
||||
<pose>0 0 0.002 0 0 0</pose>
|
||||
<geometry><cylinder><radius>1.25</radius><length>0.004</length></cylinder></geometry>
|
||||
<material>
|
||||
<ambient>0.03 0.20 0.36 1</ambient><diffuse>0.05 0.36 0.65 1</diffuse>
|
||||
<emissive>0.01 0.08 0.14 1</emissive>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
</model>
|
||||
|
||||
<model name="ground">
|
||||
<static>true</static>
|
||||
<link name="ground_link">
|
||||
<collision name="ground_collision">
|
||||
<geometry><plane><normal>0 0 1</normal><size>100 100</size></plane></geometry>
|
||||
</collision>
|
||||
<visual name="ground_visual">
|
||||
<geometry><plane><normal>0 0 1</normal><size>100 100</size></plane></geometry>
|
||||
<material><ambient>0.2 0.2 0.2 1</ambient><diffuse>0.4 0.4 0.4 1</diffuse></material>
|
||||
</visual>
|
||||
</link>
|
||||
</model>
|
||||
|
||||
<!-- This is a fixed simulated vehicle. Its M1-M4 mapping is defined in
|
||||
the plugin, not inferred from any board timer. Parameters below are a
|
||||
declared SITL model and require vehicle-specific validation before a
|
||||
physical-flight claim.
|
||||
|
||||
TODO(configuration): replace these fixed values with a reviewed,
|
||||
versioned vehicle profile selected by the ground configuration flow.
|
||||
Until then this world is only the declared Kaptan test vehicle. -->
|
||||
<model name="kaptan_sitl_quad">
|
||||
<!-- ENU/FLU Gazebo is yawed once so Kaptan's FRD body axes align with
|
||||
its NED local frame at the stationary-initializer origin. -->
|
||||
<!-- Four 80 mm landing feet support the base_link at z=0.14 m. The
|
||||
launch-local NED origin is captured only after this physical rest
|
||||
pose is established; it is not a hidden airborne hover point. -->
|
||||
<pose>0 0 0.14 0 0 1.5707963268</pose>
|
||||
<link name="base_link">
|
||||
<inertial>
|
||||
<mass>1.2</mass>
|
||||
<inertia>
|
||||
<ixx>0.021</ixx><iyy>0.021</iyy><izz>0.039</izz>
|
||||
<ixy>0</ixy><ixz>0</ixz><iyz>0</iyz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
<collision name="body_collision">
|
||||
<geometry><box><size>0.30 0.30 0.08</size></box></geometry>
|
||||
</collision>
|
||||
<visual name="body_visual">
|
||||
<geometry><box><size>0.30 0.30 0.08</size></box></geometry>
|
||||
<material><ambient>0.05 0.1 0.2 1</ambient><diffuse>0.1 0.2 0.5 1</diffuse></material>
|
||||
</visual>
|
||||
<collision name="landing_foot_front_right_collision">
|
||||
<pose>0.12 -0.12 -0.10 0 0 0</pose>
|
||||
<geometry><cylinder><radius>0.022</radius><length>0.08</length></cylinder></geometry>
|
||||
</collision>
|
||||
<visual name="landing_foot_front_right_visual">
|
||||
<pose>0.12 -0.12 -0.10 0 0 0</pose>
|
||||
<geometry><cylinder><radius>0.022</radius><length>0.08</length></cylinder></geometry>
|
||||
<material><ambient>0.07 0.07 0.08 1</ambient><diffuse>0.12 0.12 0.14 1</diffuse></material>
|
||||
</visual>
|
||||
<collision name="landing_foot_rear_right_collision">
|
||||
<pose>-0.12 -0.12 -0.10 0 0 0</pose>
|
||||
<geometry><cylinder><radius>0.022</radius><length>0.08</length></cylinder></geometry>
|
||||
</collision>
|
||||
<visual name="landing_foot_rear_right_visual">
|
||||
<pose>-0.12 -0.12 -0.10 0 0 0</pose>
|
||||
<geometry><cylinder><radius>0.022</radius><length>0.08</length></cylinder></geometry>
|
||||
<material><ambient>0.07 0.07 0.08 1</ambient><diffuse>0.12 0.12 0.14 1</diffuse></material>
|
||||
</visual>
|
||||
<collision name="landing_foot_rear_left_collision">
|
||||
<pose>-0.12 0.12 -0.10 0 0 0</pose>
|
||||
<geometry><cylinder><radius>0.022</radius><length>0.08</length></cylinder></geometry>
|
||||
</collision>
|
||||
<visual name="landing_foot_rear_left_visual">
|
||||
<pose>-0.12 0.12 -0.10 0 0 0</pose>
|
||||
<geometry><cylinder><radius>0.022</radius><length>0.08</length></cylinder></geometry>
|
||||
<material><ambient>0.07 0.07 0.08 1</ambient><diffuse>0.12 0.12 0.14 1</diffuse></material>
|
||||
</visual>
|
||||
<collision name="landing_foot_front_left_collision">
|
||||
<pose>0.12 0.12 -0.10 0 0 0</pose>
|
||||
<geometry><cylinder><radius>0.022</radius><length>0.08</length></cylinder></geometry>
|
||||
</collision>
|
||||
<visual name="landing_foot_front_left_visual">
|
||||
<pose>0.12 0.12 -0.10 0 0 0</pose>
|
||||
<geometry><cylinder><radius>0.022</radius><length>0.08</length></cylinder></geometry>
|
||||
<material><ambient>0.07 0.07 0.08 1</ambient><diffuse>0.12 0.12 0.14 1</diffuse></material>
|
||||
</visual>
|
||||
<visual name="arm_front_right">
|
||||
<pose>0.085 -0.085 0 0 0 -0.7853981634</pose>
|
||||
<geometry><box><size>0.30 0.025 0.02</size></box></geometry>
|
||||
</visual>
|
||||
<visual name="arm_rear_right">
|
||||
<pose>-0.085 -0.085 0 0 0 0.7853981634</pose>
|
||||
<geometry><box><size>0.30 0.025 0.02</size></box></geometry>
|
||||
</visual>
|
||||
<visual name="arm_rear_left">
|
||||
<pose>-0.085 0.085 0 0 0 -0.7853981634</pose>
|
||||
<geometry><box><size>0.30 0.025 0.02</size></box></geometry>
|
||||
</visual>
|
||||
<visual name="arm_front_left">
|
||||
<pose>0.085 0.085 0 0 0 0.7853981634</pose>
|
||||
<geometry><box><size>0.30 0.025 0.02</size></box></geometry>
|
||||
</visual>
|
||||
<visual name="m1">
|
||||
<pose>0.17 -0.17 0.05 0 0 0</pose>
|
||||
<geometry><cylinder><radius>0.10</radius><length>0.01</length></cylinder></geometry>
|
||||
<material><ambient>0.7 0.1 0.1 1</ambient><diffuse>0.7 0.1 0.1 1</diffuse></material>
|
||||
</visual>
|
||||
<visual name="m2">
|
||||
<pose>-0.17 -0.17 0.05 0 0 0</pose>
|
||||
<geometry><cylinder><radius>0.10</radius><length>0.01</length></cylinder></geometry>
|
||||
<material><ambient>0.1 0.7 0.1 1</ambient><diffuse>0.1 0.7 0.1 1</diffuse></material>
|
||||
</visual>
|
||||
<visual name="m3">
|
||||
<pose>-0.17 0.17 0.05 0 0 0</pose>
|
||||
<geometry><cylinder><radius>0.10</radius><length>0.01</length></cylinder></geometry>
|
||||
<material><ambient>0.1 0.1 0.7 1</ambient><diffuse>0.1 0.1 0.7 1</diffuse></material>
|
||||
</visual>
|
||||
<visual name="m4">
|
||||
<pose>0.17 0.17 0.05 0 0 0</pose>
|
||||
<geometry><cylinder><radius>0.10</radius><length>0.01</length></cylinder></geometry>
|
||||
<material><ambient>0.7 0.7 0.1 1</ambient><diffuse>0.7 0.7 0.1 1</diffuse></material>
|
||||
</visual>
|
||||
<sensor name="imu" type="imu">
|
||||
<always_on>true</always_on>
|
||||
<update_rate>100</update_rate>
|
||||
<topic>/kaptan/sitl/imu</topic>
|
||||
<imu>
|
||||
<angular_velocity>
|
||||
<x><noise type="gaussian"><mean>0</mean><stddev>0.002</stddev></noise></x>
|
||||
<y><noise type="gaussian"><mean>0</mean><stddev>0.002</stddev></noise></y>
|
||||
<z><noise type="gaussian"><mean>0</mean><stddev>0.002</stddev></noise></z>
|
||||
</angular_velocity>
|
||||
<linear_acceleration>
|
||||
<x><noise type="gaussian"><mean>0</mean><stddev>0.03</stddev></noise></x>
|
||||
<y><noise type="gaussian"><mean>0</mean><stddev>0.03</stddev></noise></y>
|
||||
<z><noise type="gaussian"><mean>0</mean><stddev>0.03</stddev></noise></z>
|
||||
</linear_acceleration>
|
||||
</imu>
|
||||
</sensor>
|
||||
</link>
|
||||
<plugin filename="libkaptan_sitl_quad.so" name="kaptan::sitl::Quad">
|
||||
<imu_topic>/kaptan/sitl/imu</imu_topic>
|
||||
<sensor_udp_port>14660</sensor_udp_port>
|
||||
<actuator_udp_port>14661</actuator_udp_port>
|
||||
<truth_udp_port>14663</truth_udp_port>
|
||||
<operator_clock_udp_port>14664</operator_clock_udp_port>
|
||||
<actuator_timeout_usec>25000</actuator_timeout_usec>
|
||||
<maximum_motor_thrust_newtons>4.5</maximum_motor_thrust_newtons>
|
||||
<motor_time_constant_seconds>0.03</motor_time_constant_seconds>
|
||||
<local_position_noise_stddev_m>0.10</local_position_noise_stddev_m>
|
||||
<local_velocity_noise_stddev_mps>0.05</local_velocity_noise_stddev_mps>
|
||||
<local_state_noise_seed>18181</local_state_noise_seed>
|
||||
<reaction_torque_per_newton_metre>0.012</reaction_torque_per_newton_metre>
|
||||
</plugin>
|
||||
</model>
|
||||
</world>
|
||||
</sdf>
|
||||
Reference in new issue
Block a user