build: add directory-oriented make workflows
Host SITL smoke / Deterministic Gazebo command path (push) Has been cancelled
Host SITL smoke / Deterministic Gazebo command path (push) Has been cancelled
This commit is contained in:
1 parent
4bc98adbc6
commit
7dc3ccb619
40 files changed
+1367
-278
No files matched your search
@@ -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":
|
||||
|
||||
Reference in new issue
Block a user