Compare commits
15 Commits
3e60d7552a
...
7785a16bff
| Author | SHA1 | Date | |
|---|---|---|---|
| 7785a16bff | |||
| 68568b2b66 | |||
| 38df5b4ebb | |||
| fea550c851 | |||
| 13b17a11e1 | |||
| 96d13052b4 | |||
| a01fa091d4 | |||
| 62aab7164e | |||
| 7e12dab4ae | |||
| 1e69337ffd | |||
| 82ad626a94 | |||
| 921eaba8b3 | |||
|
65e0009118 | ||
|
|
9b1f3ddaf0 | ||
| 837c42a00d |
386
chassis/jetson_orin_mount.scad
Normal file
386
chassis/jetson_orin_mount.scad
Normal file
@ -0,0 +1,386 @@
|
||||
// ============================================================
|
||||
// Jetson Orin Nano Carrier Board Mount — Issue #612
|
||||
// Agent : sl-mechanical
|
||||
// Date : 2026-03-15
|
||||
// Part catalogue:
|
||||
// 1. tnut_base — 2020 T-slot rail interface plate, M5 T-nut captive pockets
|
||||
// 2. standoff_post — M2.5 captive-nut standoff post (×4), 10 mm airflow gap
|
||||
// 3. side_brace — lateral stiffening brace with port-access cutouts (×2)
|
||||
// 4. duct_shroud — optional top heatsink duct / fan-exhaust channel
|
||||
// 5. cable_clip — snap-on cable management clip for brace edge
|
||||
//
|
||||
// BOM:
|
||||
// 4 × M5×10 BHCS + M5 T-nuts (tnut_base to rail, 2 per rail)
|
||||
// 4 × M2.5×20 SHCS (board to standoff posts)
|
||||
// 4 × M2.5 hex nuts (captured in standoff posts)
|
||||
// 4 × M3×8 SHCS + washers (side_brace to tnut_base)
|
||||
// 2 × M3×16 SHCS (duct_shroud to side_brace tops)
|
||||
//
|
||||
// Jetson Orin Nano carrier board (Seeed reComputer / official dev kit):
|
||||
// Board dims : 100 × 80 mm
|
||||
// Mounting hole pattern : 86 × 58 mm (centre-to-centre), M2.5, Ø3.5 pad
|
||||
// PCB thickness: 1.6 mm
|
||||
// Connector side: -Y (USB-A, USB-C, HDMI, DP, GbE, SD on one long edge)
|
||||
// Fan header & PWM header: +X short edge
|
||||
// M.2 / NVMe: bottom face
|
||||
//
|
||||
// Print settings (PETG):
|
||||
// tnut_base / standoff_post / side_brace / duct_shroud : 5 perimeters, 40 % gyroid, no supports
|
||||
// cable_clip : 3 perimeters, 30 % gyroid, no supports
|
||||
//
|
||||
// Export commands:
|
||||
// openscad -D 'RENDER="tnut_base"' -o tnut_base.stl jetson_orin_mount.scad
|
||||
// openscad -D 'RENDER="standoff_post"' -o standoff_post.stl jetson_orin_mount.scad
|
||||
// openscad -D 'RENDER="side_brace"' -o side_brace.stl jetson_orin_mount.scad
|
||||
// openscad -D 'RENDER="duct_shroud"' -o duct_shroud.stl jetson_orin_mount.scad
|
||||
// openscad -D 'RENDER="cable_clip"' -o cable_clip.stl jetson_orin_mount.scad
|
||||
// openscad -D 'RENDER="assembly"' -o assembly.png jetson_orin_mount.scad
|
||||
// ============================================================
|
||||
|
||||
// ── Render selector ─────────────────────────────────────────
|
||||
RENDER = "assembly"; // tnut_base | standoff_post | side_brace | duct_shroud | cable_clip | assembly
|
||||
|
||||
// ── Global constants ────────────────────────────────────────
|
||||
$fn = 64;
|
||||
EPS = 0.01;
|
||||
|
||||
// 2020 rail
|
||||
RAIL_W = 20.0;
|
||||
SLOT_NECK_H = 3.2;
|
||||
TNUT_W = 9.8;
|
||||
TNUT_H = 5.5;
|
||||
TNUT_L = 12.0;
|
||||
M5_D = 5.2;
|
||||
M5_HEAD_D = 9.5;
|
||||
M5_HEAD_H = 4.0;
|
||||
|
||||
// Jetson Orin Nano carrier board
|
||||
BOARD_L = 100.0; // board X
|
||||
BOARD_W = 80.0; // board Y
|
||||
BOARD_T = 1.6; // PCB thickness
|
||||
MH_SX = 86.0; // mounting hole span X (centre-to-centre)
|
||||
MH_SY = 58.0; // mounting hole span Y
|
||||
M25_D = 2.7; // M2.5 clearance bore
|
||||
M25_NUT_W = 5.0; // M2.5 hex nut across-flats
|
||||
M25_NUT_H = 2.0; // M2.5 hex nut height
|
||||
M25_HEAD_D = 5.0; // M2.5 SHCS head diameter
|
||||
M25_HEAD_H = 2.5;
|
||||
|
||||
// Base plate
|
||||
BASE_L = 120.0; // length along X (covers board + overhang for braces)
|
||||
BASE_W = 50.0; // width along Y (rail mount footprint)
|
||||
BASE_T = 6.0; // plate thickness
|
||||
BOLT_PITCH = 40.0; // M5 rail bolt pitch (per rail, 2 rails at Y=0 & Y=BASE_W)
|
||||
M3_D = 3.2;
|
||||
M3_HEAD_D = 6.0;
|
||||
M3_HEAD_H = 3.0;
|
||||
|
||||
// Standoff posts
|
||||
POST_H = 12.0; // airflow gap + PCB seating (>= 10 mm clearance spec)
|
||||
POST_OD = 8.0; // outer diameter
|
||||
POST_BASE_D = 11.0; // flange diameter
|
||||
POST_BASE_H = 3.0; // flange height
|
||||
NUT_TRAP_H = M25_NUT_H + 0.3;
|
||||
NUT_TRAP_W = M25_NUT_W + 0.4;
|
||||
|
||||
// Side braces
|
||||
BRACE_T = 5.0; // brace thickness (X)
|
||||
BRACE_H = POST_H + POST_BASE_H + BOARD_T + 4.0; // full height
|
||||
BRACE_W = BASE_W; // same width as base
|
||||
|
||||
// Port-access cutouts (connector side -Y)
|
||||
USB_CUT_W = 60.0; // wide cutout for USB-A stack + HDMI + DP
|
||||
USB_CUT_H = 22.0;
|
||||
GBE_CUT_W = 20.0; // GbE jack
|
||||
GBE_CUT_H = 18.0;
|
||||
|
||||
// Duct shroud
|
||||
DUCT_T = 3.0; // wall thickness
|
||||
DUCT_FLANGE = 6.0; // side tab width for M3 attachment
|
||||
FAN_W = 40.0; // standard 40 mm blower clearance cutout
|
||||
FAN_H = 10.0; // duct outlet height
|
||||
|
||||
// Cable clip
|
||||
CLIP_OD = 12.0;
|
||||
CLIP_ID = 7.0;
|
||||
CLIP_GAP = 7.5;
|
||||
CLIP_W = 10.0;
|
||||
SNAP_T = 1.8;
|
||||
|
||||
// ── 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: tnut_base ───────────────────────────────────────
|
||||
module tnut_base() {
|
||||
difference() {
|
||||
union() {
|
||||
chamfer_cube([BASE_L, BASE_W, BASE_T], ch=1.5);
|
||||
|
||||
// Raised mounting bosses for M3 brace attachment (4 corners)
|
||||
for (x = [8, BASE_L-8])
|
||||
for (y = [8, BASE_W-8])
|
||||
translate([x, y, BASE_T])
|
||||
cylinder(d=10, h=2.5);
|
||||
}
|
||||
|
||||
// T-nut pockets and M5 bolts — front rail (y = BASE_W/4)
|
||||
for (x = [BASE_L/2 - BOLT_PITCH/2, BASE_L/2 + BOLT_PITCH/2]) {
|
||||
translate([x, BASE_W/4, -EPS]) {
|
||||
cylinder(d=M5_D, h=BASE_T + 2*EPS);
|
||||
cylinder(d=M5_HEAD_D, h=M5_HEAD_H + EPS);
|
||||
}
|
||||
translate([x - TNUT_L/2, BASE_W/4 - TNUT_W/2, BASE_T - TNUT_H])
|
||||
cube([TNUT_L, TNUT_W, TNUT_H + EPS]);
|
||||
}
|
||||
|
||||
// T-nut pockets and M5 bolts — rear rail (y = 3*BASE_W/4)
|
||||
for (x = [BASE_L/2 - BOLT_PITCH/2, BASE_L/2 + BOLT_PITCH/2]) {
|
||||
translate([x, 3*BASE_W/4, -EPS]) {
|
||||
cylinder(d=M5_D, h=BASE_T + 2*EPS);
|
||||
cylinder(d=M5_HEAD_D, h=M5_HEAD_H + EPS);
|
||||
}
|
||||
translate([x - TNUT_L/2, 3*BASE_W/4 - TNUT_W/2, BASE_T - TNUT_H])
|
||||
cube([TNUT_L, TNUT_W, TNUT_H + EPS]);
|
||||
}
|
||||
|
||||
// M3 boss bolt holes (corner braces)
|
||||
for (x = [8, BASE_L-8])
|
||||
for (y = [8, BASE_W-8])
|
||||
translate([x, y, -EPS])
|
||||
cylinder(d=M3_D, h=BASE_T + 2.5 + 2*EPS);
|
||||
|
||||
// M3 boss counterbores (head from bottom)
|
||||
for (x = [8, BASE_L-8])
|
||||
for (y = [8, BASE_W-8])
|
||||
translate([x, y, -EPS])
|
||||
cylinder(d=M3_HEAD_D, h=M3_HEAD_H + EPS);
|
||||
|
||||
// Standoff post seating holes (board hole pattern, centred on plate)
|
||||
bx0 = BASE_L/2 - MH_SX/2;
|
||||
by0 = BASE_W/2 - MH_SY/2;
|
||||
for (dx = [0, MH_SX])
|
||||
for (dy = [0, MH_SY])
|
||||
translate([bx0+dx, by0+dy, -EPS])
|
||||
cylinder(d=POST_BASE_D + 0.4, h=BASE_T + 2*EPS);
|
||||
|
||||
// Weight relief grid (2 pockets)
|
||||
translate([20, 12, -EPS]) cube([30, BASE_W-24, BASE_T/2]);
|
||||
translate([BASE_L-50, 12, -EPS]) cube([30, BASE_W-24, BASE_T/2]);
|
||||
|
||||
// Cable pass-through slot
|
||||
translate([BASE_L/2 - 8, BASE_W/2 - 3, -EPS])
|
||||
cube([16, 6, BASE_T + 2*EPS]);
|
||||
}
|
||||
}
|
||||
|
||||
// ── Part 2: standoff_post ───────────────────────────────────
|
||||
module standoff_post() {
|
||||
difference() {
|
||||
union() {
|
||||
// Flange
|
||||
cylinder(d=POST_BASE_D, h=POST_BASE_H);
|
||||
// Post body
|
||||
translate([0, 0, POST_BASE_H])
|
||||
cylinder(d=POST_OD, h=POST_H);
|
||||
}
|
||||
|
||||
// M2.5 through bore
|
||||
translate([0, 0, -EPS])
|
||||
cylinder(d=M25_D, h=POST_BASE_H + POST_H + 2*EPS);
|
||||
|
||||
// Captured hex nut trap (from top)
|
||||
translate([0, 0, POST_BASE_H + POST_H - NUT_TRAP_H])
|
||||
hex_pocket(NUT_TRAP_W, NUT_TRAP_H + EPS);
|
||||
|
||||
// Anti-rotation flat on nut pocket
|
||||
translate([-M25_NUT_W/2 - 0.2, -POST_OD/2 - EPS,
|
||||
POST_BASE_H + POST_H - NUT_TRAP_H])
|
||||
cube([M25_NUT_W + 0.4, 2.0, NUT_TRAP_H + EPS]);
|
||||
}
|
||||
}
|
||||
|
||||
// ── Part 3: side_brace ──────────────────────────────────────
|
||||
// Printed as +X face. Mirror for -X side.
|
||||
module side_brace() {
|
||||
difference() {
|
||||
union() {
|
||||
chamfer_cube([BRACE_T, BRACE_W, BRACE_H], ch=1.0);
|
||||
|
||||
// Top lip to retain board edge
|
||||
translate([0, 0, BRACE_H])
|
||||
cube([BRACE_T + 8.0, BRACE_W, 2.5]);
|
||||
}
|
||||
|
||||
// M3 bolt holes at base (attach to tnut_base bosses)
|
||||
for (y = [8, BRACE_W-8])
|
||||
translate([-EPS, y, 4])
|
||||
rotate([0, 90, 0])
|
||||
cylinder(d=M3_D, h=BRACE_T + 2*EPS);
|
||||
|
||||
// M3 counterbore from outer face
|
||||
for (y = [8, BRACE_W-8])
|
||||
translate([-EPS, y, 4])
|
||||
rotate([0, 90, 0])
|
||||
cylinder(d=M3_HEAD_D, h=M3_HEAD_H + EPS);
|
||||
|
||||
// Port-access cutout — USB/HDMI/DP cluster (centred on brace face)
|
||||
translate([-EPS, BRACE_W/2 - USB_CUT_W/2, POST_BASE_H + 2.0])
|
||||
cube([BRACE_T + 2*EPS, USB_CUT_W, USB_CUT_H]);
|
||||
|
||||
// GbE cutout (offset toward +Y)
|
||||
translate([-EPS, BRACE_W/2 + USB_CUT_W/2 - GBE_CUT_W - 2, POST_BASE_H + 2.0])
|
||||
cube([BRACE_T + 2*EPS, GBE_CUT_W, GBE_CUT_H]);
|
||||
|
||||
// M3 duct attachment holes (top edge)
|
||||
for (y = [BRACE_W/4, 3*BRACE_W/4])
|
||||
translate([BRACE_T/2, y, BRACE_H - 2])
|
||||
cylinder(d=M3_D, h=10);
|
||||
|
||||
// Ventilation slots (3 tall slots for airflow)
|
||||
for (i = [0:2])
|
||||
translate([-EPS,
|
||||
(BRACE_W - 3*8 - 2*4) / 2 + i*(8+4),
|
||||
POST_BASE_H + USB_CUT_H + 6])
|
||||
cube([BRACE_T + 2*EPS, 8, BRACE_H - POST_BASE_H - USB_CUT_H - 10]);
|
||||
}
|
||||
}
|
||||
|
||||
// ── Part 4: duct_shroud ─────────────────────────────────────
|
||||
// Top cap that channels fan exhaust away from board; optional print.
|
||||
module duct_shroud() {
|
||||
duct_l = BASE_L - 2*BRACE_T - 1.0; // span between inner brace faces
|
||||
duct_w = BRACE_W;
|
||||
|
||||
difference() {
|
||||
union() {
|
||||
// Top plate
|
||||
cube([duct_l, duct_w, DUCT_T]);
|
||||
|
||||
// Front wall (fan inlet side)
|
||||
translate([0, 0, -FAN_H])
|
||||
cube([DUCT_T, duct_w, FAN_H + DUCT_T]);
|
||||
|
||||
// Rear wall (exhaust side — open centre)
|
||||
translate([duct_l - DUCT_T, 0, -FAN_H])
|
||||
cube([DUCT_T, duct_w, FAN_H + DUCT_T]);
|
||||
|
||||
// Side flanges for M3 attachment
|
||||
translate([-DUCT_FLANGE, 0, -FAN_H])
|
||||
cube([DUCT_FLANGE, duct_w, FAN_H + DUCT_T]);
|
||||
translate([duct_l, 0, -FAN_H])
|
||||
cube([DUCT_FLANGE, duct_w, FAN_H + DUCT_T]);
|
||||
}
|
||||
|
||||
// Fan cutout on top plate (centred)
|
||||
translate([duct_l/2 - FAN_W/2, duct_w/2 - FAN_W/2, -EPS])
|
||||
cube([FAN_W, FAN_W, DUCT_T + 2*EPS]);
|
||||
|
||||
// Fan screw holes (40 mm fan, Ø3.2 at 32 mm BC)
|
||||
for (dx = [-16, 16])
|
||||
for (dy = [-16, 16])
|
||||
translate([duct_l/2 + dx, duct_w/2 + dy, -EPS])
|
||||
cylinder(d=M3_D, h=DUCT_T + 2*EPS);
|
||||
|
||||
// Exhaust slot on rear wall (full width minus corners)
|
||||
translate([duct_l - DUCT_T - EPS, 4, -FAN_H + 2])
|
||||
cube([DUCT_T + 2*EPS, duct_w - 8, FAN_H - 2]);
|
||||
|
||||
// M3 flange attachment holes
|
||||
for (y = [duct_w/4, 3*duct_w/4]) {
|
||||
translate([-DUCT_FLANGE - EPS, y, -FAN_H/2])
|
||||
rotate([0, 90, 0])
|
||||
cylinder(d=M3_D, h=DUCT_FLANGE + 2*EPS);
|
||||
translate([duct_l + DUCT_T - EPS, y, -FAN_H/2])
|
||||
rotate([0, 90, 0])
|
||||
cylinder(d=M3_D, h=DUCT_FLANGE + 2*EPS);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// ── Part 5: cable_clip ──────────────────────────────────────
|
||||
module cable_clip() {
|
||||
difference() {
|
||||
union() {
|
||||
// Snap-wrap body
|
||||
difference() {
|
||||
cylinder(d=CLIP_OD + 2*SNAP_T, h=CLIP_W);
|
||||
translate([0, 0, -EPS])
|
||||
cylinder(d=CLIP_ID, h=CLIP_W + 2*EPS);
|
||||
// Front gap
|
||||
translate([-CLIP_GAP/2, 0, -EPS])
|
||||
cube([CLIP_GAP, CLIP_OD, CLIP_W + 2*EPS]);
|
||||
}
|
||||
|
||||
// Mounting tab for brace edge
|
||||
translate([CLIP_OD/2 + SNAP_T - EPS, -SNAP_T, 0])
|
||||
cube([8, SNAP_T*2, CLIP_W]);
|
||||
}
|
||||
|
||||
// Tab screw hole
|
||||
translate([CLIP_OD/2 + SNAP_T + 4, 0, CLIP_W/2])
|
||||
rotate([90, 0, 0])
|
||||
cylinder(d=M3_D, h=SNAP_T*2 + 2*EPS, center=true);
|
||||
}
|
||||
}
|
||||
|
||||
// ── Assembly ────────────────────────────────────────────────
|
||||
module assembly() {
|
||||
// Base plate
|
||||
color("SteelBlue")
|
||||
tnut_base();
|
||||
|
||||
// Standoff posts (board hole pattern)
|
||||
bx0 = BASE_L/2 - MH_SX/2;
|
||||
by0 = BASE_W/2 - MH_SY/2;
|
||||
for (dx = [0, MH_SX])
|
||||
for (dy = [0, MH_SY])
|
||||
color("DodgerBlue")
|
||||
translate([bx0+dx, by0+dy, BASE_T])
|
||||
standoff_post();
|
||||
|
||||
// Side braces (left and right)
|
||||
color("CornflowerBlue")
|
||||
translate([0, 0, BASE_T])
|
||||
side_brace();
|
||||
color("CornflowerBlue")
|
||||
translate([BASE_L, BRACE_W, BASE_T])
|
||||
mirror([1, 0, 0])
|
||||
mirror([0, 1, 0])
|
||||
side_brace();
|
||||
|
||||
// Board silhouette (translucent, for clearance visualisation)
|
||||
color("ForestGreen", 0.25)
|
||||
translate([BASE_L/2 - BOARD_L/2, BASE_W/2 - BOARD_W/2,
|
||||
BASE_T + POST_BASE_H + POST_H])
|
||||
cube([BOARD_L, BOARD_W, BOARD_T]);
|
||||
|
||||
// Duct shroud (above board)
|
||||
color("LightSteelBlue", 0.7)
|
||||
translate([BRACE_T + 0.5, 0,
|
||||
BASE_T + POST_BASE_H + POST_H + BOARD_T + 2.0])
|
||||
duct_shroud();
|
||||
|
||||
// Cable clips (on brace edge, 2×)
|
||||
for (y = [BRACE_W/3, 2*BRACE_W/3])
|
||||
color("SlateGray")
|
||||
translate([BASE_L + 2, y, BASE_T + BRACE_H/2 - CLIP_W/2])
|
||||
rotate([0, 90, 0])
|
||||
cable_clip();
|
||||
}
|
||||
|
||||
// ── Dispatch ────────────────────────────────────────────────
|
||||
if (RENDER == "tnut_base") tnut_base();
|
||||
else if (RENDER == "standoff_post") standoff_post();
|
||||
else if (RENDER == "side_brace") side_brace();
|
||||
else if (RENDER == "duct_shroud") duct_shroud();
|
||||
else if (RENDER == "cable_clip") cable_clip();
|
||||
else assembly();
|
||||
@ -262,4 +262,13 @@
|
||||
#define CAN_RPM_SCALE 10 // motor_cmd to RPM: 1 cmd count = 10 RPM
|
||||
#define CAN_TLM_HZ 1u // JLINK_TLM_CAN_STATS transmit rate (Hz)
|
||||
|
||||
|
||||
// --- LVC: Low Voltage Cutoff (Issue #613) ---
|
||||
// 3-stage undervoltage protection; voltages in mV
|
||||
#define LVC_WARNING_MV 21000u // 21.0 V -- buzzer alert, full power
|
||||
#define LVC_CRITICAL_MV 19800u // 19.8 V -- 50% motor power reduction
|
||||
#define LVC_CUTOFF_MV 18600u // 18.6 V -- motors disabled, latch until reboot
|
||||
#define LVC_HYSTERESIS_MV 200u // recovery hysteresis to prevent threshold chatter
|
||||
#define LVC_TLM_HZ 1u // JLINK_TLM_LVC transmit rate (Hz)
|
||||
|
||||
#endif // CONFIG_H
|
||||
|
||||
@ -50,6 +50,8 @@
|
||||
* 0x87 FAULT_LOG - jlink_tlm_fault_log_t (20 bytes), sent on boot + FAULT_LOG_GET (Issue #565)
|
||||
* 0x88 SLOPE - jlink_tlm_slope_t (4 bytes), sent at SLOPE_TLM_HZ (Issue #600)
|
||||
* 0x89 CAN_STATS - jlink_tlm_can_stats_t (16 bytes), sent at CAN_TLM_HZ + CAN_STATS_GET (Issue #597)
|
||||
* 0x8A STEERING - jlink_tlm_steering_t (8 bytes), sent at STEER_TLM_HZ (Issue #616)
|
||||
* 0x8B LVC - jlink_tlm_lvc_t (4 bytes), sent at LVC_TLM_HZ (Issue #613)
|
||||
*
|
||||
* Priority: CRSF RC always takes precedence. Jetson steer/speed only applied
|
||||
* when mode_manager_active() == MODE_AUTONOMOUS (CH6 high). In RC_MANUAL and
|
||||
@ -93,6 +95,8 @@
|
||||
#define JLINK_TLM_FAULT_LOG 0x87u /* jlink_tlm_fault_log_t (20 bytes, Issue #565) */
|
||||
#define JLINK_TLM_SLOPE 0x88u /* jlink_tlm_slope_t (4 bytes, Issue #600) */
|
||||
#define JLINK_TLM_CAN_STATS 0x89u /* jlink_tlm_can_stats_t (16 bytes, Issue #597) */
|
||||
#define JLINK_TLM_STEERING 0x8Au /* jlink_tlm_steering_t (8 bytes, Issue #616) */
|
||||
#define JLINK_TLM_LVC 0x8Bu /* jlink_tlm_lvc_t (4 bytes, Issue #613) */
|
||||
|
||||
/* ---- Telemetry STATUS payload (20 bytes, packed) ---- */
|
||||
typedef struct __attribute__((packed)) {
|
||||
@ -204,6 +208,24 @@ typedef struct __attribute__((packed)) {
|
||||
int16_t vel1_rpm; /* node 1 current velocity (RPM) */
|
||||
} jlink_tlm_can_stats_t; /* 16 bytes */
|
||||
|
||||
/* ---- Telemetry STEERING payload (8 bytes, packed) Issue #616 ---- */
|
||||
/* Published at STEER_TLM_HZ (10 Hz); reports yaw-rate PID state. */
|
||||
typedef struct __attribute__((packed)) {
|
||||
int16_t target_x10; /* target yaw rate, deg/s × 10 (0.1 deg/s resolution) */
|
||||
int16_t actual_x10; /* measured yaw rate, deg/s × 10 */
|
||||
int16_t output; /* differential motor output (-STEER_OUTPUT_MAX..+MAX) */
|
||||
uint8_t enabled; /* 1 = PID active */
|
||||
uint8_t _pad; /* reserved */
|
||||
} jlink_tlm_steering_t; /* 8 bytes */
|
||||
|
||||
/* ---- Telemetry LVC payload (4 bytes, packed) Issue #613 ---- */
|
||||
/* Sent at LVC_TLM_HZ (1 Hz); reports battery voltage and LVC protection state. */
|
||||
typedef struct __attribute__((packed)) {
|
||||
uint16_t voltage_mv; /* battery voltage (mV) */
|
||||
uint8_t percent; /* 0-100: fuel gauge within CUTOFF..WARNING; 255=unknown */
|
||||
uint8_t protection_state; /* LvcState: 0=NORMAL,1=WARNING,2=CRITICAL,3=CUTOFF */
|
||||
} jlink_tlm_lvc_t; /* 4 bytes */
|
||||
|
||||
/* ---- Volatile state (read from main loop) ---- */
|
||||
typedef struct {
|
||||
/* Drive command - updated on JLINK_CMD_DRIVE */
|
||||
@ -322,4 +344,17 @@ void jlink_send_slope_tlm(const jlink_tlm_slope_t *tlm);
|
||||
*/
|
||||
void jlink_send_can_stats(const jlink_tlm_can_stats_t *tlm);
|
||||
|
||||
/*
|
||||
* jlink_send_steering_tlm(tlm) - transmit JLINK_TLM_STEERING (0x8A) frame
|
||||
* (14 bytes total) to Jetson. Issue #616.
|
||||
* Rate-limiting is handled by steering_pid_send_tlm(); call from there only.
|
||||
*/
|
||||
void jlink_send_steering_tlm(const jlink_tlm_steering_t *tlm);
|
||||
|
||||
/*
|
||||
* jlink_send_lvc_tlm(tlm) - transmit JLINK_TLM_LVC (0x8B) frame
|
||||
* (10 bytes total) at LVC_TLM_HZ (1 Hz). Issue #613.
|
||||
*/
|
||||
void jlink_send_lvc_tlm(const jlink_tlm_lvc_t *tlm);
|
||||
|
||||
#endif /* JLINK_H */
|
||||
|
||||
39
include/lvc.h
Normal file
39
include/lvc.h
Normal file
@ -0,0 +1,39 @@
|
||||
#ifndef LVC_H
|
||||
#define LVC_H
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
|
||||
/*
|
||||
* lvc.h -- Low Voltage Cutoff (LVC) protection (Issue #613)
|
||||
*
|
||||
* 3-stage battery voltage protection using battery_read_mv():
|
||||
*
|
||||
* LVC_WARNING (21.0 V) -- periodic buzzer alert; full power maintained
|
||||
* LVC_CRITICAL (19.8 V) -- faster buzzer; motor commands scaled to 50%
|
||||
* LVC_CUTOFF (18.6 V) -- error buzzer; motors disabled; latched until reboot
|
||||
*
|
||||
* Recovery uses LVC_HYSTERESIS_MV to prevent threshold chatter.
|
||||
* CUTOFF is one-way: once latched, only a power-cycle clears it.
|
||||
*
|
||||
* Integration:
|
||||
* lvc_init() -- call once during system init
|
||||
* lvc_tick(now_ms, vbat_mv) -- call each main loop tick (1 kHz)
|
||||
* lvc_get_power_scale() -- returns 0/50/100; apply to motor speed
|
||||
* lvc_is_cutoff() -- true when motors must be disabled
|
||||
*/
|
||||
|
||||
typedef enum {
|
||||
LVC_NORMAL = 0, /* Vbat >= WARNING threshold */
|
||||
LVC_WARNING = 1, /* Vbat < 21.0 V -- alert only */
|
||||
LVC_CRITICAL = 2, /* Vbat < 19.8 V -- 50% power */
|
||||
LVC_CUTOFF = 3, /* Vbat < 18.6 V -- motors off */
|
||||
} LvcState;
|
||||
|
||||
void lvc_init(void);
|
||||
void lvc_tick(uint32_t now_ms, uint32_t vbat_mv);
|
||||
LvcState lvc_get_state(void);
|
||||
uint8_t lvc_get_power_scale(void); /* 100 = full, 50 = critical, 0 = cutoff */
|
||||
bool lvc_is_cutoff(void);
|
||||
|
||||
#endif /* LVC_H */
|
||||
@ -9,6 +9,7 @@ typedef struct {
|
||||
float pitch_rate; // degrees/sec (raw gyro pitch axis)
|
||||
float roll; // degrees, filtered (complementary filter)
|
||||
float yaw; // degrees, gyro-integrated (drifts — no magnetometer)
|
||||
float yaw_rate; // degrees/sec (raw gyro Z / board_gz, Issue #616)
|
||||
float accel_x; // g
|
||||
float accel_z; // g
|
||||
} IMUData;
|
||||
|
||||
134
include/steering_pid.h
Normal file
134
include/steering_pid.h
Normal file
@ -0,0 +1,134 @@
|
||||
#ifndef STEERING_PID_H
|
||||
#define STEERING_PID_H
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
|
||||
/*
|
||||
* steering_pid — closed-loop yaw-rate controller for differential drive
|
||||
* (Issue #616).
|
||||
*
|
||||
* OVERVIEW:
|
||||
* Converts a desired yaw rate (from Jetson Twist.angular.z) into a
|
||||
* differential wheel speed offset. The balance PID remains the primary
|
||||
* controller; the steering PID generates a small differential term that
|
||||
* is added to the balance command inside motor_driver:
|
||||
*
|
||||
* left_speed = balance_cmd - steer_out
|
||||
* right_speed = balance_cmd + steer_out
|
||||
*
|
||||
* This is the standard differential-drive mixing already performed by
|
||||
* the ESC backend (hoverboard/VESC).
|
||||
*
|
||||
* INPUT SIGNALS:
|
||||
* target_omega_dps : desired yaw rate in deg/s (+ = clockwise from above)
|
||||
* Derived from JLINK_CMD_DRIVE steer field:
|
||||
* target_omega_dps = steer * STEER_OMEGA_SCALE
|
||||
* (steer is int16 -1000..+1000 from Jetson)
|
||||
* actual_omega_dps : measured yaw rate from IMU gyro Z (deg/s)
|
||||
* = IMUData.yaw_rate
|
||||
*
|
||||
* PID ALGORITHM:
|
||||
* error = target_omega - actual_omega
|
||||
* integral = clamp(integral + error*dt, ±STEER_INTEGRAL_MAX)
|
||||
* raw_out = Kp*error + Ki*integral + Kd*(error - prev_error)/dt
|
||||
* raw_out = clamp(raw_out, ±STEER_OUTPUT_MAX)
|
||||
* output = rate_limit(raw_out, STEER_RAMP_RATE_PER_MS * dt_ms)
|
||||
*
|
||||
* ANTI-WINDUP:
|
||||
* Integral is clamped to ±STEER_INTEGRAL_MAX counts before the Ki
|
||||
* multiply, bounding the integrator contribution independently of Kp/Kd.
|
||||
*
|
||||
* RATE LIMITER:
|
||||
* Output changes at most STEER_RAMP_RATE_PER_MS counts per millisecond.
|
||||
* Prevents a sudden step in steering demand from disturbing the balance
|
||||
* PID (which has no knowledge of the steering channel).
|
||||
*
|
||||
* OMEGA SCALING:
|
||||
* STEER_OMEGA_SCALE = 0.1 deg/s per steer unit.
|
||||
* Range: steer -1000..+1000 → omega -100..+100 deg/s.
|
||||
* 100 deg/s ≈ 1.75 rad/s — covers aggressive turns without exceeding
|
||||
* the hoverboard ESC differential authority (STEER_OUTPUT_MAX = 400).
|
||||
*
|
||||
* DISABLING:
|
||||
* steering_pid_set_enabled(s, false) zeroes target_omega and integral,
|
||||
* then freezes output at 0. Use when Jetson is not active or in
|
||||
* RC_MANUAL mode to avoid fighting the RC steer channel.
|
||||
*
|
||||
* TELEMETRY:
|
||||
* JLINK_TLM_STEERING (0x8A) published at STEER_TLM_HZ (10 Hz):
|
||||
* jlink_tlm_steering_t { int16 target_x10, int16 actual_x10,
|
||||
* int16 output, uint8 enabled, uint8 _pad }
|
||||
* 8 bytes total.
|
||||
*/
|
||||
|
||||
/* ---- Configuration ---- */
|
||||
#define STEER_KP 2.0f /* proportional gain (counts / (deg/s)) */
|
||||
#define STEER_KI 0.5f /* integral gain (counts / (deg)) */
|
||||
#define STEER_KD 0.05f /* derivative gain (counts / (deg/s²)) */
|
||||
#define STEER_INTEGRAL_MAX 200.0f /* integrator clamp (motor counts) */
|
||||
#define STEER_OUTPUT_MAX 400 /* peak differential output (counts) */
|
||||
#define STEER_RAMP_RATE_PER_MS 10 /* max output change per ms (counts/ms) */
|
||||
#define STEER_OMEGA_SCALE 0.1f /* steer units → deg/s (0.1 deg/s/unit) */
|
||||
#define STEER_TLM_HZ 10u /* JLINK_TLM_STEERING publish rate (Hz) */
|
||||
|
||||
/* ---- State ---- */
|
||||
typedef struct {
|
||||
float target_omega_dps; /* setpoint: desired yaw rate (deg/s) */
|
||||
float actual_omega_dps; /* feedback: measured yaw rate (deg/s) */
|
||||
float integral; /* PID integrator (motor counts·s) */
|
||||
float prev_error; /* error at last update (deg/s) */
|
||||
int16_t output; /* rate-limited differential output */
|
||||
bool enabled; /* false = output held at 0 */
|
||||
uint32_t last_tlm_ms; /* rate-limit for TLM */
|
||||
} steering_pid_t;
|
||||
|
||||
/* ---- API ---- */
|
||||
|
||||
/*
|
||||
* steering_pid_init(s) — zero state, enable controller.
|
||||
* Call once during system init.
|
||||
*/
|
||||
void steering_pid_init(steering_pid_t *s);
|
||||
|
||||
/*
|
||||
* steering_pid_reset(s) — zero integrator, setpoint and output.
|
||||
* Preserves enabled flag. Call on disarm.
|
||||
*/
|
||||
void steering_pid_reset(steering_pid_t *s);
|
||||
|
||||
/*
|
||||
* steering_pid_set_target(s, omega_dps) — update setpoint.
|
||||
* omega_dps : desired yaw rate in deg/s.
|
||||
* Converts from JLINK_CMD_DRIVE steer field: omega = steer * STEER_OMEGA_SCALE.
|
||||
* No-op if disabled (output remains 0).
|
||||
*/
|
||||
void steering_pid_set_target(steering_pid_t *s, float omega_dps);
|
||||
|
||||
/*
|
||||
* steering_pid_update(s, actual_omega_dps, dt) — advance PID one step.
|
||||
* actual_omega_dps : IMU gyro Z rate (deg/s) — use IMUData.yaw_rate.
|
||||
* dt : loop interval (seconds).
|
||||
* Returns differential output (-STEER_OUTPUT_MAX..+STEER_OUTPUT_MAX).
|
||||
* Returns 0 if disabled or dt <= 0.
|
||||
*/
|
||||
int16_t steering_pid_update(steering_pid_t *s, float actual_omega_dps, float dt);
|
||||
|
||||
/*
|
||||
* steering_pid_get_output(s) — last computed differential output.
|
||||
*/
|
||||
int16_t steering_pid_get_output(const steering_pid_t *s);
|
||||
|
||||
/*
|
||||
* steering_pid_set_enabled(s, en) — enable or disable the controller.
|
||||
* Disabling resets integrator and zeroes output.
|
||||
*/
|
||||
void steering_pid_set_enabled(steering_pid_t *s, bool en);
|
||||
|
||||
/*
|
||||
* steering_pid_send_tlm(s, now_ms) — transmit JLINK_TLM_STEERING (0x8A)
|
||||
* frame to Jetson. Rate-limited to STEER_TLM_HZ; safe to call every tick.
|
||||
*/
|
||||
void steering_pid_send_tlm(const steering_pid_t *s, uint32_t now_ms);
|
||||
|
||||
#endif /* STEERING_PID_H */
|
||||
@ -1,28 +1,33 @@
|
||||
# bag_recorder.yaml — Bag recording manager config (Issue #615).
|
||||
bag_recorder:
|
||||
ros__parameters:
|
||||
# Path where bags are stored (Issue #488: mission logging)
|
||||
bag_dir: '~/saltybot-data/bags'
|
||||
|
||||
# Topics to record for mission logging (Issue #488)
|
||||
storage_paths:
|
||||
- '/media/usb0'
|
||||
- '/media/usb1'
|
||||
- '/mnt/nvme/saltybot-bags'
|
||||
- '~/saltybot-data/bags'
|
||||
topics:
|
||||
- '/scan'
|
||||
- '/cmd_vel'
|
||||
- '/odom'
|
||||
- '/tf'
|
||||
- '/camera/color/image_raw/compressed'
|
||||
- '/tf_static'
|
||||
- '/saltybot/pose/fused_cov'
|
||||
- '/saltybot/diagnostics'
|
||||
|
||||
# Rotation interval: save new bag every N minutes (Issue #488: 30 min)
|
||||
buffer_duration_minutes: 30
|
||||
|
||||
# Storage management (Issue #488: FIFO 20GB limit)
|
||||
storage_ttl_days: 7 # Remove bags older than 7 days
|
||||
max_storage_gb: 20 # Enforce 20GB FIFO quota
|
||||
|
||||
# Storage format (Issue #488: prefer MCAP)
|
||||
storage_format: 'mcap' # Options: mcap, sqlite3
|
||||
|
||||
# NAS sync (optional)
|
||||
enable_rsync: false
|
||||
rsync_destination: ''
|
||||
# rsync_destination: 'user@nas:/path/to/backups/'
|
||||
- '/saltybot/sensor_health'
|
||||
- '/saltybot/battery'
|
||||
- '/saltybot/motor_daemon/status'
|
||||
- '/camera/color/image_raw/compressed'
|
||||
- '/camera/depth/image_rect_raw/compressed'
|
||||
- '/imu/data'
|
||||
motion_trigger: true
|
||||
idle_timeout_s: 30.0
|
||||
split_size_gb: 1.0
|
||||
split_duration_min: 10.0
|
||||
storage_format: 'mcap'
|
||||
warn_disk_pct: 70.0
|
||||
cleanup_disk_pct: 80.0
|
||||
cleanup_age_days: 7
|
||||
min_free_gb: 2.0
|
||||
status_hz: 1.0
|
||||
check_hz: 0.033
|
||||
|
||||
@ -0,0 +1,293 @@
|
||||
"""bag_policy.py — Pure-Python recording policy logic (Issue #615).
|
||||
|
||||
No ROS2 dependencies — importable and fully unit-testable without a live
|
||||
ROS2 install.
|
||||
|
||||
Classes
|
||||
───────
|
||||
MotionState — tracks /cmd_vel activity and idle timeout
|
||||
DiskInfo — disk-usage snapshot with threshold helpers
|
||||
StorageSelector — picks the best storage path from a priority list
|
||||
BagPolicy — decides when to start/stop/split/clean up
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
import shutil
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
from typing import List, Optional, Tuple
|
||||
|
||||
|
||||
# ── Motion tracking ───────────────────────────────────────────────────────────
|
||||
|
||||
class MotionState:
|
||||
"""Tracks /cmd_vel activity and determines idle timeout.
|
||||
|
||||
Usage::
|
||||
|
||||
ms = MotionState(idle_timeout_s=30.0)
|
||||
ms.update(linear_x=0.5, angular_z=0.0, now=time.monotonic())
|
||||
if ms.should_start_recording(now):
|
||||
...
|
||||
if ms.should_stop_recording(now):
|
||||
...
|
||||
"""
|
||||
|
||||
def __init__(self, idle_timeout_s: float = 30.0) -> None:
|
||||
if idle_timeout_s <= 0:
|
||||
raise ValueError(f"idle_timeout_s must be > 0, got {idle_timeout_s!r}")
|
||||
self._idle_timeout = idle_timeout_s
|
||||
self._last_motion_t: Optional[float] = None
|
||||
self._ever_moved: bool = False
|
||||
|
||||
# ── Mutators ───────────────────────────────────────────────────────────
|
||||
|
||||
def update(self, linear_x: float, angular_z: float, now: float) -> None:
|
||||
"""Feed the latest cmd_vel values. Non-zero → reset idle timer."""
|
||||
if abs(linear_x) > 1e-3 or abs(angular_z) > 1e-3:
|
||||
self._last_motion_t = now
|
||||
self._ever_moved = True
|
||||
|
||||
def reset(self) -> None:
|
||||
"""Forget all motion history (e.g. after a recording session ends)."""
|
||||
self._last_motion_t = None
|
||||
self._ever_moved = False
|
||||
|
||||
# ── Queries ────────────────────────────────────────────────────────────
|
||||
|
||||
def idle_seconds(self, now: float) -> float:
|
||||
"""Seconds since last non-zero cmd_vel (inf if never moved)."""
|
||||
if self._last_motion_t is None:
|
||||
return math.inf
|
||||
return now - self._last_motion_t
|
||||
|
||||
def is_idle(self, now: float) -> bool:
|
||||
"""True if idle_timeout has elapsed since last non-zero cmd_vel."""
|
||||
return self.idle_seconds(now) >= self._idle_timeout
|
||||
|
||||
def is_moving(self, now: float) -> bool:
|
||||
"""True if non-zero cmd_vel was received within the idle window."""
|
||||
return self._ever_moved and not self.is_idle(now)
|
||||
|
||||
def should_start_recording(self, now: float) -> bool:
|
||||
"""True when motion is detected (used to trigger auto-record start)."""
|
||||
return self._ever_moved and not self.is_idle(now)
|
||||
|
||||
def should_stop_recording(self, now: float) -> bool:
|
||||
"""True when idle timeout has elapsed after motion (trigger auto-stop)."""
|
||||
return self._ever_moved and self.is_idle(now)
|
||||
|
||||
@property
|
||||
def ever_moved(self) -> bool:
|
||||
return self._ever_moved
|
||||
|
||||
@property
|
||||
def idle_timeout_s(self) -> float:
|
||||
return self._idle_timeout
|
||||
|
||||
|
||||
# ── Disk information ──────────────────────────────────────────────────────────
|
||||
|
||||
@dataclass
|
||||
class DiskInfo:
|
||||
"""Snapshot of disk usage for a single mount point.
|
||||
|
||||
Construct with :meth:`DiskInfo.from_path` for live data or build
|
||||
directly from known values for unit tests.
|
||||
"""
|
||||
|
||||
path: str
|
||||
total_bytes: int
|
||||
used_bytes: int
|
||||
free_bytes: int
|
||||
|
||||
@classmethod
|
||||
def from_path(cls, path: str) -> "DiskInfo":
|
||||
"""Read live disk usage from the OS."""
|
||||
u = shutil.disk_usage(path)
|
||||
return cls(path=path, total_bytes=u.total,
|
||||
used_bytes=u.used, free_bytes=u.free)
|
||||
|
||||
# ── Derived properties ─────────────────────────────────────────────────
|
||||
|
||||
@property
|
||||
def used_pct(self) -> float:
|
||||
"""Percentage of disk space used (0–100)."""
|
||||
if self.total_bytes == 0:
|
||||
return 100.0
|
||||
return self.used_bytes / self.total_bytes * 100.0
|
||||
|
||||
@property
|
||||
def free_pct(self) -> float:
|
||||
return 100.0 - self.used_pct
|
||||
|
||||
@property
|
||||
def free_gb(self) -> float:
|
||||
return self.free_bytes / (1024 ** 3)
|
||||
|
||||
@property
|
||||
def used_gb(self) -> float:
|
||||
return self.used_bytes / (1024 ** 3)
|
||||
|
||||
@property
|
||||
def total_gb(self) -> float:
|
||||
return self.total_bytes / (1024 ** 3)
|
||||
|
||||
# ── Threshold helpers ──────────────────────────────────────────────────
|
||||
|
||||
def exceeds_pct(self, threshold_pct: float) -> bool:
|
||||
return self.used_pct >= threshold_pct
|
||||
|
||||
def has_min_free(self, min_free_gb: float) -> bool:
|
||||
return self.free_gb >= min_free_gb
|
||||
|
||||
def summary(self) -> str:
|
||||
return (f"{self.path}: {self.used_gb:.1f}/{self.total_gb:.1f} GB "
|
||||
f"({self.used_pct:.1f}% used, {self.free_gb:.1f} GB free)")
|
||||
|
||||
|
||||
# ── Storage path selection ────────────────────────────────────────────────────
|
||||
|
||||
class StorageSelector:
|
||||
"""Selects the best storage path from a priority-ordered list.
|
||||
|
||||
Paths are tried in order; the first accessible path with at least
|
||||
``min_free_gb`` of free space is returned. Typical priority order:
|
||||
USB → NVMe → internal home directory.
|
||||
|
||||
Example::
|
||||
|
||||
sel = StorageSelector(['/media/usb0', '/mnt/nvme', '~/bags'])
|
||||
path = sel.select() # e.g. '/media/usb0'
|
||||
"""
|
||||
|
||||
DEFAULT_MIN_FREE_GB = 2.0
|
||||
|
||||
def __init__(self,
|
||||
paths: List[str],
|
||||
min_free_gb: float = DEFAULT_MIN_FREE_GB) -> None:
|
||||
if not paths:
|
||||
raise ValueError("paths list must not be empty")
|
||||
self._paths = [str(Path(p).expanduser()) for p in paths]
|
||||
self._min_free_gb = min_free_gb
|
||||
|
||||
def select(self) -> Optional[str]:
|
||||
"""Return the best available storage path, or None if none qualify."""
|
||||
for path in self._paths:
|
||||
p = Path(path)
|
||||
if not p.exists():
|
||||
continue
|
||||
try:
|
||||
info = DiskInfo.from_path(path)
|
||||
if info.has_min_free(self._min_free_gb):
|
||||
return path
|
||||
except OSError:
|
||||
continue
|
||||
return None
|
||||
|
||||
def select_or_default(self, default: str) -> str:
|
||||
"""Return the best path, falling back to ``default`` if none qualify."""
|
||||
return self.select() or default
|
||||
|
||||
def disk_infos(self) -> List[DiskInfo]:
|
||||
"""Return DiskInfo for all accessible paths (for status publishing)."""
|
||||
result = []
|
||||
for path in self._paths:
|
||||
if Path(path).exists():
|
||||
try:
|
||||
result.append(DiskInfo.from_path(path))
|
||||
except OSError:
|
||||
pass
|
||||
return result
|
||||
|
||||
|
||||
# ── Recording policy ──────────────────────────────────────────────────────────
|
||||
|
||||
class BagPolicy:
|
||||
"""Encapsulates all recording policy thresholds (Issue #615).
|
||||
|
||||
Separates business logic from ROS2 so it can be unit-tested without a
|
||||
live ROS2 install.
|
||||
|
||||
Parameters
|
||||
──────────
|
||||
split_size_gb float Split bag when it reaches this size. Default 1.0
|
||||
split_duration_min float Split bag after this many minutes. Default 10.0
|
||||
cleanup_age_days int Bags older than this are cleanup candidates. Default 7
|
||||
cleanup_disk_pct float Trigger cleanup when disk exceeds this %. Default 80.0
|
||||
warn_disk_pct float Log a warning when disk exceeds this %. Default 70.0
|
||||
min_free_gb float Minimum free space to start recording. Default 2.0
|
||||
"""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
split_size_gb: float = 1.0,
|
||||
split_duration_min: float = 10.0,
|
||||
cleanup_age_days: int = 7,
|
||||
cleanup_disk_pct: float = 80.0,
|
||||
warn_disk_pct: float = 70.0,
|
||||
min_free_gb: float = 2.0,
|
||||
) -> None:
|
||||
if split_size_gb <= 0:
|
||||
raise ValueError(f"split_size_gb must be > 0, got {split_size_gb!r}")
|
||||
if split_duration_min <= 0:
|
||||
raise ValueError(f"split_duration_min must be > 0, got {split_duration_min!r}")
|
||||
if cleanup_age_days < 1:
|
||||
raise ValueError(f"cleanup_age_days must be >= 1, got {cleanup_age_days!r}")
|
||||
if not (0 < warn_disk_pct < cleanup_disk_pct <= 100):
|
||||
raise ValueError(
|
||||
f"Must have 0 < warn_disk_pct({warn_disk_pct}) "
|
||||
f"< cleanup_disk_pct({cleanup_disk_pct}) <= 100"
|
||||
)
|
||||
|
||||
self.split_size_bytes = int(split_size_gb * 1024 ** 3)
|
||||
self.split_duration_s = split_duration_min * 60.0
|
||||
self.cleanup_age_days = cleanup_age_days
|
||||
self.cleanup_disk_pct = cleanup_disk_pct
|
||||
self.warn_disk_pct = warn_disk_pct
|
||||
self.min_free_gb = min_free_gb
|
||||
|
||||
# ── Split decision ─────────────────────────────────────────────────────
|
||||
|
||||
def needs_split(
|
||||
self,
|
||||
bag_size_bytes: int,
|
||||
bag_duration_s: float,
|
||||
) -> Tuple[bool, str]:
|
||||
"""Return (True, reason) if the active bag segment should be split."""
|
||||
if bag_size_bytes >= self.split_size_bytes:
|
||||
size_gb = bag_size_bytes / 1024 ** 3
|
||||
lim_gb = self.split_size_bytes / 1024 ** 3
|
||||
return True, f"size {size_gb:.2f} GB ≥ {lim_gb:.1f} GB"
|
||||
if bag_duration_s >= self.split_duration_s:
|
||||
return True, (
|
||||
f"duration {bag_duration_s / 60:.1f} min "
|
||||
f"≥ {self.split_duration_s / 60:.0f} min"
|
||||
)
|
||||
return False, ""
|
||||
|
||||
# ── Disk thresholds ────────────────────────────────────────────────────
|
||||
|
||||
def needs_cleanup(self, disk: DiskInfo) -> bool:
|
||||
"""True if disk is full enough to warrant deleting old bags."""
|
||||
return disk.exceeds_pct(self.cleanup_disk_pct)
|
||||
|
||||
def should_warn_disk(self, disk: DiskInfo) -> bool:
|
||||
"""True if disk usage deserves a warning log."""
|
||||
return disk.exceeds_pct(self.warn_disk_pct)
|
||||
|
||||
def can_record(self, disk: DiskInfo) -> bool:
|
||||
"""True if there is enough free space to start/continue recording."""
|
||||
return disk.has_min_free(self.min_free_gb)
|
||||
|
||||
# ── Age-based cleanup ──────────────────────────────────────────────────
|
||||
|
||||
def bag_is_expired(self, bag_age_days: float) -> bool:
|
||||
"""True if a bag is old enough to be a cleanup candidate."""
|
||||
return bag_age_days >= self.cleanup_age_days
|
||||
|
||||
def bag_age_days(self, bag_mtime: float, now: float) -> float:
|
||||
"""Compute bag age in fractional days from mtime and current time."""
|
||||
return (now - bag_mtime) / 86400.0
|
||||
@ -1,265 +1,383 @@
|
||||
#!/usr/bin/env python3
|
||||
"""bag_recorder_node.py — ROS2 bag recording manager (Issue #615).
|
||||
|
||||
import os
|
||||
import signal
|
||||
import shutil
|
||||
import subprocess
|
||||
import threading
|
||||
import time
|
||||
Upgrades the Issue #488 basic recorder with:
|
||||
• Motion-triggered auto-record (/cmd_vel non-zero → start, 30s idle → stop)
|
||||
• Auto-split at 1 GB or 10 min
|
||||
• USB/NVMe storage path selection with disk monitoring
|
||||
• Disk-usage-triggered cleanup (>80% → delete bags >7 days old)
|
||||
• JSON status topic at 1 Hz
|
||||
|
||||
Services (Trigger):
|
||||
/saltybot/bag_recorder/start /saltybot/bag_recorder/stop
|
||||
/saltybot/bag_recorder/split (plus legacy /saltybot/start_recording etc.)
|
||||
|
||||
Topics:
|
||||
SUB /cmd_vel (geometry_msgs/Twist)
|
||||
PUB /saltybot/bag_recorder/status (std_msgs/String, JSON)
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import json, os, shutil, signal, subprocess, threading, time
|
||||
from datetime import datetime
|
||||
from pathlib import Path
|
||||
from datetime import datetime, timedelta
|
||||
from typing import List, Optional
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from std_srvs.srv import Trigger
|
||||
from geometry_msgs.msg import Twist
|
||||
from std_msgs.msg import String
|
||||
from std_srvs.srv import Trigger
|
||||
|
||||
from saltybot_bag_recorder.bag_policy import BagPolicy, DiskInfo, MotionState, StorageSelector
|
||||
|
||||
|
||||
class _BagSession:
|
||||
"""Tracks one recording session (possibly multiple split segments)."""
|
||||
|
||||
def __init__(self, bag_dir: Path, session_name: str) -> None:
|
||||
self.bag_dir = bag_dir
|
||||
self.session_name = session_name
|
||||
self.start_time = time.monotonic()
|
||||
self.split_count = 0
|
||||
self.proc: Optional[subprocess.Popen] = None
|
||||
self.segment_start = time.monotonic()
|
||||
self._lock = threading.Lock()
|
||||
|
||||
def segment_name(self) -> str:
|
||||
return f"{self.session_name}_s{self.split_count:03d}"
|
||||
|
||||
def segment_path(self) -> Path:
|
||||
return self.bag_dir / self.segment_name()
|
||||
|
||||
def segment_size_bytes(self) -> int:
|
||||
p = self.segment_path()
|
||||
if not p.exists():
|
||||
return 0
|
||||
return sum(f.stat().st_size for f in p.rglob("*") if f.is_file())
|
||||
|
||||
def segment_duration_s(self) -> float:
|
||||
return time.monotonic() - self.segment_start
|
||||
|
||||
def session_duration_s(self) -> float:
|
||||
return time.monotonic() - self.start_time
|
||||
|
||||
def is_alive(self) -> bool:
|
||||
with self._lock:
|
||||
return self.proc is not None and self.proc.poll() is None
|
||||
|
||||
def stop_proc(self, timeout: float = 5.0) -> None:
|
||||
with self._lock:
|
||||
if self.proc is None:
|
||||
return
|
||||
try:
|
||||
self.proc.send_signal(signal.SIGINT)
|
||||
self.proc.wait(timeout=timeout)
|
||||
except subprocess.TimeoutExpired:
|
||||
self.proc.kill()
|
||||
self.proc.wait()
|
||||
except OSError:
|
||||
pass
|
||||
self.proc = None
|
||||
|
||||
|
||||
class BagRecorderNode(Node):
|
||||
"""ROS2 bag recording service for mission logging (Issue #488)."""
|
||||
"""ROS2 bag recording manager with motion trigger and disk monitoring (Issue #615)."""
|
||||
|
||||
def __init__(self):
|
||||
super().__init__('saltybot_bag_recorder')
|
||||
DEFAULT_TOPICS: List[str] = [
|
||||
"/scan", "/cmd_vel", "/odom", "/tf", "/tf_static",
|
||||
"/saltybot/pose/fused_cov", "/saltybot/diagnostics",
|
||||
"/saltybot/sensor_health", "/saltybot/battery",
|
||||
"/saltybot/motor_daemon/status",
|
||||
"/camera/color/image_raw/compressed",
|
||||
"/camera/depth/image_rect_raw/compressed",
|
||||
"/imu/data",
|
||||
]
|
||||
|
||||
# Configuration (Issue #488: mission logging)
|
||||
default_bag_dir = str(Path.home() / 'saltybot-data' / 'bags')
|
||||
self.declare_parameter('bag_dir', default_bag_dir)
|
||||
self.declare_parameter('topics', [
|
||||
'/scan',
|
||||
'/cmd_vel',
|
||||
'/odom',
|
||||
'/tf',
|
||||
'/camera/color/image_raw/compressed',
|
||||
'/saltybot/diagnostics'
|
||||
])
|
||||
self.declare_parameter('buffer_duration_minutes', 30)
|
||||
self.declare_parameter('storage_ttl_days', 7)
|
||||
self.declare_parameter('max_storage_gb', 20)
|
||||
self.declare_parameter('enable_rsync', False)
|
||||
self.declare_parameter('rsync_destination', '')
|
||||
self.declare_parameter('storage_format', 'mcap')
|
||||
DEFAULT_STORAGE_PATHS: List[str] = [
|
||||
"/media/usb0", "/media/usb1",
|
||||
"/mnt/nvme/saltybot-bags",
|
||||
"~/saltybot-data/bags",
|
||||
]
|
||||
|
||||
self.bag_dir = Path(self.get_parameter('bag_dir').value)
|
||||
self.topics = self.get_parameter('topics').value
|
||||
self.buffer_duration = self.get_parameter('buffer_duration_minutes').value * 60
|
||||
self.storage_ttl_days = self.get_parameter('storage_ttl_days').value
|
||||
self.max_storage_gb = self.get_parameter('max_storage_gb').value
|
||||
self.enable_rsync = self.get_parameter('enable_rsync').value
|
||||
self.rsync_destination = self.get_parameter('rsync_destination').value
|
||||
self.storage_format = self.get_parameter('storage_format').value
|
||||
def __init__(self) -> None:
|
||||
super().__init__("bag_recorder")
|
||||
|
||||
self.bag_dir.mkdir(parents=True, exist_ok=True)
|
||||
self.declare_parameter("storage_paths", self.DEFAULT_STORAGE_PATHS)
|
||||
self.declare_parameter("topics", self.DEFAULT_TOPICS)
|
||||
self.declare_parameter("motion_trigger", True)
|
||||
self.declare_parameter("idle_timeout_s", 30.0)
|
||||
self.declare_parameter("split_size_gb", 1.0)
|
||||
self.declare_parameter("split_duration_min", 10.0)
|
||||
self.declare_parameter("storage_format", "mcap")
|
||||
self.declare_parameter("warn_disk_pct", 70.0)
|
||||
self.declare_parameter("cleanup_disk_pct", 80.0)
|
||||
self.declare_parameter("cleanup_age_days", 7)
|
||||
self.declare_parameter("min_free_gb", 2.0)
|
||||
self.declare_parameter("status_hz", 1.0)
|
||||
self.declare_parameter("check_hz", 0.033)
|
||||
|
||||
# Recording state
|
||||
self.is_recording = False
|
||||
self.current_bag_process = None
|
||||
self.current_bag_name = None
|
||||
self.buffer_bags: List[str] = []
|
||||
self.recording_lock = threading.Lock()
|
||||
self._topics = list(self.get_parameter("topics").value)
|
||||
self._motion_trigger = self.get_parameter("motion_trigger").value
|
||||
self._storage_fmt = self.get_parameter("storage_format").value
|
||||
|
||||
# Services
|
||||
self.save_service = self.create_service(Trigger, '/saltybot/save_bag', self.save_bag_callback)
|
||||
self.start_service = self.create_service(Trigger, '/saltybot/start_recording', self.start_recording_callback)
|
||||
self.stop_service = self.create_service(Trigger, '/saltybot/stop_recording', self.stop_recording_callback)
|
||||
|
||||
# Watchdog to handle crash recovery
|
||||
self.setup_signal_handlers()
|
||||
|
||||
# Start recording
|
||||
self.start_recording()
|
||||
|
||||
# Periodic maintenance (cleanup old bags, manage storage)
|
||||
self.maintenance_timer = self.create_timer(300.0, self.maintenance_callback)
|
||||
|
||||
self.get_logger().info(
|
||||
f'Bag recorder initialized: {self.bag_dir}, format={self.storage_format}, '
|
||||
f'buffer={self.buffer_duration}s, max={self.max_storage_gb}GB, topics={len(self.topics)}'
|
||||
self._policy = BagPolicy(
|
||||
split_size_gb = self.get_parameter("split_size_gb").value,
|
||||
split_duration_min = self.get_parameter("split_duration_min").value,
|
||||
cleanup_age_days = int(self.get_parameter("cleanup_age_days").value),
|
||||
cleanup_disk_pct = self.get_parameter("cleanup_disk_pct").value,
|
||||
warn_disk_pct = self.get_parameter("warn_disk_pct").value,
|
||||
min_free_gb = self.get_parameter("min_free_gb").value,
|
||||
)
|
||||
self._motion = MotionState(
|
||||
idle_timeout_s = self.get_parameter("idle_timeout_s").value,
|
||||
)
|
||||
self._selector = StorageSelector(
|
||||
paths = list(self.get_parameter("storage_paths").value),
|
||||
min_free_gb = self.get_parameter("min_free_gb").value,
|
||||
)
|
||||
|
||||
def setup_signal_handlers(self):
|
||||
"""Setup signal handlers for graceful shutdown and crash recovery."""
|
||||
def signal_handler(sig, frame):
|
||||
self.get_logger().warn(f'Signal {sig} received, saving current bag')
|
||||
self.stop_recording(save=True)
|
||||
self.cleanup()
|
||||
rclpy.shutdown()
|
||||
self._bag_dir: Optional[Path] = None
|
||||
self._session: Optional[_BagSession] = None
|
||||
self._lock: threading.Lock = threading.Lock()
|
||||
self._recording: bool = False
|
||||
|
||||
signal.signal(signal.SIGINT, signal_handler)
|
||||
signal.signal(signal.SIGTERM, signal_handler)
|
||||
self._status_pub = self.create_publisher(
|
||||
String, "/saltybot/bag_recorder/status", 10)
|
||||
|
||||
def start_recording(self):
|
||||
"""Start bag recording in the background."""
|
||||
with self.recording_lock:
|
||||
if self.is_recording:
|
||||
self.create_service(Trigger, "/saltybot/bag_recorder/start", self._svc_start)
|
||||
self.create_service(Trigger, "/saltybot/bag_recorder/stop", self._svc_stop)
|
||||
self.create_service(Trigger, "/saltybot/bag_recorder/split", self._svc_split)
|
||||
# Legacy (Issue #488 compatibility)
|
||||
self.create_service(Trigger, "/saltybot/start_recording", self._svc_start)
|
||||
self.create_service(Trigger, "/saltybot/stop_recording", self._svc_stop)
|
||||
self.create_service(Trigger, "/saltybot/save_bag", self._svc_split)
|
||||
|
||||
if self._motion_trigger:
|
||||
self.create_subscription(Twist, "/cmd_vel", self._on_cmd_vel, 10)
|
||||
|
||||
hz = self.get_parameter("status_hz").value
|
||||
self.create_timer(1.0 / hz, self._publish_status)
|
||||
chz = self.get_parameter("check_hz").value
|
||||
self.create_timer(1.0 / chz, self._periodic_check)
|
||||
self.create_timer(60.0, self._disk_maintenance)
|
||||
|
||||
self.get_logger().info(
|
||||
f"BagRecorder ready | motion={self._motion_trigger} "
|
||||
f"idle={self._motion.idle_timeout_s:.0f}s "
|
||||
f"split={self._policy.split_size_bytes//1024**3}GB/"
|
||||
f"{int(self._policy.split_duration_s//60)}min "
|
||||
f"fmt={self._storage_fmt} topics={len(self._topics)}"
|
||||
)
|
||||
|
||||
def _on_cmd_vel(self, msg: Twist) -> None:
|
||||
now = time.monotonic()
|
||||
self._motion.update(msg.linear.x, msg.angular.z, now)
|
||||
with self._lock:
|
||||
if not self._recording and self._motion.should_start_recording(now):
|
||||
self.get_logger().info("Motion detected — auto-starting bag recording")
|
||||
self._start_recording_locked()
|
||||
|
||||
def _svc_start(self, _req, resp):
|
||||
with self._lock:
|
||||
if self._recording:
|
||||
resp.success = False
|
||||
resp.message = f"Already recording: {self._session.session_name}"
|
||||
return resp
|
||||
ok, msg = self._start_recording_locked()
|
||||
resp.success = ok
|
||||
resp.message = msg
|
||||
return resp
|
||||
|
||||
def _svc_stop(self, _req, resp):
|
||||
with self._lock:
|
||||
if not self._recording:
|
||||
resp.success = False
|
||||
resp.message = "Not recording"
|
||||
return resp
|
||||
_, ok, msg = self._stop_recording_locked()
|
||||
resp.success = ok
|
||||
resp.message = msg
|
||||
return resp
|
||||
|
||||
def _svc_split(self, _req, resp):
|
||||
with self._lock:
|
||||
if not self._recording:
|
||||
resp.success = False
|
||||
resp.message = "Not recording — nothing to split"
|
||||
return resp
|
||||
ok, msg = self._split_locked()
|
||||
resp.success = ok
|
||||
resp.message = msg
|
||||
return resp
|
||||
|
||||
def _periodic_check(self) -> None:
|
||||
now = time.monotonic()
|
||||
with self._lock:
|
||||
if self._recording and self._motion_trigger:
|
||||
if self._motion.should_stop_recording(now):
|
||||
self.get_logger().info(
|
||||
f"Idle {self._motion.idle_seconds(now):.0f}s — auto-stopping")
|
||||
self._stop_recording_locked()
|
||||
return
|
||||
if self._recording and self._session is not None:
|
||||
size = self._session.segment_size_bytes()
|
||||
dur = self._session.segment_duration_s()
|
||||
split, reason = self._policy.needs_split(size, dur)
|
||||
if split:
|
||||
self.get_logger().info(f"Auto-split: {reason}")
|
||||
self._split_locked()
|
||||
|
||||
def _disk_maintenance(self) -> None:
|
||||
if self._bag_dir is None:
|
||||
sel = self._selector.select()
|
||||
if sel is None:
|
||||
return
|
||||
|
||||
timestamp = datetime.now().strftime('%Y%m%d_%H%M%S')
|
||||
self.current_bag_name = f'saltybot_{timestamp}'
|
||||
bag_path = self.bag_dir / self.current_bag_name
|
||||
|
||||
try:
|
||||
# Build rosbag2 record command
|
||||
cmd = ['ros2', 'bag', 'record', '--output', str(bag_path), '--storage', self.storage_format]
|
||||
|
||||
# Add compression for sqlite3 format
|
||||
if self.storage_format == 'sqlite3':
|
||||
cmd.extend(['--compression-format', 'zstd', '--compression-mode', 'file'])
|
||||
|
||||
# Add topics (required for mission logging)
|
||||
if self.topics and self.topics[0]:
|
||||
cmd.extend(self.topics)
|
||||
else:
|
||||
cmd.extend(['/scan', '/cmd_vel', '/odom', '/tf', '/camera/color/image_raw/compressed', '/saltybot/diagnostics'])
|
||||
|
||||
self.current_bag_process = subprocess.Popen(cmd, stdout=subprocess.PIPE, stderr=subprocess.PIPE)
|
||||
self.is_recording = True
|
||||
self.buffer_bags.append(self.current_bag_name)
|
||||
self.get_logger().info(f'Started recording: {self.current_bag_name}')
|
||||
|
||||
except Exception as e:
|
||||
self.get_logger().error(f'Failed to start recording: {e}')
|
||||
|
||||
def save_bag_callback(self, request, response):
|
||||
"""Service callback to manually trigger bag save."""
|
||||
self._bag_dir = Path(sel)
|
||||
try:
|
||||
self.stop_recording(save=True)
|
||||
self.start_recording()
|
||||
response.success = True
|
||||
response.message = f'Saved: {self.current_bag_name}'
|
||||
self.get_logger().info(response.message)
|
||||
except Exception as e:
|
||||
response.success = False
|
||||
response.message = f'Failed to save bag: {e}'
|
||||
self.get_logger().error(response.message)
|
||||
return response
|
||||
|
||||
def start_recording_callback(self, request, response):
|
||||
"""Service callback to start recording."""
|
||||
if self.is_recording:
|
||||
response.success = False
|
||||
response.message = 'Recording already in progress'
|
||||
return response
|
||||
try:
|
||||
self.start_recording()
|
||||
response.success = True
|
||||
response.message = f'Recording started: {self.current_bag_name}'
|
||||
self.get_logger().info(response.message)
|
||||
except Exception as e:
|
||||
response.success = False
|
||||
response.message = f'Failed to start recording: {e}'
|
||||
self.get_logger().error(response.message)
|
||||
return response
|
||||
|
||||
def stop_recording_callback(self, request, response):
|
||||
"""Service callback to stop recording."""
|
||||
if not self.is_recording:
|
||||
response.success = False
|
||||
response.message = 'No recording in progress'
|
||||
return response
|
||||
try:
|
||||
self.stop_recording(save=True)
|
||||
response.success = True
|
||||
response.message = f'Recording stopped and saved: {self.current_bag_name}'
|
||||
self.get_logger().info(response.message)
|
||||
except Exception as e:
|
||||
response.success = False
|
||||
response.message = f'Failed to stop recording: {e}'
|
||||
self.get_logger().error(response.message)
|
||||
return response
|
||||
|
||||
def stop_recording(self, save: bool = False):
|
||||
"""Stop the current bag recording."""
|
||||
with self.recording_lock:
|
||||
if not self.is_recording or not self.current_bag_process:
|
||||
return
|
||||
try:
|
||||
self.current_bag_process.send_signal(signal.SIGINT)
|
||||
self.current_bag_process.wait(timeout=5.0)
|
||||
except subprocess.TimeoutExpired:
|
||||
self.get_logger().warn(f'Force terminating {self.current_bag_name}')
|
||||
self.current_bag_process.kill()
|
||||
self.current_bag_process.wait()
|
||||
except Exception as e:
|
||||
self.get_logger().error(f'Error stopping recording: {e}')
|
||||
self.is_recording = False
|
||||
self.get_logger().info(f'Stopped recording: {self.current_bag_name}')
|
||||
if save:
|
||||
self.apply_compression()
|
||||
|
||||
def apply_compression(self):
|
||||
"""Compress the current bag using zstd."""
|
||||
if not self.current_bag_name:
|
||||
disk = DiskInfo.from_path(str(self._bag_dir))
|
||||
except OSError as exc:
|
||||
self.get_logger().warn(f"Disk check failed: {exc}")
|
||||
return
|
||||
bag_path = self.bag_dir / self.current_bag_name
|
||||
try:
|
||||
tar_path = f'{bag_path}.tar.zst'
|
||||
if bag_path.exists():
|
||||
cmd = ['tar', '-I', 'zstd', '-cf', tar_path, '-C', str(self.bag_dir), self.current_bag_name]
|
||||
subprocess.run(cmd, check=True, capture_output=True, timeout=60)
|
||||
self.get_logger().info(f'Compressed: {tar_path}')
|
||||
except Exception as e:
|
||||
self.get_logger().warn(f'Compression skipped: {e}')
|
||||
if self._policy.should_warn_disk(disk):
|
||||
self.get_logger().warn(
|
||||
f"Disk {disk.used_pct:.1f}% > {self._policy.warn_disk_pct:.0f}%")
|
||||
if self._policy.needs_cleanup(disk):
|
||||
self.get_logger().warn(
|
||||
f"Disk {disk.used_pct:.1f}% >= {self._policy.cleanup_disk_pct:.0f}% — cleaning")
|
||||
self._cleanup_expired_bags()
|
||||
|
||||
def maintenance_callback(self):
|
||||
"""Periodic maintenance: cleanup old bags and manage storage."""
|
||||
self.cleanup_expired_bags()
|
||||
self.enforce_storage_quota()
|
||||
if self.enable_rsync and self.rsync_destination:
|
||||
self.rsync_bags()
|
||||
def _start_recording_locked(self):
|
||||
sel = self._selector.select()
|
||||
if sel is None:
|
||||
msg = f"No storage path with >={self._policy.min_free_gb:.1f}GB free"
|
||||
self.get_logger().error(msg)
|
||||
return False, msg
|
||||
self._bag_dir = Path(sel)
|
||||
self._bag_dir.mkdir(parents=True, exist_ok=True)
|
||||
ts = datetime.now().strftime("%Y%m%d_%H%M%S")
|
||||
self._session = _BagSession(self._bag_dir, f"saltybot_{ts}")
|
||||
ok, msg = self._launch_ros2bag_locked()
|
||||
if ok:
|
||||
self._recording = True
|
||||
self.get_logger().info(
|
||||
f"Recording: {self._session.segment_name()} -> {self._bag_dir}")
|
||||
return ok, msg
|
||||
|
||||
def cleanup_expired_bags(self):
|
||||
"""Remove bags older than TTL."""
|
||||
def _stop_recording_locked(self):
|
||||
if self._session is None:
|
||||
return None, False, "No active session"
|
||||
name = self._session.session_name
|
||||
self._session.stop_proc()
|
||||
self._recording = False
|
||||
self.get_logger().info(f"Stopped: {name}")
|
||||
return name, True, f"Stopped: {name}"
|
||||
|
||||
def _split_locked(self):
|
||||
if self._session is None:
|
||||
return False, "No active session"
|
||||
old = self._session.segment_name()
|
||||
self._session.stop_proc()
|
||||
self._session.split_count += 1
|
||||
self._session.segment_start = time.monotonic()
|
||||
ok, msg = self._launch_ros2bag_locked()
|
||||
if ok:
|
||||
self.get_logger().info(f"Split: {old} -> {self._session.segment_name()}")
|
||||
return True, f"Split: {old} -> {self._session.segment_name()}"
|
||||
return ok, msg
|
||||
|
||||
def _launch_ros2bag_locked(self):
|
||||
seg_path = self._session.segment_path()
|
||||
cmd = ["ros2", "bag", "record",
|
||||
"--output", str(seg_path),
|
||||
"--storage", self._storage_fmt]
|
||||
if self._storage_fmt == "sqlite3":
|
||||
cmd += ["--compression-format", "zstd", "--compression-mode", "file"]
|
||||
cmd += self._topics
|
||||
try:
|
||||
cutoff_time = datetime.now() - timedelta(days=self.storage_ttl_days)
|
||||
for item in self.bag_dir.iterdir():
|
||||
if item.is_dir() and item.name.startswith('saltybot_'):
|
||||
self._session.proc = subprocess.Popen(
|
||||
cmd, stdout=subprocess.DEVNULL, stderr=subprocess.PIPE,
|
||||
preexec_fn=os.setsid)
|
||||
return True, f"Recording: {self._session.segment_name()}"
|
||||
except Exception as exc:
|
||||
msg = f"Failed to launch ros2 bag record: {exc}"
|
||||
self.get_logger().error(msg)
|
||||
return False, msg
|
||||
|
||||
def _cleanup_expired_bags(self) -> None:
|
||||
if self._bag_dir is None or not self._bag_dir.exists():
|
||||
return
|
||||
now = time.time()
|
||||
for item in sorted(self._bag_dir.iterdir(),
|
||||
key=lambda p: p.stat().st_mtime):
|
||||
if not item.is_dir() or not item.name.startswith("saltybot_"):
|
||||
continue
|
||||
if (self._session and
|
||||
item.name.startswith(self._session.session_name)):
|
||||
continue
|
||||
age = self._policy.bag_age_days(item.stat().st_mtime, now)
|
||||
if self._policy.bag_is_expired(age):
|
||||
try:
|
||||
shutil.rmtree(item)
|
||||
self.get_logger().info(
|
||||
f"Cleanup: removed {item.name} (age {age:.1f}d)")
|
||||
try:
|
||||
timestamp_str = item.name.replace('saltybot_', '')
|
||||
item_time = datetime.strptime(timestamp_str, '%Y%m%d_%H%M%S')
|
||||
if item_time < cutoff_time:
|
||||
shutil.rmtree(item, ignore_errors=True)
|
||||
self.get_logger().info(f'Removed expired bag: {item.name}')
|
||||
except (ValueError, OSError) as e:
|
||||
self.get_logger().warn(f'Error processing {item.name}: {e}')
|
||||
except Exception as e:
|
||||
self.get_logger().error(f'Cleanup failed: {e}')
|
||||
|
||||
def enforce_storage_quota(self):
|
||||
"""Remove oldest bags if total size exceeds quota (FIFO)."""
|
||||
try:
|
||||
total_size = sum(f.stat().st_size for f in self.bag_dir.rglob('*') if f.is_file()) / (1024 ** 3)
|
||||
if total_size > self.max_storage_gb:
|
||||
self.get_logger().warn(f'Storage quota exceeded: {total_size:.2f}GB > {self.max_storage_gb}GB')
|
||||
bags = sorted([d for d in self.bag_dir.iterdir() if d.is_dir() and d.name.startswith('saltybot_')], key=lambda x: x.stat().st_mtime)
|
||||
for bag in bags:
|
||||
if total_size <= self.max_storage_gb:
|
||||
disk = DiskInfo.from_path(str(self._bag_dir))
|
||||
if not self._policy.needs_cleanup(disk):
|
||||
break
|
||||
except OSError:
|
||||
break
|
||||
bag_size = sum(f.stat().st_size for f in bag.rglob('*') if f.is_file()) / (1024 ** 3)
|
||||
shutil.rmtree(bag, ignore_errors=True)
|
||||
total_size -= bag_size
|
||||
self.get_logger().info(f'Removed bag to enforce quota: {bag.name}')
|
||||
except Exception as e:
|
||||
self.get_logger().error(f'Storage quota enforcement failed: {e}')
|
||||
except OSError as exc:
|
||||
self.get_logger().warn(f"Cleanup failed {item.name}: {exc}")
|
||||
|
||||
def rsync_bags(self):
|
||||
"""Optional: rsync bags to NAS."""
|
||||
try:
|
||||
cmd = ['rsync', '-avz', '--delete', f'{self.bag_dir}/', self.rsync_destination]
|
||||
subprocess.run(cmd, check=False, timeout=300)
|
||||
self.get_logger().info(f'Synced bags to NAS: {self.rsync_destination}')
|
||||
except subprocess.TimeoutExpired:
|
||||
self.get_logger().warn('Rsync timed out')
|
||||
except Exception as e:
|
||||
self.get_logger().error(f'Rsync failed: {e}')
|
||||
def _publish_status(self) -> None:
|
||||
with self._lock:
|
||||
s_name = self._session.session_name if self._session else None
|
||||
seg = self._session.segment_name() if self._session else None
|
||||
splits = self._session.split_count if self._session else 0
|
||||
seg_sz = self._session.segment_size_bytes() if self._session else 0
|
||||
s_dur = self._session.session_duration_s() if self._session else 0.0
|
||||
seg_dur = self._session.segment_duration_s() if self._session else 0.0
|
||||
rec = self._recording
|
||||
bdir = str(self._bag_dir) if self._bag_dir else None
|
||||
now = time.monotonic()
|
||||
idle = self._motion.idle_seconds(now)
|
||||
disk_status = {}
|
||||
if self._bag_dir and self._bag_dir.exists():
|
||||
try:
|
||||
di = DiskInfo.from_path(str(self._bag_dir))
|
||||
disk_status = {
|
||||
"path": bdir,
|
||||
"used_pct": round(di.used_pct, 1),
|
||||
"free_gb": round(di.free_gb, 2),
|
||||
"total_gb": round(di.total_gb, 2),
|
||||
"warn": self._policy.should_warn_disk(di),
|
||||
"cleanup": self._policy.needs_cleanup(di),
|
||||
}
|
||||
except OSError:
|
||||
pass
|
||||
payload = {
|
||||
"recording": rec,
|
||||
"session": s_name,
|
||||
"segment": seg,
|
||||
"split_count": splits,
|
||||
"segment_size_mb": round(seg_sz / 1e6, 1),
|
||||
"session_dur_s": round(s_dur, 1),
|
||||
"segment_dur_s": round(seg_dur, 1),
|
||||
"motion_idle_s": round(idle, 1) if idle != float("inf") else None,
|
||||
"motion_trigger": self._motion_trigger,
|
||||
"bag_dir": bdir,
|
||||
"disk": disk_status,
|
||||
}
|
||||
self._status_pub.publish(String(data=json.dumps(payload)))
|
||||
|
||||
def cleanup(self):
|
||||
"""Cleanup resources."""
|
||||
self.stop_recording(save=True)
|
||||
self.get_logger().info('Bag recorder shutdown complete')
|
||||
def _shutdown(self) -> None:
|
||||
with self._lock:
|
||||
if self._recording:
|
||||
self._stop_recording_locked()
|
||||
|
||||
|
||||
def main(args=None):
|
||||
def main(args=None) -> None:
|
||||
rclpy.init(args=args)
|
||||
node = BagRecorderNode()
|
||||
try:
|
||||
@ -267,10 +385,6 @@ def main(args=None):
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
node.cleanup()
|
||||
node._shutdown()
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
rclpy.try_shutdown()
|
||||
|
||||
379
jetson/ros2_ws/src/saltybot_bag_recorder/test/test_bag_policy.py
Normal file
379
jetson/ros2_ws/src/saltybot_bag_recorder/test/test_bag_policy.py
Normal file
@ -0,0 +1,379 @@
|
||||
"""test_bag_policy.py — Unit tests for bag recording policy (Issue #615).
|
||||
|
||||
ROS2-free. Run with:
|
||||
python3 -m pytest \
|
||||
jetson/ros2_ws/src/saltybot_bag_recorder/test/test_bag_policy.py -v
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
import os
|
||||
import sys
|
||||
from unittest.mock import MagicMock, patch
|
||||
|
||||
sys.path.insert(0, os.path.join(os.path.dirname(__file__), "..",
|
||||
"saltybot_bag_recorder"))
|
||||
|
||||
import pytest
|
||||
from bag_policy import BagPolicy, DiskInfo, MotionState, StorageSelector
|
||||
|
||||
|
||||
# ─── MotionState ──────────────────────────────────────────────────────────────
|
||||
|
||||
class TestMotionStateInit:
|
||||
def test_default_idle_timeout(self):
|
||||
assert MotionState().idle_timeout_s == 30.0
|
||||
|
||||
def test_custom_idle_timeout(self):
|
||||
assert MotionState(idle_timeout_s=60.0).idle_timeout_s == 60.0
|
||||
|
||||
def test_zero_timeout_raises(self):
|
||||
with pytest.raises(ValueError, match="idle_timeout_s"):
|
||||
MotionState(idle_timeout_s=0.0)
|
||||
|
||||
def test_negative_timeout_raises(self):
|
||||
with pytest.raises(ValueError):
|
||||
MotionState(idle_timeout_s=-5.0)
|
||||
|
||||
def test_initial_not_ever_moved(self):
|
||||
assert not MotionState().ever_moved
|
||||
|
||||
def test_initial_idle_seconds_inf(self):
|
||||
assert MotionState().idle_seconds(1000.0) == math.inf
|
||||
|
||||
def test_initial_is_idle(self):
|
||||
assert MotionState().is_idle(1000.0)
|
||||
|
||||
def test_initial_not_moving(self):
|
||||
assert not MotionState().is_moving(1000.0)
|
||||
|
||||
|
||||
class TestMotionStateUpdate:
|
||||
def setup_method(self):
|
||||
self.ms = MotionState(idle_timeout_s=30.0)
|
||||
|
||||
def test_nonzero_linear_sets_moved(self):
|
||||
self.ms.update(0.5, 0.0, 100.0)
|
||||
assert self.ms.ever_moved
|
||||
|
||||
def test_nonzero_angular_sets_moved(self):
|
||||
self.ms.update(0.0, 0.3, 100.0)
|
||||
assert self.ms.ever_moved
|
||||
|
||||
def test_zero_cmd_vel_does_not_set_moved(self):
|
||||
self.ms.update(0.0, 0.0, 100.0)
|
||||
assert not self.ms.ever_moved
|
||||
|
||||
def test_noise_below_threshold_ignored(self):
|
||||
self.ms.update(0.0005, 0.0005, 100.0)
|
||||
assert not self.ms.ever_moved
|
||||
|
||||
def test_idle_seconds_after_motion(self):
|
||||
self.ms.update(0.5, 0.0, 100.0)
|
||||
assert abs(self.ms.idle_seconds(105.0) - 5.0) < 1e-9
|
||||
|
||||
def test_not_idle_immediately(self):
|
||||
self.ms.update(0.5, 0.0, 100.0)
|
||||
assert not self.ms.is_idle(100.0)
|
||||
|
||||
def test_not_idle_within_timeout(self):
|
||||
self.ms.update(0.5, 0.0, 100.0)
|
||||
assert not self.ms.is_idle(129.9)
|
||||
|
||||
def test_idle_at_boundary(self):
|
||||
self.ms.update(0.5, 0.0, 100.0)
|
||||
assert self.ms.is_idle(130.0)
|
||||
|
||||
def test_idle_after_timeout(self):
|
||||
self.ms.update(0.5, 0.0, 100.0)
|
||||
assert self.ms.is_idle(200.0)
|
||||
|
||||
def test_moving_within_window(self):
|
||||
self.ms.update(0.5, 0.0, 100.0)
|
||||
assert self.ms.is_moving(115.0)
|
||||
|
||||
def test_not_moving_after_timeout(self):
|
||||
self.ms.update(0.5, 0.0, 100.0)
|
||||
assert not self.ms.is_moving(200.0)
|
||||
|
||||
def test_second_motion_resets_timer(self):
|
||||
self.ms.update(0.5, 0.0, 100.0)
|
||||
self.ms.update(0.5, 0.0, 125.0)
|
||||
assert not self.ms.is_idle(150.0) # 25s since last motion
|
||||
assert self.ms.is_idle(160.0) # 35s since last motion
|
||||
|
||||
|
||||
class TestMotionStateAutoDecisions:
|
||||
def setup_method(self):
|
||||
self.ms = MotionState(idle_timeout_s=30.0)
|
||||
|
||||
def test_no_start_before_motion(self):
|
||||
assert not self.ms.should_start_recording(1000.0)
|
||||
|
||||
def test_start_after_motion(self):
|
||||
self.ms.update(0.5, 0.0, 1000.0)
|
||||
assert self.ms.should_start_recording(1001.0)
|
||||
|
||||
def test_no_start_after_idle(self):
|
||||
self.ms.update(0.5, 0.0, 1000.0)
|
||||
assert not self.ms.should_start_recording(1040.0)
|
||||
|
||||
def test_no_stop_before_motion(self):
|
||||
assert not self.ms.should_stop_recording(1000.0)
|
||||
|
||||
def test_no_stop_while_active(self):
|
||||
self.ms.update(0.5, 0.0, 1000.0)
|
||||
assert not self.ms.should_stop_recording(1010.0)
|
||||
|
||||
def test_stop_after_idle(self):
|
||||
self.ms.update(0.5, 0.0, 1000.0)
|
||||
assert self.ms.should_stop_recording(1035.0)
|
||||
|
||||
|
||||
class TestMotionStateReset:
|
||||
def test_reset_clears_motion(self):
|
||||
ms = MotionState()
|
||||
ms.update(1.0, 0.0, 100.0)
|
||||
ms.reset()
|
||||
assert not ms.ever_moved
|
||||
assert ms.idle_seconds(200.0) == math.inf
|
||||
|
||||
|
||||
# ─── DiskInfo ─────────────────────────────────────────────────────────────────
|
||||
|
||||
def _disk(total_gb=100.0, free_gb=50.0, path="/fake"):
|
||||
total = int(total_gb * 1024**3)
|
||||
free = int(free_gb * 1024**3)
|
||||
used = total - free
|
||||
return DiskInfo(path=path, total_bytes=total, used_bytes=used, free_bytes=free)
|
||||
|
||||
|
||||
class TestDiskInfo:
|
||||
def test_used_pct_50(self):
|
||||
assert abs(_disk(100, 50).used_pct - 50.0) < 0.01
|
||||
|
||||
def test_used_pct_100(self):
|
||||
assert _disk(100, 0).used_pct == 100.0
|
||||
|
||||
def test_used_pct_0(self):
|
||||
assert abs(_disk(100, 100).used_pct) < 0.01
|
||||
|
||||
def test_free_pct(self):
|
||||
assert abs(_disk(100, 30).free_pct - 30.0) < 0.01
|
||||
|
||||
def test_free_gb(self):
|
||||
assert abs(_disk(100, 20).free_gb - 20.0) < 0.01
|
||||
|
||||
def test_total_gb(self):
|
||||
assert abs(_disk(250, 100).total_gb - 250.0) < 0.01
|
||||
|
||||
def test_used_gb(self):
|
||||
assert abs(_disk(100, 30).used_gb - 70.0) < 0.01
|
||||
|
||||
def test_exceeds_pct_true(self):
|
||||
assert _disk(100, 10).exceeds_pct(80.0) # 90% used
|
||||
|
||||
def test_exceeds_pct_false(self):
|
||||
assert not _disk(100, 50).exceeds_pct(80.0) # 50% used
|
||||
|
||||
def test_exceeds_pct_boundary(self):
|
||||
assert _disk(100, 20).exceeds_pct(80.0) # exactly 80%
|
||||
|
||||
def test_has_min_free_true(self):
|
||||
assert _disk(100, 10).has_min_free(2.0)
|
||||
|
||||
def test_has_min_free_false(self):
|
||||
assert not _disk(100, 1).has_min_free(2.0)
|
||||
|
||||
def test_zero_total_is_100pct(self):
|
||||
di = DiskInfo(path="/", total_bytes=0, used_bytes=0, free_bytes=0)
|
||||
assert di.used_pct == 100.0
|
||||
|
||||
def test_summary_has_path(self):
|
||||
di = _disk(path="/media/usb0")
|
||||
assert "/media/usb0" in di.summary()
|
||||
|
||||
|
||||
# ─── StorageSelector ─────────────────────────────────────────────────────────
|
||||
|
||||
class TestStorageSelector:
|
||||
def test_empty_paths_raises(self):
|
||||
with pytest.raises(ValueError, match="paths"):
|
||||
StorageSelector([])
|
||||
|
||||
def test_select_none_when_no_paths_exist(self):
|
||||
sel = StorageSelector(["/nonexistent/a", "/nonexistent/b"])
|
||||
assert sel.select() is None
|
||||
|
||||
def test_select_first_accessible(self, tmp_path):
|
||||
p1 = tmp_path / "p1"; p1.mkdir()
|
||||
p2 = tmp_path / "p2"; p2.mkdir()
|
||||
sel = StorageSelector([str(p1), str(p2)], min_free_gb=0.0)
|
||||
assert sel.select() == str(p1)
|
||||
|
||||
def test_select_skips_insufficient_free(self, tmp_path):
|
||||
small = tmp_path / "small"; small.mkdir()
|
||||
large = tmp_path / "large"; large.mkdir()
|
||||
|
||||
def fake_from(path):
|
||||
if "small" in path:
|
||||
return DiskInfo(path=path, total_bytes=10*1024**3,
|
||||
used_bytes=9*1024**3, free_bytes=1*1024**3)
|
||||
return DiskInfo(path=path, total_bytes=100*1024**3,
|
||||
used_bytes=10*1024**3, free_bytes=90*1024**3)
|
||||
|
||||
with patch("bag_policy.DiskInfo.from_path", side_effect=fake_from):
|
||||
sel = StorageSelector([str(small), str(large)], min_free_gb=2.0)
|
||||
assert sel.select() == str(large)
|
||||
|
||||
def test_select_or_default_fallback(self):
|
||||
sel = StorageSelector(["/nonexistent"], min_free_gb=1.0)
|
||||
assert sel.select_or_default("/fb") == "/fb"
|
||||
|
||||
def test_disk_infos_empty_nonexistent(self):
|
||||
sel = StorageSelector(["/nonexistent"])
|
||||
assert sel.disk_infos() == []
|
||||
|
||||
|
||||
# ─── BagPolicy constructor ────────────────────────────────────────────────────
|
||||
|
||||
class TestBagPolicyConstructor:
|
||||
def test_defaults(self):
|
||||
p = BagPolicy()
|
||||
assert p.split_size_bytes == 1 * 1024**3
|
||||
assert p.split_duration_s == 600.0
|
||||
assert p.cleanup_age_days == 7
|
||||
assert p.cleanup_disk_pct == 80.0
|
||||
assert p.warn_disk_pct == 70.0
|
||||
|
||||
def test_size_stored_as_bytes(self):
|
||||
assert BagPolicy(split_size_gb=2.0).split_size_bytes == 2 * 1024**3
|
||||
|
||||
def test_duration_stored_as_seconds(self):
|
||||
assert BagPolicy(split_duration_min=5.0).split_duration_s == 300.0
|
||||
|
||||
def test_zero_size_raises(self):
|
||||
with pytest.raises(ValueError, match="split_size_gb"):
|
||||
BagPolicy(split_size_gb=0.0)
|
||||
|
||||
def test_negative_size_raises(self):
|
||||
with pytest.raises(ValueError):
|
||||
BagPolicy(split_size_gb=-1.0)
|
||||
|
||||
def test_zero_duration_raises(self):
|
||||
with pytest.raises(ValueError, match="split_duration_min"):
|
||||
BagPolicy(split_duration_min=0.0)
|
||||
|
||||
def test_cleanup_age_zero_raises(self):
|
||||
with pytest.raises(ValueError, match="cleanup_age_days"):
|
||||
BagPolicy(cleanup_age_days=0)
|
||||
|
||||
def test_warn_above_cleanup_raises(self):
|
||||
with pytest.raises(ValueError):
|
||||
BagPolicy(warn_disk_pct=85.0, cleanup_disk_pct=80.0)
|
||||
|
||||
def test_equal_warn_cleanup_raises(self):
|
||||
with pytest.raises(ValueError):
|
||||
BagPolicy(warn_disk_pct=80.0, cleanup_disk_pct=80.0)
|
||||
|
||||
|
||||
# ─── BagPolicy split ─────────────────────────────────────────────────────────
|
||||
|
||||
class TestBagPolicySplit:
|
||||
def setup_method(self):
|
||||
self.p = BagPolicy(split_size_gb=1.0, split_duration_min=10.0)
|
||||
|
||||
def test_no_split_small_short(self):
|
||||
split, reason = self.p.needs_split(100*1024**2, 60)
|
||||
assert not split and reason == ""
|
||||
|
||||
def test_split_at_size_boundary(self):
|
||||
split, reason = self.p.needs_split(1024**3, 60)
|
||||
assert split
|
||||
assert "size" in reason.lower() or "gb" in reason.lower()
|
||||
|
||||
def test_split_over_size(self):
|
||||
split, _ = self.p.needs_split(int(1.5*1024**3), 60)
|
||||
assert split
|
||||
|
||||
def test_split_at_duration_boundary(self):
|
||||
split, reason = self.p.needs_split(100*1024**2, 600)
|
||||
assert split
|
||||
assert "min" in reason.lower() or "duration" in reason.lower()
|
||||
|
||||
def test_split_over_duration(self):
|
||||
split, _ = self.p.needs_split(100*1024**2, 700)
|
||||
assert split
|
||||
|
||||
def test_just_below_both(self):
|
||||
split, _ = self.p.needs_split(1024**3 - 1, 599)
|
||||
assert not split
|
||||
|
||||
def test_both_exceeded_still_splits(self):
|
||||
split, _ = self.p.needs_split(2*1024**3, 700)
|
||||
assert split
|
||||
|
||||
|
||||
# ─── BagPolicy disk thresholds ────────────────────────────────────────────────
|
||||
|
||||
class TestBagPolicyDisk:
|
||||
def setup_method(self):
|
||||
self.p = BagPolicy(warn_disk_pct=70.0, cleanup_disk_pct=80.0,
|
||||
min_free_gb=2.0)
|
||||
|
||||
def test_no_warn_below(self):
|
||||
assert not self.p.should_warn_disk(_disk(100, 40)) # 60% used
|
||||
|
||||
def test_warn_at_boundary(self):
|
||||
assert self.p.should_warn_disk(_disk(100, 30)) # 70% used
|
||||
|
||||
def test_warn_above(self):
|
||||
assert self.p.should_warn_disk(_disk(100, 25)) # 75% used
|
||||
|
||||
def test_no_cleanup_at_warn(self):
|
||||
assert not self.p.needs_cleanup(_disk(100, 30)) # 70% used
|
||||
|
||||
def test_cleanup_at_boundary(self):
|
||||
assert self.p.needs_cleanup(_disk(100, 20)) # 80% used
|
||||
|
||||
def test_cleanup_above(self):
|
||||
assert self.p.needs_cleanup(_disk(100, 5)) # 95% used
|
||||
|
||||
def test_can_record_exact_min_free(self):
|
||||
total = 100 * 1024**3
|
||||
free = int(2.0 * 1024**3)
|
||||
di = DiskInfo(path="/", total_bytes=total,
|
||||
used_bytes=total-free, free_bytes=free)
|
||||
assert self.p.can_record(di)
|
||||
|
||||
def test_cannot_record_below_min_free(self):
|
||||
total = 100 * 1024**3
|
||||
free = int(1.0 * 1024**3)
|
||||
di = DiskInfo(path="/", total_bytes=total,
|
||||
used_bytes=total-free, free_bytes=free)
|
||||
assert not self.p.can_record(di)
|
||||
|
||||
|
||||
# ─── BagPolicy age ────────────────────────────────────────────────────────────
|
||||
|
||||
class TestBagPolicyAge:
|
||||
def setup_method(self):
|
||||
self.p = BagPolicy(cleanup_age_days=7)
|
||||
|
||||
def test_fresh_not_expired(self):
|
||||
assert not self.p.bag_is_expired(1.0)
|
||||
|
||||
def test_at_boundary_expired(self):
|
||||
assert self.p.bag_is_expired(7.0)
|
||||
|
||||
def test_old_expired(self):
|
||||
assert self.p.bag_is_expired(30.0)
|
||||
|
||||
def test_age_computation(self):
|
||||
mtime = 1000.0
|
||||
age = self.p.bag_age_days(mtime, mtime + 8 * 86400)
|
||||
assert abs(age - 8.0) < 1e-9
|
||||
|
||||
def test_age_zero(self):
|
||||
assert self.p.bag_age_days(5000.0, 5000.0) == 0.0
|
||||
@ -0,0 +1,35 @@
|
||||
obstacle_detect:
|
||||
ros__parameters:
|
||||
# ── Depth processing ──────────────────────────────────────────────────────
|
||||
downsample_factor: 8 # stride-based downsample before processing
|
||||
# 640x480 / 8 = 80x60 = 4800 pts (fast RANSAC)
|
||||
|
||||
# ── RANSAC ground plane fitting ───────────────────────────────────────────
|
||||
ransac_n_iter: 50 # RANSAC iterations
|
||||
ransac_inlier_m: 0.06 # inlier distance threshold (m)
|
||||
|
||||
# ── Obstacle height filter ────────────────────────────────────────────────
|
||||
min_obstacle_h_m: 0.05 # min height above ground to count as obstacle (m)
|
||||
# — avoids false positives from ground noise
|
||||
max_obstacle_h_m: 0.80 # max height above ground to count as obstacle (m)
|
||||
# — ignores ceiling / upper structure (D435i ~0.5m mount)
|
||||
|
||||
# ── Clustering ────────────────────────────────────────────────────────────
|
||||
cluster_cell_m: 0.30 # 2D grid cell size for BFS clustering (m)
|
||||
cluster_min_pts: 5 # minimum points for a cluster to be reported
|
||||
max_obstacle_dist_m: 5.0 # discard obstacle points beyond this forward distance
|
||||
|
||||
# ── Alert / e-stop thresholds ─────────────────────────────────────────────
|
||||
danger_dist_m: 0.40 # closest obstacle z < this → DANGER alert
|
||||
warn_dist_m: 1.20 # closest obstacle z < this → WARN alert
|
||||
|
||||
# ── Safety zone integration ───────────────────────────────────────────────
|
||||
# When enabled, DANGER obstacles publish zero Twist to depth_estop_topic.
|
||||
# Wire this into the cmd_vel safety chain so safety_zone can latch e-stop.
|
||||
# Typical chain: depth_estop_topic=/cmd_vel_input → safety_zone → /cmd_vel
|
||||
depth_estop_enabled: false
|
||||
depth_estop_topic: /cmd_vel_input
|
||||
|
||||
# ── Frame / topic configuration ───────────────────────────────────────────
|
||||
camera_frame: camera_link # frame_id for MarkerArray and PointCloud2
|
||||
marker_lifetime_s: 0.20 # Marker.lifetime — 0.2 s = 6 frames at 30 Hz
|
||||
@ -0,0 +1,29 @@
|
||||
"""obstacle_detect.launch.py — RealSense depth obstacle detection (Issue #611)."""
|
||||
|
||||
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_obstacle_detect')
|
||||
default_params = os.path.join(pkg_share, 'config', 'obstacle_detect_params.yaml')
|
||||
|
||||
params_arg = DeclareLaunchArgument(
|
||||
'params_file',
|
||||
default_value=default_params,
|
||||
description='Path to obstacle_detect_params.yaml',
|
||||
)
|
||||
|
||||
obstacle_detect_node = Node(
|
||||
package='saltybot_obstacle_detect',
|
||||
executable='obstacle_detect',
|
||||
name='obstacle_detect',
|
||||
output='screen',
|
||||
parameters=[LaunchConfiguration('params_file')],
|
||||
)
|
||||
|
||||
return LaunchDescription([params_arg, obstacle_detect_node])
|
||||
36
jetson/ros2_ws/src/saltybot_obstacle_detect/package.xml
Normal file
36
jetson/ros2_ws/src/saltybot_obstacle_detect/package.xml
Normal file
@ -0,0 +1,36 @@
|
||||
<?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_obstacle_detect</name>
|
||||
<version>0.1.0</version>
|
||||
<description>
|
||||
RealSense D435i depth-based obstacle detection node (Issue #611).
|
||||
RANSAC ground-plane fitting, 2D grid-BFS obstacle clustering, publishes
|
||||
MarkerArray centroids (/saltybot/obstacles), PointCloud2 of non-ground
|
||||
points (/saltybot/obstacles/cloud), and JSON alert (/saltybot/obstacles/alert)
|
||||
with safety_zone e-stop integration via configurable Twist output.
|
||||
</description>
|
||||
<maintainer email="sl-perception@saltylab.local">sl-perception</maintainer>
|
||||
<license>MIT</license>
|
||||
|
||||
<buildtool_depend>ament_python</buildtool_depend>
|
||||
|
||||
<depend>rclpy</depend>
|
||||
<depend>std_msgs</depend>
|
||||
<depend>sensor_msgs</depend>
|
||||
<depend>geometry_msgs</depend>
|
||||
<depend>visualization_msgs</depend>
|
||||
<depend>cv_bridge</depend>
|
||||
|
||||
<exec_depend>python3-numpy</exec_depend>
|
||||
<exec_depend>python3-opencv</exec_depend>
|
||||
|
||||
<test_depend>ament_copyright</test_depend>
|
||||
<test_depend>ament_flake8</test_depend>
|
||||
<test_depend>ament_pep257</test_depend>
|
||||
<test_depend>python3-pytest</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_python</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@ -0,0 +1,192 @@
|
||||
"""
|
||||
ground_plane.py — RANSAC ground plane fitting for D435i depth images (Issue #611).
|
||||
|
||||
No ROS2 dependencies — pure Python + numpy, fully unit-testable.
|
||||
|
||||
Algorithm
|
||||
─────────
|
||||
RANSAC over 3D point cloud in camera optical frame:
|
||||
+X = right, +Y = down, +Z = forward (depth)
|
||||
|
||||
1. Sample 3 random non-collinear points.
|
||||
2. Fit plane: normal n, offset d (n·p = d, ‖n‖=1).
|
||||
3. Count inliers: |n·p - d| < inlier_thresh_m.
|
||||
4. Repeat n_iter times; keep plane with most inliers.
|
||||
5. Optionally refine: refit normal via SVD on inlier subset.
|
||||
|
||||
Normal orientation:
|
||||
After RANSAC the normal is flipped so it points in the −Y direction
|
||||
(upward in world / toward the camera). This makes "height above ground"
|
||||
unambiguous: h = d − n·p > 0 means the point is above the plane.
|
||||
|
||||
Equivalently: normal.y < 0 after orientation.
|
||||
|
||||
Height above ground:
|
||||
h(p) = d − n·p
|
||||
|
||||
h > 0 → above plane (obstacle candidate)
|
||||
h ≈ 0 → on the ground
|
||||
h < 0 → below ground (artefact / noise)
|
||||
|
||||
Obstacle filter:
|
||||
min_obstacle_h < h < max_obstacle_h (configurable, metres)
|
||||
|
||||
Typical D435i camera mount ~0.5 m above ground:
|
||||
min_obstacle_h = 0.05 m (ignore ground noise)
|
||||
max_obstacle_h = 0.80 m (ignore ceiling / upper structure)
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
from typing import Optional, Tuple
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
PlaneParams = Tuple[np.ndarray, float] # (normal_3, d_scalar)
|
||||
|
||||
|
||||
# ── RANSAC ─────────────────────────────────────────────────────────────────────
|
||||
|
||||
def fit_ground_plane(
|
||||
points: np.ndarray,
|
||||
n_iter: int = 50,
|
||||
inlier_thresh_m: float = 0.06,
|
||||
min_inlier_frac: float = 0.30,
|
||||
refine: bool = True,
|
||||
) -> Optional[PlaneParams]:
|
||||
"""
|
||||
RANSAC ground-plane estimation from an (N, 3) point array.
|
||||
|
||||
Parameters
|
||||
----------
|
||||
points : (N, 3) float32/64 point cloud in camera optical frame
|
||||
n_iter : RANSAC iterations
|
||||
inlier_thresh_m : inlier distance threshold (m)
|
||||
min_inlier_frac : minimum fraction of points that must be inliers
|
||||
refine : if True, refit plane via SVD on best inlier set
|
||||
|
||||
Returns
|
||||
-------
|
||||
(normal, d) if a plane was found, else None.
|
||||
Normal is oriented so that normal.y < 0 (points upward in world).
|
||||
"""
|
||||
n = len(points)
|
||||
if n < 10:
|
||||
return None
|
||||
|
||||
rng = np.random.default_rng(seed=42)
|
||||
best_n_in = -1
|
||||
best_params: Optional[PlaneParams] = None
|
||||
pts = points.astype(np.float64)
|
||||
|
||||
for _ in range(n_iter):
|
||||
idx = rng.integers(0, n, size=3)
|
||||
p0, p1, p2 = pts[idx[0]], pts[idx[1]], pts[idx[2]]
|
||||
|
||||
edge1 = p1 - p0
|
||||
edge2 = p2 - p0
|
||||
normal = np.cross(edge1, edge2)
|
||||
norm_len = float(np.linalg.norm(normal))
|
||||
if norm_len < 1e-8:
|
||||
continue
|
||||
|
||||
normal /= norm_len
|
||||
d = float(np.dot(normal, p0))
|
||||
|
||||
dists = np.abs(pts @ normal - d)
|
||||
inliers = dists < inlier_thresh_m
|
||||
n_in = int(inliers.sum())
|
||||
|
||||
if n_in > best_n_in:
|
||||
best_n_in = n_in
|
||||
best_params = (normal.copy(), d)
|
||||
|
||||
if best_params is None:
|
||||
return None
|
||||
if best_n_in < int(n * min_inlier_frac):
|
||||
return None
|
||||
|
||||
normal, d = best_params
|
||||
|
||||
# ── SVD refinement ──────────────────────────────────────────────────────
|
||||
if refine:
|
||||
dists = np.abs(pts @ normal - d)
|
||||
in_mask = dists < inlier_thresh_m
|
||||
if in_mask.sum() >= 4:
|
||||
in_pts = pts[in_mask]
|
||||
centroid = in_pts.mean(axis=0)
|
||||
_, _, Vt = np.linalg.svd(in_pts - centroid)
|
||||
normal = Vt[-1] # smallest singular value = plane normal
|
||||
normal /= float(np.linalg.norm(normal))
|
||||
d = float(np.dot(normal, centroid))
|
||||
|
||||
# Orient normal upward: ensure n.y < 0 (−Y = up in camera frame)
|
||||
if normal[1] > 0:
|
||||
normal = -normal
|
||||
d = -d
|
||||
|
||||
return normal, d
|
||||
|
||||
|
||||
# ── Point classification ───────────────────────────────────────────────────────
|
||||
|
||||
def height_above_plane(
|
||||
points: np.ndarray,
|
||||
plane: PlaneParams,
|
||||
) -> np.ndarray:
|
||||
"""
|
||||
Signed height of each point above the ground plane.
|
||||
|
||||
h > 0 → above ground (potential obstacle)
|
||||
h ≈ 0 → on ground
|
||||
h < 0 → subsurface artefact / sensor noise
|
||||
|
||||
Parameters
|
||||
----------
|
||||
points : (N, 3) point cloud
|
||||
plane : (normal, d) from fit_ground_plane()
|
||||
|
||||
Returns
|
||||
-------
|
||||
heights : (N,) float64
|
||||
"""
|
||||
normal, d = plane
|
||||
# h = d - n·p (positive when point is "above" the upward-facing normal)
|
||||
return d - (points.astype(np.float64) @ normal)
|
||||
|
||||
|
||||
def obstacle_mask(
|
||||
points: np.ndarray,
|
||||
plane: PlaneParams,
|
||||
min_height_m: float = 0.05,
|
||||
max_height_m: float = 0.80,
|
||||
) -> np.ndarray:
|
||||
"""
|
||||
Boolean mask: True where a point is an obstacle (above ground, in height window).
|
||||
|
||||
Parameters
|
||||
----------
|
||||
points : (N, 3) point cloud
|
||||
plane : ground plane params
|
||||
min_height_m : minimum height above ground to count as obstacle
|
||||
max_height_m : maximum height above ground (ceiling / upper structure cut-off)
|
||||
|
||||
Returns
|
||||
-------
|
||||
mask : (N,) bool
|
||||
"""
|
||||
h = height_above_plane(points, plane)
|
||||
return (h > min_height_m) & (h < max_height_m)
|
||||
|
||||
|
||||
def ground_mask(
|
||||
points: np.ndarray,
|
||||
plane: PlaneParams,
|
||||
inlier_thresh: float = 0.06,
|
||||
) -> np.ndarray:
|
||||
"""Boolean mask: True for ground-plane inlier points."""
|
||||
normal, d = plane
|
||||
dists = np.abs(points.astype(np.float64) @ normal - d)
|
||||
return dists < inlier_thresh
|
||||
@ -0,0 +1,170 @@
|
||||
"""
|
||||
obstacle_clusterer.py — 2D grid-based obstacle clustering (Issue #611).
|
||||
|
||||
No ROS2 dependencies — pure Python + numpy, fully unit-testable.
|
||||
|
||||
Algorithm
|
||||
─────────
|
||||
After RANSAC ground removal, obstacle points (non-ground) are clustered
|
||||
using a 2D occupancy grid on the horizontal (X–Z) plane in camera frame.
|
||||
|
||||
Steps:
|
||||
1. Project obstacle 3D points onto the (X, Z) bird's-eye plane.
|
||||
2. Discretise into grid cells of side `cell_m`.
|
||||
3. 4-connected BFS flood-fill to find connected components.
|
||||
4. For each component with ≥ min_pts 3D points:
|
||||
centroid = mean of the component's 3D points
|
||||
radius = max distance from centroid (bounding-sphere radius)
|
||||
|
||||
Camera frame convention:
|
||||
+X = right, +Y = down (not used for 2D projection), +Z = forward (depth)
|
||||
|
||||
Output per cluster
|
||||
──────────────────
|
||||
ObstacleCluster (dataclass):
|
||||
centroid : (3,) float array — (cx, cy, cz) in camera frame
|
||||
radius_m : float — bounding-sphere radius (m)
|
||||
height_m : float — mean height above ground plane (m)
|
||||
n_pts : int — number of 3D points in cluster
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from collections import deque
|
||||
from dataclasses import dataclass, field
|
||||
from typing import List, Tuple
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
@dataclass
|
||||
class ObstacleCluster:
|
||||
centroid: np.ndarray # (3,) float64 in camera optical frame
|
||||
radius_m: float
|
||||
height_m: float
|
||||
n_pts: int
|
||||
|
||||
@property
|
||||
def distance_m(self) -> float:
|
||||
"""Forward distance (Z in camera frame)."""
|
||||
return float(self.centroid[2])
|
||||
|
||||
@property
|
||||
def lateral_m(self) -> float:
|
||||
"""Lateral offset (X in camera frame; positive = right)."""
|
||||
return float(self.centroid[0])
|
||||
|
||||
|
||||
def cluster_obstacles(
|
||||
points: np.ndarray,
|
||||
heights: np.ndarray,
|
||||
cell_m: float = 0.30,
|
||||
min_pts: int = 5,
|
||||
x_range: Tuple[float, float] = (-4.0, 4.0),
|
||||
z_range: Tuple[float, float] = (0.15, 6.0),
|
||||
) -> List[ObstacleCluster]:
|
||||
"""
|
||||
Cluster obstacle points into connected groups.
|
||||
|
||||
Parameters
|
||||
----------
|
||||
points : (N, 3) float array — obstacle points in camera frame
|
||||
heights : (N,) float array — height above ground for each point
|
||||
cell_m : grid cell side length (m)
|
||||
min_pts : minimum points for a cluster to be reported
|
||||
x_range : (min_x, max_x) bounds for the bird's-eye grid (m)
|
||||
z_range : (min_z, max_z) forward-distance bounds (m)
|
||||
|
||||
Returns
|
||||
-------
|
||||
clusters : list of ObstacleCluster, sorted by ascending distance (z)
|
||||
"""
|
||||
if len(points) == 0:
|
||||
return []
|
||||
|
||||
pts = points.astype(np.float64)
|
||||
|
||||
# ── Discard out-of-range points ─────────────────────────────────────────
|
||||
x = pts[:, 0]
|
||||
z = pts[:, 2]
|
||||
valid = (
|
||||
(x >= x_range[0]) & (x <= x_range[1]) &
|
||||
(z >= z_range[0]) & (z <= z_range[1])
|
||||
)
|
||||
pts = pts[valid]
|
||||
heights = heights.astype(np.float64)[valid]
|
||||
|
||||
if len(pts) == 0:
|
||||
return []
|
||||
|
||||
# ── Build 2D occupancy grid (X, Z) ──────────────────────────────────────
|
||||
x_min, x_max = x_range
|
||||
z_min, z_max = z_range
|
||||
n_x = max(1, int(math.ceil((x_max - x_min) / cell_m)))
|
||||
n_z = max(1, int(math.ceil((z_max - z_min) / cell_m)))
|
||||
|
||||
xi = np.clip(((pts[:, 0] - x_min) / cell_m).astype(np.int32), 0, n_x - 1)
|
||||
zi = np.clip(((pts[:, 2] - z_min) / cell_m).astype(np.int32), 0, n_z - 1)
|
||||
|
||||
# Map each grid cell to list of 3D-point indices
|
||||
cell_pts: dict[Tuple[int, int], list] = {}
|
||||
for idx, (gx, gz) in enumerate(zip(xi.tolist(), zi.tolist())):
|
||||
key = (gx, gz)
|
||||
if key not in cell_pts:
|
||||
cell_pts[key] = []
|
||||
cell_pts[key].append(idx)
|
||||
|
||||
occupied = set(cell_pts.keys())
|
||||
|
||||
# ── BFS connected-component labelling ────────────────────────────────────
|
||||
visited = set()
|
||||
clusters = []
|
||||
|
||||
for start in occupied:
|
||||
if start in visited:
|
||||
continue
|
||||
|
||||
# BFS flood fill
|
||||
component_cells = []
|
||||
queue = deque([start])
|
||||
visited.add(start)
|
||||
|
||||
while queue:
|
||||
cx, cz = queue.popleft()
|
||||
component_cells.append((cx, cz))
|
||||
|
||||
for dx, dz in [(-1, 0), (1, 0), (0, -1), (0, 1)]:
|
||||
nb = (cx + dx, cz + dz)
|
||||
if nb in occupied and nb not in visited:
|
||||
visited.add(nb)
|
||||
queue.append(nb)
|
||||
|
||||
# Collect 3D points in this component
|
||||
pt_indices = []
|
||||
for cell in component_cells:
|
||||
pt_indices.extend(cell_pts[cell])
|
||||
|
||||
if len(pt_indices) < min_pts:
|
||||
continue
|
||||
|
||||
cluster_pts = pts[pt_indices]
|
||||
cluster_h = heights[pt_indices]
|
||||
centroid = cluster_pts.mean(axis=0)
|
||||
dists = np.linalg.norm(cluster_pts - centroid, axis=1)
|
||||
radius_m = float(dists.max())
|
||||
mean_height = float(cluster_h.mean())
|
||||
|
||||
clusters.append(ObstacleCluster(
|
||||
centroid = centroid,
|
||||
radius_m = radius_m,
|
||||
height_m = mean_height,
|
||||
n_pts = len(pt_indices),
|
||||
))
|
||||
|
||||
# Sort by forward distance
|
||||
clusters.sort(key=lambda c: c.distance_m)
|
||||
return clusters
|
||||
|
||||
|
||||
# ── Local import for math ─────────────────────────────────────────────────────
|
||||
import math
|
||||
@ -0,0 +1,512 @@
|
||||
"""
|
||||
obstacle_detect_node.py — RealSense depth obstacle detection node (Issue #611).
|
||||
|
||||
Pipeline per frame (30 Hz)
|
||||
──────────────────────────
|
||||
1. Receive D435i aligned depth image (float32, metres) + camera_info.
|
||||
2. Downsample by `downsample_factor` for efficiency.
|
||||
3. Back-project non-zero pixels to 3D (camera optical frame, +Z forward).
|
||||
4. RANSAC ground-plane fitting on the 3D cloud.
|
||||
5. Classify points: ground (inlier) vs obstacle (above ground, in height window).
|
||||
6. Cluster obstacle points using 2D grid BFS on the (X, Z) plane.
|
||||
7. Publish:
|
||||
/saltybot/obstacles visualization_msgs/MarkerArray (centroids)
|
||||
/saltybot/obstacles/cloud sensor_msgs/PointCloud2 (non-ground pts)
|
||||
/saltybot/obstacles/alert std_msgs/String (JSON alert)
|
||||
8. When `depth_estop_enabled` and DANGER detected: publish zero Twist to
|
||||
`depth_estop_topic` (default /cmd_vel_input) for safety_zone integration.
|
||||
|
||||
Obstacle alert levels (JSON on /saltybot/obstacles/alert)
|
||||
──────────────────────────────────────────────────────────
|
||||
DANGER — closest obstacle z < danger_dist_m (requires immediate stop)
|
||||
WARN — closest obstacle z < warn_dist_m (caution)
|
||||
CLEAR — no obstacles in range
|
||||
|
||||
Safety zone integration
|
||||
───────────────────────
|
||||
When `depth_estop_enabled: true`, DANGER obstacles trigger a zero Twist
|
||||
published to `depth_estop_topic` (default: /cmd_vel_input).
|
||||
This feeds into the safety_zone's cmd_vel chain and triggers its e-stop,
|
||||
giving independent depth-based stopping complementary to the LIDAR zones.
|
||||
|
||||
Subscribes
|
||||
──────────
|
||||
/camera/depth/image_rect_raw sensor_msgs/Image 30 Hz float32 (m)
|
||||
/camera/depth/camera_info sensor_msgs/CameraInfo (once)
|
||||
|
||||
Publishes
|
||||
─────────
|
||||
/saltybot/obstacles visualization_msgs/MarkerArray
|
||||
/saltybot/obstacles/cloud sensor_msgs/PointCloud2
|
||||
/saltybot/obstacles/alert std_msgs/String (JSON)
|
||||
[depth_estop_topic] geometry_msgs/Twist (zero, when DANGER)
|
||||
|
||||
Parameters (see config/obstacle_detect_params.yaml)
|
||||
────────────────────────────────────────────────────
|
||||
downsample_factor int 8 factor to reduce image before processing
|
||||
ransac_n_iter int 50 RANSAC iterations for ground plane
|
||||
ransac_inlier_m float 0.06 inlier threshold (m)
|
||||
min_obstacle_h_m float 0.05 min height above ground to be obstacle
|
||||
max_obstacle_h_m float 0.80 max height above ground to be obstacle
|
||||
cluster_cell_m float 0.30 clustering grid cell size (m)
|
||||
cluster_min_pts int 5 minimum points per cluster
|
||||
max_obstacle_dist_m float 5.0 discard obstacles beyond this distance
|
||||
danger_dist_m float 0.40 obstacle z < this → DANGER
|
||||
warn_dist_m float 1.20 obstacle z < this → WARN
|
||||
depth_estop_enabled bool false publish zero-vel when DANGER
|
||||
depth_estop_topic str /cmd_vel_input
|
||||
camera_frame str camera_link
|
||||
marker_lifetime_s float 0.2 MarkerArray marker lifetime (s)
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import json
|
||||
import math
|
||||
import struct
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from rclpy.qos import (
|
||||
QoSProfile, ReliabilityPolicy, HistoryPolicy, DurabilityPolicy
|
||||
)
|
||||
|
||||
import numpy as np
|
||||
from cv_bridge import CvBridge
|
||||
|
||||
from geometry_msgs.msg import Point, Twist, Vector3
|
||||
from sensor_msgs.msg import Image, CameraInfo, PointCloud2, PointField
|
||||
from std_msgs.msg import Header, String, ColorRGBA
|
||||
from visualization_msgs.msg import Marker, MarkerArray
|
||||
|
||||
from .ground_plane import fit_ground_plane, height_above_plane, obstacle_mask
|
||||
from .obstacle_clusterer import ObstacleCluster, cluster_obstacles
|
||||
|
||||
|
||||
# ── 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,
|
||||
)
|
||||
|
||||
# Alert levels
|
||||
CLEAR = 'CLEAR'
|
||||
WARN = 'WARN'
|
||||
DANGER = 'DANGER'
|
||||
|
||||
# Marker colours per alert level
|
||||
_COLOUR = {
|
||||
CLEAR: (0.0, 1.0, 0.0, 0.8), # green
|
||||
WARN: (1.0, 0.8, 0.0, 0.9), # yellow
|
||||
DANGER: (1.0, 0.1, 0.1, 1.0), # red
|
||||
}
|
||||
|
||||
# D435i depth scale (image is already float32 in metres when using 32FC1)
|
||||
_DEPTH_ENCODING = '32FC1'
|
||||
|
||||
|
||||
class ObstacleDetectNode(Node):
|
||||
"""RealSense depth-based obstacle detection — see module docstring."""
|
||||
|
||||
def __init__(self) -> None:
|
||||
super().__init__('obstacle_detect')
|
||||
|
||||
# ── Parameters ────────────────────────────────────────────────────────
|
||||
self.declare_parameter('downsample_factor', 8)
|
||||
self.declare_parameter('ransac_n_iter', 50)
|
||||
self.declare_parameter('ransac_inlier_m', 0.06)
|
||||
self.declare_parameter('min_obstacle_h_m', 0.05)
|
||||
self.declare_parameter('max_obstacle_h_m', 0.80)
|
||||
self.declare_parameter('cluster_cell_m', 0.30)
|
||||
self.declare_parameter('cluster_min_pts', 5)
|
||||
self.declare_parameter('max_obstacle_dist_m', 5.0)
|
||||
self.declare_parameter('danger_dist_m', 0.40)
|
||||
self.declare_parameter('warn_dist_m', 1.20)
|
||||
self.declare_parameter('depth_estop_enabled', False)
|
||||
self.declare_parameter('depth_estop_topic', '/cmd_vel_input')
|
||||
self.declare_parameter('camera_frame', 'camera_link')
|
||||
self.declare_parameter('marker_lifetime_s', 0.2)
|
||||
|
||||
self._ds = self.get_parameter('downsample_factor').value
|
||||
self._n_iter = self.get_parameter('ransac_n_iter').value
|
||||
self._inlier_m = self.get_parameter('ransac_inlier_m').value
|
||||
self._min_h = self.get_parameter('min_obstacle_h_m').value
|
||||
self._max_h = self.get_parameter('max_obstacle_h_m').value
|
||||
self._cell_m = self.get_parameter('cluster_cell_m').value
|
||||
self._min_pts = self.get_parameter('cluster_min_pts').value
|
||||
self._max_dist = self.get_parameter('max_obstacle_dist_m').value
|
||||
self._danger_m = self.get_parameter('danger_dist_m').value
|
||||
self._warn_m = self.get_parameter('warn_dist_m').value
|
||||
self._estop_en = self.get_parameter('depth_estop_enabled').value
|
||||
self._cam_frame = self.get_parameter('camera_frame').value
|
||||
self._marker_life = self.get_parameter('marker_lifetime_s').value
|
||||
|
||||
estop_topic = self.get_parameter('depth_estop_topic').value
|
||||
|
||||
# ── Internal state ────────────────────────────────────────────────────
|
||||
self._bridge = CvBridge()
|
||||
self._fx: float | None = None
|
||||
self._fy: float | None = None
|
||||
self._cx: float | None = None
|
||||
self._cy: float | None = None
|
||||
|
||||
# Ground plane stability: blend current with previous (exponential)
|
||||
self._plane_normal: np.ndarray | None = None
|
||||
self._plane_d: float | None = None
|
||||
self._plane_alpha = 0.3 # blend weight for new plane estimate
|
||||
|
||||
# ── Publishers ─────────────────────────────────────────────────────────
|
||||
self._marker_pub = self.create_publisher(
|
||||
MarkerArray, '/saltybot/obstacles', 10
|
||||
)
|
||||
self._cloud_pub = self.create_publisher(
|
||||
PointCloud2, '/saltybot/obstacles/cloud', 10
|
||||
)
|
||||
self._alert_pub = self.create_publisher(
|
||||
String, '/saltybot/obstacles/alert', 10
|
||||
)
|
||||
if self._estop_en:
|
||||
self._estop_pub = self.create_publisher(
|
||||
Twist, estop_topic, 10
|
||||
)
|
||||
else:
|
||||
self._estop_pub = None
|
||||
|
||||
# ── Subscriptions ──────────────────────────────────────────────────────
|
||||
self.create_subscription(
|
||||
CameraInfo,
|
||||
'/camera/depth/camera_info',
|
||||
self._on_camera_info,
|
||||
_LATCHED_QOS,
|
||||
)
|
||||
self.create_subscription(
|
||||
Image,
|
||||
'/camera/depth/image_rect_raw',
|
||||
self._on_depth,
|
||||
_SENSOR_QOS,
|
||||
)
|
||||
|
||||
self.get_logger().info(
|
||||
f'obstacle_detect ready — '
|
||||
f'ds={self._ds} ransac={self._n_iter}it '
|
||||
f'danger={self._danger_m}m warn={self._warn_m}m '
|
||||
f'estop={"ON → " + estop_topic if self._estop_en else "OFF"}'
|
||||
)
|
||||
|
||||
# ── Camera info ───────────────────────────────────────────────────────────
|
||||
|
||||
def _on_camera_info(self, msg: CameraInfo) -> None:
|
||||
if self._fx is not None:
|
||||
return
|
||||
self._fx = float(msg.k[0])
|
||||
self._fy = float(msg.k[4])
|
||||
self._cx = float(msg.k[2])
|
||||
self._cy = float(msg.k[5])
|
||||
self.get_logger().info(
|
||||
f'camera_info: fx={self._fx:.1f} fy={self._fy:.1f} '
|
||||
f'cx={self._cx:.1f} cy={self._cy:.1f}'
|
||||
)
|
||||
|
||||
# ── Main depth callback ───────────────────────────────────────────────────
|
||||
|
||||
def _on_depth(self, msg: Image) -> None:
|
||||
if self._fx is None:
|
||||
return # wait for camera_info
|
||||
|
||||
# ── Decode depth ──────────────────────────────────────────────────────
|
||||
try:
|
||||
depth = self._bridge.imgmsg_to_cv2(msg, desired_encoding=_DEPTH_ENCODING)
|
||||
except Exception as exc:
|
||||
self.get_logger().warning(f'depth decode error: {exc}', throttle_duration_sec=5.0)
|
||||
return
|
||||
|
||||
h_img, w_img = depth.shape
|
||||
stamp = msg.header.stamp
|
||||
|
||||
# ── Downsample ────────────────────────────────────────────────────────
|
||||
ds = self._ds
|
||||
depth_ds = depth[::ds, ::ds] # stride-based downsample
|
||||
h_ds, w_ds = depth_ds.shape
|
||||
|
||||
# Back-projection intrinsics at downsampled resolution
|
||||
fx = self._fx / ds
|
||||
fy = self._fy / ds
|
||||
cx = self._cx / ds
|
||||
cy = self._cy / ds
|
||||
|
||||
# ── Back-project to 3D ────────────────────────────────────────────────
|
||||
points = _backproject(depth_ds, fx, fy, cx, cy, max_dist=self._max_dist)
|
||||
|
||||
if len(points) < 30:
|
||||
self._publish_empty(stamp)
|
||||
return
|
||||
|
||||
# ── RANSAC ground plane ───────────────────────────────────────────────
|
||||
result = fit_ground_plane(
|
||||
points,
|
||||
n_iter = self._n_iter,
|
||||
inlier_thresh_m = self._inlier_m,
|
||||
)
|
||||
|
||||
if result is None:
|
||||
self._publish_empty(stamp)
|
||||
return
|
||||
|
||||
new_normal, new_d = result
|
||||
|
||||
# Blend with previous plane for temporal stability
|
||||
if self._plane_normal is None:
|
||||
self._plane_normal = new_normal
|
||||
self._plane_d = new_d
|
||||
else:
|
||||
alpha = self._plane_alpha
|
||||
blended = (1.0 - alpha) * self._plane_normal + alpha * new_normal
|
||||
norm_len = float(np.linalg.norm(blended))
|
||||
if norm_len > 1e-8:
|
||||
self._plane_normal = blended / norm_len
|
||||
self._plane_d = (1.0 - alpha) * self._plane_d + alpha * new_d
|
||||
|
||||
plane = (self._plane_normal, self._plane_d)
|
||||
|
||||
# ── Obstacle mask ─────────────────────────────────────────────────────
|
||||
obs_mask = obstacle_mask(points, plane, self._min_h, self._max_h)
|
||||
obs_pts = points[obs_mask]
|
||||
|
||||
heights_all = height_above_plane(points, plane)
|
||||
obs_heights = heights_all[obs_mask]
|
||||
|
||||
# ── Cluster obstacles ─────────────────────────────────────────────────
|
||||
clusters = cluster_obstacles(
|
||||
obs_pts, obs_heights,
|
||||
cell_m = self._cell_m,
|
||||
min_pts = self._min_pts,
|
||||
z_range = (0.15, self._max_dist),
|
||||
)
|
||||
|
||||
# ── Alert level ───────────────────────────────────────────────────────
|
||||
alert_level = CLEAR
|
||||
closest_dist = float('inf')
|
||||
|
||||
for cluster in clusters:
|
||||
d = cluster.distance_m
|
||||
if d < closest_dist:
|
||||
closest_dist = d
|
||||
if d < self._danger_m:
|
||||
alert_level = DANGER
|
||||
break
|
||||
if d < self._warn_m and alert_level != DANGER:
|
||||
alert_level = WARN
|
||||
|
||||
if closest_dist == float('inf'):
|
||||
closest_dist = -1.0
|
||||
|
||||
# ── Publish ───────────────────────────────────────────────────────────
|
||||
self._publish_markers(clusters, stamp)
|
||||
self._publish_cloud(obs_pts, stamp)
|
||||
self._publish_alert(alert_level, clusters, closest_dist, stamp)
|
||||
|
||||
# Depth e-stop: feed zero vel to safety zone chain
|
||||
if self._estop_en and self._estop_pub is not None and alert_level == DANGER:
|
||||
self._estop_pub.publish(Twist())
|
||||
|
||||
# ── Publishers ────────────────────────────────────────────────────────────
|
||||
|
||||
def _publish_markers(self, clusters: list, stamp) -> None:
|
||||
"""Publish obstacle centroids as sphere MarkerArray."""
|
||||
ma = MarkerArray()
|
||||
lifetime = _ros_duration(self._marker_life)
|
||||
|
||||
# Delete-all marker first for clean slate
|
||||
del_marker = Marker()
|
||||
del_marker.action = Marker.DELETEALL
|
||||
del_marker.header.stamp = stamp
|
||||
del_marker.header.frame_id = self._cam_frame
|
||||
ma.markers.append(del_marker)
|
||||
|
||||
for idx, cluster in enumerate(clusters):
|
||||
level = self._cluster_alert_level(cluster)
|
||||
r, g, b, a = _COLOUR[level]
|
||||
cx, cy, cz = cluster.centroid
|
||||
|
||||
m = Marker()
|
||||
m.header.stamp = stamp
|
||||
m.header.frame_id = self._cam_frame
|
||||
m.ns = 'obstacles'
|
||||
m.id = idx + 1
|
||||
m.type = Marker.SPHERE
|
||||
m.action = Marker.ADD
|
||||
m.pose.position = Point(x=float(cx), y=float(cy), z=float(cz))
|
||||
m.pose.orientation.w = 1.0
|
||||
rad = max(0.05, cluster.radius_m)
|
||||
m.scale = Vector3(x=rad * 2.0, y=rad * 2.0, z=rad * 2.0)
|
||||
m.color = ColorRGBA(r=r, g=g, b=b, a=a)
|
||||
m.lifetime = lifetime
|
||||
ma.markers.append(m)
|
||||
|
||||
# Text label above sphere
|
||||
lbl = Marker()
|
||||
lbl.header = m.header
|
||||
lbl.ns = 'obstacle_labels'
|
||||
lbl.id = idx + 1
|
||||
lbl.type = Marker.TEXT_VIEW_FACING
|
||||
lbl.action = Marker.ADD
|
||||
lbl.pose.position = Point(x=float(cx), y=float(cy) - 0.15, z=float(cz))
|
||||
lbl.pose.orientation.w = 1.0
|
||||
lbl.scale.z = 0.12
|
||||
lbl.color = ColorRGBA(r=1.0, g=1.0, b=1.0, a=0.9)
|
||||
lbl.text = f'{cluster.distance_m:.2f}m'
|
||||
lbl.lifetime = lifetime
|
||||
ma.markers.append(lbl)
|
||||
|
||||
self._marker_pub.publish(ma)
|
||||
|
||||
def _publish_cloud(self, points: np.ndarray, stamp) -> None:
|
||||
"""Publish non-ground obstacle points as PointCloud2 (xyz float32)."""
|
||||
if len(points) == 0:
|
||||
# Publish empty cloud
|
||||
cloud = PointCloud2()
|
||||
cloud.header.stamp = stamp
|
||||
cloud.header.frame_id = self._cam_frame
|
||||
self._cloud_pub.publish(cloud)
|
||||
return
|
||||
|
||||
pts_f32 = points.astype(np.float32)
|
||||
|
||||
fields = [
|
||||
PointField(name='x', offset=0, datatype=PointField.FLOAT32, count=1),
|
||||
PointField(name='y', offset=4, datatype=PointField.FLOAT32, count=1),
|
||||
PointField(name='z', offset=8, datatype=PointField.FLOAT32, count=1),
|
||||
]
|
||||
cloud = PointCloud2()
|
||||
cloud.header.stamp = stamp
|
||||
cloud.header.frame_id = self._cam_frame
|
||||
cloud.height = 1
|
||||
cloud.width = len(pts_f32)
|
||||
cloud.fields = fields
|
||||
cloud.is_bigendian = False
|
||||
cloud.point_step = 12 # 3 × float32
|
||||
cloud.row_step = cloud.point_step * cloud.width
|
||||
cloud.data = pts_f32.tobytes()
|
||||
cloud.is_dense = True
|
||||
self._cloud_pub.publish(cloud)
|
||||
|
||||
def _publish_alert(
|
||||
self,
|
||||
level: str,
|
||||
clusters: list,
|
||||
closest_m: float,
|
||||
stamp,
|
||||
) -> None:
|
||||
obstacles_json = [
|
||||
{
|
||||
'x': round(float(c.centroid[0]), 3),
|
||||
'y': round(float(c.centroid[1]), 3),
|
||||
'z': round(float(c.centroid[2]), 3),
|
||||
'radius_m': round(c.radius_m, 3),
|
||||
'height_m': round(c.height_m, 3),
|
||||
'n_pts': c.n_pts,
|
||||
'level': self._cluster_alert_level(c),
|
||||
}
|
||||
for c in clusters
|
||||
]
|
||||
alert = {
|
||||
'alert_level': level,
|
||||
'closest_m': round(closest_m, 3),
|
||||
'obstacle_count': len(clusters),
|
||||
'obstacles': obstacles_json,
|
||||
}
|
||||
msg = String()
|
||||
msg.data = json.dumps(alert)
|
||||
self._alert_pub.publish(msg)
|
||||
|
||||
def _publish_empty(self, stamp) -> None:
|
||||
"""Publish empty state (no obstacles detected / ground plane failed)."""
|
||||
self._publish_markers([], stamp)
|
||||
self._publish_cloud(np.zeros((0, 3), np.float32), stamp)
|
||||
alert = {
|
||||
'alert_level': CLEAR,
|
||||
'closest_m': -1.0,
|
||||
'obstacle_count': 0,
|
||||
'obstacles': [],
|
||||
}
|
||||
msg = String()
|
||||
msg.data = json.dumps(alert)
|
||||
self._alert_pub.publish(msg)
|
||||
|
||||
# ── Helpers ───────────────────────────────────────────────────────────────
|
||||
|
||||
def _cluster_alert_level(self, cluster: ObstacleCluster) -> str:
|
||||
d = cluster.distance_m
|
||||
if d < self._danger_m:
|
||||
return DANGER
|
||||
if d < self._warn_m:
|
||||
return WARN
|
||||
return CLEAR
|
||||
|
||||
|
||||
# ── Pure-function helpers ──────────────────────────────────────────────────────
|
||||
|
||||
def _backproject(
|
||||
depth: np.ndarray,
|
||||
fx: float, fy: float,
|
||||
cx: float, cy: float,
|
||||
min_dist: float = 0.15,
|
||||
max_dist: float = 6.0,
|
||||
) -> np.ndarray:
|
||||
"""
|
||||
Back-project a float32 depth image (metres) to a (N, 3) 3D point array.
|
||||
|
||||
Camera optical frame: +X right, +Y down, +Z forward.
|
||||
Only returns pixels where depth is in [min_dist, max_dist].
|
||||
"""
|
||||
h, w = depth.shape
|
||||
|
||||
# Build pixel-grid meshgrid
|
||||
u_arr = np.arange(w, dtype=np.float32)
|
||||
v_arr = np.arange(h, dtype=np.float32)
|
||||
uu, vv = np.meshgrid(u_arr, v_arr)
|
||||
|
||||
z = depth.astype(np.float32).ravel()
|
||||
u = uu.ravel()
|
||||
v = vv.ravel()
|
||||
|
||||
valid = (z > min_dist) & (z < max_dist) & np.isfinite(z)
|
||||
z = z[valid]
|
||||
u = u[valid]
|
||||
v = v[valid]
|
||||
|
||||
x = (u - cx) * z / fx
|
||||
y = (v - cy) * z / fy
|
||||
|
||||
return np.stack([x, y, z], axis=1).astype(np.float64)
|
||||
|
||||
|
||||
def _ros_duration(seconds: float):
|
||||
"""Build a rclpy Duration-compatible value for Marker.lifetime."""
|
||||
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 = ObstacleDetectNode()
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.try_shutdown()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
5
jetson/ros2_ws/src/saltybot_obstacle_detect/setup.cfg
Normal file
5
jetson/ros2_ws/src/saltybot_obstacle_detect/setup.cfg
Normal file
@ -0,0 +1,5 @@
|
||||
[develop]
|
||||
script_dir=$base/lib/saltybot_obstacle_detect
|
||||
|
||||
[install]
|
||||
install_scripts=$base/lib/saltybot_obstacle_detect
|
||||
30
jetson/ros2_ws/src/saltybot_obstacle_detect/setup.py
Normal file
30
jetson/ros2_ws/src/saltybot_obstacle_detect/setup.py
Normal file
@ -0,0 +1,30 @@
|
||||
import os
|
||||
from glob import glob
|
||||
from setuptools import setup
|
||||
|
||||
package_name = 'saltybot_obstacle_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='RealSense depth obstacle detection — RANSAC ground + clustering (Issue #611)',
|
||||
license='MIT',
|
||||
tests_require=['pytest'],
|
||||
entry_points={
|
||||
'console_scripts': [
|
||||
'obstacle_detect = saltybot_obstacle_detect.obstacle_detect_node:main',
|
||||
],
|
||||
},
|
||||
)
|
||||
@ -0,0 +1,129 @@
|
||||
"""Unit tests for ground_plane.py — RANSAC fitting and point classification."""
|
||||
|
||||
import math
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from saltybot_obstacle_detect.ground_plane import (
|
||||
fit_ground_plane,
|
||||
height_above_plane,
|
||||
obstacle_mask,
|
||||
ground_mask,
|
||||
)
|
||||
|
||||
|
||||
def make_flat_ground(n=500, y_ground=1.0, noise=0.02, rng=None):
|
||||
"""Flat ground plane at y = y_ground (camera frame: Y down)."""
|
||||
if rng is None:
|
||||
rng = np.random.default_rng(0)
|
||||
x = rng.uniform(-2.0, 2.0, n).astype(np.float64)
|
||||
z = rng.uniform(0.5, 4.0, n).astype(np.float64)
|
||||
y = np.full(n, y_ground) + rng.normal(0, noise, n)
|
||||
return np.stack([x, y, z], axis=1)
|
||||
|
||||
|
||||
def make_obstacle_above_ground(cx=0.0, y_ground=1.0, height=0.3, n=50, rng=None):
|
||||
"""Points forming a box above the ground plane."""
|
||||
if rng is None:
|
||||
rng = np.random.default_rng(1)
|
||||
x = rng.uniform(cx - 0.15, cx + 0.15, n)
|
||||
z = rng.uniform(1.5, 2.0, n)
|
||||
y = np.full(n, y_ground - height) # Y_down: smaller y = higher in world
|
||||
return np.stack([x, y, z], axis=1)
|
||||
|
||||
|
||||
# ── fit_ground_plane ──────────────────────────────────────────────────────────
|
||||
|
||||
def test_fit_flat_ground_returns_result():
|
||||
pts = make_flat_ground(n=400)
|
||||
plane = fit_ground_plane(pts, n_iter=50)
|
||||
assert plane is not None
|
||||
|
||||
|
||||
def test_fit_ground_normal_points_upward():
|
||||
"""Normal should have negative Y component (upward in camera frame)."""
|
||||
pts = make_flat_ground(n=400)
|
||||
plane = fit_ground_plane(pts, n_iter=50)
|
||||
assert plane is not None
|
||||
normal, d = plane
|
||||
assert normal[1] < 0.0, f"Expected normal.y < 0, got {normal[1]:.3f}"
|
||||
|
||||
|
||||
def test_fit_ground_normal_unit_vector():
|
||||
pts = make_flat_ground(n=300)
|
||||
plane = fit_ground_plane(pts, n_iter=50)
|
||||
assert plane is not None
|
||||
normal, _ = plane
|
||||
assert abs(np.linalg.norm(normal) - 1.0) < 1e-6
|
||||
|
||||
|
||||
def test_fit_too_few_points_returns_none():
|
||||
pts = make_flat_ground(n=5)
|
||||
plane = fit_ground_plane(pts, n_iter=10)
|
||||
assert plane is None
|
||||
|
||||
|
||||
def test_fit_empty_returns_none():
|
||||
plane = fit_ground_plane(np.zeros((0, 3)))
|
||||
assert plane is None
|
||||
|
||||
|
||||
def test_fit_tilted_ground():
|
||||
"""Tilted ground plane (robot on slope) should still be found."""
|
||||
rng = np.random.default_rng(42)
|
||||
n = 400
|
||||
x = rng.uniform(-2.0, 2.0, n)
|
||||
z = rng.uniform(0.5, 4.0, n)
|
||||
# Tilt: y = 0.5 + 0.1*z (plane tilted 5.7° forward)
|
||||
y = 0.5 + 0.1 * z + rng.normal(0, 0.02, n)
|
||||
pts = np.stack([x, y, z], axis=1)
|
||||
plane = fit_ground_plane(pts, n_iter=80)
|
||||
assert plane is not None
|
||||
|
||||
|
||||
# ── height_above_plane ────────────────────────────────────────────────────────
|
||||
|
||||
def test_ground_pts_near_zero_height():
|
||||
ground_pts = make_flat_ground(n=200, y_ground=1.0, noise=0.005)
|
||||
plane = fit_ground_plane(ground_pts, n_iter=60)
|
||||
assert plane is not None
|
||||
heights = height_above_plane(ground_pts, plane)
|
||||
assert np.abs(heights).mean() < 0.10 # mean residual < 10 cm
|
||||
|
||||
|
||||
def test_obstacle_pts_positive_height():
|
||||
rng = np.random.default_rng(7)
|
||||
ground_pts = make_flat_ground(n=300, y_ground=1.0, rng=rng)
|
||||
obs_pts = make_obstacle_above_ground(y_ground=1.0, height=0.3, rng=rng)
|
||||
all_pts = np.vstack([ground_pts, obs_pts])
|
||||
plane = fit_ground_plane(all_pts, n_iter=60)
|
||||
assert plane is not None
|
||||
heights = height_above_plane(obs_pts, plane)
|
||||
assert heights.mean() > 0.1, f"Expected obstacles above ground, got mean h={heights.mean():.3f}"
|
||||
|
||||
|
||||
# ── obstacle_mask ─────────────────────────────────────────────────────────────
|
||||
|
||||
def test_obstacle_mask_selects_correct_points():
|
||||
rng = np.random.default_rng(3)
|
||||
ground_pts = make_flat_ground(n=300, y_ground=1.0, rng=rng)
|
||||
obs_pts = make_obstacle_above_ground(y_ground=1.0, height=0.4, n=60, rng=rng)
|
||||
all_pts = np.vstack([ground_pts, obs_pts])
|
||||
plane = fit_ground_plane(all_pts, n_iter=60)
|
||||
assert plane is not None
|
||||
mask = obstacle_mask(all_pts, plane, min_height_m=0.05, max_height_m=0.80)
|
||||
# Most of the obs_pts should be selected
|
||||
n_ground_selected = mask[:300].sum()
|
||||
n_obs_selected = mask[300:].sum()
|
||||
assert n_obs_selected > 40, f"Expected most obs selected, got {n_obs_selected}/60"
|
||||
assert n_ground_selected < 30, f"Expected few ground points selected, got {n_ground_selected}/300"
|
||||
|
||||
|
||||
# ── ground_mask ───────────────────────────────────────────────────────────────
|
||||
|
||||
def test_ground_mask_covers_inliers():
|
||||
pts = make_flat_ground(n=300, noise=0.01)
|
||||
plane = fit_ground_plane(pts, n_iter=60)
|
||||
assert plane is not None
|
||||
gmask = ground_mask(pts, plane, inlier_thresh=0.08)
|
||||
assert gmask.mean() > 0.7 # at least 70% of flat ground should be ground inliers
|
||||
@ -0,0 +1,105 @@
|
||||
"""Unit tests for obstacle_clusterer.py — 2D grid BFS clustering."""
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from saltybot_obstacle_detect.obstacle_clusterer import cluster_obstacles, ObstacleCluster
|
||||
|
||||
|
||||
def _make_cluster_pts(cx, cz, n=30, spread=0.15, rng=None):
|
||||
"""Generate points around a centroid in camera frame (x=lateral, z=forward)."""
|
||||
if rng is None:
|
||||
rng = np.random.default_rng(0)
|
||||
x = rng.uniform(cx - spread, cx + spread, n)
|
||||
y = rng.uniform(-0.3, 0.0, n) # above ground (height ~0.3m)
|
||||
z = rng.uniform(cz - spread, cz + spread, n)
|
||||
return np.stack([x, y, z], axis=1)
|
||||
|
||||
|
||||
def _make_heights(pts, base_h=0.3, noise=0.05, rng=None):
|
||||
if rng is None:
|
||||
rng = np.random.default_rng(1)
|
||||
return np.full(len(pts), base_h) + rng.normal(0, noise, len(pts))
|
||||
|
||||
|
||||
# ── Basic clustering ──────────────────────────────────────────────────────────
|
||||
|
||||
def test_single_cluster_detected():
|
||||
pts = _make_cluster_pts(cx=0.0, cz=2.0, n=40)
|
||||
heights = _make_heights(pts)
|
||||
clusters = cluster_obstacles(pts, heights, cell_m=0.30, min_pts=5)
|
||||
assert len(clusters) == 1
|
||||
|
||||
|
||||
def test_two_clusters_separated():
|
||||
rng = np.random.default_rng(42)
|
||||
pts1 = _make_cluster_pts(cx=-1.0, cz=2.0, n=40, rng=rng)
|
||||
pts2 = _make_cluster_pts(cx=+1.5, cz=3.5, n=40, rng=rng)
|
||||
pts = np.vstack([pts1, pts2])
|
||||
h = _make_heights(pts, rng=rng)
|
||||
clusters = cluster_obstacles(pts, h, cell_m=0.30, min_pts=5)
|
||||
assert len(clusters) == 2
|
||||
|
||||
|
||||
def test_clusters_sorted_by_distance():
|
||||
rng = np.random.default_rng(7)
|
||||
pts_near = _make_cluster_pts(cx=0.0, cz=1.5, n=40, rng=rng)
|
||||
pts_far = _make_cluster_pts(cx=0.0, cz=4.0, n=40, rng=rng)
|
||||
pts = np.vstack([pts_far, pts_near]) # far first
|
||||
h = _make_heights(pts, rng=rng)
|
||||
clusters = cluster_obstacles(pts, h, cell_m=0.30, min_pts=5)
|
||||
assert len(clusters) == 2
|
||||
assert clusters[0].distance_m < clusters[1].distance_m
|
||||
|
||||
|
||||
def test_empty_input():
|
||||
clusters = cluster_obstacles(
|
||||
np.zeros((0, 3), np.float64),
|
||||
np.zeros(0, np.float64),
|
||||
)
|
||||
assert clusters == []
|
||||
|
||||
|
||||
def test_min_pts_filters_small_cluster():
|
||||
rng = np.random.default_rng(9)
|
||||
pts_big = _make_cluster_pts(cx=0.0, cz=2.0, n=50, rng=rng)
|
||||
pts_tiny = _make_cluster_pts(cx=2.0, cz=2.0, n=2, rng=rng)
|
||||
pts = np.vstack([pts_big, pts_tiny])
|
||||
h = _make_heights(pts, rng=rng)
|
||||
clusters = cluster_obstacles(pts, h, cell_m=0.30, min_pts=5)
|
||||
# Only the big cluster should pass
|
||||
assert len(clusters) == 1
|
||||
assert clusters[0].n_pts >= 40
|
||||
|
||||
|
||||
def test_centroid_near_true_center():
|
||||
rng = np.random.default_rng(5)
|
||||
true_cx, true_cz = 0.0, 2.5
|
||||
pts = _make_cluster_pts(cx=true_cx, cz=true_cz, n=100, spread=0.1, rng=rng)
|
||||
h = _make_heights(pts, rng=rng)
|
||||
clusters = cluster_obstacles(pts, h, cell_m=0.20, min_pts=5)
|
||||
assert len(clusters) >= 1
|
||||
c = clusters[0]
|
||||
assert abs(c.centroid[0] - true_cx) < 0.3
|
||||
assert abs(c.centroid[2] - true_cz) < 0.3
|
||||
|
||||
|
||||
def test_out_of_range_points_ignored():
|
||||
rng = np.random.default_rng(3)
|
||||
pts_ok = _make_cluster_pts(cx=0.0, cz=2.0, n=30, rng=rng)
|
||||
# Points beyond max_obstacle_dist_m=5.0
|
||||
pts_far = _make_cluster_pts(cx=0.0, cz=7.0, n=30, rng=rng)
|
||||
pts = np.vstack([pts_ok, pts_far])
|
||||
h = _make_heights(pts, rng=rng)
|
||||
clusters = cluster_obstacles(pts, h, cell_m=0.30, min_pts=5, z_range=(0.15, 5.0))
|
||||
# Only the near cluster should survive
|
||||
assert all(c.distance_m <= 5.0 for c in clusters)
|
||||
|
||||
|
||||
# ── ObstacleCluster accessors ─────────────────────────────────────────────────
|
||||
|
||||
def test_cluster_distance_and_lateral():
|
||||
centroid = np.array([1.5, -0.3, 3.0])
|
||||
c = ObstacleCluster(centroid=centroid, radius_m=0.2, height_m=0.3, n_pts=30)
|
||||
assert abs(c.distance_m - 3.0) < 1e-9
|
||||
assert abs(c.lateral_m - 1.5) < 1e-9
|
||||
@ -0,0 +1,22 @@
|
||||
espnow_relay:
|
||||
ros__parameters:
|
||||
# USB serial port for the ESP32 ESP-NOW receiver.
|
||||
# Add a udev rule to create a stable symlink:
|
||||
# SUBSYSTEM=="tty", ATTRS{idVendor}=="303a", ATTRS{idProduct}=="1001",
|
||||
# ATTRS{serial}=="<RECEIVER_SERIAL>", SYMLINK+="espnow-relay"
|
||||
serial_port: /dev/espnow-relay
|
||||
baudrate: 115200
|
||||
|
||||
# Serial read timeout in seconds (tune for latency vs CPU usage)
|
||||
read_timeout_s: 0.1
|
||||
|
||||
# Range validity window in metres
|
||||
min_range_m: 0.1
|
||||
max_range_m: 120.0
|
||||
|
||||
# How long (seconds) to hold /saltybot/estop True after last ESTOP packet.
|
||||
# Tag sends 3× clear packets on button release; latch handles packet loss.
|
||||
estop_latch_s: 2.0
|
||||
|
||||
# Whether to publish MSG_HEARTBEAT frames on /uwb/espnow/heartbeat
|
||||
publish_heartbeat: true
|
||||
@ -0,0 +1,43 @@
|
||||
"""
|
||||
espnow_relay.launch.py — Launch ESP-NOW to ROS2 serial relay.
|
||||
|
||||
Usage:
|
||||
ros2 launch saltybot_uwb_espnow_relay espnow_relay.launch.py
|
||||
ros2 launch saltybot_uwb_espnow_relay espnow_relay.launch.py \\
|
||||
serial_port:=/dev/ttyUSB3
|
||||
"""
|
||||
|
||||
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_espnow_relay")
|
||||
params_file = os.path.join(pkg_dir, "config", "espnow_relay_params.yaml")
|
||||
|
||||
serial_port_arg = DeclareLaunchArgument(
|
||||
"serial_port",
|
||||
default_value="/dev/espnow-relay",
|
||||
description="USB serial port for ESP-NOW receiver ESP32",
|
||||
)
|
||||
|
||||
relay_node = Node(
|
||||
package="saltybot_uwb_espnow_relay",
|
||||
executable="espnow_relay",
|
||||
name="espnow_relay",
|
||||
parameters=[
|
||||
params_file,
|
||||
{"serial_port": LaunchConfiguration("serial_port")},
|
||||
],
|
||||
output="screen",
|
||||
emulate_tty=True,
|
||||
)
|
||||
|
||||
return LaunchDescription([
|
||||
serial_port_arg,
|
||||
relay_node,
|
||||
])
|
||||
28
jetson/ros2_ws/src/saltybot_uwb_espnow_relay/package.xml
Normal file
28
jetson/ros2_ws/src/saltybot_uwb_espnow_relay/package.xml
Normal file
@ -0,0 +1,28 @@
|
||||
<?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_espnow_relay</name>
|
||||
<version>0.1.0</version>
|
||||
<description>
|
||||
ESP-NOW to ROS2 serial relay for SaltyBot UWB tags (Issue #618).
|
||||
Reads 20-byte EspNowPacket frames from an ESP32 receiver over USB serial.
|
||||
Publishes RANGE as UwbRange, ESTOP as std_msgs/Bool, HEARTBEAT as EspNowHeartbeat.
|
||||
</description>
|
||||
<maintainer email="sl-uwb@saltylab.local">sl-uwb</maintainer>
|
||||
<license>Apache-2.0</license>
|
||||
|
||||
<depend>rclpy</depend>
|
||||
<depend>std_msgs</depend>
|
||||
<depend>saltybot_uwb_msgs</depend>
|
||||
|
||||
<exec_depend>python3-serial</exec_depend>
|
||||
|
||||
<test_depend>ament_copyright</test_depend>
|
||||
<test_depend>ament_flake8</test_depend>
|
||||
<test_depend>ament_pep257</test_depend>
|
||||
<test_depend>python3-pytest</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_python</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@ -0,0 +1,174 @@
|
||||
"""
|
||||
packet.py — ESP-NOW packet parser for SaltyBot UWB tag relay (Issue #618)
|
||||
|
||||
The tag firmware broadcasts 20-byte ESP-NOW packets over the air.
|
||||
An ESP32 receiver (acting as a WiFi sniffer or paired ESP-NOW node)
|
||||
forwards each received packet verbatim over USB serial as a SLIP-framed
|
||||
or length-prefixed binary blob.
|
||||
|
||||
Serial framing (receiver firmware convention)
|
||||
─────────────────────────────────────────────
|
||||
STX 0x02 (1 byte, start of frame)
|
||||
LEN 0x14 (=20) (1 byte, payload length — always 20 for this protocol)
|
||||
DATA [20 bytes] (raw EspNowPacket)
|
||||
CRC 0x00 (1 byte, XOR of DATA bytes, simple integrity check)
|
||||
|
||||
→ Total wire bytes per frame: 23
|
||||
|
||||
Packet layout (EspNowPacket, 20 bytes, little-endian)
|
||||
──────────────────────────────────────────────────────
|
||||
[0-1] magic 0x5B 0x01
|
||||
[2] tag_id uint8
|
||||
[3] msg_type 0x10=RANGE, 0x20=ESTOP, 0x30=HEARTBEAT
|
||||
[4] anchor_id uint8
|
||||
[5-8] range_mm int32 LE
|
||||
[9-12] rssi_dbm float32 LE
|
||||
[13-16] timestamp_ms uint32 LE (ESP32 millis())
|
||||
[17] battery_pct uint8 (0-100, or 0xFF = unknown)
|
||||
[18] flags uint8 bit0 = estop_active
|
||||
[19] seq_num uint8 (rolling)
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import struct
|
||||
from dataclasses import dataclass
|
||||
from typing import Optional
|
||||
|
||||
MAGIC_0 = 0x5B
|
||||
MAGIC_1 = 0x01
|
||||
|
||||
MSG_RANGE = 0x10
|
||||
MSG_ESTOP = 0x20
|
||||
MSG_HEARTBEAT = 0x30
|
||||
|
||||
PACKET_LEN = 20
|
||||
|
||||
_FRAME_STX = 0x02
|
||||
_FRAME_LEN_BYTE = PACKET_LEN # always 20
|
||||
|
||||
FLAG_ESTOP_ACTIVE = 0x01
|
||||
|
||||
_FMT = "<2BBI f I BBB x" # x = 1-byte pad (matches struct alignment)
|
||||
# Breakdown:
|
||||
# 2B = magic[2]
|
||||
# B = tag_id
|
||||
# B = msg_type
|
||||
# B = anchor_id ← wait, need to account for actual layout carefully
|
||||
|
||||
# Use explicit offset-based unpacking to be safe:
|
||||
# [0] magic[0] B
|
||||
# [1] magic[1] B
|
||||
# [2] tag_id B
|
||||
# [3] msg_type B
|
||||
# [4] anchor_id B
|
||||
# [5-8] range_mm i (signed 32-bit LE)
|
||||
# [9-12]rssi_dbm f (float32 LE)
|
||||
# [13-16]timestamp_ms I (uint32 LE)
|
||||
# [17] battery_pct B
|
||||
# [18] flags B
|
||||
# [19] seq_num B
|
||||
|
||||
_STRUCT = struct.Struct("<BBBBBifIBBB") # 1+1+1+1+1+4+4+4+1+1+1 = 20 ✓
|
||||
assert _STRUCT.size == 20, f"struct size mismatch: {_STRUCT.size}"
|
||||
|
||||
|
||||
@dataclass
|
||||
class EspNowPacket:
|
||||
tag_id: int
|
||||
msg_type: int # MSG_RANGE, MSG_ESTOP, MSG_HEARTBEAT
|
||||
anchor_id: int
|
||||
range_mm: int # signed, valid only for MSG_RANGE
|
||||
rssi_dbm: float # valid for MSG_RANGE
|
||||
timestamp_ms: int # ESP32 millis() at send time
|
||||
battery_pct: int # 0-100 or 255 (unknown)
|
||||
flags: int # bit0 = estop_active
|
||||
seq_num: int
|
||||
|
||||
@property
|
||||
def estop_active(self) -> bool:
|
||||
return bool(self.flags & FLAG_ESTOP_ACTIVE)
|
||||
|
||||
@staticmethod
|
||||
def from_bytes(data: bytes) -> "EspNowPacket":
|
||||
if len(data) != PACKET_LEN:
|
||||
raise ValueError(f"Expected {PACKET_LEN} bytes, got {len(data)}")
|
||||
m0, m1, tag_id, msg_type, anchor_id, range_mm, rssi_dbm, \
|
||||
timestamp_ms, battery_pct, flags, seq_num = _STRUCT.unpack(data)
|
||||
if m0 != MAGIC_0 or m1 != MAGIC_1:
|
||||
raise ValueError(
|
||||
f"Bad magic: 0x{m0:02X} 0x{m1:02X} (expected 0x5B 0x01)"
|
||||
)
|
||||
return EspNowPacket(
|
||||
tag_id=tag_id,
|
||||
msg_type=msg_type,
|
||||
anchor_id=anchor_id,
|
||||
range_mm=range_mm,
|
||||
rssi_dbm=rssi_dbm,
|
||||
timestamp_ms=timestamp_ms,
|
||||
battery_pct=battery_pct,
|
||||
flags=flags,
|
||||
seq_num=seq_num,
|
||||
)
|
||||
|
||||
|
||||
class FrameReader:
|
||||
"""
|
||||
Stateful framing decoder for the serial stream from the ESP32 receiver.
|
||||
|
||||
Framing: STX(0x02) + LEN(0x14) + DATA(20 bytes) + XOR-CRC(1 byte)
|
||||
Yields EspNowPacket objects for each valid, CRC-passing frame.
|
||||
Invalid bytes before STX are silently discarded (sync recovery).
|
||||
"""
|
||||
|
||||
_STATE_HUNT = 0
|
||||
_STATE_LEN = 1
|
||||
_STATE_DATA = 2
|
||||
_STATE_CRC = 3
|
||||
|
||||
def __init__(self) -> None:
|
||||
self._state = self._STATE_HUNT
|
||||
self._buf: bytearray = bytearray()
|
||||
|
||||
def feed(self, data: bytes) -> list[EspNowPacket]:
|
||||
"""Feed raw bytes; return list of parsed packets (may be empty)."""
|
||||
packets: list[EspNowPacket] = []
|
||||
for byte in data:
|
||||
pkt = self._step(byte)
|
||||
if pkt is not None:
|
||||
packets.append(pkt)
|
||||
return packets
|
||||
|
||||
def _step(self, byte: int) -> Optional[EspNowPacket]:
|
||||
if self._state == self._STATE_HUNT:
|
||||
if byte == _FRAME_STX:
|
||||
self._state = self._STATE_LEN
|
||||
return None
|
||||
|
||||
if self._state == self._STATE_LEN:
|
||||
if byte == _FRAME_LEN_BYTE:
|
||||
self._buf = bytearray()
|
||||
self._state = self._STATE_DATA
|
||||
else:
|
||||
self._state = self._STATE_HUNT # unexpected length — re-hunt
|
||||
return None
|
||||
|
||||
if self._state == self._STATE_DATA:
|
||||
self._buf.append(byte)
|
||||
if len(self._buf) == PACKET_LEN:
|
||||
self._state = self._STATE_CRC
|
||||
return None
|
||||
|
||||
if self._state == self._STATE_CRC:
|
||||
self._state = self._STATE_HUNT
|
||||
crc = 0
|
||||
for b in self._buf:
|
||||
crc ^= b
|
||||
if crc != byte:
|
||||
return None # CRC fail — drop frame
|
||||
try:
|
||||
return EspNowPacket.from_bytes(bytes(self._buf))
|
||||
except ValueError:
|
||||
return None
|
||||
|
||||
return None # unreachable
|
||||
@ -0,0 +1,244 @@
|
||||
"""
|
||||
relay_node.py — ESP-NOW → ROS2 serial relay (Issue #618)
|
||||
|
||||
Hardware
|
||||
────────
|
||||
An ESP32 (e.g. a spare UWB-Pro board in WiFi-only mode) acts as an
|
||||
ESP-NOW broadcast receiver. It receives 20-byte EspNowPacket frames
|
||||
from nearby UWB tags and forwards each one verbatim over USB serial
|
||||
with a lightweight framing wrapper:
|
||||
|
||||
STX(0x02) LEN(0x14) DATA[20] XOR-CRC(1) = 23 bytes per frame
|
||||
|
||||
Subscriptions
|
||||
─────────────
|
||||
(none — passive relay)
|
||||
|
||||
Publishes
|
||||
─────────
|
||||
/uwb/espnow/ranges saltybot_uwb_msgs/UwbRange — MSG_RANGE frames
|
||||
/uwb/espnow/estop std_msgs/Bool — MSG_ESTOP frames
|
||||
True = e-stop active
|
||||
False = e-stop cleared
|
||||
/uwb/espnow/heartbeat saltybot_uwb_espnow_relay/msg/ — MSG_HEARTBEAT
|
||||
EspNowHeartbeat (see below)
|
||||
/saltybot/estop std_msgs/Bool — mirrors /uwb/espnow/estop
|
||||
(shared e-stop bus, latched True until explicit clear)
|
||||
|
||||
Parameters
|
||||
──────────
|
||||
serial_port /dev/espnow-relay (udev symlink for receiver ESP32)
|
||||
baudrate 115200
|
||||
read_timeout_s 0.1 (serial read timeout)
|
||||
min_range_m 0.1 (discard implausibly short ranges)
|
||||
max_range_m 120.0 (DW3000 rated max)
|
||||
estop_latch_s 2.0 (hold /saltybot/estop True for N s
|
||||
after last ESTOP packet before clearing)
|
||||
publish_heartbeat true
|
||||
|
||||
udev rule for receiver ESP32 (add to /etc/udev/rules.d/99-uwb-anchors.rules):
|
||||
SUBSYSTEM=="tty", ATTRS{idVendor}=="303a", ATTRS{idProduct}=="1001",
|
||||
ATTRS{serial}=="<RECEIVER_SERIAL>", SYMLINK+="espnow-relay"
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import threading
|
||||
import time
|
||||
|
||||
import serial
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
|
||||
|
||||
from std_msgs.msg import Bool, Header
|
||||
from saltybot_uwb_msgs.msg import UwbRange, EspNowHeartbeat
|
||||
from saltybot_uwb_espnow_relay.packet import (
|
||||
FrameReader, MSG_RANGE, MSG_ESTOP, MSG_HEARTBEAT,
|
||||
)
|
||||
|
||||
|
||||
_SENSOR_QOS = QoSProfile(
|
||||
reliability=ReliabilityPolicy.BEST_EFFORT,
|
||||
history=HistoryPolicy.KEEP_LAST,
|
||||
depth=10,
|
||||
)
|
||||
|
||||
|
||||
class EspNowRelayNode(Node):
|
||||
|
||||
def __init__(self) -> None:
|
||||
super().__init__("espnow_relay")
|
||||
|
||||
# ── Parameters ────────────────────────────────────────────────────
|
||||
self.declare_parameter("serial_port", "/dev/espnow-relay")
|
||||
self.declare_parameter("baudrate", 115200)
|
||||
self.declare_parameter("read_timeout_s", 0.1)
|
||||
self.declare_parameter("min_range_m", 0.1)
|
||||
self.declare_parameter("max_range_m", 120.0)
|
||||
self.declare_parameter("estop_latch_s", 2.0)
|
||||
self.declare_parameter("publish_heartbeat", True)
|
||||
|
||||
self._port_path = self.get_parameter("serial_port").value
|
||||
self._baudrate = self.get_parameter("baudrate").value
|
||||
self._read_timeout = self.get_parameter("read_timeout_s").value
|
||||
self._min_range = self.get_parameter("min_range_m").value
|
||||
self._max_range = self.get_parameter("max_range_m").value
|
||||
self._estop_latch = self.get_parameter("estop_latch_s").value
|
||||
self._pub_hb = self.get_parameter("publish_heartbeat").value
|
||||
|
||||
# ── Publishers ────────────────────────────────────────────────────
|
||||
self._range_pub = self.create_publisher(
|
||||
UwbRange, "/uwb/espnow/ranges", _SENSOR_QOS
|
||||
)
|
||||
self._estop_espnow_pub = self.create_publisher(
|
||||
Bool, "/uwb/espnow/estop", _SENSOR_QOS
|
||||
)
|
||||
self._estop_shared_pub = self.create_publisher(
|
||||
Bool, "/saltybot/estop", _SENSOR_QOS
|
||||
)
|
||||
if self._pub_hb:
|
||||
self._hb_pub = self.create_publisher(
|
||||
EspNowHeartbeat, "/uwb/espnow/heartbeat", _SENSOR_QOS
|
||||
)
|
||||
else:
|
||||
self._hb_pub = None
|
||||
|
||||
# ── E-stop latch ──────────────────────────────────────────────────
|
||||
self._estop_last_t: float = 0.0
|
||||
self._estop_active: bool = False
|
||||
self._estop_lock = threading.Lock()
|
||||
self.create_timer(0.1, self._estop_latch_check)
|
||||
|
||||
# ── Serial reader thread ──────────────────────────────────────────
|
||||
self._reader = FrameReader()
|
||||
self._running = True
|
||||
self._ser: serial.Serial | None = None
|
||||
self._serial_thread = threading.Thread(
|
||||
target=self._serial_loop, daemon=True, name="espnow-serial"
|
||||
)
|
||||
self._serial_thread.start()
|
||||
|
||||
self.get_logger().info(
|
||||
f"ESP-NOW relay ready — port={self._port_path} "
|
||||
f"baud={self._baudrate} "
|
||||
f"range=[{self._min_range:.1f}, {self._max_range:.1f}] m"
|
||||
)
|
||||
|
||||
def destroy_node(self) -> None:
|
||||
self._running = False
|
||||
if self._ser and self._ser.is_open:
|
||||
self._ser.close()
|
||||
super().destroy_node()
|
||||
|
||||
# ── Serial read loop ───────────────────────────────────────────────────
|
||||
|
||||
def _serial_loop(self) -> None:
|
||||
while self._running:
|
||||
try:
|
||||
self._ser = serial.Serial(
|
||||
self._port_path,
|
||||
baudrate=self._baudrate,
|
||||
timeout=self._read_timeout,
|
||||
)
|
||||
self.get_logger().info(f"Serial opened: {self._port_path}")
|
||||
self._read_loop()
|
||||
except serial.SerialException as exc:
|
||||
if self._running:
|
||||
self.get_logger().warn(
|
||||
f"Serial error: {exc} — retrying in 2 s",
|
||||
throttle_duration_sec=10.0,
|
||||
)
|
||||
time.sleep(2.0)
|
||||
except Exception as exc:
|
||||
if self._running:
|
||||
self.get_logger().error(f"Unexpected serial error: {exc}")
|
||||
time.sleep(2.0)
|
||||
finally:
|
||||
if self._ser and self._ser.is_open:
|
||||
self._ser.close()
|
||||
|
||||
def _read_loop(self) -> None:
|
||||
while self._running and self._ser and self._ser.is_open:
|
||||
raw = self._ser.read(64)
|
||||
if not raw:
|
||||
continue
|
||||
packets = self._reader.feed(raw)
|
||||
for pkt in packets:
|
||||
self._dispatch(pkt)
|
||||
|
||||
# ── Packet dispatch ────────────────────────────────────────────────────
|
||||
|
||||
def _dispatch(self, pkt) -> None:
|
||||
now = self.get_clock().now().to_msg()
|
||||
hdr = Header()
|
||||
hdr.stamp = now
|
||||
hdr.frame_id = "espnow"
|
||||
|
||||
if pkt.msg_type == MSG_RANGE:
|
||||
range_m = pkt.range_mm / 1000.0
|
||||
if not (self._min_range <= range_m <= self._max_range):
|
||||
return
|
||||
msg = UwbRange()
|
||||
msg.header = hdr
|
||||
msg.anchor_id = pkt.anchor_id
|
||||
msg.range_m = float(range_m)
|
||||
msg.raw_mm = max(0, pkt.range_mm)
|
||||
msg.rssi = float(pkt.rssi_dbm)
|
||||
msg.tag_id = str(pkt.tag_id)
|
||||
self._range_pub.publish(msg)
|
||||
|
||||
elif pkt.msg_type == MSG_ESTOP:
|
||||
active = pkt.estop_active
|
||||
with self._estop_lock:
|
||||
if active:
|
||||
self._estop_last_t = time.monotonic()
|
||||
self._estop_active = True
|
||||
else:
|
||||
# Explicit clear from tag (3× clear packets on release)
|
||||
self._estop_active = False
|
||||
b = Bool()
|
||||
b.data = active
|
||||
self._estop_espnow_pub.publish(b)
|
||||
b2 = Bool()
|
||||
b2.data = self._estop_active
|
||||
self._estop_shared_pub.publish(b2)
|
||||
|
||||
elif pkt.msg_type == MSG_HEARTBEAT and self._hb_pub:
|
||||
msg = EspNowHeartbeat()
|
||||
msg.header = hdr
|
||||
msg.tag_id = pkt.tag_id
|
||||
msg.battery_pct = pkt.battery_pct
|
||||
msg.seq_num = pkt.seq_num
|
||||
msg.timestamp_ms = pkt.timestamp_ms
|
||||
self._hb_pub.publish(msg)
|
||||
|
||||
# ── E-stop latch timer ─────────────────────────────────────────────────
|
||||
|
||||
def _estop_latch_check(self) -> None:
|
||||
"""Clear /saltybot/estop if no ESTOP packet received within latch window."""
|
||||
with self._estop_lock:
|
||||
if self._estop_active:
|
||||
age = time.monotonic() - self._estop_last_t
|
||||
if age > self._estop_latch:
|
||||
self._estop_active = False
|
||||
b = Bool()
|
||||
b.data = False
|
||||
self._estop_shared_pub.publish(b)
|
||||
|
||||
|
||||
def main(args=None) -> None:
|
||||
rclpy.init(args=args)
|
||||
node = EspNowRelayNode()
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.try_shutdown()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
4
jetson/ros2_ws/src/saltybot_uwb_espnow_relay/setup.cfg
Normal file
4
jetson/ros2_ws/src/saltybot_uwb_espnow_relay/setup.cfg
Normal file
@ -0,0 +1,4 @@
|
||||
[develop]
|
||||
script_dir=$base/lib/saltybot_uwb_espnow_relay
|
||||
[install]
|
||||
install_scripts=$base/lib/saltybot_uwb_espnow_relay
|
||||
29
jetson/ros2_ws/src/saltybot_uwb_espnow_relay/setup.py
Normal file
29
jetson/ros2_ws/src/saltybot_uwb_espnow_relay/setup.py
Normal file
@ -0,0 +1,29 @@
|
||||
import os
|
||||
from glob import glob
|
||||
from setuptools import setup
|
||||
|
||||
package_name = "saltybot_uwb_espnow_relay"
|
||||
|
||||
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="ESP-NOW to ROS2 serial relay for SaltyBot UWB tags (Issue #618)",
|
||||
license="Apache-2.0",
|
||||
entry_points={
|
||||
"console_scripts": [
|
||||
"espnow_relay = saltybot_uwb_espnow_relay.relay_node:main",
|
||||
],
|
||||
},
|
||||
)
|
||||
175
jetson/ros2_ws/src/saltybot_uwb_espnow_relay/test/test_packet.py
Normal file
175
jetson/ros2_ws/src/saltybot_uwb_espnow_relay/test/test_packet.py
Normal file
@ -0,0 +1,175 @@
|
||||
"""
|
||||
Unit tests for saltybot_uwb_espnow_relay.packet (Issue #618).
|
||||
No ROS2 or hardware required.
|
||||
"""
|
||||
|
||||
import struct
|
||||
import sys
|
||||
import os
|
||||
|
||||
import pytest
|
||||
|
||||
sys.path.insert(0, os.path.join(os.path.dirname(__file__), ".."))
|
||||
from saltybot_uwb_espnow_relay.packet import (
|
||||
EspNowPacket, FrameReader,
|
||||
MAGIC_0, MAGIC_1, MSG_RANGE, MSG_ESTOP, MSG_HEARTBEAT,
|
||||
PACKET_LEN, _STRUCT,
|
||||
)
|
||||
|
||||
|
||||
# ── Helpers ────────────────────────────────────────────────────────────────
|
||||
|
||||
def _make_raw(
|
||||
tag_id=1, msg_type=MSG_RANGE, anchor_id=0,
|
||||
range_mm=1500, rssi=-75.0, timestamp_ms=12345,
|
||||
battery_pct=80, flags=0, seq_num=42
|
||||
) -> bytes:
|
||||
"""Build a valid 20-byte raw EspNowPacket."""
|
||||
return _STRUCT.pack(
|
||||
MAGIC_0, MAGIC_1,
|
||||
tag_id, msg_type, anchor_id,
|
||||
range_mm, rssi, timestamp_ms,
|
||||
battery_pct, flags, seq_num,
|
||||
)
|
||||
|
||||
|
||||
def _frame(raw: bytes) -> bytes:
|
||||
"""Wrap raw packet in STX+LEN+DATA+CRC framing."""
|
||||
crc = 0
|
||||
for b in raw:
|
||||
crc ^= b
|
||||
return bytes([0x02, len(raw)]) + raw + bytes([crc])
|
||||
|
||||
|
||||
# ── EspNowPacket.from_bytes ────────────────────────────────────────────────
|
||||
|
||||
class TestFromBytes:
|
||||
def test_range_packet_parsed(self):
|
||||
raw = _make_raw(tag_id=1, msg_type=MSG_RANGE, anchor_id=0,
|
||||
range_mm=2345, rssi=-68.5)
|
||||
pkt = EspNowPacket.from_bytes(raw)
|
||||
assert pkt.tag_id == 1
|
||||
assert pkt.msg_type == MSG_RANGE
|
||||
assert pkt.anchor_id == 0
|
||||
assert pkt.range_mm == 2345
|
||||
assert abs(pkt.rssi_dbm - (-68.5)) < 0.01
|
||||
|
||||
def test_estop_active_flag(self):
|
||||
raw = _make_raw(msg_type=MSG_ESTOP, flags=0x01)
|
||||
pkt = EspNowPacket.from_bytes(raw)
|
||||
assert pkt.msg_type == MSG_ESTOP
|
||||
assert pkt.estop_active is True
|
||||
|
||||
def test_estop_cleared_flag(self):
|
||||
raw = _make_raw(msg_type=MSG_ESTOP, flags=0x00)
|
||||
pkt = EspNowPacket.from_bytes(raw)
|
||||
assert pkt.estop_active is False
|
||||
|
||||
def test_heartbeat_parsed(self):
|
||||
raw = _make_raw(msg_type=MSG_HEARTBEAT, battery_pct=55, seq_num=7)
|
||||
pkt = EspNowPacket.from_bytes(raw)
|
||||
assert pkt.msg_type == MSG_HEARTBEAT
|
||||
assert pkt.battery_pct == 55
|
||||
assert pkt.seq_num == 7
|
||||
|
||||
def test_bad_magic_raises(self):
|
||||
raw = bytearray(_make_raw())
|
||||
raw[0] = 0xAA
|
||||
with pytest.raises(ValueError, match="magic"):
|
||||
EspNowPacket.from_bytes(bytes(raw))
|
||||
|
||||
def test_wrong_length_raises(self):
|
||||
with pytest.raises(ValueError):
|
||||
EspNowPacket.from_bytes(b"\x00" * 10)
|
||||
|
||||
def test_negative_range_mm(self):
|
||||
raw = _make_raw(range_mm=-50)
|
||||
pkt = EspNowPacket.from_bytes(raw)
|
||||
assert pkt.range_mm == -50
|
||||
|
||||
def test_battery_unknown(self):
|
||||
raw = _make_raw(battery_pct=0xFF)
|
||||
pkt = EspNowPacket.from_bytes(raw)
|
||||
assert pkt.battery_pct == 255
|
||||
|
||||
|
||||
# ── FrameReader ────────────────────────────────────────────────────────────
|
||||
|
||||
class TestFrameReader:
|
||||
def _make_frame(self, **kwargs) -> bytes:
|
||||
return _frame(_make_raw(**kwargs))
|
||||
|
||||
def test_single_frame_decoded(self):
|
||||
r = FrameReader()
|
||||
frame = self._make_frame(range_mm=3000, anchor_id=1)
|
||||
pkts = r.feed(frame)
|
||||
assert len(pkts) == 1
|
||||
assert pkts[0].range_mm == 3000
|
||||
assert pkts[0].anchor_id == 1
|
||||
|
||||
def test_two_consecutive_frames(self):
|
||||
r = FrameReader()
|
||||
f1 = self._make_frame(seq_num=1)
|
||||
f2 = self._make_frame(seq_num=2)
|
||||
pkts = r.feed(f1 + f2)
|
||||
assert len(pkts) == 2
|
||||
assert pkts[0].seq_num == 1
|
||||
assert pkts[1].seq_num == 2
|
||||
|
||||
def test_garbage_before_stx_skipped(self):
|
||||
r = FrameReader()
|
||||
junk = bytes([0xDE, 0xAD, 0xBE, 0xEF, 0x99])
|
||||
frame = self._make_frame()
|
||||
pkts = r.feed(junk + frame)
|
||||
assert len(pkts) == 1
|
||||
|
||||
def test_bad_crc_dropped(self):
|
||||
r = FrameReader()
|
||||
raw = _make_raw()
|
||||
crc = 0
|
||||
for b in raw:
|
||||
crc ^= b
|
||||
frame = bytes([0x02, 0x14]) + raw + bytes([crc ^ 0xFF]) # corrupt CRC
|
||||
pkts = r.feed(frame)
|
||||
assert len(pkts) == 0
|
||||
|
||||
def test_wrong_len_byte_dropped(self):
|
||||
r = FrameReader()
|
||||
raw = _make_raw()
|
||||
frame = bytes([0x02, 0x10]) + raw # wrong LEN
|
||||
pkts = r.feed(frame)
|
||||
assert len(pkts) == 0
|
||||
|
||||
def test_split_feed_reassembles(self):
|
||||
"""Feed the frame in two chunks — should still parse."""
|
||||
r = FrameReader()
|
||||
frame = self._make_frame(seq_num=99)
|
||||
half = len(frame) // 2
|
||||
pkts1 = r.feed(frame[:half])
|
||||
pkts2 = r.feed(frame[half:])
|
||||
assert len(pkts1) == 0
|
||||
assert len(pkts2) == 1
|
||||
assert pkts2[0].seq_num == 99
|
||||
|
||||
def test_byte_by_byte_feed(self):
|
||||
"""Feed one byte at a time."""
|
||||
r = FrameReader()
|
||||
frame = self._make_frame(tag_id=3)
|
||||
all_pkts = []
|
||||
for b in frame:
|
||||
all_pkts.extend(r.feed(bytes([b])))
|
||||
assert len(all_pkts) == 1
|
||||
assert all_pkts[0].tag_id == 3
|
||||
|
||||
def test_recovery_after_truncated_frame(self):
|
||||
"""Truncated frame followed by valid frame — valid one should parse."""
|
||||
r = FrameReader()
|
||||
truncated = bytes([0x02, 0x14]) + b"\x00" * 10 # no CRC
|
||||
good = self._make_frame(seq_num=77)
|
||||
pkts = r.feed(truncated + good)
|
||||
# Might or might not get 77 depending on CRC collision, but must not crash
|
||||
assert isinstance(pkts, list)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
pytest.main([__file__, "-v"])
|
||||
@ -9,6 +9,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
|
||||
"msg/UwbRange.msg"
|
||||
"msg/UwbRangeArray.msg"
|
||||
"msg/UwbBearing.msg"
|
||||
"msg/EspNowHeartbeat.msg"
|
||||
DEPENDENCIES std_msgs
|
||||
)
|
||||
|
||||
|
||||
10
jetson/ros2_ws/src/saltybot_uwb_msgs/msg/EspNowHeartbeat.msg
Normal file
10
jetson/ros2_ws/src/saltybot_uwb_msgs/msg/EspNowHeartbeat.msg
Normal file
@ -0,0 +1,10 @@
|
||||
# EspNowHeartbeat.msg — heartbeat status from ESP-NOW UWB tag (Issue #618)
|
||||
#
|
||||
# Published by the ESP-NOW relay node on each MSG_HEARTBEAT (0x30) frame.
|
||||
|
||||
std_msgs/Header header
|
||||
|
||||
uint8 tag_id # tag identifier
|
||||
uint8 battery_pct # 0-100, or 255 = unknown
|
||||
uint8 seq_num # rolling sequence number (detect loss)
|
||||
uint32 timestamp_ms # ESP32 millis() at time of transmission
|
||||
705
phone/waypoint_logger.py
Normal file
705
phone/waypoint_logger.py
Normal file
@ -0,0 +1,705 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
waypoint_logger.py — GPS waypoint logger and route planner for SaltyBot (Issue #617)
|
||||
|
||||
Interactive CLI for recording, managing, and publishing GPS waypoints from
|
||||
an Android/Termux phone. Uses termux-location for GPS fixes (same provider
|
||||
as sensor_dashboard.py) and paho-mqtt to publish routes to the broker.
|
||||
|
||||
Features
|
||||
────────
|
||||
• Record GPS waypoint at current position (with name and optional tags)
|
||||
• List all saved waypoints with distance/bearing from previous
|
||||
• Delete a waypoint by index
|
||||
• Clear all waypoints in the active route
|
||||
• Publish the route as a Nav2-compatible waypoint list to MQTT
|
||||
• Subscribe to saltybot/phone/gps for live GPS if sensor_dashboard.py
|
||||
is already running (avoids double GPS call)
|
||||
• Load/save route to JSON file (persistent between sessions)
|
||||
|
||||
MQTT topics
|
||||
───────────
|
||||
Publish: saltybot/phone/route
|
||||
Subscribe: saltybot/phone/gps (live GPS from sensor_dashboard.py)
|
||||
|
||||
Route JSON published to MQTT
|
||||
─────────────────────────────
|
||||
{
|
||||
"route_name": "patrol_loop",
|
||||
"ts": 1710000000.0,
|
||||
"waypoints": [
|
||||
{
|
||||
"index": 0,
|
||||
"name": "start",
|
||||
"tags": ["patrol", "home"],
|
||||
"lat": 45.123, "lon": -73.456, "alt_m": 12.3,
|
||||
"accuracy_m": 2.1,
|
||||
"recorded_at": 1710000000.0
|
||||
}, ...
|
||||
]
|
||||
}
|
||||
|
||||
Nav2 PoseStamped list format (also included under "nav2_poses" key):
|
||||
Each entry has frame_id, position {x,y,z}, orientation {x,y,z,w}
|
||||
using a flat-earth approximation relative to the first waypoint.
|
||||
|
||||
Usage
|
||||
─────
|
||||
python3 phone/waypoint_logger.py [OPTIONS]
|
||||
|
||||
--broker HOST MQTT broker IP (default: 192.168.1.100)
|
||||
--port PORT MQTT port (default: 1883)
|
||||
--file PATH Route JSON file (default: ~/saltybot_route.json)
|
||||
--route NAME Route name (default: route)
|
||||
--provider PROV GPS provider: gps|network|passive (default: gps)
|
||||
--live-gps Subscribe to sensor_dashboard GPS instead of polling
|
||||
--no-mqtt Disable MQTT (log-only mode)
|
||||
--debug Verbose logging
|
||||
|
||||
Dependencies (Termux)
|
||||
──────────────────────
|
||||
pkg install termux-api python
|
||||
pip install paho-mqtt
|
||||
"""
|
||||
|
||||
import argparse
|
||||
import json
|
||||
import logging
|
||||
import math
|
||||
import os
|
||||
import subprocess
|
||||
import sys
|
||||
import threading
|
||||
import time
|
||||
from dataclasses import dataclass, field, asdict
|
||||
from datetime import datetime, timezone
|
||||
from typing import Optional
|
||||
|
||||
try:
|
||||
import paho.mqtt.client as mqtt
|
||||
MQTT_AVAILABLE = True
|
||||
except ImportError:
|
||||
MQTT_AVAILABLE = False
|
||||
|
||||
# ── Constants ─────────────────────────────────────────────────────────────────
|
||||
|
||||
TOPIC_ROUTE = "saltybot/phone/route"
|
||||
TOPIC_GPS_LIVE = "saltybot/phone/gps"
|
||||
|
||||
EARTH_RADIUS_M = 6_371_000.0 # mean Earth radius in metres
|
||||
|
||||
DEFAULT_FILE = os.path.expanduser("~/saltybot_route.json")
|
||||
|
||||
# ── Data model ────────────────────────────────────────────────────────────────
|
||||
|
||||
@dataclass
|
||||
class Waypoint:
|
||||
index: int
|
||||
name: str
|
||||
lat: float
|
||||
lon: float
|
||||
alt_m: float = 0.0
|
||||
accuracy_m: float = -1.0
|
||||
tags: list[str] = field(default_factory=list)
|
||||
recorded_at: float = field(default_factory=time.time)
|
||||
|
||||
def to_dict(self) -> dict:
|
||||
return asdict(self)
|
||||
|
||||
@staticmethod
|
||||
def from_dict(d: dict) -> "Waypoint":
|
||||
return Waypoint(
|
||||
index = int(d.get("index", 0)),
|
||||
name = str(d.get("name", "")),
|
||||
lat = float(d["lat"]),
|
||||
lon = float(d["lon"]),
|
||||
alt_m = float(d.get("alt_m", 0.0)),
|
||||
accuracy_m = float(d.get("accuracy_m", -1.0)),
|
||||
tags = list(d.get("tags", [])),
|
||||
recorded_at = float(d.get("recorded_at", 0.0)),
|
||||
)
|
||||
|
||||
|
||||
# ── Geo maths ─────────────────────────────────────────────────────────────────
|
||||
|
||||
def haversine_m(lat1: float, lon1: float, lat2: float, lon2: float) -> float:
|
||||
"""Great-circle distance between two WGS84 points (metres)."""
|
||||
φ1, φ2 = math.radians(lat1), math.radians(lat2)
|
||||
Δφ = math.radians(lat2 - lat1)
|
||||
Δλ = math.radians(lon2 - lon1)
|
||||
a = math.sin(Δφ / 2) ** 2 + math.cos(φ1) * math.cos(φ2) * math.sin(Δλ / 2) ** 2
|
||||
return 2 * EARTH_RADIUS_M * math.asin(math.sqrt(a))
|
||||
|
||||
|
||||
def bearing_deg(lat1: float, lon1: float, lat2: float, lon2: float) -> float:
|
||||
"""Initial bearing from point-1 to point-2 (degrees, 0=N, 90=E)."""
|
||||
φ1, φ2 = math.radians(lat1), math.radians(lat2)
|
||||
Δλ = math.radians(lon2 - lon1)
|
||||
x = math.sin(Δλ) * math.cos(φ2)
|
||||
y = math.cos(φ1) * math.sin(φ2) - math.sin(φ1) * math.cos(φ2) * math.cos(Δλ)
|
||||
return (math.degrees(math.atan2(x, y)) + 360) % 360
|
||||
|
||||
|
||||
def _compass(deg: float) -> str:
|
||||
"""Convert bearing degrees to 8-point compass label."""
|
||||
dirs = ["N", "NE", "E", "SE", "S", "SW", "W", "NW"]
|
||||
return dirs[round(deg / 45) % 8]
|
||||
|
||||
|
||||
def flat_earth_xy(origin_lat: float, origin_lon: float,
|
||||
lat: float, lon: float) -> tuple[float, float]:
|
||||
"""
|
||||
Simple flat-earth approximation — metres east/north from origin.
|
||||
Accurate within ~1% for distances < 100 km.
|
||||
"""
|
||||
lat_rad = math.radians((origin_lat + lat) / 2.0)
|
||||
x = math.radians(lon - origin_lon) * EARTH_RADIUS_M * math.cos(lat_rad)
|
||||
y = math.radians(lat - origin_lat) * EARTH_RADIUS_M
|
||||
return x, y
|
||||
|
||||
|
||||
# ── GPS acquisition ───────────────────────────────────────────────────────────
|
||||
|
||||
def get_gps_fix(provider: str = "gps", timeout: float = 12.0) -> Optional[dict]:
|
||||
"""
|
||||
Acquire a GPS fix via termux-location.
|
||||
Falls back to 'network' provider if 'gps' times out.
|
||||
Returns a dict with lat, lon, alt_m, accuracy_m or None on failure.
|
||||
"""
|
||||
def _try(prov: str, t: float) -> Optional[dict]:
|
||||
try:
|
||||
r = subprocess.run(
|
||||
["termux-location", "-p", prov, "-r", "once"],
|
||||
capture_output=True, text=True, timeout=t,
|
||||
)
|
||||
if r.returncode != 0 or not r.stdout.strip():
|
||||
return None
|
||||
raw = json.loads(r.stdout)
|
||||
lat = float(raw.get("latitude", 0.0))
|
||||
lon = float(raw.get("longitude", 0.0))
|
||||
if lat == 0.0 and lon == 0.0:
|
||||
return None
|
||||
return {
|
||||
"lat": lat,
|
||||
"lon": lon,
|
||||
"alt_m": float(raw.get("altitude", 0.0)),
|
||||
"accuracy_m": float(raw.get("accuracy", -1.0)),
|
||||
"provider": prov,
|
||||
}
|
||||
except (subprocess.TimeoutExpired, json.JSONDecodeError,
|
||||
FileNotFoundError, KeyError):
|
||||
return None
|
||||
|
||||
fix = _try(provider, timeout)
|
||||
if fix is None and provider == "gps":
|
||||
logging.debug("GPS timeout — retrying with network provider")
|
||||
fix = _try("network", 6.0)
|
||||
return fix
|
||||
|
||||
|
||||
# ── Live GPS subscriber (optional, uses sensor_dashboard.py stream) ──────────
|
||||
|
||||
class LiveGPS:
|
||||
"""
|
||||
Subscribe to saltybot/phone/gps on MQTT and cache the latest fix.
|
||||
Used when sensor_dashboard.py is already running to avoid double GPS calls.
|
||||
"""
|
||||
|
||||
def __init__(self, broker: str, port: int):
|
||||
self._lock = threading.Lock()
|
||||
self._latest: Optional[dict] = None
|
||||
|
||||
if not MQTT_AVAILABLE:
|
||||
return
|
||||
|
||||
self._client = mqtt.Client(client_id="wp-live-gps", clean_session=True)
|
||||
self._client.on_connect = lambda c, u, f, rc: c.subscribe(TOPIC_GPS_LIVE)
|
||||
self._client.on_message = self._on_msg
|
||||
try:
|
||||
self._client.connect(broker, port, keepalive=30)
|
||||
self._client.loop_start()
|
||||
except Exception as e:
|
||||
logging.warning("LiveGPS connect failed: %s", e)
|
||||
|
||||
def _on_msg(self, client, userdata, message) -> None:
|
||||
try:
|
||||
data = json.loads(message.payload)
|
||||
with self._lock:
|
||||
self._latest = data
|
||||
except json.JSONDecodeError:
|
||||
pass
|
||||
|
||||
def get(self, max_age_s: float = 3.0) -> Optional[dict]:
|
||||
with self._lock:
|
||||
d = self._latest
|
||||
if d is None:
|
||||
return None
|
||||
if time.time() - d.get("ts", 0.0) > max_age_s:
|
||||
return None
|
||||
return {
|
||||
"lat": d.get("lat", 0.0),
|
||||
"lon": d.get("lon", 0.0),
|
||||
"alt_m": d.get("alt_m", 0.0),
|
||||
"accuracy_m": d.get("accuracy_m", -1.0),
|
||||
"provider": d.get("provider", "live"),
|
||||
}
|
||||
|
||||
def stop(self) -> None:
|
||||
if MQTT_AVAILABLE and hasattr(self, "_client"):
|
||||
self._client.loop_stop()
|
||||
self._client.disconnect()
|
||||
|
||||
|
||||
# ── Route ─────────────────────────────────────────────────────────────────────
|
||||
|
||||
class Route:
|
||||
"""Ordered list of waypoints with persistence and Nav2 export."""
|
||||
|
||||
def __init__(self, name: str, path: str):
|
||||
self.name = name
|
||||
self._path = path
|
||||
self._waypoints: list[Waypoint] = []
|
||||
self._load()
|
||||
|
||||
# ── Persistence ───────────────────────────────────────────────────────────
|
||||
|
||||
def _load(self) -> None:
|
||||
if not os.path.exists(self._path):
|
||||
return
|
||||
try:
|
||||
with open(self._path, "r") as f:
|
||||
data = json.load(f)
|
||||
self.name = data.get("route_name", self.name)
|
||||
self._waypoints = [Waypoint.from_dict(w) for w in data.get("waypoints", [])]
|
||||
# Re-index to keep indices contiguous after deletes
|
||||
for i, wp in enumerate(self._waypoints):
|
||||
wp.index = i
|
||||
logging.info("Loaded %d waypoints from %s", len(self._waypoints), self._path)
|
||||
except (json.JSONDecodeError, KeyError, ValueError) as e:
|
||||
logging.warning("Could not load route file: %s", e)
|
||||
|
||||
def save(self) -> None:
|
||||
with open(self._path, "w") as f:
|
||||
json.dump(self.to_dict(), f, indent=2)
|
||||
logging.debug("Saved route to %s", self._path)
|
||||
|
||||
# ── Waypoint operations ───────────────────────────────────────────────────
|
||||
|
||||
def add(self, name: str, lat: float, lon: float, alt_m: float,
|
||||
accuracy_m: float, tags: list[str]) -> Waypoint:
|
||||
wp = Waypoint(
|
||||
index = len(self._waypoints),
|
||||
name = name,
|
||||
lat = lat,
|
||||
lon = lon,
|
||||
alt_m = alt_m,
|
||||
accuracy_m = accuracy_m,
|
||||
tags = tags,
|
||||
)
|
||||
self._waypoints.append(wp)
|
||||
self.save()
|
||||
return wp
|
||||
|
||||
def delete(self, index: int) -> bool:
|
||||
if not (0 <= index < len(self._waypoints)):
|
||||
return False
|
||||
self._waypoints.pop(index)
|
||||
for i, wp in enumerate(self._waypoints):
|
||||
wp.index = i
|
||||
self.save()
|
||||
return True
|
||||
|
||||
def clear(self) -> int:
|
||||
n = len(self._waypoints)
|
||||
self._waypoints.clear()
|
||||
self.save()
|
||||
return n
|
||||
|
||||
def __len__(self) -> int:
|
||||
return len(self._waypoints)
|
||||
|
||||
def __iter__(self):
|
||||
return iter(self._waypoints)
|
||||
|
||||
def get(self, index: int) -> Optional[Waypoint]:
|
||||
if 0 <= index < len(self._waypoints):
|
||||
return self._waypoints[index]
|
||||
return None
|
||||
|
||||
# ── Serialisation ─────────────────────────────────────────────────────────
|
||||
|
||||
def to_dict(self) -> dict:
|
||||
return {
|
||||
"route_name": self.name,
|
||||
"ts": time.time(),
|
||||
"waypoints": [wp.to_dict() for wp in self._waypoints],
|
||||
"nav2_poses": self._nav2_poses(),
|
||||
}
|
||||
|
||||
def _nav2_poses(self) -> list[dict]:
|
||||
"""
|
||||
Flat-earth Nav2 PoseStamped list (x east, y north) relative to
|
||||
the first waypoint. Yaw faces the next waypoint; last faces prev.
|
||||
"""
|
||||
if not self._waypoints:
|
||||
return []
|
||||
|
||||
origin = self._waypoints[0]
|
||||
poses = []
|
||||
|
||||
for i, wp in enumerate(self._waypoints):
|
||||
x, y = flat_earth_xy(origin.lat, origin.lon, wp.lat, wp.lon)
|
||||
|
||||
# Compute yaw: face toward next waypoint (or prev for last)
|
||||
if i + 1 < len(self._waypoints):
|
||||
nxt = self._waypoints[i + 1]
|
||||
nx, ny = flat_earth_xy(origin.lat, origin.lon, nxt.lat, nxt.lon)
|
||||
yaw = math.atan2(ny - y, nx - x)
|
||||
elif i > 0:
|
||||
prv = self._waypoints[i - 1]
|
||||
px, py = flat_earth_xy(origin.lat, origin.lon, prv.lat, prv.lon)
|
||||
yaw = math.atan2(y - py, x - px)
|
||||
else:
|
||||
yaw = 0.0
|
||||
|
||||
poses.append({
|
||||
"frame_id": "map",
|
||||
"position": {"x": round(x, 3), "y": round(y, 3), "z": round(wp.alt_m, 3)},
|
||||
"orientation": {
|
||||
"x": 0.0, "y": 0.0,
|
||||
"z": round(math.sin(yaw / 2), 6),
|
||||
"w": round(math.cos(yaw / 2), 6),
|
||||
},
|
||||
"waypoint_name": wp.name,
|
||||
})
|
||||
|
||||
return poses
|
||||
|
||||
|
||||
# ── MQTT publisher ────────────────────────────────────────────────────────────
|
||||
|
||||
class MQTTClient:
|
||||
"""Simple synchronous paho wrapper for route publishing."""
|
||||
|
||||
def __init__(self, broker: str, port: int):
|
||||
self._broker = broker
|
||||
self._port = port
|
||||
self._client = None
|
||||
self._connected = False
|
||||
|
||||
if not MQTT_AVAILABLE:
|
||||
return
|
||||
self._client = mqtt.Client(client_id="saltybot-waypoint-logger", clean_session=True)
|
||||
self._client.on_connect = self._on_connect
|
||||
self._client.on_disconnect = self._on_disconnect
|
||||
self._client.reconnect_delay_set(min_delay=3, max_delay=30)
|
||||
try:
|
||||
self._client.connect_async(broker, port, keepalive=60)
|
||||
self._client.loop_start()
|
||||
except Exception as e:
|
||||
logging.warning("MQTT connect failed: %s", e)
|
||||
|
||||
def _on_connect(self, c, u, f, rc) -> None:
|
||||
self._connected = (rc == 0)
|
||||
if rc == 0:
|
||||
logging.debug("MQTT connected")
|
||||
|
||||
def _on_disconnect(self, c, u, rc) -> None:
|
||||
self._connected = False
|
||||
|
||||
def publish(self, topic: str, payload: dict, qos: int = 1) -> bool:
|
||||
if self._client is None:
|
||||
return False
|
||||
# Wait briefly for connection
|
||||
deadline = time.monotonic() + 5.0
|
||||
while not self._connected and time.monotonic() < deadline:
|
||||
time.sleep(0.1)
|
||||
if not self._connected:
|
||||
return False
|
||||
info = self._client.publish(
|
||||
topic, json.dumps(payload, separators=(",", ":")), qos=qos, retain=True
|
||||
)
|
||||
return info.rc == 0
|
||||
|
||||
def stop(self) -> None:
|
||||
if self._client:
|
||||
self._client.loop_stop()
|
||||
self._client.disconnect()
|
||||
|
||||
|
||||
# ── CLI helpers ───────────────────────────────────────────────────────────────
|
||||
|
||||
def _fmt_ts(epoch: float) -> str:
|
||||
return datetime.fromtimestamp(epoch, tz=timezone.utc).strftime("%Y-%m-%d %H:%M:%S UTC")
|
||||
|
||||
|
||||
def _fmt_dist(metres: float) -> str:
|
||||
if metres < 1000:
|
||||
return f"{metres:.1f} m"
|
||||
return f"{metres / 1000:.3f} km"
|
||||
|
||||
|
||||
def _print_waypoints(route: Route) -> None:
|
||||
if not len(route):
|
||||
print(" (no waypoints)")
|
||||
return
|
||||
print(f"\n {'#':>3} {'Name':<18} {'Lat':>10} {'Lon':>11} {'Alt':>7} {'Acc':>7} {'Tags'}")
|
||||
print(" " + "─" * 75)
|
||||
prev = None
|
||||
for wp in route:
|
||||
dist_str = ""
|
||||
brg_str = ""
|
||||
if prev is not None:
|
||||
d = haversine_m(prev.lat, prev.lon, wp.lat, wp.lon)
|
||||
b = bearing_deg(prev.lat, prev.lon, wp.lat, wp.lon)
|
||||
dist_str = f" ↑ {_fmt_dist(d)} {_compass(b)}"
|
||||
tags = ",".join(wp.tags) if wp.tags else "—"
|
||||
print(f" {wp.index:>3} {wp.name:<18} {wp.lat:>10.6f} {wp.lon:>11.6f}"
|
||||
f" {wp.alt_m:>6.1f}m "
|
||||
f"{wp.accuracy_m:>5.1f}m {tags}{dist_str}")
|
||||
prev = wp
|
||||
print()
|
||||
|
||||
|
||||
def _acquire_fix(args, live_gps: Optional[LiveGPS]) -> Optional[dict]:
|
||||
"""Get a GPS fix — from live MQTT stream if available, else termux-location."""
|
||||
if live_gps is not None:
|
||||
fix = live_gps.get(max_age_s=3.0)
|
||||
if fix is not None:
|
||||
return fix
|
||||
print(" (live GPS stale — falling back to termux-location)")
|
||||
print(" Acquiring GPS fix...", end="", flush=True)
|
||||
fix = get_gps_fix(provider=args.provider)
|
||||
if fix:
|
||||
print(f" {fix['lat']:.6f}, {fix['lon']:.6f} ±{fix['accuracy_m']:.1f}m")
|
||||
else:
|
||||
print(" FAILED")
|
||||
return fix
|
||||
|
||||
|
||||
# ── Interactive menu ──────────────────────────────────────────────────────────
|
||||
|
||||
def _menu_record(route: Route, args, live_gps: Optional[LiveGPS]) -> None:
|
||||
fix = _acquire_fix(args, live_gps)
|
||||
if fix is None:
|
||||
print(" Could not get GPS fix.")
|
||||
return
|
||||
|
||||
default_name = f"wp{len(route)}"
|
||||
name = input(f" Waypoint name [{default_name}]: ").strip() or default_name
|
||||
tags_raw = input(" Tags (comma-separated, or Enter to skip): ").strip()
|
||||
tags = [t.strip() for t in tags_raw.split(",") if t.strip()] if tags_raw else []
|
||||
|
||||
wp = route.add(name, fix["lat"], fix["lon"], fix["alt_m"], fix["accuracy_m"], tags)
|
||||
print(f" ✓ Recorded #{wp.index}: {wp.name} ({wp.lat:.6f}, {wp.lon:.6f})")
|
||||
|
||||
|
||||
def _menu_list(route: Route) -> None:
|
||||
print(f"\n Route: '{route.name}' ({len(route)} waypoints)")
|
||||
_print_waypoints(route)
|
||||
|
||||
if len(route) >= 2:
|
||||
wps = list(route)
|
||||
total = sum(
|
||||
haversine_m(wps[i].lat, wps[i].lon, wps[i+1].lat, wps[i+1].lon)
|
||||
for i in range(len(wps) - 1)
|
||||
)
|
||||
print(f" Total route distance: {_fmt_dist(total)}")
|
||||
|
||||
|
||||
def _menu_delete(route: Route) -> None:
|
||||
if not len(route):
|
||||
print(" No waypoints to delete.")
|
||||
return
|
||||
_print_waypoints(route)
|
||||
raw = input(" Enter index to delete (or Enter to cancel): ").strip()
|
||||
if not raw:
|
||||
return
|
||||
try:
|
||||
idx = int(raw)
|
||||
except ValueError:
|
||||
print(" Invalid index.")
|
||||
return
|
||||
wp = route.get(idx)
|
||||
if wp is None:
|
||||
print(f" No waypoint at index {idx}.")
|
||||
return
|
||||
confirm = input(f" Delete '{wp.name}'? [y/N]: ").strip().lower()
|
||||
if confirm == "y":
|
||||
route.delete(idx)
|
||||
print(f" ✓ Deleted '{wp.name}'.")
|
||||
else:
|
||||
print(" Cancelled.")
|
||||
|
||||
|
||||
def _menu_publish(route: Route, mqtt_client: Optional[MQTTClient]) -> None:
|
||||
if not len(route):
|
||||
print(" No waypoints to publish.")
|
||||
return
|
||||
|
||||
payload = route.to_dict()
|
||||
print(f"\n Publishing {len(route)} waypoints to {TOPIC_ROUTE}...")
|
||||
|
||||
if mqtt_client is None:
|
||||
print(" (MQTT disabled — printing payload)")
|
||||
print(json.dumps(payload, indent=2))
|
||||
return
|
||||
|
||||
ok = mqtt_client.publish(TOPIC_ROUTE, payload, qos=1)
|
||||
if ok:
|
||||
print(f" ✓ Published to {TOPIC_ROUTE} (retained=true)")
|
||||
else:
|
||||
print(" ✗ MQTT publish failed — check broker connection.")
|
||||
|
||||
# Also print Nav2 summary
|
||||
poses = payload.get("nav2_poses", [])
|
||||
if poses:
|
||||
print(f"\n Nav2 poses ({len(poses)}):")
|
||||
for p in poses:
|
||||
pos = p["position"]
|
||||
print(f" {p['waypoint_name']:<18} x={pos['x']:>8.2f}m y={pos['y']:>8.2f}m")
|
||||
|
||||
|
||||
def _menu_clear(route: Route) -> None:
|
||||
if not len(route):
|
||||
print(" Route is already empty.")
|
||||
return
|
||||
confirm = input(f" Clear all {len(route)} waypoints? [y/N]: ").strip().lower()
|
||||
if confirm == "y":
|
||||
n = route.clear()
|
||||
print(f" ✓ Cleared {n} waypoints.")
|
||||
else:
|
||||
print(" Cancelled.")
|
||||
|
||||
|
||||
def _menu_rename(route: Route) -> None:
|
||||
new_name = input(f" New route name [{route.name}]: ").strip()
|
||||
if new_name:
|
||||
route.name = new_name
|
||||
route.save()
|
||||
print(f" ✓ Route renamed to '{route.name}'.")
|
||||
|
||||
|
||||
def _menu_info(route: Route) -> None:
|
||||
print(f"\n Route file: {route._path}")
|
||||
print(f" Route name: {route.name}")
|
||||
print(f" Waypoints: {len(route)}")
|
||||
if len(route):
|
||||
wps = list(route)
|
||||
print(f" First wp: {wps[0].name} ({_fmt_ts(wps[0].recorded_at)})")
|
||||
print(f" Last wp: {wps[-1].name} ({_fmt_ts(wps[-1].recorded_at)})")
|
||||
if len(route) >= 2:
|
||||
total = sum(
|
||||
haversine_m(wps[i].lat, wps[i].lon, wps[i+1].lat, wps[i+1].lon)
|
||||
for i in range(len(wps) - 1)
|
||||
)
|
||||
print(f" Total dist: {_fmt_dist(total)}")
|
||||
print()
|
||||
|
||||
|
||||
# ── Main loop ─────────────────────────────────────────────────────────────────
|
||||
|
||||
MENU = """
|
||||
┌─ SaltyBot Waypoint Logger ──────────────────────────┐
|
||||
│ r Record waypoint at current GPS position │
|
||||
│ l List all waypoints │
|
||||
│ d Delete a waypoint │
|
||||
│ p Publish route to MQTT │
|
||||
│ c Clear all waypoints │
|
||||
│ n Rename route │
|
||||
│ i Route info │
|
||||
│ q Quit │
|
||||
└─────────────────────────────────────────────────────┘
|
||||
"""
|
||||
|
||||
|
||||
def main() -> None:
|
||||
parser = argparse.ArgumentParser(
|
||||
description="SaltyBot GPS waypoint logger (Issue #617)"
|
||||
)
|
||||
parser.add_argument("--broker", default="192.168.1.100",
|
||||
help="MQTT broker IP (default: 192.168.1.100)")
|
||||
parser.add_argument("--port", type=int, default=1883,
|
||||
help="MQTT port (default: 1883)")
|
||||
parser.add_argument("--file", default=DEFAULT_FILE,
|
||||
help=f"Route JSON file (default: {DEFAULT_FILE})")
|
||||
parser.add_argument("--route", default="route",
|
||||
help="Route name (default: route)")
|
||||
parser.add_argument("--provider", default="gps",
|
||||
choices=["gps", "network", "passive"],
|
||||
help="GPS provider (default: gps)")
|
||||
parser.add_argument("--live-gps", action="store_true",
|
||||
help="Subscribe to saltybot/phone/gps for live GPS")
|
||||
parser.add_argument("--no-mqtt", action="store_true",
|
||||
help="Disable MQTT (log-only mode)")
|
||||
parser.add_argument("--debug", action="store_true",
|
||||
help="Verbose logging")
|
||||
args = parser.parse_args()
|
||||
|
||||
logging.basicConfig(
|
||||
level=logging.DEBUG if args.debug else logging.WARNING,
|
||||
format="%(asctime)s [%(levelname)s] %(message)s",
|
||||
)
|
||||
|
||||
# Load or create route
|
||||
route = Route(args.route, args.file)
|
||||
|
||||
# MQTT client
|
||||
mqtt_client: Optional[MQTTClient] = None
|
||||
if not args.no_mqtt:
|
||||
if MQTT_AVAILABLE:
|
||||
mqtt_client = MQTTClient(args.broker, args.port)
|
||||
else:
|
||||
print("Warning: paho-mqtt not installed — MQTT disabled. Run: pip install paho-mqtt")
|
||||
|
||||
# Optional live GPS subscription
|
||||
live_gps: Optional[LiveGPS] = None
|
||||
if args.live_gps and MQTT_AVAILABLE:
|
||||
live_gps = LiveGPS(args.broker, args.port)
|
||||
print(f" Subscribed to live GPS on {TOPIC_GPS_LIVE}")
|
||||
|
||||
print(MENU)
|
||||
print(f" Route: '{route.name}' | {len(route)} waypoints loaded | file: {args.file}")
|
||||
print()
|
||||
|
||||
try:
|
||||
while True:
|
||||
try:
|
||||
choice = input(" > ").strip().lower()
|
||||
except EOFError:
|
||||
break
|
||||
|
||||
if choice in ("q", "quit", "exit"):
|
||||
break
|
||||
elif choice in ("r", "record"):
|
||||
_menu_record(route, args, live_gps)
|
||||
elif choice in ("l", "list"):
|
||||
_menu_list(route)
|
||||
elif choice in ("d", "delete"):
|
||||
_menu_delete(route)
|
||||
elif choice in ("p", "publish"):
|
||||
_menu_publish(route, mqtt_client)
|
||||
elif choice in ("c", "clear"):
|
||||
_menu_clear(route)
|
||||
elif choice in ("n", "rename"):
|
||||
_menu_rename(route)
|
||||
elif choice in ("i", "info"):
|
||||
_menu_info(route)
|
||||
elif choice == "":
|
||||
pass
|
||||
else:
|
||||
print(" Unknown command. Type r/l/d/p/c/n/i/q.")
|
||||
|
||||
except KeyboardInterrupt:
|
||||
print("\n Interrupted.")
|
||||
finally:
|
||||
if live_gps:
|
||||
live_gps.stop()
|
||||
if mqtt_client:
|
||||
mqtt_client.stop()
|
||||
|
||||
print(" Bye.")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
24
src/jlink.c
24
src/jlink.c
@ -618,3 +618,27 @@ void jlink_send_can_stats(const jlink_tlm_can_stats_t *tlm)
|
||||
|
||||
jlink_tx_locked(frame, sizeof(frame));
|
||||
}
|
||||
|
||||
/* ---- jlink_send_lvc_tlm() -- Issue #613 ---- */
|
||||
void jlink_send_lvc_tlm(const jlink_tlm_lvc_t *tlm)
|
||||
{
|
||||
/*
|
||||
* Frame: [STX][LEN][0x8B][4 bytes LVC][CRC_hi][CRC_lo][ETX]
|
||||
* LEN = 1 + 4 = 5; total = 10 bytes
|
||||
*/
|
||||
static uint8_t frame[10];
|
||||
const uint8_t plen = (uint8_t)sizeof(jlink_tlm_lvc_t); /* 4 */
|
||||
const uint8_t len = 1u + plen; /* 5 */
|
||||
|
||||
frame[0] = JLINK_STX;
|
||||
frame[1] = len;
|
||||
frame[2] = JLINK_TLM_LVC;
|
||||
memcpy(&frame[3], tlm, plen);
|
||||
|
||||
uint16_t crc = crc16_xmodem(&frame[2], len);
|
||||
frame[3 + plen] = (uint8_t)(crc >> 8);
|
||||
frame[3 + plen + 1] = (uint8_t)(crc & 0xFFu);
|
||||
frame[3 + plen + 2] = JLINK_ETX;
|
||||
|
||||
jlink_tx_locked(frame, sizeof(frame));
|
||||
}
|
||||
|
||||
144
src/lvc.c
Normal file
144
src/lvc.c
Normal file
@ -0,0 +1,144 @@
|
||||
/*
|
||||
* lvc.c -- Low Voltage Cutoff (LVC) protection (Issue #613)
|
||||
*
|
||||
* State machine:
|
||||
* NORMAL -> WARNING when vbat < LVC_WARNING_MV
|
||||
* WARNING -> CRITICAL when vbat < LVC_CRITICAL_MV
|
||||
* CRITICAL -> CUTOFF when vbat < LVC_CUTOFF_MV
|
||||
*
|
||||
* Recovery: step down severity only when voltage exceeds the threshold
|
||||
* by LVC_HYSTERESIS_MV. CUTOFF is latched for the remainder of the session.
|
||||
*
|
||||
* Buzzer alerts:
|
||||
* WARNING -- MELODY_LOW_BATTERY once, then every 30 s
|
||||
* CRITICAL -- MELODY_LOW_BATTERY x2, then every 10 s
|
||||
* CUTOFF -- MELODY_ERROR (one-shot; motor disable handled by main.c)
|
||||
*/
|
||||
|
||||
#include "lvc.h"
|
||||
#include "buzzer.h"
|
||||
#include "config.h"
|
||||
|
||||
/* Periodic buzzer reminder intervals */
|
||||
#define LVC_WARN_INTERVAL_MS 30000u /* 30 s */
|
||||
#define LVC_CRIT_INTERVAL_MS 10000u /* 10 s */
|
||||
|
||||
static LvcState s_state = LVC_NORMAL;
|
||||
static bool s_cutoff_latched = false;
|
||||
static uint32_t s_buzzer_tick = 0u;
|
||||
|
||||
/* ---- lvc_init() ---- */
|
||||
void lvc_init(void)
|
||||
{
|
||||
s_state = LVC_NORMAL;
|
||||
s_cutoff_latched = false;
|
||||
s_buzzer_tick = 0u;
|
||||
}
|
||||
|
||||
/* ---- lvc_tick() ---- */
|
||||
void lvc_tick(uint32_t now_ms, uint32_t vbat_mv)
|
||||
{
|
||||
if (vbat_mv == 0u) {
|
||||
return; /* ADC not ready; hold current state */
|
||||
}
|
||||
|
||||
/* Determine new state from raw voltage */
|
||||
LvcState new_state;
|
||||
if (vbat_mv < LVC_CUTOFF_MV) {
|
||||
new_state = LVC_CUTOFF;
|
||||
} else if (vbat_mv < LVC_CRITICAL_MV) {
|
||||
new_state = LVC_CRITICAL;
|
||||
} else if (vbat_mv < LVC_WARNING_MV) {
|
||||
new_state = LVC_WARNING;
|
||||
} else {
|
||||
new_state = LVC_NORMAL;
|
||||
}
|
||||
|
||||
/* Hysteresis on recovery: only decrease severity when voltage exceeds
|
||||
* the threshold by LVC_HYSTERESIS_MV to prevent rapid toggling. */
|
||||
if (new_state < s_state) {
|
||||
LvcState recovered;
|
||||
if (vbat_mv >= LVC_CUTOFF_MV + LVC_HYSTERESIS_MV) {
|
||||
recovered = LVC_CRITICAL;
|
||||
} else if (vbat_mv >= LVC_CRITICAL_MV + LVC_HYSTERESIS_MV) {
|
||||
recovered = LVC_WARNING;
|
||||
} else if (vbat_mv >= LVC_WARNING_MV + LVC_HYSTERESIS_MV) {
|
||||
recovered = LVC_NORMAL;
|
||||
} else {
|
||||
recovered = s_state; /* insufficient margin; stay at current level */
|
||||
}
|
||||
new_state = recovered;
|
||||
}
|
||||
|
||||
/* CUTOFF latch: once triggered, only a reboot clears it */
|
||||
if (s_cutoff_latched) {
|
||||
new_state = LVC_CUTOFF;
|
||||
}
|
||||
if (new_state == LVC_CUTOFF) {
|
||||
s_cutoff_latched = true;
|
||||
}
|
||||
|
||||
/* Buzzer alerts */
|
||||
bool state_changed = (new_state != s_state);
|
||||
s_state = new_state;
|
||||
|
||||
switch (s_state) {
|
||||
case LVC_WARNING:
|
||||
if (state_changed ||
|
||||
(now_ms - s_buzzer_tick) >= LVC_WARN_INTERVAL_MS) {
|
||||
s_buzzer_tick = now_ms;
|
||||
buzzer_play_melody(MELODY_LOW_BATTERY);
|
||||
}
|
||||
break;
|
||||
|
||||
case LVC_CRITICAL:
|
||||
if (state_changed ||
|
||||
(now_ms - s_buzzer_tick) >= LVC_CRIT_INTERVAL_MS) {
|
||||
s_buzzer_tick = now_ms;
|
||||
/* Double alert to distinguish critical from warning */
|
||||
buzzer_play_melody(MELODY_LOW_BATTERY);
|
||||
buzzer_play_melody(MELODY_LOW_BATTERY);
|
||||
}
|
||||
break;
|
||||
|
||||
case LVC_CUTOFF:
|
||||
if (state_changed) {
|
||||
/* One-shot alarm; motors disabled by main.c */
|
||||
buzzer_play_melody(MELODY_ERROR);
|
||||
}
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
/* ---- lvc_get_state() ---- */
|
||||
LvcState lvc_get_state(void)
|
||||
{
|
||||
return s_state;
|
||||
}
|
||||
|
||||
/* ---- lvc_get_power_scale() ---- */
|
||||
/*
|
||||
* Returns the motor power scale factor (0-100):
|
||||
* NORMAL / WARNING : 100% -- no reduction
|
||||
* CRITICAL : 50% -- halve motor commands
|
||||
* CUTOFF : 0% -- all commands zeroed
|
||||
*/
|
||||
uint8_t lvc_get_power_scale(void)
|
||||
{
|
||||
switch (s_state) {
|
||||
case LVC_NORMAL: /* fall-through */
|
||||
case LVC_WARNING: return 100u;
|
||||
case LVC_CRITICAL: return 50u;
|
||||
case LVC_CUTOFF: return 0u;
|
||||
default: return 100u;
|
||||
}
|
||||
}
|
||||
|
||||
/* ---- lvc_is_cutoff() ---- */
|
||||
bool lvc_is_cutoff(void)
|
||||
{
|
||||
return s_cutoff_latched;
|
||||
}
|
||||
40
src/main.c
40
src/main.c
@ -35,6 +35,7 @@
|
||||
#include "can_driver.h"
|
||||
#include "servo_bus.h"
|
||||
#include "gimbal.h"
|
||||
#include "lvc.h"
|
||||
#include <math.h>
|
||||
#include <string.h>
|
||||
#include <stdio.h>
|
||||
@ -257,6 +258,9 @@ int main(void) {
|
||||
/* Init battery ADC (PC1/ADC3 — Vbat divider 11:1) for CRSF telemetry */
|
||||
battery_init();
|
||||
|
||||
/* Init LVC: low voltage cutoff state machine (Issue #613) */
|
||||
lvc_init();
|
||||
|
||||
/* Probe I2C1 for optional sensors — skip gracefully if not found */
|
||||
int baro_chip = 0; /* chip_id: 0x58=BMP280, 0x60=BME280, 0=absent */
|
||||
mag_type_t mag_type = MAG_NONE;
|
||||
@ -288,6 +292,7 @@ int main(void) {
|
||||
uint32_t pm_tlm_tick = 0; /* JLINK_TLM_POWER transmit timer */
|
||||
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 lvc_tlm_tick = 0; /* JLINK_TLM_LVC transmit timer (Issue #613) */
|
||||
uint8_t pm_pwm_phase = 0; /* Software PWM counter for sleep LED */
|
||||
const float dt = 1.0f / PID_LOOP_HZ; /* 1ms at 1kHz */
|
||||
|
||||
@ -320,6 +325,9 @@ int main(void) {
|
||||
/* Accumulate coulombs for battery state-of-charge estimation (Issue #325) */
|
||||
battery_accumulate_coulombs();
|
||||
|
||||
/* LVC: update low-voltage protection state machine (Issue #613) */
|
||||
lvc_tick(now, battery_read_mv());
|
||||
|
||||
/* Sleep LED: software PWM on LED1 (active-low PC15) driven by PM brightness.
|
||||
* pm_pwm_phase rolls over each ms; brightness sets duty cycle 0-255. */
|
||||
pm_pwm_phase++;
|
||||
@ -600,6 +608,13 @@ int main(void) {
|
||||
motor_driver_estop_clear(&motors);
|
||||
}
|
||||
|
||||
/* LVC cutoff: disarm and estop on undervoltage latch (Issue #613) */
|
||||
if (lvc_is_cutoff() && bal.state == BALANCE_ARMED) {
|
||||
safety_arm_cancel();
|
||||
balance_disarm(&bal);
|
||||
motor_driver_estop(&motors);
|
||||
}
|
||||
|
||||
/* Feed autonomous steer from Jetson into mode manager.
|
||||
* jlink takes priority over legacy CDC jetson_cmd.
|
||||
* mode_manager_get_steer() blends it with RC steer per active mode. */
|
||||
@ -616,6 +631,11 @@ int main(void) {
|
||||
int16_t steer = mode_manager_get_steer(&mode);
|
||||
int16_t spd_bias = mode_manager_get_speed_bias(&mode);
|
||||
int32_t speed = (int32_t)bal.motor_cmd + spd_bias;
|
||||
/* LVC power scaling: 100% normal, 50% critical, 0% cutoff (Issue #613) */
|
||||
uint8_t lvc_scale = lvc_get_power_scale();
|
||||
if (lvc_scale < 100u) {
|
||||
speed = (speed * (int32_t)lvc_scale) / 100;
|
||||
}
|
||||
if (speed > 1000) speed = 1000;
|
||||
if (speed < -1000) speed = -1000;
|
||||
motor_driver_update(&motors, (int16_t)speed, steer, now);
|
||||
@ -728,6 +748,26 @@ int main(void) {
|
||||
jlink_send_power_telemetry(&pow);
|
||||
}
|
||||
|
||||
/* JLINK_TLM_LVC telemetry at LVC_TLM_HZ (1 Hz) -- battery voltage + protection state (Issue #613) */
|
||||
if (now - lvc_tlm_tick >= (1000u / LVC_TLM_HZ)) {
|
||||
lvc_tlm_tick = now;
|
||||
uint32_t lvc_vbat = battery_read_mv();
|
||||
jlink_tlm_lvc_t ltlm;
|
||||
ltlm.voltage_mv = (lvc_vbat > 65535u) ? 65535u : (uint16_t)lvc_vbat;
|
||||
ltlm.protection_state = (uint8_t)lvc_get_state();
|
||||
if (lvc_vbat == 0u) {
|
||||
ltlm.percent = 255u;
|
||||
} else if (lvc_vbat <= LVC_CUTOFF_MV) {
|
||||
ltlm.percent = 0u;
|
||||
} else if (lvc_vbat >= LVC_WARNING_MV) {
|
||||
ltlm.percent = 100u;
|
||||
} else {
|
||||
ltlm.percent = (uint8_t)(((lvc_vbat - LVC_CUTOFF_MV) * 100u) /
|
||||
(LVC_WARNING_MV - LVC_CUTOFF_MV));
|
||||
}
|
||||
jlink_send_lvc_tlm(<lm);
|
||||
}
|
||||
|
||||
/* USB telemetry at 50Hz (only when streaming enabled and calibration done) */
|
||||
if (cdc_streaming && mpu6000_is_calibrated() && now - send_tick >= 20) {
|
||||
send_tick = now;
|
||||
|
||||
@ -151,6 +151,7 @@ void mpu6000_read(IMUData *data) {
|
||||
data->pitch_rate = gyro_pitch_rate;
|
||||
data->roll = s_roll;
|
||||
data->yaw = s_yaw;
|
||||
data->yaw_rate = gyro_yaw_rate; /* board_gz: raw bias-corrected gyro Z (Issue #616) */
|
||||
data->accel_x = ax;
|
||||
data->accel_z = az;
|
||||
}
|
||||
|
||||
140
src/steering_pid.c
Normal file
140
src/steering_pid.c
Normal file
@ -0,0 +1,140 @@
|
||||
/*
|
||||
* steering_pid.c — closed-loop yaw-rate PID for differential drive (Issue #616).
|
||||
*
|
||||
* Converts Jetson Twist.angular.z (encoded as steer * STEER_OMEGA_SCALE deg/s)
|
||||
* into a differential wheel speed offset using IMU gyro Z as feedback.
|
||||
*
|
||||
* Anti-windup: integral clamped to ±STEER_INTEGRAL_MAX before Ki multiply.
|
||||
* Rate limiter: output changes at most STEER_RAMP_RATE_PER_MS per ms so that
|
||||
* a step in steering demand cannot disturb the balance PID.
|
||||
*/
|
||||
|
||||
#include "steering_pid.h"
|
||||
#include "jlink.h"
|
||||
|
||||
/* ---- steering_pid_init() ---- */
|
||||
void steering_pid_init(steering_pid_t *s)
|
||||
{
|
||||
s->target_omega_dps = 0.0f;
|
||||
s->actual_omega_dps = 0.0f;
|
||||
s->integral = 0.0f;
|
||||
s->prev_error = 0.0f;
|
||||
s->output = 0;
|
||||
s->enabled = true;
|
||||
/* Initialize so first send_tlm call fires immediately */
|
||||
s->last_tlm_ms = (uint32_t)(-(uint32_t)(1000u / (STEER_TLM_HZ > 0u ? STEER_TLM_HZ : 1u)));
|
||||
}
|
||||
|
||||
/* ---- steering_pid_reset() ---- */
|
||||
void steering_pid_reset(steering_pid_t *s)
|
||||
{
|
||||
s->target_omega_dps = 0.0f;
|
||||
s->integral = 0.0f;
|
||||
s->prev_error = 0.0f;
|
||||
s->output = 0;
|
||||
}
|
||||
|
||||
/* ---- steering_pid_set_target() ---- */
|
||||
void steering_pid_set_target(steering_pid_t *s, float omega_dps)
|
||||
{
|
||||
if (!s->enabled) return;
|
||||
s->target_omega_dps = omega_dps;
|
||||
}
|
||||
|
||||
/* ---- steering_pid_update() ---- */
|
||||
int16_t steering_pid_update(steering_pid_t *s, float actual_omega_dps, float dt)
|
||||
{
|
||||
if (!s->enabled || dt <= 0.0f) {
|
||||
s->output = 0;
|
||||
return 0;
|
||||
}
|
||||
|
||||
s->actual_omega_dps = actual_omega_dps;
|
||||
|
||||
/* PID error */
|
||||
float error = s->target_omega_dps - actual_omega_dps;
|
||||
|
||||
/* Proportional */
|
||||
float p_term = STEER_KP * error;
|
||||
|
||||
/* Integral with anti-windup clamp */
|
||||
s->integral += error * dt;
|
||||
if (s->integral > STEER_INTEGRAL_MAX) s->integral = STEER_INTEGRAL_MAX;
|
||||
if (s->integral < -STEER_INTEGRAL_MAX) s->integral = -STEER_INTEGRAL_MAX;
|
||||
float i_term = STEER_KI * s->integral;
|
||||
|
||||
/* Derivative on error (avoids setpoint kick for smooth yaw changes) */
|
||||
float d_term = 0.0f;
|
||||
if (dt > 0.0f) {
|
||||
d_term = STEER_KD * (error - s->prev_error) / dt;
|
||||
}
|
||||
s->prev_error = error;
|
||||
|
||||
/* Sum and clamp raw output */
|
||||
float raw = p_term + i_term + d_term;
|
||||
if (raw > (float)STEER_OUTPUT_MAX) raw = (float)STEER_OUTPUT_MAX;
|
||||
if (raw < -(float)STEER_OUTPUT_MAX) raw = -(float)STEER_OUTPUT_MAX;
|
||||
|
||||
/* Rate limiter: bound change per step by STEER_RAMP_RATE_PER_MS * dt */
|
||||
float max_step = (float)STEER_RAMP_RATE_PER_MS * (dt * 1000.0f);
|
||||
float delta = raw - (float)s->output;
|
||||
if (delta > max_step) delta = max_step;
|
||||
if (delta < -max_step) delta = -max_step;
|
||||
|
||||
float limited = (float)s->output + delta;
|
||||
/* Final clamp after rate limit */
|
||||
if (limited > (float)STEER_OUTPUT_MAX) limited = (float)STEER_OUTPUT_MAX;
|
||||
if (limited < -(float)STEER_OUTPUT_MAX) limited = -(float)STEER_OUTPUT_MAX;
|
||||
|
||||
s->output = (int16_t)limited;
|
||||
return s->output;
|
||||
}
|
||||
|
||||
/* ---- steering_pid_get_output() ---- */
|
||||
int16_t steering_pid_get_output(const steering_pid_t *s)
|
||||
{
|
||||
return s->output;
|
||||
}
|
||||
|
||||
/* ---- steering_pid_set_enabled() ---- */
|
||||
void steering_pid_set_enabled(steering_pid_t *s, bool en)
|
||||
{
|
||||
if (!en && s->enabled) {
|
||||
/* Disabling: zero out state */
|
||||
s->target_omega_dps = 0.0f;
|
||||
s->integral = 0.0f;
|
||||
s->prev_error = 0.0f;
|
||||
s->output = 0;
|
||||
}
|
||||
s->enabled = en;
|
||||
}
|
||||
|
||||
/* ---- steering_pid_send_tlm() ---- */
|
||||
void steering_pid_send_tlm(const steering_pid_t *s, uint32_t now_ms)
|
||||
{
|
||||
if (STEER_TLM_HZ == 0u) return;
|
||||
|
||||
uint32_t interval_ms = 1000u / STEER_TLM_HZ;
|
||||
if ((now_ms - s->last_tlm_ms) < interval_ms) return;
|
||||
/* Cast away const for timestamp update — only mutable field */
|
||||
((steering_pid_t *)s)->last_tlm_ms = now_ms;
|
||||
|
||||
jlink_tlm_steering_t tlm;
|
||||
|
||||
/* Scale to ×10 for 0.1 deg/s resolution, clamped to int16 range */
|
||||
float t = s->target_omega_dps * 10.0f;
|
||||
if (t > 32767.0f) t = 32767.0f;
|
||||
if (t < -32768.0f) t = -32768.0f;
|
||||
tlm.target_x10 = (int16_t)t;
|
||||
|
||||
float a = s->actual_omega_dps * 10.0f;
|
||||
if (a > 32767.0f) a = 32767.0f;
|
||||
if (a < -32768.0f) a = -32768.0f;
|
||||
tlm.actual_x10 = (int16_t)a;
|
||||
|
||||
tlm.output = s->output;
|
||||
tlm.enabled = s->enabled ? 1u : 0u;
|
||||
tlm._pad = 0u;
|
||||
|
||||
jlink_send_steering_tlm(&tlm);
|
||||
}
|
||||
618
test/test_steering_pid.c
Normal file
618
test/test_steering_pid.c
Normal file
@ -0,0 +1,618 @@
|
||||
/*
|
||||
* test_steering_pid.c — unit tests for steering_pid (Issue #616).
|
||||
*
|
||||
* Host build (no HAL):
|
||||
* gcc -I include -I src -DTEST_HOST -lm -o /tmp/test_steering test/test_steering_pid.c
|
||||
* /tmp/test_steering
|
||||
*
|
||||
* Stubs out jlink.h to avoid the full HAL dependency chain.
|
||||
*/
|
||||
|
||||
/* ---- Minimal stubs ---- */
|
||||
#define JLINK_H /* prevent real jlink.h from loading */
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
|
||||
/* Minimal jlink_tlm_steering_t and send stub */
|
||||
typedef struct __attribute__((packed)) {
|
||||
int16_t target_x10;
|
||||
int16_t actual_x10;
|
||||
int16_t output;
|
||||
uint8_t enabled;
|
||||
uint8_t _pad;
|
||||
} jlink_tlm_steering_t;
|
||||
|
||||
static jlink_tlm_steering_t g_last_tlm;
|
||||
static int g_tlm_count = 0;
|
||||
|
||||
void jlink_send_steering_tlm(const jlink_tlm_steering_t *tlm)
|
||||
{
|
||||
g_last_tlm = *tlm;
|
||||
g_tlm_count++;
|
||||
}
|
||||
|
||||
/* ---- Include the module under test ---- */
|
||||
#include "steering_pid.h"
|
||||
#include "steering_pid.c" /* single-TU build; steering_pid.c found via -I src */
|
||||
|
||||
/* ---- Test framework ---- */
|
||||
#include <stdio.h>
|
||||
#include <math.h>
|
||||
#include <string.h>
|
||||
|
||||
static int s_pass = 0;
|
||||
static int s_fail = 0;
|
||||
|
||||
#define EXPECT(cond, msg) do { \
|
||||
if (cond) { s_pass++; } \
|
||||
else { s_fail++; printf("FAIL [%s:%d]: %s\n", __func__, __LINE__, msg); } \
|
||||
} while(0)
|
||||
|
||||
#define EXPECT_EQ(a, b, msg) EXPECT((a) == (b), msg)
|
||||
#define EXPECT_NEAR(a, b, eps, msg) EXPECT(fabsf((a) - (b)) <= (eps), msg)
|
||||
#define EXPECT_TRUE(x, msg) EXPECT((x), msg)
|
||||
#define EXPECT_FALSE(x, msg) EXPECT(!(x), msg)
|
||||
|
||||
/* ---- Tests ---- */
|
||||
|
||||
static void test_init(void)
|
||||
{
|
||||
steering_pid_t s;
|
||||
steering_pid_init(&s);
|
||||
|
||||
EXPECT_NEAR(s.target_omega_dps, 0.0f, 1e-6f, "init: target_omega 0");
|
||||
EXPECT_NEAR(s.actual_omega_dps, 0.0f, 1e-6f, "init: actual_omega 0");
|
||||
EXPECT_NEAR(s.integral, 0.0f, 1e-6f, "init: integral 0");
|
||||
EXPECT_NEAR(s.prev_error, 0.0f, 1e-6f, "init: prev_error 0");
|
||||
EXPECT_EQ (s.output, 0, "init: output 0");
|
||||
EXPECT_TRUE(s.enabled, "init: enabled true");
|
||||
}
|
||||
|
||||
static void test_reset(void)
|
||||
{
|
||||
steering_pid_t s;
|
||||
steering_pid_init(&s);
|
||||
s.target_omega_dps = 30.0f;
|
||||
s.integral = 100.0f;
|
||||
s.prev_error = 5.0f;
|
||||
s.output = 200;
|
||||
|
||||
steering_pid_reset(&s);
|
||||
|
||||
EXPECT_NEAR(s.target_omega_dps, 0.0f, 1e-6f, "reset: target 0");
|
||||
EXPECT_NEAR(s.integral, 0.0f, 1e-6f, "reset: integral 0");
|
||||
EXPECT_NEAR(s.prev_error, 0.0f, 1e-6f, "reset: prev_error 0");
|
||||
EXPECT_EQ (s.output, 0, "reset: output 0");
|
||||
EXPECT_TRUE(s.enabled, "reset: preserves enabled");
|
||||
}
|
||||
|
||||
static void test_disabled_returns_zero(void)
|
||||
{
|
||||
steering_pid_t s;
|
||||
steering_pid_init(&s);
|
||||
steering_pid_set_enabled(&s, false);
|
||||
steering_pid_set_target(&s, 50.0f);
|
||||
|
||||
int16_t out = steering_pid_update(&s, 0.0f, 0.001f);
|
||||
|
||||
EXPECT_EQ(out, 0, "disabled: update returns 0");
|
||||
EXPECT_EQ(steering_pid_get_output(&s), 0, "disabled: get_output 0");
|
||||
/* Target should not have been set when disabled */
|
||||
EXPECT_NEAR(s.target_omega_dps, 0.0f, 1e-6f, "disabled: target not set");
|
||||
}
|
||||
|
||||
static void test_zero_dt_returns_zero(void)
|
||||
{
|
||||
steering_pid_t s;
|
||||
steering_pid_init(&s);
|
||||
steering_pid_set_target(&s, 30.0f);
|
||||
|
||||
int16_t out = steering_pid_update(&s, 0.0f, 0.0f);
|
||||
EXPECT_EQ(out, 0, "zero_dt: returns 0");
|
||||
}
|
||||
|
||||
static void test_negative_dt_returns_zero(void)
|
||||
{
|
||||
steering_pid_t s;
|
||||
steering_pid_init(&s);
|
||||
steering_pid_set_target(&s, 30.0f);
|
||||
|
||||
int16_t out = steering_pid_update(&s, 0.0f, -0.01f);
|
||||
EXPECT_EQ(out, 0, "neg_dt: returns 0");
|
||||
}
|
||||
|
||||
static void test_proportional_only(void)
|
||||
{
|
||||
/*
|
||||
* With Ki=0 and Kd=0, output = Kp * error.
|
||||
* Set STEER_KP=2.0, error=50 deg/s → raw = 100 counts.
|
||||
* Rate limiter allows max_step = STEER_RAMP_RATE_PER_MS * dt_ms = 10 * 1 = 10 per step.
|
||||
* After 1 step (dt=0.001s): output capped at 10 by rate limiter.
|
||||
*/
|
||||
steering_pid_t s;
|
||||
memset(&s, 0, sizeof(s));
|
||||
steering_pid_init(&s);
|
||||
s.integral = 0.0f;
|
||||
s.prev_error = 0.0f;
|
||||
s.output = 0;
|
||||
|
||||
steering_pid_set_target(&s, 50.0f);
|
||||
int16_t out = steering_pid_update(&s, 0.0f, 0.001f);
|
||||
|
||||
/* Rate limiter: max_step = 10*0.001*1000 = 10 → output = 10 */
|
||||
EXPECT_EQ(out, 10, "prop_only: rate-limited step");
|
||||
}
|
||||
|
||||
static void test_converges_to_setpoint(void)
|
||||
{
|
||||
/*
|
||||
* With no disturbance, run many steps until output reaches the setpoint.
|
||||
* With target=20 deg/s, Kp=2, after many steps the P term = 2*20 = 40,
|
||||
* plus I accumulation. The rate limiter will eventually allow the output
|
||||
* to grow. Run for 1s at 1kHz (1000 steps, dt=0.001s each).
|
||||
* Expected: eventually output > 0 (PID working), integral accumulates.
|
||||
*/
|
||||
steering_pid_t s;
|
||||
steering_pid_init(&s);
|
||||
steering_pid_set_target(&s, 20.0f);
|
||||
|
||||
int16_t out = 0;
|
||||
for (int i = 0; i < 1000; i++) {
|
||||
out = steering_pid_update(&s, 0.0f, 0.001f);
|
||||
}
|
||||
EXPECT_TRUE(out > 0, "convergence: output positive after 1s");
|
||||
EXPECT_TRUE(s.integral > 0.0f, "convergence: integral positive");
|
||||
}
|
||||
|
||||
static void test_output_clamped_to_max(void)
|
||||
{
|
||||
/*
|
||||
* Very large error should produce output clamped to STEER_OUTPUT_MAX.
|
||||
* Run many ticks so rate limiter is satisfied.
|
||||
* STEER_OUTPUT_MAX = 400; 400 ticks at 10 counts/ms should saturate.
|
||||
*/
|
||||
steering_pid_t s;
|
||||
steering_pid_init(&s);
|
||||
steering_pid_set_target(&s, 500.0f); /* 500 deg/s — way beyond robot capability */
|
||||
|
||||
int16_t out = 0;
|
||||
for (int i = 0; i < 500; i++) {
|
||||
out = steering_pid_update(&s, 0.0f, 0.001f);
|
||||
}
|
||||
EXPECT_EQ(out, STEER_OUTPUT_MAX, "clamp: output saturates at STEER_OUTPUT_MAX");
|
||||
}
|
||||
|
||||
static void test_output_clamped_negative(void)
|
||||
{
|
||||
steering_pid_t s;
|
||||
steering_pid_init(&s);
|
||||
steering_pid_set_target(&s, -500.0f);
|
||||
|
||||
int16_t out = 0;
|
||||
for (int i = 0; i < 500; i++) {
|
||||
out = steering_pid_update(&s, 0.0f, 0.001f);
|
||||
}
|
||||
EXPECT_EQ(out, -STEER_OUTPUT_MAX, "clamp_neg: output saturates at -STEER_OUTPUT_MAX");
|
||||
}
|
||||
|
||||
static void test_anti_windup(void)
|
||||
{
|
||||
/*
|
||||
* Run with a persistent error for a long time.
|
||||
* Integral should be clamped to ±STEER_INTEGRAL_MAX.
|
||||
*/
|
||||
steering_pid_t s;
|
||||
steering_pid_init(&s);
|
||||
steering_pid_set_target(&s, 50.0f);
|
||||
|
||||
for (int i = 0; i < 10000; i++) {
|
||||
steering_pid_update(&s, 0.0f, 0.001f); /* actual stays 0, error = 50 */
|
||||
}
|
||||
EXPECT_NEAR(s.integral, STEER_INTEGRAL_MAX, 0.01f, "anti-windup: integral clamped to max");
|
||||
}
|
||||
|
||||
static void test_anti_windup_negative(void)
|
||||
{
|
||||
steering_pid_t s;
|
||||
steering_pid_init(&s);
|
||||
steering_pid_set_target(&s, -50.0f);
|
||||
|
||||
for (int i = 0; i < 10000; i++) {
|
||||
steering_pid_update(&s, 0.0f, 0.001f);
|
||||
}
|
||||
EXPECT_NEAR(s.integral, -STEER_INTEGRAL_MAX, 0.01f, "anti-windup-neg: integral clamped");
|
||||
}
|
||||
|
||||
static void test_rate_limiter_single_step(void)
|
||||
{
|
||||
/*
|
||||
* At dt=0.001s: max_step = STEER_RAMP_RATE_PER_MS * 1 = 10.
|
||||
* Even if raw PID output = 400, first step can only reach 10.
|
||||
*/
|
||||
steering_pid_t s;
|
||||
steering_pid_init(&s);
|
||||
steering_pid_set_target(&s, 500.0f);
|
||||
|
||||
int16_t out = steering_pid_update(&s, 0.0f, 0.001f);
|
||||
EXPECT_EQ(out, STEER_RAMP_RATE_PER_MS * 1, "rate_lim: first step bounded");
|
||||
}
|
||||
|
||||
static void test_rate_limiter_larger_dt(void)
|
||||
{
|
||||
/*
|
||||
* At dt=0.01s (100Hz): max_step = 10 * 10 = 100.
|
||||
* First step limited to 100.
|
||||
*/
|
||||
steering_pid_t s;
|
||||
steering_pid_init(&s);
|
||||
steering_pid_set_target(&s, 500.0f);
|
||||
|
||||
int16_t out = steering_pid_update(&s, 0.0f, 0.01f);
|
||||
EXPECT_EQ(out, STEER_RAMP_RATE_PER_MS * 10, "rate_lim_100hz: first step bounded");
|
||||
}
|
||||
|
||||
static void test_rate_limiter_decreasing(void)
|
||||
{
|
||||
/*
|
||||
* After saturating to STEER_OUTPUT_MAX, set target to 0.
|
||||
* Output should decrease at most STEER_RAMP_RATE_PER_MS per ms.
|
||||
*/
|
||||
steering_pid_t s;
|
||||
steering_pid_init(&s);
|
||||
steering_pid_set_target(&s, 500.0f);
|
||||
for (int i = 0; i < 500; i++) {
|
||||
steering_pid_update(&s, 0.0f, 0.001f);
|
||||
}
|
||||
/* Now output = STEER_OUTPUT_MAX = 400 */
|
||||
int16_t before = s.output;
|
||||
steering_pid_reset(&s); /* reset integral and target */
|
||||
/* output is also zeroed by reset — which is fine: the test is
|
||||
really about rate limiting when driving toward 0 after a step */
|
||||
(void)before;
|
||||
|
||||
/* Re-saturate, then drive to 0 with matching actual */
|
||||
steering_pid_init(&s);
|
||||
steering_pid_set_target(&s, 500.0f);
|
||||
for (int i = 0; i < 500; i++) {
|
||||
steering_pid_update(&s, 0.0f, 0.001f);
|
||||
}
|
||||
steering_pid_set_target(&s, 0.0f);
|
||||
int16_t pre = s.output;
|
||||
int16_t post = steering_pid_update(&s, 0.0f, 0.001f);
|
||||
|
||||
/* Should decrease by no more than STEER_RAMP_RATE_PER_MS per tick */
|
||||
int16_t delta = pre - post;
|
||||
EXPECT_TRUE(delta <= STEER_RAMP_RATE_PER_MS, "rate_lim_dec: decrease bounded per tick");
|
||||
}
|
||||
|
||||
static void test_zero_error_zero_output(void)
|
||||
{
|
||||
/*
|
||||
* When target == actual from the first tick, error = 0 and output stays 0.
|
||||
*/
|
||||
steering_pid_t s;
|
||||
steering_pid_init(&s);
|
||||
steering_pid_set_target(&s, 30.0f);
|
||||
int16_t out = steering_pid_update(&s, 30.0f, 0.001f);
|
||||
EXPECT_EQ(out, 0, "zero_error: no output when tracking exactly");
|
||||
}
|
||||
|
||||
static void test_enable_disable_clears_state(void)
|
||||
{
|
||||
steering_pid_t s;
|
||||
steering_pid_init(&s);
|
||||
steering_pid_set_target(&s, 50.0f);
|
||||
for (int i = 0; i < 100; i++) {
|
||||
steering_pid_update(&s, 0.0f, 0.001f);
|
||||
}
|
||||
|
||||
/* Disable */
|
||||
steering_pid_set_enabled(&s, false);
|
||||
|
||||
EXPECT_FALSE(s.enabled, "disable: enabled false");
|
||||
EXPECT_NEAR (s.target_omega_dps, 0.0f, 1e-6f, "disable: target zeroed");
|
||||
EXPECT_NEAR (s.integral, 0.0f, 1e-6f, "disable: integral zeroed");
|
||||
EXPECT_EQ (s.output, 0, "disable: output zeroed");
|
||||
|
||||
/* Calls while disabled return 0 */
|
||||
int16_t out = steering_pid_update(&s, 50.0f, 0.001f);
|
||||
EXPECT_EQ(out, 0, "disable: update returns 0");
|
||||
}
|
||||
|
||||
static void test_re_enable(void)
|
||||
{
|
||||
steering_pid_t s;
|
||||
steering_pid_init(&s);
|
||||
steering_pid_set_enabled(&s, false);
|
||||
steering_pid_set_enabled(&s, true);
|
||||
|
||||
EXPECT_TRUE(s.enabled, "re-enable: enabled true");
|
||||
|
||||
/* After re-enable, new target and update should work */
|
||||
steering_pid_set_target(&s, 20.0f);
|
||||
EXPECT_NEAR(s.target_omega_dps, 20.0f, 1e-6f, "re-enable: set_target works");
|
||||
}
|
||||
|
||||
static void test_set_target_ignored_when_disabled(void)
|
||||
{
|
||||
steering_pid_t s;
|
||||
steering_pid_init(&s);
|
||||
steering_pid_set_enabled(&s, false);
|
||||
steering_pid_set_target(&s, 99.0f);
|
||||
|
||||
EXPECT_NEAR(s.target_omega_dps, 0.0f, 1e-6f, "disabled_target: target not set");
|
||||
}
|
||||
|
||||
static void test_actual_omega_updated(void)
|
||||
{
|
||||
steering_pid_t s;
|
||||
steering_pid_init(&s);
|
||||
steering_pid_update(&s, 42.5f, 0.001f);
|
||||
EXPECT_NEAR(s.actual_omega_dps, 42.5f, 1e-4f, "actual_omega: stored after update");
|
||||
}
|
||||
|
||||
static void test_get_output_matches_update(void)
|
||||
{
|
||||
steering_pid_t s;
|
||||
steering_pid_init(&s);
|
||||
steering_pid_set_target(&s, 20.0f);
|
||||
int16_t from_update = steering_pid_update(&s, 0.0f, 0.001f);
|
||||
EXPECT_EQ(from_update, steering_pid_get_output(&s), "get_output: matches update return");
|
||||
}
|
||||
|
||||
static void test_omega_scale_convention(void)
|
||||
{
|
||||
/*
|
||||
* Verify that setting target via STEER_OMEGA_SCALE:
|
||||
* omega_dps = steer * STEER_OMEGA_SCALE
|
||||
* steer=100 → omega=10 deg/s
|
||||
*/
|
||||
float omega = 100.0f * STEER_OMEGA_SCALE;
|
||||
EXPECT_NEAR(omega, 10.0f, 1e-4f, "omega_scale: 100 units = 10 deg/s");
|
||||
|
||||
omega = -1000.0f * STEER_OMEGA_SCALE;
|
||||
EXPECT_NEAR(omega, -100.0f, 1e-4f, "omega_scale: -1000 units = -100 deg/s");
|
||||
}
|
||||
|
||||
static void test_tlm_first_call_fires(void)
|
||||
{
|
||||
/*
|
||||
* After init, last_tlm_ms is set so that the first send_tlm fires immediately
|
||||
* at now_ms = 0.
|
||||
*/
|
||||
steering_pid_t s;
|
||||
steering_pid_init(&s);
|
||||
g_tlm_count = 0;
|
||||
|
||||
steering_pid_send_tlm(&s, 0);
|
||||
EXPECT_EQ(g_tlm_count, 1, "tlm_first: fires at ms=0 after init");
|
||||
}
|
||||
|
||||
static void test_tlm_rate_limited(void)
|
||||
{
|
||||
/*
|
||||
* STEER_TLM_HZ = 10 → interval = 100ms.
|
||||
* Two calls at ms=0 and ms=50 should only send once.
|
||||
*/
|
||||
steering_pid_t s;
|
||||
steering_pid_init(&s);
|
||||
g_tlm_count = 0;
|
||||
|
||||
steering_pid_send_tlm(&s, 0); /* fires (first call) */
|
||||
steering_pid_send_tlm(&s, 50); /* blocked (< 100ms) */
|
||||
EXPECT_EQ(g_tlm_count, 1, "tlm_rate: blocked within interval");
|
||||
|
||||
steering_pid_send_tlm(&s, 100); /* fires (interval elapsed) */
|
||||
EXPECT_EQ(g_tlm_count, 2, "tlm_rate: fires after interval");
|
||||
}
|
||||
|
||||
static void test_tlm_payload_target(void)
|
||||
{
|
||||
steering_pid_t s;
|
||||
steering_pid_init(&s);
|
||||
s.target_omega_dps = 25.5f; /* → target_x10 = 255 */
|
||||
g_tlm_count = 0;
|
||||
|
||||
steering_pid_send_tlm(&s, 0);
|
||||
EXPECT_EQ(g_last_tlm.target_x10, 255, "tlm_payload: target_x10 correct");
|
||||
}
|
||||
|
||||
static void test_tlm_payload_actual(void)
|
||||
{
|
||||
steering_pid_t s;
|
||||
steering_pid_init(&s);
|
||||
steering_pid_set_target(&s, 10.0f);
|
||||
steering_pid_update(&s, 12.3f, 0.001f);
|
||||
g_tlm_count = 0;
|
||||
|
||||
steering_pid_send_tlm(&s, 0);
|
||||
/* actual = 12.3 deg/s → actual_x10 = 123 */
|
||||
EXPECT_EQ(g_last_tlm.actual_x10, 123, "tlm_payload: actual_x10 correct");
|
||||
}
|
||||
|
||||
static void test_tlm_payload_output(void)
|
||||
{
|
||||
steering_pid_t s;
|
||||
steering_pid_init(&s);
|
||||
s.output = 150;
|
||||
g_tlm_count = 0;
|
||||
|
||||
steering_pid_send_tlm(&s, 0);
|
||||
EXPECT_EQ(g_last_tlm.output, 150, "tlm_payload: output correct");
|
||||
}
|
||||
|
||||
static void test_tlm_payload_enabled_flag(void)
|
||||
{
|
||||
steering_pid_t s;
|
||||
steering_pid_init(&s);
|
||||
steering_pid_set_enabled(&s, true);
|
||||
g_tlm_count = 0;
|
||||
steering_pid_send_tlm(&s, 0);
|
||||
EXPECT_EQ(g_last_tlm.enabled, 1u, "tlm_payload: enabled=1 when on");
|
||||
|
||||
steering_pid_init(&s);
|
||||
steering_pid_set_enabled(&s, false);
|
||||
steering_pid_send_tlm(&s, 0);
|
||||
EXPECT_EQ(g_last_tlm.enabled, 0u, "tlm_payload: enabled=0 when off");
|
||||
}
|
||||
|
||||
static void test_tlm_payload_pad_zero(void)
|
||||
{
|
||||
steering_pid_t s;
|
||||
steering_pid_init(&s);
|
||||
g_tlm_count = 0;
|
||||
steering_pid_send_tlm(&s, 0);
|
||||
EXPECT_EQ(g_last_tlm._pad, 0u, "tlm_payload: _pad = 0");
|
||||
}
|
||||
|
||||
static void test_tlm_target_x10_negative(void)
|
||||
{
|
||||
steering_pid_t s;
|
||||
steering_pid_init(&s);
|
||||
s.target_omega_dps = -30.7f; /* → target_x10 = -307 */
|
||||
g_tlm_count = 0;
|
||||
steering_pid_send_tlm(&s, 0);
|
||||
EXPECT_EQ(g_last_tlm.target_x10, -307, "tlm_payload: negative target_x10");
|
||||
}
|
||||
|
||||
static void test_tlm_int16_clamp_positive(void)
|
||||
{
|
||||
steering_pid_t s;
|
||||
steering_pid_init(&s);
|
||||
s.target_omega_dps = 4000.0f; /* × 10 = 40000 > INT16_MAX */
|
||||
g_tlm_count = 0;
|
||||
steering_pid_send_tlm(&s, 0);
|
||||
EXPECT_EQ(g_last_tlm.target_x10, 32767, "tlm_clamp: target_x10 clamped to INT16_MAX");
|
||||
}
|
||||
|
||||
static void test_tlm_int16_clamp_negative(void)
|
||||
{
|
||||
steering_pid_t s;
|
||||
steering_pid_init(&s);
|
||||
s.target_omega_dps = -4000.0f; /* × 10 = -40000 < INT16_MIN */
|
||||
g_tlm_count = 0;
|
||||
steering_pid_send_tlm(&s, 0);
|
||||
EXPECT_EQ(g_last_tlm.target_x10, (int16_t)-32768, "tlm_clamp: target_x10 clamped to INT16_MIN");
|
||||
}
|
||||
|
||||
static void test_balance_not_disturbed_by_small_step(void)
|
||||
{
|
||||
/*
|
||||
* A balance bot is most sensitive to sudden changes in steer_cmd.
|
||||
* Verify that a large target step at t=0 only increments output by
|
||||
* STEER_RAMP_RATE_PER_MS per millisecond — not a step change.
|
||||
* This confirms the rate limiter protects the balance PID.
|
||||
*/
|
||||
steering_pid_t s;
|
||||
steering_pid_init(&s);
|
||||
steering_pid_set_target(&s, 100.0f); /* large yaw demand */
|
||||
|
||||
/* 10ms at 1kHz */
|
||||
for (int i = 0; i < 10; i++) {
|
||||
int16_t pre = s.output;
|
||||
int16_t post = steering_pid_update(&s, 0.0f, 0.001f);
|
||||
int16_t step = post - pre;
|
||||
EXPECT_TRUE(step <= STEER_RAMP_RATE_PER_MS, "balance_protect: step bounded each tick");
|
||||
EXPECT_TRUE(step >= -STEER_RAMP_RATE_PER_MS, "balance_protect: neg step bounded each tick");
|
||||
}
|
||||
}
|
||||
|
||||
static void test_derivative_on_error_change(void)
|
||||
{
|
||||
/*
|
||||
* When error changes suddenly (e.g. actual_omega jumps), the derivative
|
||||
* term should add a braking contribution.
|
||||
* With STEER_KD=0.05 and a 10 deg/s²-equivalent error rate change:
|
||||
* d_term = 0.05 * (error1 - error0) / dt
|
||||
* We just verify output changes when error changes, indicating D is active.
|
||||
*/
|
||||
steering_pid_t s;
|
||||
steering_pid_init(&s);
|
||||
steering_pid_set_target(&s, 20.0f);
|
||||
|
||||
/* Step 1: actual = 0, error = 20 */
|
||||
int16_t out1 = steering_pid_update(&s, 0.0f, 0.001f);
|
||||
|
||||
/* Step 2: actual jumps to 20, error = 0. D will oppose. */
|
||||
int16_t out2 = steering_pid_update(&s, 20.0f, 0.001f);
|
||||
|
||||
/* out2 should be less than out1 (derivative braking as error drops) */
|
||||
EXPECT_TRUE(out2 <= out1, "derivative: braking when error shrinks");
|
||||
}
|
||||
|
||||
static void test_reset_then_restart(void)
|
||||
{
|
||||
steering_pid_t s;
|
||||
steering_pid_init(&s);
|
||||
steering_pid_set_target(&s, 50.0f);
|
||||
for (int i = 0; i < 100; i++) {
|
||||
steering_pid_update(&s, 0.0f, 0.001f);
|
||||
}
|
||||
|
||||
steering_pid_reset(&s);
|
||||
EXPECT_EQ (s.output, 0, "reset_restart: output 0");
|
||||
EXPECT_NEAR(s.integral, 0.0f, 1e-6f, "reset_restart: integral 0");
|
||||
EXPECT_NEAR(s.target_omega_dps, 0.0f, 1e-6f, "reset_restart: target 0");
|
||||
|
||||
/* After reset, new target should work cleanly */
|
||||
steering_pid_set_target(&s, 10.0f);
|
||||
int16_t out = steering_pid_update(&s, 0.0f, 0.001f);
|
||||
EXPECT_TRUE(out > 0, "reset_restart: positive output after fresh start");
|
||||
}
|
||||
|
||||
static void test_tlm_hz_constant(void)
|
||||
{
|
||||
EXPECT_TRUE(STEER_TLM_HZ == 10u, "tlm_hz: STEER_TLM_HZ is 10");
|
||||
}
|
||||
|
||||
static void test_output_max_constant(void)
|
||||
{
|
||||
EXPECT_TRUE(STEER_OUTPUT_MAX == 400, "output_max: STEER_OUTPUT_MAX is 400");
|
||||
}
|
||||
|
||||
/* ---- main ---- */
|
||||
int main(void)
|
||||
{
|
||||
printf("=== test_steering_pid ===\n\n");
|
||||
|
||||
test_init();
|
||||
test_reset();
|
||||
test_disabled_returns_zero();
|
||||
test_zero_dt_returns_zero();
|
||||
test_negative_dt_returns_zero();
|
||||
test_proportional_only();
|
||||
test_converges_to_setpoint();
|
||||
test_output_clamped_to_max();
|
||||
test_output_clamped_negative();
|
||||
test_anti_windup();
|
||||
test_anti_windup_negative();
|
||||
test_rate_limiter_single_step();
|
||||
test_rate_limiter_larger_dt();
|
||||
test_rate_limiter_decreasing();
|
||||
test_zero_error_zero_output();
|
||||
test_enable_disable_clears_state();
|
||||
test_re_enable();
|
||||
test_set_target_ignored_when_disabled();
|
||||
test_actual_omega_updated();
|
||||
test_get_output_matches_update();
|
||||
test_omega_scale_convention();
|
||||
test_tlm_first_call_fires();
|
||||
test_tlm_rate_limited();
|
||||
test_tlm_payload_target();
|
||||
test_tlm_payload_actual();
|
||||
test_tlm_payload_output();
|
||||
test_tlm_payload_enabled_flag();
|
||||
test_tlm_payload_pad_zero();
|
||||
test_tlm_target_x10_negative();
|
||||
test_tlm_int16_clamp_positive();
|
||||
test_tlm_int16_clamp_negative();
|
||||
test_balance_not_disturbed_by_small_step();
|
||||
test_derivative_on_error_change();
|
||||
test_reset_then_restart();
|
||||
test_tlm_hz_constant();
|
||||
test_output_max_constant();
|
||||
|
||||
printf("\nResults: %d passed, %d failed\n", s_pass, s_fail);
|
||||
return s_fail ? 1 : 0;
|
||||
}
|
||||
343
ui/settings_panel.css
Normal file
343
ui/settings_panel.css
Normal file
@ -0,0 +1,343 @@
|
||||
/* settings_panel.css — Saltybot Settings (Issue #614) */
|
||||
|
||||
*, *::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;
|
||||
}
|
||||
|
||||
body {
|
||||
font-family: 'Courier New', Courier, monospace;
|
||||
font-size: 12px;
|
||||
background: var(--bg0);
|
||||
color: var(--base);
|
||||
height: 100dvh;
|
||||
display: flex;
|
||||
flex-direction: column;
|
||||
overflow: hidden;
|
||||
}
|
||||
|
||||
/* ── Header ── */
|
||||
#header {
|
||||
display: flex;
|
||||
align-items: center;
|
||||
padding: 5px 12px;
|
||||
background: var(--bg1);
|
||||
border-bottom: 1px solid var(--bd);
|
||||
flex-shrink: 0;
|
||||
gap: 8px;
|
||||
flex-wrap: wrap;
|
||||
}
|
||||
.logo { color: #f97316; font-weight: bold; letter-spacing: .15em; font-size: 13px; flex-shrink: 0; }
|
||||
|
||||
#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; }
|
||||
|
||||
@keyframes blink { 0%,100%{opacity:1} 50%{opacity:.3} }
|
||||
@keyframes fadeout { 0%{opacity:1} 70%{opacity:1} 100%{opacity:0} }
|
||||
|
||||
.save-ind {
|
||||
font-size: 10px; font-weight: bold; letter-spacing: .1em;
|
||||
color: var(--green); padding: 2px 8px;
|
||||
border: 1px solid var(--green); border-radius: 3px;
|
||||
animation: fadeout 2s forwards;
|
||||
}
|
||||
.save-ind.hidden { display: none; }
|
||||
|
||||
#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: 185px;
|
||||
}
|
||||
#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; }
|
||||
.apply-btn { border-color: #14532d; color: #86efac; }
|
||||
.apply-btn:hover { background: #052010; }
|
||||
.del-btn { border-color: #7f1d1d; color: #fca5a5; }
|
||||
.del-btn:hover { background: #1c0505; }
|
||||
|
||||
/* ── Tab bar ── */
|
||||
#tab-bar {
|
||||
display: flex;
|
||||
background: var(--bg1);
|
||||
border-bottom: 1px solid var(--bd);
|
||||
flex-shrink: 0;
|
||||
padding: 0 12px;
|
||||
gap: 2px;
|
||||
}
|
||||
.tab-btn {
|
||||
padding: 6px 14px;
|
||||
background: none;
|
||||
border: none;
|
||||
border-bottom: 2px solid transparent;
|
||||
color: var(--mid);
|
||||
font-family: 'Courier New', monospace;
|
||||
font-size: 11px;
|
||||
font-weight: bold;
|
||||
letter-spacing: .1em;
|
||||
cursor: pointer;
|
||||
transition: color .15s, border-color .15s;
|
||||
}
|
||||
.tab-btn:hover { color: var(--base); }
|
||||
.tab-btn.active { color: var(--cyan); border-bottom-color: var(--cyan); }
|
||||
|
||||
/* ── Main ── */
|
||||
#main {
|
||||
flex: 1;
|
||||
overflow-y: auto;
|
||||
padding: 12px;
|
||||
min-height: 0;
|
||||
}
|
||||
|
||||
/* ── Tab panels ── */
|
||||
.tab-panel { display: none; }
|
||||
.tab-panel.active { display: block; }
|
||||
|
||||
.panel-col {
|
||||
display: flex;
|
||||
flex-direction: column;
|
||||
gap: 12px;
|
||||
max-width: 860px;
|
||||
margin: 0 auto;
|
||||
}
|
||||
|
||||
/* ── Section card ── */
|
||||
.section-card {
|
||||
background: var(--bg2);
|
||||
border: 1px solid var(--bd2);
|
||||
border-radius: 8px;
|
||||
padding: 12px;
|
||||
}
|
||||
.sec-header {
|
||||
display: flex;
|
||||
justify-content: space-between;
|
||||
align-items: flex-start;
|
||||
margin-bottom: 10px;
|
||||
gap: 8px;
|
||||
flex-wrap: wrap;
|
||||
}
|
||||
.sec-title {
|
||||
font-size: 11px; font-weight: bold; letter-spacing: .1em;
|
||||
color: #67e8f9; text-transform: uppercase;
|
||||
}
|
||||
.sec-node {
|
||||
font-size: 9px; color: var(--mid); margin-top: 2px;
|
||||
}
|
||||
.sec-actions { display: flex; gap: 6px; flex-shrink: 0; }
|
||||
|
||||
/* ── Parameter fields ── */
|
||||
.param-row {
|
||||
display: grid;
|
||||
grid-template-columns: 180px 1fr 80px 50px;
|
||||
align-items: center;
|
||||
gap: 8px;
|
||||
padding: 5px 0;
|
||||
border-bottom: 1px solid var(--bd);
|
||||
}
|
||||
.param-row:last-child { border-bottom: none; }
|
||||
.param-label { font-size: 10px; color: var(--mid); }
|
||||
.param-unit { font-size: 9px; color: var(--dim); text-align: right; }
|
||||
|
||||
/* Slider */
|
||||
.param-slider {
|
||||
-webkit-appearance: none;
|
||||
width: 100%; height: 4px; border-radius: 2px;
|
||||
background: var(--bd2); outline: none; cursor: pointer;
|
||||
}
|
||||
.param-slider::-webkit-slider-thumb {
|
||||
-webkit-appearance: none;
|
||||
width: 12px; height: 12px; border-radius: 50%;
|
||||
background: var(--cyan); cursor: pointer;
|
||||
}
|
||||
.param-slider::-moz-range-thumb {
|
||||
width: 12px; height: 12px; border-radius: 50%;
|
||||
background: var(--cyan); cursor: pointer; border: none;
|
||||
}
|
||||
.param-slider.changed::-webkit-slider-thumb { background: var(--amber); }
|
||||
.param-slider.changed::-moz-range-thumb { background: var(--amber); }
|
||||
|
||||
/* Number input */
|
||||
.param-input {
|
||||
background: var(--bg0); border: 1px solid var(--bd2);
|
||||
border-radius: 3px; color: var(--hi); padding: 2px 5px;
|
||||
font-family: monospace; font-size: 10px; text-align: right;
|
||||
width: 72px;
|
||||
}
|
||||
.param-input:focus { outline: none; border-color: var(--cyan); }
|
||||
.param-input.changed { border-color: var(--amber); color: var(--amber); }
|
||||
|
||||
/* Toggle switch */
|
||||
.toggle-row {
|
||||
display: grid;
|
||||
grid-template-columns: 180px 1fr auto;
|
||||
align-items: center;
|
||||
gap: 8px;
|
||||
padding: 6px 0;
|
||||
border-bottom: 1px solid var(--bd);
|
||||
}
|
||||
.toggle-row:last-child { border-bottom: none; }
|
||||
.toggle-desc { font-size: 9px; color: var(--dim); }
|
||||
|
||||
.toggle-switch {
|
||||
position: relative;
|
||||
width: 36px; height: 18px; flex-shrink: 0;
|
||||
}
|
||||
.toggle-switch input { opacity: 0; width: 0; height: 0; }
|
||||
.toggle-track {
|
||||
position: absolute; inset: 0;
|
||||
background: var(--dim); border-radius: 9px; cursor: pointer;
|
||||
transition: background .2s;
|
||||
}
|
||||
.toggle-track::after {
|
||||
content: '';
|
||||
position: absolute;
|
||||
left: 2px; top: 2px;
|
||||
width: 14px; height: 14px;
|
||||
border-radius: 50%;
|
||||
background: #fff;
|
||||
transition: transform .2s;
|
||||
}
|
||||
.toggle-switch input:checked + .toggle-track { background: var(--cyan); }
|
||||
.toggle-switch input:checked + .toggle-track::after { transform: translateX(18px); }
|
||||
|
||||
/* Status / feedback line */
|
||||
.sec-status {
|
||||
margin-top: 6px;
|
||||
font-size: 9px;
|
||||
min-height: 14px;
|
||||
transition: color .3s;
|
||||
}
|
||||
.sec-status.ok { color: var(--green); }
|
||||
.sec-status.err { color: var(--red); }
|
||||
.sec-status.loading { color: var(--amber); }
|
||||
|
||||
/* ── System tab ── */
|
||||
.diag-placeholder {
|
||||
color: var(--dim); font-size: 10px; padding: 12px 0; text-align: center;
|
||||
}
|
||||
#diag-grid {
|
||||
display: grid;
|
||||
grid-template-columns: repeat(auto-fill, minmax(180px, 1fr));
|
||||
gap: 8px;
|
||||
}
|
||||
.diag-card {
|
||||
background: var(--bg0);
|
||||
border: 1px solid var(--bd);
|
||||
border-radius: 5px;
|
||||
padding: 8px;
|
||||
}
|
||||
.diag-card-title {
|
||||
font-size: 9px; font-weight: bold; letter-spacing: .08em;
|
||||
color: #0891b2; margin-bottom: 5px; text-transform: uppercase;
|
||||
}
|
||||
.diag-kv {
|
||||
display: flex; justify-content: space-between;
|
||||
padding: 2px 0; font-size: 9px;
|
||||
border-bottom: 1px solid var(--bd);
|
||||
}
|
||||
.diag-kv:last-child { border-bottom: none; }
|
||||
.diag-k { color: var(--mid); }
|
||||
.diag-v { color: var(--hi); font-family: monospace; }
|
||||
.diag-v.ok { color: var(--green); }
|
||||
.diag-v.warn { color: var(--amber); }
|
||||
.diag-v.err { color: var(--red); }
|
||||
|
||||
#net-grid {
|
||||
display: grid;
|
||||
grid-template-columns: repeat(auto-fill, minmax(180px, 1fr));
|
||||
gap: 8px;
|
||||
}
|
||||
|
||||
/* Signal bars */
|
||||
.sig-bars {
|
||||
display: inline-flex; align-items: flex-end; gap: 2px; height: 14px;
|
||||
}
|
||||
.sig-bar {
|
||||
width: 4px; border-radius: 1px; background: var(--dim);
|
||||
}
|
||||
.sig-bar.lit { background: var(--cyan); }
|
||||
|
||||
/* Node list */
|
||||
#node-list-wrap {
|
||||
display: flex; flex-wrap: wrap; gap: 4px;
|
||||
max-height: 160px; overflow-y: auto;
|
||||
}
|
||||
.node-chip {
|
||||
font-size: 9px; padding: 1px 6px; border-radius: 2px;
|
||||
border: 1px solid var(--bd); background: var(--bg0);
|
||||
color: var(--mid);
|
||||
}
|
||||
.node-chip.active-node { border-color: var(--bd2); color: var(--base); }
|
||||
|
||||
/* ── Preset bar ── */
|
||||
#preset-bar {
|
||||
display: flex;
|
||||
align-items: center;
|
||||
gap: 8px;
|
||||
padding: 5px 12px;
|
||||
background: var(--bg1);
|
||||
border-top: 1px solid var(--bd);
|
||||
flex-shrink: 0;
|
||||
flex-wrap: wrap;
|
||||
}
|
||||
.plbl { font-size: 10px; color: var(--mid); letter-spacing: .08em; }
|
||||
|
||||
#preset-select {
|
||||
background: var(--bg2); border: 1px solid var(--bd2); border-radius: 4px;
|
||||
color: #67e8f9; padding: 2px 6px; font-family: monospace; font-size: 10px;
|
||||
cursor: pointer;
|
||||
}
|
||||
#preset-select:focus { outline: none; }
|
||||
|
||||
#preset-name {
|
||||
background: var(--bg2); border: 1px solid var(--bd2); border-radius: 4px;
|
||||
color: var(--hi); padding: 2px 7px; font-family: monospace; font-size: 11px; width: 140px;
|
||||
}
|
||||
#preset-name:focus { outline: none; border-color: var(--cyan); }
|
||||
|
||||
/* ── Footer ── */
|
||||
#footer {
|
||||
background: var(--bg1); border-top: 1px solid var(--bd);
|
||||
padding: 3px 12px;
|
||||
display: flex; align-items: center; justify-content: space-between;
|
||||
flex-shrink: 0; font-size: 10px; color: var(--dim);
|
||||
}
|
||||
|
||||
/* ── Responsive ── */
|
||||
@media (max-width: 640px) {
|
||||
.param-row {
|
||||
grid-template-columns: 140px 1fr 70px 44px;
|
||||
gap: 5px;
|
||||
}
|
||||
#diag-grid { grid-template-columns: 1fr 1fr; }
|
||||
}
|
||||
@media (max-width: 420px) {
|
||||
.param-row {
|
||||
grid-template-columns: 1fr 1fr;
|
||||
grid-template-rows: auto auto;
|
||||
}
|
||||
.param-label { grid-column: 1 / 3; }
|
||||
.param-unit { text-align: left; }
|
||||
}
|
||||
313
ui/settings_panel.html
Normal file
313
ui/settings_panel.html
Normal file
@ -0,0 +1,313 @@
|
||||
<!DOCTYPE html>
|
||||
<html lang="en">
|
||||
<head>
|
||||
<meta charset="UTF-8">
|
||||
<meta name="viewport" content="width=device-width, initial-scale=1.0, maximum-scale=1.0">
|
||||
<title>Saltybot — Settings</title>
|
||||
<link rel="stylesheet" href="settings_panel.css">
|
||||
<script src="https://cdn.jsdelivr.net/npm/roslib@1.3.0/build/roslib.min.js"></script>
|
||||
</head>
|
||||
<body>
|
||||
|
||||
<!-- ── Header ── -->
|
||||
<div id="header">
|
||||
<div class="logo">⚡ SALTYBOT — SETTINGS</div>
|
||||
<div id="conn-dot"></div>
|
||||
<input id="ws-input" type="text" value="ws://localhost:9090" placeholder="ws://robot-ip:9090" />
|
||||
<button id="btn-connect" class="hbtn">CONNECT</button>
|
||||
<span id="conn-label" style="color:#4b5563;font-size:10px">Not connected</span>
|
||||
<div style="flex:1"></div>
|
||||
<div id="save-indicator" class="save-ind hidden">✓ SAVED</div>
|
||||
</div>
|
||||
|
||||
<!-- ── Tab bar ── -->
|
||||
<div id="tab-bar">
|
||||
<button class="tab-btn active" data-tab="pid">PID</button>
|
||||
<button class="tab-btn" data-tab="speed">SPEED</button>
|
||||
<button class="tab-btn" data-tab="safety">SAFETY</button>
|
||||
<button class="tab-btn" data-tab="sensors">SENSORS</button>
|
||||
<button class="tab-btn" data-tab="system">SYSTEM</button>
|
||||
</div>
|
||||
|
||||
<!-- ── Main ── -->
|
||||
<div id="main">
|
||||
|
||||
<!-- ═══ PID TAB ═══ -->
|
||||
<div class="tab-panel active" id="tab-pid">
|
||||
<div class="panel-col">
|
||||
|
||||
<!-- Balance PID -->
|
||||
<div class="section-card">
|
||||
<div class="sec-header">
|
||||
<div>
|
||||
<div class="sec-title">Balance Controller PID</div>
|
||||
<div class="sec-node">/balance_controller</div>
|
||||
</div>
|
||||
<div class="sec-actions">
|
||||
<button class="hbtn" onclick="loadSection('balance_pid')">↓ LOAD</button>
|
||||
<button class="hbtn apply-btn" onclick="applySection('balance_pid')">↑ APPLY</button>
|
||||
</div>
|
||||
</div>
|
||||
<div id="balance_pid-fields"></div>
|
||||
<div class="sec-status" id="balance_pid-status"></div>
|
||||
</div>
|
||||
|
||||
<!-- Adaptive PID -->
|
||||
<div class="section-card">
|
||||
<div class="sec-header">
|
||||
<div>
|
||||
<div class="sec-title">Adaptive PID — Empty Load</div>
|
||||
<div class="sec-node">/adaptive_pid</div>
|
||||
</div>
|
||||
<div class="sec-actions">
|
||||
<button class="hbtn" onclick="loadSection('adaptive_pid_empty')">↓ LOAD</button>
|
||||
<button class="hbtn apply-btn" onclick="applySection('adaptive_pid_empty')">↑ APPLY</button>
|
||||
</div>
|
||||
</div>
|
||||
<div id="adaptive_pid_empty-fields"></div>
|
||||
<div class="sec-status" id="adaptive_pid_empty-status"></div>
|
||||
</div>
|
||||
|
||||
<!-- Adaptive PID bounds -->
|
||||
<div class="section-card">
|
||||
<div class="sec-header">
|
||||
<div>
|
||||
<div class="sec-title">Adaptive PID — Bounds</div>
|
||||
<div class="sec-node">/adaptive_pid</div>
|
||||
</div>
|
||||
<div class="sec-actions">
|
||||
<button class="hbtn" onclick="loadSection('adaptive_pid_bounds')">↓ LOAD</button>
|
||||
<button class="hbtn apply-btn" onclick="applySection('adaptive_pid_bounds')">↑ APPLY</button>
|
||||
</div>
|
||||
</div>
|
||||
<div id="adaptive_pid_bounds-fields"></div>
|
||||
<div class="sec-status" id="adaptive_pid_bounds-status"></div>
|
||||
</div>
|
||||
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<!-- ═══ SPEED TAB ═══ -->
|
||||
<div class="tab-panel" id="tab-speed">
|
||||
<div class="panel-col">
|
||||
|
||||
<div class="section-card">
|
||||
<div class="sec-header">
|
||||
<div>
|
||||
<div class="sec-title">Tank Driver Limits</div>
|
||||
<div class="sec-node">/tank_driver</div>
|
||||
</div>
|
||||
<div class="sec-actions">
|
||||
<button class="hbtn" onclick="loadSection('tank_limits')">↓ LOAD</button>
|
||||
<button class="hbtn apply-btn" onclick="applySection('tank_limits')">↑ APPLY</button>
|
||||
</div>
|
||||
</div>
|
||||
<div id="tank_limits-fields"></div>
|
||||
<div class="sec-status" id="tank_limits-status"></div>
|
||||
</div>
|
||||
|
||||
<div class="section-card">
|
||||
<div class="sec-header">
|
||||
<div>
|
||||
<div class="sec-title">Smooth Velocity Controller</div>
|
||||
<div class="sec-node">/smooth_velocity_controller</div>
|
||||
</div>
|
||||
<div class="sec-actions">
|
||||
<button class="hbtn" onclick="loadSection('smooth_vel')">↓ LOAD</button>
|
||||
<button class="hbtn apply-btn" onclick="applySection('smooth_vel')">↑ APPLY</button>
|
||||
</div>
|
||||
</div>
|
||||
<div id="smooth_vel-fields"></div>
|
||||
<div class="sec-status" id="smooth_vel-status"></div>
|
||||
</div>
|
||||
|
||||
<div class="section-card">
|
||||
<div class="sec-header">
|
||||
<div>
|
||||
<div class="sec-title">Battery Speed Limiter</div>
|
||||
<div class="sec-node">/battery_speed_limiter</div>
|
||||
</div>
|
||||
<div class="sec-actions">
|
||||
<button class="hbtn" onclick="loadSection('batt_speed')">↓ LOAD</button>
|
||||
<button class="hbtn apply-btn" onclick="applySection('batt_speed')">↑ APPLY</button>
|
||||
</div>
|
||||
</div>
|
||||
<div id="batt_speed-fields"></div>
|
||||
<div class="sec-status" id="batt_speed-status"></div>
|
||||
</div>
|
||||
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<!-- ═══ SAFETY TAB ═══ -->
|
||||
<div class="tab-panel" id="tab-safety">
|
||||
<div class="panel-col">
|
||||
|
||||
<div class="section-card">
|
||||
<div class="sec-header">
|
||||
<div>
|
||||
<div class="sec-title">Safety Zone</div>
|
||||
<div class="sec-node">/safety_zone</div>
|
||||
</div>
|
||||
<div class="sec-actions">
|
||||
<button class="hbtn" onclick="loadSection('safety_zone')">↓ LOAD</button>
|
||||
<button class="hbtn apply-btn" onclick="applySection('safety_zone')">↑ APPLY</button>
|
||||
</div>
|
||||
</div>
|
||||
<div id="safety_zone-fields"></div>
|
||||
<div class="sec-status" id="safety_zone-status"></div>
|
||||
</div>
|
||||
|
||||
<div class="section-card">
|
||||
<div class="sec-header">
|
||||
<div>
|
||||
<div class="sec-title">Power Supervisor</div>
|
||||
<div class="sec-node">/power_supervisor_node</div>
|
||||
</div>
|
||||
<div class="sec-actions">
|
||||
<button class="hbtn" onclick="loadSection('power_sup')">↓ LOAD</button>
|
||||
<button class="hbtn apply-btn" onclick="applySection('power_sup')">↑ APPLY</button>
|
||||
</div>
|
||||
</div>
|
||||
<div id="power_sup-fields"></div>
|
||||
<div class="sec-status" id="power_sup-status"></div>
|
||||
</div>
|
||||
|
||||
<div class="section-card">
|
||||
<div class="sec-header">
|
||||
<div>
|
||||
<div class="sec-title">LIDAR Avoidance</div>
|
||||
<div class="sec-node">/lidar_avoidance</div>
|
||||
</div>
|
||||
<div class="sec-actions">
|
||||
<button class="hbtn" onclick="loadSection('lidar_avoid')">↓ LOAD</button>
|
||||
<button class="hbtn apply-btn" onclick="applySection('lidar_avoid')">↑ APPLY</button>
|
||||
</div>
|
||||
</div>
|
||||
<div id="lidar_avoid-fields"></div>
|
||||
<div class="sec-status" id="lidar_avoid-status"></div>
|
||||
</div>
|
||||
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<!-- ═══ SENSORS TAB ═══ -->
|
||||
<div class="tab-panel" id="tab-sensors">
|
||||
<div class="panel-col">
|
||||
|
||||
<div class="section-card">
|
||||
<div class="sec-header">
|
||||
<div>
|
||||
<div class="sec-title">Sensor Enable / Disable</div>
|
||||
<div class="sec-node">various nodes</div>
|
||||
</div>
|
||||
<div class="sec-actions">
|
||||
<button class="hbtn" onclick="loadSection('sensor_toggles')">↓ LOAD ALL</button>
|
||||
<button class="hbtn apply-btn" onclick="applySection('sensor_toggles')">↑ APPLY ALL</button>
|
||||
</div>
|
||||
</div>
|
||||
<div id="sensor_toggles-fields"></div>
|
||||
<div class="sec-status" id="sensor_toggles-status"></div>
|
||||
</div>
|
||||
|
||||
<div class="section-card">
|
||||
<div class="sec-header">
|
||||
<div>
|
||||
<div class="sec-title">IMU / Odometry Fusion</div>
|
||||
<div class="sec-node">/uwb_imu_fusion</div>
|
||||
</div>
|
||||
<div class="sec-actions">
|
||||
<button class="hbtn" onclick="loadSection('imu_fusion')">↓ LOAD</button>
|
||||
<button class="hbtn apply-btn" onclick="applySection('imu_fusion')">↑ APPLY</button>
|
||||
</div>
|
||||
</div>
|
||||
<div id="imu_fusion-fields"></div>
|
||||
<div class="sec-status" id="imu_fusion-status"></div>
|
||||
</div>
|
||||
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<!-- ═══ SYSTEM TAB ═══ -->
|
||||
<div class="tab-panel" id="tab-system">
|
||||
<div class="panel-col">
|
||||
|
||||
<!-- Live diagnostics -->
|
||||
<div class="section-card">
|
||||
<div class="sec-header">
|
||||
<div>
|
||||
<div class="sec-title">System Diagnostics</div>
|
||||
<div class="sec-node">/diagnostics · auto-refresh 2 s</div>
|
||||
</div>
|
||||
<div class="sec-actions">
|
||||
<button class="hbtn" id="btn-refresh-diag" onclick="refreshDiag()">⟳ REFRESH</button>
|
||||
</div>
|
||||
</div>
|
||||
<div id="diag-grid">
|
||||
<div class="diag-placeholder">Connect to rosbridge to view diagnostics.</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<!-- WiFi / network -->
|
||||
<div class="section-card">
|
||||
<div class="sec-header">
|
||||
<div>
|
||||
<div class="sec-title">Network</div>
|
||||
<div class="sec-node">/diagnostics (wifi keys)</div>
|
||||
</div>
|
||||
</div>
|
||||
<div id="net-grid">
|
||||
<div class="diag-placeholder">Waiting for network data…</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<!-- Node list -->
|
||||
<div class="section-card">
|
||||
<div class="sec-header">
|
||||
<div>
|
||||
<div class="sec-title">Active ROS2 Nodes</div>
|
||||
<div class="sec-node">rosbridge /rosapi/nodes</div>
|
||||
</div>
|
||||
<div class="sec-actions">
|
||||
<button class="hbtn" onclick="loadNodeList()">⟳ REFRESH</button>
|
||||
</div>
|
||||
</div>
|
||||
<div id="node-list-wrap">
|
||||
<div class="diag-placeholder">Click refresh to list active nodes.</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
</div>
|
||||
</div>
|
||||
|
||||
</div><!-- /#main -->
|
||||
|
||||
<!-- ── Preset bar ── -->
|
||||
<div id="preset-bar">
|
||||
<span class="plbl">PRESETS:</span>
|
||||
<select id="preset-select">
|
||||
<option value="">— select —</option>
|
||||
</select>
|
||||
<button class="hbtn" onclick="loadPreset()">↓ LOAD</button>
|
||||
<input id="preset-name" type="text" placeholder="Preset name…" />
|
||||
<button class="hbtn" onclick="savePreset()">↑ SAVE</button>
|
||||
<button class="hbtn del-btn" onclick="deletePreset()">✕ DELETE</button>
|
||||
<div style="flex:1"></div>
|
||||
<button class="hbtn" onclick="resetAllToDefaults()">⟲ DEFAULTS</button>
|
||||
</div>
|
||||
|
||||
<!-- ── Footer ── -->
|
||||
<div id="footer">
|
||||
<span>ROS2 param services · rcl_interfaces/srv/GetParameters · SetParameters</span>
|
||||
<span>rosbridge: <code id="footer-ws">ws://localhost:9090</code></span>
|
||||
<span>settings panel — issue #614</span>
|
||||
</div>
|
||||
|
||||
<script src="settings_panel.js"></script>
|
||||
<script>
|
||||
document.getElementById('ws-input').addEventListener('input', (e) => {
|
||||
document.getElementById('footer-ws').textContent = e.target.value;
|
||||
});
|
||||
</script>
|
||||
</body>
|
||||
</html>
|
||||
716
ui/settings_panel.js
Normal file
716
ui/settings_panel.js
Normal file
@ -0,0 +1,716 @@
|
||||
/* settings_panel.js — Saltybot Settings Panel (Issue #614) */
|
||||
'use strict';
|
||||
|
||||
// ── ROS2 parameter type constants ──────────────────────────────────────────
|
||||
const P_BOOL = 1;
|
||||
const P_INT = 2;
|
||||
const P_DOUBLE = 3;
|
||||
|
||||
// ── Section / parameter definitions ───────────────────────────────────────
|
||||
// Each section: { id, node, params: [{name, label, type, min, max, step, unit, def}] }
|
||||
const SECTIONS = {
|
||||
|
||||
balance_pid: {
|
||||
node: 'balance_controller',
|
||||
params: [
|
||||
{ name: 'pid_p', label: 'Proportional (Kp)', type: P_DOUBLE, min: 0, max: 5, step: 0.01, unit: '', def: 0.5 },
|
||||
{ name: 'pid_i', label: 'Integral (Ki)', type: P_DOUBLE, min: 0, max: 2, step: 0.005, unit: '', def: 0.1 },
|
||||
{ name: 'pid_d', label: 'Derivative (Kd)', type: P_DOUBLE, min: 0, max: 1, step: 0.005, unit: '', def: 0.05 },
|
||||
{ name: 'i_clamp',label:'I clamp', type: P_DOUBLE, min: 0, max: 50, step: 0.5, unit: '', def: 10.0 },
|
||||
{ name: 'frequency',label:'Control rate', type: P_INT, min: 10, max: 200, step: 10, unit: 'Hz', def: 50 },
|
||||
],
|
||||
},
|
||||
|
||||
adaptive_pid_empty: {
|
||||
node: 'adaptive_pid',
|
||||
params: [
|
||||
{ name: 'kp_empty', label: 'Kp (empty load)', type: P_DOUBLE, min: 0, max: 50, step: 0.5, unit: '', def: 15.0 },
|
||||
{ name: 'ki_empty', label: 'Ki (empty load)', type: P_DOUBLE, min: 0, max: 5, step: 0.05, unit: '', def: 0.5 },
|
||||
{ name: 'kd_empty', label: 'Kd (empty load)', type: P_DOUBLE, min: 0, max: 10, step: 0.1, unit: '', def: 1.5 },
|
||||
{ name: 'kp_light', label: 'Kp (light load)', type: P_DOUBLE, min: 0, max: 50, step: 0.5, unit: '', def: 18.0 },
|
||||
{ name: 'ki_light', label: 'Ki (light load)', type: P_DOUBLE, min: 0, max: 5, step: 0.05, unit: '', def: 0.6 },
|
||||
{ name: 'kd_light', label: 'Kd (light load)', type: P_DOUBLE, min: 0, max: 10, step: 0.1, unit: '', def: 2.0 },
|
||||
{ name: 'kp_heavy', label: 'Kp (heavy load)', type: P_DOUBLE, min: 0, max: 50, step: 0.5, unit: '', def: 22.0 },
|
||||
{ name: 'ki_heavy', label: 'Ki (heavy load)', type: P_DOUBLE, min: 0, max: 5, step: 0.05, unit: '', def: 0.8 },
|
||||
{ name: 'kd_heavy', label: 'Kd (heavy load)', type: P_DOUBLE, min: 0, max: 10, step: 0.1, unit: '', def: 2.5 },
|
||||
],
|
||||
},
|
||||
|
||||
adaptive_pid_bounds: {
|
||||
node: 'adaptive_pid',
|
||||
params: [
|
||||
{ name: 'kp_min', label: 'Kp min', type: P_DOUBLE, min: 0, max: 20, step: 0.5, unit: '', def: 5.0 },
|
||||
{ name: 'kp_max', label: 'Kp max', type: P_DOUBLE, min: 0, max: 80, step: 1, unit: '', def: 40.0 },
|
||||
{ name: 'ki_min', label: 'Ki min', type: P_DOUBLE, min: 0, max: 5, step: 0.05, unit: '', def: 0.0 },
|
||||
{ name: 'ki_max', label: 'Ki max', type: P_DOUBLE, min: 0, max: 10, step: 0.1, unit: '', def: 5.0 },
|
||||
{ name: 'kd_min', label: 'Kd min', type: P_DOUBLE, min: 0, max: 5, step: 0.05, unit: '', def: 0.0 },
|
||||
{ name: 'kd_max', label: 'Kd max', type: P_DOUBLE, min: 0, max: 20, step: 0.2, unit: '', def: 10.0 },
|
||||
{ name: 'output_min', label: 'Output min', type: P_DOUBLE, min: -100, max: 0, step: 1, unit: '', def: -50.0},
|
||||
{ name: 'output_max', label: 'Output max', type: P_DOUBLE, min: 0, max: 100, step: 1, unit: '', def: 50.0 },
|
||||
],
|
||||
},
|
||||
|
||||
tank_limits: {
|
||||
node: 'tank_driver',
|
||||
params: [
|
||||
{ name: 'max_linear_vel', label: 'Max linear vel', type: P_DOUBLE, min: 0.1, max: 3.0, step: 0.05, unit: 'm/s', def: 1.0 },
|
||||
{ name: 'max_angular_vel', label: 'Max angular vel', type: P_DOUBLE, min: 0.1, max: 5.0, step: 0.1, unit: 'rad/s', def: 2.5 },
|
||||
{ name: 'max_speed_ms', label: 'Max drive speed', type: P_DOUBLE, min: 0.1, max: 5.0, step: 0.1, unit: 'm/s', def: 1.5 },
|
||||
{ name: 'slip_factor', label: 'Track slip factor',type: P_DOUBLE,min: 0, max: 0.5, step: 0.01, unit: '', def: 0.3 },
|
||||
{ name: 'watchdog_timeout_s',label:'Watchdog timeout',type:P_DOUBLE, min: 0.1, max: 2.0, step: 0.05, unit: 's', def: 0.3 },
|
||||
],
|
||||
},
|
||||
|
||||
smooth_vel: {
|
||||
node: 'smooth_velocity_controller',
|
||||
params: [
|
||||
{ name: 'max_linear_accel', label: 'Max linear accel', type: P_DOUBLE, min: 0.1, max: 5.0, step: 0.05, unit: 'm/s²', def: 0.5 },
|
||||
{ name: 'max_linear_decel', label: 'Max linear decel', type: P_DOUBLE, min: 0.1, max: 5.0, step: 0.05, unit: 'm/s²', def: 0.8 },
|
||||
{ name: 'max_angular_accel', label: 'Max angular accel', type: P_DOUBLE, min: 0.1, max: 5.0, step: 0.1, unit: 'rad/s²', def: 1.0 },
|
||||
{ name: 'max_angular_decel', label: 'Max angular decel', type: P_DOUBLE, min: 0.1, max: 5.0, step: 0.1, unit: 'rad/s²', def: 1.0 },
|
||||
],
|
||||
},
|
||||
|
||||
batt_speed: {
|
||||
node: 'battery_speed_limiter',
|
||||
params: [
|
||||
{ name: 'speed_factor_full', label: 'Speed factor (full)', type: P_DOUBLE, min: 0, max: 1, step: 0.05, unit: '', def: 1.0 },
|
||||
{ name: 'speed_factor_reduced', label: 'Speed factor (reduced)', type: P_DOUBLE, min: 0, max: 1, step: 0.05, unit: '', def: 0.7 },
|
||||
{ name: 'speed_factor_critical', label: 'Speed factor (critical)', type: P_DOUBLE, min: 0, max: 1, step: 0.05, unit: '', def: 0.4 },
|
||||
],
|
||||
},
|
||||
|
||||
safety_zone: {
|
||||
node: 'safety_zone',
|
||||
params: [
|
||||
{ name: 'danger_range_m', label: 'Danger range', type: P_DOUBLE, min: 0.05, max: 1.0, step: 0.01, unit: 'm', def: 0.30 },
|
||||
{ name: 'warn_range_m', label: 'Warn range', type: P_DOUBLE, min: 0.2, max: 5.0, step: 0.05, unit: 'm', def: 1.00 },
|
||||
{ name: 'forward_arc_deg', label: 'Forward arc (±)', type: P_DOUBLE, min: 10, max: 180, step: 5, unit: '°', def: 60.0 },
|
||||
{ name: 'estop_debounce_frames',label: 'E-stop debounce', type: P_INT, min: 1, max: 20, step: 1, unit: 'frames', def: 2 },
|
||||
{ name: 'min_valid_range_m', label: 'Min valid range', type: P_DOUBLE, min: 0.01, max: 0.5, step: 0.01, unit: 'm', def: 0.05 },
|
||||
{ name: 'publish_rate', label: 'Publish rate', type: P_DOUBLE, min: 1, max: 50, step: 1, unit: 'Hz', def: 10.0 },
|
||||
],
|
||||
},
|
||||
|
||||
power_sup: {
|
||||
node: 'power_supervisor_node',
|
||||
params: [
|
||||
{ name: 'warning_pct', label: 'Warning threshold', type: P_DOUBLE, min: 5, max: 60, step: 1, unit: '%', def: 30.0 },
|
||||
{ name: 'dock_search_pct', label: 'Dock-search threshold',type: P_DOUBLE, min: 5, max: 50, step: 1, unit: '%', def: 20.0 },
|
||||
{ name: 'critical_pct', label: 'Critical threshold', type: P_DOUBLE, min: 2, max: 30, step: 1, unit: '%', def: 10.0 },
|
||||
{ name: 'emergency_pct', label: 'Emergency threshold', type: P_DOUBLE, min: 1, max: 15, step: 1, unit: '%', def: 5.0 },
|
||||
{ name: 'warn_speed_factor', label: 'Speed factor (warn)',type: P_DOUBLE, min: 0, max: 1, step: 0.05, unit: '', def: 0.6 },
|
||||
{ name: 'critical_speed_factor',label:'Speed factor (crit)',type: P_DOUBLE, min: 0, max: 1, step: 0.05, unit: '', def: 0.3 },
|
||||
],
|
||||
},
|
||||
|
||||
lidar_avoid: {
|
||||
node: 'lidar_avoidance',
|
||||
params: [
|
||||
{ name: 'emergency_stop_distance', label: 'E-stop distance', type: P_DOUBLE, min: 0.1, max: 3.0, step: 0.05, unit: 'm', def: 0.5 },
|
||||
{ name: 'min_safety_zone', label: 'Min safety zone', type: P_DOUBLE, min: 0.1, max: 2.0, step: 0.05, unit: 'm', def: 0.6 },
|
||||
{ name: 'safety_zone_at_max_speed',label: 'Zone at max speed', type: P_DOUBLE, min: 0.5, max: 10, step: 0.1, unit: 'm', def: 3.0 },
|
||||
{ name: 'max_speed_reference', label: 'Max speed reference',type: P_DOUBLE, min: 0.5, max: 20, step: 0.1, unit: 'm/s', def: 5.56 },
|
||||
],
|
||||
},
|
||||
|
||||
sensor_toggles: {
|
||||
node: 'safety_zone', // placeholder — booleans often live on their own node
|
||||
params: [
|
||||
{ name: 'estop_all_arcs', label: 'E-stop all arcs', type: P_BOOL, unit: '', def: false, desc: 'Any sector triggers e-stop' },
|
||||
{ name: 'lidar_enabled', label: 'LIDAR enabled', type: P_BOOL, unit: '', def: true, desc: '/scan input active' },
|
||||
{ name: 'uwb_enabled', label: 'UWB positioning', type: P_BOOL, unit: '', def: true, desc: 'UWB anchors active' },
|
||||
],
|
||||
},
|
||||
|
||||
imu_fusion: {
|
||||
node: 'uwb_imu_fusion',
|
||||
params: [
|
||||
{ name: 'uwb_weight', label: 'UWB weight', type: P_DOUBLE, min: 0, max: 1, step: 0.05, unit: '', def: 0.7 },
|
||||
{ name: 'imu_weight', label: 'IMU weight', type: P_DOUBLE, min: 0, max: 1, step: 0.05, unit: '', def: 0.3 },
|
||||
{ name: 'publish_rate', label: 'Publish rate', type: P_DOUBLE, min: 1, max: 200, step: 1, unit: 'Hz', def: 50.0 },
|
||||
],
|
||||
},
|
||||
|
||||
};
|
||||
|
||||
// ── Runtime state: current values per section ──────────────────────────────
|
||||
const values = {}; // values[sectionId][paramName] = currentValue
|
||||
const dirty = {}; // dirty[sectionId][paramName] = true if changed vs loaded
|
||||
|
||||
// Initialise values from defaults
|
||||
Object.keys(SECTIONS).forEach(sid => {
|
||||
values[sid] = {};
|
||||
dirty[sid] = {};
|
||||
SECTIONS[sid].params.forEach(p => {
|
||||
values[sid][p.name] = p.def;
|
||||
dirty[sid][p.name] = false;
|
||||
});
|
||||
});
|
||||
|
||||
// ── ROS ────────────────────────────────────────────────────────────────────
|
||||
let ros = null;
|
||||
|
||||
function getService(nodeName, type) {
|
||||
return new ROSLIB.Service({ ros, name: `/${nodeName}/${type}`, serviceType: `rcl_interfaces/srv/${type === 'get_parameters' ? 'GetParameters' : 'SetParameters'}` });
|
||||
}
|
||||
|
||||
function extractValue(rosVal) {
|
||||
switch (rosVal.type) {
|
||||
case P_BOOL: return rosVal.bool_value;
|
||||
case P_INT: return rosVal.integer_value;
|
||||
case P_DOUBLE: return rosVal.double_value;
|
||||
default: return undefined;
|
||||
}
|
||||
}
|
||||
|
||||
function makeRosValue(type, value) {
|
||||
const v = { type };
|
||||
if (type === P_BOOL) v.bool_value = !!value;
|
||||
if (type === P_INT) v.integer_value = Math.round(value);
|
||||
if (type === P_DOUBLE) v.double_value = parseFloat(value);
|
||||
return v;
|
||||
}
|
||||
|
||||
// ── Section load / apply ───────────────────────────────────────────────────
|
||||
window.loadSection = function(sid) {
|
||||
if (!ros) { setStatus(sid, 'err', 'Not connected'); return; }
|
||||
const sec = SECTIONS[sid];
|
||||
const svc = getService(sec.node, 'get_parameters');
|
||||
const names = sec.params.map(p => p.name);
|
||||
|
||||
setStatus(sid, 'loading', `Loading from /${sec.node}…`);
|
||||
|
||||
svc.callService({ names }, (resp) => {
|
||||
if (!resp || !resp.values) {
|
||||
setStatus(sid, 'err', 'No response'); return;
|
||||
}
|
||||
sec.params.forEach((p, i) => {
|
||||
const v = extractValue(resp.values[i]);
|
||||
if (v !== undefined) {
|
||||
values[sid][p.name] = v;
|
||||
dirty[sid][p.name] = false;
|
||||
updateFieldUI(sid, p.name, v, false);
|
||||
}
|
||||
});
|
||||
setStatus(sid, 'ok', `Loaded ${resp.values.length} params`);
|
||||
}, (err) => {
|
||||
setStatus(sid, 'err', `Error: ${err}`);
|
||||
});
|
||||
};
|
||||
|
||||
window.applySection = function(sid) {
|
||||
if (!ros) { setStatus(sid, 'err', 'Not connected'); return; }
|
||||
const sec = SECTIONS[sid];
|
||||
const svc = getService(sec.node, 'set_parameters');
|
||||
|
||||
const parameters = sec.params.map(p => ({
|
||||
name: p.name,
|
||||
value: makeRosValue(p.type, values[sid][p.name]),
|
||||
}));
|
||||
|
||||
setStatus(sid, 'loading', `Applying to /${sec.node}…`);
|
||||
|
||||
svc.callService({ parameters }, (resp) => {
|
||||
if (!resp || !resp.results) {
|
||||
setStatus(sid, 'err', 'No response'); return;
|
||||
}
|
||||
const failures = resp.results.filter(r => !r.successful);
|
||||
if (failures.length === 0) {
|
||||
sec.params.forEach(p => { dirty[sid][p.name] = false; });
|
||||
refreshFieldDirty(sid);
|
||||
setStatus(sid, 'ok', `Applied ${parameters.length} params ✓`);
|
||||
} else {
|
||||
const reasons = failures.map(r => r.reason).join('; ');
|
||||
setStatus(sid, 'err', `${failures.length} failed: ${reasons}`);
|
||||
}
|
||||
}, (err) => {
|
||||
setStatus(sid, 'err', `Error: ${err}`);
|
||||
});
|
||||
};
|
||||
|
||||
// ── UI builder ─────────────────────────────────────────────────────────────
|
||||
function buildFields() {
|
||||
Object.keys(SECTIONS).forEach(sid => {
|
||||
const sec = SECTIONS[sid];
|
||||
const container = document.getElementById(`${sid}-fields`);
|
||||
if (!container) return;
|
||||
|
||||
sec.params.forEach(p => {
|
||||
if (p.type === P_BOOL) {
|
||||
// Toggle row
|
||||
const row = document.createElement('div');
|
||||
row.className = 'toggle-row';
|
||||
row.innerHTML = `
|
||||
<span class="param-label">${p.label}</span>
|
||||
<span class="toggle-desc">${p.desc || ''}</span>
|
||||
<label class="toggle-switch">
|
||||
<input type="checkbox" id="tgl-${sid}-${p.name}"
|
||||
${p.def ? 'checked' : ''}>
|
||||
<div class="toggle-track"></div>
|
||||
</label>`;
|
||||
container.appendChild(row);
|
||||
|
||||
const cb = row.querySelector(`#tgl-${sid}-${p.name}`);
|
||||
cb.addEventListener('change', () => {
|
||||
values[sid][p.name] = cb.checked;
|
||||
dirty[sid][p.name] = true;
|
||||
});
|
||||
} else {
|
||||
// Slider + number input row
|
||||
const row = document.createElement('div');
|
||||
row.className = 'param-row';
|
||||
const sliderMin = p.min !== undefined ? p.min : 0;
|
||||
const sliderMax = p.max !== undefined ? p.max : 100;
|
||||
const sliderStep = p.step || 0.01;
|
||||
const defVal = p.def !== undefined ? p.def : 0;
|
||||
|
||||
row.innerHTML = `
|
||||
<span class="param-label">${p.label}</span>
|
||||
<input type="range" class="param-slider"
|
||||
id="sld-${sid}-${p.name}"
|
||||
min="${sliderMin}" max="${sliderMax}" step="${sliderStep}"
|
||||
value="${defVal}">
|
||||
<input type="number" class="param-input"
|
||||
id="inp-${sid}-${p.name}"
|
||||
min="${sliderMin}" max="${sliderMax}" step="${sliderStep}"
|
||||
value="${defVal}">
|
||||
<span class="param-unit">${p.unit || ''}</span>`;
|
||||
container.appendChild(row);
|
||||
|
||||
const slider = row.querySelector(`#sld-${sid}-${p.name}`);
|
||||
const input = row.querySelector(`#inp-${sid}-${p.name}`);
|
||||
|
||||
slider.addEventListener('input', () => {
|
||||
const v = parseFloat(slider.value);
|
||||
input.value = v;
|
||||
values[sid][p.name] = v;
|
||||
dirty[sid][p.name] = true;
|
||||
input.classList.add('changed');
|
||||
slider.classList.add('changed');
|
||||
});
|
||||
|
||||
input.addEventListener('change', () => {
|
||||
let v = parseFloat(input.value);
|
||||
v = Math.max(sliderMin, Math.min(sliderMax, v));
|
||||
input.value = v;
|
||||
slider.value = v;
|
||||
values[sid][p.name] = v;
|
||||
dirty[sid][p.name] = true;
|
||||
input.classList.add('changed');
|
||||
slider.classList.add('changed');
|
||||
});
|
||||
}
|
||||
});
|
||||
});
|
||||
}
|
||||
|
||||
function updateFieldUI(sid, paramName, value, markDirty) {
|
||||
const sec = SECTIONS[sid];
|
||||
const p = sec.params.find(x => x.name === paramName);
|
||||
if (!p) return;
|
||||
|
||||
if (p.type === P_BOOL) {
|
||||
const cb = document.getElementById(`tgl-${sid}-${paramName}`);
|
||||
if (cb) cb.checked = !!value;
|
||||
} else {
|
||||
const sld = document.getElementById(`sld-${sid}-${paramName}`);
|
||||
const inp = document.getElementById(`inp-${sid}-${paramName}`);
|
||||
if (sld) sld.value = value;
|
||||
if (inp) inp.value = value;
|
||||
if (!markDirty) {
|
||||
if (sld) sld.classList.remove('changed');
|
||||
if (inp) inp.classList.remove('changed');
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
function refreshFieldDirty(sid) {
|
||||
const sec = SECTIONS[sid];
|
||||
sec.params.forEach(p => {
|
||||
if (p.type !== P_BOOL) {
|
||||
const sld = document.getElementById(`sld-${sid}-${p.name}`);
|
||||
const inp = document.getElementById(`inp-${sid}-${p.name}`);
|
||||
if (!dirty[sid][p.name]) {
|
||||
if (sld) sld.classList.remove('changed');
|
||||
if (inp) inp.classList.remove('changed');
|
||||
}
|
||||
}
|
||||
});
|
||||
}
|
||||
|
||||
function setStatus(sid, cls, msg) {
|
||||
const el = document.getElementById(`${sid}-status`);
|
||||
if (!el) return;
|
||||
el.className = `sec-status ${cls}`;
|
||||
el.textContent = msg;
|
||||
if (cls === 'ok') setTimeout(() => { el.textContent = ''; el.className = 'sec-status'; }, 4000);
|
||||
}
|
||||
|
||||
// ── Tabs ───────────────────────────────────────────────────────────────────
|
||||
document.querySelectorAll('.tab-btn').forEach(btn => {
|
||||
btn.addEventListener('click', () => {
|
||||
document.querySelectorAll('.tab-btn').forEach(b => b.classList.remove('active'));
|
||||
document.querySelectorAll('.tab-panel').forEach(p => p.classList.remove('active'));
|
||||
btn.classList.add('active');
|
||||
document.getElementById(`tab-${btn.dataset.tab}`).classList.add('active');
|
||||
if (btn.dataset.tab === 'system') startDiagRefresh();
|
||||
});
|
||||
});
|
||||
|
||||
// ── Presets ────────────────────────────────────────────────────────────────
|
||||
const PRESET_KEY = 'saltybot_settings_presets';
|
||||
|
||||
function getPresets() {
|
||||
try { return JSON.parse(localStorage.getItem(PRESET_KEY) || '{}'); } catch(_) { return {}; }
|
||||
}
|
||||
|
||||
function savePresetsToStorage(presets) {
|
||||
localStorage.setItem(PRESET_KEY, JSON.stringify(presets));
|
||||
}
|
||||
|
||||
function refreshPresetSelect() {
|
||||
const sel = document.getElementById('preset-select');
|
||||
const cur = sel.value;
|
||||
sel.innerHTML = '<option value="">— select —</option>';
|
||||
const presets = getPresets();
|
||||
Object.keys(presets).sort().forEach(name => {
|
||||
const opt = document.createElement('option');
|
||||
opt.value = name;
|
||||
opt.textContent = name;
|
||||
sel.appendChild(opt);
|
||||
});
|
||||
if (cur) sel.value = cur;
|
||||
}
|
||||
|
||||
function snapshotAllValues() {
|
||||
const snap = {};
|
||||
Object.keys(values).forEach(sid => {
|
||||
snap[sid] = Object.assign({}, values[sid]);
|
||||
});
|
||||
return snap;
|
||||
}
|
||||
|
||||
window.savePreset = function() {
|
||||
const nameInput = document.getElementById('preset-name');
|
||||
const name = nameInput.value.trim();
|
||||
if (!name) { alert('Enter a preset name'); return; }
|
||||
const presets = getPresets();
|
||||
presets[name] = snapshotAllValues();
|
||||
savePresetsToStorage(presets);
|
||||
nameInput.value = '';
|
||||
refreshPresetSelect();
|
||||
document.getElementById('preset-select').value = name;
|
||||
flashSaved();
|
||||
};
|
||||
|
||||
window.loadPreset = function() {
|
||||
const name = document.getElementById('preset-select').value;
|
||||
if (!name) return;
|
||||
const presets = getPresets();
|
||||
const snap = presets[name];
|
||||
if (!snap) return;
|
||||
Object.keys(snap).forEach(sid => {
|
||||
if (!values[sid]) return;
|
||||
Object.keys(snap[sid]).forEach(paramName => {
|
||||
values[sid][paramName] = snap[sid][paramName];
|
||||
dirty[sid][paramName] = true;
|
||||
updateFieldUI(sid, paramName, snap[sid][paramName], true);
|
||||
});
|
||||
});
|
||||
flashSaved();
|
||||
};
|
||||
|
||||
window.deletePreset = function() {
|
||||
const name = document.getElementById('preset-select').value;
|
||||
if (!name) return;
|
||||
if (!confirm(`Delete preset "${name}"?`)) return;
|
||||
const presets = getPresets();
|
||||
delete presets[name];
|
||||
savePresetsToStorage(presets);
|
||||
refreshPresetSelect();
|
||||
};
|
||||
|
||||
window.resetAllToDefaults = function() {
|
||||
if (!confirm('Reset all fields to built-in defaults?')) return;
|
||||
Object.keys(SECTIONS).forEach(sid => {
|
||||
SECTIONS[sid].params.forEach(p => {
|
||||
values[sid][p.name] = p.def;
|
||||
dirty[sid][p.name] = false;
|
||||
updateFieldUI(sid, p.name, p.def, false);
|
||||
});
|
||||
});
|
||||
};
|
||||
|
||||
function flashSaved() {
|
||||
const el = document.getElementById('save-indicator');
|
||||
el.classList.remove('hidden');
|
||||
el.style.animation = 'none';
|
||||
void el.offsetHeight;
|
||||
el.style.animation = 'fadeout 2s forwards';
|
||||
setTimeout(() => el.classList.add('hidden'), 2100);
|
||||
}
|
||||
|
||||
// ── System tab: diagnostics ────────────────────────────────────────────────
|
||||
let diagTopic = null;
|
||||
let diagRefreshTimer = null;
|
||||
let diagState = {
|
||||
cpuTemp: null, gpuTemp: null, boardTemp: null,
|
||||
motorTempL: null, motorTempR: null,
|
||||
ramUsed: null, ramTotal: null,
|
||||
gpuUsed: null, gpuTotal: null,
|
||||
diskUsed: null, diskTotal: null,
|
||||
rssi: null, latency: null, mqttConnected: null,
|
||||
nodes: [],
|
||||
lastUpdate: null,
|
||||
};
|
||||
|
||||
function startDiagRefresh() {
|
||||
if (!ros || diagTopic) return;
|
||||
diagTopic = new ROSLIB.Topic({
|
||||
ros, name: '/diagnostics', messageType: 'diagnostic_msgs/DiagnosticArray',
|
||||
throttle_rate: 2000,
|
||||
});
|
||||
diagTopic.subscribe(onDiagnostics);
|
||||
}
|
||||
|
||||
function stopDiagRefresh() {
|
||||
if (diagTopic) { diagTopic.unsubscribe(); diagTopic = null; }
|
||||
}
|
||||
|
||||
function onDiagnostics(msg) {
|
||||
const kv = {};
|
||||
(msg.status || []).forEach(status => {
|
||||
(status.values || []).forEach(pair => {
|
||||
kv[pair.key] = pair.value;
|
||||
});
|
||||
diagState.nodes.push({ name: status.name, level: status.level, msg: status.message });
|
||||
if (diagState.nodes.length > 40) diagState.nodes.splice(0, diagState.nodes.length - 40);
|
||||
});
|
||||
|
||||
// Extract known keys
|
||||
if (kv.cpu_temp_c) diagState.cpuTemp = parseFloat(kv.cpu_temp_c);
|
||||
if (kv.gpu_temp_c) diagState.gpuTemp = parseFloat(kv.gpu_temp_c);
|
||||
if (kv.board_temp_c) diagState.boardTemp = parseFloat(kv.board_temp_c);
|
||||
if (kv.motor_temp_l_c) diagState.motorTempL = parseFloat(kv.motor_temp_l_c);
|
||||
if (kv.motor_temp_r_c) diagState.motorTempR = parseFloat(kv.motor_temp_r_c);
|
||||
if (kv.ram_used_mb) diagState.ramUsed = parseFloat(kv.ram_used_mb);
|
||||
if (kv.ram_total_mb) diagState.ramTotal = parseFloat(kv.ram_total_mb);
|
||||
if (kv.gpu_used_mb) diagState.gpuUsed = parseFloat(kv.gpu_used_mb);
|
||||
if (kv.gpu_total_mb) diagState.gpuTotal = parseFloat(kv.gpu_total_mb);
|
||||
if (kv.disk_used_gb) diagState.diskUsed = parseFloat(kv.disk_used_gb);
|
||||
if (kv.disk_total_gb) diagState.diskTotal = parseFloat(kv.disk_total_gb);
|
||||
if (kv.wifi_rssi_dbm) diagState.rssi = parseFloat(kv.wifi_rssi_dbm);
|
||||
if (kv.wifi_latency_ms) diagState.latency = parseFloat(kv.wifi_latency_ms);
|
||||
if (kv.mqtt_connected !== undefined) diagState.mqttConnected = kv.mqtt_connected === 'true';
|
||||
|
||||
diagState.lastUpdate = new Date();
|
||||
renderDiag();
|
||||
renderNet();
|
||||
}
|
||||
|
||||
window.refreshDiag = function() {
|
||||
startDiagRefresh();
|
||||
renderDiag();
|
||||
};
|
||||
|
||||
function tempColor(t) {
|
||||
if (t === null) return '';
|
||||
if (t > 80) return 'err';
|
||||
if (t > 65) return 'warn';
|
||||
return 'ok';
|
||||
}
|
||||
|
||||
function pct(used, total) {
|
||||
if (!total) return '—';
|
||||
return ((used / total) * 100).toFixed(0) + '%';
|
||||
}
|
||||
|
||||
function renderDiag() {
|
||||
const g = document.getElementById('diag-grid');
|
||||
if (!g) return;
|
||||
const d = diagState;
|
||||
if (d.lastUpdate === null) {
|
||||
g.innerHTML = '<div class="diag-placeholder">Waiting for /diagnostics…</div>';
|
||||
return;
|
||||
}
|
||||
|
||||
const cards = [
|
||||
{
|
||||
title: 'Temperature',
|
||||
rows: [
|
||||
{ k: 'CPU', v: d.cpuTemp !== null ? d.cpuTemp.toFixed(1) + ' °C' : '—', cls: tempColor(d.cpuTemp) },
|
||||
{ k: 'GPU', v: d.gpuTemp !== null ? d.gpuTemp.toFixed(1) + ' °C' : '—', cls: tempColor(d.gpuTemp) },
|
||||
{ k: 'Board',v:d.boardTemp !== null ? d.boardTemp.toFixed(1)+' °C':'—', cls:tempColor(d.boardTemp)},
|
||||
{ k: 'Motor L', v: d.motorTempL !== null ? d.motorTempL.toFixed(1)+' °C':'—', cls:tempColor(d.motorTempL)},
|
||||
{ k: 'Motor R', v: d.motorTempR !== null ? d.motorTempR.toFixed(1)+' °C':'—', cls:tempColor(d.motorTempR)},
|
||||
],
|
||||
},
|
||||
{
|
||||
title: 'Memory',
|
||||
rows: [
|
||||
{ k: 'RAM used', v: d.ramUsed !== null ? d.ramUsed.toFixed(0)+' MB': '—', cls:'' },
|
||||
{ k: 'RAM total', v: d.ramTotal !== null ? d.ramTotal.toFixed(0)+' MB': '—', cls:'' },
|
||||
{ k: 'RAM %', v: pct(d.ramUsed, d.ramTotal), cls: d.ramUsed/d.ramTotal > 0.85 ? 'warn' : 'ok' },
|
||||
{ k: 'GPU mem', v: d.gpuUsed !== null ? d.gpuUsed.toFixed(0)+' MB': '—', cls:'' },
|
||||
{ k: 'GPU %', v: pct(d.gpuUsed, d.gpuTotal), cls:'' },
|
||||
],
|
||||
},
|
||||
{
|
||||
title: 'Storage',
|
||||
rows: [
|
||||
{ k: 'Disk used', v: d.diskUsed !== null ? d.diskUsed.toFixed(1)+' GB': '—', cls:'' },
|
||||
{ k: 'Disk total', v: d.diskTotal !== null ? d.diskTotal.toFixed(1)+' GB': '—', cls:'' },
|
||||
{ k: 'Disk %', v: pct(d.diskUsed, d.diskTotal), cls: d.diskUsed/d.diskTotal > 0.9 ? 'err' : d.diskUsed/d.diskTotal > 0.75 ? 'warn' : 'ok' },
|
||||
],
|
||||
},
|
||||
{
|
||||
title: 'Updated',
|
||||
rows: [
|
||||
{ k: 'Last msg', v: d.lastUpdate ? d.lastUpdate.toLocaleTimeString() : '—', cls:'' },
|
||||
],
|
||||
},
|
||||
];
|
||||
|
||||
g.innerHTML = cards.map(c => `
|
||||
<div class="diag-card">
|
||||
<div class="diag-card-title">${c.title}</div>
|
||||
${c.rows.map(r => `<div class="diag-kv"><span class="diag-k">${r.k}</span><span class="diag-v ${r.cls||''}">${r.v}</span></div>`).join('')}
|
||||
</div>`).join('');
|
||||
}
|
||||
|
||||
function rssiColor(rssi) {
|
||||
if (rssi === null) return '';
|
||||
if (rssi > -50) return 'ok';
|
||||
if (rssi > -70) return 'warn';
|
||||
return 'err';
|
||||
}
|
||||
|
||||
function rssiBarCount(rssi) {
|
||||
if (rssi === null) return 0;
|
||||
if (rssi > -50) return 5;
|
||||
if (rssi > -60) return 4;
|
||||
if (rssi > -70) return 3;
|
||||
if (rssi > -80) return 2;
|
||||
return 1;
|
||||
}
|
||||
|
||||
function latencyColor(ms) {
|
||||
if (ms === null) return '';
|
||||
if (ms < 50) return 'ok';
|
||||
if (ms < 150) return 'warn';
|
||||
return 'err';
|
||||
}
|
||||
|
||||
function renderNet() {
|
||||
const g = document.getElementById('net-grid');
|
||||
if (!g) return;
|
||||
const d = diagState;
|
||||
const bars = rssiBarCount(d.rssi);
|
||||
const heights = [4, 6, 9, 12, 15];
|
||||
|
||||
const barsHtml = heights.map((h, i) =>
|
||||
`<div class="sig-bar ${i < bars ? 'lit' : ''}" style="height:${h}px"></div>`
|
||||
).join('');
|
||||
|
||||
g.innerHTML = `
|
||||
<div class="diag-card">
|
||||
<div class="diag-card-title">WiFi</div>
|
||||
<div class="diag-kv">
|
||||
<span class="diag-k">RSSI</span>
|
||||
<span class="diag-v ${rssiColor(d.rssi)}">${d.rssi !== null ? d.rssi + ' dBm' : '—'}</span>
|
||||
</div>
|
||||
<div class="diag-kv">
|
||||
<span class="diag-k">Signal</span>
|
||||
<span class="diag-v"><div class="sig-bars">${barsHtml}</div></span>
|
||||
</div>
|
||||
<div class="diag-kv">
|
||||
<span class="diag-k">Latency</span>
|
||||
<span class="diag-v ${latencyColor(d.latency)}">${d.latency !== null ? d.latency.toFixed(0) + ' ms' : '—'}</span>
|
||||
</div>
|
||||
</div>
|
||||
<div class="diag-card">
|
||||
<div class="diag-card-title">Services</div>
|
||||
<div class="diag-kv">
|
||||
<span class="diag-k">MQTT</span>
|
||||
<span class="diag-v ${d.mqttConnected === null ? '' : d.mqttConnected ? 'ok' : 'err'}">
|
||||
${d.mqttConnected === null ? '—' : d.mqttConnected ? 'Connected' : 'Disconnected'}
|
||||
</span>
|
||||
</div>
|
||||
<div class="diag-kv">
|
||||
<span class="diag-k">rosbridge</span>
|
||||
<span class="diag-v ${ros ? 'ok' : 'err'}">${ros ? 'Connected' : 'Disconnected'}</span>
|
||||
</div>
|
||||
</div>`;
|
||||
}
|
||||
|
||||
// ── Node list ──────────────────────────────────────────────────────────────
|
||||
window.loadNodeList = function() {
|
||||
if (!ros) return;
|
||||
const svc = new ROSLIB.Service({
|
||||
ros, name: '/rosapi/nodes', serviceType: 'rosapi/Nodes',
|
||||
});
|
||||
svc.callService({}, (resp) => {
|
||||
const wrap = document.getElementById('node-list-wrap');
|
||||
if (!resp || !resp.nodes) { wrap.innerHTML = '<div class="diag-placeholder">No response</div>'; return; }
|
||||
const sorted = [...resp.nodes].sort();
|
||||
wrap.innerHTML = sorted.map(n =>
|
||||
`<span class="node-chip active-node">${n}</span>`
|
||||
).join('');
|
||||
}, () => {
|
||||
document.getElementById('node-list-wrap').innerHTML =
|
||||
'<div class="diag-placeholder">Service unavailable — ensure rosapi is running</div>';
|
||||
});
|
||||
};
|
||||
|
||||
// ── Connection ─────────────────────────────────────────────────────────────
|
||||
const $connDot = document.getElementById('conn-dot');
|
||||
const $connLabel = document.getElementById('conn-label');
|
||||
|
||||
function connect() {
|
||||
const url = document.getElementById('ws-input').value.trim() || 'ws://localhost:9090';
|
||||
if (ros) { stopDiagRefresh(); try { ros.close(); } catch(_){} }
|
||||
|
||||
$connLabel.textContent = 'Connecting…';
|
||||
$connLabel.style.color = '#d97706';
|
||||
$connDot.className = '';
|
||||
|
||||
ros = new ROSLIB.Ros({ url });
|
||||
|
||||
ros.on('connection', () => {
|
||||
$connDot.className = 'connected';
|
||||
$connLabel.style.color = '#22c55e';
|
||||
$connLabel.textContent = 'Connected';
|
||||
localStorage.setItem('settings_ws_url', url);
|
||||
// Auto-start diagnostics if system tab visible
|
||||
const sysPanel = document.getElementById('tab-system');
|
||||
if (sysPanel && sysPanel.classList.contains('active')) startDiagRefresh();
|
||||
});
|
||||
|
||||
ros.on('error', () => {
|
||||
$connDot.className = 'error';
|
||||
$connLabel.style.color = '#ef4444';
|
||||
$connLabel.textContent = 'Error';
|
||||
});
|
||||
|
||||
ros.on('close', () => {
|
||||
$connDot.className = '';
|
||||
$connLabel.style.color = '#6b7280';
|
||||
$connLabel.textContent = 'Disconnected';
|
||||
stopDiagRefresh();
|
||||
ros = null;
|
||||
});
|
||||
}
|
||||
|
||||
document.getElementById('btn-connect').addEventListener('click', connect);
|
||||
|
||||
// ── Init ───────────────────────────────────────────────────────────────────
|
||||
buildFields();
|
||||
refreshPresetSelect();
|
||||
|
||||
const savedUrl = localStorage.getItem('settings_ws_url');
|
||||
if (savedUrl) {
|
||||
document.getElementById('ws-input').value = savedUrl;
|
||||
document.getElementById('footer-ws').textContent = savedUrl;
|
||||
connect();
|
||||
}
|
||||
Loading…
x
Reference in New Issue
Block a user