build: add directory-oriented make workflows
Host SITL smoke / Deterministic Gazebo command path (push) Has been cancelled

This commit is contained in:
Onur Özkan committed 2026-08-30 07:05:01 +03:00
1 parent 4bc98adbc6
commit 7dc3ccb619
40 files changed
+1367 -278

No files matched your search

+42 -17
View File
@@ -50,9 +50,13 @@ 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 = 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 = 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)
@@ -78,9 +82,15 @@ def arguments() -> argparse.Namespace:
"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(
"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",
@@ -122,7 +132,9 @@ def latest_simulation_time(
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")
raise RuntimeError(
"Gazebo simulation clock reset; restart the command in the new epoch"
)
latest = simulation_time
@@ -145,7 +157,10 @@ def wait_ready(socket_: socket.socket, timeout: float) -> None:
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")
raise RuntimeError(
"host SITL is not running; run docker compose -f "
"firmware/tools/sitl/compose.yaml up -d --build from the repository root"
)
names = (
(ESTIMATOR_READY, "estimator initialization"),
(IMU_FRESH, "fresh simulator IMU"),
@@ -174,10 +189,7 @@ def truth_sample_within_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
)
or (require_ground_plane and ground_plane_error > GROUND_PLANE_TOLERANCE_METRES)
)
@@ -217,11 +229,16 @@ def verify_truth(
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)
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))
payload = recv(
truth_socket, min(acquisition_deadline, time.monotonic() + 0.01)
)
if payload is None or len(payload) != TRUTH_PACKET.size:
continue
(
@@ -261,8 +278,12 @@ def verify_truth(
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))
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)
@@ -449,7 +470,9 @@ def command_goal(
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")
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")
@@ -512,7 +535,9 @@ def main() -> int:
or not math.isfinite(values.truth_hold_seconds)
)
if tolerances:
raise ValueError("truth tolerances and hold duration must be finite and positive")
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(USER_CONTROL)
if values.command == "ready":