From f635aa27d535ea6771225e0115c80ef2cfeb85c5 Mon Sep 17 00:00:00 2001 From: Florian-S7 Date: Tue, 23 Jun 2026 01:27:44 +0200 Subject: [PATCH 1/2] Add Lighthouse positioning support for the real drone (single drone) Adds an opt-in lighthouse mode so the real environment can localize the drone from its onboard Lighthouse state estimate instead of a motion capture system. Instead of pushing external poses to the drone, the onboard estimate is read back via two cflib2 log blocks (pos+vel, orientation+gyro; split to respect the CRTP log packet size) by a background reader, decoded into the simulation observation (float32 pos, quat xyzw, vel, ang_vel) by the pure, unit-tested utils.lighthouse.decode_state. The mocap path is unchanged and remains the default; lighthouse is enabled via a [deploy].lighthouse config flag plumbed through deploy.py. In lighthouse mode the deck is verified (deck.bcLighthouse4), no ROS connection is created, external poses are not pushed, the estimator converges on its own, and real_track_objects must be false (no tracker to measure the gates/obstacles). Lighthouse is single-drone only (each process can only read its own drone). Adds unit tests for decode_state and the CRTP block sizing. Co-Authored-By: Claude Opus 4.8 --- config/level2.toml | 4 + lsy_drone_racing/envs/real_race_env.py | 97 ++++++++++--- lsy_drone_racing/utils/crazyflie.py | 188 ++++++++++++++++++++++--- lsy_drone_racing/utils/lighthouse.py | 85 +++++++++++ scripts/deploy.py | 1 + tests/unit/utils/test_lighthouse.py | 63 +++++++++ 6 files changed, 403 insertions(+), 35 deletions(-) create mode 100644 lsy_drone_racing/utils/lighthouse.py create mode 100644 tests/unit/utils/test_lighthouse.py diff --git a/config/level2.toml b/config/level2.toml index b14a0c6b6..48705e636 100644 --- a/config/level2.toml +++ b/config/level2.toml @@ -14,6 +14,10 @@ check_race_track = true check_drone_start_pos = true # Lets you practice your controller without putting up gates & obstacles, assumes nominal positions given below. real_track_objects = true +# Use the onboard Lighthouse positioning estimate instead of a motion capture system. Requires a +# Lighthouse deck + base stations, a single drone, and real_track_objects = false (no tracker can +# measure the gates/obstacles). Defaults to false (motion capture). +lighthouse = false [[deploy.drones]] id = 10 diff --git a/lsy_drone_racing/envs/real_race_env.py b/lsy_drone_racing/envs/real_race_env.py index 3ed2d3327..1a31b9ee4 100644 --- a/lsy_drone_racing/envs/real_race_env.py +++ b/lsy_drone_racing/envs/real_race_env.py @@ -76,6 +76,7 @@ def __init__( randomizations: ConfigDict, sensor_range: float = 0.5, control_mode: Literal["state", "attitude"] = "state", + lighthouse: bool = False, ): """Create a deployable version of the drone racing environment. @@ -88,10 +89,19 @@ def __init__( sensor_range: Sensor range. Determines at which distance the exact position of the gates and obstacles is reveiled. control_mode: Control mode of the drone. + lighthouse: If True, localize the drone with its onboard Lighthouse estimate read back + from the drone instead of a motion capture system (no ROS, single drone only). """ - assert rclpy.ok(), "ROS2 is not running. Please start ROS2 before creating a deploy env." + self.lighthouse = lighthouse + if not lighthouse: + assert rclpy.ok(), "ROS2 is not running. Start it before creating a deploy env." # Static env data self.n_drones = len(drones) + if lighthouse and self.n_drones > 1: + raise NotImplementedError( + "Lighthouse mode is only supported for a single drone: each process can only read " + "its own drone's onboard estimate, with no shared tracker for the others." + ) self.gates, self.obstacles, self.drones = load_track(track) self.n_gates = len(self.gates.pos) self.n_obstacles = len(self.obstacles.pos) @@ -113,8 +123,11 @@ def __init__( radio_channel=drone_config["channel"], drone_id=drone_config["id"], drone_name=self.drone_name, + lighthouse=lighthouse, + ) + self._ros_connector = ( + None if lighthouse else ROSConnector(estimator_names=self.drone_names, timeout=10.0) ) - self._ros_connector = ROSConnector(estimator_names=self.drone_names, timeout=10.0) # Dynamic data self.data = EnvData.create( n_drones=self.n_drones, n_gates=self.n_gates, n_obstacles=self.n_obstacles @@ -127,6 +140,12 @@ def _reset(self, *, seed: int | None = None, options: dict | None = None) -> tup # Update the position of gates and obstacles with the real positions measured from Mocap. If # disabled, they are equal to the nominal positions defined in the track config. if options.get("real_track_objects", True): + if self.lighthouse: + raise ValueError( + "real_track_objects=True is not supported in lighthouse mode (no tracker " + "to measure the gates/obstacles). Place the track at the configured " + "coordinates and set real_track_objects=False." + ) self._update_track_poses() if options.get("check_race_track", True): check_race_track( @@ -138,17 +157,33 @@ def _reset(self, *, seed: int | None = None, options: dict | None = None) -> tup nominal_obstacles_pos=self.obstacles.nominal_pos, rng_config=self.randomizations, ) - if options.get("check_drone_start_pos", True): - check_drone_start_pos( - nominal_pos=self.drones.pos[self.rank], - real_pos=self._ros_connector.pos[self.drone_name], - rng_config=self.randomizations, - drone_name=self.drone_name, - ) - self.data.reset(np.stack([self._ros_connector.pos[n] for n in self.drone_names])) - self.drone.connect(timeout=10.0) - self.drone.reset(arm=True) + if self.lighthouse: + # The onboard estimate is only available after the drone is connected and its estimator + # has converged, so bring it up first and arm only once the start position checks out. + self.drone.connect(timeout=10.0) + self.drone.reset(arm=False) + drone_pos, *_ = self._drone_states() + if options.get("check_drone_start_pos", True): + check_drone_start_pos( + nominal_pos=self.drones.pos[self.rank], + real_pos=drone_pos[self.rank], + rng_config=self.randomizations, + drone_name=self.drone_name, + ) + self.data.reset(drone_pos) + self.drone.arm() + else: + if options.get("check_drone_start_pos", True): + check_drone_start_pos( + nominal_pos=self.drones.pos[self.rank], + real_pos=self._ros_connector.pos[self.drone_name], + rng_config=self.randomizations, + drone_name=self.drone_name, + ) + self.data.reset(np.stack([self._ros_connector.pos[n] for n in self.drone_names])) + self.drone.connect(timeout=10.0) + self.drone.reset(arm=True) self._last_drone_pos_update = 0 # Last time a position was sent to the drone estimator return self.obs(), self.info() @@ -164,9 +199,8 @@ def _step(self, action: NDArray) -> tuple[dict, float, bool, bool, dict]: action[:3], action[3:6], action[6:9], action[9], action[10:] ) - drone_pos = np.stack([self._ros_connector.pos[drone] for drone in self.drone_names]) + drone_pos, drone_quat, _, _ = self._drone_states() assert drone_pos.dtype == np.float32, "Drone position must be of type float32" - drone_quat = np.stack([self._ros_connector.quat[drone] for drone in self.drone_names]) assert drone_quat.dtype == np.float32, "Drone quaternion must be of type float32" # Check if the drone is in the sensor range of the gates and obstacles dpos = drone_pos[:, None, :2] - self.gates.pos[None, :, :2] @@ -192,6 +226,27 @@ def _step(self, action: NDArray) -> tuple[dict, float, bool, bool, dict]: self._last_drone_pos_update = t return self.obs(), self.reward(), self.terminated(), self.truncated(), self.info() + def _drone_states(self) -> tuple[NDArray, NDArray, NDArray, NDArray]: + """Return stacked (pos, quat, vel, ang_vel) for all drones from the active source. + + Uses the onboard Lighthouse estimate in lighthouse mode, otherwise the motion capture + system via ROS. All arrays are float32 to match the observation space. + """ + if self.lighthouse: + state = self.drone.get_obs() + return ( + state["pos"][None, ...], + state["quat"][None, ...], + state["vel"][None, ...], + state["ang_vel"][None, ...], + ) + rc = self._ros_connector + pos = np.stack([rc.pos[drone] for drone in self.drone_names]) + quat = np.stack([rc.quat[drone] for drone in self.drone_names]) + vel = np.stack([rc.vel[drone] for drone in self.drone_names]) + ang_vel = np.stack([rc.ang_vel[drone] for drone in self.drone_names]) + return pos, quat, vel, ang_vel + def obs(self) -> dict[str, NDArray]: """Return the observation of the environment.""" # If gates/obstacles are in sensor range use the actual pose, otherwise use the nominal pose @@ -204,10 +259,7 @@ def obs(self) -> dict[str, NDArray]: obstacles_pos = np.where(mask, self.obstacles.pos, self.obstacles.nominal_pos).astype( np.float32 ) - drone_pos = np.stack([self._ros_connector.pos[drone] for drone in self.drone_names]) - drone_quat = np.stack([self._ros_connector.quat[drone] for drone in self.drone_names]) - drone_vel = np.stack([self._ros_connector.vel[drone] for drone in self.drone_names]) - drone_ang_vel = np.stack([self._ros_connector.ang_vel[drone] for drone in self.drone_names]) + drone_pos, drone_quat, drone_vel, drone_ang_vel = self._drone_states() obs = { "pos": drone_pos, "quat": drone_quat, @@ -311,7 +363,8 @@ def close(self): self.drone.close(emergency_stop=True) finally: # Close all ROS connections - self._ros_connector.close() + if self._ros_connector is not None: + self._ros_connector.close() # region Single Drone Env @@ -346,6 +399,7 @@ def __init__( randomizations: ConfigDict, sensor_range: float = 0.5, control_mode: Literal["state", "attitude"] = "state", + lighthouse: bool = False, ): """Initialize the multi-drone environment. @@ -371,6 +425,7 @@ def __init__( sensor_range: Sensor range. Determines at which distance the exact position of the gates and obstacles is reveiled. control_mode: Control mode of the drone. + lighthouse: Use the onboard Lighthouse estimate instead of motion capture. """ super().__init__( drones=drones, @@ -380,6 +435,7 @@ def __init__( randomizations=randomizations, sensor_range=sensor_range, control_mode=control_mode, + lighthouse=lighthouse, ) def reset(self, *, seed: int | None = None, options: dict | None = None) -> tuple[dict, dict]: @@ -450,6 +506,7 @@ def __init__( randomizations: ConfigDict, sensor_range: float = 0.5, control_mode: Literal["state", "attitude"] = "state", + lighthouse: bool = False, ): """Initialize the multi-drone environment. @@ -462,6 +519,7 @@ def __init__( sensor_range: Sensor range. Determines at which distance the exact position of the gates and obstacles is reveiled. control_mode: Control mode of the drone. + lighthouse: Use the onboard Lighthouse estimate instead of motion capture. """ super().__init__( drones=drones, @@ -471,6 +529,7 @@ def __init__( randomizations=randomizations, sensor_range=sensor_range, control_mode=control_mode, + lighthouse=lighthouse, ) def reset(self, *, seed: int | None = None, options: dict | None = None) -> tuple[dict, dict]: diff --git a/lsy_drone_racing/utils/crazyflie.py b/lsy_drone_racing/utils/crazyflie.py index ce742fedb..64bcb5e06 100644 --- a/lsy_drone_racing/utils/crazyflie.py +++ b/lsy_drone_racing/utils/crazyflie.py @@ -4,6 +4,8 @@ import asyncio import logging +import threading +import time from pathlib import Path from typing import TYPE_CHECKING, Any, Literal @@ -16,8 +18,16 @@ from drone_models.transform import force2pwm from scipy.spatial.transform import Rotation as R +from lsy_drone_racing.utils.lighthouse import ( + LIGHTHOUSE_DECK_PARAM, + ORI_RATE_VARS, + POS_VEL_VARS, + decode_state, +) + if TYPE_CHECKING: from collections.abc import Awaitable, Callable + from concurrent.futures import Future from numpy.typing import NDArray @@ -26,6 +36,10 @@ __all__ = ["Crazyflie"] _POWER_CYCLE_BOOT_WAIT = 3.0 # 3 seconds is sufficient for a reboot +# Argument passed to cflib2's log block ``start`` (matches swarmGPT usage). The resulting onboard +# log rate should be verified on hardware; see the Lighthouse implementation plan. +_STATE_LOG_START_ARG = 10 +_STATE_LOG_FIRST_SAMPLE_TIMEOUT = 5.0 # seconds to wait for the first lighthouse sample class Crazyflie: @@ -33,6 +47,10 @@ class Crazyflie: The environment owns ROS and observation assembly. This class owns only the Crazyflie radio link, firmware parameters, command streaming, external-pose injection, and shutdown. + + In lighthouse mode the drone localizes itself from the Lighthouse base stations. We then read + the onboard state estimate back from the drone (see :meth:`get_obs`) instead of using ROS, and + do not inject external poses. """ def __init__( @@ -41,6 +59,7 @@ def __init__( drone_name: str, cache_dir: str | Path | None = None, power_cycle_on_connect: bool = True, + lighthouse: bool = False, ): """Create a Crazyflie wrapper. @@ -49,21 +68,36 @@ def __init__( drone_name: Name of the drone in ROS, e.g. cf10. cache_dir: Directory used for cflib2 TOC caching. power_cycle_on_connect: Whether to power-cycle the STM32 domain before connecting. + lighthouse: If True, use the onboard Lighthouse state estimate (read back from the + drone) instead of an external motion capture system. No ROS connection is created + and external poses are not pushed to the drone. """ self.uri = uri self.drone_name = drone_name self.power_cycle_on_connect = power_cycle_on_connect + self.lighthouse = lighthouse self.context = LinkContext() cache_dir = Path(__file__).parent / ".cache" if cache_dir is None else Path(cache_dir) self.toc_cache = FileTocCache(str(cache_dir)) - self._ros_connector = ROSConnector( - tf_names=[self.drone_name], cmd_topic=f"/drones/{self.drone_name}/command", timeout=10.0 + self._ros_connector = ( + None + if lighthouse + else ROSConnector( + tf_names=[self.drone_name], + cmd_topic=f"/drones/{self.drone_name}/command", + timeout=10.0, + ) ) self._cf: CflibCrazyflie | None = None self._commander_level: Literal["low", "high"] | None = None self._state_setpoint_fallback_warned = False self._loop = asyncio.new_event_loop() + # Lighthouse state-log reader (started in reset() when lighthouse is enabled). + self._loop_thread: threading.Thread | None = None + self._log_stop: threading.Event | None = None + self._log_future: Future[None] | None = None + self._latest_state: dict[str, NDArray[np.floating]] | None = None @classmethod def from_radio( @@ -74,6 +108,7 @@ def from_radio( drone_name: str | None = None, cache_dir: str | Path | None = None, power_cycle_on_connect: bool = True, + lighthouse: bool = False, ) -> Crazyflie: """Create a Crazyflie wrapper from deployment radio settings.""" return cls( @@ -81,6 +116,7 @@ def from_radio( f"cf{drone_id}" if drone_name is None else drone_name, cache_dir=cache_dir, power_cycle_on_connect=power_cycle_on_connect, + lighthouse=lighthouse, ) @property @@ -101,14 +137,37 @@ def reset(self, arm: bool = False) -> None: """Apply race settings, reset the estimator, and optionally arm the drone.""" self._run(self._apply_settings) self._run(self._reset_estimator) + if self.lighthouse: + self._start_state_log() if arm: - self._run(self._arm) - self._run(self._unlock_thrust) + self.arm() + + def arm(self) -> None: + """Arm the drone and unlock thrust for low-level setpoints.""" + self._run(self._arm) + self._run(self._unlock_thrust) def send_external_pose(self) -> None: - """Send an external mocap pose to the Crazyflie estimator.""" + """Send an external mocap pose to the Crazyflie estimator (no-op in lighthouse mode).""" + if self.lighthouse: + return self._run(self._send_external_pose) + def get_obs(self) -> dict[str, NDArray[np.floating]]: + """Return the latest onboard state estimate (lighthouse mode only). + + Returns: + A dictionary with ``float32`` ``pos``, ``quat`` (xyzw), ``vel`` and ``ang_vel``, read + from the drone's onboard estimator by the background state-log reader. The fields match + the per-drone simulation observation. + """ + if not self.lighthouse: + raise RuntimeError("get_obs() is only available in lighthouse mode.") + state = self._latest_state + if state is None: + raise RuntimeError("No lighthouse state available yet. Call reset() first.") + return state + def send_action_attitude( self, attitude: NDArray[np.floating], @@ -121,7 +180,7 @@ def send_action_attitude( pwm = np.clip(pwm, drone_parameters["pwm_min"], drone_parameters["pwm_max"]) command = (*np.rad2deg(attitude), int(pwm)) self._run(self._send_attitude_setpoint, *command) - if publish_to_ros: + if publish_to_ros and self._ros_connector is not None: self._ros_connector.publish_cmd(command) def send_action_state( @@ -205,6 +264,7 @@ def close(self, emergency_stop: bool = True) -> None: if self._loop.is_closed(): return try: + self._stop_state_log() if emergency_stop and self.is_connected: self._run(self._emergency_stop) self._run(asyncio.sleep, 0.1) @@ -213,15 +273,106 @@ def close(self, emergency_stop: bool = True) -> None: self._run(self._disconnect) finally: try: - self._ros_connector.close() + if self._ros_connector is not None: + self._ros_connector.close() finally: + self._stop_loop_thread() self._loop.close() def _run(self, operation: Callable[..., Awaitable[Any]], *args: Any, **kwargs: Any) -> Any: - """Run an asynchronous operation on this drone's event loop.""" + """Run an asynchronous operation on this drone's event loop. + + In lighthouse mode the event loop is driven by a background thread (for the state-log + reader), so coroutines are scheduled onto it thread-safely. Otherwise the loop is driven + directly via ``run_until_complete``. + """ if self._loop.is_closed(): raise RuntimeError("Crazyflie wrapper is already closed.") - return self._loop.run_until_complete(operation(*args, **kwargs)) + coro = operation(*args, **kwargs) + if self._loop_thread is not None: + return asyncio.run_coroutine_threadsafe(coro, self._loop).result() + return self._loop.run_until_complete(coro) + + # region Lighthouse state log + + def _start_state_log(self) -> None: + """Start the background reader that streams the onboard state estimate.""" + if self._loop_thread is not None: + return + self._latest_state = None + self._log_stop = threading.Event() + self._loop_thread = threading.Thread( + target=self._loop.run_forever, name=f"cf-{self.drone_name}-loop", daemon=True + ) + self._loop_thread.start() + self._log_future = asyncio.run_coroutine_threadsafe( + self._state_log_loop(self._log_stop), self._loop + ) + deadline = time.time() + _STATE_LOG_FIRST_SAMPLE_TIMEOUT + while self._latest_state is None and time.time() < deadline: + if self._log_future.done(): # surface reader errors instead of waiting for the timeout + self._log_future.result() + time.sleep(0.01) + if self._latest_state is None: + raise RuntimeError( + f"No lighthouse state sample within {_STATE_LOG_FIRST_SAMPLE_TIMEOUT}s. Is the " + "Lighthouse system powered and the drone within the tracked volume?" + ) + + def _stop_state_log(self) -> None: + """Signal the background state-log reader to stop and wait for it to finish.""" + if self._log_stop is not None: + self._log_stop.set() + if self._log_future is not None: + try: + self._log_future.result(timeout=2.0) + except Exception as exc: + logger.warning(f"Stopping the lighthouse state log failed: {exc}") + self._log_future = None + self._log_stop = None + + def _stop_loop_thread(self) -> None: + """Stop the background event loop thread, if one is running.""" + if self._loop_thread is None: + return + self._loop.call_soon_threadsafe(self._loop.stop) + self._loop_thread.join() + self._loop_thread = None + + async def _state_log_loop(self, stop: threading.Event) -> None: + """Continuously decode the onboard state estimate into ``self._latest_state``. + + Two log blocks are used because a single CRTP log packet cannot hold all twelve values. + """ + log = self.cf.log() + pos_vel_block = await log.create_block() + for variable in POS_VEL_VARS: + await pos_vel_block.add_variable(variable) + ori_rate_block = await log.create_block() + for variable in ORI_RATE_VARS: + await ori_rate_block.add_variable(variable) + + pos_vel_stream = await pos_vel_block.start(_STATE_LOG_START_ARG) + ori_rate_stream = await ori_rate_block.start(_STATE_LOG_START_ARG) + try: + while not stop.is_set(): + pos_vel = (await pos_vel_stream.next()).data + ori_rate = (await ori_rate_stream.next()).data + self._latest_state = decode_state(pos_vel, ori_rate) + finally: + await pos_vel_stream.stop() + await ori_rate_stream.stop() + + async def _check_lighthouse_deck(self) -> None: + """Verify a Lighthouse deck is attached and detected.""" + value = await self.cf.param().get(LIGHTHOUSE_DECK_PARAM) + if value != 1: + raise RuntimeError( + f"Lighthouse deck not detected ({LIGHTHOUSE_DECK_PARAM}={value!r}). Is the deck " + "attached and flashed, and are the base stations powered?" + ) + + # region Async cflib2 operations async def _connect(self, timeout: float) -> None: if self.is_connected: @@ -255,6 +406,8 @@ async def _power_cycle(uri: str) -> None: self._cf = result logger.info(f"Crazyflie connected to {self.uri}") + if self.lighthouse: + await self._check_lighthouse_deck() async def _disconnect(self) -> None: if self._cf is None: @@ -268,14 +421,17 @@ async def _disconnect(self) -> None: self._commander_level = None async def _reset_estimator(self) -> None: - pos = self._ros_connector.pos[self.drone_name] - quat = self._ros_connector.quat[self.drone_name] param = self.cf.param() - await param.set("kalman.initialX", pos[0]) - await param.set("kalman.initialY", pos[1]) - await param.set("kalman.initialZ", pos[2]) - yaw = R.from_quat(quat).as_euler("xyz", degrees=False)[2] - await param.set("kalman.initialYaw", yaw) + # In lighthouse mode the drone derives its absolute pose from the base stations, so we do + # not seed the estimator with an external pose and let it converge on its own. + if not self.lighthouse: + pos = self._ros_connector.pos[self.drone_name] + quat = self._ros_connector.quat[self.drone_name] + await param.set("kalman.initialX", pos[0]) + await param.set("kalman.initialY", pos[1]) + await param.set("kalman.initialZ", pos[2]) + yaw = R.from_quat(quat).as_euler("xyz", degrees=False)[2] + await param.set("kalman.initialYaw", yaw) await param.set("kalman.resetEstimation", 1) await asyncio.sleep(0.1) await param.set("kalman.resetEstimation", 0) diff --git a/lsy_drone_racing/utils/lighthouse.py b/lsy_drone_racing/utils/lighthouse.py new file mode 100644 index 000000000..b90a51140 --- /dev/null +++ b/lsy_drone_racing/utils/lighthouse.py @@ -0,0 +1,85 @@ +"""Lighthouse positioning helpers. + +When using the Lighthouse positioning system instead of a motion capture system, the drone +estimates its own pose onboard. We read that estimate back from the drone via cflib2 log blocks +instead of receiving it from an external tracker. This module contains the pure, hardware-free +parts of that path (the log variable layout and the decoding into an observation) so they can be +unit tested without a radio or the ``cflib2`` dependency. + +The :class:`~lsy_drone_racing.utils.crazyflie.Crazyflie` wrapper imports these helpers and adds +the actual cflib2 log streaming on top. +""" + +from __future__ import annotations + +from typing import TYPE_CHECKING + +import numpy as np +from scipy.spatial.transform import Rotation as R + +if TYPE_CHECKING: + from numpy.typing import NDArray + +__all__ = ["LIGHTHOUSE_DECK_PARAM", "POS_VEL_VARS", "ORI_RATE_VARS", "decode_state"] + +# Firmware parameter that is 1 when a Lighthouse deck is attached and detected. +LIGHTHOUSE_DECK_PARAM = "deck.bcLighthouse4" + +# A single CRTP log block carries at most ~26 bytes of payload, so the twelve floats we need do +# not fit into one block. We split them across two blocks of six floats (24 bytes) each. +POS_VEL_VARS = ( + "stateEstimate.x", + "stateEstimate.y", + "stateEstimate.z", + "stateEstimate.vx", + "stateEstimate.vy", + "stateEstimate.vz", +) +ORI_RATE_VARS = ( + "stateEstimate.roll", + "stateEstimate.pitch", + "stateEstimate.yaw", + "gyro.x", + "gyro.y", + "gyro.z", +) + + +def decode_state( + pos_vel: dict[str, float], ori_rate: dict[str, float] +) -> dict[str, NDArray[np.floating]]: + """Decode two onboard log samples into a simulation-compatible observation. + + The returned dictionary matches the per-drone fields of the simulation observation space (see + :func:`lsy_drone_racing.envs.race_core.build_observation_space`), so the real environment can + drop it in without any further conversion. + + Args: + pos_vel: A sample containing ``stateEstimate.x/y/z`` (world-frame position, m) and + ``stateEstimate.vx/vy/vz`` (world-frame linear velocity, m/s). + ori_rate: A sample containing ``stateEstimate.roll/pitch/yaw`` (world-frame orientation, + degrees) and ``gyro.x/y/z`` (body-frame angular velocity, degrees/s). + + Returns: + A dictionary with ``float32`` ``pos``, ``quat`` (xyzw), ``vel`` and ``ang_vel`` (rad/s). + """ + pos = np.array( + [pos_vel["stateEstimate.x"], pos_vel["stateEstimate.y"], pos_vel["stateEstimate.z"]], + dtype=np.float32, + ) + vel = np.array( + [pos_vel["stateEstimate.vx"], pos_vel["stateEstimate.vy"], pos_vel["stateEstimate.vz"]], + dtype=np.float32, + ) + rpy = np.deg2rad( + [ + ori_rate["stateEstimate.roll"], + ori_rate["stateEstimate.pitch"], + ori_rate["stateEstimate.yaw"], + ] + ) + quat = R.from_euler("xyz", rpy).as_quat().astype(np.float32) + ang_vel = np.deg2rad([ori_rate["gyro.x"], ori_rate["gyro.y"], ori_rate["gyro.z"]]).astype( + np.float32 + ) + return {"pos": pos, "quat": quat, "vel": vel, "ang_vel": ang_vel} diff --git a/scripts/deploy.py b/scripts/deploy.py index 81a89e7f0..ce3f2e422 100644 --- a/scripts/deploy.py +++ b/scripts/deploy.py @@ -46,6 +46,7 @@ def main(config: str = "level2.toml", controller: str | None = None): randomizations=config.env.randomizations, sensor_range=config.env.sensor_range, control_mode=config.env.control_mode, + lighthouse=config.deploy.get("lighthouse", False), ) try: obs, info = env.reset(options=config.deploy) diff --git a/tests/unit/utils/test_lighthouse.py b/tests/unit/utils/test_lighthouse.py new file mode 100644 index 000000000..88a61c577 --- /dev/null +++ b/tests/unit/utils/test_lighthouse.py @@ -0,0 +1,63 @@ +"""Unit tests for the hardware-free lighthouse helpers.""" + +import numpy as np +import pytest +from scipy.spatial.transform import Rotation as R + +from lsy_drone_racing.utils.lighthouse import ORI_RATE_VARS, POS_VEL_VARS, decode_state + + +@pytest.mark.unit +def test_decode_state_values_and_types(): + pos_vel = { + "stateEstimate.x": 1.0, + "stateEstimate.y": 2.0, + "stateEstimate.z": 3.0, + "stateEstimate.vx": 0.1, + "stateEstimate.vy": 0.2, + "stateEstimate.vz": 0.3, + } + ori_rate = { + "stateEstimate.roll": 0.0, + "stateEstimate.pitch": 0.0, + "stateEstimate.yaw": 90.0, + "gyro.x": 10.0, + "gyro.y": -20.0, + "gyro.z": 30.0, + } + obs = decode_state(pos_vel, ori_rate) + + assert set(obs) == {"pos", "quat", "vel", "ang_vel"} + for key, value in obs.items(): + assert value.dtype == np.float32, f"{key} must be float32" + assert obs["pos"].shape == (3,) + assert obs["quat"].shape == (4,) + assert obs["vel"].shape == (3,) + assert obs["ang_vel"].shape == (3,) + + np.testing.assert_allclose(obs["pos"], [1.0, 2.0, 3.0], rtol=1e-6) + np.testing.assert_allclose(obs["vel"], [0.1, 0.2, 0.3], rtol=1e-6) + # gyro is reported in deg/s and must be converted to rad/s. + np.testing.assert_allclose(obs["ang_vel"], np.deg2rad([10.0, -20.0, 30.0]), rtol=1e-6) + # 90 deg yaw about z, returned in xyzw convention to match the simulation observation. + expected_quat = R.from_euler("z", 90, degrees=True).as_quat() + np.testing.assert_allclose(obs["quat"], expected_quat, atol=1e-6) + + +@pytest.mark.unit +def test_decode_state_identity_quat(): + pos_vel = dict.fromkeys(POS_VEL_VARS, 0.0) + ori_rate = dict.fromkeys(ORI_RATE_VARS, 0.0) + obs = decode_state(pos_vel, ori_rate) + # Zero roll/pitch/yaw -> identity quaternion (0, 0, 0, 1) in xyzw. + np.testing.assert_allclose(obs["quat"], [0.0, 0.0, 0.0, 1.0], atol=1e-7) + np.testing.assert_allclose(obs["ang_vel"], [0.0, 0.0, 0.0], atol=1e-7) + + +@pytest.mark.unit +def test_log_var_blocks_fit_crtp_limit(): + # A single CRTP log block carries at most ~26 bytes; six 4-byte floats = 24 bytes per block. + assert len(POS_VEL_VARS) == 6 + assert len(ORI_RATE_VARS) == 6 + assert len(POS_VEL_VARS) * 4 <= 26 + assert len(ORI_RATE_VARS) * 4 <= 26 From a9d7dc583dd8c255404fb7689232f87fc671a47f Mon Sep 17 00:00:00 2001 From: Florian-S7 Date: Tue, 23 Jun 2026 01:37:03 +0200 Subject: [PATCH 2/2] Add read-only Lighthouse bench script for hardware bring-up scripts/lighthouse_bench.py connects a single drone in lighthouse mode and prints the onboard pos/rpy/vel/ang_vel from get_obs() so the frames/units and base-station calibration can be validated by hand. Never arms the drone and sends no setpoints. Co-Authored-By: Claude Opus 4.8 --- scripts/lighthouse_bench.py | 77 +++++++++++++++++++++++++++++++++++++ 1 file changed, 77 insertions(+) create mode 100644 scripts/lighthouse_bench.py diff --git a/scripts/lighthouse_bench.py b/scripts/lighthouse_bench.py new file mode 100644 index 000000000..23c6c1568 --- /dev/null +++ b/scripts/lighthouse_bench.py @@ -0,0 +1,77 @@ +"""Lighthouse bench check (read-only, no flight). + +Connects to a single Crazyflie in lighthouse mode and continuously prints the onboard state +estimate so you can validate the Lighthouse setup before ever arming the drone: + +- confirms the Lighthouse deck is detected (``deck.bcLighthouse4``), +- starts the same background state-log reader used by the real environment, +- prints ``pos`` / ``rpy`` / ``vel`` / ``ang_vel`` from :meth:`Crazyflie.get_obs`. + +Move and rotate the drone **by hand** and check that the values are sane and in the right frame: +position should match a tape measure in the Lighthouse frame (origin = track origin), and the +velocity / angular velocity signs should follow your motion. This is the make-or-break check for +the frames/units before any closed-loop flight. + +The drone is never armed and no setpoints are sent, so the motors stay off. + +Usage: + + python scripts/lighthouse_bench.py --drone_id 10 --channel 100 +""" + +from __future__ import annotations + +import logging +import time + +import fire +import numpy as np +from scipy.spatial.transform import Rotation as R + +from lsy_drone_racing.utils.crazyflie import Crazyflie + +logger = logging.getLogger(__name__) + + +def main(drone_id: int = 10, channel: int = 100, radio_id: int = 0, rate: float = 10.0) -> None: + """Print the onboard Lighthouse state estimate of a single drone (read-only, no flight). + + Args: + drone_id: Crazyflie id (the last byte of the radio address, e.g. 10 for cf10). + channel: Radio channel of the drone. + radio_id: Crazyradio device index. + rate: Print frequency in Hz. + """ + drone = Crazyflie.from_radio( + radio_id=radio_id, radio_channel=channel, drone_id=drone_id, lighthouse=True + ) + logger.info("Read-only bench check: the drone is never armed and no setpoints are sent.") + logger.info("Connecting to cf%d (channel %d)...", drone_id, channel) + drone.connect(timeout=10.0) # also verifies the Lighthouse deck + drone.reset(arm=False) # applies settings, resets the estimator, starts the state-log reader + logger.info("Connected. Move the drone by hand and watch the values (Ctrl-C to stop).") + + period = 1.0 / rate + try: + while True: + obs = drone.get_obs() + pos, vel, ang_vel = obs["pos"], obs["vel"], obs["ang_vel"] + rpy = R.from_quat(obs["quat"]).as_euler("xyz", degrees=True) + speed = float(np.linalg.norm(vel)) + print( + f"pos=[{pos[0]:+.3f} {pos[1]:+.3f} {pos[2]:+.3f}] m | " + f"rpy=[{rpy[0]:+6.1f} {rpy[1]:+6.1f} {rpy[2]:+6.1f}] deg | " + f"vel=[{vel[0]:+.2f} {vel[1]:+.2f} {vel[2]:+.2f}] |v|={speed:.2f} m/s | " + f"ang_vel=[{ang_vel[0]:+.2f} {ang_vel[1]:+.2f} {ang_vel[2]:+.2f}] rad/s" + ) + time.sleep(period) + except KeyboardInterrupt: + logger.info("Stopping bench check.") + finally: + drone.close(emergency_stop=False) # never armed, so no emergency stop is needed + + +if __name__ == "__main__": + logging.basicConfig(level=logging.INFO) + logger.setLevel(logging.INFO) + fire.Fire(main)