Lighthouse positioning support for the real-drone env (single drone) - #106
Draft
Florian-S7 wants to merge 2 commits into
Draft
Lighthouse positioning support for the real-drone env (single drone)#106Florian-S7 wants to merge 2 commits into
Florian-S7 wants to merge 2 commits into
Conversation
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 <noreply@anthropic.com>
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 <noreply@anthropic.com>
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
Summary
Adds an opt-in
lighthousemode to the real-drone environment so it can localize the dronefrom its onboard Lighthouse state estimate instead of a motion capture system. This lets the
deployment stack run without Vicon/ROS-based tracking.
The data flow is flipped relative to mocap: instead of pushing external poses to the drone, we
read the onboard estimate back from it.
What changed (mocap path unchanged, lighthouse is opt-in & default off)
lsy_drone_racing/utils/lighthouse.py(new): pure, hardware-free helpers — the two logvariable groups and
decode_state(...)that turns onboard log samples into the sim observation(
float32pos,quatxyzw,vel,ang_vel). Pure so it's unit-testable withoutcflib2.lsy_drone_racing/utils/crazyflie.py:lighthouseflag; verifiesdeck.bcLighthouse4;send_external_pose()becomes a no-op; estimator reset no longer seeds an external pose;ROS command mirroring is skipped; a background reader streams two log blocks into a snapshot
exposed via a new
get_obs().lsy_drone_racing/envs/real_race_env.py:lighthouseflag; sourcespos/quat/vel/ang_velfrom
drone.get_obs(); brings the drone up and checks the start pose before arming; requiresreal_track_objects=false(no tracker can measure the gates/obstacles); single-drone only.scripts/deploy.py+config/level2.toml:[deploy].lighthouseflag (default false).Design notes / decisions
pos+vel (24 B) and orientation+gyro (24 B).
deploy.pyreadsobs()multiple times per control loop, so a persistentbackground drainer keeps a fresh snapshot (mirroring how the mocap
ROSConnectorbehaves) ratherthan creating a log block per call. In lighthouse mode the event loop runs on a dedicated thread;
the mocap path keeps using
run_until_completeunchanged.tracker), so multi-drone + lighthouse raises
NotImplementedError.swarmGPT's_read_lighthouse_observation, extended withvelocity and angular velocity.
Needs hardware verification (we can't test these without the drone yet)
vel/ang_velvs. the sim observation —gyro.*is body-frame deg/s(converted to rad/s),
stateEstimate.v*is world-frame m/s. This is the main risk; we'llbench-test by hand before flying.
start()argument / resulting rate (we mirrored swarmGPT's usage).Testing done
ruff check+ruff format --check: clean.decode_state(values,float32, xyzw quat, deg→rad) and CRTP block sizing.py_compileon all changed files. (Hardware/integration tests pending — see above.)🤖 Generated with Claude Code