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/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) 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