Compare commits
1 Commits
da3ee19688
...
8d35adfb9b
| Author | SHA1 | Date | |
|---|---|---|---|
| 8d35adfb9b |
@ -1,530 +0,0 @@
|
||||
// ============================================================
|
||||
// charging_dock.scad — SaltyLab Charging Dock Station
|
||||
// Issue: #159 Agent: sl-mechanical Date: 2026-03-01
|
||||
// ============================================================
|
||||
//
|
||||
// Universal charging dock for SaltyLab / SaltyRover / SaltyTank.
|
||||
// Robot drives forward into V-guide funnel; spring-loaded pogo pins
|
||||
// make contact with the robot receiver plate (charging_dock_receiver.scad).
|
||||
//
|
||||
// Power: 5 V / 5 A (25 W) via 2× high-current pogo pins (+/-)
|
||||
// Alignment tolerance: ±20 mm lateral (V-guide funnels to centre)
|
||||
//
|
||||
// Dock architecture (top view):
|
||||
//
|
||||
// ┌─────────────────────────────────┐ ← back wall (robot stops here)
|
||||
// │ PSU shelf │
|
||||
// │ [PSU] [LED ×4] │
|
||||
// │ [POGO+][POGO-] │ ← pogo face (robot contact)
|
||||
// └────\ /────────┘
|
||||
// \ V-guide rails /
|
||||
// \ /
|
||||
// ╲ ╱ ← dock entry, ±20 mm funnel
|
||||
//
|
||||
// Components (this file):
|
||||
// Part A — dock_base() weighted base plate with ballast pockets
|
||||
// Part B — back_wall() upright back panel + pogo housing + LED bezel
|
||||
// Part C — guide_rail(side) V-funnel guide rail, L/R (print 2×)
|
||||
// Part D — aruco_mount() ArUco marker frame at dock entrance
|
||||
// Part E — psu_bracket() PSU retention bracket (rear of base)
|
||||
// Part F — led_bezel() 4-LED status bezel
|
||||
//
|
||||
// Robot-side receiver → see charging_dock_receiver.scad
|
||||
//
|
||||
// Coordinate system:
|
||||
// Z = 0 at dock floor (base plate top face)
|
||||
// Y = 0 at back wall front face (robot approaches from +Y)
|
||||
// X = 0 at dock centre
|
||||
// Robot drives in -Y direction to dock.
|
||||
//
|
||||
// RENDER options:
|
||||
// "assembly" full dock preview (default)
|
||||
// "base_stl" base plate (print 1×)
|
||||
// "back_wall_stl" back wall + pogo housing (print 1×)
|
||||
// "guide_rail_stl" V-guide rail (print 2×, mirror for R side)
|
||||
// "aruco_mount_stl" ArUco marker frame (print 1×)
|
||||
// "psu_bracket_stl" PSU mounting bracket (print 1×)
|
||||
// "led_bezel_stl" LED status bezel (print 1×)
|
||||
//
|
||||
// Export commands:
|
||||
// openscad charging_dock.scad -D 'RENDER="base_stl"' -o dock_base.stl
|
||||
// openscad charging_dock.scad -D 'RENDER="back_wall_stl"' -o dock_back_wall.stl
|
||||
// openscad charging_dock.scad -D 'RENDER="guide_rail_stl"' -o dock_guide_rail.stl
|
||||
// openscad charging_dock.scad -D 'RENDER="aruco_mount_stl"' -o dock_aruco_mount.stl
|
||||
// openscad charging_dock.scad -D 'RENDER="psu_bracket_stl"' -o dock_psu_bracket.stl
|
||||
// openscad charging_dock.scad -D 'RENDER="led_bezel_stl"' -o dock_led_bezel.stl
|
||||
// ============================================================
|
||||
|
||||
$fn = 64;
|
||||
e = 0.01;
|
||||
|
||||
// ── Base plate dimensions ─────────────────────────────────────────────────────
|
||||
BASE_W = 280.0; // base width (X)
|
||||
BASE_D = 250.0; // base depth (Y, extends behind and in front of back wall)
|
||||
BASE_T = 12.0; // base thickness
|
||||
BASE_R = 10.0; // corner radius
|
||||
|
||||
// Ballast pockets (for steel hex bar / bolt weights):
|
||||
// 4× pockets in base underside, accept M20 hex nuts (30 mm AF) stacked
|
||||
BALLAST_N = 4;
|
||||
BALLAST_W = 32.0; // pocket width (hex nut AF + 2 mm)
|
||||
BALLAST_D = 32.0; // pocket depth
|
||||
BALLAST_T = 8.0; // pocket depth (≤ BASE_T/2)
|
||||
BALLAST_INSET_X = 50.0;
|
||||
BALLAST_INSET_Y = 40.0;
|
||||
|
||||
// Floor bolt holes (M8, for bolting dock to bench/floor — optional)
|
||||
FLOOR_BOLT_D = 8.5;
|
||||
FLOOR_BOLT_INSET_X = 30.0;
|
||||
FLOOR_BOLT_INSET_Y = 25.0;
|
||||
|
||||
// ── Back wall (upright panel) ─────────────────────────────────────────────────
|
||||
WALL_W = 250.0; // wall width (X) — same as guide entry span
|
||||
WALL_H = 85.0; // wall height (Z)
|
||||
WALL_T = 10.0; // wall thickness (Y)
|
||||
|
||||
// Back wall Y position relative to base rear edge
|
||||
// Wall sits at Y=0 (its front face); base extends behind it (-Y) and in front (+Y)
|
||||
BASE_REAR_Y = -80.0; // base rear edge Y coordinate
|
||||
|
||||
// ── Pogo pin housing (in back wall front face) ────────────────────────────────
|
||||
// High-current pogo pins: Ø5.5 mm body, 20 mm long (compressed), 4 mm spring travel
|
||||
// Rated 5 A each; 2× pins for +/- power
|
||||
POGO_D = 5.5; // pogo pin body OD
|
||||
POGO_BORE_D = 5.7; // bore diameter (0.2 mm clearance)
|
||||
POGO_L = 20.0; // pogo full length (uncompressed)
|
||||
POGO_TRAVEL = 4.0; // spring travel
|
||||
POGO_FLANGE_D = 8.0; // pogo flange / retention shoulder OD
|
||||
POGO_FLANGE_T = 1.5; // flange thickness
|
||||
POGO_SPACING = 20.0; // CL-to-CL spacing between + and - pins
|
||||
POGO_Z = 35.0; // pogo CL height above dock floor
|
||||
POGO_PROTRUDE = 8.0; // pogo tip protrusion beyond wall face (uncompressed)
|
||||
// Wiring channel behind pogo (runs down to base)
|
||||
WIRE_CH_W = 8.0;
|
||||
WIRE_CH_H = POGO_Z + 5;
|
||||
|
||||
// ── LED bezel (4 status LEDs in back wall, above pogo pins) ───────────────────
|
||||
// LED order (left to right): Searching | Aligned | Charging | Full
|
||||
// Colours (suggested): Red | Yellow | Blue | Green
|
||||
LED_D = 5.0; // 5 mm through-hole LED
|
||||
LED_BORE_D = 5.2; // bore diameter
|
||||
LED_BEZEL_W = 80.0; // bezel plate width
|
||||
LED_BEZEL_H = 18.0; // bezel plate height
|
||||
LED_BEZEL_T = 4.0; // bezel plate thickness
|
||||
LED_SPACING = 16.0; // LED centre-to-centre
|
||||
LED_Z = 65.0; // LED centre height above floor
|
||||
LED_INSET_D = 2.0; // LED recess depth (LED body recessed for protection)
|
||||
|
||||
// ── V-guide rails ─────────────────────────────────────────────────────────────
|
||||
// Robot receiver width (contact block): 30 mm.
|
||||
// Alignment tolerance: ±20 mm → entry gap = 30 + 2×20 = 70 mm.
|
||||
// Guide rail tapers from 70 mm entry (at Y = GUIDE_L) to 30 mm exit (at Y=0).
|
||||
// Each rail is a wedge-shaped wall.
|
||||
GUIDE_L = 100.0; // guide rail length (Y depth, from back wall)
|
||||
GUIDE_H = 50.0; // guide rail height (Z)
|
||||
GUIDE_T = 8.0; // guide rail wall thickness
|
||||
RECV_W = 30.0; // robot receiver contact block width
|
||||
ENTRY_GAP = 70.0; // guide entry gap (= RECV_W + 2×20 mm tolerance)
|
||||
EXIT_GAP = RECV_W + 2.0; // guide exit gap (2 mm clearance on each side)
|
||||
// Derived: half-gap at entry = 35 mm, at exit = 16 mm; taper = 19 mm over 100 mm
|
||||
// Half-angle = atan(19/100) ≈ 10.8° — gentle enough for reliable self-alignment
|
||||
|
||||
// ── ArUco marker mount ────────────────────────────────────────────────────────
|
||||
// Mounted at dock entry arch (forward of guide rails), tilted 15° back.
|
||||
// Robot camera acquires marker for coarse approach alignment.
|
||||
// Standard ArUco marker size: 100×100 mm (printed/laminated on paper).
|
||||
ARUCO_MARKER_W = 100.0;
|
||||
ARUCO_MARKER_H = 100.0;
|
||||
ARUCO_FRAME_T = 3.0; // frame plate thickness
|
||||
ARUCO_FRAME_BDR = 10.0; // frame border around marker
|
||||
ARUCO_SLOT_T = 1.5; // marker slip-in slot depth
|
||||
ARUCO_MAST_H = 95.0; // mast height above base (centres marker at camera height)
|
||||
ARUCO_MAST_W = 10.0;
|
||||
ARUCO_TILT = 15.0; // backward tilt (degrees) — faces approaching robot
|
||||
ARUCO_Y = GUIDE_L + 60; // mast Y position (in front of guide entry)
|
||||
|
||||
// ── PSU bracket ───────────────────────────────────────────────────────────────
|
||||
// Meanwell IRM-30-5 (or similar): 63×45×28 mm body
|
||||
// Bracket sits behind back wall, on base plate.
|
||||
PSU_W = 68.0; // bracket internal width (+5 mm clearance)
|
||||
PSU_D = 50.0; // bracket internal depth
|
||||
PSU_H = 32.0; // bracket internal height
|
||||
PSU_T = 3.0; // bracket wall thickness
|
||||
PSU_Y = BASE_REAR_Y + PSU_D/2 + PSU_T + 10; // PSU Y centre
|
||||
|
||||
// ── Fasteners ─────────────────────────────────────────────────────────────────
|
||||
M3_D = 3.3;
|
||||
M4_D = 4.3;
|
||||
M5_D = 5.3;
|
||||
M8_D = 8.5;
|
||||
|
||||
// ============================================================
|
||||
// RENDER DISPATCH
|
||||
// ============================================================
|
||||
RENDER = "assembly";
|
||||
|
||||
if (RENDER == "assembly") assembly();
|
||||
else if (RENDER == "base_stl") dock_base();
|
||||
else if (RENDER == "back_wall_stl") back_wall();
|
||||
else if (RENDER == "guide_rail_stl") guide_rail("left");
|
||||
else if (RENDER == "aruco_mount_stl") aruco_mount();
|
||||
else if (RENDER == "psu_bracket_stl") psu_bracket();
|
||||
else if (RENDER == "led_bezel_stl") led_bezel();
|
||||
|
||||
// ============================================================
|
||||
// ASSEMBLY PREVIEW
|
||||
// ============================================================
|
||||
module assembly() {
|
||||
// Base plate
|
||||
color("SaddleBrown", 0.85) dock_base();
|
||||
|
||||
// Back wall
|
||||
color("Sienna", 0.85)
|
||||
translate([0, 0, BASE_T])
|
||||
back_wall();
|
||||
|
||||
// Left guide rail
|
||||
color("Peru", 0.85)
|
||||
translate([0, 0, BASE_T])
|
||||
guide_rail("left");
|
||||
|
||||
// Right guide rail (mirror in X)
|
||||
color("Peru", 0.85)
|
||||
translate([0, 0, BASE_T])
|
||||
mirror([1, 0, 0])
|
||||
guide_rail("left");
|
||||
|
||||
// ArUco mount
|
||||
color("DimGray", 0.85)
|
||||
translate([0, 0, BASE_T])
|
||||
aruco_mount();
|
||||
|
||||
// PSU bracket
|
||||
color("DarkSlateGray", 0.80)
|
||||
translate([0, PSU_Y, BASE_T])
|
||||
psu_bracket();
|
||||
|
||||
// LED bezel
|
||||
color("LightGray", 0.90)
|
||||
translate([0, -WALL_T/2, BASE_T + LED_Z])
|
||||
led_bezel();
|
||||
|
||||
// Ghost robot receiver approaching from +Y
|
||||
%color("SteelBlue", 0.25)
|
||||
translate([0, GUIDE_L + 30, BASE_T + POGO_Z])
|
||||
cube([RECV_W, 20, 8], center = true);
|
||||
|
||||
// Ghost pogo pins
|
||||
for (px = [-POGO_SPACING/2, POGO_SPACING/2])
|
||||
%color("Gold", 0.60)
|
||||
translate([px, -POGO_PROTRUDE, BASE_T + POGO_Z])
|
||||
rotate([90, 0, 0])
|
||||
cylinder(d = POGO_D, h = POGO_L);
|
||||
}
|
||||
|
||||
// ============================================================
|
||||
// PART A — DOCK BASE PLATE
|
||||
// ============================================================
|
||||
module dock_base() {
|
||||
difference() {
|
||||
// ── Main base block (rounded rect) ──────────────────────────
|
||||
linear_extrude(BASE_T)
|
||||
minkowski() {
|
||||
square([BASE_W - 2*BASE_R,
|
||||
BASE_D - 2*BASE_R], center = true);
|
||||
circle(r = BASE_R);
|
||||
}
|
||||
|
||||
// ── Ballast pockets (underside) ──────────────────────────────
|
||||
// 4× pockets: 2 front, 2 rear
|
||||
for (bx = [-1, 1])
|
||||
for (by = [-1, 1])
|
||||
translate([bx * (BASE_W/2 - BALLAST_INSET_X),
|
||||
by * (BASE_D/2 - BALLAST_INSET_Y),
|
||||
-e])
|
||||
cube([BALLAST_W, BALLAST_D, BALLAST_T + e], center = true);
|
||||
|
||||
// ── Floor bolt holes (M8, 4 corners) ────────────────────────
|
||||
for (bx = [-1, 1])
|
||||
for (by = [-1, 1])
|
||||
translate([bx * (BASE_W/2 - FLOOR_BOLT_INSET_X),
|
||||
by * (BASE_D/2 - FLOOR_BOLT_INSET_Y), -e])
|
||||
cylinder(d = FLOOR_BOLT_D, h = BASE_T + 2*e);
|
||||
|
||||
// ── Back wall attachment slots (M4, top face) ─────────────────
|
||||
for (bx = [-WALL_W/2 + 30, 0, WALL_W/2 - 30])
|
||||
translate([bx, -BASE_D/4, BASE_T - 3])
|
||||
cylinder(d = M4_D, h = 4 + e);
|
||||
|
||||
// ── Guide rail attachment holes (M4) ──────────────────────────
|
||||
for (side = [-1, 1])
|
||||
for (gy = [20, GUIDE_L - 20])
|
||||
translate([side * (EXIT_GAP/2 + GUIDE_T/2), gy, BASE_T - 3])
|
||||
cylinder(d = M4_D, h = 4 + e);
|
||||
|
||||
// ── Cable routing slot (from pogo wires to PSU, through base) ─
|
||||
translate([0, -WALL_T - 5, -e])
|
||||
cube([WIRE_CH_W, 15, BASE_T + 2*e], center = true);
|
||||
|
||||
// ── Anti-skid texture (front face chamfer) ───────────────────
|
||||
// Chamfer front-bottom edge for easy robot approach
|
||||
translate([0, BASE_D/2 + e, -e])
|
||||
rotate([45, 0, 0])
|
||||
cube([BASE_W + 2*e, 5, 5], center = true);
|
||||
}
|
||||
}
|
||||
|
||||
// ============================================================
|
||||
// PART B — BACK WALL (upright panel)
|
||||
// ============================================================
|
||||
module back_wall() {
|
||||
difference() {
|
||||
union() {
|
||||
// ── Wall slab ────────────────────────────────────────────
|
||||
translate([-WALL_W/2, -WALL_T, 0])
|
||||
cube([WALL_W, WALL_T, WALL_H]);
|
||||
|
||||
// ── Pogo pin housing bosses (front face) ─────────────────
|
||||
for (px = [-POGO_SPACING/2, POGO_SPACING/2])
|
||||
translate([px, -WALL_T, POGO_Z])
|
||||
rotate([90, 0, 0])
|
||||
cylinder(d = POGO_FLANGE_D + 6,
|
||||
h = POGO_PROTRUDE);
|
||||
|
||||
// ── Wiring channel reinforcement (inside wall face) ───────
|
||||
translate([-WIRE_CH_W/2 - 2, -WALL_T, 0])
|
||||
cube([WIRE_CH_W + 4, 4, WIRE_CH_H]);
|
||||
}
|
||||
|
||||
// ── Pogo pin bores (through wall into housing boss) ───────────
|
||||
for (px = [-POGO_SPACING/2, POGO_SPACING/2])
|
||||
translate([px, POGO_PROTRUDE + e, POGO_Z])
|
||||
rotate([90, 0, 0]) {
|
||||
// Main bore (full depth through wall + boss)
|
||||
cylinder(d = POGO_BORE_D,
|
||||
h = WALL_T + POGO_PROTRUDE + 2*e);
|
||||
// Flange shoulder counterbore (retains pogo from pulling out)
|
||||
translate([0, 0, WALL_T + POGO_PROTRUDE - POGO_FLANGE_T - 1])
|
||||
cylinder(d = POGO_FLANGE_D + 0.4,
|
||||
h = POGO_FLANGE_T + 2);
|
||||
}
|
||||
|
||||
// ── Wiring channel (vertical slot, inside face → base cable hole) ─
|
||||
translate([-WIRE_CH_W/2, 0 + e, 0])
|
||||
cube([WIRE_CH_W, WALL_T/2, WIRE_CH_H]);
|
||||
|
||||
// ── LED bezel recess (in front face, above pogo) ──────────────
|
||||
translate([-LED_BEZEL_W/2, -LED_BEZEL_T, LED_Z - LED_BEZEL_H/2])
|
||||
cube([LED_BEZEL_W, LED_BEZEL_T + e, LED_BEZEL_H]);
|
||||
|
||||
// ── M4 base attachment bores (3 through bottom of wall) ───────
|
||||
for (bx = [-WALL_W/2 + 30, 0, WALL_W/2 - 30])
|
||||
translate([bx, -WALL_T/2, -e])
|
||||
cylinder(d = M4_D, h = 8 + e);
|
||||
|
||||
// ── Cable tie slots (in wall body, for neat wire routing) ─────
|
||||
for (cz = [15, POGO_Z - 15])
|
||||
translate([WIRE_CH_W/2 + 3, -WALL_T/2, cz])
|
||||
cube([4, WALL_T + 2*e, 3], center = true);
|
||||
|
||||
// ── Lightening cutout (rear face pocket) ──────────────────────
|
||||
translate([-WALL_W/2 + 40, 0, 20])
|
||||
cube([WALL_W - 80, WALL_T/2 + e, WALL_H - 30]);
|
||||
}
|
||||
}
|
||||
|
||||
// ============================================================
|
||||
// PART C — V-GUIDE RAIL
|
||||
// ============================================================
|
||||
// Print 2×; mirror in X for right side.
|
||||
// Rail tapers from ENTRY_GAP/2 (at Y=GUIDE_L) to EXIT_GAP/2 (at Y=0).
|
||||
// Inner (guiding) face is angled; outer face is vertical.
|
||||
module guide_rail(side = "left") {
|
||||
// Inner face X at back wall = EXIT_GAP/2
|
||||
// Inner face X at entry = ENTRY_GAP/2
|
||||
x_back = EXIT_GAP/2; // 16 mm
|
||||
x_entry = ENTRY_GAP/2; // 35 mm
|
||||
|
||||
difference() {
|
||||
union() {
|
||||
// ── Main wedge body ─────────────────────────────────────
|
||||
// Hull between two rectangles: narrow at Y=0, wide at Y=GUIDE_L
|
||||
hull() {
|
||||
// Back end (at Y=0, flush with back wall)
|
||||
translate([x_back, 0, 0])
|
||||
cube([GUIDE_T, e, GUIDE_H]);
|
||||
// Entry end (at Y=GUIDE_L)
|
||||
translate([x_entry, GUIDE_L, 0])
|
||||
cube([GUIDE_T, e, GUIDE_H]);
|
||||
}
|
||||
|
||||
// ── Entry flare (chamfered lip at guide entry for bump-entry) ─
|
||||
hull() {
|
||||
translate([x_entry, GUIDE_L, 0])
|
||||
cube([GUIDE_T, e, GUIDE_H]);
|
||||
translate([x_entry + 15, GUIDE_L + 20, 0])
|
||||
cube([GUIDE_T, e, GUIDE_H * 0.6]);
|
||||
}
|
||||
}
|
||||
|
||||
// ── M4 base attachment bores ─────────────────────────────────
|
||||
for (gy = [20, GUIDE_L - 20])
|
||||
translate([x_back + GUIDE_T/2, gy, -e])
|
||||
cylinder(d = M4_D, h = 8 + e);
|
||||
|
||||
// ── Chamfer on inner top corner (smooth robot entry) ─────────
|
||||
translate([x_back - e, -e, GUIDE_H - 5])
|
||||
rotate([0, -45, 0])
|
||||
cube([8, GUIDE_L + 30, 8]);
|
||||
}
|
||||
}
|
||||
|
||||
// ============================================================
|
||||
// PART D — ArUco MARKER MOUNT
|
||||
// ============================================================
|
||||
// Free-standing mast at dock entry. Mounts to base plate.
|
||||
// Marker face tilted 15° toward approaching robot.
|
||||
// Accepts 100×100 mm printed/laminated paper marker in slot.
|
||||
module aruco_mount() {
|
||||
frame_w = ARUCO_MARKER_W + 2*ARUCO_FRAME_BDR;
|
||||
frame_h = ARUCO_MARKER_H + 2*ARUCO_FRAME_BDR;
|
||||
mast_y = ARUCO_Y;
|
||||
|
||||
union() {
|
||||
// ── Mast column ───────────────────────────────────────────────
|
||||
translate([-ARUCO_MAST_W/2, mast_y - ARUCO_MAST_W/2, 0])
|
||||
cube([ARUCO_MAST_W, ARUCO_MAST_W, ARUCO_MAST_H]);
|
||||
|
||||
// ── Marker frame (tilted back ARUCO_TILT°) ────────────────────
|
||||
translate([0, mast_y, ARUCO_MAST_H])
|
||||
rotate([-ARUCO_TILT, 0, 0]) {
|
||||
difference() {
|
||||
// Frame plate
|
||||
translate([-frame_w/2, -ARUCO_FRAME_T, -frame_h/2])
|
||||
cube([frame_w, ARUCO_FRAME_T, frame_h]);
|
||||
|
||||
// Marker window (cutout for marker visibility)
|
||||
translate([-ARUCO_MARKER_W/2, -ARUCO_FRAME_T - e,
|
||||
-ARUCO_MARKER_H/2])
|
||||
cube([ARUCO_MARKER_W,
|
||||
ARUCO_FRAME_T + 2*e,
|
||||
ARUCO_MARKER_H]);
|
||||
|
||||
// Marker slip-in slot (insert from side)
|
||||
translate([-frame_w/2 - e,
|
||||
-ARUCO_SLOT_T - 0.3,
|
||||
-ARUCO_MARKER_H/2])
|
||||
cube([frame_w + 2*e,
|
||||
ARUCO_SLOT_T + 0.3,
|
||||
ARUCO_MARKER_H]);
|
||||
}
|
||||
}
|
||||
|
||||
// ── Mast base foot (M4 bolts to dock base) ────────────────────
|
||||
difference() {
|
||||
translate([-20, mast_y - 20, 0])
|
||||
cube([40, 40, 5]);
|
||||
for (fx = [-12, 12]) for (fy = [-12, 12])
|
||||
translate([fx, mast_y + fy, -e])
|
||||
cylinder(d = M4_D, h = 6 + e);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// ============================================================
|
||||
// PART E — PSU BRACKET
|
||||
// ============================================================
|
||||
// Open-top retention bracket for PSU module.
|
||||
// PSU slides in from top; 2× M3 straps or cable ties retain it.
|
||||
// Bracket bolts to base plate via 4× M4 screws.
|
||||
module psu_bracket() {
|
||||
difference() {
|
||||
union() {
|
||||
// ── Outer bracket box (open top) ─────────────────────────
|
||||
_box_open_top(PSU_W + 2*PSU_T,
|
||||
PSU_D + 2*PSU_T,
|
||||
PSU_H + PSU_T);
|
||||
|
||||
// ── Base flange ──────────────────────────────────────────
|
||||
translate([-(PSU_W/2 + PSU_T + 8),
|
||||
-(PSU_D/2 + PSU_T + 8), -PSU_T])
|
||||
cube([PSU_W + 2*PSU_T + 16,
|
||||
PSU_D + 2*PSU_T + 16, PSU_T]);
|
||||
}
|
||||
|
||||
// ── PSU cavity ───────────────────────────────────────────────
|
||||
translate([0, 0, PSU_T])
|
||||
cube([PSU_W, PSU_D, PSU_H + e], center = true);
|
||||
|
||||
// ── Ventilation slots (sides) ─────────────────────────────────
|
||||
for (a = [0, 90, 180, 270])
|
||||
rotate([0, 0, a])
|
||||
translate([0, (PSU_D/2 + PSU_T)/2, PSU_H/2 + PSU_T])
|
||||
for (sz = [-PSU_H/4, 0, PSU_H/4])
|
||||
translate([0, 0, sz])
|
||||
cube([PSU_W * 0.5, PSU_T + 2*e, 5],
|
||||
center = true);
|
||||
|
||||
// ── Cable exit slot (bottom) ──────────────────────────────────
|
||||
translate([0, 0, -e])
|
||||
cube([15, PSU_D + 2*PSU_T + 2*e, PSU_T + 2*e],
|
||||
center = true);
|
||||
|
||||
// ── Base flange M4 bolts ──────────────────────────────────────
|
||||
for (fx = [-1, 1]) for (fy = [-1, 1])
|
||||
translate([fx * (PSU_W/2 + PSU_T + 4),
|
||||
fy * (PSU_D/2 + PSU_T + 4),
|
||||
-PSU_T - e])
|
||||
cylinder(d = M4_D, h = PSU_T + 2*e);
|
||||
|
||||
// ── Cable tie slots ───────────────────────────────────────────
|
||||
for (sz = [PSU_H/3, 2*PSU_H/3])
|
||||
translate([0, 0, PSU_T + sz])
|
||||
cube([PSU_W + 2*PSU_T + 2*e, 4, 4], center = true);
|
||||
}
|
||||
}
|
||||
|
||||
module _box_open_top(w, d, h) {
|
||||
difference() {
|
||||
cube([w, d, h], center = true);
|
||||
translate([0, 0, PSU_T + e])
|
||||
cube([w - 2*PSU_T, d - 2*PSU_T, h], center = true);
|
||||
}
|
||||
}
|
||||
|
||||
// ============================================================
|
||||
// PART F — LED STATUS BEZEL
|
||||
// ============================================================
|
||||
// 4 × 5 mm LEDs in a row. Press-fits into recess in back wall.
|
||||
// LED labels (L→R): SEARCHING | ALIGNED | CHARGING | FULL
|
||||
// Suggested colours: Red | Yellow | Blue | Green
|
||||
module led_bezel() {
|
||||
difference() {
|
||||
// Bezel plate
|
||||
cube([LED_BEZEL_W, LED_BEZEL_T, LED_BEZEL_H], center = true);
|
||||
|
||||
// 4× LED bores
|
||||
for (i = [-1.5, -0.5, 0.5, 1.5])
|
||||
translate([i * LED_SPACING, -LED_BEZEL_T - e, 0])
|
||||
rotate([90, 0, 0]) {
|
||||
// LED body bore (recess, not through)
|
||||
cylinder(d = LED_BORE_D + 1,
|
||||
h = LED_INSET_D + e);
|
||||
// LED pin bore (through bezel)
|
||||
translate([0, 0, LED_INSET_D])
|
||||
cylinder(d = LED_BORE_D,
|
||||
h = LED_BEZEL_T + 2*e);
|
||||
}
|
||||
|
||||
// Label recesses between LEDs (for colour-dot stickers or printed inserts)
|
||||
for (i = [-1.5, -0.5, 0.5, 1.5])
|
||||
translate([i * LED_SPACING, LED_BEZEL_T/2, LED_BEZEL_H/2 - 3])
|
||||
cube([LED_SPACING - 3, 1 + e, 5], center = true);
|
||||
|
||||
// M3 mounting holes (2× into back wall)
|
||||
for (mx = [-LED_BEZEL_W/2 + 6, LED_BEZEL_W/2 - 6])
|
||||
translate([mx, -LED_BEZEL_T - e, 0])
|
||||
rotate([90, 0, 0])
|
||||
cylinder(d = M3_D, h = LED_BEZEL_T + 2*e);
|
||||
}
|
||||
}
|
||||
@ -1,202 +0,0 @@
|
||||
# Charging Dock BOM — Issue #159
|
||||
**Agent:** sl-mechanical | **Date:** 2026-03-01
|
||||
|
||||
Cross-variant charging dock: 5 V / 5 A pogo-pin contact, V-guide funnel, ArUco marker, LED status.
|
||||
|
||||
---
|
||||
|
||||
## A. Dock Station Hardware
|
||||
|
||||
| # | Description | Spec | Qty | Notes / Source |
|
||||
|---|-------------|------|-----|----------------|
|
||||
| D1 | High-current pogo pin | OD 5.5 mm, 5 A rated, 20 mm length, 4 mm travel | 2 | Generic HC pogo "PBX-5" type; AliExpress or Preci-Dip 821 series. Specify press-fit shoulder. |
|
||||
| D2 | Brass contact pad | Ø12 × 2 mm, bare brass | 2 | Machine from Ø12 mm brass rod, or order PCB contact pad. Robot-side receiver. |
|
||||
| D3 | Meanwell IRM-30-5 PSU | 5 V / 5 A (30 W), open-frame | 1 | Or equivalent: Hi-Link HLK-30M05, Recom RAC30-05SK |
|
||||
| D4 | PG7 cable gland | IP67, M12 thread, 3–6 mm cable | 2 | PSU mains in + DC out to pogo; fits bracket cable exit |
|
||||
| D5 | M20 hex nut (ballast) | Steel, 30 mm AF, ~86 g each | 8 | Stack in 4× base pockets (2 nuts/pocket) for ~690 g ballast |
|
||||
| D6 | M4×16 SHCS | Stainless | 12 | Back wall + guide rail attachment to base |
|
||||
| D7 | M4×10 BHCS | Stainless | 8 | ArUco mast foot + PSU bracket to base |
|
||||
| D8 | M4 T-nut or insert | Heat-set, M4 | 20 | Into base plate slots |
|
||||
| D9 | M3×16 SHCS | Stainless | 4 | LED bezel to back wall |
|
||||
| D10 | M3 hex nut | DIN 934 | 4 | LED bezel |
|
||||
| D11 | M8×40 BHCS | Zinc | 4 | Optional floor anchor bolts |
|
||||
| D12 | Rubber foot | Ø20 × 5 mm, self-adhesive | 4 | Underside of base plate (no floor bolts variant) |
|
||||
| D13 | 16 AWG silicone wire | Red + black, 300 mm each | 2 | Pogo pin to PSU |
|
||||
| D14 | Crimp ring terminal | M3, for 16 AWG | 4 | Pogo pin terminal connection |
|
||||
| D15 | Silicone sleeve | 4 mm ID, 100 mm | 2 | Wire strain relief in cable routing channel |
|
||||
|
||||
## B. LED Status Circuit Components
|
||||
|
||||
| # | Description | Spec | Qty | Notes |
|
||||
|---|-------------|------|-----|-------|
|
||||
| L1 | 5 mm LED — Red | Vf ≈ 2.0 V, 20 mA | 1 | SEARCHING state |
|
||||
| L2 | 5 mm LED — Yellow | Vf ≈ 2.1 V, 20 mA | 1 | ALIGNED state |
|
||||
| L3 | 5 mm LED — Blue | Vf ≈ 3.2 V, 20 mA | 1 | CHARGING state |
|
||||
| L4 | 5 mm LED — Green | Vf ≈ 2.1 V, 20 mA | 1 | FULL state |
|
||||
| L5 | Current limiting resistor | 150 Ω 1/4 W (for 5 V rail) | 4 | R = (5 V − Vf) / 20 mA |
|
||||
| L6 | TP4056 or MCU GPIO | LED driver / controller | 1 | GPIO from Jetson Orin NX via I2C LED driver, OR direct GPIO with resistors |
|
||||
| L7 | 2.54 mm pin header | 1×6, right-angle | 1 | LED→controller connection |
|
||||
|
||||
> **LED current calc:** R = (5.0 − 2.1) / 0.020 = 145 Ω → use 150 Ω for red/yellow/green.
|
||||
> Blue: R = (5.0 − 3.2) / 0.020 = 90 Ω → use 100 Ω.
|
||||
|
||||
## C. Robot Receiver Hardware (per robot)
|
||||
|
||||
| # | Description | Spec | Qty | Notes |
|
||||
|---|-------------|------|-----|-------|
|
||||
| R1 | Brass contact pad | Ø12 × 2 mm | 2 | Press-fit into receiver housing (0.1 mm interference) |
|
||||
| R2 | 16 AWG silicone wire | Red + black, 300 mm | 2 | From pads to battery charging PCB |
|
||||
| R3 | M4×10 SHCS | Stainless | 4 | Receiver to chassis (rover/tank flange bolts) |
|
||||
| R4 | M4×16 SHCS | Stainless | 2 | Lab variant stem collar clamp |
|
||||
| R5 | Solder lug, M3 | For wire termination to pad | 2 | Solder to brass pad rear face |
|
||||
|
||||
## D. ArUco Marker
|
||||
|
||||
| # | Description | Spec | Qty | Notes |
|
||||
|---|-------------|------|-----|-------|
|
||||
| A1 | ArUco marker print | 100×100 mm, ID = 0 (DICT_4X4_50) | 1 | Print on photo paper or laminate; slip into frame slot |
|
||||
| A2 | Lamination pouch | A5, 80 micron | 1 | Weather-protect printed marker |
|
||||
|
||||
---
|
||||
|
||||
## Printed Parts
|
||||
|
||||
| Part | File | Qty | Print settings | Mass est. |
|
||||
|------|------|-----|----------------|-----------|
|
||||
| Dock base plate | `charging_dock.scad` `base_stl` | 1 | PETG, 5 perims, 60% infill (heavy = stable), 0.3 mm layer | ~380 g |
|
||||
| Back wall | `charging_dock.scad` `back_wall_stl` | 1 | PETG, 5 perims, 40% infill | ~120 g |
|
||||
| Guide rail | `charging_dock.scad` `guide_rail_stl` | 2 (mirror R) | PETG, 5 perims, 60% infill | ~45 g each |
|
||||
| ArUco mast | `charging_dock.scad` `aruco_mount_stl` | 1 | PETG, 4 perims, 40% infill | ~55 g |
|
||||
| PSU bracket | `charging_dock.scad` `psu_bracket_stl` | 1 | PETG, 4 perims, 30% infill | ~35 g |
|
||||
| LED bezel | `charging_dock.scad` `led_bezel_stl` | 1 | PETG, 4 perims, 40% infill | ~10 g |
|
||||
| Lab receiver | `charging_dock_receiver.scad` `lab_stl` | 1 | PETG, 5 perims, 60% infill | ~28 g |
|
||||
| Rover receiver | `charging_dock_receiver.scad` `rover_stl` | 1 | PETG, 5 perims, 60% infill | ~32 g |
|
||||
| Tank receiver | `charging_dock_receiver.scad` `tank_stl` | 1 | PETG, 5 perims, 60% infill | ~38 g |
|
||||
|
||||
---
|
||||
|
||||
## Mass Summary
|
||||
|
||||
| Assembly | Mass |
|
||||
|----------|------|
|
||||
| Dock printed parts (all) | ~690 g |
|
||||
| Steel ballast (8× M20 hex nuts) | ~690 g |
|
||||
| PSU + hardware | ~250 g |
|
||||
| **Dock total** | **~1630 g** |
|
||||
| Receiver (per robot) | ~30–38 g |
|
||||
|
||||
---
|
||||
|
||||
## Pogo Pin Contact Height — Cross-Variant Alignment
|
||||
|
||||
The dock `POGO_Z = 35 mm` (contact height above dock floor) is set for **SaltyLab** (stem receiver height ≈ 35 mm).
|
||||
|
||||
| Robot | Chassis floor-to-contact height | Dock adapter |
|
||||
|-------|--------------------------------|--------------|
|
||||
| SaltyLab | ~35 mm (stem base) | None — direct fit |
|
||||
| SaltyRover | ~55 mm (deck belly) | 20 mm ramp shim under dock base |
|
||||
| SaltyTank | ~90 mm (hull floor) | 55 mm ramp shim under dock base |
|
||||
|
||||
> **Ramp shim:** print `dock_base.scad` with `BASE_T` increased, or print a separate shim block (not included — cut from plywood or print as needed).
|
||||
> **Alternative:** in firmware, vary approach Z offset per variant. Single dock at POGO_Z = 60 mm midpoint ± 25 mm spring travel gives rough cross-variant coverage.
|
||||
|
||||
---
|
||||
|
||||
## Wiring Diagram
|
||||
|
||||
```
|
||||
MAINS INPUT (AC)
|
||||
│
|
||||
▼
|
||||
┌─────────────────┐
|
||||
│ IRM-30-5 PSU │ 5 V / 5 A out
|
||||
│ 63×45×28 mm │
|
||||
└────────┬────────┘
|
||||
│ 5 V (RED 16 AWG) 0 V (BLK 16 AWG)
|
||||
│ │
|
||||
▼ ▼
|
||||
┌───────────┐ ┌───────────┐
|
||||
│ POGO + │ (spring-loaded) │ POGO - │
|
||||
│ pin │◄────────────────────►│ pin │
|
||||
└─────┬─────┘ robot docks └─────┬─────┘
|
||||
│ │
|
||||
─ ─ ─ ┼ ─ ─ ─ ─ ─ DOCK/ROBOT GAP ─ ─ ─ ┼ ─ ─ ─
|
||||
│ │
|
||||
┌─────▼─────┐ ┌─────▼─────┐
|
||||
│ CONTACT + │ (brass pad Ø12 mm) │ CONTACT - │
|
||||
└─────┬─────┘ └─────┬─────┘
|
||||
│ │
|
||||
└──────────────┬────────────────────┘
|
||||
│
|
||||
┌────────▼────────┐
|
||||
│ Robot charging │
|
||||
│ PCB / BMS │
|
||||
│ (on robot) │
|
||||
└────────┬────────┘
|
||||
│
|
||||
[Battery pack]
|
||||
|
||||
LED STATUS CIRCUIT (dock side):
|
||||
│ 5 V from PSU
|
||||
│
|
||||
┌─────────────┼──────────────────────┐
|
||||
│ │ │
|
||||
[R] [R] [R]
|
||||
150 Ω 150 Ω 100 Ω 150 Ω
|
||||
│ │ │ │
|
||||
[LED1] [LED2] [LED3] [LED4]
|
||||
RED YELLOW BLUE GREEN
|
||||
SEARCHING ALIGNED CHARGING FULL
|
||||
│ │ │ │
|
||||
└─────────────┴─────────────┴─────────┘
|
||||
│
|
||||
GPIO (Jetson Orin NX)
|
||||
or TP4056 charge state output
|
||||
|
||||
CURRENT SENSING (optional — recommended):
|
||||
Insert INA219 (I2C) in series with POGO+ line.
|
||||
I2C addr 0x40; reads dock current to detect:
|
||||
0 mA → SEARCHING (no robot contact)
|
||||
>10 mA → ALIGNED (contact made, BMS pre-charge)
|
||||
>1000 mA → CHARGING
|
||||
<50 mA → FULL (BMS trickle / float)
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## Assembly Sequence
|
||||
|
||||
1. **Print all parts** (see table above). Base at 60% infill for mass.
|
||||
2. **Press ballast nuts** into base pockets from underside. Optional: fill pockets with epoxy to lock.
|
||||
3. **Install heat-set M4 inserts** in base plate slots (back wall × 3, guide × 4 each side, ArUco foot × 4, PSU × 4).
|
||||
4. **Press pogo pins** into back wall bores from the front face. Flange seats against counterbore shoulder. Apply drop of Loctite 243 to bore wall.
|
||||
5. **Solder 16 AWG wires** to pogo pin terminals. Route down wiring channel. Thread through base cable slot.
|
||||
6. **Assemble PSU bracket** to base (rear). Connect pogo wires to PSU DC terminals (observe polarity: POGO+ → V+, POGO- → COM). Route AC mains input via PG7 glands on bracket.
|
||||
7. **Install LED bezel**: press 5 mm LEDs through bores (body recessed 2 mm), solder resistors and wires on rear, plug into controller header.
|
||||
8. **Bolt back wall** to base (3× M4×16 from underneath).
|
||||
9. **Bolt guide rails** to base (2× M4 each side). Mirror the right rail — print a second copy, insert STL mirrored in slicer OR use `mirror([1,0,0])` in OpenSCAD.
|
||||
10. **Mount ArUco mast** to base front (4× M4×10).
|
||||
11. **Insert ArUco marker** (laminated 100×100 mm, ID=0, DICT_4X4_50) into frame slot from side.
|
||||
12. **Attach rubber feet** (or drill floor anchor holes).
|
||||
13. **Robot receiver**: press brass contact pads into bores (interference fit, apply Loctite 603 retaining compound). Solder wires to pad rear lugs before pressing in.
|
||||
14. **Mount receiver to robot**: align pads to dock pogo height, fasten M4 bolts.
|
||||
|
||||
---
|
||||
|
||||
## Export Commands
|
||||
|
||||
```bash
|
||||
# Dock parts
|
||||
openscad charging_dock.scad -D 'RENDER="base_stl"' -o dock_base.stl
|
||||
openscad charging_dock.scad -D 'RENDER="back_wall_stl"' -o dock_back_wall.stl
|
||||
openscad charging_dock.scad -D 'RENDER="guide_rail_stl"' -o dock_guide_rail.stl
|
||||
openscad charging_dock.scad -D 'RENDER="aruco_mount_stl"' -o dock_aruco_mount.stl
|
||||
openscad charging_dock.scad -D 'RENDER="psu_bracket_stl"' -o dock_psu_bracket.stl
|
||||
openscad charging_dock.scad -D 'RENDER="led_bezel_stl"' -o dock_led_bezel.stl
|
||||
|
||||
# Robot receivers
|
||||
openscad charging_dock_receiver.scad -D 'RENDER="lab_stl"' -o receiver_lab.stl
|
||||
openscad charging_dock_receiver.scad -D 'RENDER="rover_stl"' -o receiver_rover.stl
|
||||
openscad charging_dock_receiver.scad -D 'RENDER="tank_stl"' -o receiver_tank.stl
|
||||
openscad charging_dock_receiver.scad -D 'RENDER="contact_pad_2d"' -o contact_pad.dxf
|
||||
```
|
||||
@ -1,331 +0,0 @@
|
||||
// ============================================================
|
||||
// charging_dock_receiver.scad — Robot-Side Charging Receiver
|
||||
// Issue: #159 Agent: sl-mechanical Date: 2026-03-01
|
||||
// ============================================================
|
||||
//
|
||||
// Robot-side contact plate that mates with the charging dock pogo pins.
|
||||
// Each robot variant has a different mounting interface; the contact
|
||||
// geometry is identical across all variants (same pogo pin spacing).
|
||||
//
|
||||
// Variants:
|
||||
// A — lab_receiver() SaltyLab — mounts to underside of stem base ring
|
||||
// B — rover_receiver() SaltyRover — mounts to chassis belly (M4 deck holes)
|
||||
// C — tank_receiver() SaltyTank — mounts to skid plate / hull floor
|
||||
//
|
||||
// Contact geometry (common across variants):
|
||||
// 2× brass contact pads, Ø12 mm × 2 mm (press-fit into PETG housing)
|
||||
// Pad spacing: 20 mm CL-to-CL (matches dock POGO_SPACING exactly)
|
||||
// Contact face Z height matches dock pogo pin Z when robot is level
|
||||
// Polarity: marked + on top pin (conventional: positive = right when
|
||||
// facing dock; negative = left) — must match dock wiring.
|
||||
//
|
||||
// Approach guide nose:
|
||||
// A chamfered V-nose on the forward face guides the receiver block
|
||||
// into the dock's V-funnel. Taper half-angle ≈ 14° matches guide rails.
|
||||
// Nose width = RECV_W = 30 mm (matches dock EXIT_GAP - 2 mm clearance).
|
||||
//
|
||||
// Coordinate convention:
|
||||
// Z = 0 at receiver mounting face (against robot chassis/deck underside).
|
||||
// +Z points downward (toward dock floor).
|
||||
// Contact pads face +Y (toward dock back wall when docked).
|
||||
// Receiver centred on X = 0 (robot centreline).
|
||||
//
|
||||
// RENDER options:
|
||||
// "assembly" all 3 receivers side by side
|
||||
// "lab_stl" SaltyLab receiver (print 1×)
|
||||
// "rover_stl" SaltyRover receiver (print 1×)
|
||||
// "tank_stl" SaltyTank receiver (print 1×)
|
||||
// "contact_pad_2d" DXF — Ø12 mm brass pad profile (order from metal shop)
|
||||
//
|
||||
// Export:
|
||||
// openscad charging_dock_receiver.scad -D 'RENDER="lab_stl"' -o receiver_lab.stl
|
||||
// openscad charging_dock_receiver.scad -D 'RENDER="rover_stl"' -o receiver_rover.stl
|
||||
// openscad charging_dock_receiver.scad -D 'RENDER="tank_stl"' -o receiver_tank.stl
|
||||
// openscad charging_dock_receiver.scad -D 'RENDER="contact_pad_2d"' -o contact_pad.dxf
|
||||
// ============================================================
|
||||
|
||||
$fn = 64;
|
||||
e = 0.01;
|
||||
|
||||
// ── Contact geometry (must match charging_dock.scad) ─────────────────────────
|
||||
POGO_SPACING = 20.0; // CL-to-CL (dock POGO_SPACING)
|
||||
PAD_D = 12.0; // contact pad OD (brass disc)
|
||||
PAD_T = 2.0; // contact pad thickness
|
||||
PAD_RECESS = 1.8; // pad pressed into housing (0.2 mm proud for contact)
|
||||
PAD_PROUD = 0.2; // pad face protrudes from housing face
|
||||
|
||||
// ── Common receiver body geometry ────────────────────────────────────────────
|
||||
RECV_W = 30.0; // receiver body width (X) — matches dock EXIT_GAP inner
|
||||
RECV_D = 25.0; // receiver body depth (Y, docking direction)
|
||||
RECV_H = 12.0; // receiver body height (Z, from mount face down)
|
||||
RECV_R = 3.0; // corner radius
|
||||
// V-nose geometry (front Y face — faces dock back wall)
|
||||
NOSE_CHAMFER = 10.0; // chamfer depth on X corners of front face
|
||||
|
||||
// Polarity indicator slot (on top/mount face: + on right, - on left)
|
||||
POL_SLOT_W = 4.0;
|
||||
POL_SLOT_D = 8.0;
|
||||
POL_SLOT_H = 1.0;
|
||||
|
||||
// Fasteners
|
||||
M2_D = 2.4;
|
||||
M3_D = 3.3;
|
||||
M4_D = 4.3;
|
||||
|
||||
// ── Mounting patterns ─────────────────────────────────────────────────────────
|
||||
// SaltyLab stem base ring (Ø25 mm stem, 4× M3 in ring at Ø40 mm BC)
|
||||
LAB_BC_D = 40.0;
|
||||
LAB_BOLT_D = M3_D;
|
||||
LAB_COLLAR_H = 15.0; // collar height above receiver body
|
||||
|
||||
// SaltyRover deck (M4 grid pattern, 30.5×30.5 mm matching FC pattern on deck)
|
||||
// Receiver uses 4× M4 holes at ±20 mm from centre (clear of deck electronics)
|
||||
ROVER_BOLT_SPC = 40.0;
|
||||
|
||||
// SaltyTank skid plate (M4 holes matching skid plate bolt pattern)
|
||||
// Uses 4× M4 at ±20 mm X, ±10 mm Y (inset from skid plate M4 positions)
|
||||
TANK_BOLT_SPC_X = 40.0;
|
||||
TANK_BOLT_SPC_Y = 20.0;
|
||||
TANK_NOSE_L = 20.0; // extended nose for tank (wider hull)
|
||||
|
||||
// ============================================================
|
||||
// RENDER DISPATCH
|
||||
// ============================================================
|
||||
RENDER = "assembly";
|
||||
|
||||
if (RENDER == "assembly") assembly();
|
||||
else if (RENDER == "lab_stl") lab_receiver();
|
||||
else if (RENDER == "rover_stl") rover_receiver();
|
||||
else if (RENDER == "tank_stl") tank_receiver();
|
||||
else if (RENDER == "contact_pad_2d") {
|
||||
projection(cut = true) translate([0, 0, -0.5])
|
||||
linear_extrude(1) circle(d = PAD_D);
|
||||
}
|
||||
|
||||
// ============================================================
|
||||
// ASSEMBLY PREVIEW
|
||||
// ============================================================
|
||||
module assembly() {
|
||||
// SaltyLab receiver
|
||||
color("RoyalBlue", 0.85)
|
||||
translate([-80, 0, 0])
|
||||
lab_receiver();
|
||||
|
||||
// SaltyRover receiver
|
||||
color("OliveDrab", 0.85)
|
||||
translate([0, 0, 0])
|
||||
rover_receiver();
|
||||
|
||||
// SaltyTank receiver
|
||||
color("SaddleBrown", 0.85)
|
||||
translate([80, 0, 0])
|
||||
tank_receiver();
|
||||
}
|
||||
|
||||
// ============================================================
|
||||
// COMMON RECEIVER BODY
|
||||
// ============================================================
|
||||
// Internal helper: the shared contact housing + V-nose.
|
||||
// Orientation: mount face = +Z top; contact face = +Y front.
|
||||
// All variant-specific modules call this, then add their mount interface.
|
||||
module _receiver_body() {
|
||||
difference() {
|
||||
union() {
|
||||
// ── Main housing block (rounded) ─────────────────────────
|
||||
linear_extrude(RECV_H)
|
||||
_recv_profile_2d();
|
||||
|
||||
// ── V-nose chamfer reinforcement ribs ─────────────────────
|
||||
// Two diagonal ribs at 45° reinforce the chamfered corners
|
||||
for (sx = [-1, 1])
|
||||
hull() {
|
||||
translate([sx*(RECV_W/2 - NOSE_CHAMFER),
|
||||
RECV_D/2, 0])
|
||||
cylinder(d = 3, h = RECV_H * 0.6);
|
||||
translate([sx*(RECV_W/2), RECV_D/2 - NOSE_CHAMFER, 0])
|
||||
cylinder(d = 3, h = RECV_H * 0.6);
|
||||
}
|
||||
}
|
||||
|
||||
// ── Contact pad bores (2× Ø12 mm, press-fit) ─────────────────
|
||||
// Pads face +Y; bores from Y face into housing
|
||||
for (px = [-POGO_SPACING/2, POGO_SPACING/2])
|
||||
translate([px, RECV_D/2 + e, RECV_H/2])
|
||||
rotate([90, 0, 0]) {
|
||||
// Pad press-fit bore
|
||||
cylinder(d = PAD_D + 0.1,
|
||||
h = PAD_RECESS + e);
|
||||
// Wire bore (behind pad, to mount face)
|
||||
translate([0, 0, PAD_RECESS])
|
||||
cylinder(d = 3.0,
|
||||
h = RECV_D + 2*e);
|
||||
}
|
||||
|
||||
// ── Polarity indicator slots on top face ──────────────────────
|
||||
// "+" slot: right pad (+X side)
|
||||
translate([POGO_SPACING/2, 0, -e])
|
||||
cube([POL_SLOT_W, POL_SLOT_D, POL_SLOT_H + e], center = true);
|
||||
// "-" indent: left pad (no slot = negative)
|
||||
|
||||
// ── Wire routing channel (on mount face / underside) ──────────
|
||||
// Trough connecting both pad bores for neat wire run
|
||||
translate([0, RECV_D/2 - POGO_SPACING/2, RECV_H - 3])
|
||||
cube([POGO_SPACING + 6, POGO_SPACING, 4], center = true);
|
||||
}
|
||||
}
|
||||
|
||||
// ── 2D profile of receiver body with chamfered V-nose ────────────────────────
|
||||
module _recv_profile_2d() {
|
||||
hull() {
|
||||
// Rear corners (full width)
|
||||
for (sx = [-1, 1])
|
||||
translate([sx*(RECV_W/2 - RECV_R), -RECV_D/2 + RECV_R])
|
||||
circle(r = RECV_R);
|
||||
// Front corners (chamfered — narrowed by NOSE_CHAMFER)
|
||||
for (sx = [-1, 1])
|
||||
translate([sx*(RECV_W/2 - NOSE_CHAMFER - RECV_R),
|
||||
RECV_D/2 - RECV_R])
|
||||
circle(r = RECV_R);
|
||||
}
|
||||
}
|
||||
|
||||
// ============================================================
|
||||
// PART A — SALTYLAB RECEIVER
|
||||
// ============================================================
|
||||
// Mounts to the underside of the SaltyLab chassis stem base ring.
|
||||
// Split collar grips Ø25 mm stem; receiver body hangs below collar.
|
||||
// Z height set so contact pads align with dock pogo pins when robot
|
||||
// rests on flat surface (robot wheel-to-contact-pad height calibrated).
|
||||
//
|
||||
// Receiver height above floor: tune LAB_CONTACT_Z in firmware (UWB/ArUco
|
||||
// approach). Mechanically: receiver sits ~35 mm above ground (stem base
|
||||
// height), matching dock POGO_Z = 35 mm.
|
||||
module lab_receiver() {
|
||||
collar_od = 46.0; // matches sensor_rail.scad STEM_COL_OD
|
||||
collar_h = LAB_COLLAR_H;
|
||||
|
||||
union() {
|
||||
// ── Common receiver body ────────────────────────────────────
|
||||
_receiver_body();
|
||||
|
||||
// ── Stem collar (split, 2 halves joined with M4 bolts) ───────
|
||||
// Only the front half printed here; rear half is mirror.
|
||||
translate([0, -RECV_D/2, RECV_H])
|
||||
difference() {
|
||||
// Half-collar cylinder
|
||||
rotate_extrude(angle = 180)
|
||||
translate([collar_od/2 - 8, 0, 0])
|
||||
square([8, collar_h]);
|
||||
|
||||
// Stem bore clearance
|
||||
translate([0, 0, -e])
|
||||
cylinder(d = 25.5, h = collar_h + 2*e);
|
||||
|
||||
// 2× M4 clamping bolt bores (through collar flanges)
|
||||
for (cx = [-collar_od/2 + 4, collar_od/2 - 4])
|
||||
translate([cx, 0, collar_h/2])
|
||||
rotate([90, 0, 0])
|
||||
cylinder(d = M4_D,
|
||||
h = collar_od + 2*e,
|
||||
center = true);
|
||||
}
|
||||
|
||||
// ── M3 receiver-to-collar bolts ───────────────────────────────
|
||||
// 4× M3 holes connecting collar flange to receiver body top
|
||||
// (These are mounting holes for assembly; not holes in the part)
|
||||
}
|
||||
}
|
||||
|
||||
// ============================================================
|
||||
// PART B — SALTYOVER RECEIVER
|
||||
// ============================================================
|
||||
// Mounts to the underside of the SaltyRover deck plate.
|
||||
// 4× M4 bolts into deck underside (blind holes tapped in deck).
|
||||
// Receiver sits flush with deck belly; contact pads protrude 5 mm below.
|
||||
// Dock pogo Z = 35 mm must equal ground-to-deck-belly height for rover
|
||||
// (approximately 60 mm chassis clearance — shim with spacer if needed).
|
||||
module rover_receiver() {
|
||||
mount_h = 5.0; // mounting flange thickness
|
||||
|
||||
union() {
|
||||
// ── Common receiver body ────────────────────────────────────
|
||||
_receiver_body();
|
||||
|
||||
// ── Mounting flange (attaches to deck belly) ─────────────────
|
||||
difference() {
|
||||
translate([-(ROVER_BOLT_SPC/2 + 12),
|
||||
-RECV_D/2 - 10,
|
||||
RECV_H])
|
||||
cube([ROVER_BOLT_SPC + 24,
|
||||
RECV_D + 20,
|
||||
mount_h]);
|
||||
|
||||
// 4× M4 bolt holes
|
||||
for (fx = [-1, 1]) for (fy = [-1, 1])
|
||||
translate([fx*ROVER_BOLT_SPC/2,
|
||||
fy*(RECV_D/2 + 5),
|
||||
RECV_H - e])
|
||||
cylinder(d = M4_D,
|
||||
h = mount_h + 2*e);
|
||||
|
||||
// Weight-reduction pockets
|
||||
for (sx = [-1, 1])
|
||||
translate([sx*(ROVER_BOLT_SPC/4 + 6),
|
||||
0, RECV_H + 1])
|
||||
cube([ROVER_BOLT_SPC/2 - 4, RECV_D - 4, mount_h],
|
||||
center = true);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// ============================================================
|
||||
// PART C — SALTYTANK RECEIVER
|
||||
// ============================================================
|
||||
// Mounts to SaltyTank hull floor or replaces a section of skid plate.
|
||||
// Extended front nose (TANK_NOSE_L) for tank's wider hull approach.
|
||||
// Contact pads exposed through skid plate via a 30×16 mm slot.
|
||||
// Ground clearance: tank chassis = 90 mm; dock POGO_Z = 35 mm.
|
||||
// Use ramp shim (see BOM) under dock base to elevate pogo pins to 90 mm
|
||||
// OR set POGO_Z = 90 in dock for a tank-specific dock configuration.
|
||||
// ⚠ Cross-variant dock: set POGO_Z per robot if heights differ.
|
||||
// Compromise: POGO_Z = 60 mm with 25 mm ramp for tank, 25 mm spacer for lab.
|
||||
module tank_receiver() {
|
||||
mount_h = 4.0;
|
||||
nose_l = RECV_D/2 + TANK_NOSE_L;
|
||||
|
||||
union() {
|
||||
// ── Common receiver body ────────────────────────────────────
|
||||
_receiver_body();
|
||||
|
||||
// ── Extended nose for tank approach ──────────────────────────
|
||||
// Additional chamfered wedge ahead of standard receiver body
|
||||
hull() {
|
||||
// Receiver front face corners
|
||||
for (sx = [-1, 1])
|
||||
translate([sx*(RECV_W/2 - NOSE_CHAMFER), RECV_D/2, 0])
|
||||
cylinder(d = 2*RECV_R, h = RECV_H * 0.5);
|
||||
// Extended nose tip (narrowed to 20 mm)
|
||||
for (sx = [-1, 1])
|
||||
translate([sx*10, RECV_D/2 + TANK_NOSE_L, 0])
|
||||
cylinder(d = 2*RECV_R, h = RECV_H * 0.4);
|
||||
}
|
||||
|
||||
// ── Mounting flange (bolts to tank skid plate) ────────────────
|
||||
difference() {
|
||||
translate([-(TANK_BOLT_SPC_X/2 + 10),
|
||||
-RECV_D/2 - 8,
|
||||
RECV_H])
|
||||
cube([TANK_BOLT_SPC_X + 20,
|
||||
RECV_D + 16,
|
||||
mount_h]);
|
||||
|
||||
// 4× M4 bolt holes
|
||||
for (fx = [-1, 1]) for (fy = [-1, 1])
|
||||
translate([fx*TANK_BOLT_SPC_X/2,
|
||||
fy*TANK_BOLT_SPC_Y/2,
|
||||
RECV_H - e])
|
||||
cylinder(d = M4_D,
|
||||
h = mount_h + 2*e);
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -1,43 +0,0 @@
|
||||
/**:
|
||||
ros__parameters:
|
||||
# Control loop rate (Hz)
|
||||
control_rate: 20.0
|
||||
|
||||
# Topic names
|
||||
image_topic: "/camera/image_raw"
|
||||
camera_info_topic: "/camera/camera_info"
|
||||
ir_topic: "/saltybot/ir_beacon"
|
||||
battery_topic: "/saltybot/battery_state"
|
||||
|
||||
# ── ArUco detection ────────────────────────────────────────────────────────
|
||||
aruco_marker_id: 42 # marker ID on the dock face
|
||||
aruco_marker_size_m: 0.10 # printed marker side length (m)
|
||||
|
||||
# ── IR beacon ─────────────────────────────────────────────────────────────
|
||||
ir_threshold: 0.50 # amplitude threshold for beacon detection
|
||||
|
||||
# ── Battery thresholds ────────────────────────────────────────────────────
|
||||
battery_low_pct: 15.0 # SOC triggering auto-dock (%)
|
||||
battery_high_pct: 80.0 # SOC declaring charge complete (%)
|
||||
|
||||
# ── Visual servo ──────────────────────────────────────────────────────────
|
||||
servo_range_m: 1.00 # switch from Nav2 to IBVS at this distance (m)
|
||||
k_linear: 0.30 # distance P-gain
|
||||
k_angular: 0.80 # yaw P-gain
|
||||
lateral_tol_m: 0.005 # ±5 mm alignment tolerance
|
||||
contact_distance_m: 0.05 # distance below which contact is assumed (m)
|
||||
max_linear_ms: 0.10 # forward speed ceiling during servo (m/s)
|
||||
max_angular_rads: 0.30 # yaw rate ceiling (rad/s)
|
||||
|
||||
# ── Undocking maneuver ────────────────────────────────────────────────────
|
||||
undock_speed_ms: -0.20 # reverse speed (m/s; must be negative)
|
||||
undock_ticks_max: 50 # ticks at undock speed (2.5 s at 20 Hz)
|
||||
|
||||
# ── FSM timeouts ──────────────────────────────────────────────────────────
|
||||
lost_ticks_max: 40 # ticks of lost detection before retry (2 s)
|
||||
contact_timeout_ticks: 20 # ticks at CONTACT without charging before retry
|
||||
|
||||
# ── Nav2 waypoints [x, y, z, qx, qy, qz, qw] in map frame ───────────────
|
||||
# Set these to the actual dock and pre-dock poses in your environment.
|
||||
dock_pose: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]
|
||||
pre_dock_pose: [0.0, -1.0, 0.0, 0.0, 0.0, 0.0, 1.0] # 1 m in front of dock
|
||||
@ -1,53 +0,0 @@
|
||||
"""
|
||||
docking.launch.py — Launch the autonomous docking node (Issue #158).
|
||||
|
||||
Usage
|
||||
-----
|
||||
ros2 launch saltybot_docking docking.launch.py
|
||||
ros2 launch saltybot_docking docking.launch.py \
|
||||
battery_low_pct:=15.0 servo_range_m:=1.0
|
||||
"""
|
||||
|
||||
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_docking")
|
||||
default_params = os.path.join(pkg_share, "config", "docking_params.yaml")
|
||||
|
||||
return LaunchDescription([
|
||||
DeclareLaunchArgument(
|
||||
"params_file",
|
||||
default_value=default_params,
|
||||
description="Path to docking_params.yaml",
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"battery_low_pct",
|
||||
default_value="15.0",
|
||||
description="SOC threshold to trigger auto-dock (%)",
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"servo_range_m",
|
||||
default_value="1.0",
|
||||
description="Switch to visual servo within this distance (m)",
|
||||
),
|
||||
Node(
|
||||
package="saltybot_docking",
|
||||
executable="docking_node",
|
||||
name="docking",
|
||||
output="screen",
|
||||
parameters=[
|
||||
LaunchConfiguration("params_file"),
|
||||
{
|
||||
"battery_low_pct": LaunchConfiguration("battery_low_pct"),
|
||||
"servo_range_m": LaunchConfiguration("servo_range_m"),
|
||||
},
|
||||
],
|
||||
),
|
||||
])
|
||||
@ -1,24 +0,0 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>saltybot_docking</name>
|
||||
<version>0.1.0</version>
|
||||
<description>Docking station auto-return with ArUco/IR detection and visual servo (Issue #158)</description>
|
||||
<maintainer email="sl-controls@saltylab.local">sl-controls</maintainer>
|
||||
<license>MIT</license>
|
||||
|
||||
<depend>rclpy</depend>
|
||||
<depend>sensor_msgs</depend>
|
||||
<depend>geometry_msgs</depend>
|
||||
<depend>std_msgs</depend>
|
||||
<depend>nav2_msgs</depend>
|
||||
|
||||
<test_depend>ament_copyright</test_depend>
|
||||
<test_depend>ament_flake8</test_depend>
|
||||
<test_depend>ament_pep257</test_depend>
|
||||
<test_depend>pytest</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_python</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@ -1,137 +0,0 @@
|
||||
"""
|
||||
charge_monitor.py — Battery charge monitoring for auto-dock return (Issue #158).
|
||||
|
||||
Consumes BatteryState data (voltage, current, percentage) each control tick
|
||||
and emits discrete ChargeEvents when meaningful thresholds are crossed.
|
||||
|
||||
Events
|
||||
──────
|
||||
CHARGING_STARTED : charge current rises above charging_current_threshold_a
|
||||
(docking pins making contact and charger active)
|
||||
CHARGING_STOPPED : charge current drops back to near-zero (unplugged or
|
||||
charge complete at hardware level)
|
||||
THRESHOLD_LOW : SOC dropped below low_threshold_pct → trigger auto-dock
|
||||
THRESHOLD_HIGH : SOC rose above high_threshold_pct → resume mission
|
||||
|
||||
Each event is emitted at most once per crossing (edge-triggered).
|
||||
|
||||
Pure module — no ROS2 dependency.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from enum import Enum, auto
|
||||
from typing import List
|
||||
|
||||
|
||||
# ── Events ────────────────────────────────────────────────────────────────────
|
||||
|
||||
class ChargeEvent(Enum):
|
||||
CHARGING_STARTED = auto()
|
||||
CHARGING_STOPPED = auto()
|
||||
THRESHOLD_LOW = auto()
|
||||
THRESHOLD_HIGH = auto()
|
||||
|
||||
|
||||
# ── ChargeMonitor ─────────────────────────────────────────────────────────────
|
||||
|
||||
class ChargeMonitor:
|
||||
"""
|
||||
Edge-triggered monitor for battery charging state and SOC thresholds.
|
||||
|
||||
Parameters
|
||||
----------
|
||||
low_threshold_pct : SOC below which THRESHOLD_LOW fires (default 15 %)
|
||||
high_threshold_pct : SOC above which THRESHOLD_HIGH fires (default 80 %)
|
||||
charging_current_threshold_a : minimum current (A) to declare charging
|
||||
(positive = charging convention)
|
||||
"""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
low_threshold_pct: float = 15.0,
|
||||
high_threshold_pct: float = 80.0,
|
||||
charging_current_threshold_a: float = 0.10,
|
||||
):
|
||||
self._low_pct = float(low_threshold_pct)
|
||||
self._high_pct = float(high_threshold_pct)
|
||||
self._i_thresh = float(charging_current_threshold_a)
|
||||
|
||||
# Internal state (edge-detection flags)
|
||||
self._is_charging: bool = False
|
||||
self._was_low: bool = False # True while SOC is in low zone
|
||||
self._was_high: bool = True # True while SOC is in high zone
|
||||
self._percentage: float = 100.0
|
||||
|
||||
# ── Properties ────────────────────────────────────────────────────────────
|
||||
|
||||
@property
|
||||
def is_charging(self) -> bool:
|
||||
"""True when charge current has been detected."""
|
||||
return self._is_charging
|
||||
|
||||
@property
|
||||
def percentage(self) -> float:
|
||||
"""Most recently observed state of charge (0–100 %)."""
|
||||
return self._percentage
|
||||
|
||||
@property
|
||||
def is_depleted(self) -> bool:
|
||||
"""True when SOC is currently below the low threshold."""
|
||||
return self._percentage < self._low_pct
|
||||
|
||||
@property
|
||||
def is_charged(self) -> bool:
|
||||
"""True when SOC is currently above the high threshold."""
|
||||
return self._percentage >= self._high_pct
|
||||
|
||||
# ── Update ────────────────────────────────────────────────────────────────
|
||||
|
||||
def update(
|
||||
self,
|
||||
percentage: float,
|
||||
current_a: float,
|
||||
) -> List[ChargeEvent]:
|
||||
"""
|
||||
Process one sample from BatteryState.
|
||||
|
||||
Parameters
|
||||
----------
|
||||
percentage : state of charge 0–100 (from BatteryState.percentage × 100)
|
||||
current_a : charge/discharge current (A); positive = charging
|
||||
|
||||
Returns
|
||||
-------
|
||||
List of ChargeEvents (may be empty; never duplicates per edge).
|
||||
"""
|
||||
events: List[ChargeEvent] = []
|
||||
self._percentage = float(percentage)
|
||||
now_charging = current_a >= self._i_thresh
|
||||
|
||||
# ── Charging edge detection ───────────────────────────────────────────
|
||||
if now_charging and not self._is_charging:
|
||||
events.append(ChargeEvent.CHARGING_STARTED)
|
||||
elif not now_charging and self._is_charging:
|
||||
events.append(ChargeEvent.CHARGING_STOPPED)
|
||||
self._is_charging = now_charging
|
||||
|
||||
# ── Low threshold — edge on falling through ───────────────────────────
|
||||
in_low_zone = self._percentage < self._low_pct
|
||||
if in_low_zone and not self._was_low:
|
||||
events.append(ChargeEvent.THRESHOLD_LOW)
|
||||
self._was_low = in_low_zone
|
||||
|
||||
# ── High threshold — edge on rising through ───────────────────────────
|
||||
in_high_zone = self._percentage >= self._high_pct
|
||||
if in_high_zone and not self._was_high:
|
||||
events.append(ChargeEvent.THRESHOLD_HIGH)
|
||||
self._was_high = in_high_zone
|
||||
|
||||
return events
|
||||
|
||||
def reset(self) -> None:
|
||||
"""Clear all state (use when node restarts)."""
|
||||
self._is_charging = False
|
||||
self._was_low = False
|
||||
self._was_high = True
|
||||
self._percentage = 100.0
|
||||
@ -1,210 +0,0 @@
|
||||
"""
|
||||
dock_detector.py — ArUco marker + IR beacon detection for docking (Issue #158).
|
||||
|
||||
Detection sources
|
||||
─────────────────
|
||||
ArUco marker : cv2.aruco marker on the dock face; PnP solve gives full 3-D
|
||||
pose relative to the robot camera. Marker ID and physical
|
||||
size are configurable.
|
||||
|
||||
IR beacon : single ADC channel (normalised 0–1) with envelope follower
|
||||
and amplitude threshold. Provides presence detection only
|
||||
(no pose), used as a fallback or range-alert.
|
||||
|
||||
Outputs
|
||||
───────
|
||||
DockPose(distance_m, yaw_rad, lateral_m, source)
|
||||
distance_m — straight-line distance to dock marker centre (m)
|
||||
yaw_rad — bearing error (rad); +ve = dock is right of camera axis (tx > 0)
|
||||
lateral_m — lateral offset (m) in camera frame; +ve = dock is right
|
||||
source — "aruco" | "ir"
|
||||
|
||||
ArucoDetector.detect() returns Optional[DockPose]; None if marker absent or
|
||||
OpenCV / cv2.aruco are unavailable at runtime.
|
||||
|
||||
IRBeaconDetector.update(sample) returns bool (True = beacon detected).
|
||||
|
||||
Pure module — no ROS2 dependency.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
from dataclasses import dataclass
|
||||
from typing import Optional
|
||||
|
||||
|
||||
# ── DockPose ─────────────────────────────────────────────────────────────────
|
||||
|
||||
@dataclass
|
||||
class DockPose:
|
||||
"""Dock pose in the robot's camera frame."""
|
||||
distance_m: float # straight-line distance to marker (m)
|
||||
yaw_rad: float # bearing error (rad); +ve = dock is right of camera axis (tx > 0)
|
||||
lateral_m: float # lateral offset (m) in camera X; +ve = dock is right
|
||||
source: str # "aruco" | "ir"
|
||||
|
||||
|
||||
# ── ArucoDetector ─────────────────────────────────────────────────────────────
|
||||
|
||||
class ArucoDetector:
|
||||
"""
|
||||
Detect a single ArUco marker and solve its 3-D pose with PnP.
|
||||
|
||||
Parameters
|
||||
----------
|
||||
marker_id : ArUco marker ID placed on the dock face (default 42)
|
||||
marker_size_m : physical side length of the printed marker (m)
|
||||
aruco_dict_id : cv2.aruco dictionary constant (default DICT_4X4_50 = 0)
|
||||
"""
|
||||
|
||||
MARKER_ID_DEFAULT = 42
|
||||
MARKER_SIZE_DEFAULT = 0.10 # 10 cm
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
marker_id: int = MARKER_ID_DEFAULT,
|
||||
marker_size_m: float = MARKER_SIZE_DEFAULT,
|
||||
aruco_dict_id: int = 0, # cv2.aruco.DICT_4X4_50
|
||||
):
|
||||
self._marker_id = marker_id
|
||||
self._marker_size = marker_size_m
|
||||
self._dict_id = aruco_dict_id
|
||||
self._ready: Optional[bool] = None # None = not yet attempted
|
||||
|
||||
# Set by _init(); None when OpenCV unavailable
|
||||
self._cv2 = None
|
||||
self._detector = None
|
||||
self._obj_pts = None
|
||||
|
||||
# ── Initialisation ────────────────────────────────────────────────────────
|
||||
|
||||
def _init(self) -> bool:
|
||||
"""Lazy-initialise cv2.aruco. Returns False if unavailable."""
|
||||
if self._ready is not None:
|
||||
return self._ready
|
||||
try:
|
||||
import cv2
|
||||
import numpy as np
|
||||
aruco_dict = cv2.aruco.getPredefinedDictionary(self._dict_id)
|
||||
aruco_params = cv2.aruco.DetectorParameters()
|
||||
self._detector = cv2.aruco.ArucoDetector(aruco_dict, aruco_params)
|
||||
half = self._marker_size / 2.0
|
||||
self._obj_pts = np.array([
|
||||
[-half, half, 0.0],
|
||||
[ half, half, 0.0],
|
||||
[ half, -half, 0.0],
|
||||
[-half, -half, 0.0],
|
||||
], dtype=np.float64)
|
||||
self._cv2 = cv2
|
||||
self._ready = True
|
||||
except Exception:
|
||||
self._ready = False
|
||||
return self._ready
|
||||
|
||||
# ── Public API ────────────────────────────────────────────────────────────
|
||||
|
||||
@property
|
||||
def available(self) -> bool:
|
||||
"""True if cv2.aruco is importable and initialised."""
|
||||
return self._init()
|
||||
|
||||
def detect(
|
||||
self,
|
||||
image, # numpy.ndarray H×W×3 BGR (or greyscale)
|
||||
camera_matrix, # numpy.ndarray 3×3
|
||||
dist_coeffs, # numpy.ndarray 1×4 or 1×5
|
||||
) -> Optional[DockPose]:
|
||||
"""
|
||||
Detect the dock marker in *image* and return its pose.
|
||||
|
||||
Returns None if the target marker is absent, PnP fails, or OpenCV is
|
||||
not available.
|
||||
"""
|
||||
if not self._init():
|
||||
return None
|
||||
|
||||
try:
|
||||
corners, ids, _ = self._detector.detectMarkers(image)
|
||||
except Exception:
|
||||
return None
|
||||
|
||||
if ids is None or len(ids) == 0:
|
||||
return None
|
||||
|
||||
import numpy as np
|
||||
for i, mid in enumerate(ids.flatten()):
|
||||
if int(mid) != self._marker_id:
|
||||
continue
|
||||
img_pts = corners[i].reshape(4, 2).astype(np.float64)
|
||||
ok, rvec, tvec = self._cv2.solvePnP(
|
||||
self._obj_pts, img_pts,
|
||||
camera_matrix, dist_coeffs,
|
||||
flags=self._cv2.SOLVEPNP_IPPE_SQUARE,
|
||||
)
|
||||
if not ok:
|
||||
continue
|
||||
tx = float(tvec[0])
|
||||
ty = float(tvec[1])
|
||||
tz = float(tvec[2])
|
||||
distance = math.sqrt(tx * tx + ty * ty + tz * tz)
|
||||
yaw = math.atan2(tx, tz) # camera-x deviation from optical axis
|
||||
return DockPose(
|
||||
distance_m=distance,
|
||||
yaw_rad=yaw,
|
||||
lateral_m=tx,
|
||||
source="aruco",
|
||||
)
|
||||
return None
|
||||
|
||||
|
||||
# ── IRBeaconDetector ──────────────────────────────────────────────────────────
|
||||
|
||||
class IRBeaconDetector:
|
||||
"""
|
||||
IR beacon presence detector from a single normalised ADC channel.
|
||||
|
||||
Maintains an exponential envelope follower. When the smoothed amplitude
|
||||
exceeds *threshold* the beacon is considered detected.
|
||||
|
||||
Parameters
|
||||
----------
|
||||
threshold : amplitude above which beacon is 'detected' (default 0.5)
|
||||
smoothing_alpha : EMA coefficient ∈ (0, 1]; 1 = no smoothing (default 0.3)
|
||||
"""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
threshold: float = 0.5,
|
||||
smoothing_alpha: float = 0.3,
|
||||
):
|
||||
self._threshold = max(1e-9, float(threshold))
|
||||
self._alpha = max(1e-9, min(1.0, smoothing_alpha))
|
||||
self._envelope = 0.0
|
||||
|
||||
# ── Properties ────────────────────────────────────────────────────────────
|
||||
|
||||
@property
|
||||
def envelope(self) -> float:
|
||||
"""Current EMA envelope value."""
|
||||
return self._envelope
|
||||
|
||||
@property
|
||||
def detected(self) -> bool:
|
||||
"""True when smoothed amplitude ≥ threshold."""
|
||||
return self._envelope >= self._threshold
|
||||
|
||||
# ── Update ────────────────────────────────────────────────────────────────
|
||||
|
||||
def update(self, sample: float) -> bool:
|
||||
"""
|
||||
Submit one ADC sample (normalised 0–1 or raw voltage).
|
||||
Returns True if the beacon is currently detected.
|
||||
"""
|
||||
amp = abs(sample)
|
||||
self._envelope = (1.0 - self._alpha) * self._envelope + self._alpha * amp
|
||||
return self.detected
|
||||
|
||||
def reset(self) -> None:
|
||||
"""Clear envelope to zero (e.g. when node restarts)."""
|
||||
self._envelope = 0.0
|
||||
@ -1,482 +0,0 @@
|
||||
"""
|
||||
docking_node.py — Autonomous docking station return (Issue #158).
|
||||
|
||||
Overview
|
||||
────────
|
||||
Orchestrates dock detection, Nav2 corridor approach, visual-servo final
|
||||
alignment, and charge monitoring. Interrupts the active Nav2 mission when
|
||||
battery drops to 15 % and resumes when charged to 80 %.
|
||||
|
||||
Pipeline (20 Hz)
|
||||
────────────────
|
||||
1. Camera callback → ArucoDetector → DockPose (if marker visible)
|
||||
2. IR callback → IRBeaconDetector → presence flag (fallback)
|
||||
3. Battery callback→ ChargeMonitor → ChargeEvents
|
||||
4. 20 Hz timer → DockingStateMachine.tick()
|
||||
→ publish cmd_vel (VISUAL_SERVO + UNDOCKING)
|
||||
→ Nav2 action calls (NAV2_APPROACH)
|
||||
→ publish DockingStatus
|
||||
|
||||
Subscribes
|
||||
──────────
|
||||
<image_topic> sensor_msgs/Image — dock camera
|
||||
<camera_info_topic> sensor_msgs/CameraInfo — intrinsics for PnP
|
||||
<ir_topic> std_msgs/Float32 — IR beacon ADC sample
|
||||
<battery_topic> sensor_msgs/BatteryState — SOC + current
|
||||
|
||||
Publishes
|
||||
─────────
|
||||
/saltybot/docking_status saltybot_docking_msgs/DockingStatus
|
||||
/saltybot/docking_cmd_vel geometry_msgs/Twist — servo + undock commands
|
||||
/saltybot/resume_mission std_msgs/Bool — trigger mission resume
|
||||
|
||||
Services
|
||||
────────
|
||||
/saltybot/dock saltybot_docking_msgs/Dock
|
||||
/saltybot/undock saltybot_docking_msgs/Undock
|
||||
|
||||
Action client
|
||||
─────────────
|
||||
/navigate_to_pose nav2_msgs/action/NavigateToPose (optional)
|
||||
|
||||
Parameters
|
||||
──────────
|
||||
control_rate 20.0 Hz
|
||||
image_topic /camera/image_raw
|
||||
camera_info_topic /camera/camera_info
|
||||
ir_topic /saltybot/ir_beacon
|
||||
battery_topic /saltybot/battery_state
|
||||
aruco_marker_id 42
|
||||
aruco_marker_size_m 0.10
|
||||
ir_threshold 0.50
|
||||
battery_low_pct 15.0
|
||||
battery_high_pct 80.0
|
||||
servo_range_m 1.00 m (switch to IBVS within this distance)
|
||||
k_linear 0.30
|
||||
k_angular 0.80
|
||||
lateral_tol_m 0.005 m (±5 mm alignment tolerance)
|
||||
contact_distance_m 0.05 m
|
||||
max_linear_ms 0.10 m/s
|
||||
max_angular_rads 0.30 rad/s
|
||||
undock_speed_ms -0.20 m/s
|
||||
undock_ticks_max 50 (2.5 s at 20 Hz)
|
||||
lost_ticks_max 40 (2.0 s at 20 Hz)
|
||||
contact_timeout_ticks 20 (1.0 s at 20 Hz)
|
||||
dock_pose [0,0,0,0,0,0,1] [x,y,z,qx,qy,qz,qw] map frame
|
||||
pre_dock_pose [0,0,0,0,0,0,1] 1 m in front of dock
|
||||
"""
|
||||
|
||||
import json
|
||||
import math
|
||||
|
||||
import rclpy
|
||||
from rclpy.action import ActionClient
|
||||
from rclpy.node import Node
|
||||
from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy
|
||||
|
||||
from geometry_msgs.msg import PoseStamped, Twist
|
||||
from sensor_msgs.msg import BatteryState, CameraInfo, Image
|
||||
from std_msgs.msg import Bool, Float32
|
||||
|
||||
from saltybot_docking.charge_monitor import ChargeEvent, ChargeMonitor
|
||||
from saltybot_docking.dock_detector import ArucoDetector, DockPose, IRBeaconDetector
|
||||
from saltybot_docking.docking_state_machine import (
|
||||
DockingInputs,
|
||||
DockingState,
|
||||
DockingStateMachine,
|
||||
)
|
||||
from saltybot_docking.visual_servo import VisualServo
|
||||
|
||||
try:
|
||||
from saltybot_docking_msgs.msg import DockingStatus
|
||||
from saltybot_docking_msgs.srv import Dock, Undock
|
||||
_MSGS_OK = True
|
||||
except ImportError:
|
||||
_MSGS_OK = False
|
||||
|
||||
try:
|
||||
from nav2_msgs.action import NavigateToPose
|
||||
_NAV2_OK = True
|
||||
except ImportError:
|
||||
_NAV2_OK = False
|
||||
|
||||
try:
|
||||
import cv2
|
||||
import numpy as np
|
||||
_CV2_OK = True
|
||||
except ImportError:
|
||||
_CV2_OK = False
|
||||
|
||||
|
||||
# ── Helper ────────────────────────────────────────────────────────────────────
|
||||
|
||||
def _pose_param_to_stamped(vals: list, frame_id: str, stamp) -> PoseStamped:
|
||||
"""Convert [x,y,z,qx,qy,qz,qw] list to PoseStamped."""
|
||||
ps = PoseStamped()
|
||||
ps.header.frame_id = frame_id
|
||||
ps.header.stamp = stamp
|
||||
if len(vals) >= 7:
|
||||
ps.pose.position.x = float(vals[0])
|
||||
ps.pose.position.y = float(vals[1])
|
||||
ps.pose.position.z = float(vals[2])
|
||||
ps.pose.orientation.x = float(vals[3])
|
||||
ps.pose.orientation.y = float(vals[4])
|
||||
ps.pose.orientation.z = float(vals[5])
|
||||
ps.pose.orientation.w = float(vals[6])
|
||||
return ps
|
||||
|
||||
|
||||
# ── Node ──────────────────────────────────────────────────────────────────────
|
||||
|
||||
class DockingNode(Node):
|
||||
|
||||
def __init__(self):
|
||||
super().__init__("docking")
|
||||
|
||||
# ── Parameters ───────────────────────────────────────────────────────
|
||||
self._declare_params()
|
||||
p = self._load_params()
|
||||
|
||||
# ── Sub-systems ──────────────────────────────────────────────────────
|
||||
self._aruco = ArucoDetector(
|
||||
marker_id=p["aruco_marker_id"],
|
||||
marker_size_m=p["aruco_marker_size_m"],
|
||||
)
|
||||
self._ir = IRBeaconDetector(
|
||||
threshold=p["ir_threshold"],
|
||||
)
|
||||
self._servo = VisualServo(
|
||||
k_linear=p["k_linear"],
|
||||
k_angular=p["k_angular"],
|
||||
lateral_tol_m=p["lateral_tol_m"],
|
||||
contact_distance_m=p["contact_distance_m"],
|
||||
max_linear_ms=p["max_linear_ms"],
|
||||
max_angular_rads=p["max_angular_rads"],
|
||||
)
|
||||
self._charge = ChargeMonitor(
|
||||
low_threshold_pct=p["battery_low_pct"],
|
||||
high_threshold_pct=p["battery_high_pct"],
|
||||
)
|
||||
self._fsm = DockingStateMachine(
|
||||
battery_low_pct=p["battery_low_pct"],
|
||||
battery_high_pct=p["battery_high_pct"],
|
||||
servo_range_m=p["servo_range_m"],
|
||||
undock_speed_ms=p["undock_speed_ms"],
|
||||
undock_ticks_max=p["undock_ticks_max"],
|
||||
lost_ticks_max=p["lost_ticks_max"],
|
||||
contact_timeout_ticks=p["contact_timeout_ticks"],
|
||||
)
|
||||
|
||||
# ── Mutable state ────────────────────────────────────────────────────
|
||||
self._latest_dock_pose: DockPose | None = None
|
||||
self._latest_servo_aligned: bool = False
|
||||
self._nav2_arrived: bool = False
|
||||
self._nav2_failed: bool = False
|
||||
self._dock_requested: bool = False
|
||||
self._undock_requested: bool = False
|
||||
self._camera_matrix = None
|
||||
self._dist_coeffs = None
|
||||
self._nav2_goal_handle = None
|
||||
|
||||
# ── QoS ──────────────────────────────────────────────────────────────
|
||||
reliable = QoSProfile(
|
||||
reliability=ReliabilityPolicy.RELIABLE,
|
||||
history=HistoryPolicy.KEEP_LAST,
|
||||
depth=10,
|
||||
)
|
||||
best_effort = QoSProfile(
|
||||
reliability=ReliabilityPolicy.BEST_EFFORT,
|
||||
history=HistoryPolicy.KEEP_LAST,
|
||||
depth=1,
|
||||
)
|
||||
|
||||
# ── Subscriptions ────────────────────────────────────────────────────
|
||||
self.create_subscription(Image, p["image_topic"], self._image_cb, best_effort)
|
||||
self.create_subscription(CameraInfo, p["camera_info_topic"], self._cam_info_cb, reliable)
|
||||
self.create_subscription(Float32, p["ir_topic"], self._ir_cb, best_effort)
|
||||
self.create_subscription(BatteryState, p["battery_topic"], self._battery_cb, reliable)
|
||||
|
||||
# ── Publishers ───────────────────────────────────────────────────────
|
||||
self._cmd_vel_pub = self.create_publisher(
|
||||
Twist, "/saltybot/docking_cmd_vel", reliable
|
||||
)
|
||||
self._resume_pub = self.create_publisher(
|
||||
Bool, "/saltybot/resume_mission", reliable
|
||||
)
|
||||
self._status_pub = None
|
||||
if _MSGS_OK:
|
||||
self._status_pub = self.create_publisher(
|
||||
DockingStatus, "/saltybot/docking_status", reliable
|
||||
)
|
||||
|
||||
# ── Services ─────────────────────────────────────────────────────────
|
||||
if _MSGS_OK:
|
||||
self.create_service(Dock, "/saltybot/dock", self._dock_srv)
|
||||
self.create_service(Undock, "/saltybot/undock", self._undock_srv)
|
||||
|
||||
# ── Nav2 action client (optional) ────────────────────────────────────
|
||||
self._nav2_client = None
|
||||
if _NAV2_OK:
|
||||
self._nav2_client = ActionClient(self, NavigateToPose, "navigate_to_pose")
|
||||
|
||||
# ── Timer ────────────────────────────────────────────────────────────
|
||||
rate = p["control_rate"]
|
||||
self._timer = self.create_timer(1.0 / rate, self._control_cb)
|
||||
|
||||
self.get_logger().info(
|
||||
f"DockingNode ready rate={rate}Hz "
|
||||
f"nav2={'yes' if _NAV2_OK else 'no'} "
|
||||
f"aruco={'yes' if _CV2_OK else 'no'}"
|
||||
)
|
||||
|
||||
# ── Parameters ────────────────────────────────────────────────────────────
|
||||
|
||||
def _declare_params(self) -> None:
|
||||
self.declare_parameter("control_rate", 20.0)
|
||||
self.declare_parameter("image_topic", "/camera/image_raw")
|
||||
self.declare_parameter("camera_info_topic", "/camera/camera_info")
|
||||
self.declare_parameter("ir_topic", "/saltybot/ir_beacon")
|
||||
self.declare_parameter("battery_topic", "/saltybot/battery_state")
|
||||
self.declare_parameter("aruco_marker_id", 42)
|
||||
self.declare_parameter("aruco_marker_size_m", 0.10)
|
||||
self.declare_parameter("ir_threshold", 0.50)
|
||||
self.declare_parameter("battery_low_pct", 15.0)
|
||||
self.declare_parameter("battery_high_pct", 80.0)
|
||||
self.declare_parameter("servo_range_m", 1.00)
|
||||
self.declare_parameter("k_linear", 0.30)
|
||||
self.declare_parameter("k_angular", 0.80)
|
||||
self.declare_parameter("lateral_tol_m", 0.005)
|
||||
self.declare_parameter("contact_distance_m", 0.05)
|
||||
self.declare_parameter("max_linear_ms", 0.10)
|
||||
self.declare_parameter("max_angular_rads", 0.30)
|
||||
self.declare_parameter("undock_speed_ms", -0.20)
|
||||
self.declare_parameter("undock_ticks_max", 50)
|
||||
self.declare_parameter("lost_ticks_max", 40)
|
||||
self.declare_parameter("contact_timeout_ticks", 20)
|
||||
self.declare_parameter("dock_pose", [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0])
|
||||
self.declare_parameter("pre_dock_pose", [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0])
|
||||
|
||||
def _load_params(self) -> dict:
|
||||
g = self.get_parameter
|
||||
keys = [
|
||||
"control_rate",
|
||||
"image_topic", "camera_info_topic", "ir_topic", "battery_topic",
|
||||
"aruco_marker_id", "aruco_marker_size_m", "ir_threshold",
|
||||
"battery_low_pct", "battery_high_pct",
|
||||
"servo_range_m", "k_linear", "k_angular",
|
||||
"lateral_tol_m", "contact_distance_m", "max_linear_ms", "max_angular_rads",
|
||||
"undock_speed_ms", "undock_ticks_max", "lost_ticks_max", "contact_timeout_ticks",
|
||||
"dock_pose", "pre_dock_pose",
|
||||
]
|
||||
return {k: g(k).value for k in keys}
|
||||
|
||||
# ── Subscriptions ─────────────────────────────────────────────────────────
|
||||
|
||||
def _cam_info_cb(self, msg: CameraInfo) -> None:
|
||||
if _CV2_OK and self._camera_matrix is None:
|
||||
self._camera_matrix = np.array(msg.k, dtype=np.float64).reshape(3, 3)
|
||||
self._dist_coeffs = np.array(msg.d, dtype=np.float64)
|
||||
|
||||
def _image_cb(self, msg: Image) -> None:
|
||||
if not _CV2_OK or self._camera_matrix is None:
|
||||
return
|
||||
try:
|
||||
img = self._ros_image_to_bgr(msg)
|
||||
except Exception:
|
||||
return
|
||||
pose = self._aruco.detect(img, self._camera_matrix, self._dist_coeffs)
|
||||
if pose is not None:
|
||||
self._latest_dock_pose = pose
|
||||
# Update visual servo for aligned flag
|
||||
servo_result = self._servo.update(pose)
|
||||
self._latest_servo_aligned = servo_result.aligned
|
||||
|
||||
def _ir_cb(self, msg: Float32) -> None:
|
||||
detected = self._ir.update(msg.data)
|
||||
# IR gives presence only — supply a synthetic far DockPose if no ArUco
|
||||
if detected and self._latest_dock_pose is None:
|
||||
# Beacon says dock is ahead, distance unknown; use sentinel 5.0 m
|
||||
self._latest_dock_pose = DockPose(
|
||||
distance_m=5.0, yaw_rad=0.0, lateral_m=0.0, source="ir"
|
||||
)
|
||||
|
||||
def _battery_cb(self, msg: BatteryState) -> None:
|
||||
pct = float(msg.percentage) * 100.0 if msg.percentage <= 1.0 else float(msg.percentage)
|
||||
current = float(msg.current)
|
||||
events = self._charge.update(pct, current)
|
||||
for ev in events:
|
||||
if ev == ChargeEvent.THRESHOLD_LOW:
|
||||
self.get_logger().warn(
|
||||
f"Battery low ({pct:.1f}%) — initiating auto-dock"
|
||||
)
|
||||
elif ev == ChargeEvent.CHARGING_STARTED:
|
||||
self.get_logger().info("Charging started")
|
||||
elif ev == ChargeEvent.THRESHOLD_HIGH:
|
||||
self.get_logger().info(
|
||||
f"Battery charged ({pct:.1f}%) — ready to undock"
|
||||
)
|
||||
|
||||
# ── 20 Hz control loop ────────────────────────────────────────────────────
|
||||
|
||||
def _control_cb(self) -> None:
|
||||
inp = DockingInputs(
|
||||
battery_pct=self._charge.percentage,
|
||||
is_charging=self._charge.is_charging,
|
||||
dock_pose=self._latest_dock_pose,
|
||||
servo_aligned=self._latest_servo_aligned,
|
||||
dock_requested=self._dock_requested,
|
||||
undock_requested=self._undock_requested,
|
||||
nav2_arrived=self._nav2_arrived,
|
||||
nav2_failed=self._nav2_failed,
|
||||
)
|
||||
# Consume one-shot flags
|
||||
self._dock_requested = False
|
||||
self._undock_requested = False
|
||||
self._nav2_arrived = False
|
||||
self._nav2_failed = False
|
||||
# Clear dock pose so stale detections don't persist past one tick
|
||||
self._latest_dock_pose = None
|
||||
self._latest_servo_aligned = False
|
||||
|
||||
out = self._fsm.tick(inp)
|
||||
|
||||
# ── State-entry side effects ──────────────────────────────────────────
|
||||
if out.state_changed:
|
||||
self.get_logger().info(f"Docking FSM → {out.state.value}")
|
||||
|
||||
if out.request_nav2:
|
||||
self._send_nav2_goal()
|
||||
if out.cancel_nav2:
|
||||
self._cancel_nav2_goal()
|
||||
|
||||
if out.mission_interrupted:
|
||||
self.get_logger().warn("Mission interrupted for auto-dock (battery low)")
|
||||
|
||||
if out.mission_resume:
|
||||
msg = Bool()
|
||||
msg.data = True
|
||||
self._resume_pub.publish(msg)
|
||||
self.get_logger().info("Auto-dock complete — resuming mission")
|
||||
|
||||
# ── Velocity commands ─────────────────────────────────────────────────
|
||||
if out.state in (DockingState.VISUAL_SERVO, DockingState.UNDOCKING):
|
||||
twist = Twist()
|
||||
twist.linear.x = out.cmd_linear
|
||||
twist.angular.z = out.cmd_angular
|
||||
self._cmd_vel_pub.publish(twist)
|
||||
|
||||
# ── Status ────────────────────────────────────────────────────────────
|
||||
if self._status_pub is not None:
|
||||
self._publish_status(inp, out)
|
||||
|
||||
# ── Nav2 integration ──────────────────────────────────────────────────────
|
||||
|
||||
def _send_nav2_goal(self) -> None:
|
||||
if not _NAV2_OK or self._nav2_client is None:
|
||||
self.get_logger().warn("Nav2 unavailable — skipping pre-dock approach")
|
||||
self._nav2_arrived = True # skip straight to VISUAL_SERVO next tick
|
||||
return
|
||||
if not self._nav2_client.wait_for_server(timeout_sec=1.0):
|
||||
self.get_logger().warn("Nav2 action server not ready — skipping approach")
|
||||
self._nav2_arrived = True
|
||||
return
|
||||
p = self._load_params()
|
||||
goal = NavigateToPose.Goal()
|
||||
goal.pose = _pose_param_to_stamped(
|
||||
p["pre_dock_pose"], "map", self.get_clock().now().to_msg()
|
||||
)
|
||||
send_future = self._nav2_client.send_goal_async(goal)
|
||||
send_future.add_done_callback(self._nav2_goal_response_cb)
|
||||
|
||||
def _nav2_goal_response_cb(self, future) -> None:
|
||||
goal_handle = future.result()
|
||||
if not goal_handle or not goal_handle.accepted:
|
||||
self.get_logger().warn("Nav2 goal rejected")
|
||||
self._nav2_failed = True
|
||||
return
|
||||
self._nav2_goal_handle = goal_handle
|
||||
result_future = goal_handle.get_result_async()
|
||||
result_future.add_done_callback(self._nav2_result_cb)
|
||||
|
||||
def _nav2_result_cb(self, future) -> None:
|
||||
result = future.result()
|
||||
if result and result.status == 4: # SUCCEEDED
|
||||
self._nav2_arrived = True
|
||||
else:
|
||||
self._nav2_failed = True
|
||||
|
||||
def _cancel_nav2_goal(self) -> None:
|
||||
if self._nav2_goal_handle is not None:
|
||||
self._nav2_goal_handle.cancel_goal_async()
|
||||
self._nav2_goal_handle = None
|
||||
|
||||
# ── Services ──────────────────────────────────────────────────────────────
|
||||
|
||||
def _dock_srv(self, request, response):
|
||||
if self._fsm.state not in (DockingState.IDLE,) and not request.force:
|
||||
response.success = False
|
||||
response.message = f"Already in state {self._fsm.state.value}; use force=True to override"
|
||||
return response
|
||||
self._dock_requested = True
|
||||
response.success = True
|
||||
response.message = "Docking initiated"
|
||||
return response
|
||||
|
||||
def _undock_srv(self, request, response):
|
||||
if self._fsm.state == DockingState.IDLE:
|
||||
response.success = False
|
||||
response.message = "Already idle"
|
||||
return response
|
||||
self._undock_requested = True
|
||||
response.success = True
|
||||
response.message = "Undocking initiated"
|
||||
return response
|
||||
|
||||
# ── Publishing ────────────────────────────────────────────────────────────
|
||||
|
||||
def _publish_status(self, inp: DockingInputs, out) -> None:
|
||||
msg = DockingStatus()
|
||||
msg.stamp = self.get_clock().now().to_msg()
|
||||
msg.state = out.state.value
|
||||
msg.dock_detected = inp.dock_pose is not None
|
||||
msg.distance_m = float(inp.dock_pose.distance_m) if inp.dock_pose else float("nan")
|
||||
msg.lateral_error_m = float(inp.dock_pose.lateral_m) if inp.dock_pose else float("nan")
|
||||
msg.battery_pct = float(self._charge.percentage)
|
||||
msg.charging = self._charge.is_charging
|
||||
msg.aligned = inp.servo_aligned
|
||||
self._status_pub.publish(msg)
|
||||
|
||||
# ── Utilities ─────────────────────────────────────────────────────────────
|
||||
|
||||
@staticmethod
|
||||
def _ros_image_to_bgr(msg: Image):
|
||||
"""Convert sensor_msgs/Image to cv2 BGR numpy array."""
|
||||
import numpy as np
|
||||
data = np.frombuffer(msg.data, dtype=np.uint8)
|
||||
if msg.encoding in ("bgr8", "rgb8"):
|
||||
img = data.reshape(msg.height, msg.width, 3)
|
||||
if msg.encoding == "rgb8":
|
||||
img = img[:, :, ::-1].copy()
|
||||
elif msg.encoding == "mono8":
|
||||
img = data.reshape(msg.height, msg.width)
|
||||
else:
|
||||
img = data.reshape(msg.height, msg.width, -1)
|
||||
return img
|
||||
|
||||
|
||||
# ── Entry point ───────────────────────────────────────────────────────────────
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
node = DockingNode()
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.try_shutdown()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@ -1,257 +0,0 @@
|
||||
"""
|
||||
docking_state_machine.py — FSM for autonomous docking / undocking (Issue #158).
|
||||
|
||||
States
|
||||
──────
|
||||
IDLE : normal operation; watching battery.
|
||||
DETECTING : scanning for dock marker or IR beacon.
|
||||
NAV2_APPROACH : Nav2 navigating to pre-dock waypoint (~1 m from dock face).
|
||||
VISUAL_SERVO : IBVS final approach (<= servo_range_m to dock).
|
||||
CONTACT : dock pins touched; waiting for charge current.
|
||||
CHARGING : charging in progress.
|
||||
UNDOCKING : backing away from dock at fixed reverse speed.
|
||||
|
||||
Transitions
|
||||
───────────
|
||||
IDLE → DETECTING : battery_pct < battery_low_pct OR dock_requested
|
||||
DETECTING → NAV2_APPROACH : dock_pose found AND distance > servo_range_m
|
||||
DETECTING → VISUAL_SERVO : dock_pose found AND distance ≤ servo_range_m
|
||||
NAV2_APPROACH → VISUAL_SERVO : dock_pose found AND distance ≤ servo_range_m
|
||||
OR nav2_arrived
|
||||
NAV2_APPROACH → DETECTING : nav2_failed (retry detection)
|
||||
VISUAL_SERVO → CONTACT : servo_aligned
|
||||
VISUAL_SERVO → DETECTING : dock lost for > lost_ticks_max ticks
|
||||
CONTACT → CHARGING : is_charging
|
||||
CONTACT → VISUAL_SERVO : contact_timeout_ticks exceeded (retry)
|
||||
CHARGING → UNDOCKING : battery_pct >= battery_high_pct
|
||||
OR undock_requested
|
||||
UNDOCKING → IDLE : undock_ticks >= undock_ticks_max
|
||||
|
||||
Any state → IDLE : undock_requested (except CHARGING → UNDOCKING)
|
||||
DETECTING → IDLE : undock_requested (abort scan)
|
||||
|
||||
Output signals
|
||||
──────────────
|
||||
cmd_linear, cmd_angular : velocity during VISUAL_SERVO and UNDOCKING
|
||||
request_nav2 : send Nav2 goal (emitted once on ENTRY to NAV2_APPROACH)
|
||||
cancel_nav2 : cancel Nav2 goal
|
||||
mission_interrupted : battery forced dock (for mission planner)
|
||||
mission_resume : auto-resume after charge cycle completes
|
||||
state_changed : FSM state changed this tick
|
||||
|
||||
Pure module — no ROS2 dependency.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
from dataclasses import dataclass, field
|
||||
from enum import Enum
|
||||
from typing import Optional
|
||||
|
||||
from saltybot_docking.dock_detector import DockPose
|
||||
|
||||
|
||||
# ── States ────────────────────────────────────────────────────────────────────
|
||||
|
||||
class DockingState(Enum):
|
||||
IDLE = "IDLE"
|
||||
DETECTING = "DETECTING"
|
||||
NAV2_APPROACH = "NAV2_APPROACH"
|
||||
VISUAL_SERVO = "VISUAL_SERVO"
|
||||
CONTACT = "CONTACT"
|
||||
CHARGING = "CHARGING"
|
||||
UNDOCKING = "UNDOCKING"
|
||||
|
||||
|
||||
# ── I/O dataclasses ───────────────────────────────────────────────────────────
|
||||
|
||||
@dataclass
|
||||
class DockingInputs:
|
||||
"""Sensor snapshot fed to the FSM each tick."""
|
||||
battery_pct: float = 100.0
|
||||
is_charging: bool = False
|
||||
dock_pose: Optional[DockPose] = None # None = dock not visible
|
||||
servo_aligned: bool = False # visual servo declared aligned
|
||||
dock_requested: bool = False # explicit /saltybot/dock call
|
||||
undock_requested: bool = False # explicit /saltybot/undock call
|
||||
nav2_arrived: bool = False # Nav2 action succeeded
|
||||
nav2_failed: bool = False # Nav2 action failed/cancelled
|
||||
|
||||
|
||||
@dataclass
|
||||
class DockingOutputs:
|
||||
"""Commands and signals produced by the FSM each tick."""
|
||||
state: DockingState = DockingState.IDLE
|
||||
cmd_linear: float = 0.0 # m/s (VISUAL_SERVO + UNDOCKING)
|
||||
cmd_angular: float = 0.0 # rad/s (VISUAL_SERVO)
|
||||
request_nav2: bool = False # send Nav2 goal
|
||||
cancel_nav2: bool = False # cancel Nav2 goal
|
||||
mission_interrupted: bool = False # battery dock forced
|
||||
mission_resume: bool = False # charge cycle complete, resume
|
||||
state_changed: bool = False # FSM state changed this tick
|
||||
|
||||
|
||||
# ── DockingStateMachine ───────────────────────────────────────────────────────
|
||||
|
||||
class DockingStateMachine:
|
||||
"""
|
||||
Deterministic docking FSM.
|
||||
|
||||
Parameters
|
||||
----------
|
||||
battery_low_pct : SOC that triggers auto-dock (default 15 %)
|
||||
battery_high_pct : SOC at which charging completes (default 80 %)
|
||||
servo_range_m : switch from Nav2 to visual servo (default 1.0 m)
|
||||
undock_speed_ms : reverse speed during undocking (m/s, must be ≤ 0)
|
||||
undock_ticks_max : ticks at undock_speed before returning to IDLE
|
||||
lost_ticks_max : allowed VISUAL_SERVO ticks with no detection
|
||||
contact_timeout_ticks : CONTACT ticks before retry alignment
|
||||
"""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
battery_low_pct: float = 15.0,
|
||||
battery_high_pct: float = 80.0,
|
||||
servo_range_m: float = 1.0,
|
||||
undock_speed_ms: float = -0.20,
|
||||
undock_ticks_max: int = 50, # 2.5 s at 20 Hz
|
||||
lost_ticks_max: int = 40, # 2.0 s at 20 Hz
|
||||
contact_timeout_ticks: int = 20, # 1.0 s at 20 Hz
|
||||
):
|
||||
self._bat_low = float(battery_low_pct)
|
||||
self._bat_high = float(battery_high_pct)
|
||||
self._servo_range = float(servo_range_m)
|
||||
self._undock_speed = min(0.0, float(undock_speed_ms))
|
||||
self._undock_max = max(1, int(undock_ticks_max))
|
||||
self._lost_max = max(1, int(lost_ticks_max))
|
||||
self._contact_max = max(1, int(contact_timeout_ticks))
|
||||
|
||||
self._state = DockingState.IDLE
|
||||
self._undock_ticks = 0
|
||||
self._lost_ticks = 0
|
||||
self._contact_ticks = 0
|
||||
self._mission_interrupted = False
|
||||
self._resume_on_undock = False
|
||||
|
||||
# ── Public API ────────────────────────────────────────────────────────────
|
||||
|
||||
@property
|
||||
def state(self) -> DockingState:
|
||||
return self._state
|
||||
|
||||
def reset(self) -> None:
|
||||
"""Return to IDLE and clear all counters (use for unit tests)."""
|
||||
self._state = DockingState.IDLE
|
||||
self._undock_ticks = 0
|
||||
self._lost_ticks = 0
|
||||
self._contact_ticks = 0
|
||||
self._mission_interrupted = False
|
||||
self._resume_on_undock = False
|
||||
|
||||
def tick(self, inputs: DockingInputs) -> DockingOutputs:
|
||||
"""Advance the FSM by one tick and return output commands."""
|
||||
prev_state = self._state
|
||||
out = self._step(inputs)
|
||||
out.state = self._state
|
||||
out.state_changed = (self._state != prev_state)
|
||||
if out.state_changed:
|
||||
self._on_enter(self._state)
|
||||
return out
|
||||
|
||||
# ── Internal step ─────────────────────────────────────────────────────────
|
||||
|
||||
def _step(self, inp: DockingInputs) -> DockingOutputs:
|
||||
out = DockingOutputs(state=self._state)
|
||||
|
||||
# ── IDLE ──────────────────────────────────────────────────────────────
|
||||
if self._state == DockingState.IDLE:
|
||||
if inp.dock_requested:
|
||||
self._state = DockingState.DETECTING
|
||||
elif inp.battery_pct < self._bat_low:
|
||||
self._state = DockingState.DETECTING
|
||||
out.mission_interrupted = True
|
||||
self._mission_interrupted = True
|
||||
|
||||
# ── DETECTING ─────────────────────────────────────────────────────────
|
||||
elif self._state == DockingState.DETECTING:
|
||||
if inp.undock_requested:
|
||||
self._state = DockingState.IDLE
|
||||
self._mission_interrupted = False
|
||||
elif inp.dock_pose is not None:
|
||||
if inp.dock_pose.distance_m <= self._servo_range:
|
||||
self._state = DockingState.VISUAL_SERVO
|
||||
else:
|
||||
self._state = DockingState.NAV2_APPROACH
|
||||
out.request_nav2 = True
|
||||
|
||||
# ── NAV2_APPROACH ─────────────────────────────────────────────────────
|
||||
elif self._state == DockingState.NAV2_APPROACH:
|
||||
if inp.undock_requested:
|
||||
self._state = DockingState.IDLE
|
||||
out.cancel_nav2 = True
|
||||
self._mission_interrupted = False
|
||||
elif inp.dock_pose is not None and inp.dock_pose.distance_m <= self._servo_range:
|
||||
self._state = DockingState.VISUAL_SERVO
|
||||
out.cancel_nav2 = True
|
||||
elif inp.nav2_arrived:
|
||||
self._state = DockingState.VISUAL_SERVO
|
||||
elif inp.nav2_failed:
|
||||
self._state = DockingState.DETECTING # retry scan
|
||||
|
||||
# ── VISUAL_SERVO ──────────────────────────────────────────────────────
|
||||
elif self._state == DockingState.VISUAL_SERVO:
|
||||
if inp.undock_requested:
|
||||
self._state = DockingState.IDLE
|
||||
elif inp.dock_pose is None:
|
||||
self._lost_ticks += 1
|
||||
if self._lost_ticks >= self._lost_max:
|
||||
self._state = DockingState.DETECTING
|
||||
else:
|
||||
self._lost_ticks = 0
|
||||
if inp.servo_aligned:
|
||||
self._state = DockingState.CONTACT
|
||||
|
||||
# ── CONTACT ───────────────────────────────────────────────────────────
|
||||
elif self._state == DockingState.CONTACT:
|
||||
if inp.undock_requested:
|
||||
self._state = DockingState.UNDOCKING
|
||||
self._resume_on_undock = False
|
||||
elif inp.is_charging:
|
||||
self._state = DockingState.CHARGING
|
||||
else:
|
||||
self._contact_ticks += 1
|
||||
if self._contact_ticks >= self._contact_max:
|
||||
self._state = DockingState.VISUAL_SERVO # realign
|
||||
|
||||
# ── CHARGING ──────────────────────────────────────────────────────────
|
||||
elif self._state == DockingState.CHARGING:
|
||||
if inp.undock_requested:
|
||||
self._state = DockingState.UNDOCKING
|
||||
self._resume_on_undock = False
|
||||
elif inp.battery_pct >= self._bat_high:
|
||||
self._state = DockingState.UNDOCKING
|
||||
self._resume_on_undock = self._mission_interrupted
|
||||
|
||||
# ── UNDOCKING ─────────────────────────────────────────────────────────
|
||||
elif self._state == DockingState.UNDOCKING:
|
||||
out.cmd_linear = self._undock_speed
|
||||
self._undock_ticks += 1
|
||||
if self._undock_ticks >= self._undock_max:
|
||||
out.mission_resume = self._resume_on_undock
|
||||
self._mission_interrupted = False
|
||||
self._resume_on_undock = False
|
||||
self._state = DockingState.IDLE
|
||||
|
||||
return out
|
||||
|
||||
# ── Entry actions ─────────────────────────────────────────────────────────
|
||||
|
||||
def _on_enter(self, state: DockingState) -> None:
|
||||
"""Reset per-state counters when entering a new state."""
|
||||
if state == DockingState.VISUAL_SERVO:
|
||||
self._lost_ticks = 0
|
||||
elif state == DockingState.CONTACT:
|
||||
self._contact_ticks = 0
|
||||
elif state == DockingState.UNDOCKING:
|
||||
self._undock_ticks = 0
|
||||
@ -1,130 +0,0 @@
|
||||
"""
|
||||
visual_servo.py — Image-based visual servo for final dock approach (Issue #158).
|
||||
|
||||
Control law
|
||||
───────────
|
||||
Operates once the robot is within *servo_range_m* of the dock.
|
||||
|
||||
Linear velocity (forward):
|
||||
v = clip( k_linear × (distance_m − target_distance_m), 0, max_linear_ms )
|
||||
|
||||
Angular velocity (yaw correction):
|
||||
ω = clip( −k_angular × yaw_rad, −max_angular_rads, +max_angular_rads )
|
||||
|
||||
The linear term drives the robot toward the dock until it reaches
|
||||
target_distance_m. The clip to [0, …] prevents reversing.
|
||||
|
||||
The angular term zeroes the bearing error; sign convention:
|
||||
yaw_rad > 0 → dock is to the robot's left → turn left (ω > 0).
|
||||
|
||||
Alignment declaration
|
||||
─────────────────────
|
||||
`aligned` is True when both:
|
||||
|lateral_m| < lateral_tol_m (default ±5 mm)
|
||||
distance_m < contact_distance_m (default 50 mm)
|
||||
|
||||
Pure module — no ROS2 dependency.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
from dataclasses import dataclass
|
||||
from typing import Optional
|
||||
|
||||
from saltybot_docking.dock_detector import DockPose
|
||||
|
||||
|
||||
# ── Result ────────────────────────────────────────────────────────────────────
|
||||
|
||||
@dataclass
|
||||
class VisualServoResult:
|
||||
linear_x: float # forward velocity (m/s); ≥ 0
|
||||
angular_z: float # yaw velocity (rad/s)
|
||||
distance_m: float # current distance to dock (m)
|
||||
lateral_error_m: float # current lateral offset (m); +ve = dock is right
|
||||
aligned: bool # True when within contact tolerances
|
||||
|
||||
|
||||
# ── VisualServo ───────────────────────────────────────────────────────────────
|
||||
|
||||
class VisualServo:
|
||||
"""
|
||||
Proportional visual servo controller for dock final approach.
|
||||
|
||||
Parameters
|
||||
----------
|
||||
k_linear : distance gain (m/s per metre of error)
|
||||
k_angular : yaw gain (rad/s per radian of bearing error)
|
||||
target_distance_m : desired stop distance from dock face (m)
|
||||
lateral_tol_m : lateral alignment tolerance (m) [spec ±5 mm → 0.005]
|
||||
contact_distance_m: distance below which contact is assumed (m)
|
||||
max_linear_ms : velocity ceiling (m/s)
|
||||
max_angular_rads : yaw-rate ceiling (rad/s)
|
||||
"""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
k_linear: float = 0.30,
|
||||
k_angular: float = 0.80,
|
||||
target_distance_m: float = 0.02, # 20 mm — at the dock face
|
||||
lateral_tol_m: float = 0.005, # ±5 mm
|
||||
contact_distance_m: float = 0.05, # 50 mm — within contact zone
|
||||
max_linear_ms: float = 0.10, # slow for precision approach
|
||||
max_angular_rads: float = 0.30,
|
||||
):
|
||||
self._k_lin = float(k_linear)
|
||||
self._k_ang = float(k_angular)
|
||||
self._target = float(target_distance_m)
|
||||
self._lat_tol = max(1e-6, float(lateral_tol_m))
|
||||
self._contact = max(self._target, float(contact_distance_m))
|
||||
self._v_max = max(0.0, float(max_linear_ms))
|
||||
self._w_max = max(0.0, float(max_angular_rads))
|
||||
|
||||
# ── Control step ─────────────────────────────────────────────────────────
|
||||
|
||||
def update(self, dock_pose: DockPose) -> VisualServoResult:
|
||||
"""
|
||||
Compute one velocity command from the current dock pose.
|
||||
|
||||
Parameters
|
||||
----------
|
||||
dock_pose : DockPose from ArucoDetector or IRBeaconDetector
|
||||
|
||||
Returns
|
||||
-------
|
||||
VisualServoResult with cmd velocities and alignment flag
|
||||
"""
|
||||
dist = dock_pose.distance_m
|
||||
yaw = dock_pose.yaw_rad
|
||||
lat = dock_pose.lateral_m
|
||||
|
||||
# Linear: approach until target_distance; no reversing
|
||||
v_raw = self._k_lin * (dist - self._target)
|
||||
v = max(0.0, min(self._v_max, v_raw))
|
||||
|
||||
# Angular: null yaw error; positive yaw → turn left (+ω)
|
||||
w_raw = -self._k_ang * yaw
|
||||
w = max(-self._w_max, min(self._w_max, w_raw))
|
||||
|
||||
aligned = (abs(lat) < self._lat_tol) and (dist < self._contact)
|
||||
|
||||
return VisualServoResult(
|
||||
linear_x=v,
|
||||
angular_z=w,
|
||||
distance_m=dist,
|
||||
lateral_error_m=lat,
|
||||
aligned=aligned,
|
||||
)
|
||||
|
||||
def stop(self) -> VisualServoResult:
|
||||
"""
|
||||
Zero-velocity command — used when dock pose is momentarily lost.
|
||||
"""
|
||||
return VisualServoResult(
|
||||
linear_x=0.0,
|
||||
angular_z=0.0,
|
||||
distance_m=float("nan"),
|
||||
lateral_error_m=float("nan"),
|
||||
aligned=False,
|
||||
)
|
||||
@ -1,5 +0,0 @@
|
||||
[develop]
|
||||
script_dir=$base/lib/saltybot_docking
|
||||
|
||||
[install]
|
||||
install_scripts=$base/lib/saltybot_docking
|
||||
@ -1,32 +0,0 @@
|
||||
from setuptools import setup, find_packages
|
||||
import os
|
||||
from glob import glob
|
||||
|
||||
package_name = "saltybot_docking"
|
||||
|
||||
setup(
|
||||
name=package_name,
|
||||
version="0.1.0",
|
||||
packages=find_packages(exclude=["test"]),
|
||||
data_files=[
|
||||
("share/ament_index/resource_index/packages",
|
||||
[f"resource/{package_name}"]),
|
||||
(f"share/{package_name}", ["package.xml"]),
|
||||
(os.path.join("share", package_name, "config"),
|
||||
glob("config/*.yaml")),
|
||||
(os.path.join("share", package_name, "launch"),
|
||||
glob("launch/*.py")),
|
||||
],
|
||||
install_requires=["setuptools"],
|
||||
zip_safe=True,
|
||||
maintainer="sl-controls",
|
||||
maintainer_email="sl-controls@saltylab.local",
|
||||
description="Docking station auto-return with ArUco/IR detection and visual servo",
|
||||
license="MIT",
|
||||
tests_require=["pytest"],
|
||||
entry_points={
|
||||
"console_scripts": [
|
||||
f"docking_node = {package_name}.docking_node:main",
|
||||
],
|
||||
},
|
||||
)
|
||||
@ -1,580 +0,0 @@
|
||||
"""
|
||||
test_docking.py — Unit tests for Issue #158 docking modules.
|
||||
|
||||
Covers:
|
||||
IRBeaconDetector — envelope follower, threshold, reset
|
||||
VisualServo — velocity commands, clamping, alignment declaration
|
||||
ChargeMonitor — event detection, edge-triggering, threshold crossing
|
||||
DockingStateMachine — all state transitions and guard conditions
|
||||
"""
|
||||
|
||||
import math
|
||||
import sys
|
||||
import os
|
||||
|
||||
sys.path.insert(0, os.path.join(os.path.dirname(__file__), ".."))
|
||||
|
||||
import pytest
|
||||
|
||||
from saltybot_docking.dock_detector import DockPose, IRBeaconDetector
|
||||
from saltybot_docking.visual_servo import VisualServo, VisualServoResult
|
||||
from saltybot_docking.charge_monitor import ChargeEvent, ChargeMonitor
|
||||
from saltybot_docking.docking_state_machine import (
|
||||
DockingInputs,
|
||||
DockingState,
|
||||
DockingStateMachine,
|
||||
)
|
||||
|
||||
|
||||
# ── Helpers ───────────────────────────────────────────────────────────────────
|
||||
|
||||
def _make_ir(**kw):
|
||||
defaults = dict(threshold=0.5, smoothing_alpha=1.0)
|
||||
defaults.update(kw)
|
||||
return IRBeaconDetector(**defaults)
|
||||
|
||||
|
||||
def _make_servo(**kw):
|
||||
defaults = dict(
|
||||
k_linear=1.0,
|
||||
k_angular=1.0,
|
||||
target_distance_m=0.02,
|
||||
lateral_tol_m=0.005,
|
||||
contact_distance_m=0.05,
|
||||
max_linear_ms=0.5,
|
||||
max_angular_rads=1.0,
|
||||
)
|
||||
defaults.update(kw)
|
||||
return VisualServo(**defaults)
|
||||
|
||||
|
||||
def _make_charge(**kw):
|
||||
defaults = dict(
|
||||
low_threshold_pct=15.0,
|
||||
high_threshold_pct=80.0,
|
||||
charging_current_threshold_a=0.10,
|
||||
)
|
||||
defaults.update(kw)
|
||||
return ChargeMonitor(**defaults)
|
||||
|
||||
|
||||
def _make_fsm(**kw):
|
||||
defaults = dict(
|
||||
battery_low_pct=15.0,
|
||||
battery_high_pct=80.0,
|
||||
servo_range_m=1.0,
|
||||
undock_speed_ms=-0.20,
|
||||
undock_ticks_max=5,
|
||||
lost_ticks_max=3,
|
||||
contact_timeout_ticks=3,
|
||||
)
|
||||
defaults.update(kw)
|
||||
return DockingStateMachine(**defaults)
|
||||
|
||||
|
||||
def _inp(**kw):
|
||||
return DockingInputs(**kw)
|
||||
|
||||
|
||||
def _near_pose(dist=0.5):
|
||||
"""Dock pose within servo range (default 0.5 m)."""
|
||||
return DockPose(distance_m=dist, yaw_rad=0.0, lateral_m=0.0, source="aruco")
|
||||
|
||||
|
||||
def _far_pose(dist=3.0):
|
||||
"""Dock pose outside servo range."""
|
||||
return DockPose(distance_m=dist, yaw_rad=0.0, lateral_m=0.0, source="aruco")
|
||||
|
||||
|
||||
# ══════════════════════════════════════════════════════════════════════════════
|
||||
# IRBeaconDetector
|
||||
# ══════════════════════════════════════════════════════════════════════════════
|
||||
|
||||
class TestIRBeaconDetector:
|
||||
|
||||
def test_zero_signal_not_detected(self):
|
||||
ir = _make_ir()
|
||||
assert ir.update(0.0) is False
|
||||
assert ir.detected is False
|
||||
|
||||
def test_above_threshold_detected(self):
|
||||
ir = _make_ir(threshold=0.5, smoothing_alpha=1.0)
|
||||
assert ir.update(0.6) is True
|
||||
assert ir.detected is True
|
||||
|
||||
def test_below_threshold_not_detected(self):
|
||||
ir = _make_ir(threshold=0.5, smoothing_alpha=1.0)
|
||||
assert ir.update(0.4) is False
|
||||
|
||||
def test_exactly_at_threshold_detected(self):
|
||||
ir = _make_ir(threshold=0.5, smoothing_alpha=1.0)
|
||||
assert ir.update(0.5) is True
|
||||
|
||||
def test_envelope_smoothing_alpha_one(self):
|
||||
"""With alpha=1, envelope equals |sample| immediately."""
|
||||
ir = _make_ir(smoothing_alpha=1.0)
|
||||
ir.update(0.7)
|
||||
assert ir.envelope == pytest.approx(0.7, abs=1e-9)
|
||||
|
||||
def test_envelope_smoothing_below_one(self):
|
||||
"""With alpha=0.5, envelope is a running average."""
|
||||
ir = _make_ir(threshold=999.0, smoothing_alpha=0.5)
|
||||
ir.update(1.0) # envelope = 0.5
|
||||
assert ir.envelope == pytest.approx(0.5, abs=1e-6)
|
||||
ir.update(1.0) # envelope = 0.75
|
||||
assert ir.envelope == pytest.approx(0.75, abs=1e-6)
|
||||
|
||||
def test_reset_clears_envelope(self):
|
||||
ir = _make_ir(smoothing_alpha=1.0)
|
||||
ir.update(0.8)
|
||||
ir.reset()
|
||||
assert ir.envelope == pytest.approx(0.0, abs=1e-9)
|
||||
assert ir.detected is False
|
||||
|
||||
def test_negative_sample_uses_abs(self):
|
||||
"""update() should use absolute value of sample."""
|
||||
ir = _make_ir(threshold=0.5, smoothing_alpha=1.0)
|
||||
assert ir.update(-0.7) is True
|
||||
|
||||
|
||||
# ══════════════════════════════════════════════════════════════════════════════
|
||||
# VisualServo
|
||||
# ══════════════════════════════════════════════════════════════════════════════
|
||||
|
||||
class TestVisualServo:
|
||||
|
||||
def test_far_dock_gives_forward_velocity(self):
|
||||
servo = _make_servo(k_linear=1.0, target_distance_m=0.02)
|
||||
pose = DockPose(distance_m=0.5, yaw_rad=0.0, lateral_m=0.0, source="aruco")
|
||||
result = servo.update(pose)
|
||||
assert result.linear_x > 0.0
|
||||
|
||||
def test_at_target_distance_zero_linear(self):
|
||||
servo = _make_servo(k_linear=1.0, target_distance_m=0.02)
|
||||
pose = DockPose(distance_m=0.02, yaw_rad=0.0, lateral_m=0.0, source="aruco")
|
||||
result = servo.update(pose)
|
||||
assert result.linear_x == pytest.approx(0.0, abs=1e-9)
|
||||
|
||||
def test_behind_target_no_reverse(self):
|
||||
"""When distance < target, linear velocity must be 0 (no reversing)."""
|
||||
servo = _make_servo(k_linear=1.0, target_distance_m=0.10)
|
||||
pose = DockPose(distance_m=0.01, yaw_rad=0.0, lateral_m=0.0, source="aruco")
|
||||
result = servo.update(pose)
|
||||
assert result.linear_x == pytest.approx(0.0, abs=1e-9)
|
||||
|
||||
def test_yaw_right_gives_negative_angular(self):
|
||||
"""yaw_rad = atan2(tx, tz); tx > 0 → dock is right in camera → turn right (ω < 0)."""
|
||||
servo = _make_servo(k_angular=1.0)
|
||||
pose = DockPose(distance_m=0.5, yaw_rad=0.3, lateral_m=0.0, source="aruco")
|
||||
result = servo.update(pose)
|
||||
assert result.angular_z < 0.0
|
||||
|
||||
def test_yaw_left_gives_positive_angular(self):
|
||||
"""tx < 0 → dock is left in camera → turn left (ω > 0)."""
|
||||
servo = _make_servo(k_angular=1.0)
|
||||
pose = DockPose(distance_m=0.5, yaw_rad=-0.3, lateral_m=0.0, source="aruco")
|
||||
result = servo.update(pose)
|
||||
assert result.angular_z > 0.0
|
||||
|
||||
def test_zero_yaw_zero_angular(self):
|
||||
servo = _make_servo()
|
||||
pose = DockPose(distance_m=0.5, yaw_rad=0.0, lateral_m=0.0, source="aruco")
|
||||
result = servo.update(pose)
|
||||
assert result.angular_z == pytest.approx(0.0, abs=1e-9)
|
||||
|
||||
def test_max_linear_clamped(self):
|
||||
servo = _make_servo(k_linear=100.0, max_linear_ms=0.1)
|
||||
pose = DockPose(distance_m=5.0, yaw_rad=0.0, lateral_m=0.0, source="aruco")
|
||||
result = servo.update(pose)
|
||||
assert result.linear_x <= 0.1 + 1e-9
|
||||
|
||||
def test_max_angular_clamped(self):
|
||||
servo = _make_servo(k_angular=100.0, max_angular_rads=0.3)
|
||||
pose = DockPose(distance_m=0.5, yaw_rad=5.0, lateral_m=0.0, source="aruco")
|
||||
result = servo.update(pose)
|
||||
assert abs(result.angular_z) <= 0.3 + 1e-9
|
||||
|
||||
def test_aligned_when_within_tolerances(self):
|
||||
servo = _make_servo(lateral_tol_m=0.005, contact_distance_m=0.05)
|
||||
pose = DockPose(distance_m=0.03, yaw_rad=0.0, lateral_m=0.002, source="aruco")
|
||||
result = servo.update(pose)
|
||||
assert result.aligned is True
|
||||
|
||||
def test_not_aligned_lateral_too_large(self):
|
||||
servo = _make_servo(lateral_tol_m=0.005, contact_distance_m=0.05)
|
||||
pose = DockPose(distance_m=0.03, yaw_rad=0.0, lateral_m=0.010, source="aruco")
|
||||
result = servo.update(pose)
|
||||
assert result.aligned is False
|
||||
|
||||
def test_not_aligned_too_far(self):
|
||||
servo = _make_servo(lateral_tol_m=0.005, contact_distance_m=0.05)
|
||||
pose = DockPose(distance_m=0.10, yaw_rad=0.0, lateral_m=0.001, source="aruco")
|
||||
result = servo.update(pose)
|
||||
assert result.aligned is False
|
||||
|
||||
def test_stop_returns_zero_velocity(self):
|
||||
servo = _make_servo()
|
||||
result = servo.stop()
|
||||
assert result.linear_x == pytest.approx(0.0, abs=1e-9)
|
||||
assert result.angular_z == pytest.approx(0.0, abs=1e-9)
|
||||
assert result.aligned is False
|
||||
|
||||
def test_proportional_linear(self):
|
||||
"""Linear velocity scales with (distance − target)."""
|
||||
servo = _make_servo(k_linear=2.0, target_distance_m=0.0, max_linear_ms=999.0)
|
||||
pose = DockPose(distance_m=0.3, yaw_rad=0.0, lateral_m=0.0, source="aruco")
|
||||
result = servo.update(pose)
|
||||
assert result.linear_x == pytest.approx(2.0 * 0.3, rel=1e-6)
|
||||
|
||||
|
||||
# ══════════════════════════════════════════════════════════════════════════════
|
||||
# ChargeMonitor
|
||||
# ══════════════════════════════════════════════════════════════════════════════
|
||||
|
||||
class TestChargeMonitor:
|
||||
|
||||
def test_initially_not_charging(self):
|
||||
cm = _make_charge()
|
||||
assert cm.is_charging is False
|
||||
|
||||
def test_charging_started_event(self):
|
||||
cm = _make_charge(charging_current_threshold_a=0.1)
|
||||
events = cm.update(50.0, 0.5)
|
||||
assert ChargeEvent.CHARGING_STARTED in events
|
||||
|
||||
def test_charging_stopped_event(self):
|
||||
cm = _make_charge(charging_current_threshold_a=0.1)
|
||||
cm.update(50.0, 0.5) # start charging
|
||||
events = cm.update(50.0, 0.0) # stop
|
||||
assert ChargeEvent.CHARGING_STOPPED in events
|
||||
|
||||
def test_no_duplicate_charging_started(self):
|
||||
"""CHARGING_STARTED fires only once per rising edge."""
|
||||
cm = _make_charge(charging_current_threshold_a=0.1)
|
||||
cm.update(50.0, 0.5) # fires CHARGING_STARTED
|
||||
events = cm.update(50.0, 0.5) # still charging — no duplicate
|
||||
assert ChargeEvent.CHARGING_STARTED not in events
|
||||
|
||||
def test_low_threshold_event_falling_edge(self):
|
||||
"""THRESHOLD_LOW fires when SOC first drops below low_threshold_pct."""
|
||||
cm = _make_charge(low_threshold_pct=20.0)
|
||||
events = cm.update(19.0, 0.0)
|
||||
assert ChargeEvent.THRESHOLD_LOW in events
|
||||
|
||||
def test_no_duplicate_low_threshold(self):
|
||||
cm = _make_charge(low_threshold_pct=20.0)
|
||||
cm.update(19.0, 0.0) # fires once
|
||||
events = cm.update(18.0, 0.0) # still below — no repeat
|
||||
assert ChargeEvent.THRESHOLD_LOW not in events
|
||||
|
||||
def test_high_threshold_event_rising_edge(self):
|
||||
"""THRESHOLD_HIGH fires when SOC rises above high_threshold_pct."""
|
||||
cm = _make_charge(low_threshold_pct=15.0, high_threshold_pct=80.0)
|
||||
# Start below high threshold
|
||||
cm.update(50.0, 0.5) # sets was_high = False (50 < 80)
|
||||
events = cm.update(81.0, 0.5) # rises above 80
|
||||
assert ChargeEvent.THRESHOLD_HIGH in events
|
||||
|
||||
def test_no_high_event_if_always_above(self):
|
||||
"""THRESHOLD_HIGH should not fire if SOC starts above threshold."""
|
||||
cm = _make_charge(high_threshold_pct=80.0)
|
||||
# Default _was_high=True; remains above → no edge
|
||||
events = cm.update(90.0, 0.0)
|
||||
assert ChargeEvent.THRESHOLD_HIGH not in events
|
||||
|
||||
def test_is_depleted_property(self):
|
||||
cm = _make_charge(low_threshold_pct=20.0)
|
||||
cm.update(15.0, 0.0)
|
||||
assert cm.is_depleted is True
|
||||
|
||||
def test_is_charged_property(self):
|
||||
cm = _make_charge(high_threshold_pct=80.0)
|
||||
cm.update(85.0, 0.0)
|
||||
assert cm.is_charged is True
|
||||
|
||||
def test_percentage_tracking(self):
|
||||
cm = _make_charge()
|
||||
cm.update(42.5, 0.0)
|
||||
assert cm.percentage == pytest.approx(42.5, abs=1e-6)
|
||||
|
||||
def test_reset_clears_state(self):
|
||||
cm = _make_charge(charging_current_threshold_a=0.1)
|
||||
cm.update(10.0, 0.5) # is_charging=True, is_depleted=True
|
||||
cm.reset()
|
||||
assert cm.is_charging is False
|
||||
assert cm.is_depleted is False
|
||||
assert cm.percentage == pytest.approx(100.0, abs=1e-6)
|
||||
|
||||
|
||||
# ══════════════════════════════════════════════════════════════════════════════
|
||||
# DockingStateMachine
|
||||
# ══════════════════════════════════════════════════════════════════════════════
|
||||
|
||||
class TestDockingStateMachineBasic:
|
||||
|
||||
def test_initial_state_is_idle(self):
|
||||
fsm = _make_fsm()
|
||||
assert fsm.state == DockingState.IDLE
|
||||
|
||||
def test_idle_stays_idle_on_good_battery(self):
|
||||
fsm = _make_fsm()
|
||||
out = fsm.tick(_inp(battery_pct=80.0))
|
||||
assert fsm.state == DockingState.IDLE
|
||||
assert out.state_changed is False
|
||||
|
||||
def test_battery_low_triggers_detecting(self):
|
||||
fsm = _make_fsm()
|
||||
out = fsm.tick(_inp(battery_pct=10.0))
|
||||
assert fsm.state == DockingState.DETECTING
|
||||
assert out.mission_interrupted is True
|
||||
assert out.state_changed is True
|
||||
|
||||
def test_dock_request_triggers_detecting(self):
|
||||
fsm = _make_fsm()
|
||||
out = fsm.tick(_inp(battery_pct=80.0, dock_requested=True))
|
||||
assert fsm.state == DockingState.DETECTING
|
||||
assert out.mission_interrupted is False
|
||||
|
||||
def test_dock_request_not_duplicated_mission_interrupted(self):
|
||||
"""Explicit dock request should not set mission_interrupted."""
|
||||
fsm = _make_fsm()
|
||||
out = fsm.tick(_inp(battery_pct=80.0, dock_requested=True))
|
||||
assert out.mission_interrupted is False
|
||||
|
||||
|
||||
class TestDockingStateMachineDetecting:
|
||||
|
||||
def test_no_dock_stays_detecting(self):
|
||||
fsm = _make_fsm()
|
||||
fsm.tick(_inp(dock_requested=True))
|
||||
out = fsm.tick(_inp()) # no dock_pose
|
||||
assert fsm.state == DockingState.DETECTING
|
||||
|
||||
def test_far_dock_goes_to_nav2(self):
|
||||
fsm = _make_fsm(servo_range_m=1.0)
|
||||
fsm.tick(_inp(dock_requested=True))
|
||||
out = fsm.tick(_inp(dock_pose=_far_pose()))
|
||||
assert fsm.state == DockingState.NAV2_APPROACH
|
||||
assert out.request_nav2 is True
|
||||
|
||||
def test_near_dock_skips_nav2_to_servo(self):
|
||||
fsm = _make_fsm(servo_range_m=1.0)
|
||||
fsm.tick(_inp(dock_requested=True))
|
||||
out = fsm.tick(_inp(dock_pose=_near_pose()))
|
||||
assert fsm.state == DockingState.VISUAL_SERVO
|
||||
|
||||
def test_undock_request_aborts_detecting(self):
|
||||
fsm = _make_fsm()
|
||||
fsm.tick(_inp(dock_requested=True))
|
||||
fsm.tick(_inp(undock_requested=True))
|
||||
assert fsm.state == DockingState.IDLE
|
||||
|
||||
|
||||
class TestDockingStateMachineNav2Approach:
|
||||
|
||||
def _reach_nav2(self, fsm):
|
||||
fsm.tick(_inp(dock_requested=True))
|
||||
fsm.tick(_inp(dock_pose=_far_pose()))
|
||||
assert fsm.state == DockingState.NAV2_APPROACH
|
||||
|
||||
def test_nav2_arrived_enters_visual_servo(self):
|
||||
fsm = _make_fsm()
|
||||
self._reach_nav2(fsm)
|
||||
fsm.tick(_inp(nav2_arrived=True))
|
||||
assert fsm.state == DockingState.VISUAL_SERVO
|
||||
|
||||
def test_dock_close_during_nav2_enters_servo(self):
|
||||
fsm = _make_fsm(servo_range_m=1.0)
|
||||
self._reach_nav2(fsm)
|
||||
out = fsm.tick(_inp(dock_pose=_near_pose()))
|
||||
assert fsm.state == DockingState.VISUAL_SERVO
|
||||
assert out.cancel_nav2 is True
|
||||
|
||||
def test_nav2_failed_back_to_detecting(self):
|
||||
fsm = _make_fsm()
|
||||
self._reach_nav2(fsm)
|
||||
fsm.tick(_inp(nav2_failed=True))
|
||||
assert fsm.state == DockingState.DETECTING
|
||||
|
||||
def test_undock_during_nav2_cancels_and_idles(self):
|
||||
fsm = _make_fsm()
|
||||
self._reach_nav2(fsm)
|
||||
out = fsm.tick(_inp(undock_requested=True))
|
||||
assert fsm.state == DockingState.IDLE
|
||||
assert out.cancel_nav2 is True
|
||||
|
||||
|
||||
class TestDockingStateMachineVisualServo:
|
||||
|
||||
def _reach_servo(self, fsm):
|
||||
fsm.tick(_inp(dock_requested=True))
|
||||
fsm.tick(_inp(dock_pose=_near_pose()))
|
||||
assert fsm.state == DockingState.VISUAL_SERVO
|
||||
|
||||
def test_aligned_enters_contact(self):
|
||||
fsm = _make_fsm()
|
||||
self._reach_servo(fsm)
|
||||
fsm.tick(_inp(dock_pose=_near_pose(), servo_aligned=True))
|
||||
assert fsm.state == DockingState.CONTACT
|
||||
|
||||
def test_not_aligned_stays_in_servo(self):
|
||||
fsm = _make_fsm()
|
||||
self._reach_servo(fsm)
|
||||
fsm.tick(_inp(dock_pose=_near_pose(), servo_aligned=False))
|
||||
assert fsm.state == DockingState.VISUAL_SERVO
|
||||
|
||||
def test_lost_detection_timeout_enters_detecting(self):
|
||||
fsm = _make_fsm(lost_ticks_max=3)
|
||||
self._reach_servo(fsm)
|
||||
for _ in range(3): # dock_pose=None each tick
|
||||
fsm.tick(_inp())
|
||||
assert fsm.state == DockingState.DETECTING
|
||||
|
||||
def test_lost_counter_resets_on_rediscovery(self):
|
||||
"""Lost count should reset when pose reappears."""
|
||||
fsm = _make_fsm(lost_ticks_max=3)
|
||||
self._reach_servo(fsm)
|
||||
fsm.tick(_inp()) # 1 lost tick
|
||||
fsm.tick(_inp(dock_pose=_near_pose())) # rediscovered
|
||||
# Need 3 more lost ticks to time out
|
||||
fsm.tick(_inp())
|
||||
fsm.tick(_inp())
|
||||
assert fsm.state == DockingState.VISUAL_SERVO
|
||||
fsm.tick(_inp())
|
||||
assert fsm.state == DockingState.DETECTING
|
||||
|
||||
def test_undock_aborts_visual_servo(self):
|
||||
fsm = _make_fsm()
|
||||
self._reach_servo(fsm)
|
||||
fsm.tick(_inp(undock_requested=True))
|
||||
assert fsm.state == DockingState.IDLE
|
||||
|
||||
|
||||
class TestDockingStateMachineContact:
|
||||
|
||||
def _reach_contact(self, fsm):
|
||||
fsm.tick(_inp(dock_requested=True))
|
||||
fsm.tick(_inp(dock_pose=_near_pose()))
|
||||
fsm.tick(_inp(dock_pose=_near_pose(), servo_aligned=True))
|
||||
assert fsm.state == DockingState.CONTACT
|
||||
|
||||
def test_charging_enters_charging_state(self):
|
||||
fsm = _make_fsm()
|
||||
self._reach_contact(fsm)
|
||||
fsm.tick(_inp(is_charging=True))
|
||||
assert fsm.state == DockingState.CHARGING
|
||||
|
||||
def test_contact_timeout_retries_servo(self):
|
||||
fsm = _make_fsm(contact_timeout_ticks=3)
|
||||
self._reach_contact(fsm)
|
||||
for _ in range(3):
|
||||
fsm.tick(_inp(is_charging=False))
|
||||
assert fsm.state == DockingState.VISUAL_SERVO
|
||||
|
||||
def test_undock_from_contact_to_undocking(self):
|
||||
fsm = _make_fsm()
|
||||
self._reach_contact(fsm)
|
||||
fsm.tick(_inp(undock_requested=True))
|
||||
assert fsm.state == DockingState.UNDOCKING
|
||||
|
||||
|
||||
class TestDockingStateMachineCharging:
|
||||
|
||||
def _reach_charging(self, fsm):
|
||||
fsm.tick(_inp(dock_requested=True))
|
||||
fsm.tick(_inp(dock_pose=_near_pose()))
|
||||
fsm.tick(_inp(dock_pose=_near_pose(), servo_aligned=True))
|
||||
fsm.tick(_inp(is_charging=True))
|
||||
assert fsm.state == DockingState.CHARGING
|
||||
|
||||
def test_full_battery_triggers_undocking(self):
|
||||
fsm = _make_fsm(battery_high_pct=80.0)
|
||||
self._reach_charging(fsm)
|
||||
fsm.tick(_inp(is_charging=True, battery_pct=85.0))
|
||||
assert fsm.state == DockingState.UNDOCKING
|
||||
|
||||
def test_undock_request_during_charging(self):
|
||||
fsm = _make_fsm()
|
||||
self._reach_charging(fsm)
|
||||
fsm.tick(_inp(undock_requested=True))
|
||||
assert fsm.state == DockingState.UNDOCKING
|
||||
|
||||
def test_charging_stays_charging_below_high_threshold(self):
|
||||
fsm = _make_fsm(battery_high_pct=80.0)
|
||||
self._reach_charging(fsm)
|
||||
fsm.tick(_inp(is_charging=True, battery_pct=70.0))
|
||||
assert fsm.state == DockingState.CHARGING
|
||||
|
||||
|
||||
class TestDockingStateMachineUndocking:
|
||||
|
||||
def _reach_undocking_via_charge(self, fsm):
|
||||
fsm.tick(_inp(dock_requested=True))
|
||||
fsm.tick(_inp(dock_pose=_near_pose()))
|
||||
fsm.tick(_inp(dock_pose=_near_pose(), servo_aligned=True))
|
||||
fsm.tick(_inp(is_charging=True))
|
||||
fsm.tick(_inp(is_charging=True, battery_pct=85.0)) # → UNDOCKING
|
||||
assert fsm.state == DockingState.UNDOCKING
|
||||
|
||||
def test_undocking_backs_away(self):
|
||||
fsm = _make_fsm(undock_ticks_max=5)
|
||||
self._reach_undocking_via_charge(fsm)
|
||||
out = fsm.tick(_inp())
|
||||
assert out.cmd_linear < 0.0
|
||||
|
||||
def test_undocking_completes_back_to_idle(self):
|
||||
fsm = _make_fsm(undock_ticks_max=5)
|
||||
self._reach_undocking_via_charge(fsm)
|
||||
for _ in range(5):
|
||||
out = fsm.tick(_inp())
|
||||
assert fsm.state == DockingState.IDLE
|
||||
|
||||
def test_mission_resume_after_battery_auto_dock(self):
|
||||
"""mission_resume must be True when auto-dock cycle completes."""
|
||||
fsm = _make_fsm(undock_ticks_max=5)
|
||||
# Trigger via battery low
|
||||
fsm.tick(_inp(battery_pct=10.0)) # IDLE → DETECTING
|
||||
fsm.tick(_inp(dock_pose=_near_pose())) # → VISUAL_SERVO
|
||||
fsm.tick(_inp(dock_pose=_near_pose(), servo_aligned=True)) # → CONTACT
|
||||
fsm.tick(_inp(is_charging=True)) # → CHARGING
|
||||
fsm.tick(_inp(is_charging=True, battery_pct=85.0)) # → UNDOCKING
|
||||
out = None
|
||||
for _ in range(5):
|
||||
out = fsm.tick(_inp())
|
||||
assert out is not None
|
||||
assert out.mission_resume is True
|
||||
assert fsm.state == DockingState.IDLE
|
||||
|
||||
def test_no_mission_resume_on_explicit_undock(self):
|
||||
"""Explicit undock should not trigger mission_resume."""
|
||||
fsm = _make_fsm(undock_ticks_max=5)
|
||||
fsm.tick(_inp(dock_requested=True)) # explicit dock — no interrupt
|
||||
fsm.tick(_inp(dock_pose=_near_pose()))
|
||||
fsm.tick(_inp(dock_pose=_near_pose(), servo_aligned=True))
|
||||
fsm.tick(_inp(is_charging=True))
|
||||
fsm.tick(_inp(undock_requested=True)) # explicit undock
|
||||
last_out = None
|
||||
for _ in range(5):
|
||||
last_out = fsm.tick(_inp())
|
||||
assert last_out is not None
|
||||
assert last_out.mission_resume is False
|
||||
|
||||
|
||||
class TestDockingStateMachineReset:
|
||||
|
||||
def test_reset_returns_to_idle(self):
|
||||
fsm = _make_fsm()
|
||||
fsm.tick(_inp(dock_requested=True))
|
||||
assert fsm.state == DockingState.DETECTING
|
||||
fsm.reset()
|
||||
assert fsm.state == DockingState.IDLE
|
||||
|
||||
def test_state_changed_flag_on_transition(self):
|
||||
fsm = _make_fsm()
|
||||
out = fsm.tick(_inp(dock_requested=True))
|
||||
assert out.state_changed is True
|
||||
|
||||
def test_no_state_changed_when_same_state(self):
|
||||
fsm = _make_fsm()
|
||||
out = fsm.tick(_inp()) # stays IDLE
|
||||
assert out.state_changed is False
|
||||
@ -1,16 +0,0 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(saltybot_docking_msgs)
|
||||
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(rosidl_default_generators REQUIRED)
|
||||
find_package(builtin_interfaces REQUIRED)
|
||||
|
||||
rosidl_generate_interfaces(${PROJECT_NAME}
|
||||
"msg/DockingStatus.msg"
|
||||
"srv/Dock.srv"
|
||||
"srv/Undock.srv"
|
||||
DEPENDENCIES builtin_interfaces
|
||||
)
|
||||
|
||||
ament_export_dependencies(rosidl_default_runtime)
|
||||
ament_package()
|
||||
@ -1,21 +0,0 @@
|
||||
# DockingStatus.msg — Real-time docking state snapshot (Issue #158)
|
||||
# Published by: /saltybot/docking_node
|
||||
# Topic: /saltybot/docking_status
|
||||
|
||||
builtin_interfaces/Time stamp
|
||||
|
||||
# Current FSM state
|
||||
# Values: "IDLE" | "DETECTING" | "NAV2_APPROACH" | "VISUAL_SERVO" | "CONTACT" | "CHARGING" | "UNDOCKING"
|
||||
string state
|
||||
|
||||
# Dock detection
|
||||
bool dock_detected # ArUco marker or IR beacon visible
|
||||
float32 distance_m # distance to dock (m); NaN if unknown
|
||||
float32 lateral_error_m # lateral offset to dock centre (m); +ve = dock is right
|
||||
|
||||
# Battery / charging
|
||||
float32 battery_pct # state of charge [0–100]
|
||||
bool charging # charge current detected on dock pins
|
||||
|
||||
# Alignment quality (active only during VISUAL_SERVO)
|
||||
bool aligned # within ±5 mm lateral and contact distance
|
||||
@ -1,22 +0,0 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>saltybot_docking_msgs</name>
|
||||
<version>0.1.0</version>
|
||||
<description>Docking station message and service definitions for SaltyBot (Issue #158)</description>
|
||||
<maintainer email="sl-controls@saltylab.local">sl-controls</maintainer>
|
||||
<license>MIT</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
<buildtool_depend>rosidl_default_generators</buildtool_depend>
|
||||
|
||||
<depend>builtin_interfaces</depend>
|
||||
|
||||
<exec_depend>rosidl_default_runtime</exec_depend>
|
||||
|
||||
<member_of_group>rosidl_interface_packages</member_of_group>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@ -1,9 +0,0 @@
|
||||
# Dock.srv — Request the robot to autonomously dock (Issue #158)
|
||||
# Service: /saltybot/dock
|
||||
|
||||
# If force=True the current Nav2 mission is cancelled immediately.
|
||||
# If force=False the dock request is honoured only when the robot is idle.
|
||||
bool force
|
||||
---
|
||||
bool success
|
||||
string message
|
||||
@ -1,9 +0,0 @@
|
||||
# Undock.srv — Request the robot to leave the dock (Issue #158)
|
||||
# Service: /saltybot/undock
|
||||
|
||||
# If resume_mission=True and a mission was interrupted for battery, it is
|
||||
# re-queued after the robot backs away from the dock.
|
||||
bool resume_mission
|
||||
---
|
||||
bool success
|
||||
string message
|
||||
@ -1,31 +0,0 @@
|
||||
gesture_node:
|
||||
ros__parameters:
|
||||
# Number of active CSI camera streams to subscribe to
|
||||
n_cameras: 4
|
||||
|
||||
# Target processing rate in Hz. At 10 Hz with 4 cameras, each camera
|
||||
# contributes frames at approximately 2.5 Hz (round-robin scheduling).
|
||||
process_fps: 10.0
|
||||
|
||||
# Minimum MediaPipe detection confidence to publish a gesture
|
||||
min_confidence: 0.60
|
||||
|
||||
# MediaPipe model complexity:
|
||||
# 0 = lite (fastest, ≈ 13 ms/frame on Orin Nano — use for production)
|
||||
# 1 = full (more accurate, ≈ 25 ms/frame)
|
||||
# 2 = heavy (highest accuracy, ≈ 50 ms/frame, requires large VRAM)
|
||||
model_complexity: 0
|
||||
|
||||
# Maximum hands detected per frame
|
||||
max_hands: 2
|
||||
|
||||
# Enable/disable inference modules
|
||||
pose_enabled: true
|
||||
hands_enabled: true
|
||||
|
||||
# Comma-separated gesture allowlist. Empty string = all gestures enabled.
|
||||
# Example: "wave,stop_palm,thumbs_up,arms_up"
|
||||
enabled_gestures: ""
|
||||
|
||||
# Camera name list (must match saltybot_cameras topic naming)
|
||||
camera_names: "front,left,rear,right"
|
||||
@ -1,67 +0,0 @@
|
||||
"""gesture.launch.py — Launch gesture_node for multi-camera gesture recognition (Issue #140)."""
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument
|
||||
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
|
||||
from launch_ros.actions import Node
|
||||
from launch_ros.substitutions import FindPackageShare
|
||||
|
||||
|
||||
def generate_launch_description() -> LaunchDescription:
|
||||
pkg = FindPackageShare("saltybot_social")
|
||||
params_file = PathJoinSubstitution([pkg, "config", "gesture_params.yaml"])
|
||||
|
||||
return LaunchDescription([
|
||||
DeclareLaunchArgument(
|
||||
"params_file",
|
||||
default_value=params_file,
|
||||
description="Path to gesture_node parameter YAML",
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"process_fps",
|
||||
default_value="10.0",
|
||||
description="Target gesture detection rate (Hz)",
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"model_complexity",
|
||||
default_value="0",
|
||||
description="MediaPipe model complexity: 0=lite, 1=full, 2=heavy",
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"pose_enabled",
|
||||
default_value="true",
|
||||
description="Enable MediaPipe Pose body-gesture detection",
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"hands_enabled",
|
||||
default_value="true",
|
||||
description="Enable MediaPipe Hands hand-gesture detection",
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"enabled_gestures",
|
||||
default_value="",
|
||||
description="Comma-separated gesture allowlist; empty = all",
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"min_confidence",
|
||||
default_value="0.60",
|
||||
description="Minimum detection confidence threshold",
|
||||
),
|
||||
Node(
|
||||
package="saltybot_social",
|
||||
executable="gesture_node",
|
||||
name="gesture_node",
|
||||
output="screen",
|
||||
parameters=[
|
||||
LaunchConfiguration("params_file"),
|
||||
{
|
||||
"process_fps": LaunchConfiguration("process_fps"),
|
||||
"model_complexity": LaunchConfiguration("model_complexity"),
|
||||
"pose_enabled": LaunchConfiguration("pose_enabled"),
|
||||
"hands_enabled": LaunchConfiguration("hands_enabled"),
|
||||
"enabled_gestures": LaunchConfiguration("enabled_gestures"),
|
||||
"min_confidence": LaunchConfiguration("min_confidence"),
|
||||
},
|
||||
],
|
||||
),
|
||||
])
|
||||
@ -1,483 +0,0 @@
|
||||
"""gesture_classifier.py — Geometric gesture classification from MediaPipe landmarks.
|
||||
|
||||
Pure Python, no ROS2 / MediaPipe / OpenCV dependencies.
|
||||
Consumes normalised landmark arrays produced by MediaPipe Hands and Pose,
|
||||
classifies them into named gesture types, and tracks temporal gestures
|
||||
(wave, come_here) via per-stream history buffers.
|
||||
|
||||
Usage
|
||||
-----
|
||||
>>> from saltybot_social.gesture_classifier import (
|
||||
... Landmark, classify_hand, classify_pose, WaveDetector
|
||||
... )
|
||||
>>> lm = [Landmark(x, y, z) for x, y, z in raw_coords]
|
||||
>>> result = classify_hand(lm, is_right_hand=True, wave_det=None)
|
||||
>>> result.gesture_type, result.confidence
|
||||
('stop_palm', 0.92)
|
||||
|
||||
Coordinate convention
|
||||
---------------------
|
||||
MediaPipe Hands / Pose use image-normalised coordinates:
|
||||
x: 0.0 = left edge, 1.0 = right edge of frame
|
||||
y: 0.0 = top edge, 1.0 = bottom edge (↓ positive)
|
||||
z: depth (approx), 0 at wrist/hip plane; negative = towards camera
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
from collections import deque
|
||||
from dataclasses import dataclass
|
||||
from typing import Deque, List, Optional, Tuple
|
||||
|
||||
# ── Landmark type ─────────────────────────────────────────────────────────────
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class Landmark:
|
||||
x: float
|
||||
y: float
|
||||
z: float = 0.0
|
||||
visibility: float = 1.0
|
||||
|
||||
|
||||
# ── Result type ───────────────────────────────────────────────────────────────
|
||||
|
||||
|
||||
@dataclass
|
||||
class ClassifiedGesture:
|
||||
gesture_type: str # e.g. "stop_palm", "wave", "arms_up"
|
||||
confidence: float # 0.0–1.0
|
||||
hand_x: float = 0.5 # normalised image x of gesture anchor
|
||||
hand_y: float = 0.5 # normalised image y of gesture anchor
|
||||
is_right_hand: bool = True
|
||||
direction: str = "" # for "point": "left"/"right"/"up"/"forward"
|
||||
source: str = "hand" # "hand" or "body_pose"
|
||||
|
||||
|
||||
# ── MediaPipe Hands landmark indices ─────────────────────────────────────────
|
||||
# Reference: https://developers.google.com/mediapipe/solutions/vision/hand_landmarker
|
||||
|
||||
_WRIST = 0
|
||||
_THUMB_CMC = 1; _THUMB_MCP = 2; _THUMB_IP = 3; _THUMB_TIP = 4
|
||||
_INDEX_MCP = 5; _INDEX_PIP = 6; _INDEX_DIP = 7; _INDEX_TIP = 8
|
||||
_MIDDLE_MCP = 9; _MIDDLE_PIP = 10; _MIDDLE_DIP = 11; _MIDDLE_TIP = 12
|
||||
_RING_MCP = 13; _RING_PIP = 14; _RING_DIP = 15; _RING_TIP = 16
|
||||
_PINKY_MCP = 17; _PINKY_PIP = 18; _PINKY_DIP = 19; _PINKY_TIP = 20
|
||||
|
||||
# ── MediaPipe Pose landmark indices ──────────────────────────────────────────
|
||||
|
||||
_L_SHOULDER = 11; _R_SHOULDER = 12
|
||||
_L_ELBOW = 13; _R_ELBOW = 14
|
||||
_L_WRIST = 15; _R_WRIST = 16
|
||||
_L_HIP = 23; _R_HIP = 24
|
||||
_L_KNEE = 25; _R_KNEE = 26
|
||||
_L_ANKLE = 27; _R_ANKLE = 28
|
||||
|
||||
|
||||
# ── Finger-extension helpers ──────────────────────────────────────────────────
|
||||
|
||||
|
||||
def _finger_up(lm: List[Landmark], tip: int, pip: int) -> bool:
|
||||
"""Return True if the finger tip is above (smaller y) its PIP joint."""
|
||||
return lm[tip].y < lm[pip].y
|
||||
|
||||
|
||||
def _finger_up_score(lm: List[Landmark], tip: int, pip: int, mcp: int) -> float:
|
||||
"""Return extension score in [0, 1] based on how far tip is above MCP."""
|
||||
spread = lm[mcp].y - lm[tip].y # positive = tip above mcp
|
||||
palm_height = abs(lm[_WRIST].y - lm[_MIDDLE_MCP].y) or 0.01
|
||||
return max(0.0, min(1.0, spread / palm_height))
|
||||
|
||||
|
||||
def _thumb_up(lm: List[Landmark], is_right: bool) -> bool:
|
||||
"""True when the thumb is extended upward (tip clearly above CMC base)."""
|
||||
return lm[_THUMB_TIP].y < lm[_THUMB_CMC].y - 0.02
|
||||
|
||||
|
||||
def _thumb_down(lm: List[Landmark]) -> bool:
|
||||
"""True when the thumb is pointing downward (tip below the wrist)."""
|
||||
return lm[_THUMB_TIP].y > lm[_WRIST].y + 0.04
|
||||
|
||||
|
||||
def _four_fingers_curled(lm: List[Landmark]) -> bool:
|
||||
"""True when index, middle, ring, and pinky are all NOT extended."""
|
||||
fingers = [
|
||||
(_INDEX_TIP, _INDEX_PIP), (_MIDDLE_TIP, _MIDDLE_PIP),
|
||||
(_RING_TIP, _RING_PIP), (_PINKY_TIP, _PINKY_PIP),
|
||||
]
|
||||
return not any(_finger_up(lm, t, p) for t, p in fingers)
|
||||
|
||||
|
||||
def _count_fingers_up(lm: List[Landmark]) -> int:
|
||||
"""Count how many of index/middle/ring/pinky are extended."""
|
||||
pairs = [
|
||||
(_INDEX_TIP, _INDEX_PIP), (_MIDDLE_TIP, _MIDDLE_PIP),
|
||||
(_RING_TIP, _RING_PIP), (_PINKY_TIP, _PINKY_PIP),
|
||||
]
|
||||
return sum(_finger_up(lm, t, p) for t, p in pairs)
|
||||
|
||||
|
||||
def _palm_center(lm: List[Landmark]) -> Tuple[float, float]:
|
||||
"""Return (x, y) centroid of the four MCP knuckles."""
|
||||
xs = [lm[i].x for i in (_INDEX_MCP, _MIDDLE_MCP, _RING_MCP, _PINKY_MCP)]
|
||||
ys = [lm[i].y for i in (_INDEX_MCP, _MIDDLE_MCP, _RING_MCP, _PINKY_MCP)]
|
||||
return sum(xs) / 4, sum(ys) / 4
|
||||
|
||||
|
||||
# ── Point direction ───────────────────────────────────────────────────────────
|
||||
|
||||
|
||||
def _point_direction(lm: List[Landmark]) -> str:
|
||||
"""Determine the pointing direction from index MCP → TIP vector."""
|
||||
dx = lm[_INDEX_TIP].x - lm[_INDEX_MCP].x
|
||||
dy = lm[_INDEX_TIP].y - lm[_INDEX_MCP].y # positive = downward in image
|
||||
angle_deg = math.degrees(math.atan2(-dy, dx)) # flip y so up=+90
|
||||
# Quantise into 8 compass directions
|
||||
if -22.5 <= angle_deg < 22.5:
|
||||
return "right"
|
||||
elif 22.5 <= angle_deg < 67.5:
|
||||
return "upper_right"
|
||||
elif 67.5 <= angle_deg < 112.5:
|
||||
return "up"
|
||||
elif 112.5 <= angle_deg < 157.5:
|
||||
return "upper_left"
|
||||
elif angle_deg >= 157.5 or angle_deg < -157.5:
|
||||
return "left"
|
||||
elif -157.5 <= angle_deg < -112.5:
|
||||
return "lower_left"
|
||||
elif -112.5 <= angle_deg < -67.5:
|
||||
return "down"
|
||||
else:
|
||||
return "lower_right"
|
||||
|
||||
|
||||
# ── Hand gesture classifiers ──────────────────────────────────────────────────
|
||||
|
||||
|
||||
def _classify_stop_palm(lm: List[Landmark]) -> Optional[ClassifiedGesture]:
|
||||
"""All five fingers extended — 'stop' hand signal."""
|
||||
n_up = _count_fingers_up(lm)
|
||||
if n_up < 4:
|
||||
return None
|
||||
# Avg extension score for quality measure
|
||||
ext_scores = [
|
||||
_finger_up_score(lm, _INDEX_TIP, _INDEX_PIP, _INDEX_MCP),
|
||||
_finger_up_score(lm, _MIDDLE_TIP, _MIDDLE_PIP, _MIDDLE_MCP),
|
||||
_finger_up_score(lm, _RING_TIP, _RING_PIP, _RING_MCP),
|
||||
_finger_up_score(lm, _PINKY_TIP, _PINKY_PIP, _PINKY_MCP),
|
||||
]
|
||||
avg_ext = sum(ext_scores) / len(ext_scores)
|
||||
conf = 0.6 + 0.35 * avg_ext + 0.05 * (n_up == 4)
|
||||
cx, cy = _palm_center(lm)
|
||||
return ClassifiedGesture(
|
||||
gesture_type="stop_palm", confidence=round(conf, 3),
|
||||
hand_x=cx, hand_y=cy
|
||||
)
|
||||
|
||||
|
||||
def _classify_thumbs_up(lm: List[Landmark], is_right: bool) -> Optional[ClassifiedGesture]:
|
||||
"""Thumb extended upward, other fingers curled."""
|
||||
if not _thumb_up(lm, is_right):
|
||||
return None
|
||||
if not _four_fingers_curled(lm):
|
||||
return None
|
||||
gap = lm[_THUMB_CMC].y - lm[_THUMB_TIP].y # larger = more vertical
|
||||
palm_h = abs(lm[_WRIST].y - lm[_MIDDLE_MCP].y) or 0.01
|
||||
conf = min(0.95, 0.6 + 0.35 * min(1.0, gap / palm_h))
|
||||
cx, cy = _palm_center(lm)
|
||||
return ClassifiedGesture(
|
||||
gesture_type="thumbs_up", confidence=round(conf, 3),
|
||||
hand_x=cx, hand_y=cy
|
||||
)
|
||||
|
||||
|
||||
def _classify_thumbs_down(lm: List[Landmark]) -> Optional[ClassifiedGesture]:
|
||||
"""Thumb pointing downward, other fingers curled."""
|
||||
if not _thumb_down(lm):
|
||||
return None
|
||||
if not _four_fingers_curled(lm):
|
||||
return None
|
||||
drop = lm[_THUMB_TIP].y - lm[_WRIST].y
|
||||
palm_h = abs(lm[_WRIST].y - lm[_MIDDLE_MCP].y) or 0.01
|
||||
conf = min(0.95, 0.6 + 0.35 * min(1.0, drop / palm_h))
|
||||
cx, cy = _palm_center(lm)
|
||||
return ClassifiedGesture(
|
||||
gesture_type="thumbs_down", confidence=round(conf, 3),
|
||||
hand_x=cx, hand_y=cy
|
||||
)
|
||||
|
||||
|
||||
def _classify_point(lm: List[Landmark]) -> Optional[ClassifiedGesture]:
|
||||
"""Index finger extended, middle/ring/pinky curled."""
|
||||
if not _finger_up(lm, _INDEX_TIP, _INDEX_PIP):
|
||||
return None
|
||||
others_up = sum([
|
||||
_finger_up(lm, _MIDDLE_TIP, _MIDDLE_PIP),
|
||||
_finger_up(lm, _RING_TIP, _RING_PIP),
|
||||
_finger_up(lm, _PINKY_TIP, _PINKY_PIP),
|
||||
])
|
||||
if others_up >= 1:
|
||||
return None
|
||||
# Strongly horizontal pointing defers to "follow" gesture
|
||||
if abs(lm[_INDEX_TIP].x - lm[_INDEX_MCP].x) > 0.20:
|
||||
return None
|
||||
ext = _finger_up_score(lm, _INDEX_TIP, _INDEX_PIP, _INDEX_MCP)
|
||||
conf = 0.65 + 0.30 * ext - 0.10 * others_up
|
||||
direction = _point_direction(lm)
|
||||
return ClassifiedGesture(
|
||||
gesture_type="point", confidence=round(conf, 3),
|
||||
hand_x=lm[_INDEX_TIP].x, hand_y=lm[_INDEX_TIP].y,
|
||||
direction=direction,
|
||||
)
|
||||
|
||||
|
||||
def _classify_come_here(lm: List[Landmark]) -> Optional[ClassifiedGesture]:
|
||||
"""Beckoning pose: index MCP raised but index TIP curled toward palm."""
|
||||
# MCP above wrist (hand raised), TIP clearly below MCP (finger curled down)
|
||||
mcp_raised = lm[_INDEX_MCP].y < lm[_WRIST].y - 0.02
|
||||
tip_curled = lm[_INDEX_TIP].y > lm[_INDEX_MCP].y + 0.03
|
||||
if not (mcp_raised and tip_curled):
|
||||
return None
|
||||
# Middle, ring, pinky should be curled too (not fully open)
|
||||
n_others_up = sum([
|
||||
_finger_up(lm, _MIDDLE_TIP, _MIDDLE_PIP),
|
||||
_finger_up(lm, _RING_TIP, _RING_PIP),
|
||||
_finger_up(lm, _PINKY_TIP, _PINKY_PIP),
|
||||
])
|
||||
if n_others_up >= 2:
|
||||
return None
|
||||
curl_depth = lm[_INDEX_TIP].y - lm[_INDEX_PIP].y
|
||||
palm_h = abs(lm[_WRIST].y - lm[_MIDDLE_MCP].y) or 0.01
|
||||
conf = min(0.90, 0.60 + 0.30 * min(1.0, curl_depth / (palm_h * 0.5)))
|
||||
cx, cy = _palm_center(lm)
|
||||
return ClassifiedGesture(
|
||||
gesture_type="come_here", confidence=round(conf, 3),
|
||||
hand_x=cx, hand_y=cy
|
||||
)
|
||||
|
||||
|
||||
def _classify_follow(lm: List[Landmark]) -> Optional[ClassifiedGesture]:
|
||||
"""Follow gesture: index extended horizontally, others curled."""
|
||||
if not _finger_up(lm, _INDEX_TIP, _INDEX_PIP):
|
||||
return None
|
||||
others_up = sum([
|
||||
_finger_up(lm, _MIDDLE_TIP, _MIDDLE_PIP),
|
||||
_finger_up(lm, _RING_TIP, _RING_PIP),
|
||||
_finger_up(lm, _PINKY_TIP, _PINKY_PIP),
|
||||
])
|
||||
if others_up >= 2:
|
||||
return None
|
||||
# Horizontal: |dx| > |dy| between index MCP and TIP
|
||||
dx = abs(lm[_INDEX_TIP].x - lm[_INDEX_MCP].x)
|
||||
dy = abs(lm[_INDEX_TIP].y - lm[_INDEX_MCP].y)
|
||||
if dx <= dy:
|
||||
return None
|
||||
conf = 0.65 + 0.25 * min(1.0, dx / max(dy, 0.001) / 3.0)
|
||||
direction = "right" if lm[_INDEX_TIP].x > lm[_INDEX_MCP].x else "left"
|
||||
return ClassifiedGesture(
|
||||
gesture_type="follow", confidence=round(conf, 3),
|
||||
hand_x=lm[_INDEX_TIP].x, hand_y=lm[_INDEX_TIP].y,
|
||||
direction=direction,
|
||||
)
|
||||
|
||||
|
||||
# ── Temporal wave detector ────────────────────────────────────────────────────
|
||||
|
||||
|
||||
class WaveDetector:
|
||||
"""Sliding-window wave gesture detector.
|
||||
|
||||
Tracks wrist X-coordinate history and detects lateral oscillation
|
||||
(minimum 2 direction reversals above amplitude threshold).
|
||||
|
||||
Args:
|
||||
history_len: Number of frames to keep (default 24 ≈ 0.8 s at 30 fps)
|
||||
min_reversals: Direction-reversal count required to trigger (default 2)
|
||||
min_amplitude: Minimum peak-to-peak x excursion in normalised coords
|
||||
"""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
history_len: int = 24,
|
||||
min_reversals: int = 2,
|
||||
min_amplitude: float = 0.08,
|
||||
) -> None:
|
||||
self._history: Deque[float] = deque(maxlen=history_len)
|
||||
self._min_reversals = min_reversals
|
||||
self._min_amplitude = min_amplitude
|
||||
|
||||
def push(self, wrist_x: float) -> Tuple[bool, float]:
|
||||
"""Add a new wrist-X sample.
|
||||
|
||||
Returns:
|
||||
(is_waving, confidence) — confidence is 0.0 when not waving.
|
||||
"""
|
||||
self._history.append(wrist_x)
|
||||
if len(self._history) < 6:
|
||||
return False, 0.0
|
||||
return self._detect()
|
||||
|
||||
def reset(self) -> None:
|
||||
self._history.clear()
|
||||
|
||||
def _detect(self) -> Tuple[bool, float]:
|
||||
samples = list(self._history)
|
||||
mean_x = sum(samples) / len(samples)
|
||||
centered = [x - mean_x for x in samples]
|
||||
amplitude = max(centered) - min(centered)
|
||||
if amplitude < self._min_amplitude:
|
||||
return False, 0.0
|
||||
|
||||
# Count sign changes (direction reversals)
|
||||
reversals = 0
|
||||
for i in range(1, len(centered)):
|
||||
if centered[i - 1] * centered[i] < 0:
|
||||
reversals += 1
|
||||
|
||||
if reversals < self._min_reversals:
|
||||
return False, 0.0
|
||||
|
||||
# Confidence: blend amplitude and reversal quality
|
||||
amp_score = min(1.0, amplitude / 0.30)
|
||||
rev_score = min(1.0, reversals / 6.0)
|
||||
conf = round(0.5 * amp_score + 0.5 * rev_score, 3)
|
||||
return True, conf
|
||||
|
||||
|
||||
# ── Public hand-gesture API ───────────────────────────────────────────────────
|
||||
|
||||
|
||||
# Priority order for static hand-gesture classifiers
|
||||
_HAND_CLASSIFIERS = [
|
||||
("stop_palm", lambda lm, rh: _classify_stop_palm(lm)),
|
||||
("thumbs_up", lambda lm, rh: _classify_thumbs_up(lm, rh)),
|
||||
("thumbs_down", lambda lm, rh: _classify_thumbs_down(lm)),
|
||||
("point", lambda lm, rh: _classify_point(lm)),
|
||||
("come_here", lambda lm, rh: _classify_come_here(lm)),
|
||||
("follow", lambda lm, rh: _classify_follow(lm)),
|
||||
]
|
||||
|
||||
|
||||
def classify_hand(
|
||||
landmarks: List[Landmark],
|
||||
is_right_hand: bool = True,
|
||||
wave_det: Optional[WaveDetector] = None,
|
||||
enabled: Optional[set] = None,
|
||||
) -> Optional[ClassifiedGesture]:
|
||||
"""Classify a single hand's 21-landmark list into a named gesture.
|
||||
|
||||
Args:
|
||||
landmarks: 21 Landmark objects from MediaPipe Hands.
|
||||
is_right_hand: Handedness flag (affects thumb direction logic).
|
||||
wave_det: Optional WaveDetector instance for wave tracking.
|
||||
If provided, wave detection is attempted first.
|
||||
enabled: Set of gesture type strings to allow; None = all.
|
||||
|
||||
Returns:
|
||||
ClassifiedGesture or None if no gesture recognised.
|
||||
"""
|
||||
if len(landmarks) < 21:
|
||||
return None
|
||||
|
||||
def _allowed(name: str) -> bool:
|
||||
return enabled is None or name in enabled
|
||||
|
||||
# Wave check (temporal — must come before static classifiers)
|
||||
if wave_det is not None and _allowed("wave"):
|
||||
is_waving, wconf = wave_det.push(landmarks[_WRIST].x)
|
||||
if is_waving:
|
||||
cx, cy = _palm_center(landmarks)
|
||||
return ClassifiedGesture(
|
||||
gesture_type="wave", confidence=wconf,
|
||||
hand_x=cx, hand_y=cy,
|
||||
is_right_hand=is_right_hand,
|
||||
)
|
||||
|
||||
# Static classifiers — return the first (highest-priority) match
|
||||
for name, classifier in _HAND_CLASSIFIERS:
|
||||
if not _allowed(name):
|
||||
continue
|
||||
result = classifier(landmarks, is_right_hand)
|
||||
if result is not None:
|
||||
result.is_right_hand = is_right_hand
|
||||
return result
|
||||
|
||||
return None
|
||||
|
||||
|
||||
# ── Body-pose gesture classifiers ────────────────────────────────────────────
|
||||
|
||||
|
||||
def classify_pose(
|
||||
landmarks: List[Landmark],
|
||||
enabled: Optional[set] = None,
|
||||
) -> Optional[ClassifiedGesture]:
|
||||
"""Classify full-body pose landmarks (33 points) into a body gesture.
|
||||
|
||||
Args:
|
||||
landmarks: 33 Landmark objects from MediaPipe Pose.
|
||||
enabled: Set of allowed gesture types; None = all.
|
||||
|
||||
Returns:
|
||||
ClassifiedGesture or None.
|
||||
"""
|
||||
if len(landmarks) < 29: # need up to ankle (28)
|
||||
return None
|
||||
|
||||
def _ok(name: str) -> bool:
|
||||
return enabled is None or name in enabled
|
||||
|
||||
# ── arms_up: both wrists raised above their shoulders ─────────────────
|
||||
if _ok("arms_up"):
|
||||
l_above = landmarks[_L_WRIST].y < landmarks[_L_SHOULDER].y - 0.05
|
||||
r_above = landmarks[_R_WRIST].y < landmarks[_R_SHOULDER].y - 0.05
|
||||
if l_above and r_above:
|
||||
l_margin = landmarks[_L_SHOULDER].y - landmarks[_L_WRIST].y
|
||||
r_margin = landmarks[_R_SHOULDER].y - landmarks[_R_WRIST].y
|
||||
shoulder_span = abs(landmarks[_L_SHOULDER].y - landmarks[_R_HIP].y) or 0.1
|
||||
conf = min(0.95, 0.60 + 0.35 * min(1.0, (l_margin + r_margin) / (2 * shoulder_span)))
|
||||
mid_x = (landmarks[_L_WRIST].x + landmarks[_R_WRIST].x) / 2
|
||||
mid_y = (landmarks[_L_WRIST].y + landmarks[_R_WRIST].y) / 2
|
||||
return ClassifiedGesture(
|
||||
gesture_type="arms_up", confidence=round(conf, 3),
|
||||
hand_x=mid_x, hand_y=mid_y, source="body_pose",
|
||||
)
|
||||
|
||||
# ── arms_spread: both arms extended laterally ──────────────────────────
|
||||
if _ok("arms_spread"):
|
||||
l_span = abs(landmarks[_L_WRIST].x - landmarks[_L_SHOULDER].x)
|
||||
r_span = abs(landmarks[_R_WRIST].x - landmarks[_R_SHOULDER].x)
|
||||
# Wrists should be at roughly shoulder height
|
||||
l_level = abs(landmarks[_L_WRIST].y - landmarks[_L_SHOULDER].y) < 0.12
|
||||
r_level = abs(landmarks[_R_WRIST].y - landmarks[_R_SHOULDER].y) < 0.12
|
||||
shoulder_w = abs(landmarks[_L_SHOULDER].x - landmarks[_R_SHOULDER].x) or 0.1
|
||||
if l_level and r_level and l_span > shoulder_w * 0.6 and r_span > shoulder_w * 0.6:
|
||||
conf = min(0.90, 0.55 + 0.35 * min(1.0, (l_span + r_span) / (shoulder_w * 2)))
|
||||
mid_x = (landmarks[_L_WRIST].x + landmarks[_R_WRIST].x) / 2
|
||||
mid_y = (landmarks[_L_SHOULDER].y + landmarks[_R_SHOULDER].y) / 2
|
||||
return ClassifiedGesture(
|
||||
gesture_type="arms_spread", confidence=round(conf, 3),
|
||||
hand_x=mid_x, hand_y=mid_y, source="body_pose",
|
||||
)
|
||||
|
||||
# ── crouch: hips dropped below normal standing threshold ──────────────
|
||||
if _ok("crouch"):
|
||||
l_hip_y = landmarks[_L_HIP].y
|
||||
r_hip_y = landmarks[_R_HIP].y
|
||||
avg_hip_y = (l_hip_y + r_hip_y) / 2
|
||||
# In a crouching person the hips appear lower in frame (larger y).
|
||||
# We use 0.65 as threshold (hips in the lower 65% of frame height).
|
||||
if avg_hip_y > 0.65:
|
||||
# Additional check: knees should be significantly bent
|
||||
l_knee_y = landmarks[_L_KNEE].y if len(landmarks) > _L_KNEE else avg_hip_y
|
||||
r_knee_y = landmarks[_R_KNEE].y if len(landmarks) > _R_KNEE else avg_hip_y
|
||||
knee_drop = ((l_knee_y - l_hip_y) + (r_knee_y - r_hip_y)) / 2
|
||||
conf = min(0.90, 0.55 + 0.35 * min(1.0, (avg_hip_y - 0.65) / 0.20))
|
||||
mid_x = (landmarks[_L_HIP].x + landmarks[_R_HIP].x) / 2
|
||||
return ClassifiedGesture(
|
||||
gesture_type="crouch", confidence=round(conf, 3),
|
||||
hand_x=mid_x, hand_y=avg_hip_y, source="body_pose",
|
||||
)
|
||||
|
||||
return None
|
||||
@ -1,432 +0,0 @@
|
||||
"""gesture_node.py — Multi-camera hand + body-pose gesture recognition (Issue #140).
|
||||
|
||||
Subscribes to images from up to 4 CSI cameras, runs MediaPipe Hands and Pose
|
||||
on each frame, classifies gestures via gesture_classifier.py, and publishes
|
||||
/social/gestures (GestureArray).
|
||||
|
||||
Pipeline
|
||||
--------
|
||||
camera/*/image_raw → cv_bridge → MediaPipe Hands + Pose
|
||||
→ gesture_classifier → correlate with PersonStateArray
|
||||
→ publish /social/gestures
|
||||
|
||||
Multi-camera
|
||||
------------
|
||||
Up to n_cameras (default 4) camera streams are processed.
|
||||
Topic names follow the saltybot_cameras convention:
|
||||
/camera/front/image_raw, /camera/left/image_raw,
|
||||
/camera/rear/image_raw, /camera/right/image_raw
|
||||
|
||||
All cameras share a single round-robin processing queue so that GPU/CPU
|
||||
resources are not over-subscribed. At process_fps=10 Hz, each camera
|
||||
receives approximately process_fps / n_cameras updates per second.
|
||||
|
||||
Person-ID correlation
|
||||
---------------------
|
||||
When a gesture is detected on camera N, the node inspects the cached
|
||||
PersonStateArray. A person with matching camera_id is preferred; if only
|
||||
one person is tracked, they receive the gesture unconditionally.
|
||||
|
||||
ROS2 topics
|
||||
-----------
|
||||
Subscribe:
|
||||
/camera/front/image_raw (sensor_msgs/Image)
|
||||
/camera/left/image_raw (sensor_msgs/Image)
|
||||
/camera/rear/image_raw (sensor_msgs/Image)
|
||||
/camera/right/image_raw (sensor_msgs/Image)
|
||||
/social/persons (saltybot_social_msgs/PersonStateArray)
|
||||
|
||||
Publish:
|
||||
/social/gestures (saltybot_social_msgs/GestureArray)
|
||||
|
||||
Parameters
|
||||
----------
|
||||
n_cameras int 4 Number of active camera streams
|
||||
process_fps float 10.0 Target processing rate (frames/sec)
|
||||
min_confidence float 0.60 Discard detections below this threshold
|
||||
model_complexity int 0 MediaPipe model (0=lite, 1=full, 2=heavy)
|
||||
max_hands int 2 Max hands per frame (MediaPipe param)
|
||||
pose_enabled bool true Enable MediaPipe Pose body-pose detection
|
||||
hands_enabled bool true Enable MediaPipe Hands
|
||||
enabled_gestures str "" Comma-separated allowlist; empty = all
|
||||
camera_names str "front,left,rear,right" Camera name list
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import threading
|
||||
import time
|
||||
from typing import Dict, List, Optional, Set
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy
|
||||
|
||||
from sensor_msgs.msg import Image
|
||||
from saltybot_social_msgs.msg import (
|
||||
GestureArray, Gesture, PersonStateArray,
|
||||
)
|
||||
|
||||
from .gesture_classifier import (
|
||||
Landmark, ClassifiedGesture,
|
||||
classify_hand, classify_pose,
|
||||
WaveDetector,
|
||||
)
|
||||
|
||||
# Optional imports — may not be present in CI / test environments
|
||||
try:
|
||||
import cv2
|
||||
from cv_bridge import CvBridge
|
||||
_HAS_CV = True
|
||||
except ImportError:
|
||||
_HAS_CV = False
|
||||
|
||||
try:
|
||||
import mediapipe as mp
|
||||
_HAS_MP = True
|
||||
except ImportError:
|
||||
_HAS_MP = False
|
||||
|
||||
|
||||
# ── Camera index map ──────────────────────────────────────────────────────────
|
||||
|
||||
_CAMERA_NAMES = ["front", "left", "rear", "right"]
|
||||
_CAMERA_ID: Dict[str, int] = {name: i for i, name in enumerate(_CAMERA_NAMES)}
|
||||
|
||||
|
||||
# ── Latest-frame buffer ───────────────────────────────────────────────────────
|
||||
|
||||
|
||||
class _FrameBuffer:
|
||||
"""Thread-safe single-slot buffer per camera — always keeps newest frame."""
|
||||
|
||||
def __init__(self) -> None:
|
||||
self._frames: Dict[int, object] = {} # camera_id → Image msg
|
||||
self._lock = threading.Lock()
|
||||
|
||||
def put(self, camera_id: int, msg: object) -> None:
|
||||
with self._lock:
|
||||
self._frames[camera_id] = msg
|
||||
|
||||
def drain(self) -> List[tuple]:
|
||||
with self._lock:
|
||||
result = list(self._frames.items())
|
||||
self._frames = {}
|
||||
return result
|
||||
|
||||
|
||||
# ── MediaPipe wrapper ─────────────────────────────────────────────────────────
|
||||
|
||||
|
||||
class _MediaPipeProcessor:
|
||||
"""Wraps MediaPipe Hands + Pose with lazy initialisation."""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
model_complexity: int,
|
||||
max_hands: int,
|
||||
hands_enabled: bool,
|
||||
pose_enabled: bool,
|
||||
) -> None:
|
||||
self._complexity = model_complexity
|
||||
self._max_hands = max_hands
|
||||
self._hands_enabled = hands_enabled
|
||||
self._pose_enabled = pose_enabled
|
||||
self._hands = None
|
||||
self._pose = None
|
||||
self._ready = False
|
||||
|
||||
def init(self) -> None:
|
||||
if not _HAS_MP:
|
||||
return
|
||||
mp_hands = mp.solutions.hands
|
||||
mp_pose = mp.solutions.pose
|
||||
if self._hands_enabled:
|
||||
self._hands = mp_hands.Hands(
|
||||
static_image_mode=False,
|
||||
max_num_hands=self._max_hands,
|
||||
min_detection_confidence=0.50,
|
||||
min_tracking_confidence=0.50,
|
||||
model_complexity=self._complexity,
|
||||
)
|
||||
if self._pose_enabled:
|
||||
self._pose = mp_pose.Pose(
|
||||
static_image_mode=False,
|
||||
min_detection_confidence=0.50,
|
||||
min_tracking_confidence=0.50,
|
||||
model_complexity=self._complexity,
|
||||
)
|
||||
self._ready = True
|
||||
|
||||
def process(self, bgr_image) -> tuple:
|
||||
"""Returns (hands_results, pose_results) from MediaPipe."""
|
||||
if not self._ready or not _HAS_MP:
|
||||
return None, None
|
||||
try:
|
||||
import numpy as np
|
||||
rgb = cv2.cvtColor(bgr_image, cv2.COLOR_BGR2RGB)
|
||||
rgb.flags.writeable = False
|
||||
h_res = self._hands.process(rgb) if self._hands else None
|
||||
p_res = self._pose.process(rgb) if self._pose else None
|
||||
return h_res, p_res
|
||||
except Exception:
|
||||
return None, None
|
||||
|
||||
def close(self) -> None:
|
||||
if self._hands:
|
||||
self._hands.close()
|
||||
if self._pose:
|
||||
self._pose.close()
|
||||
|
||||
|
||||
# ── Gesture node ──────────────────────────────────────────────────────────────
|
||||
|
||||
|
||||
class GestureNode(Node):
|
||||
"""Multi-camera gesture recognition node."""
|
||||
|
||||
def __init__(self) -> None:
|
||||
super().__init__("gesture_node")
|
||||
|
||||
# ── Parameters ──────────────────────────────────────────────────────
|
||||
self.declare_parameter("n_cameras", 4)
|
||||
self.declare_parameter("process_fps", 10.0)
|
||||
self.declare_parameter("min_confidence", 0.60)
|
||||
self.declare_parameter("model_complexity", 0)
|
||||
self.declare_parameter("max_hands", 2)
|
||||
self.declare_parameter("pose_enabled", True)
|
||||
self.declare_parameter("hands_enabled", True)
|
||||
self.declare_parameter("enabled_gestures", "")
|
||||
self.declare_parameter("camera_names", "front,left,rear,right")
|
||||
|
||||
self._n_cameras: int = self.get_parameter("n_cameras").value
|
||||
self._process_fps: float = self.get_parameter("process_fps").value
|
||||
self._min_conf: float = self.get_parameter("min_confidence").value
|
||||
self._complexity: int = self.get_parameter("model_complexity").value
|
||||
self._max_hands: int = self.get_parameter("max_hands").value
|
||||
self._pose_enabled: bool = self.get_parameter("pose_enabled").value
|
||||
self._hands_enabled: bool = self.get_parameter("hands_enabled").value
|
||||
_enabled_str: str = self.get_parameter("enabled_gestures").value
|
||||
camera_names_str: str = self.get_parameter("camera_names").value
|
||||
|
||||
self._enabled: Optional[Set[str]] = (
|
||||
set(g.strip() for g in _enabled_str.split(",") if g.strip())
|
||||
if _enabled_str.strip() else None
|
||||
)
|
||||
self._camera_names: List[str] = [
|
||||
n.strip() for n in camera_names_str.split(",")
|
||||
if n.strip()
|
||||
][:self._n_cameras]
|
||||
|
||||
# ── QoS ─────────────────────────────────────────────────────────────
|
||||
img_qos = QoSProfile(
|
||||
reliability=QoSReliabilityPolicy.BEST_EFFORT,
|
||||
history=QoSHistoryPolicy.KEEP_LAST,
|
||||
depth=1,
|
||||
)
|
||||
reliable_qos = QoSProfile(depth=10)
|
||||
|
||||
# ── Publishers ───────────────────────────────────────────────────────
|
||||
self._gesture_pub = self.create_publisher(
|
||||
GestureArray, "/social/gestures", reliable_qos
|
||||
)
|
||||
|
||||
# ── Subscribers ──────────────────────────────────────────────────────
|
||||
self._buf = _FrameBuffer()
|
||||
self._cam_subs = []
|
||||
for name in self._camera_names:
|
||||
topic = f"/camera/{name}/image_raw"
|
||||
cam_id = _CAMERA_ID.get(name, len(self._cam_subs))
|
||||
sub = self.create_subscription(
|
||||
Image, topic,
|
||||
lambda msg, cid=cam_id: self._buf.put(cid, msg),
|
||||
img_qos,
|
||||
)
|
||||
self._cam_subs.append(sub)
|
||||
self.get_logger().info(f"Subscribed to {topic} (camera_id={cam_id})")
|
||||
|
||||
self._persons: Optional[PersonStateArray] = None
|
||||
self._persons_sub = self.create_subscription(
|
||||
PersonStateArray, "/social/persons",
|
||||
lambda msg: setattr(self, "_persons", msg),
|
||||
reliable_qos,
|
||||
)
|
||||
|
||||
# ── Per-camera WaveDetector instances: (camera_id, hand_idx) → det ──
|
||||
self._wave_detectors: Dict[tuple, WaveDetector] = {}
|
||||
|
||||
# ── MediaPipe (initialised in background thread) ─────────────────────
|
||||
self._mp = _MediaPipeProcessor(
|
||||
model_complexity=self._complexity,
|
||||
max_hands=self._max_hands,
|
||||
hands_enabled=self._hands_enabled,
|
||||
pose_enabled=self._pose_enabled,
|
||||
)
|
||||
self._bridge = CvBridge() if _HAS_CV else None
|
||||
self._mp_ready = False
|
||||
|
||||
self._init_thread = threading.Thread(
|
||||
target=self._init_mediapipe, daemon=True
|
||||
)
|
||||
self._init_thread.start()
|
||||
|
||||
# ── Processing timer ─────────────────────────────────────────────────
|
||||
self._timer = self.create_timer(
|
||||
1.0 / self._process_fps, self._process_tick
|
||||
)
|
||||
|
||||
self.get_logger().info(
|
||||
f"gesture_node starting — cameras={self._camera_names}, "
|
||||
f"fps={self._process_fps}, min_conf={self._min_conf}, "
|
||||
f"complexity={self._complexity}"
|
||||
)
|
||||
|
||||
# ── Initialisation ────────────────────────────────────────────────────────
|
||||
|
||||
def _init_mediapipe(self) -> None:
|
||||
if not _HAS_MP:
|
||||
self.get_logger().warn(
|
||||
"mediapipe not installed — gesture_node will publish no gestures. "
|
||||
"Install with: pip install mediapipe"
|
||||
)
|
||||
return
|
||||
if not _HAS_CV:
|
||||
self.get_logger().error("opencv-python / cv_bridge not available")
|
||||
return
|
||||
self.get_logger().info("Initialising MediaPipe Hands + Pose…")
|
||||
t0 = time.time()
|
||||
self._mp.init()
|
||||
self._mp_ready = True
|
||||
self.get_logger().info(
|
||||
f"MediaPipe ready ({time.time() - t0:.1f}s) — "
|
||||
f"hands={'on' if self._hands_enabled else 'off'}, "
|
||||
f"pose={'on' if self._pose_enabled else 'off'}"
|
||||
)
|
||||
|
||||
# ── Processing tick ───────────────────────────────────────────────────────
|
||||
|
||||
def _process_tick(self) -> None:
|
||||
if not self._mp_ready:
|
||||
return
|
||||
frames = self._buf.drain()
|
||||
if not frames:
|
||||
return
|
||||
|
||||
all_gestures: List[Gesture] = []
|
||||
for cam_id, img_msg in frames:
|
||||
gestures = self._process_frame(cam_id, img_msg)
|
||||
all_gestures.extend(gestures)
|
||||
|
||||
if all_gestures:
|
||||
arr = GestureArray()
|
||||
arr.header.stamp = self.get_clock().now().to_msg()
|
||||
arr.gestures = all_gestures
|
||||
self._gesture_pub.publish(arr)
|
||||
|
||||
def _process_frame(self, cam_id: int, img_msg) -> List[Gesture]:
|
||||
"""Run MediaPipe on one camera frame and return gesture messages."""
|
||||
if self._bridge is None:
|
||||
return []
|
||||
try:
|
||||
bgr = self._bridge.imgmsg_to_cv2(img_msg, desired_encoding="bgr8")
|
||||
except Exception as e:
|
||||
self.get_logger().debug(f"cv_bridge error cam {cam_id}: {e}")
|
||||
return []
|
||||
|
||||
h_results, p_results = self._mp.process(bgr)
|
||||
gestures: List[Gesture] = []
|
||||
|
||||
# ── Hand gestures ─────────────────────────────────────────────────
|
||||
if h_results and h_results.multi_hand_landmarks:
|
||||
for hand_idx, (hand_lm, hand_info) in enumerate(
|
||||
zip(h_results.multi_hand_landmarks,
|
||||
h_results.multi_handedness)
|
||||
):
|
||||
is_right = hand_info.classification[0].label == "Right"
|
||||
lm = [
|
||||
Landmark(p.x, p.y, p.z)
|
||||
for p in hand_lm.landmark
|
||||
]
|
||||
wave_key = (cam_id, hand_idx)
|
||||
wave_det = self._wave_detectors.get(wave_key)
|
||||
if wave_det is None:
|
||||
wave_det = WaveDetector()
|
||||
self._wave_detectors[wave_key] = wave_det
|
||||
|
||||
result = classify_hand(
|
||||
lm, is_right_hand=is_right,
|
||||
wave_det=wave_det,
|
||||
enabled=self._enabled,
|
||||
)
|
||||
if result and result.confidence >= self._min_conf:
|
||||
gestures.append(
|
||||
self._to_msg(result, cam_id)
|
||||
)
|
||||
|
||||
# ── Body pose gestures ────────────────────────────────────────────
|
||||
if p_results and p_results.pose_landmarks:
|
||||
lm = [
|
||||
Landmark(p.x, p.y, p.z, p.visibility)
|
||||
for p in p_results.pose_landmarks.landmark
|
||||
]
|
||||
result = classify_pose(lm, enabled=self._enabled)
|
||||
if result and result.confidence >= self._min_conf:
|
||||
gestures.append(self._to_msg(result, cam_id))
|
||||
|
||||
return gestures
|
||||
|
||||
# ── Person correlation ────────────────────────────────────────────────────
|
||||
|
||||
def _resolve_person_id(self, cam_id: int, hand_x: float) -> int:
|
||||
"""Find person_id for a gesture on camera cam_id. Returns -1 if unknown."""
|
||||
persons = self._persons
|
||||
if persons is None or not persons.persons:
|
||||
return -1
|
||||
# Filter persons seen on this camera
|
||||
on_cam = [p for p in persons.persons if p.camera_id == cam_id]
|
||||
if len(on_cam) == 1:
|
||||
return on_cam[0].person_id
|
||||
if len(on_cam) > 1:
|
||||
# Pick closest by horizontal image position (heuristic)
|
||||
return min(on_cam, key=lambda p: abs(p.bearing_deg / 90.0 - hand_x)).person_id
|
||||
# Fall back to any tracked person
|
||||
if persons.persons:
|
||||
return persons.persons[0].person_id
|
||||
return -1
|
||||
|
||||
# ── Message construction ──────────────────────────────────────────────────
|
||||
|
||||
def _to_msg(self, g: ClassifiedGesture, cam_id: int) -> Gesture:
|
||||
msg = Gesture()
|
||||
msg.header.stamp = self.get_clock().now().to_msg()
|
||||
msg.gesture_type = g.gesture_type
|
||||
msg.confidence = float(g.confidence)
|
||||
msg.camera_id = cam_id
|
||||
msg.hand_x = float(g.hand_x)
|
||||
msg.hand_y = float(g.hand_y)
|
||||
msg.is_right_hand = g.is_right_hand
|
||||
msg.direction = g.direction
|
||||
msg.source = g.source
|
||||
msg.person_id = self._resolve_person_id(cam_id, g.hand_x)
|
||||
return msg
|
||||
|
||||
# ── Cleanup ───────────────────────────────────────────────────────────────
|
||||
|
||||
def destroy_node(self) -> None:
|
||||
self._mp.close()
|
||||
super().destroy_node()
|
||||
|
||||
|
||||
# ── Entry point ───────────────────────────────────────────────────────────────
|
||||
|
||||
|
||||
def main(args=None) -> None:
|
||||
rclpy.init(args=args)
|
||||
node = GestureNode()
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
@ -35,8 +35,6 @@ setup(
|
||||
'orchestrator_node = saltybot_social.orchestrator_node:main',
|
||||
# Voice command NLU bridge (Issue #137)
|
||||
'voice_command_node = saltybot_social.voice_command_node:main',
|
||||
# Multi-camera gesture recognition (Issue #140)
|
||||
'gesture_node = saltybot_social.gesture_node:main',
|
||||
],
|
||||
},
|
||||
)
|
||||
|
||||
@ -1,640 +0,0 @@
|
||||
"""test_gesture_classifier.py — Unit tests for gesture_classifier (Issue #140).
|
||||
|
||||
Tests cover:
|
||||
- All 7 hand gestures with multiple landmark configurations
|
||||
- All 3 body-pose gestures
|
||||
- WaveDetector temporal detection
|
||||
- Entity extraction (direction for point/follow)
|
||||
- enabled allowlist filtering
|
||||
- Edge cases: too few landmarks, marginal values, near-misses
|
||||
|
||||
No ROS2 or MediaPipe runtime required.
|
||||
"""
|
||||
|
||||
import pytest
|
||||
from saltybot_social.gesture_classifier import (
|
||||
Landmark,
|
||||
ClassifiedGesture,
|
||||
WaveDetector,
|
||||
classify_hand,
|
||||
classify_pose,
|
||||
_finger_up,
|
||||
_thumb_up,
|
||||
_thumb_down,
|
||||
_four_fingers_curled,
|
||||
_count_fingers_up,
|
||||
_point_direction,
|
||||
)
|
||||
|
||||
|
||||
# ── Landmark builder helpers ──────────────────────────────────────────────────
|
||||
|
||||
|
||||
def _lm(x: float, y: float, z: float = 0.0) -> Landmark:
|
||||
return Landmark(x=x, y=y, z=z)
|
||||
|
||||
|
||||
def _make_hand_landmarks(
|
||||
fingers_up: list = None, # True/False per [index, middle, ring, pinky]
|
||||
thumb_up: bool = False,
|
||||
thumb_down: bool = False,
|
||||
wrist_y: float = 0.8,
|
||||
palm_height: float = 0.2,
|
||||
) -> list:
|
||||
"""
|
||||
Build a synthetic 21-landmark hand with controllable finger states.
|
||||
|
||||
fingers_up: [index, middle, ring, pinky] — True = finger extended
|
||||
thumb_up: thumb tip is above wrist
|
||||
thumb_down: thumb tip is below wrist
|
||||
wrist_y: y-coordinate of the wrist (image-space, 0=top, 1=bottom)
|
||||
palm_height: vertical extent of the palm
|
||||
"""
|
||||
if fingers_up is None:
|
||||
fingers_up = [False, False, False, False]
|
||||
|
||||
lm = [None] * 21
|
||||
|
||||
mcp_y = wrist_y - palm_height * 0.5 # MCP knuckles above wrist
|
||||
pip_y = mcp_y - palm_height * 0.2 # PIP above MCP
|
||||
tip_y_up = pip_y - palm_height * 0.3 # TIP above PIP (extended)
|
||||
tip_y_dn = pip_y + palm_height * 0.1 # TIP below PIP (curled)
|
||||
|
||||
# Wrist
|
||||
lm[0] = _lm(0.5, wrist_y)
|
||||
|
||||
# Thumb (CMC → MCP → IP → TIP)
|
||||
lm[1] = _lm(0.45, wrist_y - palm_height * 0.05) # CMC
|
||||
lm[2] = _lm(0.42, wrist_y - palm_height * 0.15) # MCP
|
||||
lm[3] = _lm(0.38, wrist_y - palm_height * 0.25) # IP
|
||||
if thumb_up:
|
||||
lm[4] = _lm(0.35, wrist_y - palm_height * 0.50) # TIP high up
|
||||
elif thumb_down:
|
||||
lm[4] = _lm(0.48, wrist_y + 0.06) # TIP below wrist
|
||||
else:
|
||||
lm[4] = _lm(0.40, wrist_y - palm_height * 0.10) # tucked
|
||||
|
||||
# Finger MCP, PIP, DIP, TIP — four fingers evenly spaced across x
|
||||
finger_x = [0.45, 0.50, 0.55, 0.60]
|
||||
finger_offsets = [ # (mcp, pip, dip, tip) indices
|
||||
(5, 6, 7, 8),
|
||||
(9, 10, 11, 12),
|
||||
(13, 14, 15, 16),
|
||||
(17, 18, 19, 20),
|
||||
]
|
||||
for fi, (mcp_i, pip_i, dip_i, tip_i) in enumerate(finger_offsets):
|
||||
x = finger_x[fi]
|
||||
lm[mcp_i] = _lm(x, mcp_y)
|
||||
lm[pip_i] = _lm(x, pip_y)
|
||||
lm[dip_i] = _lm(x, (pip_y + tip_y_up) / 2 if fingers_up[fi] else pip_y - 0.01)
|
||||
lm[tip_i] = _lm(x, tip_y_up if fingers_up[fi] else tip_y_dn)
|
||||
|
||||
return lm
|
||||
|
||||
|
||||
def _make_pose_landmarks(
|
||||
arms_up: bool = False,
|
||||
arms_spread: bool = False,
|
||||
crouching: bool = False,
|
||||
shoulder_y: float = 0.30,
|
||||
) -> list:
|
||||
"""Build synthetic 33-landmark body pose."""
|
||||
lm = [_lm(0.5, 0.5)] * 33
|
||||
|
||||
hip_y = 0.55 if not crouching else 0.75
|
||||
knee_y = hip_y + 0.15
|
||||
|
||||
# Shoulders
|
||||
lm[11] = _lm(0.35, shoulder_y) # L shoulder
|
||||
lm[12] = _lm(0.65, shoulder_y) # R shoulder
|
||||
|
||||
if arms_up:
|
||||
lm[15] = _lm(0.30, shoulder_y - 0.20) # L wrist above L shoulder
|
||||
lm[16] = _lm(0.70, shoulder_y - 0.20) # R wrist above R shoulder
|
||||
elif arms_spread:
|
||||
lm[13] = _lm(0.20, shoulder_y + 0.05) # L elbow out
|
||||
lm[14] = _lm(0.80, shoulder_y + 0.05) # R elbow out
|
||||
lm[15] = _lm(0.10, shoulder_y + 0.02) # L wrist far left
|
||||
lm[16] = _lm(0.90, shoulder_y + 0.02) # R wrist far right
|
||||
else:
|
||||
lm[15] = _lm(0.35, shoulder_y + 0.20) # L wrist at side
|
||||
lm[16] = _lm(0.65, shoulder_y + 0.20) # R wrist at side
|
||||
|
||||
lm[23] = _lm(0.40, hip_y) # L hip
|
||||
lm[24] = _lm(0.60, hip_y) # R hip
|
||||
lm[25] = _lm(0.40, knee_y) # L knee
|
||||
lm[26] = _lm(0.60, knee_y) # R knee
|
||||
lm[27] = _lm(0.40, knee_y + 0.15) # L ankle
|
||||
lm[28] = _lm(0.60, knee_y + 0.15) # R ankle
|
||||
|
||||
return lm
|
||||
|
||||
|
||||
# ── Helper tests ──────────────────────────────────────────────────────────────
|
||||
|
||||
|
||||
class TestFingerHelpers:
|
||||
def test_finger_up_when_tip_above_pip(self):
|
||||
lm = _make_hand_landmarks(fingers_up=[True, False, False, False])
|
||||
assert _finger_up(lm, 8, 6) is True # index tip above pip
|
||||
|
||||
def test_finger_down_when_tip_below_pip(self):
|
||||
lm = _make_hand_landmarks(fingers_up=[False, False, False, False])
|
||||
assert _finger_up(lm, 8, 6) is False
|
||||
|
||||
def test_count_fingers_all_up(self):
|
||||
lm = _make_hand_landmarks(fingers_up=[True, True, True, True])
|
||||
assert _count_fingers_up(lm) == 4
|
||||
|
||||
def test_count_fingers_none_up(self):
|
||||
lm = _make_hand_landmarks(fingers_up=[False, False, False, False])
|
||||
assert _count_fingers_up(lm) == 0
|
||||
|
||||
def test_count_fingers_two_up(self):
|
||||
lm = _make_hand_landmarks(fingers_up=[True, True, False, False])
|
||||
assert _count_fingers_up(lm) == 2
|
||||
|
||||
def test_four_fingers_curled_all_down(self):
|
||||
lm = _make_hand_landmarks(fingers_up=[False, False, False, False])
|
||||
assert _four_fingers_curled(lm) is True
|
||||
|
||||
def test_four_fingers_not_curled_one_up(self):
|
||||
lm = _make_hand_landmarks(fingers_up=[True, False, False, False])
|
||||
assert _four_fingers_curled(lm) is False
|
||||
|
||||
def test_thumb_up_when_tip_high(self):
|
||||
lm = _make_hand_landmarks(thumb_up=True)
|
||||
assert _thumb_up(lm, True) is True
|
||||
|
||||
def test_thumb_not_up_when_tucked(self):
|
||||
lm = _make_hand_landmarks(thumb_up=False)
|
||||
assert _thumb_up(lm, True) is False
|
||||
|
||||
def test_thumb_down_when_tip_below_wrist(self):
|
||||
lm = _make_hand_landmarks(thumb_down=True)
|
||||
assert _thumb_down(lm) is True
|
||||
|
||||
def test_thumb_not_down_when_tucked(self):
|
||||
lm = _make_hand_landmarks(thumb_up=False, thumb_down=False)
|
||||
assert _thumb_down(lm) is False
|
||||
|
||||
|
||||
# ── Hand gesture: stop_palm ───────────────────────────────────────────────────
|
||||
|
||||
|
||||
class TestStopPalm:
|
||||
def test_all_fingers_up(self):
|
||||
lm = _make_hand_landmarks(fingers_up=[True, True, True, True])
|
||||
r = classify_hand(lm)
|
||||
assert r is not None
|
||||
assert r.gesture_type == "stop_palm"
|
||||
|
||||
def test_confidence_range(self):
|
||||
lm = _make_hand_landmarks(fingers_up=[True, True, True, True])
|
||||
r = classify_hand(lm)
|
||||
assert 0.60 <= r.confidence <= 1.0
|
||||
|
||||
def test_three_fingers_does_not_trigger(self):
|
||||
lm = _make_hand_landmarks(fingers_up=[True, True, True, False])
|
||||
r = classify_hand(lm)
|
||||
assert r is None or r.gesture_type != "stop_palm"
|
||||
|
||||
def test_source_is_hand(self):
|
||||
lm = _make_hand_landmarks(fingers_up=[True, True, True, True])
|
||||
r = classify_hand(lm)
|
||||
assert r.source == "hand"
|
||||
|
||||
|
||||
# ── Hand gesture: thumbs_up ───────────────────────────────────────────────────
|
||||
|
||||
|
||||
class TestThumbsUp:
|
||||
def test_basic(self):
|
||||
lm = _make_hand_landmarks(
|
||||
fingers_up=[False, False, False, False], thumb_up=True
|
||||
)
|
||||
r = classify_hand(lm)
|
||||
assert r is not None
|
||||
assert r.gesture_type == "thumbs_up"
|
||||
|
||||
def test_confidence_range(self):
|
||||
lm = _make_hand_landmarks(
|
||||
fingers_up=[False, False, False, False], thumb_up=True
|
||||
)
|
||||
r = classify_hand(lm)
|
||||
assert 0.60 <= r.confidence <= 1.0
|
||||
|
||||
def test_thumb_up_with_fingers_open_not_thumbs_up(self):
|
||||
# If fingers are also open → stop_palm wins in priority
|
||||
lm = _make_hand_landmarks(
|
||||
fingers_up=[True, True, True, True], thumb_up=True
|
||||
)
|
||||
r = classify_hand(lm)
|
||||
assert r.gesture_type == "stop_palm"
|
||||
|
||||
def test_right_hand_flag(self):
|
||||
lm = _make_hand_landmarks(
|
||||
fingers_up=[False, False, False, False], thumb_up=True
|
||||
)
|
||||
r = classify_hand(lm, is_right_hand=True)
|
||||
assert r.is_right_hand is True
|
||||
|
||||
def test_left_hand_flag(self):
|
||||
lm = _make_hand_landmarks(
|
||||
fingers_up=[False, False, False, False], thumb_up=True
|
||||
)
|
||||
r = classify_hand(lm, is_right_hand=False)
|
||||
assert r.is_right_hand is False
|
||||
|
||||
|
||||
# ── Hand gesture: thumbs_down ─────────────────────────────────────────────────
|
||||
|
||||
|
||||
class TestThumbsDown:
|
||||
def test_basic(self):
|
||||
lm = _make_hand_landmarks(
|
||||
fingers_up=[False, False, False, False], thumb_down=True
|
||||
)
|
||||
r = classify_hand(lm)
|
||||
assert r is not None
|
||||
assert r.gesture_type == "thumbs_down"
|
||||
|
||||
def test_confidence_range(self):
|
||||
lm = _make_hand_landmarks(
|
||||
fingers_up=[False, False, False, False], thumb_down=True
|
||||
)
|
||||
r = classify_hand(lm)
|
||||
assert 0.60 <= r.confidence <= 1.0
|
||||
|
||||
def test_fingers_open_prevents_thumbs_down(self):
|
||||
lm = _make_hand_landmarks(
|
||||
fingers_up=[True, True, True, True], thumb_down=True
|
||||
)
|
||||
r = classify_hand(lm)
|
||||
# stop_palm takes priority
|
||||
assert r is None or r.gesture_type != "thumbs_down"
|
||||
|
||||
|
||||
# ── Hand gesture: point ───────────────────────────────────────────────────────
|
||||
|
||||
|
||||
class TestPoint:
|
||||
def test_index_only_up(self):
|
||||
lm = _make_hand_landmarks(fingers_up=[True, False, False, False])
|
||||
r = classify_hand(lm)
|
||||
assert r is not None
|
||||
assert r.gesture_type == "point"
|
||||
|
||||
def test_direction_right(self):
|
||||
"""Index tip to the right of MCP → direction='right'."""
|
||||
lm = _make_hand_landmarks(fingers_up=[True, False, False, False])
|
||||
# Override index tip to point right (TIP above PIP, dx=0.15 ≤ 0.20 threshold)
|
||||
lm[8] = _lm(lm[5].x + 0.15, lm[6].y - 0.02)
|
||||
r = classify_hand(lm)
|
||||
assert r is not None
|
||||
assert r.gesture_type == "point"
|
||||
assert r.direction == "right"
|
||||
|
||||
def test_direction_up(self):
|
||||
"""Index tip significantly above MCP → direction='up'."""
|
||||
lm = _make_hand_landmarks(fingers_up=[True, False, False, False])
|
||||
lm[8] = _lm(lm[5].x, lm[5].y - 0.20) # far above MCP
|
||||
r = classify_hand(lm)
|
||||
assert r is not None
|
||||
assert r.direction == "up"
|
||||
|
||||
def test_two_fingers_up_not_point(self):
|
||||
lm = _make_hand_landmarks(fingers_up=[True, True, False, False])
|
||||
r = classify_hand(lm)
|
||||
# With 2 fingers up, point classifier should reject
|
||||
assert r is None or r.gesture_type != "point"
|
||||
|
||||
def test_confidence_range(self):
|
||||
lm = _make_hand_landmarks(fingers_up=[True, False, False, False])
|
||||
r = classify_hand(lm)
|
||||
assert 0.60 <= r.confidence <= 1.0
|
||||
|
||||
|
||||
# ── Hand gesture: come_here ───────────────────────────────────────────────────
|
||||
|
||||
|
||||
class TestComeHere:
|
||||
def _make_beckoning_hand(self) -> list:
|
||||
"""Index MCP raised but TIP curled below PIP."""
|
||||
lm = _make_hand_landmarks(
|
||||
fingers_up=[False, False, False, False],
|
||||
wrist_y=0.85,
|
||||
palm_height=0.25,
|
||||
)
|
||||
# Index MCP should be above wrist
|
||||
assert lm[5].y < lm[0].y # sanity: MCP.y < WRIST.y (above in image)
|
||||
# Force TIP to be BELOW MCP (clearly curled — satisfies MCP+0.03 threshold)
|
||||
lm[8] = _lm(lm[5].x, lm[5].y + 0.06) # tip 0.06 below MCP
|
||||
return lm
|
||||
|
||||
def test_beckoning_detected(self):
|
||||
lm = self._make_beckoning_hand()
|
||||
r = classify_hand(lm)
|
||||
assert r is not None
|
||||
assert r.gesture_type == "come_here"
|
||||
|
||||
def test_confidence_range(self):
|
||||
lm = self._make_beckoning_hand()
|
||||
r = classify_hand(lm)
|
||||
assert 0.60 <= r.confidence <= 1.0
|
||||
|
||||
def test_open_hand_not_come_here(self):
|
||||
lm = _make_hand_landmarks(fingers_up=[True, True, True, True])
|
||||
r = classify_hand(lm)
|
||||
assert r is None or r.gesture_type != "come_here"
|
||||
|
||||
|
||||
# ── Hand gesture: follow ──────────────────────────────────────────────────────
|
||||
|
||||
|
||||
class TestFollow:
|
||||
def _make_follow_hand(self, direction: str = "right") -> list:
|
||||
"""Index pointing horizontally, others curled."""
|
||||
lm = _make_hand_landmarks(fingers_up=[True, False, False, False])
|
||||
# Move index TIP far to the right/left of MCP (dx=0.25 > 0.20 threshold → follow)
|
||||
# TIP above PIP so _finger_up returns True; dy=0.06 keeps dx >> dy
|
||||
if direction == "right":
|
||||
lm[8] = _lm(lm[5].x + 0.25, lm[6].y - 0.02)
|
||||
else:
|
||||
lm[8] = _lm(lm[5].x - 0.25, lm[6].y - 0.02)
|
||||
return lm
|
||||
|
||||
def test_follow_right(self):
|
||||
lm = self._make_follow_hand("right")
|
||||
r = classify_hand(lm)
|
||||
assert r is not None
|
||||
assert r.gesture_type == "follow"
|
||||
assert r.direction == "right"
|
||||
|
||||
def test_follow_left(self):
|
||||
lm = self._make_follow_hand("left")
|
||||
r = classify_hand(lm)
|
||||
assert r is not None
|
||||
assert r.gesture_type == "follow"
|
||||
assert r.direction == "left"
|
||||
|
||||
def test_vertical_point_not_follow(self):
|
||||
lm = _make_hand_landmarks(fingers_up=[True, False, False, False])
|
||||
# TIP straight up — dy >> dx
|
||||
lm[8] = _lm(lm[5].x, lm[5].y - 0.30)
|
||||
r = classify_hand(lm)
|
||||
# Should be "point", not "follow"
|
||||
assert r is None or r.gesture_type == "point"
|
||||
|
||||
def test_confidence_range(self):
|
||||
lm = self._make_follow_hand()
|
||||
r = classify_hand(lm)
|
||||
assert r is not None
|
||||
assert 0.60 <= r.confidence <= 1.0
|
||||
|
||||
|
||||
# ── WaveDetector ──────────────────────────────────────────────────────────────
|
||||
|
||||
|
||||
class TestWaveDetector:
|
||||
def test_not_waving_insufficient_data(self):
|
||||
det = WaveDetector()
|
||||
for x in [0.4, 0.5]:
|
||||
waving, conf = det.push(x)
|
||||
assert waving is False
|
||||
assert conf == 0.0
|
||||
|
||||
def test_wave_oscillation_detected(self):
|
||||
det = WaveDetector(history_len=20, min_reversals=2, min_amplitude=0.06)
|
||||
xs = [0.3, 0.5, 0.3, 0.5, 0.3, 0.5, 0.3, 0.5, 0.3, 0.5]
|
||||
for x in xs:
|
||||
waving, conf = det.push(x)
|
||||
assert waving is True
|
||||
assert conf > 0.0
|
||||
|
||||
def test_small_amplitude_not_wave(self):
|
||||
det = WaveDetector(min_amplitude=0.10)
|
||||
# Tiny oscillation — amplitude < threshold
|
||||
xs = [0.50, 0.51, 0.50, 0.51, 0.50, 0.51, 0.50, 0.51, 0.50, 0.51]
|
||||
for x in xs:
|
||||
waving, conf = det.push(x)
|
||||
assert waving is False
|
||||
|
||||
def test_confidence_increases_with_larger_amplitude(self):
|
||||
det_small = WaveDetector()
|
||||
det_large = WaveDetector()
|
||||
for x in [0.40, 0.60, 0.40, 0.60, 0.40, 0.60, 0.40, 0.60]:
|
||||
_, c_small = det_small.push(x)
|
||||
for x in [0.20, 0.80, 0.20, 0.80, 0.20, 0.80, 0.20, 0.80]:
|
||||
_, c_large = det_large.push(x)
|
||||
assert c_large >= c_small
|
||||
|
||||
def test_reset_clears_history(self):
|
||||
det = WaveDetector()
|
||||
for x in [0.3, 0.7, 0.3, 0.7, 0.3, 0.7, 0.3, 0.7, 0.3, 0.7]:
|
||||
det.push(x)
|
||||
det.reset()
|
||||
waving, conf = det.push(0.5)
|
||||
assert waving is False
|
||||
|
||||
def test_wave_via_classify_hand(self):
|
||||
"""Wave detector triggered through classify_hand API."""
|
||||
det = WaveDetector(min_reversals=2, min_amplitude=0.06)
|
||||
xs = [0.3, 0.6, 0.3, 0.6, 0.3, 0.6, 0.3, 0.6, 0.3, 0.6]
|
||||
result = None
|
||||
for x in xs:
|
||||
lm = _make_hand_landmarks()
|
||||
lm[0] = _lm(x, lm[0].y) # move wrist X
|
||||
result = classify_hand(lm, wave_det=det)
|
||||
assert result is not None
|
||||
assert result.gesture_type == "wave"
|
||||
|
||||
|
||||
# ── Body-pose: arms_up ────────────────────────────────────────────────────────
|
||||
|
||||
|
||||
class TestArmsUp:
|
||||
def test_both_arms_raised(self):
|
||||
lm = _make_pose_landmarks(arms_up=True)
|
||||
r = classify_pose(lm)
|
||||
assert r is not None
|
||||
assert r.gesture_type == "arms_up"
|
||||
|
||||
def test_confidence_range(self):
|
||||
lm = _make_pose_landmarks(arms_up=True)
|
||||
r = classify_pose(lm)
|
||||
assert 0.60 <= r.confidence <= 1.0
|
||||
|
||||
def test_arms_at_side_not_arms_up(self):
|
||||
lm = _make_pose_landmarks(arms_up=False, arms_spread=False)
|
||||
r = classify_pose(lm)
|
||||
assert r is None or r.gesture_type != "arms_up"
|
||||
|
||||
def test_source_is_body_pose(self):
|
||||
lm = _make_pose_landmarks(arms_up=True)
|
||||
r = classify_pose(lm)
|
||||
assert r.source == "body_pose"
|
||||
|
||||
|
||||
# ── Body-pose: arms_spread ────────────────────────────────────────────────────
|
||||
|
||||
|
||||
class TestArmsSpread:
|
||||
def test_arms_extended_laterally(self):
|
||||
lm = _make_pose_landmarks(arms_spread=True)
|
||||
r = classify_pose(lm)
|
||||
assert r is not None
|
||||
assert r.gesture_type in ("arms_spread", "arms_up")
|
||||
|
||||
def test_confidence_range(self):
|
||||
lm = _make_pose_landmarks(arms_spread=True)
|
||||
r = classify_pose(lm)
|
||||
if r and r.gesture_type == "arms_spread":
|
||||
assert 0.55 <= r.confidence <= 1.0
|
||||
|
||||
def test_arms_at_rest_not_spread(self):
|
||||
lm = _make_pose_landmarks()
|
||||
r = classify_pose(lm)
|
||||
assert r is None or r.gesture_type not in ("arms_spread", "arms_up")
|
||||
|
||||
|
||||
# ── Body-pose: crouch ─────────────────────────────────────────────────────────
|
||||
|
||||
|
||||
class TestCrouch:
|
||||
def test_crouching_detected(self):
|
||||
lm = _make_pose_landmarks(crouching=True)
|
||||
r = classify_pose(lm)
|
||||
assert r is not None
|
||||
assert r.gesture_type == "crouch"
|
||||
|
||||
def test_standing_not_crouch(self):
|
||||
lm = _make_pose_landmarks(crouching=False)
|
||||
r = classify_pose(lm)
|
||||
assert r is None or r.gesture_type != "crouch"
|
||||
|
||||
def test_confidence_range(self):
|
||||
lm = _make_pose_landmarks(crouching=True)
|
||||
r = classify_pose(lm)
|
||||
assert 0.55 <= r.confidence <= 1.0
|
||||
|
||||
def test_source_is_body_pose(self):
|
||||
lm = _make_pose_landmarks(crouching=True)
|
||||
r = classify_pose(lm)
|
||||
assert r.source == "body_pose"
|
||||
|
||||
|
||||
# ── Enabled allowlist filtering ───────────────────────────────────────────────
|
||||
|
||||
|
||||
class TestEnabledFilter:
|
||||
def test_blocked_gesture_returns_none(self):
|
||||
lm = _make_hand_landmarks(fingers_up=[True, True, True, True])
|
||||
r = classify_hand(lm, enabled={"thumbs_up"})
|
||||
assert r is None
|
||||
|
||||
def test_allowed_gesture_passes(self):
|
||||
lm = _make_hand_landmarks(fingers_up=[True, True, True, True])
|
||||
r = classify_hand(lm, enabled={"stop_palm"})
|
||||
assert r is not None
|
||||
assert r.gesture_type == "stop_palm"
|
||||
|
||||
def test_none_enabled_means_all(self):
|
||||
lm = _make_hand_landmarks(fingers_up=[True, True, True, True])
|
||||
r = classify_hand(lm, enabled=None)
|
||||
assert r is not None
|
||||
|
||||
def test_pose_filter_arms_up_blocked(self):
|
||||
lm = _make_pose_landmarks(arms_up=True)
|
||||
r = classify_pose(lm, enabled={"crouch"})
|
||||
assert r is None
|
||||
|
||||
def test_pose_filter_arms_up_allowed(self):
|
||||
lm = _make_pose_landmarks(arms_up=True)
|
||||
r = classify_pose(lm, enabled={"arms_up"})
|
||||
assert r is not None
|
||||
assert r.gesture_type == "arms_up"
|
||||
|
||||
|
||||
# ── Edge cases ────────────────────────────────────────────────────────────────
|
||||
|
||||
|
||||
class TestEdgeCases:
|
||||
def test_empty_landmarks_returns_none(self):
|
||||
assert classify_hand([]) is None
|
||||
|
||||
def test_too_few_landmarks_returns_none(self):
|
||||
lm = [_lm(0.5, 0.5)] * 10 # only 10, need 21
|
||||
assert classify_hand(lm) is None
|
||||
|
||||
def test_too_few_pose_landmarks_returns_none(self):
|
||||
lm = [_lm(0.5, 0.5)] * 10
|
||||
assert classify_pose(lm) is None
|
||||
|
||||
def test_no_wave_detector_no_wave(self):
|
||||
"""Without a WaveDetector, wave is never returned."""
|
||||
lm = _make_hand_landmarks()
|
||||
r = classify_hand(lm, wave_det=None)
|
||||
assert r is None or r.gesture_type != "wave"
|
||||
|
||||
def test_all_fingers_down_thumb_neutral_returns_none(self):
|
||||
lm = _make_hand_landmarks(
|
||||
fingers_up=[False, False, False, False],
|
||||
thumb_up=False, thumb_down=False,
|
||||
)
|
||||
r = classify_hand(lm)
|
||||
assert r is None
|
||||
|
||||
def test_hand_x_y_populated(self):
|
||||
lm = _make_hand_landmarks(fingers_up=[True, True, True, True])
|
||||
r = classify_hand(lm)
|
||||
assert r is not None
|
||||
assert 0.0 <= r.hand_x <= 1.0
|
||||
assert 0.0 <= r.hand_y <= 1.0
|
||||
|
||||
def test_is_right_hand_default_true(self):
|
||||
lm = _make_hand_landmarks(
|
||||
fingers_up=[False, False, False, False], thumb_up=True
|
||||
)
|
||||
r = classify_hand(lm)
|
||||
assert r.is_right_hand is True
|
||||
|
||||
def test_wave_confidence_zero_for_no_wave(self):
|
||||
det = WaveDetector()
|
||||
# Stationary wrist — no wave
|
||||
for _ in range(24):
|
||||
waving, conf = det.push(0.5)
|
||||
assert waving is False
|
||||
assert conf == 0.0
|
||||
|
||||
|
||||
# ── Point direction edge cases ────────────────────────────────────────────────
|
||||
|
||||
|
||||
class TestPointDirection:
|
||||
def _hand_pointing(self, dx: float, dy: float) -> list:
|
||||
lm = _make_hand_landmarks(fingers_up=[True, False, False, False])
|
||||
# Set index MCP and TIP to produce the desired vector
|
||||
lm[5] = _lm(0.5, 0.5)
|
||||
lm[8] = _lm(0.5 + dx, 0.5 + dy)
|
||||
return lm
|
||||
|
||||
def test_pointing_right(self):
|
||||
lm = self._hand_pointing(dx=0.2, dy=0.0)
|
||||
assert _point_direction(lm) == "right"
|
||||
|
||||
def test_pointing_left(self):
|
||||
lm = self._hand_pointing(dx=-0.2, dy=0.0)
|
||||
assert _point_direction(lm) == "left"
|
||||
|
||||
def test_pointing_up(self):
|
||||
lm = self._hand_pointing(dx=0.0, dy=-0.2)
|
||||
assert _point_direction(lm) == "up"
|
||||
|
||||
def test_pointing_down(self):
|
||||
lm = self._hand_pointing(dx=0.0, dy=0.2)
|
||||
assert _point_direction(lm) == "down"
|
||||
|
||||
def test_pointing_upper_right(self):
|
||||
lm = self._hand_pointing(dx=0.15, dy=-0.15)
|
||||
assert _point_direction(lm) == "upper_right"
|
||||
@ -35,9 +35,6 @@ rosidl_generate_interfaces(${PROJECT_NAME}
|
||||
"msg/ConversationResponse.msg"
|
||||
# Issue #137 — voice command NLU
|
||||
"msg/VoiceCommand.msg"
|
||||
# Issue #140 — gesture recognition
|
||||
"msg/Gesture.msg"
|
||||
"msg/GestureArray.msg"
|
||||
DEPENDENCIES std_msgs geometry_msgs builtin_interfaces
|
||||
)
|
||||
|
||||
|
||||
@ -1,31 +0,0 @@
|
||||
# Gesture.msg — Single detected gesture from hand or body-pose recognition (Issue #140).
|
||||
# Published as part of GestureArray on /social/gestures.
|
||||
#
|
||||
# gesture_type values:
|
||||
# Hand gestures (MediaPipe Hands):
|
||||
# wave — lateral wrist oscillation (temporal)
|
||||
# point — index extended, others curled
|
||||
# stop_palm — all fingers extended, palm forward
|
||||
# thumbs_up — thumb up, fist closed
|
||||
# thumbs_down — thumb down, fist closed
|
||||
# come_here — beckoning: index curled toward palm (temporal)
|
||||
# follow — index extended horizontally
|
||||
# Body-pose gestures (MediaPipe Pose):
|
||||
# arms_up — both wrists above shoulders (stop / emergency)
|
||||
# crouch — hips below normal standing threshold (come closer)
|
||||
# arms_spread — arms extended laterally (spread out / stop)
|
||||
|
||||
std_msgs/Header header
|
||||
|
||||
string gesture_type # gesture identifier (see above)
|
||||
int32 person_id # from PersonStateArray (-1 = unidentified)
|
||||
float32 confidence # detection confidence [0.0, 1.0]
|
||||
int32 camera_id # source camera (0=front, 1=left, 2=rear, 3=right)
|
||||
|
||||
# Normalised image position of the detected gesture anchor [0.0, 1.0]
|
||||
float32 hand_x
|
||||
float32 hand_y
|
||||
|
||||
bool is_right_hand # true = right hand; false = left hand (hand gestures only)
|
||||
string direction # for "point": "left"/"right"/"up"/"forward"/"down"
|
||||
string source # "hand" or "body_pose"
|
||||
@ -1,6 +0,0 @@
|
||||
# GestureArray.msg — All gestures detected in a single processing tick (Issue #140).
|
||||
# Published by gesture_node on /social/gestures.
|
||||
|
||||
std_msgs/Header header
|
||||
|
||||
Gesture[] gestures
|
||||
@ -1,11 +0,0 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(saltybot_visual_odom)
|
||||
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(ament_cmake_python REQUIRED)
|
||||
|
||||
ament_python_install_package(${PROJECT_NAME})
|
||||
|
||||
install(DIRECTORY launch config DESTINATION share/${PROJECT_NAME}/)
|
||||
|
||||
ament_package()
|
||||
@ -1,45 +0,0 @@
|
||||
# saltybot_visual_odom — runtime parameters
|
||||
#
|
||||
# Visual odometry requires the D435i IR1 stereo pair and aligned depth:
|
||||
# /camera/infra1/image_rect_raw — left IR image (mono8, 30 Hz)
|
||||
# /camera/depth/image_rect_raw — depth in metres (float32, 30 Hz)
|
||||
# /camera/infra1/camera_info — intrinsics (latched)
|
||||
#
|
||||
# Enable infrared emitter in realsense2_camera launch:
|
||||
# enable_infra1: true
|
||||
# enable_depth: true
|
||||
# align_depth.enable: true
|
||||
|
||||
visual_odom:
|
||||
ros__parameters:
|
||||
max_features: 300 # max tracked feature points per frame
|
||||
min_features: 80 # re-detect when below this
|
||||
frame_id: 'odom'
|
||||
child_frame_id: 'camera_link'
|
||||
|
||||
odom_fusion:
|
||||
ros__parameters:
|
||||
fusion_hz: 30.0 # EKF update + TF publish rate
|
||||
wheel_odom_topic: '/saltybot/rover_odom' # or /saltybot/tank_odom
|
||||
loop_closure_thr_m: 0.30 # RTAB-Map jump threshold to trigger correction
|
||||
loop_closure_alpha: 0.40 # correction weight (0=ignore, 1=snap to RTAB-Map)
|
||||
slip_noise_multiplier: 10.0 # wheel odom noise × this factor during slip
|
||||
odom_frame: 'odom'
|
||||
base_frame: 'base_link'
|
||||
|
||||
|
||||
# ── RTAB-Map integration note ─────────────────────────────────────────────────
|
||||
# To feed the fused odometry to RTAB-Map instead of its internal VO, add to
|
||||
# slam_rtabmap.launch.py rtabmap_node remappings:
|
||||
#
|
||||
# remappings=[
|
||||
# ...
|
||||
# ('odom', '/saltybot/odom_fused'),
|
||||
# ]
|
||||
#
|
||||
# And in rtabmap_params.yaml set:
|
||||
# subscribe_odom: "true"
|
||||
# Odom/Strategy: "0" # F2M still used but initialised from fused pose
|
||||
#
|
||||
# This feeds RTAB-Map's relocalization with the EKF fused pose, reducing
|
||||
# loop-closure correction magnitude on startup after wheel slip events.
|
||||
@ -1,74 +0,0 @@
|
||||
"""
|
||||
visual_odom.launch.py — Visual odometry + Kalman fusion stack.
|
||||
|
||||
Starts:
|
||||
visual_odom — D435i IR stereo + CUDA optical flow VO → /saltybot/visual_odom
|
||||
odom_fusion — EKF: visual + wheel + slip failover + RTAB loop closure
|
||||
→ /saltybot/odom_fused + odom→base_link TF
|
||||
|
||||
Launch args:
|
||||
wheel_odom_topic str '/saltybot/rover_odom' (or /saltybot/tank_odom)
|
||||
max_features int '300'
|
||||
loop_closure_thr float '0.30'
|
||||
publish_debug bool 'false'
|
||||
|
||||
Verify:
|
||||
ros2 topic hz /saltybot/visual_odom # ~30 Hz
|
||||
ros2 topic hz /saltybot/odom_fused # ~30 Hz
|
||||
ros2 topic echo /saltybot/visual_odom_status
|
||||
ros2 run tf2_tools view_frames # verify odom→base_link TF
|
||||
|
||||
RTAB-Map integration:
|
||||
RTAB-Map's odom_frame / map_frame should match the odom_frame parameter.
|
||||
In slam_rtabmap.launch.py / rtabmap_params.yaml set:
|
||||
odom_frame_id: 'odom'
|
||||
RTAB-Map will auto-subscribe to /saltybot/odom_fused via the odom remapping
|
||||
or set subscribe_odom:=true with the odom topic pointing here.
|
||||
"""
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument
|
||||
from launch.conditions import IfCondition
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
args = [
|
||||
DeclareLaunchArgument('wheel_odom_topic', default_value='/saltybot/rover_odom'),
|
||||
DeclareLaunchArgument('max_features', default_value='300'),
|
||||
DeclareLaunchArgument('loop_closure_thr', default_value='0.30'),
|
||||
DeclareLaunchArgument('loop_closure_alpha', default_value='0.40'),
|
||||
DeclareLaunchArgument('fusion_hz', default_value='30.0'),
|
||||
]
|
||||
|
||||
visual_odom = Node(
|
||||
package='saltybot_visual_odom',
|
||||
executable='visual_odom',
|
||||
name='visual_odom',
|
||||
output='screen',
|
||||
parameters=[{
|
||||
'max_features': LaunchConfiguration('max_features'),
|
||||
'min_features': 80,
|
||||
'frame_id': 'odom',
|
||||
'child_frame_id': 'camera_link',
|
||||
}],
|
||||
)
|
||||
|
||||
odom_fusion = Node(
|
||||
package='saltybot_visual_odom',
|
||||
executable='odom_fusion',
|
||||
name='odom_fusion',
|
||||
output='screen',
|
||||
parameters=[{
|
||||
'fusion_hz': LaunchConfiguration('fusion_hz'),
|
||||
'wheel_odom_topic': LaunchConfiguration('wheel_odom_topic'),
|
||||
'loop_closure_thr_m': LaunchConfiguration('loop_closure_thr'),
|
||||
'loop_closure_alpha': LaunchConfiguration('loop_closure_alpha'),
|
||||
'slip_noise_multiplier': 10.0,
|
||||
'odom_frame': 'odom',
|
||||
'base_frame': 'base_link',
|
||||
}],
|
||||
)
|
||||
|
||||
return LaunchDescription(args + [visual_odom, odom_fusion])
|
||||
@ -1,41 +0,0 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>saltybot_visual_odom</name>
|
||||
<version>0.1.0</version>
|
||||
<description>
|
||||
Visual odometry (D435i stereo, CUDA optical flow) with Kalman-filter fusion
|
||||
of wheel odometry, auto-failover on wheel slip, and RTAB-Map loop-closure
|
||||
drift correction. Publishes /saltybot/visual_odom, /saltybot/odom_fused,
|
||||
and the odom→base_link TF.
|
||||
</description>
|
||||
<maintainer email="seb@vayrette.com">seb</maintainer>
|
||||
<license>MIT</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
<buildtool_depend>ament_cmake_python</buildtool_depend>
|
||||
|
||||
<depend>rclpy</depend>
|
||||
<depend>std_msgs</depend>
|
||||
<depend>sensor_msgs</depend>
|
||||
<depend>nav_msgs</depend>
|
||||
<depend>geometry_msgs</depend>
|
||||
<depend>tf2_ros</depend>
|
||||
<depend>tf2_geometry_msgs</depend>
|
||||
<depend>message_filters</depend>
|
||||
<depend>cv_bridge</depend>
|
||||
<depend>image_transport</depend>
|
||||
|
||||
<exec_depend>python3-numpy</exec_depend>
|
||||
<exec_depend>python3-opencv</exec_depend>
|
||||
<exec_depend>python3-scipy</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_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@ -1,210 +0,0 @@
|
||||
"""
|
||||
kalman_odom_filter.py — Extended Kalman Filter for odometry fusion.
|
||||
|
||||
Fuses wheel odometry and visual odometry into a single best-estimate
|
||||
ground-truth trajectory. Handles wheel-slip failover by down-weighting
|
||||
the wheel odometry measurement covariance when slip is detected.
|
||||
|
||||
State vector: x = [px, py, θ, v, ω]
|
||||
px, py — position in map/odom frame (m)
|
||||
θ — heading (rad), wrapped to [-π, π]
|
||||
v — linear velocity (m/s, forward)
|
||||
ω — angular velocity (rad/s)
|
||||
|
||||
Process model (unicycle, constant velocity over dt):
|
||||
px' = px + v·cos(θ)·dt
|
||||
py' = py + v·sin(θ)·dt
|
||||
θ' = θ + ω·dt
|
||||
v' = v (constant velocity prediction)
|
||||
ω' = ω (constant angular velocity prediction)
|
||||
|
||||
Measurement models:
|
||||
Wheel odom: z_w = [v_wheel, ω_wheel] (velocity measurements)
|
||||
Visual odom: z_v = [v_visual, ω_visual] (velocity measurements)
|
||||
RTAB-Map pose: z_r = [px_rtab, py_rtab, θ_rtab] (absolute pose correction)
|
||||
|
||||
Covariances:
|
||||
Process noise Q is fixed (low).
|
||||
Wheel measurement noise R_w increases 10× during slip.
|
||||
Visual measurement noise R_v increases 3× when VO returns invalid.
|
||||
|
||||
Units: SI throughout (m, rad, m/s, rad/s).
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
import math
|
||||
from typing import Tuple
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
class KalmanOdomFilter:
|
||||
"""
|
||||
5-state EKF for ground-robot odometry fusion.
|
||||
|
||||
Usage:
|
||||
kf = KalmanOdomFilter()
|
||||
kf.predict(dt)
|
||||
kf.update_wheel(v, omega) # wheel odom measurement
|
||||
kf.update_visual(v, omega) # visual odom measurement
|
||||
kf.update_rtabmap(px, py, theta) # absolute RTAB-Map correction
|
||||
px, py, theta = kf.pose
|
||||
"""
|
||||
|
||||
_N = 5 # state dimension
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
# Process noise (standard deviations)
|
||||
q_pos: float = 0.02, # m per step
|
||||
q_theta: float = 0.01, # rad per step
|
||||
q_vel: float = 0.10, # m/s per step
|
||||
q_omega: float = 0.05, # rad/s per step
|
||||
# Wheel odom noise (normal)
|
||||
r_wheel_v: float = 0.05, # m/s
|
||||
r_wheel_omega: float = 0.05, # rad/s
|
||||
# Visual odom noise
|
||||
r_visual_v: float = 0.08,
|
||||
r_visual_omega:float = 0.08,
|
||||
# RTAB-Map pose noise
|
||||
r_rtab_pos: float = 0.10, # m
|
||||
r_rtab_theta: float = 0.05, # rad
|
||||
# Slip multiplier for wheel noise
|
||||
slip_noise_multiplier: float = 10.0,
|
||||
):
|
||||
self._slip_mult = slip_noise_multiplier
|
||||
|
||||
# State estimate
|
||||
self._x = np.zeros(self._N, dtype=np.float64) # [px, py, theta, v, omega]
|
||||
|
||||
# Covariance
|
||||
self._P = np.eye(self._N, dtype=np.float64) * 0.01
|
||||
|
||||
# Process noise covariance Q
|
||||
self._Q = np.diag([q_pos**2, q_pos**2, q_theta**2, q_vel**2, q_omega**2])
|
||||
|
||||
# Measurement noise matrices
|
||||
self._R_wheel_normal = np.diag([r_wheel_v**2, r_wheel_omega**2])
|
||||
self._R_wheel_slip = np.diag([
|
||||
(r_wheel_v * slip_noise_multiplier)**2,
|
||||
(r_wheel_omega * slip_noise_multiplier)**2,
|
||||
])
|
||||
self._R_visual_valid = np.diag([r_visual_v**2, r_visual_omega**2])
|
||||
self._R_visual_invalid = np.diag([
|
||||
(r_visual_v * 3)**2,
|
||||
(r_visual_omega * 3)**2,
|
||||
])
|
||||
self._R_rtabmap = np.diag([r_rtab_pos**2, r_rtab_pos**2, r_rtab_theta**2])
|
||||
|
||||
# ── Public interface ──────────────────────────────────────────────────────
|
||||
|
||||
@property
|
||||
def pose(self) -> Tuple[float, float, float]:
|
||||
"""Return (x, y, theta) current best estimate."""
|
||||
return float(self._x[0]), float(self._x[1]), float(self._x[2])
|
||||
|
||||
@property
|
||||
def velocity(self) -> Tuple[float, float]:
|
||||
"""Return (v_linear, omega_angular) current estimate."""
|
||||
return float(self._x[3]), float(self._x[4])
|
||||
|
||||
@property
|
||||
def covariance_3x3(self) -> np.ndarray:
|
||||
"""Return 3×3 position+heading covariance (px, py, theta sub-block)."""
|
||||
return self._P[:3, :3].copy()
|
||||
|
||||
def predict(self, dt: float) -> None:
|
||||
"""Advance the state estimate by dt seconds (unicycle motion model)."""
|
||||
px, py, th, v, w = self._x
|
||||
c, s = math.cos(th), math.sin(th)
|
||||
|
||||
# State transition
|
||||
self._x[0] = px + v * c * dt
|
||||
self._x[1] = py + v * s * dt
|
||||
self._x[2] = self._wrap(th + w * dt)
|
||||
# v and omega unchanged (constant velocity prediction)
|
||||
|
||||
# Jacobian F of state transition
|
||||
F = np.eye(self._N, dtype=np.float64)
|
||||
F[0, 2] = -v * s * dt
|
||||
F[0, 3] = c * dt
|
||||
F[1, 2] = v * c * dt
|
||||
F[1, 3] = s * dt
|
||||
F[2, 4] = dt
|
||||
|
||||
# Covariance prediction
|
||||
self._P = F @ self._P @ F.T + self._Q
|
||||
|
||||
def update_wheel(self, v: float, omega: float, slipping: bool = False) -> None:
|
||||
"""
|
||||
Fuse wheel-odometry velocity measurement.
|
||||
|
||||
Args:
|
||||
v: linear velocity from encoders (m/s)
|
||||
omega: angular velocity from encoders (rad/s)
|
||||
slipping: if True, use high-noise covariance for wheel measurement
|
||||
"""
|
||||
R = self._R_wheel_slip if slipping else self._R_wheel_normal
|
||||
z = np.array([v, omega], dtype=np.float64)
|
||||
H = np.zeros((2, self._N), dtype=np.float64)
|
||||
H[0, 3] = 1.0 # measure v
|
||||
H[1, 4] = 1.0 # measure omega
|
||||
self._measurement_update(z, H, R)
|
||||
|
||||
def update_visual(self, v: float, omega: float, valid: bool = True) -> None:
|
||||
"""
|
||||
Fuse visual-odometry velocity measurement.
|
||||
|
||||
Args:
|
||||
v, omega: velocity from VO
|
||||
valid: False if VO returned invalid (low feature count etc.)
|
||||
"""
|
||||
R = self._R_visual_valid if valid else self._R_visual_invalid
|
||||
z = np.array([v, omega], dtype=np.float64)
|
||||
H = np.zeros((2, self._N), dtype=np.float64)
|
||||
H[0, 3] = 1.0
|
||||
H[1, 4] = 1.0
|
||||
self._measurement_update(z, H, R)
|
||||
|
||||
def update_rtabmap(self, px: float, py: float, theta: float) -> None:
|
||||
"""
|
||||
Fuse an absolute pose from RTAB-Map (loop closure correction).
|
||||
|
||||
Uses the full pose measurement [px, py, theta] against the current
|
||||
position state.
|
||||
"""
|
||||
z = np.array([px, py, theta], dtype=np.float64)
|
||||
H = np.zeros((3, self._N), dtype=np.float64)
|
||||
H[0, 0] = 1.0 # measure px
|
||||
H[1, 1] = 1.0 # measure py
|
||||
H[2, 2] = 1.0 # measure theta
|
||||
|
||||
# Innovation with angle wrapping
|
||||
innov = z - H @ self._x
|
||||
innov[2] = self._wrap(innov[2])
|
||||
|
||||
self._measurement_update_with_innov(innov, H, self._R_rtabmap)
|
||||
|
||||
# ── Internal EKF machinery ────────────────────────────────────────────────
|
||||
|
||||
def _measurement_update(
|
||||
self, z: np.ndarray, H: np.ndarray, R: np.ndarray
|
||||
) -> None:
|
||||
innov = z - H @ self._x
|
||||
self._measurement_update_with_innov(innov, H, R)
|
||||
|
||||
def _measurement_update_with_innov(
|
||||
self, innov: np.ndarray, H: np.ndarray, R: np.ndarray
|
||||
) -> None:
|
||||
S = H @ self._P @ H.T + R
|
||||
K = self._P @ H.T @ np.linalg.inv(S)
|
||||
self._x = self._x + K @ innov
|
||||
self._x[2] = self._wrap(self._x[2])
|
||||
self._P = (np.eye(self._N) - K @ H) @ self._P
|
||||
# Joseph form for numerical stability
|
||||
I_KH = np.eye(self._N) - K @ H
|
||||
self._P = I_KH @ self._P @ I_KH.T + K @ R @ K.T
|
||||
|
||||
@staticmethod
|
||||
def _wrap(angle: float) -> float:
|
||||
return (angle + math.pi) % (2 * math.pi) - math.pi
|
||||
@ -1,304 +0,0 @@
|
||||
"""
|
||||
odom_fusion_node.py — EKF fusion of wheel + visual odometry with slip failover.
|
||||
|
||||
Fusion strategy
|
||||
───────────────
|
||||
Normal operation (no slip):
|
||||
- Both wheel odom and visual odom update the EKF at their respective rates.
|
||||
- Final fused pose published at `fusion_hz` (default 30 Hz).
|
||||
|
||||
Wheel slip detected (from /saltybot/terrain):
|
||||
- Wheel odom measurement covariance inflated 10× → EKF trusts VO more.
|
||||
- Logs a warning at each slip event transition.
|
||||
|
||||
Visual odom unavailable (no features):
|
||||
- VO update skipped → EKF relies on wheel odom alone.
|
||||
- Wheel odom covariance stays at normal level (slip permitting).
|
||||
|
||||
RTAB-Map loop closure correction:
|
||||
- Subscribes to /rtabmap/odom (graph-optimized, updated after loop closures).
|
||||
- Detects a loop closure when RTAB-Map's pose jumps > loop_closure_thr_m from
|
||||
the EKF's current estimate.
|
||||
- Calls KalmanOdomFilter.update_rtabmap() to soft-correct accumulated drift.
|
||||
|
||||
TF publishing:
|
||||
- odom → base_link transform published at fusion_hz from the fused estimate.
|
||||
|
||||
Subscribes:
|
||||
/saltybot/visual_odom nav_msgs/Odometry visual odom velocities
|
||||
/saltybot/rover_odom nav_msgs/Odometry wheel encoder odom
|
||||
/saltybot/tank_odom nav_msgs/Odometry tank ESC odom (fallback)
|
||||
/saltybot/terrain std_msgs/String JSON incl. slip_ratio + is_slipping
|
||||
/rtabmap/odom nav_msgs/Odometry RTAB-Map graph-optimized pose
|
||||
|
||||
Publishes:
|
||||
/saltybot/odom_fused nav_msgs/Odometry EKF fused odometry
|
||||
/saltybot/visual_odom_status std_msgs/String JSON health + diagnostics
|
||||
|
||||
TF:
|
||||
odom → base_link
|
||||
|
||||
Parameters:
|
||||
fusion_hz float 30.0
|
||||
wheel_odom_topic str '/saltybot/rover_odom'
|
||||
loop_closure_thr_m float 0.30 (jump > this triggers RTAB correction)
|
||||
loop_closure_alpha float 0.40 (EKF correction weight, 0–1)
|
||||
slip_noise_multiplier float 10.0
|
||||
odom_frame str 'odom'
|
||||
base_frame str 'base_link'
|
||||
"""
|
||||
|
||||
import json
|
||||
import math
|
||||
import time
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
|
||||
import tf2_ros
|
||||
from nav_msgs.msg import Odometry
|
||||
from geometry_msgs.msg import (
|
||||
TransformStamped, Point, Quaternion,
|
||||
Twist, TwistWithCovariance, PoseWithCovariance,
|
||||
)
|
||||
from std_msgs.msg import String, Header
|
||||
|
||||
from .kalman_odom_filter import KalmanOdomFilter
|
||||
|
||||
|
||||
_RELIABLE_QOS = QoSProfile(
|
||||
reliability=ReliabilityPolicy.RELIABLE,
|
||||
history=HistoryPolicy.KEEP_LAST,
|
||||
depth=10,
|
||||
)
|
||||
_BEST_EFFORT_QOS = QoSProfile(
|
||||
reliability=ReliabilityPolicy.BEST_EFFORT,
|
||||
history=HistoryPolicy.KEEP_LAST,
|
||||
depth=5,
|
||||
)
|
||||
|
||||
|
||||
def _quat_to_yaw(q: Quaternion) -> float:
|
||||
siny = 2.0 * (q.w * q.z + q.x * q.y)
|
||||
cosy = 1.0 - 2.0 * (q.y * q.y + q.z * q.z)
|
||||
return math.atan2(siny, cosy)
|
||||
|
||||
|
||||
def _yaw_to_quat(yaw: float) -> Quaternion:
|
||||
q = Quaternion()
|
||||
q.w = math.cos(yaw * 0.5)
|
||||
q.z = math.sin(yaw * 0.5)
|
||||
return q
|
||||
|
||||
|
||||
class OdomFusionNode(Node):
|
||||
|
||||
def __init__(self):
|
||||
super().__init__('odom_fusion')
|
||||
|
||||
# ── Parameters ──────────────────────────────────────────────────────
|
||||
self.declare_parameter('fusion_hz', 30.0)
|
||||
self.declare_parameter('wheel_odom_topic', '/saltybot/rover_odom')
|
||||
self.declare_parameter('loop_closure_thr_m', 0.30)
|
||||
self.declare_parameter('loop_closure_alpha', 0.40)
|
||||
self.declare_parameter('slip_noise_multiplier', 10.0)
|
||||
self.declare_parameter('odom_frame', 'odom')
|
||||
self.declare_parameter('base_frame', 'base_link')
|
||||
|
||||
hz = self.get_parameter('fusion_hz').value
|
||||
wheel_topic = self.get_parameter('wheel_odom_topic').value
|
||||
self._lc_thr = self.get_parameter('loop_closure_thr_m').value
|
||||
self._lc_alpha = self.get_parameter('loop_closure_alpha').value
|
||||
slip_mult = self.get_parameter('slip_noise_multiplier').value
|
||||
self._odom_frame = self.get_parameter('odom_frame').value
|
||||
self._base_frame = self.get_parameter('base_frame').value
|
||||
|
||||
# ── EKF ─────────────────────────────────────────────────────────────
|
||||
self._kf = KalmanOdomFilter(slip_noise_multiplier=slip_mult)
|
||||
|
||||
# ── State ────────────────────────────────────────────────────────────
|
||||
self._slipping: bool = False
|
||||
self._slip_ratio: float = 0.0
|
||||
self._vo_valid: bool = False
|
||||
self._vo_vx: float = 0.0
|
||||
self._vo_omega: float = 0.0
|
||||
self._last_predict_t: float = time.monotonic()
|
||||
|
||||
# RTAB-Map tracking
|
||||
self._rtab_prev_x: float | None = None
|
||||
self._rtab_prev_y: float | None = None
|
||||
|
||||
# ── TF broadcaster ───────────────────────────────────────────────────
|
||||
self._tf_broadcaster = tf2_ros.TransformBroadcaster(self)
|
||||
|
||||
# ── Subscriptions ────────────────────────────────────────────────────
|
||||
self.create_subscription(
|
||||
Odometry, '/saltybot/visual_odom',
|
||||
self._on_visual_odom, _BEST_EFFORT_QOS
|
||||
)
|
||||
self.create_subscription(
|
||||
Odometry, wheel_topic,
|
||||
self._on_wheel_odom, _BEST_EFFORT_QOS
|
||||
)
|
||||
# Also listen to tank odom — active if rover odom goes silent
|
||||
if '/rover' in wheel_topic:
|
||||
self.create_subscription(
|
||||
Odometry, wheel_topic.replace('/rover_odom', '/tank_odom'),
|
||||
self._on_wheel_odom, _BEST_EFFORT_QOS
|
||||
)
|
||||
self.create_subscription(
|
||||
String, '/saltybot/terrain', self._on_terrain, _BEST_EFFORT_QOS
|
||||
)
|
||||
self.create_subscription(
|
||||
Odometry, '/rtabmap/odom', self._on_rtabmap_odom, _BEST_EFFORT_QOS
|
||||
)
|
||||
|
||||
# ── Publishers ───────────────────────────────────────────────────────
|
||||
self._fused_pub = self.create_publisher(Odometry, '/saltybot/odom_fused', 10)
|
||||
self._status_pub = self.create_publisher(String, '/saltybot/visual_odom_status', 10)
|
||||
|
||||
# ── Fusion timer ─────────────────────────────────────────────────────
|
||||
self.create_timer(1.0 / hz, self._fusion_tick)
|
||||
self.create_timer(1.0, self._status_tick)
|
||||
|
||||
self.get_logger().info(
|
||||
f'odom_fusion ready — wheel={wheel_topic} lc_thr={self._lc_thr}m'
|
||||
)
|
||||
|
||||
# ── Subscription callbacks ────────────────────────────────────────────────
|
||||
|
||||
def _on_visual_odom(self, msg: Odometry) -> None:
|
||||
self._vo_vx = msg.twist.twist.linear.x
|
||||
self._vo_omega = msg.twist.twist.angular.z
|
||||
# Detect VO validity from covariance (inflated when invalid)
|
||||
self._vo_valid = msg.twist.covariance[0] < 0.3
|
||||
# Update EKF immediately on VO callback
|
||||
self._kf.update_visual(self._vo_vx, self._vo_omega, valid=self._vo_valid)
|
||||
|
||||
def _on_wheel_odom(self, msg: Odometry) -> None:
|
||||
v = msg.twist.twist.linear.x
|
||||
omega = msg.twist.twist.angular.z
|
||||
self._kf.update_wheel(v, omega, slipping=self._slipping)
|
||||
|
||||
def _on_terrain(self, msg: String) -> None:
|
||||
try:
|
||||
data = json.loads(msg.data)
|
||||
except json.JSONDecodeError:
|
||||
return
|
||||
was_slipping = self._slipping
|
||||
self._slipping = bool(data.get('is_slipping', False))
|
||||
self._slip_ratio = float(data.get('slip_ratio', 0.0))
|
||||
if self._slipping and not was_slipping:
|
||||
self.get_logger().warning(
|
||||
f'[odom_fusion] Wheel slip detected (ratio={self._slip_ratio:.2f}) '
|
||||
f'— increasing VO weight'
|
||||
)
|
||||
elif not self._slipping and was_slipping:
|
||||
self.get_logger().info('[odom_fusion] Slip cleared — wheel odom re-weighted')
|
||||
|
||||
def _on_rtabmap_odom(self, msg: Odometry) -> None:
|
||||
rx = msg.pose.pose.position.x
|
||||
ry = msg.pose.pose.position.y
|
||||
ryaw = _quat_to_yaw(msg.pose.pose.orientation)
|
||||
|
||||
# Detect loop closure: RTAB-Map pose jumps significantly from our estimate
|
||||
if self._rtab_prev_x is not None:
|
||||
fused_x, fused_y, _ = self._kf.pose
|
||||
jump = math.hypot(rx - fused_x, ry - fused_y)
|
||||
if jump > self._lc_thr:
|
||||
self.get_logger().info(
|
||||
f'[odom_fusion] Loop closure: RTAB-Map jumped {jump:.3f}m — '
|
||||
f'applying drift correction (α={self._lc_alpha})'
|
||||
)
|
||||
self._kf.update_rtabmap(rx, ry, ryaw)
|
||||
|
||||
self._rtab_prev_x = rx
|
||||
self._rtab_prev_y = ry
|
||||
|
||||
# ── Fusion timer ─────────────────────────────────────────────────────────
|
||||
|
||||
def _fusion_tick(self) -> None:
|
||||
now_mono = time.monotonic()
|
||||
dt = now_mono - self._last_predict_t
|
||||
dt = max(1e-4, min(dt, 0.1))
|
||||
self._last_predict_t = now_mono
|
||||
|
||||
# EKF predict step
|
||||
self._kf.predict(dt)
|
||||
|
||||
# Publish fused odom
|
||||
now_stamp = self.get_clock().now().to_msg()
|
||||
px, py, th = self._kf.pose
|
||||
v, omega = self._kf.velocity
|
||||
|
||||
odom = Odometry()
|
||||
odom.header.stamp = now_stamp
|
||||
odom.header.frame_id = self._odom_frame
|
||||
odom.child_frame_id = self._base_frame
|
||||
|
||||
odom.pose.pose.position = Point(x=px, y=py, z=0.0)
|
||||
odom.pose.pose.orientation = _yaw_to_quat(th)
|
||||
|
||||
cov3 = self._kf.covariance_3x3
|
||||
p_cov = [0.0] * 36
|
||||
p_cov[0] = cov3[0, 0]
|
||||
p_cov[1] = cov3[0, 1]
|
||||
p_cov[5] = cov3[0, 2]
|
||||
p_cov[6] = cov3[1, 0]
|
||||
p_cov[7] = cov3[1, 1]
|
||||
p_cov[11] = cov3[1, 2]
|
||||
p_cov[30] = cov3[2, 0]
|
||||
p_cov[31] = cov3[2, 1]
|
||||
p_cov[35] = cov3[2, 2]
|
||||
odom.pose.covariance = p_cov
|
||||
|
||||
odom.twist.twist.linear.x = v
|
||||
odom.twist.twist.angular.z = omega
|
||||
t_cov = [0.0] * 36
|
||||
t_cov[0] = 0.05
|
||||
t_cov[35] = 0.05
|
||||
odom.twist.covariance = t_cov
|
||||
|
||||
self._fused_pub.publish(odom)
|
||||
|
||||
# Broadcast odom → base_link TF
|
||||
tf_msg = TransformStamped()
|
||||
tf_msg.header.stamp = now_stamp
|
||||
tf_msg.header.frame_id = self._odom_frame
|
||||
tf_msg.child_frame_id = self._base_frame
|
||||
tf_msg.transform.translation.x = px
|
||||
tf_msg.transform.translation.y = py
|
||||
tf_msg.transform.translation.z = 0.0
|
||||
tf_msg.transform.rotation = _yaw_to_quat(th)
|
||||
self._tf_broadcaster.sendTransform(tf_msg)
|
||||
|
||||
def _status_tick(self) -> None:
|
||||
px, py, th = self._kf.pose
|
||||
v, omega = self._kf.velocity
|
||||
cov3 = self._kf.covariance_3x3
|
||||
status = {
|
||||
'pose': {'x': round(px, 3), 'y': round(py, 3), 'yaw_deg': round(math.degrees(th), 1)},
|
||||
'velocity': {'v_mps': round(v, 3), 'omega_rads': round(omega, 3)},
|
||||
'vo_valid': self._vo_valid,
|
||||
'slipping': self._slipping,
|
||||
'slip_ratio': round(self._slip_ratio, 3),
|
||||
'pos_std_m': round(float(cov3[0, 0] ** 0.5), 4),
|
||||
'active_source': 'vo+wheel' if self._vo_valid else ('vo_degraded+wheel' if not self._slipping else 'vo_primary'),
|
||||
}
|
||||
msg = String()
|
||||
msg.data = json.dumps(status)
|
||||
self._status_pub.publish(msg)
|
||||
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
node = OdomFusionNode()
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@ -1,207 +0,0 @@
|
||||
"""
|
||||
optical_flow_tracker.py — Sparse optical flow feature tracker.
|
||||
|
||||
Primary path: OpenCV CUDA (SparsePyrLKOpticalFlow + FAST detector).
|
||||
Fallback: OpenCV CPU (calcOpticalFlowPyrLK + goodFeaturesToTrack).
|
||||
|
||||
Both paths output the same data: matched pixel pairs (prev_pts, curr_pts)
|
||||
suitable for Essential-matrix estimation.
|
||||
|
||||
GPU path targets 30 Hz on Jetson Orin Nano Super. The FAST detector on the
|
||||
Ampere GPU is ~4x faster than the CPU equivalent at the same quality level.
|
||||
|
||||
Design notes:
|
||||
- Features are re-detected whenever tracking drops below `min_features`.
|
||||
- Forward-backward consistency check discards unreliable tracks.
|
||||
- Points in the centre 80% of the image (avoids fisheye edge distortion).
|
||||
- Max `max_features` points per frame — higher = more robust, more cost.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
from typing import Tuple
|
||||
|
||||
import numpy as np
|
||||
import cv2
|
||||
|
||||
|
||||
_USE_CUDA = hasattr(cv2, 'cuda') and cv2.cuda.getCudaEnabledDeviceCount() > 0
|
||||
|
||||
# LK optical flow parameters
|
||||
_LK_WIN = (21, 21)
|
||||
_LK_ITERS = 30
|
||||
_LK_EPS = 0.01
|
||||
_LK_LEVELS = 4
|
||||
|
||||
# FAST detector threshold
|
||||
_FAST_THR = 20
|
||||
|
||||
|
||||
class OpticalFlowTracker:
|
||||
"""
|
||||
Maintains a set of tracked feature points across consecutive frames.
|
||||
|
||||
Usage:
|
||||
tracker = OpticalFlowTracker()
|
||||
prev_pts, curr_pts = tracker.update(prev_gray, curr_gray)
|
||||
# prev_pts / curr_pts: (N, 2) float32 arrays of matched pixel coords.
|
||||
"""
|
||||
|
||||
def __init__(self, max_features: int = 300, min_features: int = 80):
|
||||
self._max_features = max_features
|
||||
self._min_features = min_features
|
||||
self._prev_pts: np.ndarray | None = None
|
||||
self._prev_gray: np.ndarray | None = None
|
||||
self._use_cuda = _USE_CUDA
|
||||
|
||||
if self._use_cuda:
|
||||
self._lk = cv2.cuda.SparsePyrLKOpticalFlow_create(
|
||||
winSize=_LK_WIN,
|
||||
maxLevel=_LK_LEVELS,
|
||||
iters=_LK_ITERS,
|
||||
useInitialFlow=False,
|
||||
)
|
||||
self._fast = cv2.cuda.FastFeatureDetector_create(
|
||||
threshold=_FAST_THR, nonmaxSuppression=True
|
||||
)
|
||||
|
||||
def update(
|
||||
self,
|
||||
prev_gray: np.ndarray,
|
||||
curr_gray: np.ndarray,
|
||||
) -> Tuple[np.ndarray, np.ndarray]:
|
||||
"""
|
||||
Track features from prev_gray to curr_gray.
|
||||
|
||||
Returns:
|
||||
(prev_pts, curr_pts) — matched point pairs, shape (N, 2) float32.
|
||||
Returns empty arrays if tracking fails or there is no previous frame.
|
||||
"""
|
||||
h, w = curr_gray.shape
|
||||
|
||||
# Re-detect if we have too few features or no previous frame
|
||||
if self._prev_pts is None or len(self._prev_pts) < self._min_features:
|
||||
self._prev_pts = self._detect(prev_gray, h, w)
|
||||
self._prev_gray = prev_gray
|
||||
if self._prev_pts is None or len(self._prev_pts) < 8:
|
||||
return np.zeros((0, 2), np.float32), np.zeros((0, 2), np.float32)
|
||||
|
||||
# Track features
|
||||
try:
|
||||
if self._use_cuda:
|
||||
prev_pts_c, curr_pts, good = self._track_cuda(
|
||||
self._prev_gray, curr_gray, self._prev_pts
|
||||
)
|
||||
else:
|
||||
prev_pts_c, curr_pts, good = self._track_cpu(
|
||||
self._prev_gray, curr_gray, self._prev_pts
|
||||
)
|
||||
except Exception:
|
||||
self._prev_pts = None
|
||||
return np.zeros((0, 2), np.float32), np.zeros((0, 2), np.float32)
|
||||
|
||||
prev_pts_c = prev_pts_c[good]
|
||||
curr_pts = curr_pts[good]
|
||||
|
||||
# Update state
|
||||
self._prev_pts = curr_pts
|
||||
self._prev_gray = curr_gray
|
||||
|
||||
return prev_pts_c.reshape(-1, 2), curr_pts.reshape(-1, 2)
|
||||
|
||||
def reset(self) -> None:
|
||||
self._prev_pts = None
|
||||
self._prev_gray = None
|
||||
|
||||
# ── Detection ─────────────────────────────────────────────────────────────
|
||||
|
||||
def _detect(self, gray: np.ndarray, h: int, w: int) -> np.ndarray | None:
|
||||
"""Detect FAST features in the inner 80% of the image."""
|
||||
margin_x = int(w * 0.10)
|
||||
margin_y = int(h * 0.10)
|
||||
roi = gray[margin_y:h - margin_y, margin_x:w - margin_x]
|
||||
|
||||
if self._use_cuda:
|
||||
pts = self._detect_cuda(roi)
|
||||
else:
|
||||
pts = self._detect_cpu(roi)
|
||||
|
||||
if pts is None or len(pts) == 0:
|
||||
return None
|
||||
|
||||
# Offset back to full-image coordinates
|
||||
pts[:, 0] += margin_x
|
||||
pts[:, 1] += margin_y
|
||||
|
||||
# Subsample if too many
|
||||
if len(pts) > self._max_features:
|
||||
idx = np.random.choice(len(pts), self._max_features, replace=False)
|
||||
pts = pts[idx]
|
||||
|
||||
return pts.astype(np.float32)
|
||||
|
||||
def _detect_cuda(self, roi: np.ndarray) -> np.ndarray | None:
|
||||
gpu_img = cv2.cuda_GpuMat()
|
||||
gpu_img.upload(roi)
|
||||
kp = self._fast.detect(gpu_img, None)
|
||||
if not kp:
|
||||
return None
|
||||
pts = cv2.KeyPoint_convert(kp)
|
||||
return pts.reshape(-1, 2)
|
||||
|
||||
def _detect_cpu(self, roi: np.ndarray) -> np.ndarray | None:
|
||||
pts = cv2.goodFeaturesToTrack(
|
||||
roi,
|
||||
maxCorners=self._max_features,
|
||||
qualityLevel=0.01,
|
||||
minDistance=10,
|
||||
blockSize=7,
|
||||
)
|
||||
if pts is None:
|
||||
return None
|
||||
return pts.reshape(-1, 2)
|
||||
|
||||
# ── Tracking ──────────────────────────────────────────────────────────────
|
||||
|
||||
def _track_cuda(
|
||||
self, prev: np.ndarray, curr: np.ndarray, pts: np.ndarray
|
||||
) -> Tuple[np.ndarray, np.ndarray, np.ndarray]:
|
||||
pts_gpu = cv2.cuda_GpuMat(pts.reshape(-1, 1, 2))
|
||||
prev_gpu = cv2.cuda_GpuMat(prev)
|
||||
curr_gpu = cv2.cuda_GpuMat(curr)
|
||||
curr_pts_gpu, status_gpu, _ = self._lk.calc(prev_gpu, curr_gpu, pts_gpu, None)
|
||||
curr_pts = curr_pts_gpu.download().reshape(-1, 2)
|
||||
status = status_gpu.download().ravel()
|
||||
|
||||
# Forward-backward check
|
||||
bw_pts_gpu, bw_status_gpu, _ = self._lk.calc(curr_gpu, prev_gpu, curr_pts_gpu, None)
|
||||
bw_pts = bw_pts_gpu.download().reshape(-1, 2)
|
||||
bw_status = bw_status_gpu.download().ravel()
|
||||
|
||||
fb_err = np.linalg.norm(pts.reshape(-1, 2) - bw_pts, axis=1)
|
||||
good = (status > 0) & (bw_status > 0) & (fb_err < 1.0)
|
||||
return pts, curr_pts, good
|
||||
|
||||
def _track_cpu(
|
||||
self, prev: np.ndarray, curr: np.ndarray, pts: np.ndarray
|
||||
) -> Tuple[np.ndarray, np.ndarray, np.ndarray]:
|
||||
lk_params = dict(
|
||||
winSize=_LK_WIN,
|
||||
maxLevel=_LK_LEVELS,
|
||||
criteria=(cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT,
|
||||
_LK_ITERS, _LK_EPS),
|
||||
)
|
||||
curr_pts, st_fwd, _ = cv2.calcOpticalFlowPyrLK(
|
||||
prev, curr, pts.reshape(-1, 1, 2), None, **lk_params
|
||||
)
|
||||
bw_pts, st_bwd, _ = cv2.calcOpticalFlowPyrLK(
|
||||
curr, prev, curr_pts, None, **lk_params
|
||||
)
|
||||
fb_err = np.linalg.norm(
|
||||
pts.reshape(-1, 2) - bw_pts.reshape(-1, 2), axis=1
|
||||
)
|
||||
good = (
|
||||
(st_fwd.ravel() > 0) &
|
||||
(st_bwd.ravel() > 0) &
|
||||
(fb_err < 1.0)
|
||||
)
|
||||
return pts.reshape(-1, 2), curr_pts.reshape(-1, 2), good
|
||||
@ -1,252 +0,0 @@
|
||||
"""
|
||||
stereo_vo.py — Stereo visual odometry using Essential matrix decomposition.
|
||||
|
||||
Input per frame:
|
||||
- Tracked feature pairs (prev_pts, curr_pts) from OpticalFlowTracker
|
||||
- Depth image (float32, metres) aligned to the IR1/left camera
|
||||
- Camera intrinsic matrix K
|
||||
|
||||
Pipeline:
|
||||
1. Find Essential matrix E from point correspondences (5-point RANSAC).
|
||||
2. Decompose E → [R|t] (unit-vector translation).
|
||||
3. Recover metric scale from depth at tracked feature points.
|
||||
4. Compose the incremental SE(3) pose and integrate into global estimate.
|
||||
|
||||
Scale recovery:
|
||||
Median depth at both prev_pts and curr_pts yields consistent scale.
|
||||
Only depth samples in [0.3, 6.0m] range are used (D435i reliable range).
|
||||
|
||||
Output:
|
||||
StereoVO.update() returns a PoseEstimate dataclass with:
|
||||
x, y, z — position (m)
|
||||
roll, pitch, yaw — orientation (rad)
|
||||
vx, vz, omega — linear + angular velocity (m/s, rad/s) for current step
|
||||
valid — False if insufficient inliers or depth samples
|
||||
|
||||
Coordinate frame:
|
||||
x = forward, y = left, z = up (ROS REP-105 base_link convention).
|
||||
The camera looks forward along its +Z optical axis; we rotate into base_link.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
import math
|
||||
from dataclasses import dataclass, field
|
||||
from typing import Tuple
|
||||
|
||||
import numpy as np
|
||||
import cv2
|
||||
|
||||
|
||||
@dataclass
|
||||
class PoseEstimate:
|
||||
x: float = 0.0
|
||||
y: float = 0.0
|
||||
z: float = 0.0
|
||||
roll: float = 0.0
|
||||
pitch: float = 0.0
|
||||
yaw: float = 0.0
|
||||
vx: float = 0.0
|
||||
vy: float = 0.0
|
||||
omega: float = 0.0
|
||||
valid: bool = False
|
||||
|
||||
|
||||
# Depth validity window
|
||||
_D_MIN = 0.3
|
||||
_D_MAX = 6.0
|
||||
|
||||
# Essential matrix RANSAC
|
||||
_RANSAC_THRESHOLD = 1.0 # pixels
|
||||
_RANSAC_PROB = 0.999
|
||||
_MIN_INLIERS = 20
|
||||
|
||||
# Minimum depth samples for reliable scale
|
||||
_MIN_DEPTH_SAMPLES = 10
|
||||
|
||||
# Camera→base_link rotation (camera looks forward along +Z, base_link uses +X)
|
||||
# Rotation: camera_Z → base_X, camera_-Y → base_Z
|
||||
_R_CAM_TO_BASE = np.array([
|
||||
[ 0, 0, 1],
|
||||
[-1, 0, 0],
|
||||
[ 0, -1, 0],
|
||||
], dtype=np.float64)
|
||||
|
||||
|
||||
def _median_depth(depth: np.ndarray, pts: np.ndarray) -> float:
|
||||
"""Median depth at pixel locations pts (Nx2 float32). Returns 0 on failure."""
|
||||
h, w = depth.shape
|
||||
u = np.clip(pts[:, 0].astype(int), 0, w - 1)
|
||||
v = np.clip(pts[:, 1].astype(int), 0, h - 1)
|
||||
samples = depth[v, u]
|
||||
valid = samples[(samples > _D_MIN) & (samples < _D_MAX)]
|
||||
return float(np.median(valid)) if len(valid) >= _MIN_DEPTH_SAMPLES else 0.0
|
||||
|
||||
|
||||
def _rot_to_yaw(R: np.ndarray) -> float:
|
||||
"""Extract yaw from rotation matrix (Z-axis rotation in XY plane)."""
|
||||
return math.atan2(float(R[1, 0]), float(R[0, 0]))
|
||||
|
||||
|
||||
class StereoVO:
|
||||
"""
|
||||
Incremental visual odometry with depth-aided scale recovery.
|
||||
|
||||
Call update() once per frame pair. Thread-safe for single caller.
|
||||
"""
|
||||
|
||||
def __init__(self, K: np.ndarray | None = None, dt: float = 1.0 / 30.0):
|
||||
"""
|
||||
Args:
|
||||
K: 3×3 camera intrinsic matrix. Can be set later via set_K().
|
||||
dt: nominal frame interval (seconds) for velocity estimation.
|
||||
"""
|
||||
self._K = K
|
||||
self._dt = dt
|
||||
|
||||
# Accumulated global pose as 4×4 SE(3)
|
||||
self._pose = np.eye(4, dtype=np.float64)
|
||||
|
||||
# Previous-step pose for velocity estimation
|
||||
self._prev_pose = np.eye(4, dtype=np.float64)
|
||||
|
||||
self._frame_count = 0
|
||||
|
||||
def set_K(self, K: np.ndarray) -> None:
|
||||
self._K = K.reshape(3, 3).astype(np.float64)
|
||||
|
||||
def reset(self) -> None:
|
||||
self._pose = np.eye(4)
|
||||
self._prev_pose = np.eye(4)
|
||||
self._frame_count = 0
|
||||
|
||||
def update(
|
||||
self,
|
||||
prev_pts: np.ndarray, # (N, 2) float32
|
||||
curr_pts: np.ndarray, # (N, 2) float32
|
||||
depth: np.ndarray, # float32 H×W depth in metres
|
||||
dt: float | None = None,
|
||||
) -> PoseEstimate:
|
||||
"""
|
||||
Estimate incremental motion from matched feature pairs.
|
||||
|
||||
Returns a PoseEstimate. On failure returns a copy of the last valid
|
||||
estimate with valid=False.
|
||||
"""
|
||||
self._frame_count += 1
|
||||
dt = dt or self._dt
|
||||
|
||||
if self._K is None or len(prev_pts) < 8:
|
||||
return PoseEstimate(
|
||||
x=self._pose[0, 3], y=self._pose[1, 3],
|
||||
yaw=_rot_to_yaw(self._pose[:3, :3]), valid=False
|
||||
)
|
||||
|
||||
# ── Essential matrix via 5-point RANSAC ──────────────────────────────
|
||||
E, mask = cv2.findEssentialMat(
|
||||
prev_pts, curr_pts, self._K,
|
||||
method=cv2.RANSAC,
|
||||
prob=_RANSAC_PROB,
|
||||
threshold=_RANSAC_THRESHOLD,
|
||||
)
|
||||
if E is None or mask is None:
|
||||
return self._invalid_est()
|
||||
|
||||
inlier_mask = mask.ravel().astype(bool)
|
||||
n_inliers = int(inlier_mask.sum())
|
||||
if n_inliers < _MIN_INLIERS:
|
||||
return self._invalid_est()
|
||||
|
||||
p1_in = prev_pts[inlier_mask]
|
||||
p2_in = curr_pts[inlier_mask]
|
||||
|
||||
# ── Decompose E → R, t ────────────────────────────────────────────────
|
||||
_, R, t_unit, _ = cv2.recoverPose(E, p1_in, p2_in, self._K)
|
||||
# t_unit is a unit vector; we need metric scale
|
||||
|
||||
# ── Scale recovery from depth ─────────────────────────────────────────
|
||||
scale = _median_depth(depth, p1_in)
|
||||
if scale <= 0:
|
||||
# Fallback: try current frame depth
|
||||
scale = _median_depth(depth, p2_in)
|
||||
if scale <= 0:
|
||||
return self._invalid_est()
|
||||
|
||||
# Metric translation in camera frame
|
||||
t_metric = t_unit.ravel() * scale
|
||||
|
||||
# ── Build incremental SE(3) in camera frame ───────────────────────────
|
||||
T_inc_cam = np.eye(4, dtype=np.float64)
|
||||
T_inc_cam[:3, :3] = R
|
||||
T_inc_cam[:3, 3] = t_metric
|
||||
|
||||
# ── Rotate into base_link frame ───────────────────────────────────────
|
||||
R_cb = _R_CAM_TO_BASE
|
||||
R_inc_base = R_cb @ R @ R_cb.T
|
||||
t_inc_base = R_cb @ t_metric
|
||||
|
||||
T_inc = np.eye(4, dtype=np.float64)
|
||||
T_inc[:3, :3] = R_inc_base
|
||||
T_inc[:3, 3] = t_inc_base
|
||||
|
||||
# ── Accumulate global pose ────────────────────────────────────────────
|
||||
self._prev_pose = self._pose.copy()
|
||||
self._pose = self._pose @ T_inc
|
||||
|
||||
# ── Extract pose + velocity ───────────────────────────────────────────
|
||||
x = float(self._pose[0, 3])
|
||||
y = float(self._pose[1, 3])
|
||||
z = float(self._pose[2, 3])
|
||||
yaw = _rot_to_yaw(self._pose[:3, :3])
|
||||
|
||||
dx = float(self._pose[0, 3] - self._prev_pose[0, 3])
|
||||
dy = float(self._pose[1, 3] - self._prev_pose[1, 3])
|
||||
dyaw = yaw - _rot_to_yaw(self._prev_pose[:3, :3])
|
||||
# Wrap dyaw to [-π, π]
|
||||
dyaw = (dyaw + math.pi) % (2 * math.pi) - math.pi
|
||||
|
||||
vx = math.hypot(dx, dy) / max(dt, 1e-6)
|
||||
# Signed vx: positive if moving forward
|
||||
if abs(dx) + abs(dy) > 0:
|
||||
heading_now = yaw
|
||||
move_dir = math.atan2(dy, dx)
|
||||
if abs(move_dir - heading_now) > math.pi / 2:
|
||||
vx = -vx
|
||||
|
||||
omega = dyaw / max(dt, 1e-6)
|
||||
|
||||
return PoseEstimate(
|
||||
x=x, y=y, z=z, yaw=yaw,
|
||||
vx=vx, vy=0.0, omega=omega,
|
||||
valid=True,
|
||||
)
|
||||
|
||||
def correct_from_loop_closure(
|
||||
self,
|
||||
rtabmap_x: float,
|
||||
rtabmap_y: float,
|
||||
rtabmap_yaw: float,
|
||||
alpha: float = 0.3,
|
||||
) -> None:
|
||||
"""
|
||||
Soft-correct accumulated pose using RTAB-Map's graph-optimized position.
|
||||
|
||||
Called when RTAB-Map reports a pose significantly different from ours
|
||||
(indicating a loop closure correction). alpha controls how strongly
|
||||
we shift toward RTAB-Map's estimate (0=ignore, 1=fully accept).
|
||||
"""
|
||||
self._pose[0, 3] += alpha * (rtabmap_x - self._pose[0, 3])
|
||||
self._pose[1, 3] += alpha * (rtabmap_y - self._pose[1, 3])
|
||||
curr_yaw = _rot_to_yaw(self._pose[:3, :3])
|
||||
dyaw = rtabmap_yaw - curr_yaw
|
||||
dyaw = (dyaw + math.pi) % (2 * math.pi) - math.pi
|
||||
correction_yaw = curr_yaw + alpha * dyaw
|
||||
c, s = math.cos(correction_yaw), math.sin(correction_yaw)
|
||||
self._pose[:3, :3] = np.array([[c, -s, 0], [s, c, 0], [0, 0, 1]])
|
||||
|
||||
def _invalid_est(self) -> PoseEstimate:
|
||||
return PoseEstimate(
|
||||
x=self._pose[0, 3],
|
||||
y=self._pose[1, 3],
|
||||
yaw=_rot_to_yaw(self._pose[:3, :3]),
|
||||
valid=False,
|
||||
)
|
||||
@ -1,200 +0,0 @@
|
||||
"""
|
||||
visual_odom_node.py — D435i visual odometry at 30 Hz using CUDA optical flow.
|
||||
|
||||
Pipeline per frame:
|
||||
1. Receive synchronized IR1 (left infrared) + aligned depth images.
|
||||
2. Detect/track FAST features with CUDA SparsePyrLKOpticalFlow (GPU path)
|
||||
or CPU LK fallback.
|
||||
3. Estimate incremental SE(3) via Essential matrix + depth-aided scale.
|
||||
4. Publish accumulated pose as nav_msgs/Odometry on /saltybot/visual_odom.
|
||||
|
||||
CUDA usage:
|
||||
- cv2.cuda.FastFeatureDetector_create — GPU FAST corner detection
|
||||
- cv2.cuda.SparsePyrLKOpticalFlow — GPU Lucas-Kanade tracking
|
||||
Both are ~4× faster than CPU equivalents on the Orin Ampere GPU.
|
||||
The node auto-detects CUDA availability and falls back gracefully.
|
||||
|
||||
Subscribes:
|
||||
/camera/infra1/image_rect_raw sensor_msgs/Image 30 Hz (grey IR left)
|
||||
/camera/depth/image_rect_raw sensor_msgs/Image 30 Hz float32 depth (m)
|
||||
/camera/infra1/camera_info sensor_msgs/CameraInfo intrinsics
|
||||
|
||||
Publishes:
|
||||
/saltybot/visual_odom nav_msgs/Odometry 30 Hz
|
||||
|
||||
Parameters:
|
||||
max_features int 300
|
||||
min_features int 80
|
||||
publish_hz float 30.0 (max rate; actual limited by input)
|
||||
frame_id str 'odom'
|
||||
child_frame_id str 'camera_link'
|
||||
"""
|
||||
|
||||
import time
|
||||
import math
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy, DurabilityPolicy
|
||||
import message_filters
|
||||
import numpy as np
|
||||
import cv2
|
||||
from cv_bridge import CvBridge
|
||||
|
||||
from sensor_msgs.msg import Image, CameraInfo
|
||||
from nav_msgs.msg import Odometry
|
||||
from geometry_msgs.msg import (
|
||||
Pose, Point, Quaternion,
|
||||
Twist, Vector3, TwistWithCovariance, PoseWithCovariance,
|
||||
)
|
||||
from std_msgs.msg import Header
|
||||
|
||||
from .optical_flow_tracker import OpticalFlowTracker
|
||||
from .stereo_vo import StereoVO
|
||||
|
||||
|
||||
_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,
|
||||
)
|
||||
|
||||
|
||||
def _yaw_to_quat(yaw: float) -> Quaternion:
|
||||
q = Quaternion()
|
||||
q.w = math.cos(yaw * 0.5)
|
||||
q.z = math.sin(yaw * 0.5)
|
||||
return q
|
||||
|
||||
|
||||
class VisualOdomNode(Node):
|
||||
|
||||
def __init__(self):
|
||||
super().__init__('visual_odom')
|
||||
|
||||
self.declare_parameter('max_features', 300)
|
||||
self.declare_parameter('min_features', 80)
|
||||
self.declare_parameter('frame_id', 'odom')
|
||||
self.declare_parameter('child_frame_id', 'camera_link')
|
||||
|
||||
max_feat = self.get_parameter('max_features').value
|
||||
min_feat = self.get_parameter('min_features').value
|
||||
self._frame_id = self.get_parameter('frame_id').value
|
||||
self._child_frame_id = self.get_parameter('child_frame_id').value
|
||||
|
||||
self._bridge = CvBridge()
|
||||
self._tracker = OpticalFlowTracker(max_features=max_feat, min_features=min_feat)
|
||||
self._vo = StereoVO()
|
||||
self._last_t: float | None = None
|
||||
|
||||
# Synchronized IR1 + depth
|
||||
ir_sub = message_filters.Subscriber(
|
||||
self, Image, '/camera/infra1/image_rect_raw', qos_profile=_SENSOR_QOS
|
||||
)
|
||||
depth_sub = message_filters.Subscriber(
|
||||
self, Image, '/camera/depth/image_rect_raw', qos_profile=_SENSOR_QOS
|
||||
)
|
||||
self._sync = message_filters.ApproximateTimeSynchronizer(
|
||||
[ir_sub, depth_sub], queue_size=5, slop=0.033 # 1 frame at 30Hz
|
||||
)
|
||||
self._sync.registerCallback(self._on_frame)
|
||||
|
||||
self.create_subscription(
|
||||
CameraInfo, '/camera/infra1/camera_info', self._on_camera_info, _LATCHED_QOS
|
||||
)
|
||||
|
||||
self._odom_pub = self.create_publisher(Odometry, '/saltybot/visual_odom', 10)
|
||||
|
||||
cuda_ok = hasattr(cv2, 'cuda') and cv2.cuda.getCudaEnabledDeviceCount() > 0
|
||||
self.get_logger().info(
|
||||
f'visual_odom ready — CUDA={"YES" if cuda_ok else "NO (CPU fallback)"}'
|
||||
)
|
||||
|
||||
def _on_camera_info(self, msg: CameraInfo) -> None:
|
||||
K = np.array(msg.k, dtype=np.float64).reshape(3, 3)
|
||||
self._vo.set_K(K)
|
||||
|
||||
def _on_frame(self, ir_msg: Image, depth_msg: Image) -> None:
|
||||
now = self.get_clock().now()
|
||||
t_sec = now.nanoseconds * 1e-9
|
||||
dt = (t_sec - self._last_t) if self._last_t else (1.0 / 30.0)
|
||||
dt = max(1e-4, min(dt, 0.5)) # clamp to sane range
|
||||
self._last_t = t_sec
|
||||
|
||||
# Decode
|
||||
try:
|
||||
ir_img = self._bridge.imgmsg_to_cv2(ir_msg, desired_encoding='mono8')
|
||||
depth_img = self._bridge.imgmsg_to_cv2(depth_msg, desired_encoding='32FC1')
|
||||
except Exception as exc:
|
||||
self.get_logger().warning(f'frame decode error: {exc}')
|
||||
return
|
||||
|
||||
# Build grayscale for optical flow (IR is already mono8)
|
||||
gray = ir_img
|
||||
|
||||
# Need previous frame — get it from the tracker's internal state
|
||||
if self._tracker._prev_gray is None:
|
||||
# First frame: just initialise tracker state
|
||||
self._tracker._prev_pts = self._tracker._detect(
|
||||
gray, gray.shape[0], gray.shape[1]
|
||||
)
|
||||
self._tracker._prev_gray = gray
|
||||
return
|
||||
|
||||
prev_pts, curr_pts = self._tracker.update(self._tracker._prev_gray, gray)
|
||||
|
||||
est = self._vo.update(prev_pts, curr_pts, depth_img, dt=dt)
|
||||
|
||||
odom = self._build_odom_msg(est, now.to_msg())
|
||||
self._odom_pub.publish(odom)
|
||||
|
||||
def _build_odom_msg(self, est, stamp) -> Odometry:
|
||||
from .stereo_vo import PoseEstimate
|
||||
q = _yaw_to_quat(est.yaw)
|
||||
|
||||
odom = Odometry()
|
||||
odom.header.stamp = stamp
|
||||
odom.header.frame_id = self._frame_id
|
||||
odom.child_frame_id = self._child_frame_id
|
||||
|
||||
odom.pose.pose.position = Point(x=est.x, y=est.y, z=est.z)
|
||||
odom.pose.pose.orientation = q
|
||||
|
||||
# Covariance — inflate when VO is invalid
|
||||
cov_xy = 0.05 if est.valid else 0.5
|
||||
cov_theta = 0.02 if est.valid else 0.2
|
||||
cov_v = 0.1 if est.valid else 1.0
|
||||
p_cov = [0.0] * 36
|
||||
p_cov[0] = cov_xy # xx
|
||||
p_cov[7] = cov_xy # yy
|
||||
p_cov[35] = cov_theta # yaw-yaw
|
||||
odom.pose.covariance = p_cov
|
||||
|
||||
odom.twist.twist.linear.x = est.vx
|
||||
odom.twist.twist.angular.z = est.omega
|
||||
t_cov = [0.0] * 36
|
||||
t_cov[0] = cov_v
|
||||
t_cov[35] = cov_v * 0.5
|
||||
odom.twist.covariance = t_cov
|
||||
|
||||
return odom
|
||||
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
node = VisualOdomNode()
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@ -1,26 +0,0 @@
|
||||
from setuptools import setup, find_packages
|
||||
|
||||
package_name = 'saltybot_visual_odom'
|
||||
|
||||
setup(
|
||||
name=package_name,
|
||||
version='0.1.0',
|
||||
packages=find_packages(exclude=['test']),
|
||||
data_files=[
|
||||
('share/ament_index/resource_index/packages',
|
||||
[f'resource/{package_name}']),
|
||||
(f'share/{package_name}', ['package.xml']),
|
||||
],
|
||||
install_requires=['setuptools'],
|
||||
zip_safe=True,
|
||||
maintainer='seb',
|
||||
maintainer_email='seb@vayrette.com',
|
||||
description='Visual odometry + Kalman fusion for saltybot',
|
||||
license='MIT',
|
||||
entry_points={
|
||||
'console_scripts': [
|
||||
'visual_odom = saltybot_visual_odom.visual_odom_node:main',
|
||||
'odom_fusion = saltybot_visual_odom.odom_fusion_node:main',
|
||||
],
|
||||
},
|
||||
)
|
||||
@ -1,140 +0,0 @@
|
||||
"""
|
||||
test_kalman_odom.py — Unit tests for KalmanOdomFilter and StereoVO.
|
||||
|
||||
Runs without a ROS2 environment (no rclpy imports).
|
||||
"""
|
||||
|
||||
import math
|
||||
import sys
|
||||
import os
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
sys.path.insert(0, os.path.join(os.path.dirname(__file__), '..'))
|
||||
|
||||
from saltybot_visual_odom.kalman_odom_filter import KalmanOdomFilter
|
||||
from saltybot_visual_odom.stereo_vo import StereoVO, _rot_to_yaw
|
||||
|
||||
|
||||
# ── KalmanOdomFilter tests ────────────────────────────────────────────────────
|
||||
|
||||
class TestKalmanOdomFilter:
|
||||
|
||||
def test_initial_pose_zero(self):
|
||||
kf = KalmanOdomFilter()
|
||||
x, y, th = kf.pose
|
||||
assert x == pytest.approx(0.0)
|
||||
assert y == pytest.approx(0.0)
|
||||
assert th == pytest.approx(0.0)
|
||||
|
||||
def test_predict_straight(self):
|
||||
kf = KalmanOdomFilter()
|
||||
# Set velocity to 1 m/s forward, then predict 1s
|
||||
kf.update_wheel(1.0, 0.0)
|
||||
kf.predict(1.0)
|
||||
x, y, th = kf.pose
|
||||
assert x == pytest.approx(1.0, abs=0.2)
|
||||
assert abs(y) < 0.1
|
||||
assert th == pytest.approx(0.0, abs=0.1)
|
||||
|
||||
def test_predict_pure_rotation(self):
|
||||
kf = KalmanOdomFilter()
|
||||
kf.update_wheel(0.0, math.pi / 2)
|
||||
kf.predict(1.0)
|
||||
_, _, th = kf.pose
|
||||
assert th == pytest.approx(math.pi / 2, abs=0.1)
|
||||
|
||||
def test_angle_wrap(self):
|
||||
kf = KalmanOdomFilter()
|
||||
kf.update_wheel(0.0, math.pi)
|
||||
kf.predict(1.1) # should wrap past π
|
||||
_, _, th = kf.pose
|
||||
assert -math.pi <= th <= math.pi
|
||||
|
||||
def test_wheel_slip_inflates_covariance(self):
|
||||
kf_normal = KalmanOdomFilter()
|
||||
kf_slip = KalmanOdomFilter(slip_noise_multiplier=10.0)
|
||||
for _ in range(5):
|
||||
kf_normal.update_wheel(1.0, 0.0, slipping=False)
|
||||
kf_slip.update_wheel(1.0, 0.0, slipping=True)
|
||||
kf_normal.predict(0.1)
|
||||
kf_slip.predict(0.1)
|
||||
cov_normal = kf_normal.covariance_3x3[0, 0]
|
||||
cov_slip = kf_slip.covariance_3x3[0, 0]
|
||||
# Slip covariance should be larger
|
||||
assert cov_slip > cov_normal
|
||||
|
||||
def test_rtabmap_correction(self):
|
||||
kf = KalmanOdomFilter()
|
||||
# Drive straight for 5s to accumulate drift
|
||||
for _ in range(50):
|
||||
kf.update_wheel(1.0, 0.0)
|
||||
kf.predict(0.1)
|
||||
x_before, y_before, th_before = kf.pose
|
||||
# RTAB-Map says we're actually 1m behind
|
||||
kf.update_rtabmap(x_before - 1.0, 0.0, 0.0)
|
||||
x_after, _, _ = kf.pose
|
||||
# Pose should have moved toward correction
|
||||
assert x_after < x_before
|
||||
|
||||
def test_visual_odom_fusion(self):
|
||||
kf = KalmanOdomFilter()
|
||||
kf.update_wheel(1.0, 0.0, slipping=False)
|
||||
kf.update_visual(0.95, 0.0, valid=True) # VO slightly different
|
||||
kf.predict(0.1)
|
||||
v, _ = kf.velocity
|
||||
# Fused velocity should be between 0.95 and 1.0
|
||||
assert 0.9 <= v <= 1.05
|
||||
|
||||
|
||||
# ── StereoVO tests ────────────────────────────────────────────────────────────
|
||||
|
||||
class TestStereoVO:
|
||||
|
||||
def _make_K(self) -> np.ndarray:
|
||||
return np.array([
|
||||
[600.0, 0.0, 320.0],
|
||||
[ 0.0, 600.0, 240.0],
|
||||
[ 0.0, 0.0, 1.0],
|
||||
])
|
||||
|
||||
def test_no_K_returns_invalid(self):
|
||||
vo = StereoVO()
|
||||
prev = np.random.rand(10, 2).astype(np.float32) * 100
|
||||
curr = prev + np.random.rand(10, 2).astype(np.float32) * 2
|
||||
depth = np.ones((480, 640), dtype=np.float32) * 1.5
|
||||
est = vo.update(prev, curr, depth)
|
||||
assert not est.valid
|
||||
|
||||
def test_static_scene_near_zero_motion(self):
|
||||
vo = StereoVO(K=self._make_K())
|
||||
# No real motion: prev_pts == curr_pts → zero translation
|
||||
pts = np.random.rand(80, 2).astype(np.float32)
|
||||
pts[:, 0] *= 640
|
||||
pts[:, 1] *= 480
|
||||
depth = np.ones((480, 640), dtype=np.float32) * 2.0
|
||||
est = vo.update(pts, pts.copy(), depth)
|
||||
# With identical points, E is degenerate → may be invalid
|
||||
# Just ensure it doesn't raise
|
||||
assert isinstance(est.valid, bool)
|
||||
|
||||
def test_loop_closure_correction(self):
|
||||
vo = StereoVO(K=self._make_K())
|
||||
# Manually set accumulated pose
|
||||
vo._pose[0, 3] = 5.0
|
||||
vo._pose[1, 3] = 0.0
|
||||
# RTAB-Map says we're at (4.5, 0)
|
||||
vo.correct_from_loop_closure(4.5, 0.0, 0.0, alpha=1.0)
|
||||
assert vo._pose[0, 3] == pytest.approx(4.5, abs=0.01)
|
||||
|
||||
def test_reset_clears_state(self):
|
||||
vo = StereoVO(K=self._make_K())
|
||||
vo._pose[0, 3] = 10.0
|
||||
vo.reset()
|
||||
est = vo._invalid_est()
|
||||
assert est.x == pytest.approx(0.0)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
pytest.main([__file__, '-v'])
|
||||
Loading…
x
Reference in New Issue
Block a user