wntrmute/cf-rangeview
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Initial commit: RangeView Crazyflie ToF mapping client

Browse Source 
DearPyGui app that visualizes the Multi-ranger ToF sensors and builds a top-down map of the drone's surroundings. Handheld and experimental hardened-scan capture, Flow-deck drift correction (ZUPT), props-off dry-run mode, and optional 3D point-cloud streaming to CloudView.

Co-Authored-By: Claude Opus 4.8 (1M context) <noreply@anthropic.com>
This commit is contained in:
K. Isom
2026-06-24 14:09:01 -07:00
commit 356218e8a2
11 changed files with 2763 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
rangeview/__init__.py Normal file
View File

@@ -0,0 +1,3 @@
"""rangeview: visualise Crazyflie Multi-ranger ToF data and map the surroundings."""
__version__ = "0.1.0"

6
rangeview/__main__.py Normal file
View File

@@ -0,0 +1,6 @@
"""Allow `python -m rangeview`."""
from .app import main
if __name__ == "__main__":
main()

583
rangeview/app.py Normal file
View File

@@ -0,0 +1,583 @@
#!/usr/bin/env python3
"""DearPyGui front-end for the Crazyflie range-mapping client.
Two independent parts, reflecting how the data is captured:
1. Mapping - projects the Multi-ranger ToF distances through the drone's
position estimate into a top-down point-cloud map of the surroundings. This
works whenever pose + range are streaming, so you can simply *hold the drone
in your hand* and walk it around to build a map; no flight required.
2. Flight (automated scan) - an optional capture profile: arm, lift off, then
ascend slowly while rotating in place so the sensors sweep a full circle on
their own.
The render loop is driven manually (same pattern as the test-stand / mission
clients): every frame we poll the controller's thread-safe snapshot() and the
map, and refresh the widgets. All radio I/O stays on the controller's worker
thread.
Run:
python -m rangeview
CFLIB_URI=radio://0/80/2M/E7E7E7E7E7 python -m rangeview # custom URI
"""
from __future__ import annotations
import math
import os
import shutil
import subprocess
import dearpygui.dearpygui as dpg
from .cloud_publisher import DEFAULT_ADDRESS
from .controller import (
DEFAULT_CLIMB_RATE_MPS,
DEFAULT_START_HEIGHT_M,
DEFAULT_TOP_HEIGHT_M,
DEFAULT_URI,
DEFAULT_YAW_RATE_DPS,
LOG_BUFFER,
MAX_HEIGHT_M,
MIN_HEIGHT_M,
RangeController,
State,
)
# Status text colour per state (R, G, B).
STATE_COLOUR = {
State.DISCONNECTED: (150, 150, 150),
State.CONNECTING: (220, 180, 60),
State.READY: (60, 200, 90),
State.ARMING: (220, 180, 60),
State.ARMED: (90, 150, 235),
State.SCANNING: (60, 200, 90),
State.LANDING: (220, 180, 60),
State.EMERGENCY: (235, 70, 70),
State.ERROR: (235, 70, 70),
}
# States in which a link exists (connected through on-the-ground emergency).
CONNECTED_STATES = {State.READY, State.ARMING, State.ARMED, State.SCANNING,
State.LANDING, State.EMERGENCY}
ARMABLE_STATES = {State.READY, State.EMERGENCY} # primary: ARM (green)
SCANNABLE_STATES = {State.ARMED} # primary: SCAN (blue)
FLYING_STATES = {State.SCANNING} # primary: LAND (yellow)
# Estimator reset / origin is a ground operation only.
RESETTABLE_STATES = {State.READY, State.ARMED, State.EMERGENCY}
# Scan-parameter sliders lock once airborne or mid-arm transient.
PARAMS_LOCKED_STATES = {State.ARMING, State.SCANNING, State.LANDING}
# Map sensor colours, matching the live-ray series in the plot.
RAY_COLOUR = {
'front': (90, 200, 255),
'back': (255, 170, 90),
'left': (140, 255, 140),
'right': (255, 140, 200),
}
# Heading arrow length and drone marker, in metres / plot units.
HEADING_LEN_M = 0.35
class RangeViewApp:
def __init__(self) -> None:
self.controller = RangeController(
os.environ.get('CFLIB_URI', DEFAULT_URI))
self._console_version = -1 # last LOG_BUFFER version rendered
self._map_version = -1 # last map version uploaded to the plot
# ------------------------------------------------------------------
# UI construction
# ------------------------------------------------------------------
def build(self) -> None:
dpg.create_context()
self._load_font()
self._build_themes()
with dpg.window(tag='primary'):
with dpg.group(horizontal=True):
# Left column: connection, telemetry, mapping + flight controls.
with dpg.child_window(width=440, autosize_y=True):
self._build_connection()
dpg.add_separator()
self._build_status()
dpg.add_separator()
self._build_ranges()
dpg.add_separator()
self._build_mapping()
dpg.add_separator()
self._build_flight()
dpg.add_separator()
self._build_console()
# Right column: the map fills the rest of the window.
with dpg.child_window(width=-1, autosize_y=True):
self._build_map()
# SPACE = emergency stop, available anywhere.
with dpg.handler_registry():
dpg.add_key_press_handler(dpg.mvKey_Spacebar,
callback=self._on_estop)
dpg.create_viewport(title='Crazyflie RangeView', width=1180, height=820)
dpg.setup_dearpygui()
dpg.show_viewport()
dpg.set_primary_window('primary', True)
def _load_font(self) -> None:
"""Bind Brass Mono as the UI font when available (falls back silently to
the DearPyGui default). A real mono font also renders the box/glyph
characters cleanly; the UI strings themselves are kept ASCII."""
path = _find_brass_mono()
if not path:
return
with dpg.font_registry():
font = dpg.add_font(path, 16)
dpg.bind_font(font)
def _build_themes(self) -> None:
def button_theme(base, hover, active, text=None):
with dpg.theme() as theme:
with dpg.theme_component(dpg.mvButton):
dpg.add_theme_color(dpg.mvThemeCol_Button, base)
dpg.add_theme_color(dpg.mvThemeCol_ButtonHovered, hover)
dpg.add_theme_color(dpg.mvThemeCol_ButtonActive, active)
if text is not None:
dpg.add_theme_color(dpg.mvThemeCol_Text, text)
return theme
self._estop_theme = button_theme((170, 30, 30), (210, 40, 40),
(240, 60, 60))
self._arm_theme = button_theme((30, 120, 50), (40, 150, 65),
(50, 175, 80))
self._scan_theme = button_theme((40, 95, 185), (55, 115, 210),
(70, 135, 230))
self._land_theme = button_theme((205, 170, 40), (225, 190, 55),
(240, 205, 70), text=(35, 30, 0))
def _build_connection(self) -> None:
dpg.add_text('Connection')
with dpg.group(horizontal=True):
dpg.add_combo([self.controller.uri], tag='uri_combo', width=250,
default_value=self.controller.uri)
dpg.add_button(label='Scan', tag='scan_btn', callback=self._on_scan)
dpg.add_button(label='Connect', tag='connect_btn',
callback=self._on_connect_toggle)
def _build_status(self) -> None:
dpg.add_text('Status')
dpg.add_text('Disconnected', tag='status_text',
color=STATE_COLOUR[State.DISCONNECTED])
with dpg.group(horizontal=True):
dpg.add_text('Battery:')
dpg.add_text('-- V', tag='vbat_text')
dpg.add_text(' Height:')
dpg.add_text('-- m', tag='height_text')
with dpg.group(horizontal=True):
dpg.add_text('Multi-ranger:')
dpg.add_text('unknown', tag='mr_text')
dpg.add_text(' Flow v2:')
dpg.add_text('unknown', tag='flow_text')
with dpg.group(horizontal=True):
dpg.add_text('Pos (m):')
dpg.add_text('x -- y -- z --', tag='pos_text')
dpg.add_text(' Yaw:')
dpg.add_text('-- deg', tag='yaw_text')
with dpg.group(horizontal=True):
dpg.add_text('Motion:')
dpg.add_text('--', tag='motion_text')
def _build_ranges(self) -> None:
dpg.add_text('ToF distances (m)')
# Horizontal sensors, colour-matched to their map rays.
with dpg.group(horizontal=True):
for name in ('front', 'back', 'left', 'right'):
dpg.add_text(f'{name[0].upper()}:')
dpg.add_text('--', tag=f'rng_{name}', color=RAY_COLOUR[name])
dpg.add_spacer(width=6)
with dpg.group(horizontal=True):
dpg.add_text('Up (ceiling):')
dpg.add_text('--', tag='rng_up')
dpg.add_text(' Down (floor):')
dpg.add_text('--', tag='rng_down')
def _build_mapping(self) -> None:
dpg.add_text('Mapping')
dpg.add_text('Hold the drone and move it, or run a scan - both fill the '
'map.', wrap=420, color=(150, 150, 150))
with dpg.group(horizontal=True):
dpg.add_checkbox(label='Record', tag='record_chk',
default_value=self.controller.recording,
callback=self._on_record_toggle)
dpg.add_button(label='Reset origin', tag='reset_btn',
callback=self._on_reset_origin, enabled=False)
dpg.add_button(label='Clear map',
callback=lambda: self.controller.clear_map())
dpg.add_checkbox(label='Drift correction (hold pose when still)',
tag='drift_chk',
default_value=self.controller.drift_correction,
callback=self._on_drift_toggle)
dpg.add_text('Points: 0', tag='points_text')
# 3-D stream to a CloudView viewer (the 2-D map above stays the live
# diagnostic; the full 3-D cloud goes out over the socket).
with dpg.group(horizontal=True):
dpg.add_checkbox(label='Stream 3D to', tag='stream_chk',
default_value=False,
callback=self._on_stream_change)
dpg.add_input_text(tag='stream_addr', width=210,
default_value=DEFAULT_ADDRESS,
callback=self._on_stream_change, on_enter=True)
dpg.add_text('viewer: off', tag='stream_status', color=(150, 150, 150))
def _build_flight(self) -> None:
dpg.add_text('Flight (automated scan)')
dpg.add_text('EXPERIMENTAL - rotating flight on a Flow deck is risky. '
'Bench-test props-off first; keep SPACE (e-stop) ready and '
'the area clear.', wrap=420, color=(235, 170, 70))
dpg.add_slider_float(label='Start height', tag='start_slider',
default_value=DEFAULT_START_HEIGHT_M,
min_value=MIN_HEIGHT_M, max_value=MAX_HEIGHT_M,
format='%.2f m', width=-110,
callback=self._on_param_change)
dpg.add_slider_float(label='Top height', tag='top_slider',
default_value=DEFAULT_TOP_HEIGHT_M,
min_value=MIN_HEIGHT_M, max_value=MAX_HEIGHT_M,
format='%.2f m', width=-110,
callback=self._on_param_change)
dpg.add_slider_float(label='Climb rate', tag='climb_slider',
default_value=DEFAULT_CLIMB_RATE_MPS,
min_value=0.05, max_value=0.5,
format='%.2f m/s', width=-110,
callback=self._on_param_change)
dpg.add_slider_float(label='Yaw rate', tag='yaw_slider',
default_value=DEFAULT_YAW_RATE_DPS,
min_value=15.0, max_value=120.0,
format='%.0f deg/s', width=-110,
callback=self._on_param_change)
dpg.add_checkbox(label='Dry run (props off - walk the scan, no motors)',
tag='dryrun_chk',
default_value=self.controller.dry_run,
callback=self._on_dryrun_toggle)
# Primary action: ARM (green) -> SCAN (blue) -> LAND (yellow).
dpg.add_button(label='ARM', tag='arm_btn', width=-1, height=42,
callback=self._on_primary, enabled=False)
with dpg.group(horizontal=True):
dpg.add_button(label='Disarm', tag='disarm_btn', width=140,
callback=self._on_disarm, enabled=False)
estop = dpg.add_button(label='EMERGENCY STOP (SPACE)',
tag='estop_btn', width=-1, height=42,
callback=self._on_estop)
dpg.bind_item_theme(estop, self._estop_theme)
def _build_console(self) -> None:
with dpg.group(horizontal=True):
dpg.add_text('Console')
dpg.add_button(label='Clear', callback=lambda: LOG_BUFFER.clear())
with dpg.child_window(tag='console_child', width=-1, height=150):
dpg.add_text('', tag='console_text', wrap=410)
def _build_map(self) -> None:
with dpg.group(horizontal=True):
dpg.add_text('Map (top-down - looking down on the drone)')
dpg.add_spacer(width=12)
dpg.add_text('extent +/-')
dpg.add_slider_float(tag='extent_slider', default_value=3.0,
min_value=1.0, max_value=8.0, format='%.1f m',
width=160)
with dpg.plot(tag='map_plot', height=-1, width=-1, equal_aspects=True):
dpg.add_plot_legend()
dpg.add_plot_axis(dpg.mvXAxis, label='x - forward (m)', tag='map_x')
with dpg.plot_axis(dpg.mvYAxis, label='y - left (m)', tag='map_y'):
dpg.add_scatter_series([], [], label='obstacles',
tag='map_points')
# Live sensor rays from the drone's current pose.
for name in ('front', 'back', 'left', 'right'):
dpg.add_line_series([], [], label=name, tag=f'ray_{name}')
dpg.add_line_series([], [], label='heading', tag='heading_line')
dpg.add_scatter_series([0.0], [0.0], label='drone',
tag='drone_pt')
# Series styling: small grey obstacle dots, coloured rays, bold drone.
self._style_series('map_points', (200, 200, 80), marker_size=2)
self._style_series('drone_pt', (60, 220, 90), marker_size=6,
marker=dpg.mvPlotMarker_Circle)
self._style_series('heading_line', (60, 220, 90), weight=2.0)
for name in ('front', 'back', 'left', 'right'):
self._style_series(f'ray_{name}', RAY_COLOUR[name], weight=1.5)
def _style_series(self, tag, colour, marker_size=3, weight=1.0,
marker=dpg.mvPlotMarker_Circle) -> None:
with dpg.theme() as theme:
with dpg.theme_component(dpg.mvScatterSeries):
dpg.add_theme_color(dpg.mvPlotCol_Line, colour,
category=dpg.mvThemeCat_Plots)
dpg.add_theme_color(dpg.mvPlotCol_MarkerFill, colour,
category=dpg.mvThemeCat_Plots)
dpg.add_theme_color(dpg.mvPlotCol_MarkerOutline, colour,
category=dpg.mvThemeCat_Plots)
dpg.add_theme_style(dpg.mvPlotStyleVar_Marker, marker,
category=dpg.mvThemeCat_Plots)
dpg.add_theme_style(dpg.mvPlotStyleVar_MarkerSize, marker_size,
category=dpg.mvThemeCat_Plots)
with dpg.theme_component(dpg.mvLineSeries):
dpg.add_theme_color(dpg.mvPlotCol_Line, colour,
category=dpg.mvThemeCat_Plots)
dpg.add_theme_style(dpg.mvPlotStyleVar_LineWeight, weight,
category=dpg.mvThemeCat_Plots)
dpg.bind_item_theme(tag, theme)
# ------------------------------------------------------------------
# Callbacks
# ------------------------------------------------------------------
def _on_scan(self) -> None:
uris = self.controller.scan_uris()
if uris:
dpg.configure_item('uri_combo', items=uris)
dpg.set_value('uri_combo', uris[0])
else:
dpg.configure_item('uri_combo', items=[self.controller.uri])
dpg.set_value('uri_combo', '(none found)')
def _on_connect_toggle(self) -> None:
state = self.controller.snapshot().state
if state in CONNECTED_STATES or state == State.CONNECTING:
self.controller.disconnect()
return
uri = dpg.get_value('uri_combo')
if uri and uri.startswith(('radio://', 'usb://')):
self.controller.connect(uri)
def _on_record_toggle(self, sender, value) -> None:
self.controller.set_recording(value)
def _on_reset_origin(self) -> None:
self.controller.reset_origin()
def _on_drift_toggle(self, sender, value) -> None:
self.controller.set_drift_correction(value)
def _on_dryrun_toggle(self, sender, value) -> None:
self.controller.set_dry_run(value)
def _on_stream_change(self, *_) -> None:
self.controller.set_streaming(dpg.get_value('stream_chk'),
dpg.get_value('stream_addr').strip())
def _on_param_change(self, *_) -> None:
self.controller.set_scan_params(
dpg.get_value('start_slider'), dpg.get_value('top_slider'),
dpg.get_value('climb_slider'), dpg.get_value('yaw_slider'))
def _on_primary(self) -> None:
state = self.controller.snapshot().state
if state in ARMABLE_STATES:
self._on_param_change() # latch the latest slider values
self.controller.arm()
elif state in SCANNABLE_STATES:
self.controller.scan()
elif state in FLYING_STATES:
self.controller.stop() # land
def _on_disarm(self) -> None:
if self.controller.snapshot().state in SCANNABLE_STATES:
self.controller.stop()
def _on_estop(self, *_) -> None:
self.controller.emergency_stop()
# ------------------------------------------------------------------
# Per-frame update
# ------------------------------------------------------------------
def update(self) -> None:
upd = self.controller.snapshot()
state, t = upd.state, upd.telemetry
dpg.set_value('status_text', upd.message or state.name)
dpg.configure_item('status_text',
color=STATE_COLOUR.get(state, (200, 200, 200)))
dpg.set_value('vbat_text', f'{t.vbat:.2f} V' if t.vbat else '-- V')
dpg.set_value('height_text', f'{t.z:.2f} m')
dpg.set_value('mr_text', _deck_label(t.multiranger))
dpg.set_value('flow_text', _deck_label(t.flow_deck))
dpg.set_value('pos_text',
f'x {t.x:+.2f} y {t.y:+.2f} z {t.z:+.2f}')
dpg.set_value('yaw_text', f'{t.yaw:+.1f} deg')
# Motion / drift-hold indicator: shows when the pose is being pinned, and
# the residual raw drift it is absorbing so you can see it working.
if t.still and t.drift_corrected:
drift = math.hypot(t.raw_x - t.x, t.raw_y - t.y)
dpg.set_value('motion_text',
f'still - pose held (absorbing {drift * 100:.0f} cm drift)')
dpg.configure_item('motion_text', color=(90, 150, 235))
elif t.still:
dpg.set_value('motion_text', 'still (correction off)')
dpg.configure_item('motion_text', color=(150, 150, 150))
else:
dpg.set_value('motion_text', 'moving')
dpg.configure_item('motion_text', color=(60, 200, 90))
for name in ('front', 'back', 'left', 'right', 'up', 'down'):
dpg.set_value(f'rng_{name}', _range_label(getattr(t, name)))
# 3-D stream status.
if not dpg.get_value('stream_chk'):
dpg.set_value('stream_status', 'viewer: off')
dpg.configure_item('stream_status', color=(150, 150, 150))
elif self.controller.streaming_connected:
dpg.set_value('stream_status', 'viewer: connected')
dpg.configure_item('stream_status', color=(60, 200, 90))
else:
dpg.set_value('stream_status', 'viewer: waiting for connection...')
dpg.configure_item('stream_status', color=(220, 180, 60))
self._refresh_buttons(state)
self._refresh_map(t)
self._refresh_console()
def _refresh_buttons(self, state: State) -> None:
connecting = state == State.CONNECTING
connected = state in CONNECTED_STATES
dpg.configure_item(
'connect_btn', enabled=not connecting,
label='Disconnect' if (connected or connecting) else 'Connect')
dpg.configure_item('scan_btn', enabled=not (connected or connecting))
dpg.configure_item('uri_combo', enabled=not (connected or connecting))
dpg.configure_item('reset_btn', enabled=state in RESETTABLE_STATES)
locked = state in PARAMS_LOCKED_STATES
for tag in ('start_slider', 'top_slider', 'climb_slider', 'yaw_slider'):
dpg.configure_item(tag, enabled=not locked)
# Dry run swaps the lock to keep the params editable and re-labels the
# buttons so it's obvious nothing will fly.
dry = dpg.get_value('dryrun_chk')
# Primary button across the ARM -> SCAN -> LAND flow.
if state in ARMABLE_STATES:
dpg.configure_item('arm_btn', enabled=True,
label='ARM (dry run)' if dry else 'ARM')
dpg.bind_item_theme('arm_btn', self._arm_theme)
elif state in SCANNABLE_STATES:
top = dpg.get_value('top_slider')
label = (f'DRY-RUN SCAN (walk to {top:.2f} m, no motors)' if dry
else f'SCAN (!) (rotating, to {top:.2f} m)')
dpg.configure_item('arm_btn', enabled=True, label=label)
dpg.bind_item_theme('arm_btn', self._scan_theme)
elif state in FLYING_STATES:
dpg.configure_item('arm_btn', enabled=True, label='LAND')
dpg.bind_item_theme('arm_btn', self._land_theme)
else:
dpg.configure_item('arm_btn', enabled=False, label='ARM')
dpg.bind_item_theme('arm_btn', 0)
dpg.configure_item('disarm_btn', enabled=state in SCANNABLE_STATES)
dpg.configure_item('estop_btn', enabled=connected)
def _refresh_map(self, t) -> None:
# Re-upload the accumulated cloud only when it actually changed.
version, xs, ys = self.controller.map_snapshot()
if version != self._map_version:
self._map_version = version
dpg.set_value('map_points', [xs, ys])
dpg.set_value('points_text', f'Points: {len(xs)}')
# Drone marker, heading arrow, and live rays from the current pose.
dpg.set_value('drone_pt', [[t.x], [t.y]])
yaw = math.radians(t.yaw)
hx = t.x + HEADING_LEN_M * math.cos(yaw)
hy = t.y + HEADING_LEN_M * math.sin(yaw)
dpg.set_value('heading_line', [[t.x, hx], [t.y, hy]])
for name, (bx, by) in (('front', (1.0, 0.0)), ('back', (-1.0, 0.0)),
('left', (0.0, 1.0)), ('right', (0.0, -1.0))):
dist = getattr(t, name)
if dist is None:
dpg.set_value(f'ray_{name}', [[], []])
continue
wx = math.cos(yaw) * bx - math.sin(yaw) * by
wy = math.sin(yaw) * bx + math.cos(yaw) * by
dpg.set_value(f'ray_{name}',
[[t.x, t.x + wx * dist], [t.y, t.y + wy * dist]])
# Keep the view centred on the origin with a user-set extent (equal
# aspect keeps it square regardless of window shape).
ext = dpg.get_value('extent_slider')
dpg.set_axis_limits('map_x', -ext, ext)
dpg.set_axis_limits('map_y', -ext, ext)
def _refresh_console(self) -> None:
version, lines = LOG_BUFFER.snapshot()
if version == self._console_version:
return
self._console_version = version
try:
at_bottom = (dpg.get_y_scroll('console_child')
>= dpg.get_y_scroll_max('console_child') - 2.0)
except Exception:
at_bottom = True
dpg.set_value('console_text', '\n'.join(lines))
if at_bottom:
dpg.set_y_scroll('console_child', -1.0)
# ------------------------------------------------------------------
# Run loop
# ------------------------------------------------------------------
def run(self) -> None:
self.controller.start()
self.build()
try:
while dpg.is_dearpygui_running():
self.update()
dpg.render_dearpygui_frame()
finally:
# Never leave the drone airborne: ask the controller to land (if
# flying) and close the link, and wait for the worker to finish so
# libusb is never torn down mid-call. Bounded so we can't hang.
try:
self.controller.shutdown()
self.controller.join(timeout=12)
except Exception:
pass
dpg.destroy_context()
def _find_brass_mono() -> str | None:
"""Locate the Brass Mono regular TTF, or None if it isn't installed."""
candidates = [
os.path.expanduser('~/.local/share/fonts/BrassMono-Regular.ttf'),
'/usr/share/fonts/truetype/brassmono/BrassMono-Regular.ttf',
'/usr/local/share/fonts/BrassMono-Regular.ttf',
]
for path in candidates:
if os.path.isfile(path):
return path
# Fall back to fontconfig, which resolves by family name.
if shutil.which('fc-match'):
try:
out = subprocess.run(['fc-match', '-f', '%{file}', 'Brass Mono'],
capture_output=True, text=True, timeout=3)
path = out.stdout.strip()
if path and os.path.isfile(path) and 'brass' in path.lower():
return path
except Exception:
pass
return None
def _deck_label(present) -> str:
if present is None:
return 'unknown'
return 'attached' if present else 'NOT DETECTED'
def _range_label(dist) -> str:
return f'{dist:.2f}' if dist is not None else '--'
def main() -> None:
RangeViewApp().run()
if __name__ == '__main__':
main()

191
rangeview/cloud_publisher.py Normal file
View File

@@ -0,0 +1,191 @@
"""Best-effort point-cloud publisher (producer side of the CloudView protocol).
Streams batches of ``(x, y, z)`` points to a CloudView viewer over a stream
socket (TCP or Unix) as NDJSON. Design goals:
* **Never block the caller.** Points are produced from the cflib log callback /
flight thread; those must not stall on the network. ``add_points`` only
appends to in-memory buffers and returns immediately. A dedicated socket
thread does all I/O, and outgoing batches are dropped (not queued unbounded)
if the viewer is slow or absent.
* **Survive a missing/restarted viewer.** The socket thread reconnects with
backoff, and on every (re)connect it replays the whole retained cloud, so a
viewer started *after* the drone still sees the full map.
Only the standard library is used, so RangeView gains no heavy dependency; the
NDJSON format is the sole contract with the viewer.
"""
from __future__ import annotations
import json
import logging
import socket
import threading
import time
from collections import deque
logger = logging.getLogger(__name__)
DEFAULT_ADDRESS = "tcp://127.0.0.1:9870"
def _encode(msg: dict) -> bytes:
return (json.dumps(msg, separators=(",", ":")) + "\n").encode("utf-8")
def _parse_address(addr: str):
"""('tcp', host, port) or ('unix', path, None). Mirrors cloudview.protocol
so the two programs stay independent."""
if addr.startswith("tcp://"):
host, _, port = addr[len("tcp://"):].partition(":")
return ("tcp", host or "127.0.0.1", int(port or 9870))
if addr.startswith("unix://"):
return ("unix", addr[len("unix://"):], None)
if addr.startswith("unix:"):
return ("unix", addr[len("unix:"):], None)
if ":" in addr:
host, _, port = addr.partition(":")
return ("tcp", host or "127.0.0.1", int(port))
raise ValueError(f"unrecognised address: {addr!r}")
class CloudPublisher:
"""Streams a retained point cloud to a CloudView viewer, best-effort."""
def __init__(self, source: str = "rangeview", name: str = "RangeView",
max_points: int = 200_000, max_pending: int = 2000) -> None:
self._source = source
self._name = name
self._lock = threading.Lock()
self._points: deque = deque(maxlen=max_points) # retained full cloud
self._pending: deque = deque(maxlen=max_pending) # unsent delta batches
self._enabled = False
self._address: str | None = None
self._generation = 0 # bumped on any config change to force reconnect
self._connected = False
self._stop = False
self._thread = threading.Thread(target=self._run, name="cloud-pub",
daemon=True)
self._thread.start()
# ---------------------------------------------------------------- API ---
def configure(self, enabled: bool, address: str | None) -> None:
"""Enable/disable streaming and/or change the viewer address."""
with self._lock:
self._enabled = bool(enabled)
if address:
self._address = address
self._generation += 1 # make the socket thread re-evaluate/reconnect
@property
def connected(self) -> bool:
return self._connected
def add_points(self, pts) -> None:
"""Append a batch of (x, y, z) to the retained cloud and queue it for
sending. Cheap and non-blocking; safe to call from any thread."""
batch = [(float(x), float(y), float(z)) for x, y, z in pts]
if not batch:
return
with self._lock:
self._points.extend(batch)
if self._enabled:
self._pending.append(batch)
def clear(self) -> None:
"""Drop the retained cloud and tell the viewer to clear this source."""
with self._lock:
self._points.clear()
if self._enabled:
self._pending.append("clear")
def close(self) -> None:
self._stop = True
# ------------------------------------------------------------ internals -
def _config(self):
with self._lock:
return self._enabled, self._address, self._generation
def _run(self) -> None:
while not self._stop:
enabled, address, gen = self._config()
if not enabled or not address:
self._connected = False
time.sleep(0.2)
continue
try:
sock = self._connect(address)
except Exception as exc:
logger.debug("cloud viewer connect to %s failed: %s",
address, exc)
time.sleep(1.0)
continue
self._connected = True
logger.info("streaming points to %s", address)
try:
self._on_connect(sock)
self._pump(sock, gen)
except Exception as exc:
logger.debug("cloud stream to %s ended: %s", address, exc)
finally:
self._connected = False
try:
sock.close()
except OSError:
pass
time.sleep(0.3) # backoff before reconnecting
@staticmethod
def _connect(address: str) -> socket.socket:
kind, a, b = _parse_address(address)
if kind == "tcp":
sock = socket.create_connection((a, b), timeout=2.0)
else:
sock = socket.socket(socket.AF_UNIX, socket.SOCK_STREAM)
sock.settimeout(2.0)
sock.connect(a)
sock.settimeout(2.0)
return sock
def _on_connect(self, sock: socket.socket) -> None:
"""Announce ourselves and replay the full retained cloud, so a freshly
connected viewer is immediately consistent. Any deltas queued while we
were disconnected are discarded first (the replay already covers them)."""
with self._lock:
self._pending.clear()
points = list(self._points)
sock.sendall(_encode({"type": "hello", "source": self._source,
"name": self._name}))
sock.sendall(_encode({"type": "clear", "source": self._source}))
for i in range(0, len(points), 2000):
chunk = points[i:i + 2000]
sock.sendall(_encode({"type": "points", "source": self._source,
"frame": "world",
"points": [list(p) for p in chunk]}))
def _pump(self, sock: socket.socket, gen: int) -> None:
"""Send queued batches until disabled, reconfigured, or disconnected."""
while not self._stop:
enabled, _addr, cur_gen = self._config()
if not enabled or cur_gen != gen:
return
items = self._drain()
if not items:
time.sleep(0.02)
continue
blob = b"".join(self._render(item) for item in items)
sock.sendall(blob)
def _drain(self) -> list:
with self._lock:
items = list(self._pending)
self._pending.clear()
return items
def _render(self, item) -> bytes:
if item == "clear":
return _encode({"type": "clear", "source": self._source})
return _encode({"type": "points", "source": self._source,
"frame": "world", "points": [list(p) for p in item]})

1225
rangeview/controller.py Normal file
View File

File diff suppressed because it is too large Load Diff