Compare commits
1 Commits
779f9d00e2
...
ebfc8ec468
| Author | SHA1 | Date | |
|---|---|---|---|
| ebfc8ec468 |
@ -1,410 +0,0 @@
|
|||||||
// ============================================================
|
|
||||||
// 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,13 +271,6 @@
|
|||||||
#define LVC_HYSTERESIS_MV 200u // recovery hysteresis to prevent threshold chatter
|
#define LVC_HYSTERESIS_MV 200u // recovery hysteresis to prevent threshold chatter
|
||||||
#define LVC_TLM_HZ 1u // JLINK_TLM_LVC transmit rate (Hz)
|
#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
|
|
||||||
|
|
||||||
// --- Encoder Odometry (Issue #632) ---
|
// --- Encoder Odometry (Issue #632) ---
|
||||||
// Left encoder: TIM2 (32-bit), CH1=PA15 (AF1), CH2=PB3 (AF1)
|
// Left encoder: TIM2 (32-bit), CH1=PA15 (AF1), CH2=PB3 (AF1)
|
||||||
// Right encoder: TIM3 (16-bit), CH1=PC6 (AF2), CH2=PC7 (AF2)
|
// Right encoder: TIM3 (16-bit), CH1=PC6 (AF2), CH2=PC7 (AF2)
|
||||||
|
|||||||
@ -1,96 +0,0 @@
|
|||||||
#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>
|
|
||||||
|
|
||||||
/* ── Frame delimiters ─────────────────────────────────────────────────────── */
|
|
||||||
#define UPROT_STX 0x02u
|
|
||||||
#define UPROT_ETX 0x03u
|
|
||||||
|
|
||||||
/* ── 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 */
|
|
||||||
|
|
||||||
/* ── 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 */
|
|
||||||
|
|
||||||
/* ── 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 */
|
|
||||||
|
|
||||||
/* ── STATUS payload (URESP_STATUS, 8 bytes packed) ───────────────────────── */
|
|
||||||
typedef struct __attribute__((packed)) {
|
|
||||||
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;
|
|
||||||
|
|
||||||
/* ── Shared state (read by main.c) ────────────────────────────────────────── */
|
|
||||||
typedef struct {
|
|
||||||
volatile uint8_t vel_updated; /* 1 when SET_VELOCITY received */
|
|
||||||
volatile int16_t left_rpm;
|
|
||||||
volatile int16_t right_rpm;
|
|
||||||
|
|
||||||
volatile uint8_t pid_updated; /* 1 when SET_PID received */
|
|
||||||
volatile float pid_kp;
|
|
||||||
volatile float pid_ki;
|
|
||||||
volatile float pid_kd;
|
|
||||||
|
|
||||||
volatile uint8_t estop_req; /* 1 on UCMD_ESTOP */
|
|
||||||
volatile uint8_t estop_clear_req; /* 1 on UCMD_CLEAR_ESTOP */
|
|
||||||
|
|
||||||
volatile uint32_t last_rx_ms; /* HAL_GetTick() of last valid frame */
|
|
||||||
} UartProtState;
|
|
||||||
|
|
||||||
extern UartProtState uart_prot_state;
|
|
||||||
|
|
||||||
/* ── API ───────────────────────────────────────────────────────────────────── */
|
|
||||||
|
|
||||||
/**
|
|
||||||
* 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() — drain DMA ring buffer, parse frames, dispatch commands.
|
|
||||||
* Call once per main loop iteration (every ~1 ms).
|
|
||||||
*/
|
|
||||||
void uart_protocol_process(void);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* 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);
|
|
||||||
|
|
||||||
#endif /* UART_PROTOCOL_H */
|
|
||||||
@ -1,34 +0,0 @@
|
|||||||
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
|
|
||||||
@ -1,29 +0,0 @@
|
|||||||
"""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])
|
|
||||||
@ -1,37 +0,0 @@
|
|||||||
<?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>
|
|
||||||
@ -1,518 +0,0 @@
|
|||||||
"""
|
|
||||||
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()
|
|
||||||
@ -1,156 +0,0 @@
|
|||||||
"""
|
|
||||||
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
|
|
||||||
@ -1,5 +0,0 @@
|
|||||||
[develop]
|
|
||||||
script_dir=$base/lib/saltybot_aruco_detect
|
|
||||||
|
|
||||||
[install]
|
|
||||||
install_scripts=$base/lib/saltybot_aruco_detect
|
|
||||||
@ -1,30 +0,0 @@
|
|||||||
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',
|
|
||||||
],
|
|
||||||
},
|
|
||||||
)
|
|
||||||
@ -1,206 +0,0 @@
|
|||||||
"""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
|
|
||||||
@ -1,7 +0,0 @@
|
|||||||
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
|
|
||||||
@ -1,28 +0,0 @@
|
|||||||
"""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,
|
|
||||||
),
|
|
||||||
])
|
|
||||||
@ -1,31 +0,0 @@
|
|||||||
<?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.
@ -1,99 +0,0 @@
|
|||||||
"""
|
|
||||||
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
|
|
||||||
@ -1,409 +0,0 @@
|
|||||||
"""
|
|
||||||
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()
|
|
||||||
@ -1,4 +0,0 @@
|
|||||||
[develop]
|
|
||||||
script_dir=$base/lib/saltybot_uwb_logger
|
|
||||||
[install]
|
|
||||||
install_scripts=$base/lib/saltybot_uwb_logger
|
|
||||||
@ -1,29 +0,0 @@
|
|||||||
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.
@ -1,116 +0,0 @@
|
|||||||
"""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"])
|
|
||||||
@ -1,15 +0,0 @@
|
|||||||
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()
|
|
||||||
@ -1,32 +0,0 @@
|
|||||||
# 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
|
|
||||||
@ -1,21 +0,0 @@
|
|||||||
<?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>
|
|
||||||
@ -1,15 +0,0 @@
|
|||||||
# 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
|
|
||||||
@ -1,398 +0,0 @@
|
|||||||
#!/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()
|
|
||||||
52
src/main.c
52
src/main.c
@ -36,7 +36,6 @@
|
|||||||
#include "servo_bus.h"
|
#include "servo_bus.h"
|
||||||
#include "gimbal.h"
|
#include "gimbal.h"
|
||||||
#include "lvc.h"
|
#include "lvc.h"
|
||||||
#include "uart_protocol.h"
|
|
||||||
#include <math.h>
|
#include <math.h>
|
||||||
#include <string.h>
|
#include <string.h>
|
||||||
#include <stdio.h>
|
#include <stdio.h>
|
||||||
@ -262,9 +261,6 @@ int main(void) {
|
|||||||
/* Init LVC: low voltage cutoff state machine (Issue #613) */
|
/* Init LVC: low voltage cutoff state machine (Issue #613) */
|
||||||
lvc_init();
|
lvc_init();
|
||||||
|
|
||||||
/* Init UART command protocol for Jetson (UART5 PC12/PD2, 115200 baud, Issue #629) */
|
|
||||||
uart_protocol_init();
|
|
||||||
|
|
||||||
/* Probe I2C1 for optional sensors — skip gracefully if not found */
|
/* Probe I2C1 for optional sensors — skip gracefully if not found */
|
||||||
int baro_chip = 0; /* chip_id: 0x58=BMP280, 0x60=BME280, 0=absent */
|
int baro_chip = 0; /* chip_id: 0x58=BMP280, 0x60=BME280, 0=absent */
|
||||||
mag_type_t mag_type = MAG_NONE;
|
mag_type_t mag_type = MAG_NONE;
|
||||||
@ -297,7 +293,6 @@ int main(void) {
|
|||||||
uint32_t can_cmd_tick = 0; /* CAN velocity command TX timer (Issue #597) */
|
uint32_t can_cmd_tick = 0; /* CAN velocity command TX timer (Issue #597) */
|
||||||
uint32_t can_tlm_tick = 0; /* JLINK_TLM_CAN_STATS transmit timer (Issue #597) */
|
uint32_t can_tlm_tick = 0; /* JLINK_TLM_CAN_STATS transmit timer (Issue #597) */
|
||||||
uint32_t lvc_tlm_tick = 0; /* JLINK_TLM_LVC transmit timer (Issue #613) */
|
uint32_t lvc_tlm_tick = 0; /* JLINK_TLM_LVC transmit timer (Issue #613) */
|
||||||
uint32_t uart_status_tick = 0; /* UART protocol STATUS frame timer (Issue #629) */
|
|
||||||
uint8_t pm_pwm_phase = 0; /* Software PWM counter for sleep LED */
|
uint8_t pm_pwm_phase = 0; /* Software PWM counter for sleep LED */
|
||||||
const float dt = 1.0f / PID_LOOP_HZ; /* 1ms at 1kHz */
|
const float dt = 1.0f / PID_LOOP_HZ; /* 1ms at 1kHz */
|
||||||
|
|
||||||
@ -373,9 +368,6 @@ int main(void) {
|
|||||||
/* CAN bus RX: drain FIFO0 and parse feedback frames (Issue #597) */
|
/* CAN bus RX: drain FIFO0 and parse feedback frames (Issue #597) */
|
||||||
can_driver_process();
|
can_driver_process();
|
||||||
|
|
||||||
/* UART command protocol RX: parse Jetson frames (Issue #629) */
|
|
||||||
uart_protocol_process();
|
|
||||||
|
|
||||||
/* Handle JLink one-shot flags from Jetson binary protocol */
|
/* Handle JLink one-shot flags from Jetson binary protocol */
|
||||||
if (jlink_state.estop_req) {
|
if (jlink_state.estop_req) {
|
||||||
jlink_state.estop_req = 0u;
|
jlink_state.estop_req = 0u;
|
||||||
@ -438,37 +430,6 @@ int main(void) {
|
|||||||
(double)bal.kp, (double)bal.ki, (double)bal.kd);
|
(double)bal.kp, (double)bal.ki, (double)bal.kd);
|
||||||
}
|
}
|
||||||
|
|
||||||
/* UART protocol: handle commands from Jetson (Issue #629) */
|
|
||||||
{
|
|
||||||
if (uart_prot_state.vel_updated) {
|
|
||||||
uart_prot_state.vel_updated = 0u;
|
|
||||||
if (bal.state == BALANCE_ARMED && !lvc_is_cutoff()) {
|
|
||||||
can_cmd_t ucmd_l = { uart_prot_state.left_rpm, 0 };
|
|
||||||
can_cmd_t ucmd_r = { uart_prot_state.right_rpm, 0 };
|
|
||||||
can_driver_send_cmd(CAN_NODE_LEFT, &ucmd_l);
|
|
||||||
can_driver_send_cmd(CAN_NODE_RIGHT, &ucmd_r);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
if (uart_prot_state.pid_updated) {
|
|
||||||
uart_prot_state.pid_updated = 0u;
|
|
||||||
bal.kp = uart_prot_state.pid_kp;
|
|
||||||
bal.ki = uart_prot_state.pid_ki;
|
|
||||||
bal.kd = uart_prot_state.pid_kd;
|
|
||||||
}
|
|
||||||
if (uart_prot_state.estop_req) {
|
|
||||||
uart_prot_state.estop_req = 0u;
|
|
||||||
safety_remote_estop(ESTOP_REMOTE);
|
|
||||||
safety_arm_cancel();
|
|
||||||
balance_disarm(&bal);
|
|
||||||
motor_driver_estop(&motors);
|
|
||||||
}
|
|
||||||
if (uart_prot_state.estop_clear_req) {
|
|
||||||
uart_prot_state.estop_clear_req = 0u;
|
|
||||||
if (safety_remote_estop_active() && bal.state == BALANCE_DISARMED)
|
|
||||||
safety_remote_estop_clear();
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
/* FAULT_LOG_GET: send fault log telemetry to Jetson (Issue #565) */
|
/* FAULT_LOG_GET: send fault log telemetry to Jetson (Issue #565) */
|
||||||
if (jlink_state.fault_log_req) {
|
if (jlink_state.fault_log_req) {
|
||||||
jlink_state.fault_log_req = 0u;
|
jlink_state.fault_log_req = 0u;
|
||||||
@ -807,19 +768,6 @@ int main(void) {
|
|||||||
jlink_send_lvc_tlm(<lm);
|
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.balance_state = (uint8_t)bal.state;
|
|
||||||
ups.estop_active = safety_remote_estop_active() ? 1u : 0u;
|
|
||||||
uart_protocol_send_status(&ups);
|
|
||||||
}
|
|
||||||
|
|
||||||
/* USB telemetry at 50Hz (only when streaming enabled and calibration done) */
|
/* USB telemetry at 50Hz (only when streaming enabled and calibration done) */
|
||||||
if (cdc_streaming && mpu6000_is_calibrated() && now - send_tick >= 20) {
|
if (cdc_streaming && mpu6000_is_calibrated() && now - send_tick >= 20) {
|
||||||
send_tick = now;
|
send_tick = now;
|
||||||
|
|||||||
@ -1,270 +0,0 @@
|
|||||||
/*
|
|
||||||
* uart_protocol.c — UART command protocol for Jetson-STM32 communication (Issue #629)
|
|
||||||
*
|
|
||||||
* 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.
|
|
||||||
*
|
|
||||||
* 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"
|
|
||||||
#include "config.h"
|
|
||||||
#include "stm32f7xx_hal.h"
|
|
||||||
#include <string.h>
|
|
||||||
|
|
||||||
/* ── Configuration ─────────────────────────────────────────────────────────── */
|
|
||||||
#define RX_BUF_SIZE 256u /* must be power-of-two for wrap math */
|
|
||||||
#define TX_TIMEOUT 5u /* HAL_UART_Transmit timeout ms */
|
|
||||||
|
|
||||||
/* ── Peripheral handles ───────────────────────────────────────────────────── */
|
|
||||||
static UART_HandleTypeDef huart5;
|
|
||||||
static DMA_HandleTypeDef hdma_rx;
|
|
||||||
|
|
||||||
/* ── DMA ring buffer ──────────────────────────────────────────────────────── */
|
|
||||||
static uint8_t rx_buf[RX_BUF_SIZE];
|
|
||||||
static uint32_t rx_head = 0u; /* next byte to consume */
|
|
||||||
|
|
||||||
/* ── Shared state (read by main.c) ───────────────────────────────────────── */
|
|
||||||
UartProtState uart_prot_state;
|
|
||||||
|
|
||||||
/* ── Parser state machine ─────────────────────────────────────────────────── */
|
|
||||||
typedef enum {
|
|
||||||
PS_IDLE,
|
|
||||||
PS_LEN,
|
|
||||||
PS_CMD,
|
|
||||||
PS_PAYLOAD,
|
|
||||||
PS_CRC,
|
|
||||||
PS_ETX
|
|
||||||
} ParseState;
|
|
||||||
|
|
||||||
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 ───────────────────────────────────────────────────────────── */
|
|
||||||
static uint8_t crc8(const uint8_t *data, uint8_t len)
|
|
||||||
{
|
|
||||||
uint8_t crc = 0x00u;
|
|
||||||
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 helper ────────────────────────────────────────────────────────────── */
|
|
||||||
static void tx_frame(uint8_t cmd, const uint8_t *payload, uint8_t plen)
|
|
||||||
{
|
|
||||||
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)
|
|
||||||
{
|
|
||||||
tx_frame(URESP_ACK, &cmd, 1u);
|
|
||||||
}
|
|
||||||
|
|
||||||
static void send_nack(uint8_t cmd, uint8_t err)
|
|
||||||
{
|
|
||||||
uint8_t p[2] = { cmd, err };
|
|
||||||
tx_frame(URESP_NACK, p, 2u);
|
|
||||||
}
|
|
||||||
|
|
||||||
/* ── Command dispatcher ───────────────────────────────────────────────────── */
|
|
||||||
static void dispatch(uint8_t cmd, const uint8_t *payload, uint8_t plen)
|
|
||||||
{
|
|
||||||
/* 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_prot_state.last_rx_ms = HAL_GetTick();
|
|
||||||
|
|
||||||
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 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 UCMD_ESTOP:
|
|
||||||
if (plen != 0u) { send_nack(cmd, UERR_BAD_LEN); break; }
|
|
||||||
uart_prot_state.estop_req = 1u;
|
|
||||||
send_ack(cmd);
|
|
||||||
break;
|
|
||||||
|
|
||||||
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:
|
|
||||||
send_nack(cmd, UERR_BAD_LEN);
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
/* ── Parser byte handler ──────────────────────────────────────────────────── */
|
|
||||||
static void parse_byte(uint8_t b)
|
|
||||||
{
|
|
||||||
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);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void uart_protocol_send_status(const uart_prot_status_t *s)
|
|
||||||
{
|
|
||||||
tx_frame(URESP_STATUS, (const uint8_t *)s, (uint8_t)sizeof(*s));
|
|
||||||
}
|
|
||||||
429
ui/dashboard.css
429
ui/dashboard.css
@ -1,429 +0,0 @@
|
|||||||
/* 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
480
ui/dashboard.js
@ -1,480 +0,0 @@
|
|||||||
/* 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);
|
|
||||||
}
|
|
||||||
1380
ui/index.html
1380
ui/index.html
File diff suppressed because it is too large
Load Diff
Loading…
x
Reference in New Issue
Block a user