sitl: add Gazebo plant and smoke harness
This commit is contained in:
Onur Özkan committed
1 parent
b5bdb86ba1
commit
060e540635
11 files changed
+1796
No files matched your search
@@ -0,0 +1,3 @@
|
||||
target
|
||||
.git
|
||||
tools/hitl
|
||||
@@ -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,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,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")
|
||||
@@ -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