Compare commits
13 Commits
25f0711309
...
5f0affcd79
| Author | SHA1 | Date | |
|---|---|---|---|
| 5f0affcd79 | |||
| 4c7fa938a5 | |||
| 45332f1a8b | |||
| af46b15391 | |||
| e1d605dba7 | |||
| c8c8794daa | |||
| b5862ef529 | |||
|
343e53081a | ||
| 602fbc6ab3 | |||
| 1fd935b87e | |||
| b6c6dbd838 | |||
|
|
26bf4ab8d3 | ||
| cb802ee76f |
410
chassis/cable_tray.scad
Normal file
410
chassis/cable_tray.scad
Normal file
@ -0,0 +1,410 @@
|
||||
// ============================================================
|
||||
// Cable Management Tray — Issue #628
|
||||
// Agent : sl-mechanical
|
||||
// Date : 2026-03-15
|
||||
// Part catalogue:
|
||||
// 1. tray_body — under-plate tray with snap-in cable channels, Velcro
|
||||
// tie-down slots every 40 mm, pass-through holes, label slots
|
||||
// 2. tnut_bracket — 2020 T-nut rail mount bracket (×2, slide under tray)
|
||||
// 3. channel_clip — snap-in divider clip separating power / signal / servo zones
|
||||
// 4. cover_panel — hinged snap-on lid (living-hinge PETG flexure strip)
|
||||
// 5. cable_saddle — individual cable saddle / strain-relief clip (×n)
|
||||
//
|
||||
// BOM:
|
||||
// 4 × M5×10 BHCS + M5 T-nuts (tnut_bracket × 2 to rail)
|
||||
// 4 × M3×8 SHCS (tnut_bracket to tray body)
|
||||
// n × 100 mm Velcro tie-down strips (through 6×2 mm slots, every 40 mm)
|
||||
//
|
||||
// Cable channel layout (X axis, inside tray):
|
||||
// Zone A — Power (2S–6S LiPo, XT60/XT30): 20 mm wide, 14 mm deep
|
||||
// Zone B — Signal (JST-SH, PWM, I2C, UART): 14 mm wide, 10 mm deep
|
||||
// Zone C — Servo (JST-PH, thick servo leads): 14 mm wide, 12 mm deep
|
||||
// Divider walls: 2.5 mm thick between zones
|
||||
//
|
||||
// Print settings (PETG):
|
||||
// tray_body / tnut_bracket / channel_clip : 5 perimeters, 40 % gyroid, no supports
|
||||
// cover_panel : 3 perimeters, 20 % gyroid, no supports
|
||||
// (living-hinge — print flat, thin strip flexes)
|
||||
// cable_saddle : 3 perimeters, 30 % gyroid, no supports
|
||||
//
|
||||
// Export commands:
|
||||
// openscad -D 'RENDER="tray_body"' -o tray_body.stl cable_tray.scad
|
||||
// openscad -D 'RENDER="tnut_bracket"' -o tnut_bracket.stl cable_tray.scad
|
||||
// openscad -D 'RENDER="channel_clip"' -o channel_clip.stl cable_tray.scad
|
||||
// openscad -D 'RENDER="cover_panel"' -o cover_panel.stl cable_tray.scad
|
||||
// openscad -D 'RENDER="cable_saddle"' -o cable_saddle.stl cable_tray.scad
|
||||
// openscad -D 'RENDER="assembly"' -o assembly.png cable_tray.scad
|
||||
// ============================================================
|
||||
|
||||
RENDER = "assembly"; // tray_body | tnut_bracket | channel_clip | cover_panel | cable_saddle | assembly
|
||||
|
||||
$fn = 48;
|
||||
EPS = 0.01;
|
||||
|
||||
// ── 2020 rail constants ──────────────────────────────────────
|
||||
RAIL_W = 20.0;
|
||||
TNUT_W = 9.8;
|
||||
TNUT_H = 5.5;
|
||||
TNUT_L = 12.0;
|
||||
SLOT_NECK_H = 3.2;
|
||||
M5_D = 5.2;
|
||||
M5_HEAD_D = 9.5;
|
||||
M5_HEAD_H = 4.0;
|
||||
|
||||
// ── Tray geometry ────────────────────────────────────────────
|
||||
TRAY_L = 280.0; // length along rail (7 × 40 mm tie-down pitch)
|
||||
TRAY_W = 60.0; // width across rail (covers standard 40 mm rail pair)
|
||||
TRAY_WALL = 2.5; // side / floor wall thickness
|
||||
TRAY_DEPTH = 18.0; // interior depth (tallest zone + wall)
|
||||
|
||||
// Cable channel zones (widths must sum to TRAY_W - 2*TRAY_WALL - 2*DIV_T)
|
||||
DIV_T = 2.5; // divider wall thickness
|
||||
ZONE_A_W = 20.0; // Power
|
||||
ZONE_A_D = 14.0;
|
||||
ZONE_B_W = 14.0; // Signal
|
||||
ZONE_B_D = 10.0;
|
||||
ZONE_C_W = 14.0; // Servo
|
||||
ZONE_C_D = 12.0;
|
||||
// Total inner width used: ZONE_A_W + ZONE_B_W + ZONE_C_W + 2*DIV_T = 55 mm < TRAY_W - 2*TRAY_WALL = 55 mm ✓
|
||||
|
||||
// Tie-down slots (Velcro strips)
|
||||
TIEDOWN_PITCH = 40.0;
|
||||
TIEDOWN_W = 6.0; // slot width (fits 6 mm wide Velcro)
|
||||
TIEDOWN_T = 2.2; // slot through-thickness (floor)
|
||||
TIEDOWN_CNT = 7; // 7 positions along tray
|
||||
|
||||
// Pass-through holes in floor
|
||||
PASSTHRU_D = 12.0; // circular grommet-compatible pass-through
|
||||
PASSTHRU_CNT = 3; // one per zone, at tray mid-length
|
||||
|
||||
// Label slots (rear outer wall)
|
||||
LABEL_W = 24.0;
|
||||
LABEL_H = 8.0;
|
||||
LABEL_T = 1.0; // depth from outer face
|
||||
|
||||
// Snap ledge for cover
|
||||
SNAP_LEDGE_H = 2.5;
|
||||
SNAP_LEDGE_D = 1.5;
|
||||
|
||||
// ── T-nut bracket ────────────────────────────────────────────
|
||||
BKT_L = 60.0;
|
||||
BKT_W = TRAY_W;
|
||||
BKT_T = 6.0;
|
||||
BOLT_PITCH = 40.0;
|
||||
M3_D = 3.2;
|
||||
M3_HEAD_D = 6.0;
|
||||
M3_HEAD_H = 3.0;
|
||||
M3_NUT_W = 5.5;
|
||||
M3_NUT_H = 2.4;
|
||||
|
||||
// ── Cover panel ──────────────────────────────────────────────
|
||||
CVR_T = 1.8; // panel thickness
|
||||
HINGE_T = 0.6; // living-hinge strip thickness (printed in PETG)
|
||||
HINGE_W = 3.0; // hinge strip width (flexes easily)
|
||||
SNAP_HOOK_H = 3.5; // snap hook height
|
||||
SNAP_HOOK_T = 2.2;
|
||||
|
||||
// ── Cable saddle ─────────────────────────────────────────────
|
||||
SAD_W = 12.0;
|
||||
SAD_H = 8.0;
|
||||
SAD_T = 2.5;
|
||||
SAD_BORE_D = 7.0; // cable bundle bore
|
||||
SAD_CLIP_T = 1.6; // snap arm thickness
|
||||
|
||||
// ── Utilities ────────────────────────────────────────────────
|
||||
module chamfer_cube(size, ch=1.0) {
|
||||
hull() {
|
||||
translate([ch, ch, 0]) cube([size[0]-2*ch, size[1]-2*ch, EPS]);
|
||||
translate([0, 0, ch]) cube(size - [0, 0, ch]);
|
||||
}
|
||||
}
|
||||
|
||||
module hex_pocket(af, depth) {
|
||||
cylinder(d=af/cos(30), h=depth, $fn=6);
|
||||
}
|
||||
|
||||
// ── Part 1: tray_body ───────────────────────────────────────
|
||||
module tray_body() {
|
||||
difference() {
|
||||
// Outer shell
|
||||
union() {
|
||||
chamfer_cube([TRAY_L, TRAY_W, TRAY_DEPTH + TRAY_WALL], ch=1.5);
|
||||
|
||||
// Snap ledge along top of both long walls (for cover_panel)
|
||||
for (y = [-SNAP_LEDGE_D, TRAY_W])
|
||||
translate([0, y, TRAY_DEPTH])
|
||||
cube([TRAY_L, TRAY_WALL + SNAP_LEDGE_D, SNAP_LEDGE_H]);
|
||||
}
|
||||
|
||||
// Interior cavity
|
||||
translate([TRAY_WALL, TRAY_WALL, TRAY_WALL])
|
||||
cube([TRAY_L - 2*TRAY_WALL, TRAY_W - 2*TRAY_WALL,
|
||||
TRAY_DEPTH + EPS]);
|
||||
|
||||
// ── Zone dividers (subtract from solid to leave walls) ──
|
||||
// Zone A (Power) inner floor cut — full depth A
|
||||
translate([TRAY_WALL, TRAY_WALL, TRAY_WALL + (TRAY_DEPTH - ZONE_A_D)])
|
||||
cube([TRAY_L - 2*TRAY_WALL, ZONE_A_W, ZONE_A_D + EPS]);
|
||||
|
||||
// Zone B (Signal) inner floor cut
|
||||
translate([TRAY_WALL, TRAY_WALL + ZONE_A_W + DIV_T,
|
||||
TRAY_WALL + (TRAY_DEPTH - ZONE_B_D)])
|
||||
cube([TRAY_L - 2*TRAY_WALL, ZONE_B_W, ZONE_B_D + EPS]);
|
||||
|
||||
// Zone C (Servo) inner floor cut
|
||||
translate([TRAY_WALL, TRAY_WALL + ZONE_A_W + DIV_T + ZONE_B_W + DIV_T,
|
||||
TRAY_WALL + (TRAY_DEPTH - ZONE_C_D)])
|
||||
cube([TRAY_L - 2*TRAY_WALL, ZONE_C_W, ZONE_C_D + EPS]);
|
||||
|
||||
// ── Velcro tie-down slots (floor, every 40 mm) ──────────
|
||||
for (i = [0:TIEDOWN_CNT-1]) {
|
||||
x = TRAY_WALL + 20 + i * TIEDOWN_PITCH - TIEDOWN_W/2;
|
||||
// Zone A slot
|
||||
translate([x, TRAY_WALL + 2, -EPS])
|
||||
cube([TIEDOWN_W, ZONE_A_W - 4, TRAY_WALL + 2*EPS]);
|
||||
// Zone B slot
|
||||
translate([x, TRAY_WALL + ZONE_A_W + DIV_T + 2, -EPS])
|
||||
cube([TIEDOWN_W, ZONE_B_W - 4, TRAY_WALL + 2*EPS]);
|
||||
// Zone C slot
|
||||
translate([x, TRAY_WALL + ZONE_A_W + DIV_T + ZONE_B_W + DIV_T + 2, -EPS])
|
||||
cube([TIEDOWN_W, ZONE_C_W - 4, TRAY_WALL + 2*EPS]);
|
||||
}
|
||||
|
||||
// ── Pass-through holes in floor (centre of each zone at mid-length) ──
|
||||
mid_x = TRAY_L / 2;
|
||||
// Zone A
|
||||
translate([mid_x, TRAY_WALL + ZONE_A_W/2, -EPS])
|
||||
cylinder(d=PASSTHRU_D, h=TRAY_WALL + 2*EPS);
|
||||
// Zone B
|
||||
translate([mid_x, TRAY_WALL + ZONE_A_W + DIV_T + ZONE_B_W/2, -EPS])
|
||||
cylinder(d=PASSTHRU_D, h=TRAY_WALL + 2*EPS);
|
||||
// Zone C
|
||||
translate([mid_x, TRAY_WALL + ZONE_A_W + DIV_T + ZONE_B_W + DIV_T + ZONE_C_W/2, -EPS])
|
||||
cylinder(d=PASSTHRU_D, h=TRAY_WALL + 2*EPS);
|
||||
|
||||
// ── Label slots on front wall (y = 0) — one per zone ────
|
||||
zone_ctrs = [TRAY_WALL + ZONE_A_W/2,
|
||||
TRAY_WALL + ZONE_A_W + DIV_T + ZONE_B_W/2,
|
||||
TRAY_WALL + ZONE_A_W + DIV_T + ZONE_B_W + DIV_T + ZONE_C_W/2];
|
||||
label_z = TRAY_WALL + 2;
|
||||
for (yc = zone_ctrs)
|
||||
translate([TRAY_L/2 - LABEL_W/2, -EPS, label_z])
|
||||
cube([LABEL_W, LABEL_T + EPS, LABEL_H]);
|
||||
|
||||
// ── M3 bracket bolt holes in floor (4 corners) ──────────
|
||||
for (x = [20, TRAY_L - 20])
|
||||
for (y = [TRAY_W/4, 3*TRAY_W/4])
|
||||
translate([x, y, -EPS])
|
||||
cylinder(d=M3_D, h=TRAY_WALL + 2*EPS);
|
||||
|
||||
// ── Channel clip snap sockets (top of each divider, every 80 mm) ──
|
||||
for (i = [0:2]) {
|
||||
cx = 40 + i * 80;
|
||||
for (dy = [ZONE_A_W, ZONE_A_W + DIV_T + ZONE_B_W])
|
||||
translate([cx - 3, TRAY_WALL + dy - 1, TRAY_DEPTH - 4])
|
||||
cube([6, DIV_T + 2, 4 + EPS]);
|
||||
}
|
||||
}
|
||||
|
||||
// ── Divider walls (positive geometry) ───────────────────
|
||||
// Wall between Zone A and Zone B
|
||||
translate([TRAY_WALL, TRAY_WALL + ZONE_A_W, TRAY_WALL])
|
||||
cube([TRAY_L - 2*TRAY_WALL, DIV_T,
|
||||
TRAY_DEPTH - ZONE_A_D]); // partial height — lower in A zone
|
||||
|
||||
// Wall between Zone B and Zone C
|
||||
translate([TRAY_WALL, TRAY_WALL + ZONE_A_W + DIV_T + ZONE_B_W, TRAY_WALL])
|
||||
cube([TRAY_L - 2*TRAY_WALL, DIV_T,
|
||||
TRAY_DEPTH - ZONE_B_D]);
|
||||
}
|
||||
|
||||
// ── Part 2: tnut_bracket ────────────────────────────────────
|
||||
module tnut_bracket() {
|
||||
difference() {
|
||||
chamfer_cube([BKT_L, BKT_W, BKT_T], ch=1.5);
|
||||
|
||||
// M5 T-nut holes (2 per bracket, on rail centreline)
|
||||
for (x = [BKT_L/2 - BOLT_PITCH/2, BKT_L/2 + BOLT_PITCH/2]) {
|
||||
translate([x, BKT_W/2, -EPS]) {
|
||||
cylinder(d=M5_D, h=BKT_T + 2*EPS);
|
||||
cylinder(d=M5_HEAD_D, h=M5_HEAD_H + EPS);
|
||||
}
|
||||
translate([x - TNUT_L/2, BKT_W/2 - TNUT_W/2, BKT_T - TNUT_H])
|
||||
cube([TNUT_L, TNUT_W, TNUT_H + EPS]);
|
||||
}
|
||||
|
||||
// M3 tray-attachment holes (4 corners)
|
||||
for (x = [10, BKT_L - 10])
|
||||
for (y = [10, BKT_W - 10]) {
|
||||
translate([x, y, -EPS])
|
||||
cylinder(d=M3_D, h=BKT_T + 2*EPS);
|
||||
// M3 hex nut captured pocket (from top)
|
||||
translate([x, y, BKT_T - M3_NUT_H - 0.2])
|
||||
hex_pocket(M3_NUT_W + 0.3, M3_NUT_H + 0.3);
|
||||
}
|
||||
|
||||
// Weight relief
|
||||
translate([15, 8, -EPS])
|
||||
cube([BKT_L - 30, BKT_W - 16, BKT_T/2]);
|
||||
}
|
||||
}
|
||||
|
||||
// ── Part 3: channel_clip ────────────────────────────────────
|
||||
// Snap-in clip that locks into divider-wall snap sockets;
|
||||
// holds individual bundles in their zone and acts as colour-coded zone marker.
|
||||
module channel_clip() {
|
||||
clip_body_w = 6.0;
|
||||
clip_body_h = DIV_T + 4.0;
|
||||
clip_body_t = 8.0;
|
||||
tab_h = 3.5;
|
||||
tab_w = 2.5;
|
||||
|
||||
difference() {
|
||||
union() {
|
||||
// Body spanning divider
|
||||
cube([clip_body_t, clip_body_w, clip_body_h]);
|
||||
|
||||
// Snap tabs (bottom, straddle divider)
|
||||
for (s = [0, clip_body_w - tab_w])
|
||||
translate([clip_body_t/2 - 1, s, -tab_h])
|
||||
cube([2, tab_w, tab_h + 1]);
|
||||
}
|
||||
|
||||
// Cable radius slot on each face
|
||||
translate([-EPS, clip_body_w/2, clip_body_h * 0.6])
|
||||
rotate([0, 90, 0])
|
||||
cylinder(d=5.0, h=clip_body_t + 2*EPS);
|
||||
|
||||
// Snap tab undercut for flex
|
||||
for (s = [0, clip_body_w - tab_w])
|
||||
translate([clip_body_t/2 - 2, s - EPS, -tab_h + 1.5])
|
||||
cube([4, tab_w + 2*EPS, 1.5]);
|
||||
}
|
||||
}
|
||||
|
||||
// ── Part 4: cover_panel ─────────────────────────────────────
|
||||
// Flat snap-on lid with living-hinge along one long edge.
|
||||
// Print flat; PETG living hinge flexes ~90° to snap onto tray.
|
||||
module cover_panel() {
|
||||
total_w = TRAY_W + 2 * SNAP_HOOK_T;
|
||||
|
||||
difference() {
|
||||
union() {
|
||||
// Main panel
|
||||
cube([TRAY_L, TRAY_W, CVR_T]);
|
||||
|
||||
// Living hinge strip along back edge (thin, flexes)
|
||||
translate([0, TRAY_W - EPS, 0])
|
||||
cube([TRAY_L, HINGE_W, HINGE_T]);
|
||||
|
||||
// Snap hooks along front edge (clips under tray snap ledge)
|
||||
for (x = [20, TRAY_L/2 - 20, TRAY_L/2 + 20, TRAY_L - 20])
|
||||
translate([x - SNAP_HOOK_T/2, -SNAP_HOOK_H + EPS, 0])
|
||||
difference() {
|
||||
cube([SNAP_HOOK_T, SNAP_HOOK_H, CVR_T + 1.5]);
|
||||
// Hook nose chamfer
|
||||
translate([-EPS, -EPS, CVR_T])
|
||||
rotate([0, 0, 0])
|
||||
cube([SNAP_HOOK_T + 2*EPS, 1.5, 1.5]);
|
||||
}
|
||||
}
|
||||
|
||||
// Ventilation slots (3 rows × 6 slots)
|
||||
for (row = [0:2])
|
||||
for (col = [0:5]) {
|
||||
sx = 20 + col * 40 + row * 10;
|
||||
sy = 10 + row * 12;
|
||||
if (sx + 25 < TRAY_L && sy + 6 < TRAY_W)
|
||||
translate([sx, sy, -EPS])
|
||||
cube([25, 6, CVR_T + 2*EPS]);
|
||||
}
|
||||
|
||||
// Zone label windows (align with tray label slots)
|
||||
zone_ctrs = [TRAY_WALL + ZONE_A_W/2,
|
||||
TRAY_WALL + ZONE_A_W + DIV_T + ZONE_B_W/2,
|
||||
TRAY_WALL + ZONE_A_W + DIV_T + ZONE_B_W + DIV_T + ZONE_C_W/2];
|
||||
for (yc = zone_ctrs)
|
||||
translate([TRAY_L/2 - LABEL_W/2, yc - LABEL_H/2, -EPS])
|
||||
cube([LABEL_W, LABEL_H, CVR_T + 2*EPS]);
|
||||
}
|
||||
}
|
||||
|
||||
// ── Part 5: cable_saddle ────────────────────────────────────
|
||||
// Snap-in cable saddle / strain-relief clip; press-fits onto tray top edge.
|
||||
module cable_saddle() {
|
||||
arm_gap = TRAY_WALL + 0.4; // fits over tray wall
|
||||
arm_len = 8.0;
|
||||
|
||||
difference() {
|
||||
union() {
|
||||
// Body
|
||||
chamfer_cube([SAD_W, SAD_T * 2 + arm_gap, SAD_H], ch=1.0);
|
||||
|
||||
// Cable retaining arch
|
||||
translate([SAD_W/2, SAD_T + arm_gap/2, SAD_H])
|
||||
scale([1, 0.6, 1])
|
||||
difference() {
|
||||
cylinder(d=SAD_BORE_D + SAD_CLIP_T * 2, h=SAD_T);
|
||||
translate([0, 0, -EPS])
|
||||
cylinder(d=SAD_BORE_D, h=SAD_T + 2*EPS);
|
||||
translate([-SAD_BORE_D, 0, -EPS])
|
||||
cube([SAD_BORE_D * 2, SAD_BORE_D, SAD_T + 2*EPS]);
|
||||
}
|
||||
}
|
||||
|
||||
// Slot for tray wall (negative)
|
||||
translate([0, SAD_T, -EPS])
|
||||
cube([SAD_W, arm_gap, arm_len + EPS]);
|
||||
|
||||
// M3 tie-down hole
|
||||
translate([SAD_W/2, SAD_T + arm_gap/2, -EPS])
|
||||
cylinder(d=M3_D, h=SAD_H + 2*EPS);
|
||||
}
|
||||
}
|
||||
|
||||
// ── Assembly ────────────────────────────────────────────────
|
||||
module assembly() {
|
||||
// Tray body (open face up for visibility)
|
||||
color("SteelBlue")
|
||||
tray_body();
|
||||
|
||||
// Two T-nut brackets underneath at 1/4 and 3/4 length
|
||||
for (bx = [TRAY_L/4 - BKT_L/2, 3*TRAY_L/4 - BKT_L/2])
|
||||
color("DodgerBlue")
|
||||
translate([bx, 0, -BKT_T])
|
||||
tnut_bracket();
|
||||
|
||||
// Channel clips (3 per divider position, every 80 mm)
|
||||
for (i = [0:2]) {
|
||||
cx = 40 + i * 80;
|
||||
// Divider A/B
|
||||
color("Tomato", 0.8)
|
||||
translate([cx - 4, TRAY_WALL + ZONE_A_W - 2, TRAY_DEPTH - 3])
|
||||
channel_clip();
|
||||
// Divider B/C
|
||||
color("Orange", 0.8)
|
||||
translate([cx - 4,
|
||||
TRAY_WALL + ZONE_A_W + DIV_T + ZONE_B_W - 2,
|
||||
TRAY_DEPTH - 3])
|
||||
channel_clip();
|
||||
}
|
||||
|
||||
// Cover panel (raised above tray to show interior)
|
||||
color("LightSteelBlue", 0.5)
|
||||
translate([0, 0, TRAY_DEPTH + SNAP_LEDGE_H + 4])
|
||||
cover_panel();
|
||||
|
||||
// Cable saddles along front tray edge
|
||||
for (x = [40, 120, 200])
|
||||
color("SlateGray")
|
||||
translate([x - SAD_W/2, -SAD_T * 2 - TRAY_WALL, 0])
|
||||
cable_saddle();
|
||||
}
|
||||
|
||||
// ── Dispatch ────────────────────────────────────────────────
|
||||
if (RENDER == "tray_body") tray_body();
|
||||
else if (RENDER == "tnut_bracket") tnut_bracket();
|
||||
else if (RENDER == "channel_clip") channel_clip();
|
||||
else if (RENDER == "cover_panel") cover_panel();
|
||||
else if (RENDER == "cable_saddle") cable_saddle();
|
||||
else assembly();
|
||||
@ -271,4 +271,11 @@
|
||||
#define LVC_HYSTERESIS_MV 200u // recovery hysteresis to prevent threshold chatter
|
||||
#define LVC_TLM_HZ 1u // JLINK_TLM_LVC transmit rate (Hz)
|
||||
|
||||
|
||||
// --- UART Command Protocol (Issue #629) ---
|
||||
// Jetson-STM32 binary command protocol on UART5 (PC12/PD2)
|
||||
// NOTE: Spec requested USART1 @ 115200; USART1 is occupied by JLink @ 921600.
|
||||
#define UART_PROT_BAUD 115200u // baud rate for UART5 Jetson protocol
|
||||
#define UART_PROT_HB_TIMEOUT_MS 500u // heartbeat timeout: Jetson considered lost after 500 ms
|
||||
|
||||
#endif // CONFIG_H
|
||||
|
||||
@ -1,128 +1,95 @@
|
||||
#ifndef UART_PROTOCOL_H
|
||||
#define UART_PROTOCOL_H
|
||||
|
||||
/*
|
||||
* uart_protocol.h — UART command protocol for Jetson-STM32 communication (Issue #629)
|
||||
*
|
||||
* Frame format:
|
||||
* [STX][LEN][CMD][PAYLOAD...][CRC8][ETX]
|
||||
* 0x02 1B 1B 0-12 B 1B 0x03
|
||||
*
|
||||
* CRC8-SMBUS: poly=0x07, init=0x00, computed over CMD+PAYLOAD bytes.
|
||||
*
|
||||
* Physical layer: UART5 (PC12=TX / PD2=RX), GPIO_AF8_UART5, 115200 baud, no hw flow.
|
||||
* NOTE: Spec requested USART1 @ 115200, but USART1 is occupied by JLink @ 921600.
|
||||
* Implemented on UART5 instead; Jetson must connect to PC12/PD2.
|
||||
*
|
||||
* DMA: DMA1_Stream0_Channel4, circular 256-byte ring buffer.
|
||||
* Heartbeat: if no frame received in UART_PROT_HB_TIMEOUT_MS (500 ms), Jetson is
|
||||
* considered lost; caller must handle estop if needed.
|
||||
*/
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
|
||||
/*
|
||||
* uart_protocol.h -- Structured UART command protocol for Jetson-STM32
|
||||
* communication (Issue #629)
|
||||
*
|
||||
* Hardware: UART5, PC12=TX / PD2=RX, 115200 baud
|
||||
* Note: spec references USART1 but USART1 is occupied by JLink (Issue #120)
|
||||
* at 921600 baud. UART5 is the designated secondary/debug UART and is used
|
||||
* here as the structured-command channel.
|
||||
*
|
||||
* Frame format (both directions):
|
||||
* [STX=0x02][LEN][CMD][PAYLOAD...][CRC8][ETX=0x03]
|
||||
*
|
||||
* STX : frame start sentinel (0x02)
|
||||
* LEN : CMD byte + payload bytes (1 + payload_len)
|
||||
* CMD : command / response type byte
|
||||
* PAYLOAD : 0..N bytes, CMD-dependent
|
||||
* CRC8 : CRC8-SMBUS (poly 0x07, init 0x00) over CMD+PAYLOAD
|
||||
* ETX : frame end sentinel (0x03)
|
||||
*
|
||||
* Host (Jetson) to STM32 commands:
|
||||
* 0x01 SET_VELOCITY - int16 left_rpm, int16 right_rpm (4 bytes)
|
||||
* 0x02 GET_STATUS - no payload; reply URESP_STATUS
|
||||
* 0x03 SET_PID - float kp, float ki, float kd (12 bytes, LE)
|
||||
* 0x04 ESTOP - no payload; engage emergency stop
|
||||
* 0x05 CLEAR_ESTOP - no payload; clear emergency stop
|
||||
*
|
||||
* STM32 to host responses:
|
||||
* 0x80 ACK - uint8 echoed_cmd (1 byte)
|
||||
* 0x81 NACK - uint8 echoed_cmd, uint8 error_code (2 bytes)
|
||||
* 0x82 STATUS - uart_prot_status_t (8 bytes)
|
||||
*
|
||||
* Heartbeat: if no valid frame is received within UART_PROT_HB_TIMEOUT_MS,
|
||||
* uart_protocol_is_active() returns false and velocity commands are ignored.
|
||||
*
|
||||
* DMA: UART5_RX uses DMA1_Stream0_Channel4 in circular mode (256-byte ring).
|
||||
* Parser runs in uart_protocol_process() called from the main loop.
|
||||
*/
|
||||
/* ── Frame delimiters ─────────────────────────────────────────────────────── */
|
||||
#define UPROT_STX 0x02u
|
||||
#define UPROT_ETX 0x03u
|
||||
|
||||
/* ---- Frame constants ---- */
|
||||
#define UPROT_STX 0x02u
|
||||
#define UPROT_ETX 0x03u
|
||||
#define UPROT_RX_BUF_LEN 256u /* DMA ring buffer size (power of 2) */
|
||||
#define UPROT_MAX_PAYLOAD 16u /* largest payload: SET_PID = 12 bytes */
|
||||
/* ── Command IDs (host → STM32) ───────────────────────────────────────────── */
|
||||
#define UCMD_SET_VELOCITY 0x01u /* payload: int16 left_rpm, int16 right_rpm (4 B) */
|
||||
#define UCMD_GET_STATUS 0x02u /* payload: none */
|
||||
#define UCMD_SET_PID 0x03u /* payload: float kp, float ki, float kd (12 B) */
|
||||
#define UCMD_ESTOP 0x04u /* payload: none */
|
||||
#define UCMD_CLEAR_ESTOP 0x05u /* payload: none */
|
||||
|
||||
/* ---- Command IDs (host to STM32) ---- */
|
||||
#define UCMD_SET_VELOCITY 0x01u /* int16 left_rpm + int16 right_rpm */
|
||||
#define UCMD_GET_STATUS 0x02u /* no payload */
|
||||
#define UCMD_SET_PID 0x03u /* float kp, ki, kd (12 bytes LE) */
|
||||
#define UCMD_ESTOP 0x04u /* no payload */
|
||||
#define UCMD_CLEAR_ESTOP 0x05u /* no payload */
|
||||
/* ── Response IDs (STM32 → host) ──────────────────────────────────────────── */
|
||||
#define URESP_ACK 0x80u /* payload: 1 B — echoed CMD */
|
||||
#define URESP_NACK 0x81u /* payload: 2 B — CMD, error_code */
|
||||
#define URESP_STATUS 0x82u /* payload: sizeof(uart_prot_status_t) = 8 B */
|
||||
|
||||
/* ---- Response IDs (STM32 to host) ---- */
|
||||
#define URESP_ACK 0x80u /* 1-byte payload: echoed CMD */
|
||||
#define URESP_NACK 0x81u /* 2-byte payload: CMD + error_code */
|
||||
#define URESP_STATUS 0x82u /* 8-byte payload: uart_prot_status_t */
|
||||
/* ── NACK error codes ─────────────────────────────────────────────────────── */
|
||||
#define UERR_BAD_CRC 0x01u
|
||||
#define UERR_BAD_LEN 0x02u
|
||||
#define UERR_BAD_ETX 0x03u
|
||||
#define UERR_ESTOP 0x04u /* command rejected — estop active */
|
||||
#define UERR_DISARMED 0x05u /* velocity rejected — not armed */
|
||||
|
||||
/* ---- NACK error codes ---- */
|
||||
#define UERR_UNKNOWN_CMD 0x01u /* unrecognised CMD byte */
|
||||
#define UERR_BAD_LENGTH 0x02u /* LEN does not match CMD expectation */
|
||||
#define UERR_BAD_CRC 0x03u /* CRC8 mismatch */
|
||||
#define UERR_REFUSED 0x04u /* command refused in current state */
|
||||
|
||||
/* ---- STATUS response payload (8 bytes, packed) ---- */
|
||||
/* ── STATUS payload (URESP_STATUS, 8 bytes packed) ───────────────────────── */
|
||||
typedef struct __attribute__((packed)) {
|
||||
int16_t pitch_x10; /* pitch angle, degrees x10 (0.1° resolution) */
|
||||
int16_t motor_cmd; /* balance PID ESC output, -1000..+1000 */
|
||||
uint16_t vbat_mv; /* battery voltage (mV) */
|
||||
uint8_t balance_state; /* BalanceState: 0=DISARMED, 1=ARMED, 2=TILT */
|
||||
uint8_t estop_active; /* 1 = estop currently engaged */
|
||||
} uart_prot_status_t; /* 8 bytes */
|
||||
int16_t pitch_x10; /* pitch angle ×10 deg (balance controller) */
|
||||
int16_t motor_cmd; /* ESC motor command -1000..+1000 */
|
||||
uint16_t vbat_mv; /* battery voltage in mV */
|
||||
uint8_t balance_state; /* BalanceState enum (0=DISARMED, 1=ARMED, …) */
|
||||
uint8_t estop_active; /* non-zero if remote estop is latched */
|
||||
} uart_prot_status_t;
|
||||
|
||||
/* ---- Volatile state (read from main loop) ---- */
|
||||
/* ── Shared state (read by main.c) ────────────────────────────────────────── */
|
||||
typedef struct {
|
||||
/* SET_VELOCITY: set by parser, cleared by main loop */
|
||||
volatile uint8_t vel_updated; /* 1 = new velocity command pending */
|
||||
volatile int16_t left_rpm; /* requested left wheel RPM */
|
||||
volatile int16_t right_rpm; /* requested right wheel RPM */
|
||||
volatile uint8_t vel_updated; /* 1 when SET_VELOCITY received */
|
||||
volatile int16_t left_rpm;
|
||||
volatile int16_t right_rpm;
|
||||
|
||||
/* SET_PID: set by parser, cleared by main loop */
|
||||
volatile uint8_t pid_updated; /* 1 = new PID gains pending */
|
||||
volatile uint8_t pid_updated; /* 1 when SET_PID received */
|
||||
volatile float pid_kp;
|
||||
volatile float pid_ki;
|
||||
volatile float pid_kd;
|
||||
|
||||
/* ESTOP / CLEAR_ESTOP: set by parser, cleared by main loop */
|
||||
volatile uint8_t estop_req;
|
||||
volatile uint8_t estop_clear_req;
|
||||
volatile uint8_t estop_req; /* 1 on UCMD_ESTOP */
|
||||
volatile uint8_t estop_clear_req; /* 1 on UCMD_CLEAR_ESTOP */
|
||||
|
||||
/* Heartbeat: updated on every valid frame */
|
||||
volatile uint32_t last_rx_ms; /* HAL_GetTick() at last good frame; 0=none */
|
||||
volatile uint32_t last_rx_ms; /* HAL_GetTick() of last valid frame */
|
||||
} UartProtState;
|
||||
|
||||
extern volatile UartProtState uart_prot_state;
|
||||
extern UartProtState uart_prot_state;
|
||||
|
||||
/* ---- API ---- */
|
||||
/* ── API ───────────────────────────────────────────────────────────────────── */
|
||||
|
||||
/*
|
||||
* uart_protocol_init() -- configure UART5 + DMA1_Stream0 and start reception.
|
||||
* Call once during system init, before the main loop.
|
||||
/**
|
||||
* uart_protocol_init() — configure UART5 + DMA, start circular receive.
|
||||
* Must be called once during system init, before main loop.
|
||||
*/
|
||||
void uart_protocol_init(void);
|
||||
|
||||
/*
|
||||
* uart_protocol_process() -- parse new bytes from DMA ring buffer, dispatch
|
||||
* commands, and send ACK/NACK/STATUS responses.
|
||||
* Call every main loop tick (non-blocking, < 5 µs when idle).
|
||||
/**
|
||||
* uart_protocol_process() — drain DMA ring buffer, parse frames, dispatch commands.
|
||||
* Call once per main loop iteration (every ~1 ms).
|
||||
*/
|
||||
void uart_protocol_process(void);
|
||||
|
||||
/*
|
||||
* uart_protocol_is_active(now_ms) -- returns true if a valid frame was
|
||||
* received within UART_PROT_HB_TIMEOUT_MS. Main loop uses this to gate
|
||||
* SET_VELOCITY commands when the host is disconnected.
|
||||
*/
|
||||
bool uart_protocol_is_active(uint32_t now_ms);
|
||||
|
||||
/*
|
||||
* uart_protocol_send_status(s) -- transmit a URESP_STATUS (0x82) frame.
|
||||
* Called from main loop in response to GET_STATUS or periodically.
|
||||
/**
|
||||
* uart_protocol_send_status() — build and TX a URESP_STATUS frame.
|
||||
* @param s Pointer to status payload to send.
|
||||
*/
|
||||
void uart_protocol_send_status(const uart_prot_status_t *s);
|
||||
|
||||
|
||||
@ -0,0 +1,34 @@
|
||||
aruco_detect:
|
||||
ros__parameters:
|
||||
# ── Marker specification ─────────────────────────────────────────────────
|
||||
marker_size_m: 0.10 # physical side length of printed ArUco markers (m)
|
||||
# measure the black border edge-to-edge
|
||||
|
||||
# ── Dock target filter ───────────────────────────────────────────────────
|
||||
# dock_marker_ids: IDs that may serve as dock target.
|
||||
# - Empty list [] → any detected marker is a dock candidate (closest wins)
|
||||
# - Non-empty → only listed IDs are candidates; others still appear in
|
||||
# /saltybot/aruco/markers but not in dock_target
|
||||
# The saltybot dock uses marker ID 42 on the charging face by convention.
|
||||
dock_marker_ids: [42]
|
||||
|
||||
# Maximum distance to consider a marker as dock target (m)
|
||||
max_dock_range_m: 3.0
|
||||
|
||||
# ── ArUco dictionary ─────────────────────────────────────────────────────
|
||||
aruco_dict: DICT_4X4_50 # cv2.aruco constant name
|
||||
|
||||
# Corner sub-pixel refinement (improves pose accuracy at close range)
|
||||
# CORNER_REFINE_NONE — fastest, no refinement
|
||||
# CORNER_REFINE_SUBPIX — best for high-contrast markers (recommended)
|
||||
# CORNER_REFINE_CONTOUR — good for blurry or low-res images
|
||||
corner_refinement: CORNER_REFINE_SUBPIX
|
||||
|
||||
# ── Frame / topic config ─────────────────────────────────────────────────
|
||||
camera_frame: camera_color_optical_frame
|
||||
|
||||
# ── Debug image output (disabled by default) ─────────────────────────────
|
||||
# When true, publishes annotated BGR image with markers + axes drawn.
|
||||
# Useful for tuning but costs ~10 ms/frame encoding overhead.
|
||||
draw_debug_image: false
|
||||
debug_image_topic: /saltybot/aruco/debug_image
|
||||
@ -0,0 +1,29 @@
|
||||
"""aruco_detect.launch.py — ArUco marker detection for docking (Issue #627)."""
|
||||
|
||||
import os
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
pkg_share = get_package_share_directory('saltybot_aruco_detect')
|
||||
default_params = os.path.join(pkg_share, 'config', 'aruco_detect_params.yaml')
|
||||
|
||||
params_arg = DeclareLaunchArgument(
|
||||
'params_file',
|
||||
default_value=default_params,
|
||||
description='Path to aruco_detect_params.yaml',
|
||||
)
|
||||
|
||||
aruco_node = Node(
|
||||
package='saltybot_aruco_detect',
|
||||
executable='aruco_detect',
|
||||
name='aruco_detect',
|
||||
output='screen',
|
||||
parameters=[LaunchConfiguration('params_file')],
|
||||
)
|
||||
|
||||
return LaunchDescription([params_arg, aruco_node])
|
||||
37
jetson/ros2_ws/src/saltybot_aruco_detect/package.xml
Normal file
37
jetson/ros2_ws/src/saltybot_aruco_detect/package.xml
Normal file
@ -0,0 +1,37 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>saltybot_aruco_detect</name>
|
||||
<version>0.1.0</version>
|
||||
<description>
|
||||
ArUco marker detection for docking alignment (Issue #627).
|
||||
Detects all DICT_4X4_50 markers from RealSense D435i RGB, estimates 6-DOF
|
||||
pose with estimatePoseSingleMarkers, publishes PoseArray on
|
||||
/saltybot/aruco/markers and closest dock-candidate marker on
|
||||
/saltybot/aruco/dock_target (PoseStamped) with RViz MarkerArray and
|
||||
JSON status.
|
||||
</description>
|
||||
<maintainer email="sl-perception@saltylab.local">sl-perception</maintainer>
|
||||
<license>MIT</license>
|
||||
|
||||
<buildtool_depend>ament_python</buildtool_depend>
|
||||
|
||||
<depend>rclpy</depend>
|
||||
<depend>std_msgs</depend>
|
||||
<depend>sensor_msgs</depend>
|
||||
<depend>geometry_msgs</depend>
|
||||
<depend>visualization_msgs</depend>
|
||||
<depend>cv_bridge</depend>
|
||||
|
||||
<exec_depend>python3-numpy</exec_depend>
|
||||
<exec_depend>python3-opencv</exec_depend>
|
||||
|
||||
<test_depend>ament_copyright</test_depend>
|
||||
<test_depend>ament_flake8</test_depend>
|
||||
<test_depend>ament_pep257</test_depend>
|
||||
<test_depend>python3-pytest</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_python</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@ -0,0 +1,518 @@
|
||||
"""
|
||||
aruco_detect_node.py — ArUco marker detection for docking alignment (Issue #627).
|
||||
|
||||
Detects all DICT_4X4_50 ArUco markers visible in the RealSense D435i RGB stream,
|
||||
estimates their 6-DOF poses relative to the camera using estimatePoseSingleMarkers,
|
||||
and publishes:
|
||||
|
||||
/saltybot/aruco/markers geometry_msgs/PoseArray — all detected markers
|
||||
/saltybot/aruco/dock_target geometry_msgs/PoseStamped — closest dock marker
|
||||
/saltybot/aruco/viz visualization_msgs/MarkerArray — RViz axes overlays
|
||||
/saltybot/aruco/status std_msgs/String — JSON summary (10 Hz)
|
||||
|
||||
Coordinate frame
|
||||
────────────────
|
||||
All poses are expressed in camera_color_optical_frame (ROS optical convention):
|
||||
+X = right, +Y = down, +Z = forward (into scene)
|
||||
|
||||
The dock_target pose gives the docking controller everything it needs:
|
||||
position.z — forward distance to dock (m) → throttle target
|
||||
position.x — lateral offset (m) → steer target
|
||||
orientation — marker orientation for final align → yaw servo
|
||||
|
||||
Dock target selection
|
||||
─────────────────────
|
||||
dock_marker_ids list[int] [] — empty = accept any detected marker
|
||||
non-empty = only these IDs are candidates
|
||||
closest_wins bool true — among candidates, prefer the nearest
|
||||
max_dock_range_m float 3.0 — ignore markers beyond this distance
|
||||
|
||||
estimatePoseSingleMarkers API
|
||||
──────────────────────────────
|
||||
Uses cv2.aruco.estimatePoseSingleMarkers (legacy OpenCV API still present in
|
||||
4.x with a deprecation notice). Falls back to per-marker cv2.solvePnP with
|
||||
SOLVEPNP_IPPE_SQUARE if the legacy function is unavailable.
|
||||
Both paths use the same camera_matrix / dist_coeffs from CameraInfo.
|
||||
|
||||
Subscribes
|
||||
──────────
|
||||
/camera/color/image_raw sensor_msgs/Image 30 Hz (BGR8)
|
||||
/camera/color/camera_info sensor_msgs/CameraInfo (latched, once)
|
||||
|
||||
Publishes
|
||||
─────────
|
||||
/saltybot/aruco/markers geometry_msgs/PoseArray
|
||||
/saltybot/aruco/dock_target geometry_msgs/PoseStamped
|
||||
/saltybot/aruco/viz visualization_msgs/MarkerArray
|
||||
/saltybot/aruco/status std_msgs/String (JSON, 10 Hz)
|
||||
|
||||
Parameters (see config/aruco_detect_params.yaml)
|
||||
────────────────────────────────────────────────
|
||||
marker_size_m float 0.10 physical side length of printed markers (m)
|
||||
dock_marker_ids int[] [] IDs to accept as dock targets (empty = all)
|
||||
max_dock_range_m float 3.0 ignore candidates beyond this (m)
|
||||
aruco_dict str DICT_4X4_50
|
||||
corner_refinement str CORNER_REFINE_SUBPIX
|
||||
camera_frame str camera_color_optical_frame
|
||||
draw_debug_image bool false publish annotated image for debugging
|
||||
debug_image_topic str /saltybot/aruco/debug_image
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import json
|
||||
import math
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from rclpy.qos import (
|
||||
QoSProfile, ReliabilityPolicy, HistoryPolicy, DurabilityPolicy
|
||||
)
|
||||
|
||||
import numpy as np
|
||||
import cv2
|
||||
from cv_bridge import CvBridge
|
||||
|
||||
from geometry_msgs.msg import (
|
||||
Pose, PoseArray, PoseStamped, Point, Quaternion, Vector3, TransformStamped
|
||||
)
|
||||
from sensor_msgs.msg import Image, CameraInfo
|
||||
from std_msgs.msg import Header, String, ColorRGBA
|
||||
from visualization_msgs.msg import Marker, MarkerArray
|
||||
|
||||
from .aruco_math import MarkerPose, rvec_to_quat, tvec_distance
|
||||
|
||||
|
||||
# ── QoS ───────────────────────────────────────────────────────────────────────
|
||||
_SENSOR_QOS = QoSProfile(
|
||||
reliability=ReliabilityPolicy.BEST_EFFORT,
|
||||
history=HistoryPolicy.KEEP_LAST,
|
||||
depth=5,
|
||||
)
|
||||
_LATCHED_QOS = QoSProfile(
|
||||
reliability=ReliabilityPolicy.RELIABLE,
|
||||
history=HistoryPolicy.KEEP_LAST,
|
||||
depth=1,
|
||||
durability=DurabilityPolicy.TRANSIENT_LOCAL,
|
||||
)
|
||||
|
||||
# ArUco axis length for viz (fraction of marker size)
|
||||
_AXIS_LEN_FRAC = 0.5
|
||||
|
||||
|
||||
class ArucoDetectNode(Node):
|
||||
"""ArUco marker detection + pose estimation node — see module docstring."""
|
||||
|
||||
def __init__(self) -> None:
|
||||
super().__init__('aruco_detect')
|
||||
|
||||
# ── Parameters ────────────────────────────────────────────────────────
|
||||
self.declare_parameter('marker_size_m', 0.10)
|
||||
self.declare_parameter('dock_marker_ids', []) # empty list = all
|
||||
self.declare_parameter('max_dock_range_m', 3.0)
|
||||
self.declare_parameter('aruco_dict', 'DICT_4X4_50')
|
||||
self.declare_parameter('corner_refinement', 'CORNER_REFINE_SUBPIX')
|
||||
self.declare_parameter('camera_frame', 'camera_color_optical_frame')
|
||||
self.declare_parameter('draw_debug_image', False)
|
||||
self.declare_parameter('debug_image_topic', '/saltybot/aruco/debug_image')
|
||||
|
||||
self._marker_size = self.get_parameter('marker_size_m').value
|
||||
self._dock_ids = set(self.get_parameter('dock_marker_ids').value)
|
||||
self._max_range = self.get_parameter('max_dock_range_m').value
|
||||
self._cam_frame = self.get_parameter('camera_frame').value
|
||||
self._draw_debug = self.get_parameter('draw_debug_image').value
|
||||
self._debug_topic = self.get_parameter('debug_image_topic').value
|
||||
|
||||
aruco_dict_name = self.get_parameter('aruco_dict').value
|
||||
refine_name = self.get_parameter('corner_refinement').value
|
||||
|
||||
# ── ArUco detector setup ──────────────────────────────────────────────
|
||||
dict_id = getattr(cv2.aruco, aruco_dict_name, cv2.aruco.DICT_4X4_50)
|
||||
params = cv2.aruco.DetectorParameters()
|
||||
# Apply corner refinement
|
||||
refine_id = getattr(cv2.aruco, refine_name, cv2.aruco.CORNER_REFINE_SUBPIX)
|
||||
params.cornerRefinementMethod = refine_id
|
||||
|
||||
aruco_dict = cv2.aruco.getPredefinedDictionary(dict_id)
|
||||
self._detector = cv2.aruco.ArucoDetector(aruco_dict, params)
|
||||
|
||||
# Object points for solvePnP fallback (counter-clockwise from top-left)
|
||||
half = self._marker_size / 2.0
|
||||
self._obj_pts = np.array([
|
||||
[-half, half, 0.0],
|
||||
[ half, half, 0.0],
|
||||
[ half, -half, 0.0],
|
||||
[-half, -half, 0.0],
|
||||
], dtype=np.float64)
|
||||
|
||||
# ── Camera intrinsics (set on first CameraInfo) ───────────────────────
|
||||
self._K: np.ndarray | None = None
|
||||
self._dist: np.ndarray | None = None
|
||||
self._bridge = CvBridge()
|
||||
|
||||
# ── Publishers ─────────────────────────────────────────────────────────
|
||||
self._pose_array_pub = self.create_publisher(
|
||||
PoseArray, '/saltybot/aruco/markers', 10
|
||||
)
|
||||
self._dock_pub = self.create_publisher(
|
||||
PoseStamped, '/saltybot/aruco/dock_target', 10
|
||||
)
|
||||
self._viz_pub = self.create_publisher(
|
||||
MarkerArray, '/saltybot/aruco/viz', 10
|
||||
)
|
||||
self._status_pub = self.create_publisher(
|
||||
String, '/saltybot/aruco/status', 10
|
||||
)
|
||||
if self._draw_debug:
|
||||
self._debug_pub = self.create_publisher(
|
||||
Image, self._debug_topic, 5
|
||||
)
|
||||
else:
|
||||
self._debug_pub = None
|
||||
|
||||
# ── Subscriptions ──────────────────────────────────────────────────────
|
||||
self.create_subscription(
|
||||
CameraInfo,
|
||||
'/camera/color/camera_info',
|
||||
self._on_camera_info,
|
||||
_LATCHED_QOS,
|
||||
)
|
||||
self.create_subscription(
|
||||
Image,
|
||||
'/camera/color/image_raw',
|
||||
self._on_image,
|
||||
_SENSOR_QOS,
|
||||
)
|
||||
|
||||
# ── Status timer (10 Hz) ────────────────────────────────────────────────
|
||||
self._last_detections: list[MarkerPose] = []
|
||||
self.create_timer(0.1, self._on_status_timer)
|
||||
|
||||
self.get_logger().info(
|
||||
f'aruco_detect ready — '
|
||||
f'dict={aruco_dict_name} '
|
||||
f'marker_size={self._marker_size * 100:.0f}cm '
|
||||
f'dock_ids={list(self._dock_ids) if self._dock_ids else "any"} '
|
||||
f'max_range={self._max_range}m'
|
||||
)
|
||||
|
||||
# ── Camera info ───────────────────────────────────────────────────────────
|
||||
|
||||
def _on_camera_info(self, msg: CameraInfo) -> None:
|
||||
if self._K is not None:
|
||||
return
|
||||
self._K = np.array(msg.k, dtype=np.float64).reshape(3, 3)
|
||||
self._dist = np.array(msg.d, dtype=np.float64).reshape(1, -1)
|
||||
self.get_logger().info(
|
||||
f'camera_info received — '
|
||||
f'fx={self._K[0,0]:.1f} fy={self._K[1,1]:.1f}'
|
||||
)
|
||||
|
||||
# ── Image callback ────────────────────────────────────────────────────────
|
||||
|
||||
def _on_image(self, msg: Image) -> None:
|
||||
if self._K is None:
|
||||
return
|
||||
|
||||
# ── Decode ────────────────────────────────────────────────────────────
|
||||
try:
|
||||
bgr = self._bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8')
|
||||
except Exception as exc:
|
||||
self.get_logger().warning(f'image decode error: {exc}',
|
||||
throttle_duration_sec=5.0)
|
||||
return
|
||||
|
||||
# Convert to greyscale for detection (faster, same accuracy)
|
||||
gray = cv2.cvtColor(bgr, cv2.COLOR_BGR2GRAY)
|
||||
|
||||
stamp = msg.header.stamp
|
||||
|
||||
# ── Detect markers ────────────────────────────────────────────────────
|
||||
corners, ids, _ = self._detector.detectMarkers(gray)
|
||||
|
||||
if ids is None or len(ids) == 0:
|
||||
self._last_detections = []
|
||||
self._publish_empty(stamp)
|
||||
return
|
||||
|
||||
ids_flat = ids.flatten().tolist()
|
||||
|
||||
# ── Pose estimation ───────────────────────────────────────────────────
|
||||
detections = self._estimate_poses(corners, ids_flat)
|
||||
|
||||
# Filter by range
|
||||
detections = [d for d in detections if d.distance_m <= self._max_range]
|
||||
|
||||
self._last_detections = detections
|
||||
|
||||
# ── Publish ───────────────────────────────────────────────────────────
|
||||
self._publish_pose_array(detections, stamp)
|
||||
self._publish_dock_target(detections, stamp)
|
||||
self._publish_viz(detections, stamp)
|
||||
|
||||
# Debug image
|
||||
if self._debug_pub is not None:
|
||||
self._publish_debug(bgr, corners, ids, detections, stamp)
|
||||
|
||||
# ── Pose estimation ───────────────────────────────────────────────────────
|
||||
|
||||
def _estimate_poses(
|
||||
self,
|
||||
corners: list,
|
||||
ids: list[int],
|
||||
) -> list[MarkerPose]:
|
||||
"""
|
||||
Estimate 6-DOF pose for each detected marker.
|
||||
|
||||
Uses estimatePoseSingleMarkers (legacy API, still present in cv2 4.x).
|
||||
Falls back to per-marker solvePnP(IPPE_SQUARE) if unavailable.
|
||||
"""
|
||||
results: list[MarkerPose] = []
|
||||
|
||||
# ── Try legacy estimatePoseSingleMarkers ──────────────────────────────
|
||||
if hasattr(cv2.aruco, 'estimatePoseSingleMarkers'):
|
||||
try:
|
||||
rvecs, tvecs, _ = cv2.aruco.estimatePoseSingleMarkers(
|
||||
corners, self._marker_size, self._K, self._dist
|
||||
)
|
||||
for i, mid in enumerate(ids):
|
||||
rvec = rvecs[i].ravel().astype(np.float64)
|
||||
tvec = tvecs[i].ravel().astype(np.float64)
|
||||
results.append(MarkerPose(
|
||||
marker_id = int(mid),
|
||||
tvec = tvec,
|
||||
rvec = rvec,
|
||||
corners = corners[i].reshape(4, 2).astype(np.float64),
|
||||
))
|
||||
return results
|
||||
except Exception as exc:
|
||||
self.get_logger().debug(
|
||||
f'estimatePoseSingleMarkers failed ({exc}), '
|
||||
'falling back to solvePnP'
|
||||
)
|
||||
|
||||
# ── Fallback: per-marker solvePnP ─────────────────────────────────────
|
||||
for i, mid in enumerate(ids):
|
||||
img_pts = corners[i].reshape(4, 2).astype(np.float64)
|
||||
ok, rvec, tvec = cv2.solvePnP(
|
||||
self._obj_pts, img_pts,
|
||||
self._K, self._dist,
|
||||
flags=cv2.SOLVEPNP_IPPE_SQUARE,
|
||||
)
|
||||
if not ok:
|
||||
continue
|
||||
results.append(MarkerPose(
|
||||
marker_id = int(mid),
|
||||
tvec = tvec.ravel().astype(np.float64),
|
||||
rvec = rvec.ravel().astype(np.float64),
|
||||
corners = img_pts,
|
||||
))
|
||||
return results
|
||||
|
||||
# ── Dock target selection ─────────────────────────────────────────────────
|
||||
|
||||
def _select_dock_target(
|
||||
self,
|
||||
detections: list[MarkerPose],
|
||||
) -> MarkerPose | None:
|
||||
"""Select the closest marker that matches dock_marker_ids filter."""
|
||||
candidates = [
|
||||
d for d in detections
|
||||
if not self._dock_ids or d.marker_id in self._dock_ids
|
||||
]
|
||||
if not candidates:
|
||||
return None
|
||||
return min(candidates, key=lambda d: d.distance_m)
|
||||
|
||||
# ── Publishers ────────────────────────────────────────────────────────────
|
||||
|
||||
def _publish_pose_array(
|
||||
self,
|
||||
detections: list[MarkerPose],
|
||||
stamp,
|
||||
) -> None:
|
||||
pa = PoseArray()
|
||||
pa.header.stamp = stamp
|
||||
pa.header.frame_id = self._cam_frame
|
||||
|
||||
for d in detections:
|
||||
qx, qy, qz, qw = d.quat
|
||||
pose = Pose()
|
||||
pose.position = Point(x=float(d.tvec[0]),
|
||||
y=float(d.tvec[1]),
|
||||
z=float(d.tvec[2]))
|
||||
pose.orientation = Quaternion(x=qx, y=qy, z=qz, w=qw)
|
||||
pa.poses.append(pose)
|
||||
|
||||
self._pose_array_pub.publish(pa)
|
||||
|
||||
def _publish_dock_target(
|
||||
self,
|
||||
detections: list[MarkerPose],
|
||||
stamp,
|
||||
) -> None:
|
||||
target = self._select_dock_target(detections)
|
||||
if target is None:
|
||||
return
|
||||
|
||||
qx, qy, qz, qw = target.quat
|
||||
ps = PoseStamped()
|
||||
ps.header.stamp = stamp
|
||||
ps.header.frame_id = self._cam_frame
|
||||
ps.pose.position = Point(x=float(target.tvec[0]),
|
||||
y=float(target.tvec[1]),
|
||||
z=float(target.tvec[2]))
|
||||
ps.pose.orientation = Quaternion(x=qx, y=qy, z=qz, w=qw)
|
||||
self._dock_pub.publish(ps)
|
||||
|
||||
def _publish_viz(
|
||||
self,
|
||||
detections: list[MarkerPose],
|
||||
stamp,
|
||||
) -> None:
|
||||
"""Publish coordinate-axes MarkerArray for each detected marker."""
|
||||
ma = MarkerArray()
|
||||
lifetime = _ros_duration(0.2)
|
||||
|
||||
# Delete-all
|
||||
dm = Marker()
|
||||
dm.action = Marker.DELETEALL
|
||||
dm.header.stamp = stamp
|
||||
dm.header.frame_id = self._cam_frame
|
||||
ma.markers.append(dm)
|
||||
|
||||
target = self._select_dock_target(detections)
|
||||
axis_len = self._marker_size * _AXIS_LEN_FRAC
|
||||
|
||||
for idx, d in enumerate(detections):
|
||||
is_target = (target is not None and d.marker_id == target.marker_id)
|
||||
cx, cy, cz = float(d.tvec[0]), float(d.tvec[1]), float(d.tvec[2])
|
||||
qx, qy, qz, qw = d.quat
|
||||
base_id = idx * 10
|
||||
|
||||
# Sphere at marker centre
|
||||
sph = Marker()
|
||||
sph.header.stamp = stamp
|
||||
sph.header.frame_id = self._cam_frame
|
||||
sph.ns = 'aruco_centers'
|
||||
sph.id = base_id
|
||||
sph.type = Marker.SPHERE
|
||||
sph.action = Marker.ADD
|
||||
sph.pose.position = Point(x=cx, y=cy, z=cz)
|
||||
sph.pose.orientation = Quaternion(x=qx, y=qy, z=qz, w=qw)
|
||||
r_s = 0.015 if not is_target else 0.025
|
||||
sph.scale = Vector3(x=r_s * 2, y=r_s * 2, z=r_s * 2)
|
||||
sph.color = (
|
||||
ColorRGBA(r=1.0, g=0.2, b=0.2, a=1.0) if is_target else
|
||||
ColorRGBA(r=0.2, g=0.8, b=1.0, a=0.9)
|
||||
)
|
||||
sph.lifetime = lifetime
|
||||
ma.markers.append(sph)
|
||||
|
||||
# Text label: ID + distance
|
||||
txt = Marker()
|
||||
txt.header.stamp = stamp
|
||||
txt.header.frame_id = self._cam_frame
|
||||
txt.ns = 'aruco_labels'
|
||||
txt.id = base_id + 1
|
||||
txt.type = Marker.TEXT_VIEW_FACING
|
||||
txt.action = Marker.ADD
|
||||
txt.pose.position = Point(x=cx, y=cy - 0.08, z=cz)
|
||||
txt.pose.orientation.w = 1.0
|
||||
txt.scale.z = 0.06
|
||||
txt.color = ColorRGBA(r=1.0, g=1.0, b=0.0, a=1.0)
|
||||
txt.text = f'ID={d.marker_id}\n{d.distance_m:.2f}m'
|
||||
if is_target:
|
||||
txt.text += '\n[DOCK]'
|
||||
txt.lifetime = lifetime
|
||||
ma.markers.append(txt)
|
||||
|
||||
self._viz_pub.publish(ma)
|
||||
|
||||
def _publish_empty(self, stamp) -> None:
|
||||
pa = PoseArray()
|
||||
pa.header.stamp = stamp
|
||||
pa.header.frame_id = self._cam_frame
|
||||
self._pose_array_pub.publish(pa)
|
||||
self._publish_viz([], stamp)
|
||||
|
||||
def _publish_debug(self, bgr, corners, ids, detections, stamp) -> None:
|
||||
"""Annotate and publish debug image with detected markers drawn."""
|
||||
debug = bgr.copy()
|
||||
if ids is not None and len(ids) > 0:
|
||||
cv2.aruco.drawDetectedMarkers(debug, corners, ids)
|
||||
|
||||
# Draw axes for each detection using the estimated poses
|
||||
for d in detections:
|
||||
try:
|
||||
cv2.drawFrameAxes(
|
||||
debug, self._K, self._dist,
|
||||
d.rvec.reshape(3, 1),
|
||||
d.tvec.reshape(3, 1),
|
||||
self._marker_size * _AXIS_LEN_FRAC,
|
||||
)
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
try:
|
||||
img_msg = self._bridge.cv2_to_imgmsg(debug, encoding='bgr8')
|
||||
img_msg.header.stamp = stamp
|
||||
img_msg.header.frame_id = self._cam_frame
|
||||
self._debug_pub.publish(img_msg)
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
# ── Status timer ─────────────────────────────────────────────────────────
|
||||
|
||||
def _on_status_timer(self) -> None:
|
||||
detections = self._last_detections
|
||||
target = self._select_dock_target(detections)
|
||||
|
||||
markers_info = [
|
||||
{
|
||||
'id': d.marker_id,
|
||||
'distance_m': round(d.distance_m, 3),
|
||||
'yaw_deg': round(math.degrees(d.yaw_rad), 2),
|
||||
'lateral_m': round(d.lateral_m, 3),
|
||||
'forward_m': round(d.forward_m, 3),
|
||||
'is_target': target is not None and d.marker_id == target.marker_id,
|
||||
}
|
||||
for d in detections
|
||||
]
|
||||
|
||||
status = {
|
||||
'detected_count': len(detections),
|
||||
'dock_target_id': target.marker_id if target else None,
|
||||
'dock_distance_m': round(target.distance_m, 3) if target else None,
|
||||
'dock_yaw_deg': round(math.degrees(target.yaw_rad), 2) if target else None,
|
||||
'dock_lateral_m': round(target.lateral_m, 3) if target else None,
|
||||
'markers': markers_info,
|
||||
}
|
||||
msg = String()
|
||||
msg.data = json.dumps(status)
|
||||
self._status_pub.publish(msg)
|
||||
|
||||
|
||||
# ── Helpers ───────────────────────────────────────────────────────────────────
|
||||
|
||||
def _ros_duration(seconds: float):
|
||||
from rclpy.duration import Duration
|
||||
sec = int(seconds)
|
||||
nsec = int((seconds - sec) * 1e9)
|
||||
return Duration(seconds=sec, nanoseconds=nsec).to_msg()
|
||||
|
||||
|
||||
def main(args=None) -> None:
|
||||
rclpy.init(args=args)
|
||||
node = ArucoDetectNode()
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.try_shutdown()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@ -0,0 +1,156 @@
|
||||
"""
|
||||
aruco_math.py — Pure-math helpers for ArUco detection (Issue #627).
|
||||
|
||||
No ROS2 dependencies — importable in unit tests without a ROS2 install.
|
||||
|
||||
Provides:
|
||||
rot_mat_to_quat(R) → (qx, qy, qz, qw)
|
||||
rvec_to_quat(rvec) → (qx, qy, qz, qw) [via cv2.Rodrigues]
|
||||
tvec_distance(tvec) → float (Euclidean norm)
|
||||
tvec_yaw_rad(tvec) → float (atan2(tx, tz) — lateral bearing error)
|
||||
MarkerPose dataclass — per-marker detection result
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
from dataclasses import dataclass, field
|
||||
from typing import List, Optional, Tuple
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
# ── Rotation helpers ──────────────────────────────────────────────────────────
|
||||
|
||||
def rot_mat_to_quat(R: np.ndarray) -> Tuple[float, float, float, float]:
|
||||
"""
|
||||
Convert a 3×3 rotation matrix to quaternion (qx, qy, qz, qw).
|
||||
|
||||
Uses Shepperd's method for numerical stability.
|
||||
R must be a valid rotation matrix (‖R‖₂ = 1, det R = +1).
|
||||
"""
|
||||
R = np.asarray(R, dtype=np.float64)
|
||||
trace = R[0, 0] + R[1, 1] + R[2, 2]
|
||||
|
||||
if trace > 0.0:
|
||||
s = 0.5 / math.sqrt(trace + 1.0)
|
||||
w = 0.25 / s
|
||||
x = (R[2, 1] - R[1, 2]) * s
|
||||
y = (R[0, 2] - R[2, 0]) * s
|
||||
z = (R[1, 0] - R[0, 1]) * s
|
||||
elif R[0, 0] > R[1, 1] and R[0, 0] > R[2, 2]:
|
||||
s = 2.0 * math.sqrt(1.0 + R[0, 0] - R[1, 1] - R[2, 2])
|
||||
w = (R[2, 1] - R[1, 2]) / s
|
||||
x = 0.25 * s
|
||||
y = (R[0, 1] + R[1, 0]) / s
|
||||
z = (R[0, 2] + R[2, 0]) / s
|
||||
elif R[1, 1] > R[2, 2]:
|
||||
s = 2.0 * math.sqrt(1.0 + R[1, 1] - R[0, 0] - R[2, 2])
|
||||
w = (R[0, 2] - R[2, 0]) / s
|
||||
x = (R[0, 1] + R[1, 0]) / s
|
||||
y = 0.25 * s
|
||||
z = (R[1, 2] + R[2, 1]) / s
|
||||
else:
|
||||
s = 2.0 * math.sqrt(1.0 + R[2, 2] - R[0, 0] - R[1, 1])
|
||||
w = (R[1, 0] - R[0, 1]) / s
|
||||
x = (R[0, 2] + R[2, 0]) / s
|
||||
y = (R[1, 2] + R[2, 1]) / s
|
||||
z = 0.25 * s
|
||||
|
||||
# Normalise
|
||||
norm = math.sqrt(x * x + y * y + z * z + w * w)
|
||||
if norm < 1e-10:
|
||||
return 0.0, 0.0, 0.0, 1.0
|
||||
inv = 1.0 / norm
|
||||
return x * inv, y * inv, z * inv, w * inv
|
||||
|
||||
|
||||
def rvec_to_quat(rvec: np.ndarray) -> Tuple[float, float, float, float]:
|
||||
"""
|
||||
Convert a Rodrigues rotation vector (3,) or (1,3) to quaternion.
|
||||
|
||||
Requires cv2 for cv2.Rodrigues(); falls back to manual exponential map
|
||||
if cv2 is unavailable.
|
||||
"""
|
||||
vec = np.asarray(rvec, dtype=np.float64).ravel()
|
||||
try:
|
||||
import cv2
|
||||
R, _ = cv2.Rodrigues(vec)
|
||||
except ImportError:
|
||||
# Manual Rodrigues exponential map
|
||||
angle = float(np.linalg.norm(vec))
|
||||
if angle < 1e-12:
|
||||
return 0.0, 0.0, 0.0, 1.0
|
||||
axis = vec / angle
|
||||
K = np.array([
|
||||
[ 0.0, -axis[2], axis[1]],
|
||||
[ axis[2], 0.0, -axis[0]],
|
||||
[-axis[1], axis[0], 0.0],
|
||||
])
|
||||
R = np.eye(3) + math.sin(angle) * K + (1.0 - math.cos(angle)) * K @ K
|
||||
|
||||
return rot_mat_to_quat(R)
|
||||
|
||||
|
||||
# ── Metric helpers ────────────────────────────────────────────────────────────
|
||||
|
||||
def tvec_distance(tvec: np.ndarray) -> float:
|
||||
"""Euclidean distance from camera to marker (m)."""
|
||||
t = np.asarray(tvec, dtype=np.float64).ravel()
|
||||
return float(math.sqrt(t[0] ** 2 + t[1] ** 2 + t[2] ** 2))
|
||||
|
||||
|
||||
def tvec_yaw_rad(tvec: np.ndarray) -> float:
|
||||
"""
|
||||
Horizontal bearing error to marker (rad).
|
||||
|
||||
atan2(tx, tz): +ve when marker is to the right of the camera optical axis.
|
||||
Zero when the marker is directly in front.
|
||||
"""
|
||||
t = np.asarray(tvec, dtype=np.float64).ravel()
|
||||
return float(math.atan2(t[0], t[2]))
|
||||
|
||||
|
||||
# ── Per-marker result ─────────────────────────────────────────────────────────
|
||||
|
||||
@dataclass
|
||||
class MarkerPose:
|
||||
"""Pose result for a single detected ArUco marker."""
|
||||
marker_id: int
|
||||
tvec: np.ndarray # (3,) translation in camera optical frame (m)
|
||||
rvec: np.ndarray # (3,) Rodrigues rotation vector
|
||||
corners: np.ndarray # (4, 2) image-plane corner coordinates (px)
|
||||
|
||||
# Derived (computed lazily)
|
||||
_distance_m: Optional[float] = field(default=None, repr=False)
|
||||
_yaw_rad: Optional[float] = field(default=None, repr=False)
|
||||
_quat: Optional[Tuple[float, float, float, float]] = field(default=None, repr=False)
|
||||
|
||||
@property
|
||||
def distance_m(self) -> float:
|
||||
if self._distance_m is None:
|
||||
self._distance_m = tvec_distance(self.tvec)
|
||||
return self._distance_m
|
||||
|
||||
@property
|
||||
def yaw_rad(self) -> float:
|
||||
if self._yaw_rad is None:
|
||||
self._yaw_rad = tvec_yaw_rad(self.tvec)
|
||||
return self._yaw_rad
|
||||
|
||||
@property
|
||||
def lateral_m(self) -> float:
|
||||
"""Lateral offset (m); +ve = marker is to the right."""
|
||||
return float(self.tvec[0])
|
||||
|
||||
@property
|
||||
def forward_m(self) -> float:
|
||||
"""Forward distance (Z component in camera frame, m)."""
|
||||
return float(self.tvec[2])
|
||||
|
||||
@property
|
||||
def quat(self) -> Tuple[float, float, float, float]:
|
||||
"""Quaternion (qx, qy, qz, qw) from rvec."""
|
||||
if self._quat is None:
|
||||
self._quat = rvec_to_quat(self.rvec)
|
||||
return self._quat
|
||||
5
jetson/ros2_ws/src/saltybot_aruco_detect/setup.cfg
Normal file
5
jetson/ros2_ws/src/saltybot_aruco_detect/setup.cfg
Normal file
@ -0,0 +1,5 @@
|
||||
[develop]
|
||||
script_dir=$base/lib/saltybot_aruco_detect
|
||||
|
||||
[install]
|
||||
install_scripts=$base/lib/saltybot_aruco_detect
|
||||
30
jetson/ros2_ws/src/saltybot_aruco_detect/setup.py
Normal file
30
jetson/ros2_ws/src/saltybot_aruco_detect/setup.py
Normal file
@ -0,0 +1,30 @@
|
||||
import os
|
||||
from glob import glob
|
||||
from setuptools import setup
|
||||
|
||||
package_name = 'saltybot_aruco_detect'
|
||||
|
||||
setup(
|
||||
name=package_name,
|
||||
version='0.1.0',
|
||||
packages=[package_name],
|
||||
data_files=[
|
||||
('share/ament_index/resource_index/packages',
|
||||
['resource/' + package_name]),
|
||||
('share/' + package_name, ['package.xml']),
|
||||
(os.path.join('share', package_name, 'launch'), glob('launch/*.py')),
|
||||
(os.path.join('share', package_name, 'config'), glob('config/*.yaml')),
|
||||
],
|
||||
install_requires=['setuptools'],
|
||||
zip_safe=True,
|
||||
maintainer='sl-perception',
|
||||
maintainer_email='sl-perception@saltylab.local',
|
||||
description='ArUco marker detection for docking alignment (Issue #627)',
|
||||
license='MIT',
|
||||
tests_require=['pytest'],
|
||||
entry_points={
|
||||
'console_scripts': [
|
||||
'aruco_detect = saltybot_aruco_detect.aruco_detect_node:main',
|
||||
],
|
||||
},
|
||||
)
|
||||
206
jetson/ros2_ws/src/saltybot_aruco_detect/test/test_aruco_math.py
Normal file
206
jetson/ros2_ws/src/saltybot_aruco_detect/test/test_aruco_math.py
Normal file
@ -0,0 +1,206 @@
|
||||
"""Unit tests for aruco_math.py — no ROS2, no live camera required."""
|
||||
|
||||
import math
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from saltybot_aruco_detect.aruco_math import (
|
||||
rot_mat_to_quat,
|
||||
rvec_to_quat,
|
||||
tvec_distance,
|
||||
tvec_yaw_rad,
|
||||
MarkerPose,
|
||||
)
|
||||
|
||||
|
||||
# ── Helpers ───────────────────────────────────────────────────────────────────
|
||||
|
||||
def _quat_norm(q):
|
||||
return math.sqrt(sum(x * x for x in q))
|
||||
|
||||
|
||||
def _quat_rotate(q, v):
|
||||
"""Rotate vector v by quaternion q (qx, qy, qz, qw)."""
|
||||
qx, qy, qz, qw = q
|
||||
# Quaternion product: q * (0,v) * q^-1
|
||||
# Using formula: v' = v + 2qw(q × v) + 2(q × (q × v))
|
||||
qvec = np.array([qx, qy, qz])
|
||||
t = 2.0 * np.cross(qvec, v)
|
||||
return v + qw * t + np.cross(qvec, t)
|
||||
|
||||
|
||||
# ── rot_mat_to_quat ───────────────────────────────────────────────────────────
|
||||
|
||||
def test_identity_rotation():
|
||||
R = np.eye(3)
|
||||
q = rot_mat_to_quat(R)
|
||||
assert abs(q[3] - 1.0) < 1e-9 # w = 1 for identity
|
||||
assert abs(q[0]) < 1e-9
|
||||
assert abs(q[1]) < 1e-9
|
||||
assert abs(q[2]) < 1e-9
|
||||
|
||||
|
||||
def test_90deg_yaw():
|
||||
"""90° rotation about Z axis."""
|
||||
angle = math.pi / 2
|
||||
R = np.array([
|
||||
[math.cos(angle), -math.sin(angle), 0],
|
||||
[math.sin(angle), math.cos(angle), 0],
|
||||
[0, 0, 1],
|
||||
])
|
||||
qx, qy, qz, qw = rot_mat_to_quat(R)
|
||||
# For 90° Z rotation: q = (0, 0, sin45°, cos45°)
|
||||
assert abs(qx) < 1e-6
|
||||
assert abs(qy) < 1e-6
|
||||
assert abs(qz - math.sin(angle / 2)) < 1e-6
|
||||
assert abs(qw - math.cos(angle / 2)) < 1e-6
|
||||
|
||||
|
||||
def test_unit_norm():
|
||||
for yaw in [0, 0.5, 1.0, 2.0, math.pi]:
|
||||
R = np.array([
|
||||
[math.cos(yaw), -math.sin(yaw), 0],
|
||||
[math.sin(yaw), math.cos(yaw), 0],
|
||||
[0, 0, 1],
|
||||
])
|
||||
q = rot_mat_to_quat(R)
|
||||
assert abs(_quat_norm(q) - 1.0) < 1e-9, f"yaw={yaw}: norm={_quat_norm(q)}"
|
||||
|
||||
|
||||
def test_roundtrip_rotation_matrix():
|
||||
"""Rotating a vector with the matrix and quaternion should give same result."""
|
||||
angle = 1.23
|
||||
R = np.array([
|
||||
[math.cos(angle), -math.sin(angle), 0],
|
||||
[math.sin(angle), math.cos(angle), 0],
|
||||
[0, 0, 1],
|
||||
])
|
||||
q = rot_mat_to_quat(R)
|
||||
v = np.array([1.0, 0.0, 0.0])
|
||||
v_R = R @ v
|
||||
v_q = _quat_rotate(q, v)
|
||||
np.testing.assert_allclose(v_R, v_q, atol=1e-9)
|
||||
|
||||
|
||||
def test_180deg_pitch():
|
||||
"""180° rotation about Y axis."""
|
||||
R = np.array([[-1, 0, 0], [0, 1, 0], [0, 0, -1]], dtype=float)
|
||||
q = rot_mat_to_quat(R)
|
||||
assert abs(_quat_norm(q) - 1.0) < 1e-9
|
||||
# For 180° Y: q = (0, 1, 0, 0) or (0, -1, 0, 0)
|
||||
assert abs(abs(q[1]) - 1.0) < 1e-6
|
||||
|
||||
|
||||
# ── rvec_to_quat ──────────────────────────────────────────────────────────────
|
||||
|
||||
def test_rvec_zero_is_identity():
|
||||
q = rvec_to_quat(np.array([0.0, 0.0, 0.0]))
|
||||
assert abs(_quat_norm(q) - 1.0) < 1e-6
|
||||
assert abs(q[3] - 1.0) < 1e-6 # w = 1
|
||||
|
||||
|
||||
def test_rvec_unit_norm():
|
||||
for rvec in [
|
||||
[0.1, 0.0, 0.0],
|
||||
[0.0, 0.5, 0.0],
|
||||
[0.0, 0.0, 1.57],
|
||||
[0.3, 0.4, 0.5],
|
||||
]:
|
||||
q = rvec_to_quat(np.array(rvec))
|
||||
assert abs(_quat_norm(q) - 1.0) < 1e-9, f"rvec={rvec}: norm={_quat_norm(q)}"
|
||||
|
||||
|
||||
def test_rvec_z_rotation_matches_rot_mat():
|
||||
"""rvec=[0,0,π/3] should match 60° Z rotation matrix quaternion."""
|
||||
angle = math.pi / 3
|
||||
rvec = np.array([0.0, 0.0, angle])
|
||||
R = np.array([
|
||||
[math.cos(angle), -math.sin(angle), 0],
|
||||
[math.sin(angle), math.cos(angle), 0],
|
||||
[0, 0, 1],
|
||||
])
|
||||
q_rvec = rvec_to_quat(rvec)
|
||||
q_R = rot_mat_to_quat(R)
|
||||
# Allow sign flip (q and -q represent the same rotation)
|
||||
diff = min(
|
||||
abs(_quat_norm(np.array(q_rvec) - np.array(q_R))),
|
||||
abs(_quat_norm(np.array(q_rvec) + np.array(q_R))),
|
||||
)
|
||||
assert diff < 1e-6, f"rvec q={q_rvec}, mat q={q_R}"
|
||||
|
||||
|
||||
# ── tvec_distance ─────────────────────────────────────────────────────────────
|
||||
|
||||
def test_tvec_distance_basic():
|
||||
assert abs(tvec_distance(np.array([3.0, 4.0, 0.0])) - 5.0) < 1e-9
|
||||
|
||||
|
||||
def test_tvec_distance_zero():
|
||||
assert tvec_distance(np.array([0.0, 0.0, 0.0])) == 0.0
|
||||
|
||||
|
||||
def test_tvec_distance_forward():
|
||||
assert abs(tvec_distance(np.array([0.0, 0.0, 2.5])) - 2.5) < 1e-9
|
||||
|
||||
|
||||
def test_tvec_distance_3d():
|
||||
t = np.array([1.0, 2.0, 3.0])
|
||||
expected = math.sqrt(14.0)
|
||||
assert abs(tvec_distance(t) - expected) < 1e-9
|
||||
|
||||
|
||||
# ── tvec_yaw_rad ──────────────────────────────────────────────────────────────
|
||||
|
||||
def test_yaw_zero_when_centred():
|
||||
"""Marker directly in front: tx=0 → yaw=0."""
|
||||
assert abs(tvec_yaw_rad(np.array([0.0, 0.0, 2.0]))) < 1e-9
|
||||
|
||||
|
||||
def test_yaw_positive_right():
|
||||
"""Marker to the right (tx>0) → positive yaw."""
|
||||
assert tvec_yaw_rad(np.array([1.0, 0.0, 2.0])) > 0.0
|
||||
|
||||
|
||||
def test_yaw_negative_left():
|
||||
"""Marker to the left (tx<0) → negative yaw."""
|
||||
assert tvec_yaw_rad(np.array([-1.0, 0.0, 2.0])) < 0.0
|
||||
|
||||
|
||||
def test_yaw_45deg():
|
||||
"""tx=tz → 45°."""
|
||||
assert abs(tvec_yaw_rad(np.array([1.0, 0.0, 1.0])) - math.pi / 4) < 1e-9
|
||||
|
||||
|
||||
# ── MarkerPose ────────────────────────────────────────────────────────────────
|
||||
|
||||
def _make_marker(mid=42, tx=0.0, ty=0.0, tz=2.0):
|
||||
rvec = np.array([0.0, 0.0, 0.1])
|
||||
tvec = np.array([tx, ty, tz])
|
||||
corners = np.zeros((4, 2))
|
||||
return MarkerPose(marker_id=mid, tvec=tvec, rvec=rvec, corners=corners)
|
||||
|
||||
|
||||
def test_marker_distance_cached():
|
||||
m = _make_marker(tz=3.0)
|
||||
d1 = m.distance_m
|
||||
d2 = m.distance_m
|
||||
assert d1 == d2
|
||||
|
||||
|
||||
def test_marker_lateral_and_forward():
|
||||
m = _make_marker(tx=0.5, tz=2.0)
|
||||
assert abs(m.lateral_m - 0.5) < 1e-9
|
||||
assert abs(m.forward_m - 2.0) < 1e-9
|
||||
|
||||
|
||||
def test_marker_quat_unit_norm():
|
||||
m = _make_marker()
|
||||
q = m.quat
|
||||
assert abs(_quat_norm(q) - 1.0) < 1e-9
|
||||
|
||||
|
||||
def test_marker_yaw_sign():
|
||||
m_right = _make_marker(tx=+0.3, tz=1.0)
|
||||
m_left = _make_marker(tx=-0.3, tz=1.0)
|
||||
assert m_right.yaw_rad > 0
|
||||
assert m_left.yaw_rad < 0
|
||||
@ -0,0 +1,7 @@
|
||||
uwb_logger:
|
||||
ros__parameters:
|
||||
log_dir: ~/uwb_logs
|
||||
enable_csv: true
|
||||
default_n_samples: 200
|
||||
default_timeout_s: 30.0
|
||||
flush_interval_s: 5.0
|
||||
@ -0,0 +1,28 @@
|
||||
"""uwb_logger.launch.py — Launch UWB position logger (Issue #634)."""
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
import os
|
||||
|
||||
|
||||
def generate_launch_description() -> LaunchDescription:
|
||||
pkg_dir = get_package_share_directory("saltybot_uwb_logger")
|
||||
params_file = os.path.join(pkg_dir, "config", "uwb_logger_params.yaml")
|
||||
|
||||
return LaunchDescription([
|
||||
DeclareLaunchArgument("log_dir", default_value="~/uwb_logs"),
|
||||
Node(
|
||||
package="saltybot_uwb_logger",
|
||||
executable="uwb_logger",
|
||||
name="uwb_logger",
|
||||
parameters=[
|
||||
params_file,
|
||||
{"log_dir": LaunchConfiguration("log_dir")},
|
||||
],
|
||||
output="screen",
|
||||
emulate_tty=True,
|
||||
),
|
||||
])
|
||||
31
jetson/ros2_ws/src/saltybot_uwb_logger/package.xml
Normal file
31
jetson/ros2_ws/src/saltybot_uwb_logger/package.xml
Normal file
@ -0,0 +1,31 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>saltybot_uwb_logger</name>
|
||||
<version>0.1.0</version>
|
||||
<description>
|
||||
UWB position logger and accuracy analyzer (Issue #634).
|
||||
Logs fused pose, raw UWB pose, per-anchor ranges to CSV.
|
||||
Provides /saltybot/uwb/start_accuracy_test: collects N samples at a
|
||||
known position and computes CEP50, CEP95, RMSE, max error.
|
||||
</description>
|
||||
<maintainer email="sl-uwb@saltylab.local">sl-uwb</maintainer>
|
||||
<license>Apache-2.0</license>
|
||||
|
||||
<depend>rclpy</depend>
|
||||
<depend>geometry_msgs</depend>
|
||||
<depend>std_msgs</depend>
|
||||
<depend>saltybot_uwb_msgs</depend>
|
||||
<depend>saltybot_uwb_logger_msgs</depend>
|
||||
|
||||
<exec_depend>python3-numpy</exec_depend>
|
||||
|
||||
<test_depend>ament_copyright</test_depend>
|
||||
<test_depend>ament_flake8</test_depend>
|
||||
<test_depend>ament_pep257</test_depend>
|
||||
<test_depend>python3-pytest</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_python</build_type>
|
||||
</export>
|
||||
</package>
|
||||
Binary file not shown.
Binary file not shown.
@ -0,0 +1,99 @@
|
||||
"""
|
||||
accuracy_stats.py — UWB positioning accuracy statistics (Issue #634)
|
||||
|
||||
Pure numpy, no ROS2 dependency.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
from dataclasses import dataclass, field
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
@dataclass
|
||||
class AccuracyStats:
|
||||
n_samples: int
|
||||
mean_x: float
|
||||
mean_y: float
|
||||
std_x: float
|
||||
std_y: float
|
||||
std_2d: float
|
||||
bias_x: float
|
||||
bias_y: float
|
||||
cep50: float
|
||||
cep95: float
|
||||
max_error: float
|
||||
rmse: float
|
||||
|
||||
|
||||
def compute_stats(
|
||||
xs: np.ndarray,
|
||||
ys: np.ndarray,
|
||||
truth_x: float,
|
||||
truth_y: float,
|
||||
) -> AccuracyStats:
|
||||
"""
|
||||
Compute accuracy statistics from position samples.
|
||||
|
||||
Parameters
|
||||
----------
|
||||
xs, ys : 1-D arrays of measured x / y positions (metres)
|
||||
truth_x/y : known ground-truth position (metres)
|
||||
|
||||
Raises
|
||||
------
|
||||
ValueError if fewer than 2 samples or mismatched lengths.
|
||||
"""
|
||||
xs = np.asarray(xs, dtype=float)
|
||||
ys = np.asarray(ys, dtype=float)
|
||||
n = len(xs)
|
||||
if n < 2:
|
||||
raise ValueError(f"Need at least 2 samples, got {n}")
|
||||
if len(ys) != n:
|
||||
raise ValueError("xs and ys must have the same length")
|
||||
|
||||
errors = np.sqrt((xs - truth_x) ** 2 + (ys - truth_y) ** 2)
|
||||
|
||||
mean_x = float(np.mean(xs))
|
||||
mean_y = float(np.mean(ys))
|
||||
std_x = float(np.std(xs, ddof=1))
|
||||
std_y = float(np.std(ys, ddof=1))
|
||||
|
||||
return AccuracyStats(
|
||||
n_samples = n,
|
||||
mean_x = mean_x,
|
||||
mean_y = mean_y,
|
||||
std_x = std_x,
|
||||
std_y = std_y,
|
||||
std_2d = math.sqrt(std_x ** 2 + std_y ** 2),
|
||||
bias_x = mean_x - truth_x,
|
||||
bias_y = mean_y - truth_y,
|
||||
cep50 = float(np.percentile(errors, 50)),
|
||||
cep95 = float(np.percentile(errors, 95)),
|
||||
max_error = float(np.max(errors)),
|
||||
rmse = float(np.sqrt(np.mean(errors ** 2))),
|
||||
)
|
||||
|
||||
|
||||
@dataclass
|
||||
class RangeAccum:
|
||||
"""Running accumulator for per-anchor range statistics."""
|
||||
anchor_id: int
|
||||
_samples: list = field(default_factory=list, repr=False)
|
||||
|
||||
def add(self, range_m: float) -> None:
|
||||
self._samples.append(range_m)
|
||||
|
||||
@property
|
||||
def count(self) -> int:
|
||||
return len(self._samples)
|
||||
|
||||
@property
|
||||
def mean(self) -> float:
|
||||
return float(np.mean(self._samples)) if self._samples else 0.0
|
||||
|
||||
@property
|
||||
def std(self) -> float:
|
||||
return float(np.std(self._samples, ddof=1)) if len(self._samples) >= 2 else 0.0
|
||||
@ -0,0 +1,409 @@
|
||||
"""
|
||||
logger_node.py — UWB position logger and accuracy analyzer (Issue #634)
|
||||
|
||||
Subscriptions
|
||||
─────────────
|
||||
/saltybot/pose/fused geometry_msgs/PoseStamped — EKF-fused pose (200 Hz)
|
||||
/saltybot/uwb/pose geometry_msgs/PoseStamped — raw UWB pose (10 Hz)
|
||||
/uwb/ranges saltybot_uwb_msgs/UwbRangeArray — per-anchor ranges
|
||||
|
||||
Service
|
||||
───────
|
||||
/saltybot/uwb/start_accuracy_test StartAccuracyTest
|
||||
Collects N fused-pose samples at known position, computes accuracy
|
||||
metrics, publishes AccuracyReport, writes JSON summary.
|
||||
|
||||
Publishes
|
||||
─────────
|
||||
/saltybot/uwb/accuracy_report AccuracyReport
|
||||
|
||||
CSV logging (continuous, to log_dir)
|
||||
─────────────────────────────────────
|
||||
fused_pose_<DATE>.csv — timestamp_ns, x_m, y_m, heading_rad
|
||||
uwb_pose_<DATE>.csv — timestamp_ns, x_m, y_m
|
||||
uwb_ranges_<DATE>.csv — timestamp_ns, anchor_id, range_m, raw_mm, rssi, tag_id
|
||||
accuracy_<test_id>.json — accuracy test results
|
||||
|
||||
Parameters
|
||||
──────────
|
||||
log_dir ~/uwb_logs
|
||||
enable_csv true
|
||||
default_n_samples 200
|
||||
default_timeout_s 30.0
|
||||
flush_interval_s 5.0
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import csv
|
||||
import json
|
||||
import math
|
||||
import os
|
||||
import threading
|
||||
import time
|
||||
from datetime import datetime, timezone
|
||||
from pathlib import Path
|
||||
from typing import Optional
|
||||
|
||||
import numpy as np
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
|
||||
|
||||
from geometry_msgs.msg import PoseStamped
|
||||
from saltybot_uwb_msgs.msg import UwbRangeArray
|
||||
from saltybot_uwb_logger_msgs.msg import AccuracyReport
|
||||
from saltybot_uwb_logger_msgs.srv import StartAccuracyTest
|
||||
|
||||
from saltybot_uwb_logger.accuracy_stats import compute_stats, RangeAccum
|
||||
|
||||
|
||||
_SENSOR_QOS = QoSProfile(
|
||||
reliability=ReliabilityPolicy.BEST_EFFORT,
|
||||
history=HistoryPolicy.KEEP_LAST,
|
||||
depth=10,
|
||||
)
|
||||
|
||||
|
||||
class UwbLoggerNode(Node):
|
||||
|
||||
def __init__(self) -> None:
|
||||
super().__init__("uwb_logger")
|
||||
|
||||
# ── Parameters ────────────────────────────────────────────────────
|
||||
self.declare_parameter("log_dir", os.path.expanduser("~/uwb_logs"))
|
||||
self.declare_parameter("enable_csv", True)
|
||||
self.declare_parameter("default_n_samples", 200)
|
||||
self.declare_parameter("default_timeout_s", 30.0)
|
||||
self.declare_parameter("flush_interval_s", 5.0)
|
||||
|
||||
self._log_dir = Path(self.get_parameter("log_dir").value)
|
||||
self._enable_csv = self.get_parameter("enable_csv").value
|
||||
self._default_n = self.get_parameter("default_n_samples").value
|
||||
self._default_to = self.get_parameter("default_timeout_s").value
|
||||
self._flush_iv = self.get_parameter("flush_interval_s").value
|
||||
|
||||
if self._enable_csv:
|
||||
self._log_dir.mkdir(parents=True, exist_ok=True)
|
||||
|
||||
# ── CSV writers ───────────────────────────────────────────────────
|
||||
date_tag = datetime.now(timezone.utc).strftime("%Y%m%dT%H%M%SZ")
|
||||
self._csv_lock = threading.Lock()
|
||||
self._csv_files = []
|
||||
self._fused_csv = self._open_csv(
|
||||
f"fused_pose_{date_tag}.csv",
|
||||
["timestamp_ns", "x_m", "y_m", "heading_rad"])
|
||||
self._uwb_csv = self._open_csv(
|
||||
f"uwb_pose_{date_tag}.csv",
|
||||
["timestamp_ns", "x_m", "y_m"])
|
||||
self._ranges_csv = self._open_csv(
|
||||
f"uwb_ranges_{date_tag}.csv",
|
||||
["timestamp_ns", "anchor_id", "range_m", "raw_mm", "rssi", "tag_id"])
|
||||
|
||||
# ── Accuracy test state ────────────────────────────────────────────
|
||||
self._test_lock = threading.Lock()
|
||||
self._test_active = False
|
||||
self._test_id = ""
|
||||
self._test_truth_x = 0.0
|
||||
self._test_truth_y = 0.0
|
||||
self._test_n_target = 0
|
||||
self._test_xs: list = []
|
||||
self._test_ys: list = []
|
||||
self._test_ranges: dict[int, RangeAccum] = {}
|
||||
self._test_t0 = 0.0
|
||||
self._test_timeout = 0.0
|
||||
self._test_done_event = threading.Event()
|
||||
|
||||
# ── Publisher ─────────────────────────────────────────────────────
|
||||
self._report_pub = self.create_publisher(
|
||||
AccuracyReport, "/saltybot/uwb/accuracy_report", 10
|
||||
)
|
||||
|
||||
# ── Subscriptions ─────────────────────────────────────────────────
|
||||
self.create_subscription(
|
||||
PoseStamped, "/saltybot/pose/fused",
|
||||
self._fused_cb, _SENSOR_QOS)
|
||||
self.create_subscription(
|
||||
PoseStamped, "/saltybot/uwb/pose",
|
||||
self._uwb_pose_cb, _SENSOR_QOS)
|
||||
self.create_subscription(
|
||||
UwbRangeArray, "/uwb/ranges",
|
||||
self._ranges_cb, _SENSOR_QOS)
|
||||
|
||||
# ── Service ───────────────────────────────────────────────────────
|
||||
self._srv = self.create_service(
|
||||
StartAccuracyTest,
|
||||
"/saltybot/uwb/start_accuracy_test",
|
||||
self._handle_start_test)
|
||||
|
||||
# ── Periodic flush ────────────────────────────────────────────────
|
||||
self.create_timer(self._flush_iv, self._flush_csv)
|
||||
|
||||
self.get_logger().info(
|
||||
f"UWB logger ready — log_dir={self._log_dir} csv={self._enable_csv}"
|
||||
)
|
||||
|
||||
def destroy_node(self) -> None:
|
||||
self._flush_csv()
|
||||
for fh in self._csv_files:
|
||||
try:
|
||||
fh.close()
|
||||
except Exception:
|
||||
pass
|
||||
super().destroy_node()
|
||||
|
||||
# ── Callbacks ─────────────────────────────────────────────────────────
|
||||
|
||||
def _fused_cb(self, msg: PoseStamped) -> None:
|
||||
ts = _stamp_ns(msg.header.stamp)
|
||||
x = msg.pose.position.x
|
||||
y = msg.pose.position.y
|
||||
q = msg.pose.orientation
|
||||
heading = 2.0 * math.atan2(float(q.z), float(q.w))
|
||||
|
||||
if self._enable_csv:
|
||||
with self._csv_lock:
|
||||
if self._fused_csv:
|
||||
self._fused_csv.writerow([ts, x, y, heading])
|
||||
|
||||
with self._test_lock:
|
||||
if self._test_active:
|
||||
self._test_xs.append(x)
|
||||
self._test_ys.append(y)
|
||||
if len(self._test_xs) >= self._test_n_target:
|
||||
self._test_active = False
|
||||
self._test_done_event.set()
|
||||
|
||||
def _uwb_pose_cb(self, msg: PoseStamped) -> None:
|
||||
ts = _stamp_ns(msg.header.stamp)
|
||||
x = msg.pose.position.x
|
||||
y = msg.pose.position.y
|
||||
if self._enable_csv:
|
||||
with self._csv_lock:
|
||||
if self._uwb_csv:
|
||||
self._uwb_csv.writerow([ts, x, y])
|
||||
|
||||
def _ranges_cb(self, msg: UwbRangeArray) -> None:
|
||||
ts = _stamp_ns(msg.header.stamp)
|
||||
for r in msg.ranges:
|
||||
if self._enable_csv:
|
||||
with self._csv_lock:
|
||||
if self._ranges_csv:
|
||||
self._ranges_csv.writerow([
|
||||
ts, r.anchor_id, r.range_m,
|
||||
r.raw_mm, r.rssi, r.tag_id,
|
||||
])
|
||||
with self._test_lock:
|
||||
if self._test_active:
|
||||
aid = int(r.anchor_id)
|
||||
if aid not in self._test_ranges:
|
||||
self._test_ranges[aid] = RangeAccum(anchor_id=aid)
|
||||
self._test_ranges[aid].add(float(r.range_m))
|
||||
|
||||
# ── Service ────────────────────────────────────────────────────────────
|
||||
|
||||
def _handle_start_test(
|
||||
self,
|
||||
request: StartAccuracyTest.Request,
|
||||
response: StartAccuracyTest.Response,
|
||||
) -> StartAccuracyTest.Response:
|
||||
|
||||
with self._test_lock:
|
||||
if self._test_active:
|
||||
response.success = False
|
||||
response.message = "Another test is already running"
|
||||
response.test_id = self._test_id
|
||||
return response
|
||||
|
||||
n = int(request.n_samples) if request.n_samples > 0 else self._default_n
|
||||
timeout = float(request.timeout_s) if request.timeout_s > 0.0 else self._default_to
|
||||
test_id = (
|
||||
request.test_id.strip()
|
||||
if request.test_id.strip()
|
||||
else datetime.now(timezone.utc).strftime("%Y%m%dT%H%M%SZ")
|
||||
)
|
||||
|
||||
self.get_logger().info(
|
||||
f"Accuracy test '{test_id}' — "
|
||||
f"truth=({request.truth_x_m:.3f}, {request.truth_y_m:.3f}) "
|
||||
f"n={n} timeout={timeout}s"
|
||||
)
|
||||
|
||||
with self._test_lock:
|
||||
self._test_id = test_id
|
||||
self._test_truth_x = float(request.truth_x_m)
|
||||
self._test_truth_y = float(request.truth_y_m)
|
||||
self._test_n_target = n
|
||||
self._test_xs = []
|
||||
self._test_ys = []
|
||||
self._test_ranges = {}
|
||||
self._test_t0 = time.monotonic()
|
||||
self._test_timeout = timeout
|
||||
self._test_done_event.clear()
|
||||
self._test_active = True
|
||||
|
||||
threading.Thread(
|
||||
target=self._run_test,
|
||||
args=(test_id, request.truth_x_m, request.truth_y_m, n, timeout),
|
||||
daemon=True,
|
||||
).start()
|
||||
|
||||
response.success = True
|
||||
response.message = f"Test '{test_id}' started, collecting {n} samples"
|
||||
response.test_id = test_id
|
||||
return response
|
||||
|
||||
def _run_test(
|
||||
self,
|
||||
test_id: str,
|
||||
truth_x: float,
|
||||
truth_y: float,
|
||||
n_target: int,
|
||||
timeout: float,
|
||||
) -> None:
|
||||
finished = self._test_done_event.wait(timeout=timeout)
|
||||
|
||||
with self._test_lock:
|
||||
self._test_active = False
|
||||
xs = list(self._test_xs)
|
||||
ys = list(self._test_ys)
|
||||
ranges_snap = dict(self._test_ranges)
|
||||
t0 = self._test_t0
|
||||
|
||||
duration = time.monotonic() - t0
|
||||
n_got = len(xs)
|
||||
|
||||
if n_got < 2:
|
||||
self.get_logger().error(
|
||||
f"Test '{test_id}' failed: only {n_got} samples in {duration:.1f}s"
|
||||
)
|
||||
return
|
||||
|
||||
if not finished:
|
||||
self.get_logger().warn(
|
||||
f"Test '{test_id}' timed out — {n_got}/{n_target} samples"
|
||||
)
|
||||
|
||||
try:
|
||||
stats = compute_stats(np.array(xs), np.array(ys), truth_x, truth_y)
|
||||
except ValueError as exc:
|
||||
self.get_logger().error(f"Stats failed: {exc}")
|
||||
return
|
||||
|
||||
self.get_logger().info(
|
||||
f"Test '{test_id}' — n={stats.n_samples} "
|
||||
f"CEP50={stats.cep50*100:.1f}cm CEP95={stats.cep95*100:.1f}cm "
|
||||
f"RMSE={stats.rmse*100:.1f}cm max={stats.max_error*100:.1f}cm"
|
||||
)
|
||||
|
||||
# Publish report
|
||||
report = AccuracyReport()
|
||||
report.header.stamp = self.get_clock().now().to_msg()
|
||||
report.header.frame_id = "map"
|
||||
report.truth_x_m = truth_x
|
||||
report.truth_y_m = truth_y
|
||||
report.n_samples = stats.n_samples
|
||||
report.mean_x_m = stats.mean_x
|
||||
report.mean_y_m = stats.mean_y
|
||||
report.std_x_m = stats.std_x
|
||||
report.std_y_m = stats.std_y
|
||||
report.std_2d_m = stats.std_2d
|
||||
report.bias_x_m = stats.bias_x
|
||||
report.bias_y_m = stats.bias_y
|
||||
report.cep50_m = stats.cep50
|
||||
report.cep95_m = stats.cep95
|
||||
report.max_error_m = stats.max_error
|
||||
report.rmse_m = stats.rmse
|
||||
report.test_id = test_id
|
||||
report.duration_s = duration
|
||||
|
||||
anchor_ids = sorted(ranges_snap.keys())
|
||||
report.anchor_ids = [int(a) for a in anchor_ids]
|
||||
report.anchor_range_mean_m = [ranges_snap[a].mean for a in anchor_ids]
|
||||
report.anchor_range_std_m = [ranges_snap[a].std for a in anchor_ids]
|
||||
|
||||
self._report_pub.publish(report)
|
||||
|
||||
# Write JSON
|
||||
if self._enable_csv:
|
||||
self._write_json(test_id, truth_x, truth_y, stats,
|
||||
anchor_ids, ranges_snap, duration)
|
||||
|
||||
# ── CSV / JSON helpers ─────────────────────────────────────────────────
|
||||
|
||||
def _open_csv(
|
||||
self, filename: str, headers: list
|
||||
) -> Optional[csv.writer]:
|
||||
if not self._enable_csv:
|
||||
return None
|
||||
path = self._log_dir / filename
|
||||
fh = open(path, "w", newline="", buffering=1)
|
||||
writer = csv.writer(fh)
|
||||
writer.writerow(headers)
|
||||
self._csv_files.append(fh)
|
||||
return writer
|
||||
|
||||
def _flush_csv(self) -> None:
|
||||
for fh in self._csv_files:
|
||||
try:
|
||||
fh.flush()
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
def _write_json(self, test_id, truth_x, truth_y, stats,
|
||||
anchor_ids, ranges_snap, duration) -> None:
|
||||
path = self._log_dir / f"accuracy_{test_id}.json"
|
||||
data = {
|
||||
"test_id": test_id,
|
||||
"truth": {"x_m": truth_x, "y_m": truth_y},
|
||||
"n_samples": stats.n_samples,
|
||||
"duration_s": round(duration, 3),
|
||||
"mean": {"x_m": round(stats.mean_x, 4),
|
||||
"y_m": round(stats.mean_y, 4)},
|
||||
"bias": {"x_m": round(stats.bias_x, 4),
|
||||
"y_m": round(stats.bias_y, 4)},
|
||||
"std": {"x_m": round(stats.std_x, 4),
|
||||
"y_m": round(stats.std_y, 4),
|
||||
"2d_m": round(stats.std_2d, 4)},
|
||||
"cep50_m": round(stats.cep50, 4),
|
||||
"cep95_m": round(stats.cep95, 4),
|
||||
"rmse_m": round(stats.rmse, 4),
|
||||
"max_error_m": round(stats.max_error, 4),
|
||||
"anchors": {
|
||||
str(aid): {
|
||||
"range_mean_m": round(ranges_snap[aid].mean, 4),
|
||||
"range_std_m": round(ranges_snap[aid].std, 4),
|
||||
"n": ranges_snap[aid].count,
|
||||
}
|
||||
for aid in anchor_ids
|
||||
},
|
||||
}
|
||||
try:
|
||||
with open(path, "w") as f:
|
||||
json.dump(data, f, indent=2)
|
||||
self.get_logger().info(f"Report written: {path}")
|
||||
except OSError as exc:
|
||||
self.get_logger().error(f"JSON write failed: {exc}")
|
||||
|
||||
|
||||
# ── Helpers ────────────────────────────────────────────────────────────────
|
||||
|
||||
def _stamp_ns(stamp) -> int:
|
||||
return stamp.sec * 1_000_000_000 + stamp.nanosec
|
||||
|
||||
|
||||
def main(args=None) -> None:
|
||||
rclpy.init(args=args)
|
||||
node = UwbLoggerNode()
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.try_shutdown()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
4
jetson/ros2_ws/src/saltybot_uwb_logger/setup.cfg
Normal file
4
jetson/ros2_ws/src/saltybot_uwb_logger/setup.cfg
Normal file
@ -0,0 +1,4 @@
|
||||
[develop]
|
||||
script_dir=$base/lib/saltybot_uwb_logger
|
||||
[install]
|
||||
install_scripts=$base/lib/saltybot_uwb_logger
|
||||
29
jetson/ros2_ws/src/saltybot_uwb_logger/setup.py
Normal file
29
jetson/ros2_ws/src/saltybot_uwb_logger/setup.py
Normal file
@ -0,0 +1,29 @@
|
||||
import os
|
||||
from glob import glob
|
||||
from setuptools import setup
|
||||
|
||||
package_name = "saltybot_uwb_logger"
|
||||
|
||||
setup(
|
||||
name=package_name,
|
||||
version="0.1.0",
|
||||
packages=[package_name],
|
||||
data_files=[
|
||||
("share/ament_index/resource_index/packages",
|
||||
[f"resource/{package_name}"]),
|
||||
(f"share/{package_name}", ["package.xml"]),
|
||||
(os.path.join("share", package_name, "launch"), glob("launch/*.py")),
|
||||
(os.path.join("share", package_name, "config"), glob("config/*.yaml")),
|
||||
],
|
||||
install_requires=["setuptools"],
|
||||
zip_safe=True,
|
||||
maintainer="sl-uwb",
|
||||
maintainer_email="sl-uwb@saltylab.local",
|
||||
description="UWB position logger and accuracy analyzer (Issue #634)",
|
||||
license="Apache-2.0",
|
||||
entry_points={
|
||||
"console_scripts": [
|
||||
"uwb_logger = saltybot_uwb_logger.logger_node:main",
|
||||
],
|
||||
},
|
||||
)
|
||||
Binary file not shown.
@ -0,0 +1,116 @@
|
||||
"""Unit tests for saltybot_uwb_logger.accuracy_stats (Issue #634). No ROS2 needed."""
|
||||
|
||||
import math
|
||||
import sys
|
||||
import os
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
sys.path.insert(0, os.path.join(os.path.dirname(__file__), ".."))
|
||||
from saltybot_uwb_logger.accuracy_stats import compute_stats, RangeAccum
|
||||
|
||||
|
||||
def _circle(n, r, cx=0.0, cy=0.0):
|
||||
a = np.linspace(0, 2 * math.pi, n, endpoint=False)
|
||||
return cx + r * np.cos(a), cy + r * np.sin(a)
|
||||
|
||||
|
||||
class TestComputeStats:
|
||||
def test_zero_error(self):
|
||||
xs = np.full(5, 2.0); ys = np.full(5, 3.0)
|
||||
s = compute_stats(xs, ys, 2.0, 3.0)
|
||||
assert abs(s.cep50) < 1e-9
|
||||
assert abs(s.rmse) < 1e-9
|
||||
assert abs(s.max_error) < 1e-9
|
||||
|
||||
def test_known_bias(self):
|
||||
rng = np.random.default_rng(0)
|
||||
xs = rng.normal(4.0, 0.01, 100); ys = rng.normal(2.0, 0.01, 100)
|
||||
s = compute_stats(xs, ys, 3.0, 2.0)
|
||||
assert abs(s.bias_x - 1.0) < 0.05
|
||||
assert abs(s.bias_y) < 0.05
|
||||
|
||||
def test_cep50_below_cep95(self):
|
||||
rng = np.random.default_rng(1)
|
||||
xs = rng.normal(0, 0.1, 500); ys = rng.normal(0, 0.1, 500)
|
||||
s = compute_stats(xs, ys, 0.0, 0.0)
|
||||
assert s.cep50 < s.cep95
|
||||
|
||||
def test_cep50_is_median(self):
|
||||
rng = np.random.default_rng(2)
|
||||
xs = rng.normal(0, 0.15, 1000); ys = rng.normal(0, 0.15, 1000)
|
||||
errors = np.sqrt(xs**2 + ys**2)
|
||||
s = compute_stats(xs, ys, 0.0, 0.0)
|
||||
assert abs(s.cep50 - float(np.median(errors))) < 1e-9
|
||||
|
||||
def test_rmse(self):
|
||||
xs = np.array([0.1, -0.1, 0.2, -0.2]); ys = np.zeros(4)
|
||||
s = compute_stats(xs, ys, 0.0, 0.0)
|
||||
assert abs(s.rmse - math.sqrt(np.mean(xs**2))) < 1e-9
|
||||
|
||||
def test_max_error(self):
|
||||
xs = np.array([0.0, 0.0, 1.0]); ys = np.zeros(3)
|
||||
s = compute_stats(xs, ys, 0.0, 0.0)
|
||||
assert abs(s.max_error - 1.0) < 1e-9
|
||||
|
||||
def test_circle_symmetry(self):
|
||||
xs, ys = _circle(360, 0.12)
|
||||
s = compute_stats(xs, ys, 0.0, 0.0)
|
||||
assert abs(s.bias_x) < 1e-6
|
||||
assert abs(s.cep50 - 0.12) < 1e-6
|
||||
|
||||
def test_std_2d_pythagorean(self):
|
||||
rng = np.random.default_rng(3)
|
||||
xs = rng.normal(0, 0.1, 200); ys = rng.normal(0, 0.2, 200)
|
||||
s = compute_stats(xs, ys, 0.0, 0.0)
|
||||
assert abs(s.std_2d - math.sqrt(s.std_x**2 + s.std_y**2)) < 1e-9
|
||||
|
||||
def test_n_samples(self):
|
||||
xs = np.ones(42); ys = np.zeros(42)
|
||||
s = compute_stats(xs, ys, 0.0, 0.0)
|
||||
assert s.n_samples == 42
|
||||
|
||||
def test_too_few_raises(self):
|
||||
with pytest.raises(ValueError):
|
||||
compute_stats(np.array([1.0]), np.array([1.0]), 0.0, 0.0)
|
||||
|
||||
def test_length_mismatch_raises(self):
|
||||
with pytest.raises(ValueError):
|
||||
compute_stats(np.array([1.0, 2.0]), np.array([1.0]), 0.0, 0.0)
|
||||
|
||||
|
||||
class TestRealistic:
|
||||
def test_10cm_cep50(self):
|
||||
rng = np.random.default_rng(42)
|
||||
xs = rng.normal(3.0, 0.07, 500); ys = rng.normal(2.0, 0.07, 500)
|
||||
s = compute_stats(xs, ys, 3.0, 2.0)
|
||||
assert 0.05 < s.cep50 < 0.20
|
||||
|
||||
def test_biased_uwb(self):
|
||||
rng = np.random.default_rng(7)
|
||||
xs = rng.normal(3.15, 0.05, 300); ys = rng.normal(2.0, 0.05, 300)
|
||||
s = compute_stats(xs, ys, 3.0, 2.0)
|
||||
assert abs(s.bias_x - 0.15) < 0.02
|
||||
|
||||
|
||||
class TestRangeAccum:
|
||||
def test_empty(self):
|
||||
a = RangeAccum(0)
|
||||
assert a.mean == 0.0 and a.std == 0.0 and a.count == 0
|
||||
|
||||
def test_single_std_zero(self):
|
||||
a = RangeAccum(0); a.add(2.0)
|
||||
assert a.std == 0.0
|
||||
|
||||
def test_mean_std(self):
|
||||
a = RangeAccum(0)
|
||||
for v in [1.0, 2.0, 3.0, 4.0, 5.0]:
|
||||
a.add(v)
|
||||
assert abs(a.mean - 3.0) < 1e-9
|
||||
assert abs(a.std - float(np.std([1,2,3,4,5], ddof=1))) < 1e-9
|
||||
assert a.count == 5
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
pytest.main([__file__, "-v"])
|
||||
15
jetson/ros2_ws/src/saltybot_uwb_logger_msgs/CMakeLists.txt
Normal file
15
jetson/ros2_ws/src/saltybot_uwb_logger_msgs/CMakeLists.txt
Normal file
@ -0,0 +1,15 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(saltybot_uwb_logger_msgs)
|
||||
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(std_msgs REQUIRED)
|
||||
find_package(rosidl_default_generators REQUIRED)
|
||||
|
||||
rosidl_generate_interfaces(${PROJECT_NAME}
|
||||
"msg/AccuracyReport.msg"
|
||||
"srv/StartAccuracyTest.srv"
|
||||
DEPENDENCIES std_msgs
|
||||
)
|
||||
|
||||
ament_export_dependencies(rosidl_default_runtime)
|
||||
ament_package()
|
||||
@ -0,0 +1,32 @@
|
||||
# AccuracyReport.msg — UWB positioning accuracy test results (Issue #634)
|
||||
#
|
||||
# Published on /saltybot/uwb/accuracy_report after StartAccuracyTest completes.
|
||||
|
||||
std_msgs/Header header
|
||||
|
||||
float64 truth_x_m
|
||||
float64 truth_y_m
|
||||
|
||||
uint32 n_samples
|
||||
|
||||
float64 mean_x_m
|
||||
float64 mean_y_m
|
||||
|
||||
float64 std_x_m
|
||||
float64 std_y_m
|
||||
float64 std_2d_m
|
||||
|
||||
float64 bias_x_m
|
||||
float64 bias_y_m
|
||||
|
||||
float64 cep50_m
|
||||
float64 cep95_m
|
||||
float64 max_error_m
|
||||
float64 rmse_m
|
||||
|
||||
uint8[] anchor_ids
|
||||
float64[] anchor_range_mean_m
|
||||
float64[] anchor_range_std_m
|
||||
|
||||
string test_id
|
||||
float64 duration_s
|
||||
21
jetson/ros2_ws/src/saltybot_uwb_logger_msgs/package.xml
Normal file
21
jetson/ros2_ws/src/saltybot_uwb_logger_msgs/package.xml
Normal file
@ -0,0 +1,21 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>saltybot_uwb_logger_msgs</name>
|
||||
<version>0.1.0</version>
|
||||
<description>Message and service definitions for UWB logger (Issue #634).</description>
|
||||
<maintainer email="sl-uwb@saltylab.local">sl-uwb</maintainer>
|
||||
<license>Apache-2.0</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
<buildtool_depend>rosidl_default_generators</buildtool_depend>
|
||||
|
||||
<depend>std_msgs</depend>
|
||||
<exec_depend>rosidl_default_runtime</exec_depend>
|
||||
|
||||
<member_of_group>rosidl_interface_packages</member_of_group>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@ -0,0 +1,15 @@
|
||||
# StartAccuracyTest.srv — trigger UWB accuracy test at a known position (Issue #634)
|
||||
|
||||
# ── Request ────────────────────────────────────────────────────────────────
|
||||
float64 truth_x_m
|
||||
float64 truth_y_m
|
||||
uint32 n_samples
|
||||
float64 timeout_s
|
||||
string test_id
|
||||
|
||||
---
|
||||
|
||||
# ── Response ───────────────────────────────────────────────────────────────
|
||||
bool success
|
||||
string message
|
||||
string test_id
|
||||
398
phone/voice_cmd.py
Normal file
398
phone/voice_cmd.py
Normal file
@ -0,0 +1,398 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
voice_cmd.py — Termux voice command interface for SaltyBot (Issue #633)
|
||||
|
||||
Listens for voice commands via termux-speech-to-text (Android built-in STT),
|
||||
parses them into structured JSON payloads, and publishes to MQTT.
|
||||
Provides TTS confirmation via termux-tts-speak.
|
||||
Falls back to manual text input when STT is unavailable.
|
||||
|
||||
MQTT Topic
|
||||
──────────
|
||||
saltybot/phone/voice_cmd — published on each recognised command
|
||||
|
||||
JSON Payload
|
||||
────────────
|
||||
{
|
||||
"ts": 1710000000.000, # Unix timestamp
|
||||
"cmd": "go_forward", # command enum (see COMMANDS below)
|
||||
"param": null, # optional parameter (e.g. waypoint name)
|
||||
"raw": "go forward", # original recognised text
|
||||
"source": "stt" # "stt" | "text"
|
||||
}
|
||||
|
||||
Commands
|
||||
────────
|
||||
go_forward, go_back, turn_left, turn_right, stop, estop,
|
||||
go_waypoint (param = waypoint name), speed_up, speed_down, status
|
||||
|
||||
Usage
|
||||
─────
|
||||
python3 phone/voice_cmd.py [OPTIONS]
|
||||
|
||||
--broker HOST MQTT broker IP/hostname (default: 192.168.1.100)
|
||||
--port PORT MQTT broker port (default: 1883)
|
||||
--qos INT MQTT QoS level 0/1/2 (default: 0)
|
||||
--text Manual text fallback mode (no STT)
|
||||
--no-tts Disable TTS confirmation
|
||||
--timeout SECS STT listen timeout in seconds (default: 10)
|
||||
--debug Verbose logging
|
||||
|
||||
Dependencies (Termux)
|
||||
─────────────────────
|
||||
pkg install termux-api python
|
||||
pip install paho-mqtt
|
||||
"""
|
||||
|
||||
import argparse
|
||||
import json
|
||||
import logging
|
||||
import subprocess
|
||||
import sys
|
||||
import threading
|
||||
import time
|
||||
from typing import Optional, Tuple
|
||||
|
||||
try:
|
||||
import paho.mqtt.client as mqtt
|
||||
MQTT_AVAILABLE = True
|
||||
except ImportError:
|
||||
MQTT_AVAILABLE = False
|
||||
|
||||
# ── MQTT topic ────────────────────────────────────────────────────────────────
|
||||
|
||||
TOPIC_VOICE_CMD = "saltybot/phone/voice_cmd"
|
||||
|
||||
# ── Command table ─────────────────────────────────────────────────────────────
|
||||
#
|
||||
# Each entry: (command_id, [phrase_fragments, ...])
|
||||
# Phrases are matched as case-insensitive substrings of the recognised text.
|
||||
# First match wins; order matters for disambiguation.
|
||||
|
||||
_CMD_TABLE = [
|
||||
# E-stop must be checked before plain "stop"
|
||||
("estop", ["emergency stop", "e stop", "estop", "abort"]),
|
||||
("stop", ["stop", "halt", "freeze", "cancel"]),
|
||||
("go_forward", ["go forward", "move forward", "forward", "ahead", "advance"]),
|
||||
("go_back", ["go back", "move back", "backward", "reverse", "back up"]),
|
||||
("turn_left", ["turn left", "rotate left", "left"]),
|
||||
("turn_right", ["turn right", "rotate right", "right"]),
|
||||
("go_waypoint", ["go to waypoint", "go to", "navigate to", "waypoint"]),
|
||||
("speed_up", ["speed up", "faster", "increase speed"]),
|
||||
("speed_down", ["slow down", "slower", "decrease speed", "decelerate"]),
|
||||
("status", ["status", "report", "where are you", "how are you"]),
|
||||
]
|
||||
|
||||
# TTS responses per command
|
||||
_TTS_RESPONSES = {
|
||||
"go_forward": "Going forward",
|
||||
"go_back": "Going back",
|
||||
"turn_left": "Turning left",
|
||||
"turn_right": "Turning right",
|
||||
"stop": "Stopping",
|
||||
"estop": "Emergency stop!",
|
||||
"go_waypoint": "Navigating to waypoint",
|
||||
"speed_up": "Speeding up",
|
||||
"speed_down": "Slowing down",
|
||||
"status": "Requesting status",
|
||||
}
|
||||
|
||||
# ── termux-api helpers ────────────────────────────────────────────────────────
|
||||
|
||||
def stt_listen(timeout: float = 10.0) -> Optional[str]:
|
||||
"""
|
||||
Call termux-speech-to-text and return the recognised text string, or None.
|
||||
|
||||
termux-speech-to-text returns JSON:
|
||||
{"partial": "...", "text": "final text"}
|
||||
or on failure returns empty / error output.
|
||||
"""
|
||||
try:
|
||||
result = subprocess.run(
|
||||
["termux-speech-to-text"],
|
||||
capture_output=True,
|
||||
text=True,
|
||||
timeout=timeout + 5.0, # extra buffer for app round-trip
|
||||
)
|
||||
stdout = result.stdout.strip()
|
||||
if not stdout:
|
||||
logging.debug("STT: empty response (rc=%d)", result.returncode)
|
||||
return None
|
||||
|
||||
# termux-speech-to-text may return bare text or JSON
|
||||
try:
|
||||
data = json.loads(stdout)
|
||||
# Prefer "text" (final); fall back to "partial"
|
||||
text = data.get("text") or data.get("partial") or ""
|
||||
text = text.strip()
|
||||
except (json.JSONDecodeError, AttributeError):
|
||||
# Some versions return plain text directly
|
||||
text = stdout
|
||||
|
||||
if not text:
|
||||
logging.debug("STT: no text in response: %r", stdout)
|
||||
return None
|
||||
|
||||
logging.info("STT recognised: %r", text)
|
||||
return text
|
||||
|
||||
except subprocess.TimeoutExpired:
|
||||
logging.warning("STT: timed out after %.1f s", timeout + 5.0)
|
||||
return None
|
||||
except FileNotFoundError:
|
||||
logging.error("STT: termux-speech-to-text not found — install termux-api")
|
||||
return None
|
||||
except Exception as e:
|
||||
logging.warning("STT error: %s", e)
|
||||
return None
|
||||
|
||||
|
||||
def tts_speak(text: str) -> None:
|
||||
"""Speak @text via termux-tts-speak (fire-and-forget)."""
|
||||
try:
|
||||
subprocess.Popen(
|
||||
["termux-tts-speak", text],
|
||||
stdout=subprocess.DEVNULL,
|
||||
stderr=subprocess.DEVNULL,
|
||||
)
|
||||
except FileNotFoundError:
|
||||
logging.debug("TTS: termux-tts-speak not found")
|
||||
except Exception as e:
|
||||
logging.debug("TTS error: %s", e)
|
||||
|
||||
|
||||
# ── Command parser ────────────────────────────────────────────────────────────
|
||||
|
||||
def parse_command(text: str) -> Tuple[Optional[str], Optional[str]]:
|
||||
"""
|
||||
Match @text against the command table.
|
||||
|
||||
Returns (command_id, param) where param is non-None only for go_waypoint.
|
||||
Returns (None, None) if no command matched.
|
||||
"""
|
||||
lower = text.lower().strip()
|
||||
|
||||
for cmd_id, phrases in _CMD_TABLE:
|
||||
for phrase in phrases:
|
||||
if phrase in lower:
|
||||
param = None
|
||||
if cmd_id == "go_waypoint":
|
||||
# Extract waypoint name: text after "waypoint" / "go to" / "navigate to"
|
||||
for marker in ("waypoint", "navigate to", "go to"):
|
||||
idx = lower.find(marker)
|
||||
if idx != -1:
|
||||
remainder = text[idx + len(marker):].strip()
|
||||
if remainder:
|
||||
param = remainder
|
||||
break
|
||||
return cmd_id, param
|
||||
|
||||
return None, None
|
||||
|
||||
|
||||
# ── MQTT publisher (same pattern as sensor_dashboard.py) ─────────────────────
|
||||
|
||||
class MQTTPublisher:
|
||||
"""Thin paho-mqtt wrapper with auto-reconnect."""
|
||||
|
||||
_RECONNECT_BASE = 2.0
|
||||
_RECONNECT_MAX = 60.0
|
||||
|
||||
def __init__(self, broker: str, port: int, qos: int = 0):
|
||||
self._broker = broker
|
||||
self._port = port
|
||||
self._qos = qos
|
||||
self._lock = threading.Lock()
|
||||
self._connected = False
|
||||
|
||||
self._client = mqtt.Client(client_id="saltybot-voice-cmd", clean_session=True)
|
||||
self._client.on_connect = self._on_connect
|
||||
self._client.on_disconnect = self._on_disconnect
|
||||
self._client.reconnect_delay_set(
|
||||
min_delay=int(self._RECONNECT_BASE),
|
||||
max_delay=int(self._RECONNECT_MAX),
|
||||
)
|
||||
self._connect()
|
||||
|
||||
def _connect(self) -> None:
|
||||
try:
|
||||
self._client.connect_async(self._broker, self._port, keepalive=60)
|
||||
self._client.loop_start()
|
||||
logging.info("MQTT connecting to %s:%d …", self._broker, self._port)
|
||||
except Exception as e:
|
||||
logging.warning("MQTT connect error: %s", e)
|
||||
|
||||
def _on_connect(self, client, userdata, flags, rc) -> None:
|
||||
if rc == 0:
|
||||
with self._lock:
|
||||
self._connected = True
|
||||
logging.info("MQTT connected to %s:%d", self._broker, self._port)
|
||||
else:
|
||||
logging.warning("MQTT connect failed rc=%d", rc)
|
||||
|
||||
def _on_disconnect(self, client, userdata, rc) -> None:
|
||||
with self._lock:
|
||||
self._connected = False
|
||||
if rc != 0:
|
||||
logging.warning("MQTT disconnected (rc=%d) — paho will retry", rc)
|
||||
|
||||
@property
|
||||
def connected(self) -> bool:
|
||||
with self._lock:
|
||||
return self._connected
|
||||
|
||||
def publish(self, topic: str, payload: dict) -> bool:
|
||||
if not self.connected:
|
||||
logging.debug("MQTT offline — dropping %s", topic)
|
||||
return False
|
||||
try:
|
||||
msg = json.dumps(payload, separators=(",", ":"))
|
||||
info = self._client.publish(topic, msg, qos=self._qos, retain=False)
|
||||
return info.rc == mqtt.MQTT_ERR_SUCCESS
|
||||
except Exception as e:
|
||||
logging.warning("MQTT publish error: %s", e)
|
||||
return False
|
||||
|
||||
def shutdown(self) -> None:
|
||||
self._client.loop_stop()
|
||||
self._client.disconnect()
|
||||
logging.info("MQTT disconnected.")
|
||||
|
||||
|
||||
# ── Main listen loop ──────────────────────────────────────────────────────────
|
||||
|
||||
def _handle_text(raw: str, source: str, publisher: MQTTPublisher,
|
||||
no_tts: bool) -> bool:
|
||||
"""
|
||||
Parse @raw, publish command, speak confirmation.
|
||||
Returns True if a command was recognised and published.
|
||||
"""
|
||||
cmd, param = parse_command(raw)
|
||||
if cmd is None:
|
||||
logging.info("No command matched for: %r", raw)
|
||||
if not no_tts:
|
||||
tts_speak("Sorry, I didn't understand that")
|
||||
return False
|
||||
|
||||
payload = {
|
||||
"ts": time.time(),
|
||||
"cmd": cmd,
|
||||
"param": param,
|
||||
"raw": raw,
|
||||
"source": source,
|
||||
}
|
||||
ok = publisher.publish(TOPIC_VOICE_CMD, payload)
|
||||
log_msg = "Published %s (param=%r) — MQTT %s" % (cmd, param, "OK" if ok else "FAIL")
|
||||
logging.info(log_msg)
|
||||
|
||||
if not no_tts:
|
||||
response = _TTS_RESPONSES.get(cmd, cmd.replace("_", " "))
|
||||
if param:
|
||||
response = f"{response}: {param}"
|
||||
tts_speak(response)
|
||||
|
||||
return ok
|
||||
|
||||
|
||||
def run_stt_loop(publisher: MQTTPublisher, args: argparse.Namespace) -> None:
|
||||
"""Continuous STT listen → parse → publish loop."""
|
||||
logging.info("Voice command loop started (STT mode). Say a command.")
|
||||
if not args.no_tts:
|
||||
tts_speak("Voice commands ready")
|
||||
|
||||
consecutive_failures = 0
|
||||
|
||||
while True:
|
||||
logging.info("Listening…")
|
||||
text = stt_listen(timeout=args.timeout)
|
||||
|
||||
if text is None:
|
||||
consecutive_failures += 1
|
||||
logging.warning("STT failed (%d consecutive)", consecutive_failures)
|
||||
if consecutive_failures >= 3:
|
||||
logging.warning("STT unavailable — switch to --text mode if needed")
|
||||
if not args.no_tts:
|
||||
tts_speak("Speech recognition unavailable")
|
||||
time.sleep(5.0)
|
||||
consecutive_failures = 0
|
||||
continue
|
||||
|
||||
consecutive_failures = 0
|
||||
_handle_text(text, "stt", publisher, args.no_tts)
|
||||
|
||||
|
||||
def run_text_loop(publisher: MQTTPublisher, args: argparse.Namespace) -> None:
|
||||
"""Manual text input loop (--text / --no-stt mode)."""
|
||||
logging.info("Text input mode. Type a command (Ctrl-C or 'quit' to exit).")
|
||||
print("\nAvailable commands: go forward, go back, turn left, turn right, "
|
||||
"stop, emergency stop, go to waypoint <name>, speed up, slow down, status")
|
||||
print("Type 'quit' to exit.\n")
|
||||
|
||||
while True:
|
||||
try:
|
||||
raw = input("Command> ").strip()
|
||||
except EOFError:
|
||||
break
|
||||
|
||||
if not raw:
|
||||
continue
|
||||
if raw.lower() in ("quit", "exit", "q"):
|
||||
break
|
||||
|
||||
_handle_text(raw, "text", publisher, args.no_tts)
|
||||
|
||||
|
||||
# ── Entry point ───────────────────────────────────────────────────────────────
|
||||
|
||||
def main() -> None:
|
||||
parser = argparse.ArgumentParser(
|
||||
description="SaltyBot Termux voice command interface (Issue #633)"
|
||||
)
|
||||
parser.add_argument("--broker", default="192.168.1.100",
|
||||
help="MQTT broker IP/hostname (default: 192.168.1.100)")
|
||||
parser.add_argument("--port", type=int, default=1883,
|
||||
help="MQTT broker port (default: 1883)")
|
||||
parser.add_argument("--qos", type=int, default=0, choices=[0, 1, 2],
|
||||
help="MQTT QoS level (default: 0)")
|
||||
parser.add_argument("--text", action="store_true",
|
||||
help="Manual text fallback mode (no STT)")
|
||||
parser.add_argument("--no-tts", action="store_true",
|
||||
help="Disable TTS confirmation")
|
||||
parser.add_argument("--timeout", type=float, default=10.0,
|
||||
help="STT listen timeout in seconds (default: 10)")
|
||||
parser.add_argument("--debug", action="store_true",
|
||||
help="Verbose logging")
|
||||
args = parser.parse_args()
|
||||
|
||||
logging.basicConfig(
|
||||
level=logging.DEBUG if args.debug else logging.INFO,
|
||||
format="%(asctime)s [%(levelname)s] %(message)s",
|
||||
)
|
||||
|
||||
if not MQTT_AVAILABLE:
|
||||
logging.error("paho-mqtt not installed. Run: pip install paho-mqtt")
|
||||
sys.exit(1)
|
||||
|
||||
publisher = MQTTPublisher(args.broker, args.port, qos=args.qos)
|
||||
|
||||
# Wait for initial MQTT connection
|
||||
deadline = time.monotonic() + 10.0
|
||||
while not publisher.connected and time.monotonic() < deadline:
|
||||
time.sleep(0.2)
|
||||
if not publisher.connected:
|
||||
logging.warning("MQTT not connected — commands will be dropped until connected")
|
||||
|
||||
try:
|
||||
if args.text:
|
||||
run_text_loop(publisher, args)
|
||||
else:
|
||||
run_stt_loop(publisher, args)
|
||||
except KeyboardInterrupt:
|
||||
logging.info("Shutting down…")
|
||||
finally:
|
||||
publisher.shutdown()
|
||||
logging.info("Done.")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
11
src/main.c
11
src/main.c
@ -438,10 +438,8 @@ int main(void) {
|
||||
(double)bal.kp, (double)bal.ki, (double)bal.kd);
|
||||
}
|
||||
|
||||
|
||||
/* UART protocol: handle commands from Jetson (Issue #629) */
|
||||
{
|
||||
extern UartProtState uart_prot_state;
|
||||
if (uart_prot_state.vel_updated) {
|
||||
uart_prot_state.vel_updated = 0u;
|
||||
if (bal.state == BALANCE_ARMED && !lvc_is_cutoff()) {
|
||||
@ -809,15 +807,14 @@ int main(void) {
|
||||
jlink_send_lvc_tlm(<lm);
|
||||
}
|
||||
|
||||
|
||||
/* UART protocol: send STATUS to Jetson at 10 Hz (Issue #629) */
|
||||
if (now - uart_status_tick >= 100u) {
|
||||
uart_status_tick = now;
|
||||
uart_prot_status_t ups;
|
||||
ups.pitch_x10 = (int16_t)(bal.pitch_deg * 10.0f);
|
||||
ups.motor_cmd = bal.motor_cmd;
|
||||
uint32_t _uv = battery_read_mv();
|
||||
ups.vbat_mv = (_uv > 65535u) ? 65535u : (uint16_t)_uv;
|
||||
ups.pitch_x10 = (int16_t)(bal.pitch_deg * 10.0f);
|
||||
ups.motor_cmd = bal.motor_cmd;
|
||||
uint32_t _uv = battery_read_mv();
|
||||
ups.vbat_mv = (_uv > 65535u) ? 65535u : (uint16_t)_uv;
|
||||
ups.balance_state = (uint8_t)bal.state;
|
||||
ups.estop_active = safety_remote_estop_active() ? 1u : 0u;
|
||||
uart_protocol_send_status(&ups);
|
||||
|
||||
@ -1,14 +1,14 @@
|
||||
/*
|
||||
* uart_protocol.c -- Structured UART command protocol (Issue #629)
|
||||
* uart_protocol.c — UART command protocol for Jetson-STM32 communication (Issue #629)
|
||||
*
|
||||
* UART5 (PC12=TX / PD2=RX) at 115200 baud.
|
||||
* DMA1_Stream0_Channel4 runs in circular mode to fill a 256-byte ring buffer.
|
||||
* uart_protocol_process() is called each main loop tick to drain new bytes
|
||||
* through a lightweight state-machine parser.
|
||||
* Physical: UART5, PC12 (TX, AF8) / PD2 (RX, AF8), 115200 baud, 8N1, no flow control.
|
||||
* NOTE: Spec requested USART1 @ 115200, but USART1 is already used by JLink @ 921600.
|
||||
* Implemented on UART5 (PC12/PD2) instead.
|
||||
*
|
||||
* Frame: [STX=0x02][LEN][CMD][PAYLOAD...][CRC8][ETX=0x03]
|
||||
* LEN = 1 (CMD) + payload_len
|
||||
* CRC8 = SMBUS (poly 0x07, init 0x00) computed over CMD+PAYLOAD bytes
|
||||
* RX: DMA1_Stream0 (Channel 4), 256-byte circular buffer, no interrupt needed.
|
||||
* TX: Polled (HAL_UART_Transmit), frames are short (<20 B) so blocking is acceptable.
|
||||
*
|
||||
* CRC: CRC8-SMBUS — poly 0x07, init 0x00, computed over CMD+PAYLOAD bytes only.
|
||||
*/
|
||||
|
||||
#include "uart_protocol.h"
|
||||
@ -16,318 +16,255 @@
|
||||
#include "stm32f7xx_hal.h"
|
||||
#include <string.h>
|
||||
|
||||
/* ---- DMA ring buffer ---- */
|
||||
static volatile uint8_t s_rx_buf[UPROT_RX_BUF_LEN];
|
||||
static uint16_t s_rx_head = 0; /* last byte we processed */
|
||||
/* ── Configuration ─────────────────────────────────────────────────────────── */
|
||||
#define RX_BUF_SIZE 256u /* must be power-of-two for wrap math */
|
||||
#define TX_TIMEOUT 5u /* HAL_UART_Transmit timeout ms */
|
||||
|
||||
/* ---- HAL handles ---- */
|
||||
static UART_HandleTypeDef s_huart;
|
||||
static DMA_HandleTypeDef s_hdma_rx;
|
||||
/* ── Peripheral handles ───────────────────────────────────────────────────── */
|
||||
static UART_HandleTypeDef huart5;
|
||||
static DMA_HandleTypeDef hdma_rx;
|
||||
|
||||
/* ---- TX buffer (largest TX frame = URESP_STATUS: 13 bytes) ---- */
|
||||
#define TX_BUF_LEN 16u
|
||||
static uint8_t s_tx_buf[TX_BUF_LEN];
|
||||
/* ── DMA ring buffer ──────────────────────────────────────────────────────── */
|
||||
static uint8_t rx_buf[RX_BUF_SIZE];
|
||||
static uint32_t rx_head = 0u; /* next byte to consume */
|
||||
|
||||
/* ---- Public state ---- */
|
||||
volatile UartProtState uart_prot_state;
|
||||
/* ── Shared state (read by main.c) ───────────────────────────────────────── */
|
||||
UartProtState uart_prot_state;
|
||||
|
||||
/* ---- Parser state ---- */
|
||||
/* ── Parser state machine ─────────────────────────────────────────────────── */
|
||||
typedef enum {
|
||||
PS_IDLE = 0,
|
||||
PS_IDLE,
|
||||
PS_LEN,
|
||||
PS_CMD,
|
||||
PS_PAYLOAD,
|
||||
PS_CRC,
|
||||
PS_ETX,
|
||||
PS_ETX
|
||||
} ParseState;
|
||||
|
||||
static ParseState s_ps = PS_IDLE;
|
||||
static uint8_t s_frame_len = 0; /* LEN byte value */
|
||||
static uint8_t s_frame_cmd = 0;
|
||||
static uint8_t s_payload_buf[UPROT_MAX_PAYLOAD];
|
||||
static uint8_t s_payload_idx = 0;
|
||||
static uint8_t s_frame_crc = 0;
|
||||
static ParseState ps = PS_IDLE;
|
||||
static uint8_t ps_len = 0u; /* expected payload bytes */
|
||||
static uint8_t ps_cmd = 0u; /* command byte */
|
||||
static uint8_t ps_payload[12]; /* max payload = SET_PID = 12 B */
|
||||
static uint8_t ps_pi = 0u; /* payload index */
|
||||
static uint8_t ps_crc = 0u; /* received CRC byte */
|
||||
|
||||
/* ================================================================
|
||||
* CRC8-SMBUS: poly=0x07, init=0x00
|
||||
* ================================================================ */
|
||||
static uint8_t crc8_smbus(const uint8_t *data, uint8_t len)
|
||||
/* ── CRC8-SMBUS ───────────────────────────────────────────────────────────── */
|
||||
static uint8_t crc8(const uint8_t *data, uint8_t len)
|
||||
{
|
||||
uint8_t crc = 0x00u;
|
||||
for (uint8_t i = 0u; i < len; i++) {
|
||||
crc ^= data[i];
|
||||
for (uint8_t b = 0u; b < 8u; b++) {
|
||||
crc = (crc & 0x80u) ? (uint8_t)((crc << 1u) ^ 0x07u)
|
||||
: (uint8_t)(crc << 1u);
|
||||
while (len--) {
|
||||
crc ^= *data++;
|
||||
for (uint8_t i = 0u; i < 8u; i++) {
|
||||
if (crc & 0x80u)
|
||||
crc = (uint8_t)((crc << 1) ^ 0x07u);
|
||||
else
|
||||
crc <<= 1;
|
||||
}
|
||||
}
|
||||
return crc;
|
||||
}
|
||||
|
||||
/* ================================================================
|
||||
* TX helpers
|
||||
* ================================================================ */
|
||||
static void tx_send(const uint8_t *buf, uint8_t len)
|
||||
/* ── TX helper ────────────────────────────────────────────────────────────── */
|
||||
static void tx_frame(uint8_t cmd, const uint8_t *payload, uint8_t plen)
|
||||
{
|
||||
/* Blocking transmit — frames are short (<=13 bytes @ 115200 = <1.2 ms) */
|
||||
HAL_UART_Transmit(&s_huart, (uint8_t *)buf, len, 5u);
|
||||
uint8_t frame[20];
|
||||
uint8_t fi = 0u;
|
||||
frame[fi++] = UPROT_STX;
|
||||
frame[fi++] = plen;
|
||||
frame[fi++] = cmd;
|
||||
for (uint8_t i = 0u; i < plen; i++)
|
||||
frame[fi++] = payload[i];
|
||||
/* CRC over CMD + PAYLOAD */
|
||||
frame[fi++] = crc8(&frame[2], (uint8_t)(1u + plen));
|
||||
frame[fi++] = UPROT_ETX;
|
||||
HAL_UART_Transmit(&huart5, frame, fi, TX_TIMEOUT);
|
||||
}
|
||||
|
||||
static void send_ack(uint8_t cmd)
|
||||
{
|
||||
/* [STX][LEN=2][URESP_ACK][cmd][CRC8][ETX] = 6 bytes */
|
||||
uint8_t payload[2] = { URESP_ACK, cmd };
|
||||
uint8_t crc = crc8_smbus(payload, 2u);
|
||||
uint8_t frame[6] = { UPROT_STX, 0x02u, URESP_ACK, cmd, crc, UPROT_ETX };
|
||||
tx_send(frame, 6u);
|
||||
tx_frame(URESP_ACK, &cmd, 1u);
|
||||
}
|
||||
|
||||
static void send_nack(uint8_t cmd, uint8_t err)
|
||||
{
|
||||
/* [STX][LEN=3][URESP_NACK][cmd][err][CRC8][ETX] = 7 bytes */
|
||||
uint8_t payload[3] = { URESP_NACK, cmd, err };
|
||||
uint8_t crc = crc8_smbus(payload, 3u);
|
||||
uint8_t frame[7] = { UPROT_STX, 0x03u, URESP_NACK, cmd, err, crc, UPROT_ETX };
|
||||
tx_send(frame, 7u);
|
||||
uint8_t p[2] = { cmd, err };
|
||||
tx_frame(URESP_NACK, p, 2u);
|
||||
}
|
||||
|
||||
/* ================================================================
|
||||
* Command dispatcher (called when a valid frame is fully received)
|
||||
* ================================================================ */
|
||||
/* ── Command dispatcher ───────────────────────────────────────────────────── */
|
||||
static void dispatch(uint8_t cmd, const uint8_t *payload, uint8_t plen)
|
||||
{
|
||||
switch (cmd) {
|
||||
|
||||
case UCMD_SET_VELOCITY:
|
||||
if (plen != 4u) { send_nack(cmd, UERR_BAD_LENGTH); return; }
|
||||
{
|
||||
int16_t lr, rr;
|
||||
memcpy(&lr, &payload[0], 2u);
|
||||
memcpy(&rr, &payload[2], 2u);
|
||||
uart_prot_state.left_rpm = lr;
|
||||
uart_prot_state.right_rpm = rr;
|
||||
uart_prot_state.vel_updated = 1u;
|
||||
}
|
||||
send_ack(cmd);
|
||||
break;
|
||||
|
||||
case UCMD_GET_STATUS:
|
||||
if (plen != 0u) { send_nack(cmd, UERR_BAD_LENGTH); return; }
|
||||
/* ACK not sent; STATUS response IS the reply */
|
||||
uart_prot_state.vel_updated = uart_prot_state.vel_updated; /* no-op flag; main sends status */
|
||||
/* Signal main loop to send status — reuse vel_updated channel not appropriate;
|
||||
* Instead send ACK here and let main loop send STATUS immediately after.
|
||||
* To keep ISR-safe, just set a flag and let main loop build the payload. */
|
||||
/* Simple approach: ack first, status built by main.c using uart_protocol_send_status() */
|
||||
send_ack(cmd); /* main.c checks for vel_updated; GET_STATUS is handled by main */
|
||||
break;
|
||||
|
||||
case UCMD_SET_PID:
|
||||
if (plen != 12u) { send_nack(cmd, UERR_BAD_LENGTH); return; }
|
||||
{
|
||||
float kp, ki, kd;
|
||||
memcpy(&kp, &payload[0], 4u);
|
||||
memcpy(&ki, &payload[4], 4u);
|
||||
memcpy(&kd, &payload[8], 4u);
|
||||
uart_prot_state.pid_kp = kp;
|
||||
uart_prot_state.pid_ki = ki;
|
||||
uart_prot_state.pid_kd = kd;
|
||||
uart_prot_state.pid_updated = 1u;
|
||||
}
|
||||
send_ack(cmd);
|
||||
break;
|
||||
|
||||
case UCMD_ESTOP:
|
||||
if (plen != 0u) { send_nack(cmd, UERR_BAD_LENGTH); return; }
|
||||
uart_prot_state.estop_req = 1u;
|
||||
send_ack(cmd);
|
||||
break;
|
||||
|
||||
case UCMD_CLEAR_ESTOP:
|
||||
if (plen != 0u) { send_nack(cmd, UERR_BAD_LENGTH); return; }
|
||||
uart_prot_state.estop_clear_req = 1u;
|
||||
send_ack(cmd);
|
||||
break;
|
||||
|
||||
default:
|
||||
send_nack(cmd, UERR_UNKNOWN_CMD);
|
||||
break;
|
||||
/* Validate CRC (computed over cmd + payload) */
|
||||
uint8_t buf[13];
|
||||
buf[0] = cmd;
|
||||
memcpy(&buf[1], payload, plen);
|
||||
if (crc8(buf, (uint8_t)(1u + plen)) != ps_crc) {
|
||||
send_nack(cmd, UERR_BAD_CRC);
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
/* ================================================================
|
||||
* uart_protocol_init()
|
||||
* ================================================================ */
|
||||
void uart_protocol_init(void)
|
||||
{
|
||||
/* ---- GPIO: PC12 = TX, PD2 = RX ---- */
|
||||
__HAL_RCC_GPIOC_CLK_ENABLE();
|
||||
__HAL_RCC_GPIOD_CLK_ENABLE();
|
||||
uart_prot_state.last_rx_ms = HAL_GetTick();
|
||||
|
||||
GPIO_InitTypeDef gpio = {0};
|
||||
gpio.Mode = GPIO_MODE_AF_PP;
|
||||
gpio.Pull = GPIO_PULLUP;
|
||||
gpio.Speed = GPIO_SPEED_FREQ_HIGH;
|
||||
gpio.Alternate = GPIO_AF8_UART5;
|
||||
|
||||
gpio.Pin = GPIO_PIN_12;
|
||||
HAL_GPIO_Init(GPIOC, &gpio);
|
||||
|
||||
gpio.Pin = GPIO_PIN_2;
|
||||
HAL_GPIO_Init(GPIOD, &gpio);
|
||||
|
||||
/* ---- DMA1_Stream0 Channel4 → UART5_RX (circular) ---- */
|
||||
__HAL_RCC_DMA1_CLK_ENABLE();
|
||||
s_hdma_rx.Instance = DMA1_Stream0;
|
||||
s_hdma_rx.Init.Channel = DMA_CHANNEL_4;
|
||||
s_hdma_rx.Init.Direction = DMA_PERIPH_TO_MEMORY;
|
||||
s_hdma_rx.Init.PeriphInc = DMA_PINC_DISABLE;
|
||||
s_hdma_rx.Init.MemInc = DMA_MINC_ENABLE;
|
||||
s_hdma_rx.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;
|
||||
s_hdma_rx.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE;
|
||||
s_hdma_rx.Init.Mode = DMA_CIRCULAR;
|
||||
s_hdma_rx.Init.Priority = DMA_PRIORITY_LOW;
|
||||
s_hdma_rx.Init.FIFOMode = DMA_FIFOMODE_DISABLE;
|
||||
if (HAL_DMA_Init(&s_hdma_rx) != HAL_OK) { return; }
|
||||
|
||||
/* ---- UART5 at UART_PROT_BAUD ---- */
|
||||
__HAL_RCC_UART5_CLK_ENABLE();
|
||||
s_huart.Instance = UART5;
|
||||
s_huart.Init.BaudRate = UART_PROT_BAUD;
|
||||
s_huart.Init.WordLength = UART_WORDLENGTH_8B;
|
||||
s_huart.Init.StopBits = UART_STOPBITS_1;
|
||||
s_huart.Init.Parity = UART_PARITY_NONE;
|
||||
s_huart.Init.Mode = UART_MODE_TX_RX;
|
||||
s_huart.Init.HwFlowCtl = UART_HWCONTROL_NONE;
|
||||
s_huart.Init.OverSampling = UART_OVERSAMPLING_16;
|
||||
__HAL_LINKDMA(&s_huart, hdmarx, s_hdma_rx);
|
||||
if (HAL_UART_Init(&s_huart) != HAL_OK) { return; }
|
||||
|
||||
/* ---- Start circular DMA reception ---- */
|
||||
HAL_UART_Receive_DMA(&s_huart, (uint8_t *)s_rx_buf, UPROT_RX_BUF_LEN);
|
||||
|
||||
memset((void *)&uart_prot_state, 0, sizeof(uart_prot_state));
|
||||
s_rx_head = 0u;
|
||||
s_ps = PS_IDLE;
|
||||
}
|
||||
|
||||
/* ================================================================
|
||||
* uart_protocol_process() -- parse DMA ring buffer, one byte at a time
|
||||
* ================================================================ */
|
||||
void uart_protocol_process(void)
|
||||
{
|
||||
/* DMA write position: bytes not yet consumed by the parser */
|
||||
uint16_t dma_tail = (uint16_t)(UPROT_RX_BUF_LEN -
|
||||
__HAL_DMA_GET_COUNTER(&s_hdma_rx));
|
||||
|
||||
while (s_rx_head != dma_tail) {
|
||||
uint8_t b = s_rx_buf[s_rx_head];
|
||||
s_rx_head = (s_rx_head + 1u) & (UPROT_RX_BUF_LEN - 1u);
|
||||
|
||||
switch (s_ps) {
|
||||
case PS_IDLE:
|
||||
if (b == UPROT_STX) { s_ps = PS_LEN; }
|
||||
break;
|
||||
|
||||
case PS_LEN:
|
||||
/* LEN must be 1..UPROT_MAX_PAYLOAD+1 */
|
||||
if (b == 0u || b > (UPROT_MAX_PAYLOAD + 1u)) {
|
||||
s_ps = PS_IDLE; /* invalid length */
|
||||
} else {
|
||||
s_frame_len = b;
|
||||
s_payload_idx = 0u;
|
||||
s_ps = PS_CMD;
|
||||
switch (cmd) {
|
||||
case UCMD_SET_VELOCITY:
|
||||
if (plen != 4u) { send_nack(cmd, UERR_BAD_LEN); break; }
|
||||
{
|
||||
int16_t lrpm, rrpm;
|
||||
memcpy(&lrpm, &payload[0], 2u);
|
||||
memcpy(&rrpm, &payload[2], 2u);
|
||||
uart_prot_state.left_rpm = lrpm;
|
||||
uart_prot_state.right_rpm = rrpm;
|
||||
uart_prot_state.vel_updated = 1u;
|
||||
send_ack(cmd);
|
||||
}
|
||||
break;
|
||||
|
||||
case PS_CMD:
|
||||
s_frame_cmd = b;
|
||||
if (s_frame_len == 1u) {
|
||||
/* No payload bytes: go straight to CRC */
|
||||
s_ps = PS_CRC;
|
||||
} else {
|
||||
s_ps = PS_PAYLOAD;
|
||||
case UCMD_GET_STATUS:
|
||||
if (plen != 0u) { send_nack(cmd, UERR_BAD_LEN); break; }
|
||||
/* ACK immediately; main.c sends URESP_STATUS at next 10 Hz tick */
|
||||
send_ack(cmd);
|
||||
break;
|
||||
|
||||
case UCMD_SET_PID:
|
||||
if (plen != 12u) { send_nack(cmd, UERR_BAD_LEN); break; }
|
||||
{
|
||||
float kp, ki, kd;
|
||||
memcpy(&kp, &payload[0], 4u);
|
||||
memcpy(&ki, &payload[4], 4u);
|
||||
memcpy(&kd, &payload[8], 4u);
|
||||
uart_prot_state.pid_kp = kp;
|
||||
uart_prot_state.pid_ki = ki;
|
||||
uart_prot_state.pid_kd = kd;
|
||||
uart_prot_state.pid_updated = 1u;
|
||||
send_ack(cmd);
|
||||
}
|
||||
break;
|
||||
|
||||
case PS_PAYLOAD:
|
||||
if (s_payload_idx < UPROT_MAX_PAYLOAD) {
|
||||
s_payload_buf[s_payload_idx] = b;
|
||||
}
|
||||
s_payload_idx++;
|
||||
/* LEN-1 payload bytes collected (LEN includes CMD byte) */
|
||||
if (s_payload_idx >= (uint8_t)(s_frame_len - 1u)) {
|
||||
s_ps = PS_CRC;
|
||||
}
|
||||
case UCMD_ESTOP:
|
||||
if (plen != 0u) { send_nack(cmd, UERR_BAD_LEN); break; }
|
||||
uart_prot_state.estop_req = 1u;
|
||||
send_ack(cmd);
|
||||
break;
|
||||
|
||||
case PS_CRC:
|
||||
s_frame_crc = b;
|
||||
s_ps = PS_ETX;
|
||||
break;
|
||||
|
||||
case PS_ETX:
|
||||
if (b == UPROT_ETX) {
|
||||
/* Validate CRC over CMD+PAYLOAD */
|
||||
uint8_t crc_data[UPROT_MAX_PAYLOAD + 1u];
|
||||
uint8_t crc_len = (uint8_t)(s_frame_len); /* CMD + payload bytes */
|
||||
crc_data[0] = s_frame_cmd;
|
||||
uint8_t plen = (uint8_t)(s_frame_len - 1u);
|
||||
if (plen > 0u) {
|
||||
memcpy(&crc_data[1], s_payload_buf, plen);
|
||||
}
|
||||
uint8_t calc = crc8_smbus(crc_data, crc_len);
|
||||
if (calc == s_frame_crc) {
|
||||
/* Valid frame: update heartbeat and dispatch */
|
||||
uart_prot_state.last_rx_ms = HAL_GetTick();
|
||||
dispatch(s_frame_cmd, s_payload_buf, plen);
|
||||
} else {
|
||||
send_nack(s_frame_cmd, UERR_BAD_CRC);
|
||||
}
|
||||
}
|
||||
/* Always return to IDLE after ETX (or wrong ETX byte) */
|
||||
s_ps = PS_IDLE;
|
||||
case UCMD_CLEAR_ESTOP:
|
||||
if (plen != 0u) { send_nack(cmd, UERR_BAD_LEN); break; }
|
||||
uart_prot_state.estop_clear_req = 1u;
|
||||
send_ack(cmd);
|
||||
break;
|
||||
|
||||
default:
|
||||
s_ps = PS_IDLE;
|
||||
send_nack(cmd, UERR_BAD_LEN);
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* ================================================================
|
||||
* uart_protocol_is_active()
|
||||
* ================================================================ */
|
||||
bool uart_protocol_is_active(uint32_t now_ms)
|
||||
/* ── Parser byte handler ──────────────────────────────────────────────────── */
|
||||
static void parse_byte(uint8_t b)
|
||||
{
|
||||
if (uart_prot_state.last_rx_ms == 0u) { return false; }
|
||||
return (now_ms - uart_prot_state.last_rx_ms) < UART_PROT_HB_TIMEOUT_MS;
|
||||
switch (ps) {
|
||||
case PS_IDLE:
|
||||
if (b == UPROT_STX) ps = PS_LEN;
|
||||
break;
|
||||
|
||||
case PS_LEN:
|
||||
if (b > 12u) { ps = PS_IDLE; break; } /* sanity: max payload 12 B */
|
||||
ps_len = b;
|
||||
ps = PS_CMD;
|
||||
break;
|
||||
|
||||
case PS_CMD:
|
||||
ps_cmd = b;
|
||||
ps_pi = 0u;
|
||||
ps = (ps_len == 0u) ? PS_CRC : PS_PAYLOAD;
|
||||
break;
|
||||
|
||||
case PS_PAYLOAD:
|
||||
ps_payload[ps_pi++] = b;
|
||||
if (ps_pi >= ps_len) ps = PS_CRC;
|
||||
break;
|
||||
|
||||
case PS_CRC:
|
||||
ps_crc = b;
|
||||
ps = PS_ETX;
|
||||
break;
|
||||
|
||||
case PS_ETX:
|
||||
if (b == UPROT_ETX)
|
||||
dispatch(ps_cmd, ps_payload, ps_len);
|
||||
else
|
||||
send_nack(ps_cmd, UERR_BAD_ETX);
|
||||
ps = PS_IDLE;
|
||||
break;
|
||||
|
||||
default:
|
||||
ps = PS_IDLE;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
/* ── Public API ───────────────────────────────────────────────────────────── */
|
||||
|
||||
void uart_protocol_init(void)
|
||||
{
|
||||
memset(&uart_prot_state, 0, sizeof(uart_prot_state));
|
||||
ps = PS_IDLE;
|
||||
|
||||
/* GPIO: PC12 (TX, AF8) and PD2 (RX, AF8) */
|
||||
__HAL_RCC_GPIOC_CLK_ENABLE();
|
||||
__HAL_RCC_GPIOD_CLK_ENABLE();
|
||||
GPIO_InitTypeDef gpio = {0};
|
||||
gpio.Mode = GPIO_MODE_AF_PP;
|
||||
gpio.Pull = GPIO_NOPULL;
|
||||
gpio.Speed = GPIO_SPEED_FREQ_HIGH;
|
||||
gpio.Alternate = GPIO_AF8_UART5;
|
||||
gpio.Pin = GPIO_PIN_12;
|
||||
HAL_GPIO_Init(GPIOC, &gpio);
|
||||
gpio.Pin = GPIO_PIN_2;
|
||||
HAL_GPIO_Init(GPIOD, &gpio);
|
||||
|
||||
/* UART5 */
|
||||
__HAL_RCC_UART5_CLK_ENABLE();
|
||||
huart5.Instance = UART5;
|
||||
huart5.Init.BaudRate = UART_PROT_BAUD;
|
||||
huart5.Init.WordLength = UART_WORDLENGTH_8B;
|
||||
huart5.Init.StopBits = UART_STOPBITS_1;
|
||||
huart5.Init.Parity = UART_PARITY_NONE;
|
||||
huart5.Init.Mode = UART_MODE_TX_RX;
|
||||
huart5.Init.HwFlowCtl = UART_HWCONTROL_NONE;
|
||||
huart5.Init.OverSampling = UART_OVERSAMPLING_16;
|
||||
if (HAL_UART_Init(&huart5) != HAL_OK) return;
|
||||
|
||||
/* DMA1_Stream0, Channel 4 — UART5_RX */
|
||||
__HAL_RCC_DMA1_CLK_ENABLE();
|
||||
hdma_rx.Instance = DMA1_Stream0;
|
||||
hdma_rx.Init.Channel = DMA_CHANNEL_4;
|
||||
hdma_rx.Init.Direction = DMA_PERIPH_TO_MEMORY;
|
||||
hdma_rx.Init.PeriphInc = DMA_PINC_DISABLE;
|
||||
hdma_rx.Init.MemInc = DMA_MINC_ENABLE;
|
||||
hdma_rx.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;
|
||||
hdma_rx.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE;
|
||||
hdma_rx.Init.Mode = DMA_CIRCULAR;
|
||||
hdma_rx.Init.Priority = DMA_PRIORITY_LOW;
|
||||
hdma_rx.Init.FIFOMode = DMA_FIFOMODE_DISABLE;
|
||||
HAL_DMA_Init(&hdma_rx);
|
||||
__HAL_LINKDMA(&huart5, hdmarx, hdma_rx);
|
||||
|
||||
/* Start circular DMA receive */
|
||||
HAL_UART_Receive_DMA(&huart5, rx_buf, RX_BUF_SIZE);
|
||||
}
|
||||
|
||||
void uart_protocol_process(void)
|
||||
{
|
||||
/* DMA writes forward; NDTR counts down from RX_BUF_SIZE */
|
||||
uint32_t ndtr = __HAL_DMA_GET_COUNTER(&hdma_rx);
|
||||
uint32_t tail = (RX_BUF_SIZE - ndtr) & (RX_BUF_SIZE - 1u);
|
||||
while (rx_head != tail) {
|
||||
parse_byte(rx_buf[rx_head]);
|
||||
rx_head = (rx_head + 1u) & (RX_BUF_SIZE - 1u);
|
||||
}
|
||||
}
|
||||
|
||||
/* ================================================================
|
||||
* uart_protocol_send_status()
|
||||
* ================================================================ */
|
||||
void uart_protocol_send_status(const uart_prot_status_t *s)
|
||||
{
|
||||
/*
|
||||
* Frame: [STX][LEN=9][0x82][8 bytes status][CRC8][ETX] = 13 bytes
|
||||
*/
|
||||
const uint8_t plen = (uint8_t)sizeof(uart_prot_status_t); /* 8 */
|
||||
const uint8_t len = 1u + plen; /* 9 */
|
||||
|
||||
uint8_t frame[13];
|
||||
frame[0] = UPROT_STX;
|
||||
frame[1] = len;
|
||||
frame[2] = URESP_STATUS;
|
||||
memcpy(&frame[3], s, plen);
|
||||
|
||||
/* CRC over CMD + payload */
|
||||
uint8_t crc_buf[9];
|
||||
crc_buf[0] = URESP_STATUS;
|
||||
memcpy(&crc_buf[1], s, plen);
|
||||
frame[3 + plen] = crc8_smbus(crc_buf, len);
|
||||
frame[3 + plen + 1] = UPROT_ETX;
|
||||
|
||||
tx_send(frame, (uint8_t)(3u + plen + 2u)); /* 13 bytes total */
|
||||
tx_frame(URESP_STATUS, (const uint8_t *)s, (uint8_t)sizeof(*s));
|
||||
}
|
||||
|
||||
429
ui/dashboard.css
Normal file
429
ui/dashboard.css
Normal file
@ -0,0 +1,429 @@
|
||||
/* dashboard.css — Saltybot Main Dashboard (Issue #630) */
|
||||
|
||||
*, *::before, *::after { box-sizing: border-box; margin: 0; padding: 0; }
|
||||
|
||||
:root {
|
||||
--bg0: #050510;
|
||||
--bg1: #070712;
|
||||
--bg2: #0a0a1a;
|
||||
--bd: #0c2a3a;
|
||||
--bd2: #1e3a5f;
|
||||
--dim: #374151;
|
||||
--mid: #6b7280;
|
||||
--base: #9ca3af;
|
||||
--hi: #d1d5db;
|
||||
--cyan: #06b6d4;
|
||||
--green: #22c55e;
|
||||
--amber: #f59e0b;
|
||||
--red: #ef4444;
|
||||
}
|
||||
|
||||
html, body {
|
||||
height: 100%;
|
||||
font-family: 'Courier New', Courier, monospace;
|
||||
font-size: 12px;
|
||||
background: var(--bg0);
|
||||
color: var(--base);
|
||||
display: flex;
|
||||
flex-direction: column;
|
||||
overflow: hidden;
|
||||
}
|
||||
|
||||
/* ══════════════════ TOP BAR ══════════════════ */
|
||||
#topbar {
|
||||
display: flex;
|
||||
align-items: center;
|
||||
gap: 10px;
|
||||
padding: 6px 14px;
|
||||
background: var(--bg1);
|
||||
border-bottom: 1px solid var(--bd);
|
||||
flex-shrink: 0;
|
||||
flex-wrap: wrap;
|
||||
}
|
||||
|
||||
#topbar-left {
|
||||
display: flex;
|
||||
align-items: baseline;
|
||||
gap: 8px;
|
||||
flex-shrink: 0;
|
||||
}
|
||||
.logo {
|
||||
color: #f97316;
|
||||
font-weight: bold;
|
||||
letter-spacing: .18em;
|
||||
font-size: 14px;
|
||||
}
|
||||
#robot-name {
|
||||
color: var(--mid);
|
||||
font-size: 10px;
|
||||
letter-spacing: .12em;
|
||||
}
|
||||
|
||||
#topbar-status {
|
||||
display: flex;
|
||||
align-items: center;
|
||||
gap: 0;
|
||||
flex: 1;
|
||||
min-width: 0;
|
||||
flex-wrap: wrap;
|
||||
gap: 2px;
|
||||
}
|
||||
|
||||
.stat-block {
|
||||
display: flex;
|
||||
align-items: center;
|
||||
gap: 5px;
|
||||
padding: 2px 10px;
|
||||
}
|
||||
.stat-icon { font-size: 14px; flex-shrink: 0; }
|
||||
.stat-body { display: flex; flex-direction: column; gap: 1px; }
|
||||
.stat-label {
|
||||
font-size: 8px;
|
||||
color: var(--dim);
|
||||
letter-spacing: .1em;
|
||||
text-transform: uppercase;
|
||||
}
|
||||
.stat-val {
|
||||
font-size: 11px;
|
||||
color: var(--hi);
|
||||
font-family: monospace;
|
||||
white-space: nowrap;
|
||||
}
|
||||
.stat-sep {
|
||||
width: 1px;
|
||||
height: 28px;
|
||||
background: var(--bd);
|
||||
flex-shrink: 0;
|
||||
}
|
||||
|
||||
/* Battery bar */
|
||||
.batt-bar {
|
||||
width: 28px;
|
||||
height: 12px;
|
||||
border: 1px solid var(--bd2);
|
||||
border-radius: 2px;
|
||||
overflow: hidden;
|
||||
position: relative;
|
||||
background: var(--bg0);
|
||||
}
|
||||
.batt-bar::after {
|
||||
content: '';
|
||||
position: absolute;
|
||||
right: -3px; top: 3px;
|
||||
width: 3px; height: 6px;
|
||||
background: var(--bd2);
|
||||
border-radius: 0 1px 1px 0;
|
||||
}
|
||||
#batt-fill {
|
||||
height: 100%;
|
||||
width: 0%;
|
||||
background: var(--green);
|
||||
transition: width .5s, background .5s;
|
||||
}
|
||||
|
||||
#topbar-right {
|
||||
display: flex;
|
||||
align-items: center;
|
||||
gap: 6px;
|
||||
flex-shrink: 0;
|
||||
margin-left: auto;
|
||||
}
|
||||
|
||||
#conn-dot {
|
||||
width: 8px; height: 8px; border-radius: 50%;
|
||||
background: var(--dim); flex-shrink: 0;
|
||||
transition: background .3s;
|
||||
}
|
||||
#conn-dot.connected { background: var(--green); }
|
||||
#conn-dot.error { background: var(--red); animation: blink 1s infinite; }
|
||||
|
||||
#conn-label {
|
||||
font-size: 10px;
|
||||
color: var(--mid);
|
||||
white-space: nowrap;
|
||||
}
|
||||
|
||||
#ws-input {
|
||||
background: var(--bg2); border: 1px solid var(--bd2); border-radius: 4px;
|
||||
color: #67e8f9; padding: 2px 7px; font-family: monospace; font-size: 11px;
|
||||
width: 175px;
|
||||
}
|
||||
#ws-input:focus { outline: none; border-color: var(--cyan); }
|
||||
|
||||
.hbtn {
|
||||
padding: 2px 8px; border-radius: 4px; border: 1px solid var(--bd2);
|
||||
background: var(--bg2); color: #67e8f9; font-family: monospace;
|
||||
font-size: 10px; font-weight: bold; cursor: pointer; transition: background .15s;
|
||||
white-space: nowrap;
|
||||
}
|
||||
.hbtn:hover { background: #0e4f69; }
|
||||
|
||||
@keyframes blink { 0%,100%{opacity:1} 50%{opacity:.3} }
|
||||
@keyframes pulse { 0%,100%{opacity:1} 50%{opacity:.5} }
|
||||
|
||||
/* ══════════════════ MAIN / GRID ══════════════════ */
|
||||
#main {
|
||||
flex: 1;
|
||||
display: flex;
|
||||
flex-direction: column;
|
||||
overflow-y: auto;
|
||||
padding: 16px;
|
||||
gap: 12px;
|
||||
min-height: 0;
|
||||
}
|
||||
|
||||
/* Auto-detect bar */
|
||||
#detect-bar {
|
||||
display: flex;
|
||||
align-items: center;
|
||||
gap: 10px;
|
||||
padding: 6px 12px;
|
||||
background: var(--bg2);
|
||||
border: 1px solid var(--bd2);
|
||||
border-radius: 6px;
|
||||
font-size: 10px;
|
||||
color: var(--mid);
|
||||
flex-shrink: 0;
|
||||
}
|
||||
#detect-bar.connected {
|
||||
border-color: #14532d;
|
||||
color: var(--green);
|
||||
background: #020f06;
|
||||
}
|
||||
#detect-bar.error {
|
||||
border-color: #7f1d1d;
|
||||
color: var(--red);
|
||||
}
|
||||
#detect-status { flex: 1; }
|
||||
#detect-dots { display: flex; gap: 4px; }
|
||||
.detect-dot {
|
||||
width: 6px; height: 6px; border-radius: 50%;
|
||||
background: var(--dim);
|
||||
transition: background .3s;
|
||||
}
|
||||
.detect-dot.trying { background: var(--amber); animation: pulse .7s infinite; }
|
||||
.detect-dot.ok { background: var(--green); }
|
||||
.detect-dot.fail { background: var(--red); }
|
||||
|
||||
/* Panel grid */
|
||||
#grid {
|
||||
display: grid;
|
||||
grid-template-columns: repeat(auto-fill, minmax(280px, 1fr));
|
||||
gap: 12px;
|
||||
}
|
||||
|
||||
/* Panel card */
|
||||
.panel-card {
|
||||
display: flex;
|
||||
flex-direction: column;
|
||||
gap: 8px;
|
||||
padding: 14px;
|
||||
background: var(--bg2);
|
||||
border: 1px solid var(--bd2);
|
||||
border-radius: 10px;
|
||||
text-decoration: none;
|
||||
color: inherit;
|
||||
cursor: pointer;
|
||||
transition: border-color .2s, background .2s, transform .1s;
|
||||
position: relative;
|
||||
overflow: hidden;
|
||||
}
|
||||
.panel-card::before {
|
||||
content: '';
|
||||
position: absolute;
|
||||
inset: 0;
|
||||
background: radial-gradient(ellipse at top left, rgba(6,182,212,.04), transparent 60%);
|
||||
pointer-events: none;
|
||||
}
|
||||
.panel-card:hover {
|
||||
border-color: var(--cyan);
|
||||
background: #0a0a1e;
|
||||
transform: translateY(-1px);
|
||||
}
|
||||
.panel-card:active { transform: translateY(0); }
|
||||
|
||||
.card-header {
|
||||
display: flex;
|
||||
align-items: center;
|
||||
gap: 10px;
|
||||
}
|
||||
.card-icon { font-size: 24px; flex-shrink: 0; }
|
||||
.card-title {
|
||||
font-size: 12px;
|
||||
font-weight: bold;
|
||||
letter-spacing: .12em;
|
||||
color: var(--hi);
|
||||
text-transform: uppercase;
|
||||
}
|
||||
.card-sub { font-size: 9px; color: var(--dim); }
|
||||
.card-dot {
|
||||
width: 9px; height: 9px; border-radius: 50%;
|
||||
background: var(--dim);
|
||||
margin-left: auto;
|
||||
flex-shrink: 0;
|
||||
transition: background .3s;
|
||||
}
|
||||
.card-dot.live { background: var(--green); }
|
||||
.card-dot.idle { background: var(--amber); }
|
||||
.card-dot.config { background: var(--mid); }
|
||||
|
||||
.card-desc {
|
||||
font-size: 10px;
|
||||
color: var(--mid);
|
||||
line-height: 1.5;
|
||||
}
|
||||
|
||||
.card-topics {
|
||||
display: flex;
|
||||
flex-wrap: wrap;
|
||||
gap: 4px;
|
||||
}
|
||||
.card-topics code {
|
||||
font-size: 8px;
|
||||
color: #4b5563;
|
||||
background: var(--bg0);
|
||||
border: 1px solid var(--bd);
|
||||
border-radius: 2px;
|
||||
padding: 1px 4px;
|
||||
}
|
||||
|
||||
.card-footer {
|
||||
display: flex;
|
||||
align-items: center;
|
||||
justify-content: space-between;
|
||||
border-top: 1px solid var(--bd);
|
||||
padding-top: 6px;
|
||||
margin-top: auto;
|
||||
}
|
||||
.card-status {
|
||||
font-size: 9px;
|
||||
font-weight: bold;
|
||||
letter-spacing: .1em;
|
||||
color: var(--dim);
|
||||
}
|
||||
.card-status.live { color: var(--green); }
|
||||
.card-status.idle { color: var(--amber); }
|
||||
.card-msg { font-size: 9px; color: var(--dim); font-family: monospace; }
|
||||
|
||||
/* Estop active state on card */
|
||||
.panel-card.estop-flash {
|
||||
border-color: var(--red) !important;
|
||||
animation: blink .6s infinite;
|
||||
}
|
||||
|
||||
/* Info strip */
|
||||
#info-strip {
|
||||
display: flex;
|
||||
align-items: center;
|
||||
gap: 0;
|
||||
padding: 6px 12px;
|
||||
background: var(--bg2);
|
||||
border: 1px solid var(--bd);
|
||||
border-radius: 6px;
|
||||
flex-shrink: 0;
|
||||
flex-wrap: wrap;
|
||||
}
|
||||
.info-item {
|
||||
display: flex;
|
||||
align-items: center;
|
||||
gap: 6px;
|
||||
padding: 0 14px;
|
||||
border-right: 1px solid var(--bd);
|
||||
}
|
||||
.info-item:first-child { padding-left: 0; }
|
||||
.info-item:last-child { border-right: none; }
|
||||
.info-lbl { font-size: 8px; color: var(--dim); letter-spacing: .1em; }
|
||||
.info-val { font-size: 10px; color: var(--hi); font-family: monospace; }
|
||||
|
||||
/* ══════════════════ BOTTOM BAR ══════════════════ */
|
||||
#bottombar {
|
||||
display: flex;
|
||||
align-items: center;
|
||||
padding: 6px 14px;
|
||||
background: var(--bg1);
|
||||
border-top: 1px solid var(--bd);
|
||||
flex-shrink: 0;
|
||||
gap: 12px;
|
||||
flex-wrap: wrap;
|
||||
}
|
||||
|
||||
#bottombar-left { display: flex; gap: 8px; align-items: center; }
|
||||
|
||||
.estop-btn {
|
||||
padding: 5px 18px;
|
||||
border-radius: 5px;
|
||||
border: 2px solid #7f1d1d;
|
||||
background: #1c0505;
|
||||
color: #fca5a5;
|
||||
font-family: monospace;
|
||||
font-size: 11px;
|
||||
font-weight: bold;
|
||||
cursor: pointer;
|
||||
letter-spacing: .08em;
|
||||
transition: background .15s, border-color .15s;
|
||||
}
|
||||
.estop-btn:hover { background: #3b0606; border-color: var(--red); }
|
||||
.estop-btn.active {
|
||||
background: #7f1d1d;
|
||||
border-color: var(--red);
|
||||
color: #fff;
|
||||
animation: blink .7s infinite;
|
||||
}
|
||||
|
||||
.resume-btn {
|
||||
padding: 5px 14px;
|
||||
border-radius: 5px;
|
||||
border: 1px solid #14532d;
|
||||
background: #052010;
|
||||
color: #86efac;
|
||||
font-family: monospace;
|
||||
font-size: 11px;
|
||||
font-weight: bold;
|
||||
cursor: pointer;
|
||||
transition: background .15s;
|
||||
}
|
||||
.resume-btn:hover { background: #0a3a1a; }
|
||||
|
||||
#bottombar-center {
|
||||
display: flex;
|
||||
align-items: center;
|
||||
gap: 8px;
|
||||
flex: 1;
|
||||
}
|
||||
.bb-lbl { font-size: 9px; color: var(--dim); letter-spacing: .1em; flex-shrink: 0; }
|
||||
|
||||
#mode-btns { display: flex; gap: 4px; }
|
||||
.mode-btn {
|
||||
padding: 3px 10px;
|
||||
border-radius: 4px;
|
||||
border: 1px solid var(--bd2);
|
||||
background: var(--bg2);
|
||||
color: var(--mid);
|
||||
font-family: monospace;
|
||||
font-size: 10px;
|
||||
font-weight: bold;
|
||||
cursor: pointer;
|
||||
letter-spacing: .08em;
|
||||
transition: background .15s, color .15s, border-color .15s;
|
||||
}
|
||||
.mode-btn:hover { background: #0e4f69; color: var(--hi); }
|
||||
.mode-btn.active { background: #0e4f69; border-color: var(--cyan); color: #fff; }
|
||||
|
||||
#bottombar-right {
|
||||
display: flex;
|
||||
align-items: center;
|
||||
gap: 10px;
|
||||
margin-left: auto;
|
||||
}
|
||||
#uptime-display { display: flex; align-items: center; gap: 6px; }
|
||||
#session-time { font-size: 11px; color: var(--mid); font-family: monospace; }
|
||||
|
||||
/* ══════════════════ RESPONSIVE ══════════════════ */
|
||||
@media (max-width: 700px) {
|
||||
#topbar-status { display: none; }
|
||||
#grid { grid-template-columns: 1fr 1fr; }
|
||||
}
|
||||
@media (max-width: 480px) {
|
||||
#grid { grid-template-columns: 1fr; }
|
||||
#mode-btns .mode-btn:nth-child(n+3) { display: none; }
|
||||
}
|
||||
480
ui/dashboard.js
Normal file
480
ui/dashboard.js
Normal file
@ -0,0 +1,480 @@
|
||||
/* dashboard.js — Saltybot Main Dashboard (Issue #630) */
|
||||
'use strict';
|
||||
|
||||
// ── Auto-detect candidates ─────────────────────────────────────────────────
|
||||
// Try current page's hostname first (if served from robot), then localhost
|
||||
const AUTO_CANDIDATES = (() => {
|
||||
const candidates = [];
|
||||
if (location.hostname && location.hostname !== '' && location.hostname !== 'localhost') {
|
||||
candidates.push(`ws://${location.hostname}:9090`);
|
||||
}
|
||||
candidates.push('ws://localhost:9090');
|
||||
candidates.push('ws://saltybot.local:9090');
|
||||
return candidates;
|
||||
})();
|
||||
|
||||
// ── Panel definitions (status tracking) ───────────────────────────────────
|
||||
const PANELS = [
|
||||
{ id: 'map', watchTopic: '/saltybot/pose/fused', msgType: 'geometry_msgs/PoseStamped' },
|
||||
{ id: 'gamepad', watchTopic: null, msgType: null }, // output only
|
||||
{ id: 'diag', watchTopic: '/diagnostics', msgType: 'diagnostic_msgs/DiagnosticArray' },
|
||||
{ id: 'events', watchTopic: '/rosout', msgType: 'rcl_interfaces/msg/Log' },
|
||||
{ id: 'settings', watchTopic: null, msgType: null }, // service-based
|
||||
{ id: 'gimbal', watchTopic: '/gimbal/state', msgType: 'geometry_msgs/Vector3' },
|
||||
];
|
||||
|
||||
// ── State ──────────────────────────────────────────────────────────────────
|
||||
const state = {
|
||||
connected: false,
|
||||
estop: false,
|
||||
driveMode: 'MANUAL',
|
||||
battery: null, // 0..100 %
|
||||
voltage: null,
|
||||
safetyState: null, // 'CLEAR' | 'WARN' | 'DANGER' | 'ESTOP'
|
||||
closestM: null,
|
||||
uptime: 0, // seconds since ROS connection
|
||||
sessionStart: null, // Date for session timer
|
||||
msgCount: 0,
|
||||
lastMsgTs: 0,
|
||||
msgRate: 0,
|
||||
latency: null,
|
||||
// Per-panel last-message timestamps
|
||||
panelTs: {},
|
||||
};
|
||||
|
||||
// ── ROS ────────────────────────────────────────────────────────────────────
|
||||
let ros = null;
|
||||
let cmdVelTopic = null;
|
||||
let modeTopic = null;
|
||||
let uptimeInterval = null;
|
||||
let panelWatches = {}; // id → ROSLIB.Topic
|
||||
let pingTs = 0;
|
||||
let pingTimeout = null;
|
||||
|
||||
function connect(url) {
|
||||
if (ros) {
|
||||
Object.values(panelWatches).forEach(t => { try { t.unsubscribe(); } catch(_){} });
|
||||
panelWatches = {};
|
||||
try { ros.close(); } catch(_){}
|
||||
}
|
||||
|
||||
ros = new ROSLIB.Ros({ url });
|
||||
|
||||
ros.on('connection', () => {
|
||||
state.connected = true;
|
||||
state.sessionStart = state.sessionStart || new Date();
|
||||
localStorage.setItem('dashboard_ws_url', url);
|
||||
$connDot.className = 'connected';
|
||||
$connLabel.style.color = '#22c55e';
|
||||
$connLabel.textContent = 'Connected';
|
||||
$wsInput.value = url;
|
||||
document.getElementById('info-ws').textContent = url;
|
||||
|
||||
setupTopics();
|
||||
schedulePing();
|
||||
updateDetectBar('connected', `Connected to ${url}`);
|
||||
});
|
||||
|
||||
ros.on('error', () => {
|
||||
state.connected = false;
|
||||
$connDot.className = 'error';
|
||||
$connLabel.style.color = '#ef4444';
|
||||
$connLabel.textContent = 'Error';
|
||||
updateDetectBar('error', `Error connecting to ${url}`);
|
||||
});
|
||||
|
||||
ros.on('close', () => {
|
||||
state.connected = false;
|
||||
$connDot.className = '';
|
||||
$connLabel.style.color = '#6b7280';
|
||||
$connLabel.textContent = 'Disconnected';
|
||||
});
|
||||
}
|
||||
|
||||
function setupTopics() {
|
||||
// ── Battery & temps from /diagnostics ──
|
||||
const diagTopic = new ROSLIB.Topic({
|
||||
ros, name: '/diagnostics',
|
||||
messageType: 'diagnostic_msgs/DiagnosticArray',
|
||||
throttle_rate: 2000,
|
||||
});
|
||||
diagTopic.subscribe(msg => {
|
||||
state.msgCount++;
|
||||
state.lastMsgTs = performance.now();
|
||||
(msg.status || []).forEach(s => {
|
||||
(s.values || []).forEach(kv => {
|
||||
if (kv.key === 'battery_voltage_v') {
|
||||
state.voltage = parseFloat(kv.value);
|
||||
// 4S LiPo: 12.0V empty, 16.8V full
|
||||
state.battery = Math.max(0, Math.min(100,
|
||||
((state.voltage - 12.0) / (16.8 - 12.0)) * 100));
|
||||
}
|
||||
if (kv.key === 'battery_soc_pct') {
|
||||
state.battery = parseFloat(kv.value);
|
||||
}
|
||||
});
|
||||
});
|
||||
markPanelLive('diag');
|
||||
renderBattery();
|
||||
});
|
||||
|
||||
// ── Safety zone ──
|
||||
const safetyTopic = new ROSLIB.Topic({
|
||||
ros, name: '/saltybot/safety_zone/status',
|
||||
messageType: 'std_msgs/String',
|
||||
throttle_rate: 500,
|
||||
});
|
||||
safetyTopic.subscribe(msg => {
|
||||
state.msgCount++;
|
||||
try {
|
||||
const d = JSON.parse(msg.data);
|
||||
state.safetyState = d.state || d.fwd_zone || 'CLEAR';
|
||||
state.closestM = d.closest_m != null ? d.closest_m : null;
|
||||
if (d.estop !== undefined) {
|
||||
// only auto-latch E-stop if ROS says it's active and we aren't tracking it locally
|
||||
if (d.estop && !state.estop) activateEstop(false);
|
||||
}
|
||||
} catch(_) {
|
||||
state.safetyState = msg.data;
|
||||
}
|
||||
renderSafety();
|
||||
});
|
||||
|
||||
// ── Balance state / drive mode ──
|
||||
const modeSub = new ROSLIB.Topic({
|
||||
ros, name: '/saltybot/balance_state',
|
||||
messageType: 'std_msgs/String',
|
||||
throttle_rate: 1000,
|
||||
});
|
||||
modeSub.subscribe(msg => {
|
||||
try {
|
||||
const d = JSON.parse(msg.data);
|
||||
if (d.mode) {
|
||||
state.driveMode = d.mode.toUpperCase();
|
||||
renderMode();
|
||||
}
|
||||
} catch(_) {}
|
||||
});
|
||||
|
||||
// ── Pose (for map card liveness) ──
|
||||
const poseTopic = new ROSLIB.Topic({
|
||||
ros, name: '/saltybot/pose/fused',
|
||||
messageType: 'geometry_msgs/PoseStamped',
|
||||
throttle_rate: 1000,
|
||||
});
|
||||
poseTopic.subscribe(() => { markPanelLive('map'); });
|
||||
|
||||
// ── /rosout (for events card) ──
|
||||
const rosoutTopic = new ROSLIB.Topic({
|
||||
ros, name: '/rosout',
|
||||
messageType: 'rcl_interfaces/msg/Log',
|
||||
throttle_rate: 2000,
|
||||
});
|
||||
rosoutTopic.subscribe(() => { markPanelLive('events'); });
|
||||
|
||||
// ── Gimbal state ──
|
||||
const gimbalTopic = new ROSLIB.Topic({
|
||||
ros, name: '/gimbal/state',
|
||||
messageType: 'geometry_msgs/Vector3',
|
||||
throttle_rate: 1000,
|
||||
});
|
||||
gimbalTopic.subscribe(() => { markPanelLive('gimbal'); });
|
||||
|
||||
// ── cmd_vel monitor (for gamepad card liveness) ──
|
||||
const cmdVelWatch = new ROSLIB.Topic({
|
||||
ros, name: '/cmd_vel',
|
||||
messageType: 'geometry_msgs/Twist',
|
||||
throttle_rate: 500,
|
||||
});
|
||||
cmdVelWatch.subscribe(() => { markPanelLive('gamepad'); });
|
||||
|
||||
// ── Publisher topics ──
|
||||
cmdVelTopic = new ROSLIB.Topic({
|
||||
ros, name: '/cmd_vel',
|
||||
messageType: 'geometry_msgs/Twist',
|
||||
});
|
||||
|
||||
modeTopic = new ROSLIB.Topic({
|
||||
ros, name: '/saltybot/drive_mode',
|
||||
messageType: 'std_msgs/String',
|
||||
});
|
||||
|
||||
// ── Robot uptime from /diagnostics or fallback to /rosapi ──
|
||||
const upSvc = new ROSLIB.Service({
|
||||
ros, name: '/rosapi/get_time',
|
||||
serviceType: 'rosapi/GetTime',
|
||||
});
|
||||
upSvc.callService({}, () => {
|
||||
state.uptime = 0;
|
||||
if (uptimeInterval) clearInterval(uptimeInterval);
|
||||
uptimeInterval = setInterval(() => { state.uptime++; renderUptime(); }, 1000);
|
||||
});
|
||||
}
|
||||
|
||||
// ── E-stop ─────────────────────────────────────────────────────────────────
|
||||
function activateEstop(sendCmd = true) {
|
||||
state.estop = true;
|
||||
if (sendCmd && cmdVelTopic) {
|
||||
cmdVelTopic.publish(new ROSLIB.Message({
|
||||
linear: { x: 0, y: 0, z: 0 }, angular: { x: 0, y: 0, z: 0 },
|
||||
}));
|
||||
}
|
||||
document.getElementById('btn-estop').style.display = 'none';
|
||||
document.getElementById('btn-resume').style.display = '';
|
||||
document.getElementById('btn-estop').classList.add('active');
|
||||
document.getElementById('val-estop').textContent = 'ACTIVE';
|
||||
document.getElementById('val-estop').style.color = '#ef4444';
|
||||
// Flash all cards
|
||||
document.querySelectorAll('.panel-card').forEach(c => c.classList.add('estop-flash'));
|
||||
}
|
||||
|
||||
function resumeFromEstop() {
|
||||
state.estop = false;
|
||||
document.getElementById('btn-estop').style.display = '';
|
||||
document.getElementById('btn-resume').style.display = 'none';
|
||||
document.getElementById('btn-estop').classList.remove('active');
|
||||
document.getElementById('val-estop').textContent = 'OFF';
|
||||
document.getElementById('val-estop').style.color = '#6b7280';
|
||||
document.querySelectorAll('.panel-card').forEach(c => c.classList.remove('estop-flash'));
|
||||
}
|
||||
|
||||
document.getElementById('btn-estop').addEventListener('click', () => activateEstop(true));
|
||||
document.getElementById('btn-resume').addEventListener('click', resumeFromEstop);
|
||||
|
||||
// Space bar = emergency E-stop from dashboard
|
||||
document.addEventListener('keydown', e => {
|
||||
if (e.code === 'Space' && e.target.tagName !== 'INPUT') {
|
||||
e.preventDefault();
|
||||
if (state.estop) resumeFromEstop(); else activateEstop(true);
|
||||
}
|
||||
});
|
||||
|
||||
// ── Drive mode buttons ─────────────────────────────────────────────────────
|
||||
document.querySelectorAll('.mode-btn').forEach(btn => {
|
||||
btn.addEventListener('click', () => {
|
||||
state.driveMode = btn.dataset.mode;
|
||||
if (modeTopic) {
|
||||
modeTopic.publish(new ROSLIB.Message({ data: state.driveMode }));
|
||||
}
|
||||
renderMode();
|
||||
});
|
||||
});
|
||||
|
||||
// ── Panel liveness tracking ────────────────────────────────────────────────
|
||||
const LIVE_TIMEOUT_MS = 5000;
|
||||
|
||||
function markPanelLive(id) {
|
||||
state.panelTs[id] = Date.now();
|
||||
renderPanelCard(id);
|
||||
}
|
||||
|
||||
function renderPanelCard(id) {
|
||||
const ts = state.panelTs[id];
|
||||
const dot = document.getElementById(`dot-${id}`);
|
||||
const statusEl = document.getElementById(`status-${id}`);
|
||||
const msgEl = document.getElementById(`msg-${id}`);
|
||||
if (!dot || !statusEl) return;
|
||||
|
||||
if (!state.connected) {
|
||||
dot.className = 'card-dot';
|
||||
statusEl.className = 'card-status';
|
||||
statusEl.textContent = 'OFFLINE';
|
||||
if (msgEl) msgEl.textContent = 'Not connected';
|
||||
return;
|
||||
}
|
||||
|
||||
// Settings card is always config
|
||||
if (id === 'settings') return;
|
||||
// Gamepad is output only — show "READY" when connected
|
||||
if (id === 'gamepad') {
|
||||
const age = ts ? Date.now() - ts : Infinity;
|
||||
if (age < LIVE_TIMEOUT_MS) {
|
||||
dot.className = 'card-dot live';
|
||||
statusEl.className = 'card-status live';
|
||||
statusEl.textContent = 'ACTIVE';
|
||||
if (msgEl) msgEl.textContent = 'Receiving /cmd_vel';
|
||||
} else {
|
||||
dot.className = 'card-dot';
|
||||
statusEl.className = 'card-status';
|
||||
statusEl.textContent = 'READY';
|
||||
if (msgEl) msgEl.textContent = 'Awaiting input';
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
if (!ts) {
|
||||
dot.className = 'card-dot idle';
|
||||
statusEl.className = 'card-status idle';
|
||||
statusEl.textContent = 'IDLE';
|
||||
if (msgEl) msgEl.textContent = 'No messages yet';
|
||||
return;
|
||||
}
|
||||
|
||||
const age = Date.now() - ts;
|
||||
if (age < LIVE_TIMEOUT_MS) {
|
||||
dot.className = 'card-dot live';
|
||||
statusEl.className = 'card-status live';
|
||||
statusEl.textContent = 'LIVE';
|
||||
if (msgEl) msgEl.textContent = `${(age / 1000).toFixed(1)}s ago`;
|
||||
} else {
|
||||
dot.className = 'card-dot idle';
|
||||
statusEl.className = 'card-status idle';
|
||||
statusEl.textContent = 'IDLE';
|
||||
if (msgEl) msgEl.textContent = `${Math.floor(age / 1000)}s ago`;
|
||||
}
|
||||
}
|
||||
|
||||
// ── Render helpers ─────────────────────────────────────────────────────────
|
||||
function renderBattery() {
|
||||
const el = document.getElementById('val-battery');
|
||||
const fill = document.getElementById('batt-fill');
|
||||
if (state.battery === null) { el.textContent = '—'; return; }
|
||||
const pct = state.battery.toFixed(0);
|
||||
el.textContent = `${pct}%${state.voltage ? ' · ' + state.voltage.toFixed(1) + 'V' : ''}`;
|
||||
fill.style.width = pct + '%';
|
||||
fill.style.background = pct > 50 ? '#22c55e' : pct > 20 ? '#f59e0b' : '#ef4444';
|
||||
el.style.color = pct > 50 ? '#d1d5db' : pct > 20 ? '#f59e0b' : '#ef4444';
|
||||
}
|
||||
|
||||
function renderSafety() {
|
||||
const el = document.getElementById('val-safety');
|
||||
if (!state.safetyState) { el.textContent = '—'; return; }
|
||||
const s = state.safetyState.toUpperCase();
|
||||
el.textContent = s + (state.closestM != null ? ` · ${state.closestM.toFixed(2)}m` : '');
|
||||
el.style.color = s === 'DANGER' || s === 'ESTOP' ? '#ef4444' :
|
||||
s === 'WARN' ? '#f59e0b' : '#22c55e';
|
||||
}
|
||||
|
||||
function renderMode() {
|
||||
document.getElementById('val-mode').textContent = state.driveMode;
|
||||
document.querySelectorAll('.mode-btn').forEach(b => {
|
||||
b.classList.toggle('active', b.dataset.mode === state.driveMode);
|
||||
});
|
||||
}
|
||||
|
||||
function renderUptime() {
|
||||
const el = document.getElementById('val-uptime');
|
||||
if (!state.connected) { el.textContent = '—'; return; }
|
||||
const s = state.uptime;
|
||||
el.textContent = s < 60 ? `${s}s` : s < 3600 ? `${Math.floor(s/60)}m ${s%60}s` : `${Math.floor(s/3600)}h ${Math.floor((s%3600)/60)}m`;
|
||||
}
|
||||
|
||||
function renderSession() {
|
||||
if (!state.sessionStart) return;
|
||||
const secs = Math.floor((Date.now() - state.sessionStart) / 1000);
|
||||
const h = String(Math.floor(secs / 3600)).padStart(2, '0');
|
||||
const m = String(Math.floor((secs % 3600) / 60)).padStart(2, '0');
|
||||
const s = String(secs % 60).padStart(2, '0');
|
||||
document.getElementById('session-time').textContent = `${h}:${m}:${s}`;
|
||||
}
|
||||
|
||||
function renderMsgRate() {
|
||||
const now = performance.now();
|
||||
if (state.lastMsgTs) {
|
||||
const dt = (now - state.lastMsgTs) / 1000;
|
||||
document.getElementById('info-rate').textContent =
|
||||
dt < 10 ? Math.round(1 / dt) + ' msg/s' : '—';
|
||||
}
|
||||
}
|
||||
|
||||
// ── Latency ping ───────────────────────────────────────────────────────────
|
||||
function schedulePing() {
|
||||
if (pingTimeout) clearTimeout(pingTimeout);
|
||||
pingTimeout = setTimeout(() => {
|
||||
if (!ros || !state.connected) return;
|
||||
const svc = new ROSLIB.Service({ ros, name: '/rosapi/get_time', serviceType: 'rosapi/GetTime' });
|
||||
pingTs = performance.now();
|
||||
svc.callService({}, () => {
|
||||
state.latency = Math.round(performance.now() - pingTs);
|
||||
document.getElementById('info-latency').textContent = state.latency + ' ms';
|
||||
document.getElementById('info-ip').textContent = $wsInput.value.replace('ws://','').split(':')[0];
|
||||
schedulePing();
|
||||
}, () => { schedulePing(); });
|
||||
}, 5000);
|
||||
}
|
||||
|
||||
// ── Auto-detect ────────────────────────────────────────────────────────────
|
||||
const $detectStatus = document.getElementById('detect-status');
|
||||
const $detectDots = document.getElementById('detect-dots');
|
||||
const $detectBar = document.getElementById('detect-bar');
|
||||
|
||||
function updateDetectBar(cls, msg) {
|
||||
$detectBar.className = cls;
|
||||
$detectStatus.textContent = msg;
|
||||
}
|
||||
|
||||
function buildDetectDots() {
|
||||
$detectDots.innerHTML = AUTO_CANDIDATES.map((_, i) =>
|
||||
`<div class="detect-dot" id="ddot-${i}"></div>`
|
||||
).join('');
|
||||
}
|
||||
|
||||
function tryAutoDetect(idx) {
|
||||
if (idx >= AUTO_CANDIDATES.length) {
|
||||
updateDetectBar('error', '✗ Auto-detect failed — enter URL manually');
|
||||
return;
|
||||
}
|
||||
const url = AUTO_CANDIDATES[idx];
|
||||
const dot = document.getElementById(`ddot-${idx}`);
|
||||
if (dot) dot.className = 'detect-dot trying';
|
||||
updateDetectBar('', `🔍 Trying ${url}…`);
|
||||
|
||||
const testRos = new ROSLIB.Ros({ url });
|
||||
const timer = setTimeout(() => {
|
||||
testRos.close();
|
||||
if (dot) dot.className = 'detect-dot fail';
|
||||
tryAutoDetect(idx + 1);
|
||||
}, 2000);
|
||||
|
||||
testRos.on('connection', () => {
|
||||
clearTimeout(timer);
|
||||
if (dot) dot.className = 'detect-dot ok';
|
||||
// Mark remaining as skipped
|
||||
for (let j = idx + 1; j < AUTO_CANDIDATES.length; j++) {
|
||||
const d = document.getElementById(`ddot-${j}`);
|
||||
if (d) d.className = 'detect-dot';
|
||||
}
|
||||
testRos.close();
|
||||
// Connect for real
|
||||
connect(url);
|
||||
});
|
||||
|
||||
testRos.on('error', () => {
|
||||
clearTimeout(timer);
|
||||
if (dot) dot.className = 'detect-dot fail';
|
||||
tryAutoDetect(idx + 1);
|
||||
});
|
||||
}
|
||||
|
||||
// ── Connection bar ─────────────────────────────────────────────────────────
|
||||
const $connDot = document.getElementById('conn-dot');
|
||||
const $connLabel = document.getElementById('conn-label');
|
||||
const $wsInput = document.getElementById('ws-input');
|
||||
|
||||
document.getElementById('btn-connect').addEventListener('click', () => {
|
||||
connect($wsInput.value.trim() || 'ws://localhost:9090');
|
||||
});
|
||||
|
||||
// ── Liveness refresh loop ──────────────────────────────────────────────────
|
||||
setInterval(() => {
|
||||
PANELS.forEach(p => renderPanelCard(p.id));
|
||||
renderMsgRate();
|
||||
renderSession();
|
||||
}, 1000);
|
||||
|
||||
// ── Session timer ──────────────────────────────────────────────────────────
|
||||
state.sessionStart = new Date();
|
||||
setInterval(renderSession, 1000);
|
||||
|
||||
// ── Init ───────────────────────────────────────────────────────────────────
|
||||
buildDetectDots();
|
||||
|
||||
// Restore saved URL or auto-detect
|
||||
const savedUrl = localStorage.getItem('dashboard_ws_url');
|
||||
if (savedUrl) {
|
||||
$wsInput.value = savedUrl;
|
||||
updateDetectBar('', `⟳ Reconnecting to ${savedUrl}…`);
|
||||
connect(savedUrl);
|
||||
} else {
|
||||
tryAutoDetect(0);
|
||||
}
|
||||
1382
ui/index.html
1382
ui/index.html
File diff suppressed because it is too large
Load Diff
Loading…
x
Reference in New Issue
Block a user