Compare commits
7 Commits
e67783f313
...
6d316514da
| Author | SHA1 | Date | |
|---|---|---|---|
| 6d316514da | |||
| 2d97033539 | |||
| 71ec357c93 | |||
| 19a30a1c4f | |||
| f71fdae747 | |||
| 916ad36ad5 | |||
| fc43135144 |
504
chassis/phone_mount_bracket.scad
Normal file
504
chassis/phone_mount_bracket.scad
Normal file
@ -0,0 +1,504 @@
|
||||
// ============================================================
|
||||
// phone_mount_bracket.scad — Spring-Loaded Phone Mount for T-Slot Rail
|
||||
// Issue: #535 Agent: sl-mechanical Date: 2026-03-07
|
||||
// ============================================================
|
||||
//
|
||||
// Parametric spring-loaded phone mount that clamps to the 2020 aluminium
|
||||
// T-slot sensor rail. Adjustable phone width 60–85 mm. Quick-release
|
||||
// cam lever for tool-free phone swap. Vibration-damping flexure ribs
|
||||
// on grip pads absorb motor/terrain vibration (PETG compliance).
|
||||
//
|
||||
// Design overview:
|
||||
// - Fixed jaw + sliding jaw on a 40 mm guide rail (M4 rod)
|
||||
// - Coil spring (Ø8 × 30 mm) compressed between jaw and end-stop —
|
||||
// spring pre-load keeps phone clamped at any width in range
|
||||
// - Cam lever (printed PETG) rotates 90° to release / lock spring
|
||||
// - Anti-vibration flexure ribs on both grip pad faces
|
||||
// - Landscape or portrait orientation: bracket rotates on T-nut base
|
||||
//
|
||||
// Parts (STL exports):
|
||||
// Part 1 — tnut_base() Rail attachment base (universal)
|
||||
// Part 2 — fixed_jaw() Fixed bottom jaw + guide rail bosses
|
||||
// Part 3 — sliding_jaw() Spring-loaded sliding jaw
|
||||
// Part 4 — cam_lever() Quick-release cam lever
|
||||
// Part 5 — grip_pad() Flexure grip pad (print ×2, TPU optional)
|
||||
// Part 6 — assembly_preview() Full assembly
|
||||
//
|
||||
// Hardware BOM (per mount):
|
||||
// 1× M4 × 60 mm SHCS guide rod + spring bolt
|
||||
// 1× M4 hex nut end-stop on sliding jaw
|
||||
// 1× Ø8 × 30 mm coil spring ~0.5 N/mm rate (spring clamping)
|
||||
// 2× M3 × 16 mm SHCS T-nut base thumbscrew + arm bolts
|
||||
// 1× M3 hex nut thumbscrew nut in T-nut
|
||||
// 4× M2 × 8 mm SHCS grip pad retention bolts (optional)
|
||||
//
|
||||
// Dimensions:
|
||||
// Phone width range : PHONE_W_MIN–PHONE_W_MAX (60–85 mm) parametric
|
||||
// Phone thickness : up to PHONE_THICK_MAX (12 mm) — open-front jaw
|
||||
// Phone height held : GRIP_SPAN (22 mm each jaw) — portrait/landscape
|
||||
// Overall bracket H : ~110 mm W: ~90 mm D: ~55 mm
|
||||
//
|
||||
// Print settings:
|
||||
// Material : PETG (tnut_base, fixed_jaw, sliding_jaw, cam_lever)
|
||||
// TPU 95A optional for grip_pad (or PETG for rigidity)
|
||||
// Perimeters: 5 (structural parts), 3 (grip_pad)
|
||||
// Infill : 40 % gyroid (jaws), 20 % (grip_pad)
|
||||
// Supports : none needed (designed for FDM orientation)
|
||||
// Layer ht : 0.2 mm
|
||||
//
|
||||
// Export commands:
|
||||
// openscad phone_mount_bracket.scad -D 'RENDER="tnut_base_stl"' -o pm_tnut_base.stl
|
||||
// openscad phone_mount_bracket.scad -D 'RENDER="fixed_jaw_stl"' -o pm_fixed_jaw.stl
|
||||
// openscad phone_mount_bracket.scad -D 'RENDER="sliding_jaw_stl"' -o pm_sliding_jaw.stl
|
||||
// openscad phone_mount_bracket.scad -D 'RENDER="cam_lever_stl"' -o pm_cam_lever.stl
|
||||
// openscad phone_mount_bracket.scad -D 'RENDER="grip_pad_stl"' -o pm_grip_pad.stl
|
||||
// ============================================================
|
||||
|
||||
$fn = 64;
|
||||
e = 0.01; // epsilon for boolean clearance
|
||||
|
||||
// ── Phone parameters (adjust to target device) ───────────────────────────────
|
||||
PHONE_W_MIN = 60.0; // narrowest phone width supported (mm)
|
||||
PHONE_W_MAX = 85.0; // widest phone width supported (mm)
|
||||
PHONE_THICK_MAX = 12.0; // max phone body thickness incl. case (mm)
|
||||
|
||||
// ── Rail geometry (must match sensor_rail.scad) ──────────────────────────────
|
||||
RAIL_W = 20.0;
|
||||
SLOT_OPEN = 6.0;
|
||||
SLOT_INNER_W = 10.2;
|
||||
SLOT_INNER_H = 5.8;
|
||||
SLOT_NECK_H = 3.2;
|
||||
|
||||
// ── T-nut constants ───────────────────────────────────────────────────────────
|
||||
TNUT_W = 9.8;
|
||||
TNUT_H = 5.5;
|
||||
TNUT_L = 12.0;
|
||||
TNUT_M3_NUT_AF = 5.5;
|
||||
TNUT_M3_NUT_H = 2.5;
|
||||
TNUT_BOLT_D = 3.3; // M3 clearance
|
||||
|
||||
// ── Base plate geometry ───────────────────────────────────────────────────────
|
||||
BASE_FACE_W = 30.0;
|
||||
BASE_FACE_H = 25.0;
|
||||
BASE_FACE_T = SLOT_NECK_H + 1.5;
|
||||
|
||||
// ── Jaw geometry ─────────────────────────────────────────────────────────────
|
||||
JAW_BODY_W = 88.0; // jaw outer width (> PHONE_W_MAX for rim)
|
||||
JAW_BODY_H = 28.0; // jaw height (Z) — phone grip span
|
||||
JAW_BODY_T = 14.0; // jaw depth (Y) — phone cradled this deep
|
||||
JAW_WALL_T = 4.0; // jaw side wall thickness
|
||||
JAW_LIP_T = 3.0; // front retaining lip thickness
|
||||
JAW_LIP_H = 5.0; // front lip height (retains phone)
|
||||
PHONE_POCKET_D = PHONE_THICK_MAX + 0.5; // pocket depth for phone
|
||||
|
||||
// ── Guide rod / spring system ─────────────────────────────────────────────────
|
||||
GUIDE_ROD_D = 4.3; // M4 clearance bore in sliding jaw
|
||||
GUIDE_BOSS_D = 10.0; // boss OD around guide bore
|
||||
GUIDE_BOSS_T = 6.0; // boss length
|
||||
SPRING_OD = 8.5; // coil spring OD pocket (spring is Ø8)
|
||||
SPRING_L = 32.0; // spring pocket length (spring compressed ~22 mm)
|
||||
SPRING_SEAT_T = 3.0; // spring seat wall at end-stop boss
|
||||
JAW_TRAVEL = PHONE_W_MAX - PHONE_W_MIN + 4.0; // max jaw travel (mm)
|
||||
ARM_SPAN = PHONE_W_MAX + 2 * JAW_WALL_T + 8; // fixed jaw total width
|
||||
|
||||
// ── Cam lever geometry ────────────────────────────────────────────────────────
|
||||
CAM_R_MIN = 5.0; // cam small radius (engaged / clamped)
|
||||
CAM_R_MAX = 9.0; // cam large radius (released, spring compressed)
|
||||
CAM_THICK = 8.0; // cam disc thickness
|
||||
CAM_HANDLE_L = 45.0; // lever arm length
|
||||
CAM_HANDLE_W = 8.0; // lever handle width
|
||||
CAM_HANDLE_T = 5.0; // lever handle thickness
|
||||
CAM_BORE_D = 4.3; // M4 pivot bore
|
||||
CAM_DETENT_D = 3.0; // detent ball pocket (3 mm bearing)
|
||||
|
||||
// ── Grip pad geometry (vibration dampening flexure ribs) ─────────────────────
|
||||
PAD_W = JAW_BODY_W - 2*JAW_WALL_T - 2; // pad width
|
||||
PAD_H = JAW_BODY_H - 2; // pad height
|
||||
PAD_T = 2.5; // pad body thickness
|
||||
RIB_H = 1.5; // flexure rib height above pad face
|
||||
RIB_W = 1.2; // rib width
|
||||
RIB_PITCH = 5.0; // rib pitch (centre-to-centre)
|
||||
RIB_COUNT = floor(PAD_W / RIB_PITCH) - 1;
|
||||
|
||||
// ── Arm geometry (base to jaw body) ──────────────────────────────────────────
|
||||
ARM_REACH = 38.0; // distance from rail face to jaw centreline (+Y)
|
||||
ARM_T = 4.0; // arm thickness
|
||||
ARM_H = BASE_FACE_H;
|
||||
|
||||
// ── Fasteners ─────────────────────────────────────────────────────────────────
|
||||
M2_D = 2.4;
|
||||
M3_D = 3.3;
|
||||
M4_D = 4.3;
|
||||
M4_NUT_AF = 7.0; // M4 hex nut across-flats
|
||||
M4_NUT_H = 3.2; // M4 hex nut height
|
||||
|
||||
// ============================================================
|
||||
// RENDER DISPATCH
|
||||
// ============================================================
|
||||
RENDER = "assembly";
|
||||
|
||||
if (RENDER == "assembly") assembly_preview();
|
||||
else if (RENDER == "tnut_base_stl") tnut_base();
|
||||
else if (RENDER == "fixed_jaw_stl") fixed_jaw();
|
||||
else if (RENDER == "sliding_jaw_stl") sliding_jaw();
|
||||
else if (RENDER == "cam_lever_stl") cam_lever();
|
||||
else if (RENDER == "grip_pad_stl") grip_pad();
|
||||
|
||||
// ============================================================
|
||||
// ASSEMBLY PREVIEW
|
||||
// ============================================================
|
||||
module assembly_preview() {
|
||||
// Ghost rail section (20 × 20 × 200)
|
||||
%color("Silver", 0.30)
|
||||
linear_extrude(200)
|
||||
square([RAIL_W, RAIL_W], center = true);
|
||||
|
||||
// T-nut base at Z=80 on rail
|
||||
color("OliveDrab", 0.85)
|
||||
translate([0, 0, 80])
|
||||
tnut_base();
|
||||
|
||||
// Fixed jaw assembly (centred, extending +Y from base)
|
||||
color("DarkSlateGray", 0.85)
|
||||
translate([0, SLOT_NECK_H + BASE_FACE_T + ARM_REACH, 80])
|
||||
fixed_jaw();
|
||||
|
||||
// Sliding jaw — shown at mid-travel (phone ~72 mm wide)
|
||||
color("SteelBlue", 0.85)
|
||||
translate([PHONE_W_MIN + (PHONE_W_MAX - PHONE_W_MIN)/2,
|
||||
SLOT_NECK_H + BASE_FACE_T + ARM_REACH, 80])
|
||||
sliding_jaw();
|
||||
|
||||
// Grip pads on both jaws
|
||||
color("DimGray", 0.85) {
|
||||
translate([0, SLOT_NECK_H + BASE_FACE_T + ARM_REACH, 80])
|
||||
translate([JAW_WALL_T, JAW_BODY_T, JAW_BODY_H/2])
|
||||
rotate([90, 0, 0])
|
||||
grip_pad();
|
||||
translate([PHONE_W_MIN + (PHONE_W_MAX - PHONE_W_MIN)/2,
|
||||
SLOT_NECK_H + BASE_FACE_T + ARM_REACH, 80])
|
||||
translate([-JAW_WALL_T - PAD_T, JAW_BODY_T, JAW_BODY_H/2])
|
||||
rotate([90, 0, 180])
|
||||
grip_pad();
|
||||
}
|
||||
|
||||
// Cam lever — shown in locked (clamped) position
|
||||
color("OrangeRed", 0.85)
|
||||
translate([ARM_SPAN/2 + 6,
|
||||
SLOT_NECK_H + BASE_FACE_T + ARM_REACH + GUIDE_BOSS_D/2,
|
||||
80 + JAW_BODY_H/2])
|
||||
rotate([0, 0, 0])
|
||||
cam_lever();
|
||||
}
|
||||
|
||||
// ============================================================
|
||||
// PART 1 — T-NUT BASE
|
||||
// ============================================================
|
||||
// Standard 2020 T-slot rail attachment base.
|
||||
// Identical interface to sensor_rail_brackets.scad universal_tnut_base().
|
||||
// Arm extends in +Y; rail clamp bolt in -Y face.
|
||||
//
|
||||
// Print flat (face plate down), PETG, 5 perims, 60 % infill.
|
||||
module tnut_base() {
|
||||
difference() {
|
||||
union() {
|
||||
// Face plate (flush against rail outer face)
|
||||
translate([-BASE_FACE_W/2, -BASE_FACE_T, 0])
|
||||
cube([BASE_FACE_W, BASE_FACE_T, BASE_FACE_H]);
|
||||
|
||||
// T-nut neck (enters rail slot)
|
||||
translate([-TNUT_W/2, 0, (BASE_FACE_H - TNUT_L)/2])
|
||||
cube([TNUT_W, SLOT_NECK_H + e, TNUT_L]);
|
||||
|
||||
// T-nut body (wider, inside T-groove)
|
||||
translate([-TNUT_W/2, SLOT_NECK_H - e, (BASE_FACE_H - TNUT_L)/2])
|
||||
cube([TNUT_W, TNUT_H - SLOT_NECK_H + e, TNUT_L]);
|
||||
|
||||
// Arm stub (face plate → jaw)
|
||||
translate([-BASE_FACE_W/2, -BASE_FACE_T, 0])
|
||||
cube([BASE_FACE_W, BASE_FACE_T + ARM_REACH, ARM_T]);
|
||||
}
|
||||
|
||||
// M3 rail clamp bolt bore (centre of T-nut, through face plate)
|
||||
translate([0, -BASE_FACE_T - e, BASE_FACE_H/2])
|
||||
rotate([-90, 0, 0])
|
||||
cylinder(d = TNUT_BOLT_D, h = BASE_FACE_T + TNUT_H + 2*e);
|
||||
|
||||
// M3 hex nut pocket (inside T-nut body)
|
||||
translate([0, SLOT_NECK_H + 0.3, BASE_FACE_H/2])
|
||||
rotate([-90, 0, 0])
|
||||
cylinder(d = TNUT_M3_NUT_AF / cos(30),
|
||||
h = TNUT_M3_NUT_H + 0.3,
|
||||
$fn = 6);
|
||||
|
||||
// 2× M3 bolt holes for arm-to-jaw bolting
|
||||
for (bx = [-10, 10])
|
||||
translate([bx, ARM_REACH - BASE_FACE_T - e, ARM_T/2])
|
||||
rotate([-90, 0, 0])
|
||||
cylinder(d = M3_D, h = 8 + 2*e);
|
||||
|
||||
// Lightening slot in arm
|
||||
translate([0, -BASE_FACE_T/2 + ARM_REACH/2, ARM_T/2])
|
||||
cube([BASE_FACE_W - 12, ARM_REACH - 16, ARM_T + 2*e],
|
||||
center = true);
|
||||
}
|
||||
}
|
||||
|
||||
// ============================================================
|
||||
// PART 2 — FIXED JAW
|
||||
// ============================================================
|
||||
// Fixed lower jaw of the clamping system. Phone sits in the pocket
|
||||
// formed by the fixed jaw (bottom) and sliding jaw (top).
|
||||
// Two guide bosses on the right wall carry the M4 guide rod + spring.
|
||||
// The cam lever pivot boss is on the outer right face.
|
||||
//
|
||||
// Coordinate origin: centre-bottom of jaw body.
|
||||
// Phone entry face: +Y (open front), phone pocket opens toward +Y.
|
||||
// Fixed jaw left edge is at X = -JAW_BODY_W/2.
|
||||
//
|
||||
// Print jaw-pocket-face down, PETG, 5 perims, 40 % infill.
|
||||
module fixed_jaw() {
|
||||
difference() {
|
||||
union() {
|
||||
// ── Main jaw body ────────────────────────────────────────────
|
||||
translate([-JAW_BODY_W/2, -JAW_BODY_T/2, 0])
|
||||
cube([JAW_BODY_W, JAW_BODY_T, JAW_BODY_H]);
|
||||
|
||||
// ── Front retaining lip (keeps phone from falling forward) ───
|
||||
translate([-JAW_BODY_W/2, JAW_BODY_T/2 - JAW_LIP_T, 0])
|
||||
cube([JAW_BODY_W, JAW_LIP_T, JAW_LIP_H]);
|
||||
|
||||
// ── Guide boss right (outer, carries spring + end-stop) ──────
|
||||
translate([JAW_BODY_W/2, 0, JAW_BODY_H/2])
|
||||
rotate([0, 90, 0])
|
||||
cylinder(d = GUIDE_BOSS_D, h = GUIDE_BOSS_T);
|
||||
|
||||
// ── Cam lever pivot boss (right face, above guide boss) ──────
|
||||
translate([JAW_BODY_W/2, 0, JAW_BODY_H/2 + GUIDE_BOSS_D + 4])
|
||||
rotate([0, 90, 0])
|
||||
cylinder(d = CAM_THICK + 4, h = 6);
|
||||
|
||||
// ── Arm attachment bosses (left side, connect to tnut_base) ──
|
||||
for (bx = [-10, 10])
|
||||
translate([bx, -JAW_BODY_T/2 - 8, ARM_T/2])
|
||||
cylinder(d = 8, h = 8);
|
||||
}
|
||||
|
||||
// ── Phone pocket (open-top U channel centred in jaw) ────────────
|
||||
// Pocket opens toward +Y (front), phone drops in from above.
|
||||
translate([0, -JAW_BODY_T/2 - e,
|
||||
JAW_LIP_H])
|
||||
cube([JAW_BODY_W - 2*JAW_WALL_T,
|
||||
PHONE_POCKET_D + JAW_WALL_T,
|
||||
JAW_BODY_H - JAW_LIP_H + e],
|
||||
center = [true, false, false]);
|
||||
|
||||
// ── Guide rod bore (M4 clearance, through both guide bosses) ────
|
||||
translate([-JAW_BODY_W/2 - e, 0, JAW_BODY_H/2])
|
||||
rotate([0, 90, 0])
|
||||
cylinder(d = GUIDE_ROD_D,
|
||||
h = JAW_BODY_W + GUIDE_BOSS_T + 2*e);
|
||||
|
||||
// ── Spring pocket (coaxial with guide rod, in right boss) ────────
|
||||
translate([JAW_BODY_W/2 + e, 0, JAW_BODY_H/2])
|
||||
rotate([0, -90, 0])
|
||||
cylinder(d = SPRING_OD, h = SPRING_L);
|
||||
|
||||
// ── M4 hex nut pocket in spring-seat wall (end-stop nut) ────────
|
||||
translate([JAW_BODY_W/2 + GUIDE_BOSS_T + e, 0, JAW_BODY_H/2])
|
||||
rotate([0, -90, 0])
|
||||
cylinder(d = M4_NUT_AF / cos(30), h = M4_NUT_H + 0.5,
|
||||
$fn = 6);
|
||||
|
||||
// ── Cam pivot bore (M4 pivot, through pivot boss) ────────────────
|
||||
translate([JAW_BODY_W/2 - e, 0, JAW_BODY_H/2 + GUIDE_BOSS_D + 4])
|
||||
rotate([0, 90, 0])
|
||||
cylinder(d = CAM_BORE_D, h = 6 + 2*e);
|
||||
|
||||
// ── Arm attachment bolt holes (M3, to tnut_base arm stubs) ──────
|
||||
for (bx = [-10, 10])
|
||||
translate([bx, -JAW_BODY_T/2 - 8 - e, ARM_T/2])
|
||||
rotate([-90, 0, 0])
|
||||
cylinder(d = M3_D, h = 12 + 2*e);
|
||||
|
||||
// ── Grip pad seats (recessed Ø1.5 mm, 2 mm deep, optional) ──────
|
||||
for (pz = [JAW_BODY_H * 0.3, JAW_BODY_H * 0.7])
|
||||
for (px = [-PAD_W/4, PAD_W/4])
|
||||
translate([px, -JAW_BODY_T/2 + PHONE_POCKET_D + 1, pz])
|
||||
rotate([-90, 0, 0])
|
||||
cylinder(d = M2_D, h = 10);
|
||||
|
||||
// ── Lightening pockets (non-structural core removal) ─────────────
|
||||
translate([0, 0, JAW_BODY_H/2])
|
||||
cube([JAW_BODY_W - 2*JAW_WALL_T - 4,
|
||||
JAW_BODY_T - 2*JAW_WALL_T,
|
||||
JAW_BODY_H - JAW_LIP_H - 4],
|
||||
center = true);
|
||||
}
|
||||
}
|
||||
|
||||
// ============================================================
|
||||
// PART 3 — SLIDING JAW
|
||||
// ============================================================
|
||||
// Upper clamping jaw. Slides along the M4 guide rod.
|
||||
// Spring pushes this jaw toward the phone (inward).
|
||||
// M4 hex nut on the guide rod limits maximum travel (full open).
|
||||
// Cam lever pushes on this jaw face to compress spring (release).
|
||||
//
|
||||
// Coordinate origin same convention as fixed_jaw() for assembly.
|
||||
// Jaw slides in +X direction (away from fixed jaw left wall).
|
||||
//
|
||||
// Print jaw-pocket-face down, PETG, 5 perims, 40 % infill.
|
||||
module sliding_jaw() {
|
||||
difference() {
|
||||
union() {
|
||||
// ── Main jaw body ────────────────────────────────────────────
|
||||
translate([-JAW_WALL_T, -JAW_BODY_T/2, 0])
|
||||
cube([JAW_BODY_W/2 + JAW_WALL_T, JAW_BODY_T, JAW_BODY_H]);
|
||||
|
||||
// ── Front retaining lip ──────────────────────────────────────
|
||||
translate([-JAW_WALL_T, JAW_BODY_T/2 - JAW_LIP_T, 0])
|
||||
cube([JAW_BODY_W/2 + JAW_WALL_T, JAW_LIP_T, JAW_LIP_H]);
|
||||
|
||||
// ── Guide boss (carries guide rod, spring butts against face) ─
|
||||
translate([-JAW_WALL_T - GUIDE_BOSS_T, 0, JAW_BODY_H/2])
|
||||
rotate([0, 90, 0])
|
||||
cylinder(d = GUIDE_BOSS_D, h = GUIDE_BOSS_T);
|
||||
|
||||
// ── Cam follower ear (contacts cam lever) ────────────────────
|
||||
translate([-JAW_WALL_T - 2, 0,
|
||||
JAW_BODY_H/2 + GUIDE_BOSS_D + 4])
|
||||
cube([4, CAM_THICK + 2, CAM_THICK + 2], center = true);
|
||||
}
|
||||
|
||||
// ── Phone pocket (inner face, contacts phone side) ───────────────
|
||||
translate([-JAW_WALL_T - e, -JAW_BODY_T/2 - e, JAW_LIP_H])
|
||||
cube([JAW_BODY_W/2 - JAW_WALL_T + e,
|
||||
PHONE_POCKET_D + JAW_WALL_T + 2*e,
|
||||
JAW_BODY_H - JAW_LIP_H + e]);
|
||||
|
||||
// ── Guide rod bore (M4 clearance through boss + jaw wall) ────────
|
||||
translate([-JAW_WALL_T - GUIDE_BOSS_T - e, 0, JAW_BODY_H/2])
|
||||
rotate([0, 90, 0])
|
||||
cylinder(d = GUIDE_ROD_D,
|
||||
h = GUIDE_BOSS_T + JAW_WALL_T + 2*e);
|
||||
|
||||
// ── M4 nut pocket (end-stop nut, rear of guide boss) ────────────
|
||||
translate([-JAW_WALL_T - GUIDE_BOSS_T - e, 0, JAW_BODY_H/2])
|
||||
rotate([0, 90, 0])
|
||||
cylinder(d = M4_NUT_AF / cos(30), h = M4_NUT_H + 1,
|
||||
$fn = 6);
|
||||
|
||||
// ── Cam follower bore (M4 pivot passes through ear) ─────────────
|
||||
translate([-JAW_WALL_T - 2 - e, 0,
|
||||
JAW_BODY_H/2 + GUIDE_BOSS_D + 4])
|
||||
rotate([0, 90, 0])
|
||||
cylinder(d = CAM_BORE_D, h = 6 + 2*e);
|
||||
|
||||
// ── Grip pad seats ───────────────────────────────────────────────
|
||||
for (pz = [JAW_BODY_H * 0.3, JAW_BODY_H * 0.7])
|
||||
for (px = [JAW_BODY_W/8])
|
||||
translate([px, -JAW_BODY_T/2 + PHONE_POCKET_D + 1, pz])
|
||||
rotate([-90, 0, 0])
|
||||
cylinder(d = M2_D, h = 10);
|
||||
}
|
||||
}
|
||||
|
||||
// ============================================================
|
||||
// PART 4 — CAM LEVER (QUICK-RELEASE)
|
||||
// ============================================================
|
||||
// Eccentric cam disc + integral handle lever.
|
||||
// Rotates 90° on M4 pivot pin between CLAMPED and RELEASED states:
|
||||
// CLAMPED : cam small radius (CAM_R_MIN) toward jaw → spring pushes jaw
|
||||
// RELEASED : cam large radius (CAM_R_MAX) toward jaw → compresses spring
|
||||
// by (CAM_R_MAX - CAM_R_MIN) = 4 mm, opening jaw
|
||||
//
|
||||
// Detent ball pocket (Ø3 mm) snaps into rail-dimple for each position.
|
||||
// Handle points rearward (-Y) in clamped state for low profile.
|
||||
//
|
||||
// Print standing on cam edge (cam disc vertical), PETG, 5 perims, 40%.
|
||||
module cam_lever() {
|
||||
cam_offset = (CAM_R_MAX - CAM_R_MIN) / 2; // 2 mm eccentricity
|
||||
union() {
|
||||
difference() {
|
||||
union() {
|
||||
// ── Eccentric cam disc ───────────────────────────────────
|
||||
// Offset so pivot bore is eccentric to disc profile
|
||||
translate([cam_offset, 0, 0])
|
||||
cylinder(r = CAM_R_MAX, h = CAM_THICK, center = true);
|
||||
|
||||
// ── Lever handle arm ─────────────────────────────────────
|
||||
hull() {
|
||||
translate([cam_offset, 0, 0])
|
||||
cylinder(r = CAM_R_MAX, h = CAM_THICK, center = true);
|
||||
translate([cam_offset + CAM_HANDLE_L, 0, 0])
|
||||
cylinder(r = CAM_HANDLE_W/2,
|
||||
h = CAM_HANDLE_T, center = true);
|
||||
}
|
||||
}
|
||||
|
||||
// ── M4 pivot bore (through cam centre) ───────────────────────
|
||||
cylinder(d = CAM_BORE_D, h = CAM_THICK + 2*e, center = true);
|
||||
|
||||
// ── Detent pockets (2× Ø3 mm, at 0° and 90°) ────────────────
|
||||
// Pocket at 0° → clamped detent
|
||||
translate([CAM_R_MAX - 2, 0, CAM_THICK/2 - 1.5])
|
||||
cylinder(d = CAM_DETENT_D + 0.2, h = 2);
|
||||
// Pocket at 90° → released detent
|
||||
translate([0, CAM_R_MAX - 2, CAM_THICK/2 - 1.5])
|
||||
cylinder(d = CAM_DETENT_D + 0.2, h = 2);
|
||||
|
||||
// ── Lightening recesses on cam disc face ─────────────────────
|
||||
for (a = [0, 60, 120, 180, 240, 300])
|
||||
translate([cam_offset + (CAM_R_MAX - 4) * cos(a),
|
||||
(CAM_R_MAX - 4) * sin(a), 0])
|
||||
cylinder(d = 4, h = CAM_THICK + 2*e, center = true);
|
||||
|
||||
// ── Handle grip grooves ──────────────────────────────────────
|
||||
for (i = [0:4])
|
||||
translate([cam_offset + 20 + i * 5, 0, 0])
|
||||
rotate([90, 0, 0])
|
||||
cylinder(d = 2.5, h = CAM_HANDLE_W + 2*e,
|
||||
center = true);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// ============================================================
|
||||
// PART 5 — GRIP PAD (VIBRATION DAMPENING)
|
||||
// ============================================================
|
||||
// Flat pad with transverse flexure ribs that press against phone side.
|
||||
// The rib profile (thin PETG fins) provides compliance in Z (vertical)
|
||||
// absorbing vibration transmitted through the bracket.
|
||||
// Optional: print in TPU 95A for superior damping.
|
||||
// M2 bolts or adhesive-backed foam tape attach pad to jaw pocket face.
|
||||
//
|
||||
// Pad face (+Y) contacts phone. Mounting face (-Y) bonds to jaw.
|
||||
// Ribs run parallel to Z axis (vertical).
|
||||
//
|
||||
// Print flat (mounting face down), PETG or TPU 95A, 3 perims, 20%.
|
||||
module grip_pad() {
|
||||
union() {
|
||||
// ── Base plate ───────────────────────────────────────────────────
|
||||
translate([-PAD_W/2, -PAD_T, -PAD_H/2])
|
||||
cube([PAD_W, PAD_T, PAD_H]);
|
||||
|
||||
// ── Flexure ribs (transverse, dampening in Z) ────────────────────
|
||||
// RIB_COUNT ribs spaced RIB_PITCH apart, centred on pad
|
||||
for (i = [0 : RIB_COUNT - 1]) {
|
||||
rx = -PAD_W/2 + RIB_PITCH/2 + i * RIB_PITCH;
|
||||
if (abs(rx) <= PAD_W/2 - RIB_W/2) // stay within pad
|
||||
translate([rx, 0, 0])
|
||||
cube([RIB_W, RIB_H, PAD_H - 4], center = true);
|
||||
}
|
||||
|
||||
// ── Corner retention nubs (M2 boss for optional bolt-through) ────
|
||||
for (px = [-PAD_W/2 + 5, PAD_W/2 - 5])
|
||||
for (pz = [-PAD_H/2 + 5, PAD_H/2 - 5])
|
||||
translate([px, -PAD_T/2, pz])
|
||||
difference() {
|
||||
cylinder(d = 5, h = PAD_T, center = true);
|
||||
cylinder(d = M2_D, h = PAD_T + 2*e, center = true);
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -20,17 +20,20 @@
|
||||
* CRC16 : CRC16-XModem over CMD+PAYLOAD (poly 0x1021, init 0), big-endian
|
||||
* ETX : frame end sentinel (0x03)
|
||||
*
|
||||
* Jetson → STM32 commands:
|
||||
* 0x01 HEARTBEAT — no payload; refreshes heartbeat timer
|
||||
* 0x02 DRIVE — int16 speed (-1000..+1000), int16 steer (-1000..+1000)
|
||||
* 0x03 ARM — no payload; request arm (same interlock as CDC 'A')
|
||||
* 0x04 DISARM — no payload; disarm immediately
|
||||
* 0x05 PID_SET — float kp, float ki, float kd (12 bytes, IEEE-754 LE)
|
||||
* 0x06 DFU_ENTER — no payload; request OTA DFU reboot (denied while armed)
|
||||
* 0x07 ESTOP — no payload; engage emergency stop
|
||||
* Jetson to STM32 commands:
|
||||
* 0x01 HEARTBEAT - no payload; refreshes heartbeat timer
|
||||
* 0x02 DRIVE - int16 speed (-1000..+1000), int16 steer (-1000..+1000)
|
||||
* 0x03 ARM - no payload; request arm (same interlock as CDC 'A')
|
||||
* 0x04 DISARM - no payload; disarm immediately
|
||||
* 0x05 PID_SET - float kp, float ki, float kd (12 bytes, IEEE-754 LE)
|
||||
* 0x06 DFU_ENTER - no payload; request OTA DFU reboot (denied while armed)
|
||||
* 0x07 ESTOP - no payload; engage emergency stop
|
||||
* 0x0A PID_SAVE - no payload; save current Kp/Ki/Kd to flash (Issue #531)
|
||||
*
|
||||
* STM32 → Jetson telemetry:
|
||||
* 0x80 STATUS — jlink_tlm_status_t (20 bytes), sent at JLINK_TLM_HZ
|
||||
* STM32 to Jetson telemetry:
|
||||
* 0x80 STATUS - jlink_tlm_status_t (20 bytes), sent at JLINK_TLM_HZ
|
||||
* 0x81 POWER - jlink_tlm_power_t (11 bytes), sent at PM_TLM_HZ
|
||||
* 0x83 PID_RESULT - jlink_tlm_pid_result_t (13 bytes), sent after PID_SAVE (Issue #531)
|
||||
*
|
||||
* Priority: CRSF RC always takes precedence. Jetson steer/speed only applied
|
||||
* when mode_manager_active() == MODE_AUTONOMOUS (CH6 high). In RC_MANUAL and
|
||||
@ -45,7 +48,7 @@
|
||||
#define JLINK_STX 0x02u
|
||||
#define JLINK_ETX 0x03u
|
||||
|
||||
/* ---- Command IDs (Jetson → STM32) ---- */
|
||||
/* ---- Command IDs (Jetson to STM32) ---- */
|
||||
#define JLINK_CMD_HEARTBEAT 0x01u
|
||||
#define JLINK_CMD_DRIVE 0x02u
|
||||
#define JLINK_CMD_ARM 0x03u
|
||||
@ -55,16 +58,18 @@
|
||||
#define JLINK_CMD_ESTOP 0x07u
|
||||
#define JLINK_CMD_AUDIO 0x08u /* PCM audio chunk: int16 samples, up to 126 */
|
||||
#define JLINK_CMD_SLEEP 0x09u /* no payload; request STOP-mode sleep */
|
||||
#define JLINK_CMD_PID_SAVE 0x0Au /* no payload; save Kp/Ki/Kd to flash (Issue #531) */
|
||||
|
||||
/* ---- Telemetry IDs (STM32 → Jetson) ---- */
|
||||
/* ---- Telemetry IDs (STM32 to Jetson) ---- */
|
||||
#define JLINK_TLM_STATUS 0x80u
|
||||
#define JLINK_TLM_POWER 0x81u /* jlink_tlm_power_t (11 bytes) */
|
||||
#define JLINK_TLM_PID_RESULT 0x83u /* jlink_tlm_pid_result_t (13 bytes) Issue #531 */
|
||||
|
||||
/* ---- Telemetry STATUS payload (20 bytes, packed) ---- */
|
||||
typedef struct __attribute__((packed)) {
|
||||
int16_t pitch_x10; /* pitch degrees ×10 */
|
||||
int16_t roll_x10; /* roll degrees ×10 */
|
||||
int16_t yaw_x10; /* yaw degrees ×10 (gyro-integrated) */
|
||||
int16_t pitch_x10; /* pitch degrees x10 */
|
||||
int16_t roll_x10; /* roll degrees x10 */
|
||||
int16_t yaw_x10; /* yaw degrees x10 (gyro-integrated) */
|
||||
int16_t motor_cmd; /* ESC output -1000..+1000 */
|
||||
uint16_t vbat_mv; /* battery millivolts */
|
||||
int8_t rssi_dbm; /* CRSF RSSI (dBm, negative) */
|
||||
@ -88,30 +93,41 @@ typedef struct __attribute__((packed)) {
|
||||
uint32_t idle_ms; /* ms since last cmd_vel activity */
|
||||
} jlink_tlm_power_t; /* 11 bytes */
|
||||
|
||||
/* ---- Telemetry PID_RESULT payload (13 bytes, packed) Issue #531 ---- */
|
||||
/* Sent after JLINK_CMD_PID_SAVE is processed; confirms gains written to flash. */
|
||||
typedef struct __attribute__((packed)) {
|
||||
float kp; /* Kp saved */
|
||||
float ki; /* Ki saved */
|
||||
float kd; /* Kd saved */
|
||||
uint8_t saved_ok; /* 1 = flash write verified, 0 = write failed */
|
||||
} jlink_tlm_pid_result_t; /* 13 bytes */
|
||||
|
||||
/* ---- Volatile state (read from main loop) ---- */
|
||||
typedef struct {
|
||||
/* Drive command — updated on JLINK_CMD_DRIVE */
|
||||
/* Drive command - updated on JLINK_CMD_DRIVE */
|
||||
volatile int16_t speed; /* -1000..+1000 */
|
||||
volatile int16_t steer; /* -1000..+1000 */
|
||||
|
||||
/* Heartbeat timer — updated on any valid frame */
|
||||
/* Heartbeat timer - updated on any valid frame */
|
||||
volatile uint32_t last_rx_ms; /* HAL_GetTick() of last valid frame; 0=none */
|
||||
|
||||
/* One-shot request flags — set by parser, cleared by main loop */
|
||||
/* One-shot request flags - set by parser, cleared by main loop */
|
||||
volatile uint8_t arm_req;
|
||||
volatile uint8_t disarm_req;
|
||||
volatile uint8_t estop_req;
|
||||
|
||||
/* PID update — set by parser, cleared by main loop */
|
||||
/* PID update - set by parser, cleared by main loop */
|
||||
volatile uint8_t pid_updated;
|
||||
volatile float pid_kp;
|
||||
volatile float pid_ki;
|
||||
volatile float pid_kd;
|
||||
|
||||
/* DFU reboot request — set by parser, cleared by main loop */
|
||||
/* DFU reboot request - set by parser, cleared by main loop */
|
||||
volatile uint8_t dfu_req;
|
||||
/* Sleep request — set by JLINK_CMD_SLEEP, cleared by main loop */
|
||||
/* Sleep request - set by JLINK_CMD_SLEEP, cleared by main loop */
|
||||
volatile uint8_t sleep_req;
|
||||
/* PID save request - set by JLINK_CMD_PID_SAVE, cleared by main loop (Issue #531) */
|
||||
volatile uint8_t pid_save_req;
|
||||
} JLinkState;
|
||||
|
||||
extern volatile JLinkState jlink_state;
|
||||
@ -119,34 +135,40 @@ extern volatile JLinkState jlink_state;
|
||||
/* ---- API ---- */
|
||||
|
||||
/*
|
||||
* jlink_init() — configure USART1 (PB6=TX, PB7=RX) at 921600 baud with
|
||||
* jlink_init() - configure USART1 (PB6=TX, PB7=RX) at 921600 baud with
|
||||
* DMA2_Stream2_Channel4 circular RX (128-byte buffer) and IDLE interrupt.
|
||||
* Call once before safety_init().
|
||||
*/
|
||||
void jlink_init(void);
|
||||
|
||||
/*
|
||||
* jlink_is_active(now_ms) — returns true if a valid frame arrived within
|
||||
* jlink_is_active(now_ms) - returns true if a valid frame arrived within
|
||||
* JLINK_HB_TIMEOUT_MS. Returns false if no frame ever received.
|
||||
*/
|
||||
bool jlink_is_active(uint32_t now_ms);
|
||||
|
||||
/*
|
||||
* jlink_send_telemetry(status) — build and transmit a JLINK_TLM_STATUS frame
|
||||
* jlink_send_telemetry(status) - build and transmit a JLINK_TLM_STATUS frame
|
||||
* over USART1 TX (blocking, ~0.2ms at 921600). Call at JLINK_TLM_HZ.
|
||||
*/
|
||||
void jlink_send_telemetry(const jlink_tlm_status_t *status);
|
||||
|
||||
/*
|
||||
* jlink_process() — drain DMA circular buffer and parse frames.
|
||||
* jlink_process() - drain DMA circular buffer and parse frames.
|
||||
* Call from main loop every iteration (not ISR). Lightweight: O(bytes_pending).
|
||||
*/
|
||||
void jlink_process(void);
|
||||
|
||||
/*
|
||||
* jlink_send_power_telemetry(power) — build and transmit a JLINK_TLM_POWER
|
||||
* jlink_send_power_telemetry(power) - build and transmit a JLINK_TLM_POWER
|
||||
* frame (17 bytes) at PM_TLM_HZ. Call from main loop when not in STOP mode.
|
||||
*/
|
||||
void jlink_send_power_telemetry(const jlink_tlm_power_t *power);
|
||||
|
||||
/*
|
||||
* jlink_send_pid_result(result) - build and transmit a JLINK_TLM_PID_RESULT
|
||||
* frame (19 bytes) to confirm PID flash save outcome (Issue #531).
|
||||
*/
|
||||
void jlink_send_pid_result(const jlink_tlm_pid_result_t *result);
|
||||
|
||||
#endif /* JLINK_H */
|
||||
|
||||
48
include/pid_flash.h
Normal file
48
include/pid_flash.h
Normal file
@ -0,0 +1,48 @@
|
||||
#ifndef PID_FLASH_H
|
||||
#define PID_FLASH_H
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
|
||||
/*
|
||||
* pid_flash — persistent PID storage for Issue #531 (auto-tune).
|
||||
*
|
||||
* Stores Kp, Ki, Kd in the last 64 bytes of STM32F722 flash sector 7
|
||||
* (0x0807FFC0). Magic word validates presence of saved params.
|
||||
* Sector 7 is 128KB starting at 0x08060000; firmware never exceeds sector 6.
|
||||
*
|
||||
* Flash writes require an erase of the full sector (128KB) before re-writing.
|
||||
* The store address is the very last 64-byte block so future expansion can
|
||||
* grow toward lower addresses within sector 7 without conflict.
|
||||
*/
|
||||
|
||||
#define PID_FLASH_SECTOR FLASH_SECTOR_7
|
||||
#define PID_FLASH_SECTOR_VOLTAGE VOLTAGE_RANGE_3 /* 2.7V-3.6V, 32-bit parallelism */
|
||||
|
||||
/* Sector 7: 128KB at 0x08060000; store in last 64 bytes */
|
||||
#define PID_FLASH_STORE_ADDR 0x0807FFC0UL
|
||||
#define PID_FLASH_MAGIC 0x534C5401UL /* 'SLT\x01' — version 1 */
|
||||
|
||||
typedef struct __attribute__((packed)) {
|
||||
uint32_t magic; /* PID_FLASH_MAGIC when valid */
|
||||
float kp;
|
||||
float ki;
|
||||
float kd;
|
||||
uint8_t _pad[48]; /* padding to 64 bytes */
|
||||
} pid_flash_t;
|
||||
|
||||
/*
|
||||
* pid_flash_load() — read saved PID from flash.
|
||||
* Returns true and fills *kp/*ki/*kd if magic is valid.
|
||||
* Returns false if no valid params stored (caller keeps defaults).
|
||||
*/
|
||||
bool pid_flash_load(float *kp, float *ki, float *kd);
|
||||
|
||||
/*
|
||||
* pid_flash_save() — erase sector 7 and write Kp/Ki/Kd.
|
||||
* Must not be called while armed (flash erase takes ~1s and stalls the CPU).
|
||||
* Returns true on success.
|
||||
*/
|
||||
bool pid_flash_save(float kp, float ki, float kd);
|
||||
|
||||
#endif /* PID_FLASH_H */
|
||||
@ -10,12 +10,14 @@
|
||||
# → No AMCL, no map_server needed.
|
||||
#
|
||||
# Sensors (Issue #478 — Costmap configuration):
|
||||
# /scan — RPLIDAR A1M8 360° LaserScan (obstacle)
|
||||
# /depth_scan — RealSense D435i depth_to_laserscan (obstacle)
|
||||
# /camera/depth/color/points — RealSense D435i PointCloud2 (voxel)
|
||||
# /scan — RPLIDAR A1M8 360° LaserScan (obstacle)
|
||||
# /depth_scan — RealSense D435i depth_to_laserscan (obstacle)
|
||||
# /camera/depth/color/points — RealSense D435i PointCloud2 (voxel)
|
||||
# /camera/depth/image_rect_raw — RealSense D435i depth image (Issue #532 DepthCostmapLayer)
|
||||
#
|
||||
# Inflation:
|
||||
# inflation_radius: 0.3m (robot_radius 0.15m + 0.15m padding)
|
||||
# DepthCostmapLayer in-layer inflation: 0.10m (pre-inflation before inflation_layer)
|
||||
#
|
||||
# Output: /cmd_vel (Twist) — STM32 bridge consumes this topic.
|
||||
|
||||
@ -256,7 +258,21 @@ local_costmap:
|
||||
resolution: 0.05
|
||||
robot_radius: 0.15
|
||||
footprint: "[[-0.2, -0.2], [-0.2, 0.2], [0.2, 0.2], [0.2, -0.2]]" # 0.4m x 0.4m
|
||||
plugins: ["obstacle_layer", "voxel_layer", "inflation_layer"]
|
||||
plugins: ["obstacle_layer", "voxel_layer", "depth_costmap_layer", "inflation_layer"]
|
||||
|
||||
# Issue #532: RealSense D435i depth-to-costmap plugin
|
||||
# Point cloud projection with height filter + in-layer inflation
|
||||
depth_costmap_layer:
|
||||
plugin: "saltybot_depth_costmap::DepthCostmapLayer"
|
||||
enabled: true
|
||||
depth_topic: /camera/depth/image_rect_raw
|
||||
camera_info_topic: /camera/depth/camera_info
|
||||
min_height: 0.05 # m — ignore floor reflections
|
||||
max_height: 0.80 # m — ignore ceiling/lights
|
||||
obstacle_range: 3.5 # m — max range to mark obstacles
|
||||
clearing_range: 4.0 # m — max range for clearing
|
||||
inflation_radius: 0.10 # m — in-layer inflation (before inflation_layer)
|
||||
downsample_factor: 4 # process 1 of 4 rows/cols (~19k pts @ 640x480)
|
||||
|
||||
obstacle_layer:
|
||||
plugin: "nav2_costmap_2d::ObstacleLayer"
|
||||
@ -327,7 +343,20 @@ global_costmap:
|
||||
footprint: "[[-0.2, -0.2], [-0.2, 0.2], [0.2, 0.2], [0.2, -0.2]]" # 0.4m x 0.4m
|
||||
resolution: 0.05
|
||||
track_unknown_space: true
|
||||
plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
|
||||
plugins: ["static_layer", "obstacle_layer", "depth_costmap_layer", "inflation_layer"]
|
||||
|
||||
# Issue #532: RealSense D435i depth-to-costmap plugin (global costmap)
|
||||
depth_costmap_layer:
|
||||
plugin: "saltybot_depth_costmap::DepthCostmapLayer"
|
||||
enabled: true
|
||||
depth_topic: /camera/depth/image_rect_raw
|
||||
camera_info_topic: /camera/depth/camera_info
|
||||
min_height: 0.05
|
||||
max_height: 0.80
|
||||
obstacle_range: 3.5
|
||||
clearing_range: 4.0
|
||||
inflation_radius: 0.10
|
||||
downsample_factor: 4
|
||||
|
||||
static_layer:
|
||||
plugin: "nav2_costmap_2d::StaticLayer"
|
||||
|
||||
78
jetson/ros2_ws/src/saltybot_depth_costmap/CMakeLists.txt
Normal file
78
jetson/ros2_ws/src/saltybot_depth_costmap/CMakeLists.txt
Normal file
@ -0,0 +1,78 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(saltybot_depth_costmap)
|
||||
|
||||
if(NOT CMAKE_CXX_STANDARD)
|
||||
set(CMAKE_CXX_STANDARD 17)
|
||||
endif()
|
||||
|
||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||
add_compile_options(-Wall -Wextra -Wpedantic)
|
||||
endif()
|
||||
|
||||
# ── Dependencies ──────────────────────────────────────────────────────────────
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(rclcpp REQUIRED)
|
||||
find_package(nav2_costmap_2d REQUIRED)
|
||||
find_package(sensor_msgs REQUIRED)
|
||||
find_package(geometry_msgs REQUIRED)
|
||||
find_package(tf2_ros REQUIRED)
|
||||
find_package(tf2 REQUIRED)
|
||||
find_package(pluginlib REQUIRED)
|
||||
|
||||
# ── Shared library (the plugin) ───────────────────────────────────────────────
|
||||
add_library(${PROJECT_NAME} SHARED
|
||||
src/depth_costmap_layer.cpp
|
||||
)
|
||||
|
||||
target_include_directories(${PROJECT_NAME} PUBLIC
|
||||
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
|
||||
$<INSTALL_INTERFACE:include>
|
||||
)
|
||||
|
||||
ament_target_dependencies(${PROJECT_NAME}
|
||||
rclcpp
|
||||
nav2_costmap_2d
|
||||
sensor_msgs
|
||||
geometry_msgs
|
||||
tf2_ros
|
||||
tf2
|
||||
pluginlib
|
||||
)
|
||||
|
||||
# Export plugin XML so pluginlib can discover it
|
||||
pluginlib_export_plugin_description_file(nav2_costmap_2d plugin.xml)
|
||||
|
||||
# ── Install ───────────────────────────────────────────────────────────────────
|
||||
install(TARGETS ${PROJECT_NAME}
|
||||
ARCHIVE DESTINATION lib
|
||||
LIBRARY DESTINATION lib
|
||||
RUNTIME DESTINATION bin
|
||||
)
|
||||
|
||||
install(DIRECTORY include/
|
||||
DESTINATION include
|
||||
)
|
||||
|
||||
install(FILES plugin.xml
|
||||
DESTINATION share/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
# ── Tests ─────────────────────────────────────────────────────────────────────
|
||||
if(BUILD_TESTING)
|
||||
find_package(ament_lint_auto REQUIRED)
|
||||
ament_lint_auto_find_test_dependencies()
|
||||
endif()
|
||||
|
||||
ament_export_include_directories(include)
|
||||
ament_export_libraries(${PROJECT_NAME})
|
||||
ament_export_dependencies(
|
||||
rclcpp
|
||||
nav2_costmap_2d
|
||||
sensor_msgs
|
||||
geometry_msgs
|
||||
tf2_ros
|
||||
tf2
|
||||
pluginlib
|
||||
)
|
||||
|
||||
ament_package()
|
||||
@ -0,0 +1,113 @@
|
||||
#pragma once
|
||||
|
||||
#include <memory>
|
||||
#include <mutex>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include "geometry_msgs/msg/transform_stamped.hpp"
|
||||
#include "nav2_costmap_2d/layer.hpp"
|
||||
#include "nav2_costmap_2d/layered_costmap.hpp"
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "sensor_msgs/msg/camera_info.hpp"
|
||||
#include "sensor_msgs/msg/image.hpp"
|
||||
#include "tf2_ros/buffer.h"
|
||||
#include "tf2_ros/transform_listener.h"
|
||||
|
||||
namespace saltybot_depth_costmap
|
||||
{
|
||||
|
||||
struct ObstaclePoint
|
||||
{
|
||||
double wx; // world x (m)
|
||||
double wy; // world y (m)
|
||||
};
|
||||
|
||||
/**
|
||||
* DepthCostmapLayer — Nav2 costmap2d plugin (Issue #532)
|
||||
*
|
||||
* Converts RealSense D435i depth images to a Nav2 costmap layer.
|
||||
*
|
||||
* Pipeline:
|
||||
* 1. Subscribe to /camera/depth/image_rect_raw (16UC1, mm) + camera_info
|
||||
* 2. Back-project depth pixels to 3D using camera intrinsics
|
||||
* 3. Transform points to costmap global_frame via TF2
|
||||
* 4. Apply height filter (min_height .. max_height above ground)
|
||||
* 5. Mark obstacle cells as LETHAL_OBSTACLE
|
||||
* 6. Inflate nearby cells as INSCRIBED_INFLATED_OBSTACLE (in-layer inflation)
|
||||
*
|
||||
* Nav2 yaml usage:
|
||||
* local_costmap:
|
||||
* plugins: ["obstacle_layer", "voxel_layer", "depth_costmap_layer", "inflation_layer"]
|
||||
* depth_costmap_layer:
|
||||
* plugin: "saltybot_depth_costmap::DepthCostmapLayer"
|
||||
* enabled: true
|
||||
* depth_topic: /camera/depth/image_rect_raw
|
||||
* camera_info_topic: /camera/depth/camera_info
|
||||
* min_height: 0.05 # m — floor clearance
|
||||
* max_height: 0.80 # m — ceiling cutoff
|
||||
* obstacle_range: 3.5 # m — max marking range
|
||||
* clearing_range: 4.0 # m — max clearing range
|
||||
* inflation_radius: 0.10 # m — in-layer inflation radius
|
||||
* downsample_factor: 4 # process 1 of N rows/cols
|
||||
*/
|
||||
class DepthCostmapLayer : public nav2_costmap_2d::Layer
|
||||
{
|
||||
public:
|
||||
DepthCostmapLayer();
|
||||
~DepthCostmapLayer() override = default;
|
||||
|
||||
void onInitialize() override;
|
||||
|
||||
void updateBounds(
|
||||
double robot_x, double robot_y, double robot_yaw,
|
||||
double * min_x, double * min_y,
|
||||
double * max_x, double * max_y) override;
|
||||
|
||||
void updateCosts(
|
||||
nav2_costmap_2d::Costmap2D & master_grid,
|
||||
int min_i, int min_j, int max_i, int max_j) override;
|
||||
|
||||
void reset() override;
|
||||
bool isClearable() override { return true; }
|
||||
|
||||
private:
|
||||
void depthCallback(const sensor_msgs::msg::Image::SharedPtr msg);
|
||||
void cameraInfoCallback(const sensor_msgs::msg::CameraInfo::SharedPtr msg);
|
||||
void processDepthImage(const sensor_msgs::msg::Image::SharedPtr & img);
|
||||
|
||||
// ROS interfaces
|
||||
rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr depth_sub_;
|
||||
rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::SharedPtr info_sub_;
|
||||
std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
|
||||
std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
|
||||
|
||||
// Camera intrinsics (from camera_info)
|
||||
double fx_{0.0}, fy_{0.0}, cx_{0.0}, cy_{0.0};
|
||||
bool have_camera_info_{false};
|
||||
std::string camera_frame_;
|
||||
|
||||
// Plugin parameters
|
||||
std::string depth_topic_;
|
||||
std::string camera_info_topic_;
|
||||
double min_height_{0.05}; // m — filter floor
|
||||
double max_height_{0.80}; // m — filter ceiling
|
||||
double obstacle_range_{3.5}; // m — max marking range (from camera)
|
||||
double clearing_range_{4.0}; // m — max clearing range
|
||||
double inflation_radius_{0.10}; // m — in-layer inflation
|
||||
int downsample_factor_{4}; // skip N-1 of every N pixels
|
||||
|
||||
// Latest processed obstacle points in world frame
|
||||
std::vector<ObstaclePoint> obstacle_points_;
|
||||
std::mutex points_mutex_;
|
||||
bool need_update_{false};
|
||||
|
||||
// Dirty bounds for updateBounds()
|
||||
double last_min_x_, last_min_y_, last_max_x_, last_max_y_;
|
||||
|
||||
// For throttled logging outside onInitialize
|
||||
rclcpp::Logger logger_;
|
||||
rclcpp::Clock::SharedPtr clock_;
|
||||
};
|
||||
|
||||
} // namespace saltybot_depth_costmap
|
||||
30
jetson/ros2_ws/src/saltybot_depth_costmap/package.xml
Normal file
30
jetson/ros2_ws/src/saltybot_depth_costmap/package.xml
Normal file
@ -0,0 +1,30 @@
|
||||
<?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_depth_costmap</name>
|
||||
<version>0.1.0</version>
|
||||
<description>
|
||||
Nav2 costmap2d plugin: DepthCostmapLayer (Issue #532).
|
||||
Converts RealSense D435i depth images to a Nav2 costmap layer with
|
||||
configurable height filtering and in-layer obstacle inflation.
|
||||
</description>
|
||||
<maintainer email="seb@vayrette.com">seb</maintainer>
|
||||
<license>MIT</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<depend>rclcpp</depend>
|
||||
<depend>nav2_costmap_2d</depend>
|
||||
<depend>sensor_msgs</depend>
|
||||
<depend>geometry_msgs</depend>
|
||||
<depend>tf2_ros</depend>
|
||||
<depend>tf2</depend>
|
||||
<depend>pluginlib</depend>
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
15
jetson/ros2_ws/src/saltybot_depth_costmap/plugin.xml
Normal file
15
jetson/ros2_ws/src/saltybot_depth_costmap/plugin.xml
Normal file
@ -0,0 +1,15 @@
|
||||
<library path="saltybot_depth_costmap">
|
||||
<class
|
||||
name="saltybot_depth_costmap::DepthCostmapLayer"
|
||||
type="saltybot_depth_costmap::DepthCostmapLayer"
|
||||
base_class_type="nav2_costmap_2d::Layer">
|
||||
<description>
|
||||
Nav2 costmap layer that converts RealSense D435i depth images into
|
||||
obstacle markings. Back-projects depth pixels to 3D, transforms to the
|
||||
costmap global frame via TF2, applies a configurable height filter
|
||||
(min_height .. max_height), marks lethal obstacles, and inflates them
|
||||
by inflation_radius within the layer. Integrates with existing Nav2
|
||||
costmap config (Issue #478).
|
||||
</description>
|
||||
</class>
|
||||
</library>
|
||||
@ -0,0 +1,306 @@
|
||||
#include "saltybot_depth_costmap/depth_costmap_layer.hpp"
|
||||
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
#include <cstring>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include "nav2_costmap_2d/costmap_2d.hpp"
|
||||
#include "pluginlib/class_list_macros.hpp"
|
||||
#include "tf2/exceptions.h"
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(
|
||||
saltybot_depth_costmap::DepthCostmapLayer,
|
||||
nav2_costmap_2d::Layer)
|
||||
|
||||
namespace saltybot_depth_costmap
|
||||
{
|
||||
|
||||
using nav2_costmap_2d::FREE_SPACE;
|
||||
using nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE;
|
||||
using nav2_costmap_2d::LETHAL_OBSTACLE;
|
||||
using nav2_costmap_2d::NO_INFORMATION;
|
||||
|
||||
DepthCostmapLayer::DepthCostmapLayer()
|
||||
: last_min_x_(-1e9), last_min_y_(-1e9),
|
||||
last_max_x_(1e9), last_max_y_(1e9),
|
||||
logger_(rclcpp::get_logger("DepthCostmapLayer"))
|
||||
{}
|
||||
|
||||
// ── onInitialize ──────────────────────────────────────────────────────────────
|
||||
|
||||
void DepthCostmapLayer::onInitialize()
|
||||
{
|
||||
auto node = node_.lock();
|
||||
if (!node) {
|
||||
throw std::runtime_error("DepthCostmapLayer: node handle expired");
|
||||
}
|
||||
|
||||
logger_ = node->get_logger();
|
||||
clock_ = node->get_clock();
|
||||
|
||||
declareParameter("enabled", rclcpp::ParameterValue(true));
|
||||
declareParameter("depth_topic", rclcpp::ParameterValue(
|
||||
std::string("/camera/depth/image_rect_raw")));
|
||||
declareParameter("camera_info_topic", rclcpp::ParameterValue(
|
||||
std::string("/camera/depth/camera_info")));
|
||||
declareParameter("min_height", rclcpp::ParameterValue(0.05));
|
||||
declareParameter("max_height", rclcpp::ParameterValue(0.80));
|
||||
declareParameter("obstacle_range", rclcpp::ParameterValue(3.5));
|
||||
declareParameter("clearing_range", rclcpp::ParameterValue(4.0));
|
||||
declareParameter("inflation_radius", rclcpp::ParameterValue(0.10));
|
||||
declareParameter("downsample_factor", rclcpp::ParameterValue(4));
|
||||
|
||||
enabled_ = node->get_parameter(name_ + ".enabled").as_bool();
|
||||
depth_topic_ = node->get_parameter(name_ + ".depth_topic").as_string();
|
||||
camera_info_topic_ = node->get_parameter(name_ + ".camera_info_topic").as_string();
|
||||
min_height_ = node->get_parameter(name_ + ".min_height").as_double();
|
||||
max_height_ = node->get_parameter(name_ + ".max_height").as_double();
|
||||
obstacle_range_ = node->get_parameter(name_ + ".obstacle_range").as_double();
|
||||
clearing_range_ = node->get_parameter(name_ + ".clearing_range").as_double();
|
||||
inflation_radius_ = node->get_parameter(name_ + ".inflation_radius").as_double();
|
||||
downsample_factor_ = node->get_parameter(name_ + ".downsample_factor").as_int();
|
||||
|
||||
if (downsample_factor_ < 1) downsample_factor_ = 1;
|
||||
|
||||
RCLCPP_INFO(
|
||||
logger_,
|
||||
"DepthCostmapLayer '%s': depth=%s height=[%.2f, %.2f]m range=%.1fm inflate=%.2fm ds=%d",
|
||||
name_.c_str(), depth_topic_.c_str(),
|
||||
min_height_, max_height_, obstacle_range_, inflation_radius_, downsample_factor_);
|
||||
|
||||
// TF2
|
||||
tf_buffer_ = std::make_shared<tf2_ros::Buffer>(clock_);
|
||||
tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
|
||||
|
||||
// Subscriptions
|
||||
depth_sub_ = node->create_subscription<sensor_msgs::msg::Image>(
|
||||
depth_topic_, rclcpp::SensorDataQoS(),
|
||||
std::bind(&DepthCostmapLayer::depthCallback, this, std::placeholders::_1));
|
||||
|
||||
info_sub_ = node->create_subscription<sensor_msgs::msg::CameraInfo>(
|
||||
camera_info_topic_, rclcpp::QoS(1).reliable(),
|
||||
std::bind(&DepthCostmapLayer::cameraInfoCallback, this, std::placeholders::_1));
|
||||
|
||||
current_ = true;
|
||||
}
|
||||
|
||||
// ── cameraInfoCallback ────────────────────────────────────────────────────────
|
||||
|
||||
void DepthCostmapLayer::cameraInfoCallback(
|
||||
const sensor_msgs::msg::CameraInfo::SharedPtr msg)
|
||||
{
|
||||
if (have_camera_info_) return; // Intrinsics are stable — only need once
|
||||
|
||||
// Pinhole intrinsics from K matrix (row-major 3x3)
|
||||
fx_ = msg->k[0]; // K[0,0]
|
||||
fy_ = msg->k[4]; // K[1,1]
|
||||
cx_ = msg->k[2]; // K[0,2]
|
||||
cy_ = msg->k[5]; // K[1,2]
|
||||
camera_frame_ = msg->header.frame_id;
|
||||
|
||||
if (fx_ <= 0.0 || fy_ <= 0.0) {
|
||||
RCLCPP_WARN(logger_, "DepthCostmapLayer: invalid camera intrinsics (fx=%.1f fy=%.1f)", fx_, fy_);
|
||||
return;
|
||||
}
|
||||
|
||||
have_camera_info_ = true;
|
||||
RCLCPP_INFO(
|
||||
logger_,
|
||||
"DepthCostmapLayer: camera_info received — frame=%s fx=%.1f fy=%.1f cx=%.1f cy=%.1f",
|
||||
camera_frame_.c_str(), fx_, fy_, cx_, cy_);
|
||||
}
|
||||
|
||||
// ── depthCallback / processDepthImage ────────────────────────────────────────
|
||||
|
||||
void DepthCostmapLayer::depthCallback(
|
||||
const sensor_msgs::msg::Image::SharedPtr msg)
|
||||
{
|
||||
if (!enabled_ || !have_camera_info_) return;
|
||||
processDepthImage(msg);
|
||||
}
|
||||
|
||||
void DepthCostmapLayer::processDepthImage(
|
||||
const sensor_msgs::msg::Image::SharedPtr & img)
|
||||
{
|
||||
// Resolve encoding
|
||||
bool is_16u = (img->encoding == "16UC1" || img->encoding == "mono16");
|
||||
bool is_32f = (img->encoding == "32FC1");
|
||||
if (!is_16u && !is_32f) {
|
||||
RCLCPP_WARN_ONCE(logger_, "DepthCostmapLayer: unsupported encoding '%s'", img->encoding.c_str());
|
||||
return;
|
||||
}
|
||||
|
||||
// Get TF from camera → costmap global frame (latest available)
|
||||
const std::string global_frame = layered_costmap_->getGlobalFrameID();
|
||||
geometry_msgs::msg::TransformStamped tf_stamped;
|
||||
try {
|
||||
tf_stamped = tf_buffer_->lookupTransform(
|
||||
global_frame, camera_frame_,
|
||||
rclcpp::Time(0),
|
||||
rclcpp::Duration::from_seconds(0.1));
|
||||
} catch (const tf2::TransformException & ex) {
|
||||
RCLCPP_WARN_THROTTLE(logger_, *clock_, 5000,
|
||||
"DepthCostmapLayer: TF %s→%s failed: %s",
|
||||
camera_frame_.c_str(), global_frame.c_str(), ex.what());
|
||||
return;
|
||||
}
|
||||
|
||||
// Build rotation matrix from quaternion (camera frame → world frame)
|
||||
// p_world = R * p_cam + t
|
||||
const auto & t = tf_stamped.transform.translation;
|
||||
const auto & q = tf_stamped.transform.rotation;
|
||||
double qw = q.w, qx = q.x, qy = q.y, qz = q.z;
|
||||
|
||||
double R[3][3] = {
|
||||
{1.0 - 2.0*(qy*qy + qz*qz), 2.0*(qx*qy - qz*qw), 2.0*(qx*qz + qy*qw)},
|
||||
{ 2.0*(qx*qy + qz*qw), 1.0 - 2.0*(qx*qx + qz*qz), 2.0*(qy*qz - qx*qw)},
|
||||
{ 2.0*(qx*qz - qy*qw), 2.0*(qy*qz + qx*qw), 1.0 - 2.0*(qx*qx + qy*qy)}
|
||||
};
|
||||
|
||||
int rows = static_cast<int>(img->height);
|
||||
int cols = static_cast<int>(img->width);
|
||||
int step = static_cast<int>(img->step);
|
||||
|
||||
const double obstacle_range_sq = obstacle_range_ * obstacle_range_;
|
||||
|
||||
std::vector<ObstaclePoint> new_points;
|
||||
new_points.reserve(512);
|
||||
|
||||
for (int v = 0; v < rows; v += downsample_factor_) {
|
||||
const uint8_t * row_ptr = img->data.data() + v * step;
|
||||
|
||||
for (int u = 0; u < cols; u += downsample_factor_) {
|
||||
double depth_m = 0.0;
|
||||
|
||||
if (is_16u) {
|
||||
uint16_t raw;
|
||||
std::memcpy(&raw, row_ptr + u * 2, sizeof(raw));
|
||||
if (raw == 0) continue;
|
||||
depth_m = raw * 0.001; // mm → m
|
||||
} else {
|
||||
float raw;
|
||||
std::memcpy(&raw, row_ptr + u * 4, sizeof(raw));
|
||||
if (!std::isfinite(raw) || raw <= 0.0f) continue;
|
||||
depth_m = static_cast<double>(raw);
|
||||
}
|
||||
|
||||
if (depth_m > clearing_range_) continue;
|
||||
|
||||
// Back-project: depth optical frame (Z forward, X right, Y down)
|
||||
double xc = (u - cx_) * depth_m / fx_;
|
||||
double yc = (v - cy_) * depth_m / fy_;
|
||||
double zc = depth_m;
|
||||
|
||||
// Distance check in camera frame (cheaper than world-frame dist)
|
||||
double dist2 = xc*xc + yc*yc + zc*zc;
|
||||
if (dist2 > obstacle_range_sq) continue;
|
||||
|
||||
// Transform to world frame
|
||||
double xw = R[0][0]*xc + R[0][1]*yc + R[0][2]*zc + t.x;
|
||||
double yw = R[1][0]*xc + R[1][1]*yc + R[1][2]*zc + t.y;
|
||||
double zw = R[2][0]*xc + R[2][1]*yc + R[2][2]*zc + t.z;
|
||||
|
||||
// Height filter (zw = height above ground in world frame)
|
||||
if (zw < min_height_ || zw > max_height_) continue;
|
||||
|
||||
new_points.push_back({xw, yw});
|
||||
}
|
||||
}
|
||||
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(points_mutex_);
|
||||
obstacle_points_ = std::move(new_points);
|
||||
need_update_ = true;
|
||||
}
|
||||
}
|
||||
|
||||
// ── updateBounds ──────────────────────────────────────────────────────────────
|
||||
|
||||
void DepthCostmapLayer::updateBounds(
|
||||
double robot_x, double robot_y, double /*robot_yaw*/,
|
||||
double * min_x, double * min_y,
|
||||
double * max_x, double * max_y)
|
||||
{
|
||||
if (!enabled_) return;
|
||||
|
||||
// Mark region around robot that depth camera can reach
|
||||
double margin = obstacle_range_ + inflation_radius_;
|
||||
last_min_x_ = robot_x - margin;
|
||||
last_min_y_ = robot_y - margin;
|
||||
last_max_x_ = robot_x + margin;
|
||||
last_max_y_ = robot_y + margin;
|
||||
|
||||
*min_x = std::min(*min_x, last_min_x_);
|
||||
*min_y = std::min(*min_y, last_min_y_);
|
||||
*max_x = std::max(*max_x, last_max_x_);
|
||||
*max_y = std::max(*max_y, last_max_y_);
|
||||
}
|
||||
|
||||
// ── updateCosts ───────────────────────────────────────────────────────────────
|
||||
|
||||
void DepthCostmapLayer::updateCosts(
|
||||
nav2_costmap_2d::Costmap2D & master,
|
||||
int min_i, int min_j, int max_i, int max_j)
|
||||
{
|
||||
if (!enabled_) return;
|
||||
|
||||
std::lock_guard<std::mutex> lock(points_mutex_);
|
||||
if (!need_update_ || obstacle_points_.empty()) return;
|
||||
|
||||
double res = master.getResolution();
|
||||
// Number of cells to inflate in each direction
|
||||
int inflate_cells = (inflation_radius_ > 0.0)
|
||||
? static_cast<int>(std::ceil(inflation_radius_ / res))
|
||||
: 0;
|
||||
|
||||
for (const auto & pt : obstacle_points_) {
|
||||
unsigned int mx, my;
|
||||
if (!master.worldToMap(pt.wx, pt.wy, mx, my)) continue;
|
||||
|
||||
int ci = static_cast<int>(mx);
|
||||
int cj = static_cast<int>(my);
|
||||
|
||||
// Mark the obstacle cell as lethal
|
||||
if (ci >= min_i && ci < max_i && cj >= min_j && cj < max_j) {
|
||||
master.setCost(mx, my, LETHAL_OBSTACLE);
|
||||
}
|
||||
|
||||
// In-layer inflation: mark neighbouring cells within inflation_radius
|
||||
// as INSCRIBED_INFLATED_OBSTACLE (if not already lethal)
|
||||
for (int di = -inflate_cells; di <= inflate_cells; ++di) {
|
||||
for (int dj = -inflate_cells; dj <= inflate_cells; ++dj) {
|
||||
if (di == 0 && dj == 0) continue;
|
||||
|
||||
double dist = std::hypot(static_cast<double>(di), static_cast<double>(dj)) * res;
|
||||
if (dist > inflation_radius_) continue;
|
||||
|
||||
int ni = ci + di;
|
||||
int nj = cj + dj;
|
||||
if (ni < min_i || ni >= max_i || nj < min_j || nj >= max_j) continue;
|
||||
|
||||
auto uni = static_cast<unsigned int>(ni);
|
||||
auto unj = static_cast<unsigned int>(nj);
|
||||
unsigned char existing = master.getCost(uni, unj);
|
||||
if (existing < INSCRIBED_INFLATED_OBSTACLE) {
|
||||
master.setCost(uni, unj, INSCRIBED_INFLATED_OBSTACLE);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
need_update_ = false;
|
||||
}
|
||||
|
||||
// ── reset ─────────────────────────────────────────────────────────────────────
|
||||
|
||||
void DepthCostmapLayer::reset()
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(points_mutex_);
|
||||
obstacle_points_.clear();
|
||||
need_update_ = false;
|
||||
current_ = true;
|
||||
}
|
||||
|
||||
} // namespace saltybot_depth_costmap
|
||||
@ -16,3 +16,8 @@ pid_autotune:
|
||||
|
||||
# Time to allow system to settle before starting relay (seconds)
|
||||
settle_time: 2.0
|
||||
|
||||
# JLink serial port for sending computed gains to FC (Issue #531)
|
||||
# USART1 on FC maps to this ttyTHS device on Jetson Orin
|
||||
jlink_serial_port: "/dev/ttyTHS0"
|
||||
jlink_baud_rate: 921600
|
||||
|
||||
@ -2,7 +2,9 @@
|
||||
"""PID auto-tuner for SaltyBot using Astrom-Hagglund relay feedback method.
|
||||
|
||||
Service-triggered relay oscillation measures ultimate gain (Ku) and ultimate period
|
||||
(Tu), then computes Ziegler-Nichols PD gains. Aborts on >25deg tilt for safety.
|
||||
(Tu), then computes Ziegler-Nichols PID gains (Kp, Ki, Kd). After tuning the computed
|
||||
gains are written to the FC via JLink binary protocol (JLINK_CMD_PID_SET) and
|
||||
immediately saved to flash (JLINK_CMD_PID_SAVE). Aborts on >25deg tilt for safety.
|
||||
|
||||
Service:
|
||||
/saltybot/autotune_pid (std_srvs/Trigger) - Start auto-tuning process
|
||||
@ -18,18 +20,53 @@ Subscribed topics:
|
||||
|
||||
import json
|
||||
import math
|
||||
import struct
|
||||
import time
|
||||
from enum import Enum
|
||||
from typing import Optional, Tuple, List
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from rclpy.service import Service
|
||||
from geometry_msgs.msg import Twist
|
||||
from sensor_msgs.msg import Imu
|
||||
from std_msgs.msg import Float32, String
|
||||
from std_srvs.srv import Trigger
|
||||
|
||||
try:
|
||||
import serial
|
||||
_SERIAL_AVAILABLE = True
|
||||
except ImportError:
|
||||
_SERIAL_AVAILABLE = False
|
||||
|
||||
|
||||
# ── JLink binary protocol constants (must match jlink.h) ─────────────────────
|
||||
JLINK_STX = 0x02
|
||||
JLINK_ETX = 0x03
|
||||
JLINK_CMD_PID_SET = 0x05
|
||||
JLINK_CMD_PID_SAVE = 0x0A
|
||||
|
||||
|
||||
def _crc16_xmodem(data: bytes) -> int:
|
||||
"""CRC16-XModem (poly 0x1021, init 0x0000) — matches FC implementation."""
|
||||
crc = 0x0000
|
||||
for byte in data:
|
||||
crc ^= byte << 8
|
||||
for _ in range(8):
|
||||
if crc & 0x8000:
|
||||
crc = (crc << 1) ^ 0x1021
|
||||
else:
|
||||
crc <<= 1
|
||||
crc &= 0xFFFF
|
||||
return crc
|
||||
|
||||
|
||||
def _build_jlink_frame(cmd: int, payload: bytes) -> bytes:
|
||||
"""Build a JLink binary frame: [STX][LEN][CMD][PAYLOAD][CRC_hi][CRC_lo][ETX]."""
|
||||
data = bytes([cmd]) + payload
|
||||
length = len(data)
|
||||
crc = _crc16_xmodem(data)
|
||||
return bytes([JLINK_STX, length]) + data + bytes([crc >> 8, crc & 0xFF, JLINK_ETX])
|
||||
|
||||
|
||||
class TuneState(Enum):
|
||||
"""State machine for auto-tuning process."""
|
||||
@ -47,24 +84,28 @@ class PIDAutotuneNode(Node):
|
||||
super().__init__("pid_autotune")
|
||||
|
||||
# Parameters
|
||||
self.declare_parameter("relay_magnitude", 0.3) # Relay on-off magnitude
|
||||
self.declare_parameter("tilt_safety_limit", 25.0) # degrees
|
||||
self.declare_parameter("max_tune_duration", 120.0) # seconds
|
||||
self.declare_parameter("oscillation_count", 5) # Number of cycles to measure
|
||||
self.declare_parameter("settle_time", 2.0) # Settle before starting relay
|
||||
self.declare_parameter("relay_magnitude", 0.3)
|
||||
self.declare_parameter("tilt_safety_limit", 25.0)
|
||||
self.declare_parameter("max_tune_duration", 120.0)
|
||||
self.declare_parameter("oscillation_count", 5)
|
||||
self.declare_parameter("settle_time", 2.0)
|
||||
self.declare_parameter("jlink_serial_port", "/dev/ttyTHS0")
|
||||
self.declare_parameter("jlink_baud_rate", 921600)
|
||||
|
||||
self.relay_magnitude = self.get_parameter("relay_magnitude").value
|
||||
self.relay_magnitude = self.get_parameter("relay_magnitude").value
|
||||
self.tilt_safety_limit = self.get_parameter("tilt_safety_limit").value
|
||||
self.max_tune_duration = self.get_parameter("max_tune_duration").value
|
||||
self.oscillation_count = self.get_parameter("oscillation_count").value
|
||||
self.settle_time = self.get_parameter("settle_time").value
|
||||
self.settle_time = self.get_parameter("settle_time").value
|
||||
self._jlink_port = self.get_parameter("jlink_serial_port").value
|
||||
self._jlink_baud = self.get_parameter("jlink_baud_rate").value
|
||||
|
||||
# State tracking
|
||||
# State
|
||||
self.tune_state = TuneState.IDLE
|
||||
self.last_imu_tilt = 0.0
|
||||
self.last_feedback = 0.0
|
||||
self.feedback_history: List[Tuple[float, float]] = [] # (time, feedback)
|
||||
self.peaks: List[Tuple[float, float]] = [] # (time, peak_value)
|
||||
self.feedback_history: List[Tuple[float, float]] = []
|
||||
self.peaks: List[Tuple[float, float]] = []
|
||||
self.start_time = None
|
||||
|
||||
# Subscriptions
|
||||
@ -74,7 +115,9 @@ class PIDAutotuneNode(Node):
|
||||
)
|
||||
|
||||
# Publications
|
||||
self.pub_cmd_vel = self.create_publisher(Twist, "/saltybot/autotune_cmd_vel", 10)
|
||||
self.pub_cmd_vel = self.create_publisher(
|
||||
Twist, "/saltybot/autotune_cmd_vel", 10
|
||||
)
|
||||
self.pub_info = self.create_publisher(String, "/saltybot/autotune_info", 10)
|
||||
|
||||
# Service
|
||||
@ -86,31 +129,29 @@ class PIDAutotuneNode(Node):
|
||||
"PID auto-tuner initialized. Service: /saltybot/autotune_pid\n"
|
||||
f"Relay magnitude: {self.relay_magnitude}, "
|
||||
f"Tilt safety limit: {self.tilt_safety_limit}°, "
|
||||
f"Max duration: {self.max_tune_duration}s"
|
||||
f"Max duration: {self.max_tune_duration}s, "
|
||||
f"JLink port: {self._jlink_port}"
|
||||
)
|
||||
|
||||
# ── IMU / feedback ────────────────────────────────────────────────────────
|
||||
|
||||
def _on_imu(self, msg: Imu) -> None:
|
||||
"""Extract tilt angle from IMU."""
|
||||
# Approximate tilt from acceleration (pitch angle)
|
||||
# pitch = atan2(ax, sqrt(ay^2 + az^2))
|
||||
accel = msg.linear_acceleration
|
||||
self.last_imu_tilt = math.degrees(
|
||||
math.atan2(accel.x, math.sqrt(accel.y**2 + accel.z**2))
|
||||
)
|
||||
|
||||
def _on_feedback(self, msg: Float32) -> None:
|
||||
"""Record balance feedback for oscillation measurement."""
|
||||
now = self.get_clock().now().nanoseconds / 1e9
|
||||
self.last_feedback = msg.data
|
||||
|
||||
# During tuning, record history
|
||||
if self.tune_state in (TuneState.MEASURING,):
|
||||
if self.tune_state == TuneState.MEASURING:
|
||||
self.feedback_history.append((now, msg.data))
|
||||
|
||||
# ── Service handler ───────────────────────────────────────────────────────
|
||||
|
||||
def _handle_autotune_request(
|
||||
self, request: Trigger.Request, response: Trigger.Response
|
||||
) -> Trigger.Response:
|
||||
"""Handle service request to start auto-tuning."""
|
||||
if self.tune_state != TuneState.IDLE:
|
||||
response.success = False
|
||||
response.message = f"Already tuning (state: {self.tune_state.name})"
|
||||
@ -122,15 +163,15 @@ class PIDAutotuneNode(Node):
|
||||
self.peaks = []
|
||||
self.start_time = self.get_clock().now().nanoseconds / 1e9
|
||||
|
||||
# Start tuning process
|
||||
self._run_autotuning()
|
||||
|
||||
response.success = self.tune_state == TuneState.COMPLETE
|
||||
response.message = f"Tuning {self.tune_state.name}"
|
||||
return response
|
||||
|
||||
# ── Tuning sequence ───────────────────────────────────────────────────────
|
||||
|
||||
def _run_autotuning(self) -> None:
|
||||
"""Execute the complete auto-tuning sequence."""
|
||||
# Phase 1: Settle
|
||||
self.get_logger().info("Phase 1: Settling...")
|
||||
if not self._settle():
|
||||
@ -150,7 +191,7 @@ class PIDAutotuneNode(Node):
|
||||
)
|
||||
return
|
||||
|
||||
# Phase 3: Analysis
|
||||
# Phase 3: Analyse
|
||||
self.get_logger().info("Phase 3: Analyzing measurements...")
|
||||
ku, tu = self._analyze_oscillation()
|
||||
if ku is None or tu is None:
|
||||
@ -160,10 +201,12 @@ class PIDAutotuneNode(Node):
|
||||
)
|
||||
return
|
||||
|
||||
# Phase 4: Compute gains
|
||||
kp, kd = self._compute_ziegler_nichols(ku, tu)
|
||||
# Phase 4: Compute ZN gains
|
||||
kp, ki, kd = self._compute_ziegler_nichols(ku, tu)
|
||||
|
||||
# Phase 5: Push gains to FC and save to flash
|
||||
saved = self._send_pid_to_fc(kp, ki, kd)
|
||||
|
||||
# Done
|
||||
self.tune_state = TuneState.COMPLETE
|
||||
self._publish_info(
|
||||
{
|
||||
@ -171,35 +214,33 @@ class PIDAutotuneNode(Node):
|
||||
"ku": ku,
|
||||
"tu": tu,
|
||||
"kp": kp,
|
||||
"ki": ki,
|
||||
"kd": kd,
|
||||
"flash_saved": saved,
|
||||
}
|
||||
)
|
||||
self.get_logger().info(
|
||||
f"Auto-tune complete: Ku={ku:.4f}, Tu={tu:.2f}s, Kp={kp:.4f}, Kd={kd:.4f}"
|
||||
f"Auto-tune complete: Ku={ku:.4f}, Tu={tu:.2f}s | "
|
||||
f"Kp={kp:.4f}, Ki={ki:.4f}, Kd={kd:.4f} | flash_saved={saved}"
|
||||
)
|
||||
|
||||
def _settle(self) -> bool:
|
||||
"""Allow system to settle before starting relay."""
|
||||
settle_start = self.get_clock().now().nanoseconds / 1e9
|
||||
relay_off = Twist() # Zero command
|
||||
relay_off = Twist()
|
||||
|
||||
while True:
|
||||
now = self.get_clock().now().nanoseconds / 1e9
|
||||
elapsed = now - settle_start
|
||||
|
||||
# Safety check
|
||||
if abs(self.last_imu_tilt) > self.tilt_safety_limit:
|
||||
self.get_logger().error(
|
||||
f"Safety abort: Tilt {self.last_imu_tilt:.1f}° > {self.tilt_safety_limit}°"
|
||||
f"Safety abort: Tilt {self.last_imu_tilt:.1f}° > "
|
||||
f"{self.tilt_safety_limit}°"
|
||||
)
|
||||
return False
|
||||
|
||||
# Timeout check
|
||||
if elapsed > self.max_tune_duration:
|
||||
self.get_logger().error("Settling timeout")
|
||||
return False
|
||||
|
||||
# Continue settling
|
||||
if elapsed > self.settle_time:
|
||||
return True
|
||||
|
||||
@ -207,57 +248,47 @@ class PIDAutotuneNode(Node):
|
||||
time.sleep(0.01)
|
||||
|
||||
def _relay_oscillation(self) -> bool:
|
||||
"""Apply relay control to induce oscillation."""
|
||||
relay_on = Twist()
|
||||
relay_on.linear.x = self.relay_magnitude
|
||||
|
||||
relay_off = Twist()
|
||||
|
||||
oscillation_start = self.get_clock().now().nanoseconds / 1e9
|
||||
relay_state = True # True = relay ON, False = relay OFF
|
||||
last_switch = oscillation_start
|
||||
relay_state = True
|
||||
cycles_measured = 0
|
||||
last_peak_value = None
|
||||
|
||||
while cycles_measured < self.oscillation_count:
|
||||
now = self.get_clock().now().nanoseconds / 1e9
|
||||
elapsed = now - oscillation_start
|
||||
|
||||
# Safety checks
|
||||
if abs(self.last_imu_tilt) > self.tilt_safety_limit:
|
||||
self.get_logger().error(
|
||||
f"Safety abort: Tilt {self.last_imu_tilt:.1f}° > {self.tilt_safety_limit}°"
|
||||
f"Safety abort: Tilt {self.last_imu_tilt:.1f}° > "
|
||||
f"{self.tilt_safety_limit}°"
|
||||
)
|
||||
return False
|
||||
|
||||
if elapsed > self.max_tune_duration:
|
||||
self.get_logger().error("Relay oscillation timeout")
|
||||
return False
|
||||
|
||||
# Switch relay at zero crossings
|
||||
# Switch relay at zero crossings of balance feedback
|
||||
if relay_state and self.last_feedback < 0:
|
||||
relay_state = False
|
||||
self.peaks.append((now, self.last_feedback))
|
||||
cycles_measured += 1
|
||||
last_switch = now
|
||||
self.get_logger().debug(
|
||||
f"Cycle {cycles_measured}: Peak at {now - oscillation_start:.2f}s = {self.last_feedback:.4f}"
|
||||
f"Cycle {cycles_measured}: peak={self.last_feedback:.4f} "
|
||||
f"t={elapsed:.2f}s"
|
||||
)
|
||||
|
||||
elif not relay_state and self.last_feedback > 0:
|
||||
relay_state = True
|
||||
self.peaks.append((now, self.last_feedback))
|
||||
cycles_measured += 1
|
||||
last_switch = now
|
||||
self.get_logger().debug(
|
||||
f"Cycle {cycles_measured}: Peak at {now - oscillation_start:.2f}s = {self.last_feedback:.4f}"
|
||||
f"Cycle {cycles_measured}: peak={self.last_feedback:.4f} "
|
||||
f"t={elapsed:.2f}s"
|
||||
)
|
||||
|
||||
# Publish relay command
|
||||
cmd = relay_on if relay_state else relay_off
|
||||
self.pub_cmd_vel.publish(cmd)
|
||||
|
||||
# Report progress
|
||||
self.pub_cmd_vel.publish(relay_on if relay_state else relay_off)
|
||||
self._publish_info(
|
||||
{
|
||||
"status": "measuring",
|
||||
@ -266,25 +297,24 @@ class PIDAutotuneNode(Node):
|
||||
"tilt": self.last_imu_tilt,
|
||||
}
|
||||
)
|
||||
|
||||
time.sleep(0.01)
|
||||
|
||||
# Stop relay
|
||||
self.pub_cmd_vel.publish(relay_off)
|
||||
return len(self.peaks) >= self.oscillation_count
|
||||
|
||||
def _analyze_oscillation(self) -> Tuple[Optional[float], Optional[float]]:
|
||||
"""Calculate Ku and Tu from measured peaks."""
|
||||
def _analyze_oscillation(
|
||||
self,
|
||||
) -> Tuple[Optional[float], Optional[float]]:
|
||||
if len(self.peaks) < 3:
|
||||
self.get_logger().error(f"Insufficient peaks: {len(self.peaks)}")
|
||||
return None, None
|
||||
|
||||
# Ku = 4*relay_magnitude / (pi * |peak|)
|
||||
# Ku = 4·d / (π·|a|) where d = relay magnitude, a = average peak amplitude
|
||||
peak_values = [abs(p[1]) for p in self.peaks]
|
||||
avg_peak = sum(peak_values) / len(peak_values)
|
||||
ku = (4.0 * self.relay_magnitude) / (math.pi * avg_peak)
|
||||
|
||||
# Tu = 2 * (time between peaks) - average period
|
||||
# Tu = 2 × mean half-period (time between consecutive zero crossings)
|
||||
peak_times = [p[0] for p in self.peaks]
|
||||
time_diffs = [
|
||||
peak_times[i + 1] - peak_times[i] for i in range(len(peak_times) - 1)
|
||||
@ -297,26 +327,73 @@ class PIDAutotuneNode(Node):
|
||||
return ku, tu
|
||||
|
||||
@staticmethod
|
||||
def _compute_ziegler_nichols(ku: float, tu: float) -> Tuple[float, float]:
|
||||
"""Compute Ziegler-Nichols PD gains.
|
||||
def _compute_ziegler_nichols(
|
||||
ku: float, tu: float
|
||||
) -> Tuple[float, float, float]:
|
||||
"""Ziegler-Nichols PID gains from relay feedback critical params.
|
||||
|
||||
For PD controller:
|
||||
Kp = 0.6 * Ku
|
||||
Kd = 0.075 * Ku * Tu (approximately Kp*Tu/8)
|
||||
Standard ZN PID table:
|
||||
Kp = 0.60 · Ku
|
||||
Ki = 1.20 · Ku / Tu (= 2·Kp / Tu)
|
||||
Kd = 0.075 · Ku · Tu (= Kp·Tu / 8)
|
||||
"""
|
||||
kp = 0.6 * ku
|
||||
kp = 0.60 * ku
|
||||
ki = 1.20 * ku / tu
|
||||
kd = 0.075 * ku * tu
|
||||
return kp, kd
|
||||
return kp, ki, kd
|
||||
|
||||
# ── JLink communication ───────────────────────────────────────────────────
|
||||
|
||||
def _send_pid_to_fc(self, kp: float, ki: float, kd: float) -> bool:
|
||||
"""Send JLINK_CMD_PID_SET then JLINK_CMD_PID_SAVE over JLink serial.
|
||||
|
||||
Returns True if both frames were written without serial error.
|
||||
"""
|
||||
if not _SERIAL_AVAILABLE:
|
||||
self.get_logger().warn(
|
||||
"pyserial not available — PID gains not sent to FC. "
|
||||
"Install with: pip install pyserial"
|
||||
)
|
||||
return False
|
||||
|
||||
try:
|
||||
payload_set = struct.pack("<fff", kp, ki, kd) # 12 bytes, little-endian
|
||||
frame_set = _build_jlink_frame(JLINK_CMD_PID_SET, payload_set)
|
||||
frame_save = _build_jlink_frame(JLINK_CMD_PID_SAVE, b"")
|
||||
|
||||
with serial.Serial(
|
||||
port=self._jlink_port,
|
||||
baudrate=self._jlink_baud,
|
||||
timeout=1.0,
|
||||
write_timeout=0.5,
|
||||
) as ser:
|
||||
ser.write(frame_set)
|
||||
time.sleep(0.05) # allow FC to process PID_SET
|
||||
ser.write(frame_save)
|
||||
# Flash erase takes ~1s on STM32F7; wait for it
|
||||
time.sleep(1.5)
|
||||
|
||||
self.get_logger().info(
|
||||
f"Sent PID_SET+PID_SAVE to FC via {self._jlink_port}: "
|
||||
f"Kp={kp:.4f}, Ki={ki:.4f}, Kd={kd:.4f}"
|
||||
)
|
||||
return True
|
||||
|
||||
except Exception as exc:
|
||||
self.get_logger().error(
|
||||
f"Failed to send PID to FC on {self._jlink_port}: {exc}"
|
||||
)
|
||||
return False
|
||||
|
||||
# ── Helpers ───────────────────────────────────────────────────────────────
|
||||
|
||||
def _publish_info(self, data: dict) -> None:
|
||||
"""Publish tuning information as JSON."""
|
||||
info = {
|
||||
"timestamp": self.get_clock().now().nanoseconds / 1e9,
|
||||
"state": self.tune_state.name,
|
||||
**data,
|
||||
}
|
||||
msg = String(data=json.dumps(info))
|
||||
self.pub_info.publish(msg)
|
||||
self.pub_info.publish(String(data=json.dumps(info)))
|
||||
|
||||
|
||||
def main(args=None):
|
||||
|
||||
30
src/jlink.c
30
src/jlink.c
@ -203,6 +203,11 @@ static void dispatch(const uint8_t *payload, uint8_t cmd, uint8_t plen)
|
||||
jlink_state.sleep_req = 1u;
|
||||
break;
|
||||
|
||||
case JLINK_CMD_PID_SAVE:
|
||||
/* Payload-less; main loop calls pid_flash_save() (Issue #531) */
|
||||
jlink_state.pid_save_req = 1u;
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
@ -342,3 +347,28 @@ void jlink_send_power_telemetry(const jlink_tlm_power_t *power)
|
||||
|
||||
jlink_tx_locked(frame, sizeof(frame));
|
||||
}
|
||||
|
||||
/* ---- jlink_send_pid_result() -- Issue #531 ---- */
|
||||
void jlink_send_pid_result(const jlink_tlm_pid_result_t *result)
|
||||
{
|
||||
/*
|
||||
* Frame: [STX][LEN][0x83][13 bytes PID_RESULT][CRC_hi][CRC_lo][ETX]
|
||||
* LEN = 1 (CMD) + 13 (payload) = 14; total frame = 19 bytes.
|
||||
* At 921600 baud: 19x10/921600 ~0.21 ms -- safe to block.
|
||||
*/
|
||||
static uint8_t frame_pid[19];
|
||||
const uint8_t plen = (uint8_t)sizeof(jlink_tlm_pid_result_t); /* 13 */
|
||||
const uint8_t flen = 1u + plen; /* 14 */
|
||||
|
||||
frame_pid[0] = JLINK_STX;
|
||||
frame_pid[1] = flen;
|
||||
frame_pid[2] = JLINK_TLM_PID_RESULT;
|
||||
memcpy(&frame_pid[3], result, plen);
|
||||
|
||||
uint16_t crc_pid = crc16_xmodem(&frame_pid[2], flen);
|
||||
frame_pid[3 + plen] = (uint8_t)(crc_pid >> 8);
|
||||
frame_pid[3 + plen + 1] = (uint8_t)(crc_pid & 0xFFu);
|
||||
frame_pid[3 + plen + 2] = JLINK_ETX;
|
||||
|
||||
jlink_tx_locked(frame_pid, sizeof(frame_pid));
|
||||
}
|
||||
|
||||
28
src/main.c
28
src/main.c
@ -30,6 +30,7 @@
|
||||
#include "battery.h"
|
||||
#include "coulomb_counter.h"
|
||||
#include "watchdog.h"
|
||||
#include "pid_flash.h"
|
||||
#include <math.h>
|
||||
#include <string.h>
|
||||
#include <stdio.h>
|
||||
@ -163,6 +164,18 @@ int main(void) {
|
||||
balance_t bal;
|
||||
balance_init(&bal);
|
||||
|
||||
/* Load PID from flash if previously auto-tuned (Issue #531) */
|
||||
{
|
||||
float flash_kp, flash_ki, flash_kd;
|
||||
if (pid_flash_load(&flash_kp, &flash_ki, &flash_kd)) {
|
||||
bal.kp = flash_kp;
|
||||
bal.ki = flash_ki;
|
||||
bal.kd = flash_kd;
|
||||
printf("[PID] Loaded from flash: Kp=%.3f Ki=%.3f Kd=%.3f\n",
|
||||
(double)flash_kp, (double)flash_ki, (double)flash_kd);
|
||||
}
|
||||
}
|
||||
|
||||
/* Init motor driver */
|
||||
motor_driver_t motors;
|
||||
motor_driver_init(&motors);
|
||||
@ -334,6 +347,21 @@ int main(void) {
|
||||
jlink_state.sleep_req = 0u;
|
||||
power_mgmt_request_sleep();
|
||||
}
|
||||
/* PID_SAVE: persist current gains to flash, reply with result (Issue #531).
|
||||
* Only allowed while disarmed -- flash sector erase stalls CPU for ~1 s. */
|
||||
if (jlink_state.pid_save_req && bal.state != BALANCE_ARMED) {
|
||||
jlink_state.pid_save_req = 0u;
|
||||
bool save_ok = pid_flash_save(bal.kp, bal.ki, bal.kd);
|
||||
jlink_tlm_pid_result_t pid_res;
|
||||
pid_res.kp = bal.kp;
|
||||
pid_res.ki = bal.ki;
|
||||
pid_res.kd = bal.kd;
|
||||
pid_res.saved_ok = save_ok ? 1u : 0u;
|
||||
jlink_send_pid_result(&pid_res);
|
||||
printf("[PID] Flash save %s: Kp=%.3f Ki=%.3f Kd=%.3f\n",
|
||||
save_ok ? "OK" : "FAIL",
|
||||
(double)bal.kp, (double)bal.ki, (double)bal.kd);
|
||||
}
|
||||
|
||||
/* Power management: CRSF/JLink activity or armed state resets idle timer */
|
||||
if ((crsf_state.last_rx_ms != 0 && (now - crsf_state.last_rx_ms) < 500) ||
|
||||
|
||||
72
src/pid_flash.c
Normal file
72
src/pid_flash.c
Normal file
@ -0,0 +1,72 @@
|
||||
#include "pid_flash.h"
|
||||
#include "stm32f7xx_hal.h"
|
||||
#include <string.h>
|
||||
|
||||
bool pid_flash_load(float *kp, float *ki, float *kd)
|
||||
{
|
||||
const pid_flash_t *p = (const pid_flash_t *)PID_FLASH_STORE_ADDR;
|
||||
|
||||
if (p->magic != PID_FLASH_MAGIC) return false;
|
||||
|
||||
/* Basic sanity bounds — same as JLINK_CMD_PID_SET handler */
|
||||
if (p->kp < 0.0f || p->kp > 500.0f) return false;
|
||||
if (p->ki < 0.0f || p->ki > 50.0f) return false;
|
||||
if (p->kd < 0.0f || p->kd > 50.0f) return false;
|
||||
|
||||
*kp = p->kp;
|
||||
*ki = p->ki;
|
||||
*kd = p->kd;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool pid_flash_save(float kp, float ki, float kd)
|
||||
{
|
||||
HAL_StatusTypeDef rc;
|
||||
|
||||
/* Unlock flash */
|
||||
rc = HAL_FLASH_Unlock();
|
||||
if (rc != HAL_OK) return false;
|
||||
|
||||
/* Erase sector 7 */
|
||||
FLASH_EraseInitTypeDef erase = {
|
||||
.TypeErase = FLASH_TYPEERASE_SECTORS,
|
||||
.Sector = PID_FLASH_SECTOR,
|
||||
.NbSectors = 1,
|
||||
.VoltageRange = PID_FLASH_SECTOR_VOLTAGE,
|
||||
};
|
||||
uint32_t sector_error = 0;
|
||||
rc = HAL_FLASHEx_Erase(&erase, §or_error);
|
||||
if (rc != HAL_OK || sector_error != 0xFFFFFFFFUL) {
|
||||
HAL_FLASH_Lock();
|
||||
return false;
|
||||
}
|
||||
|
||||
/* Build record */
|
||||
pid_flash_t rec;
|
||||
memset(&rec, 0xFF, sizeof(rec));
|
||||
rec.magic = PID_FLASH_MAGIC;
|
||||
rec.kp = kp;
|
||||
rec.ki = ki;
|
||||
rec.kd = kd;
|
||||
|
||||
/* Write 64 bytes as 16 × 32-bit words */
|
||||
const uint32_t *src = (const uint32_t *)&rec;
|
||||
uint32_t addr = PID_FLASH_STORE_ADDR;
|
||||
for (uint8_t i = 0; i < sizeof(rec) / 4u; i++) {
|
||||
rc = HAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD, addr, src[i]);
|
||||
if (rc != HAL_OK) {
|
||||
HAL_FLASH_Lock();
|
||||
return false;
|
||||
}
|
||||
addr += 4u;
|
||||
}
|
||||
|
||||
HAL_FLASH_Lock();
|
||||
|
||||
/* Verify readback */
|
||||
const pid_flash_t *stored = (const pid_flash_t *)PID_FLASH_STORE_ADDR;
|
||||
return (stored->magic == PID_FLASH_MAGIC &&
|
||||
stored->kp == kp &&
|
||||
stored->ki == ki &&
|
||||
stored->kd == kd);
|
||||
}
|
||||
@ -91,7 +91,17 @@ import { SaltyFace } from './components/SaltyFace.jsx';
|
||||
// Parameter server (issue #471)
|
||||
import { ParameterServer } from './components/ParameterServer.jsx';
|
||||
|
||||
// Teleop web interface (issue #534)
|
||||
import { TeleopWebUI } from './components/TeleopWebUI.jsx';
|
||||
|
||||
const TAB_GROUPS = [
|
||||
{
|
||||
label: 'TELEOP',
|
||||
color: 'text-orange-600',
|
||||
tabs: [
|
||||
{ id: 'teleop-webui', label: 'Drive' },
|
||||
],
|
||||
},
|
||||
{
|
||||
label: 'DISPLAY',
|
||||
color: 'text-rose-600',
|
||||
@ -286,8 +296,10 @@ export default function App() {
|
||||
{/* ── Content ── */}
|
||||
<main className={`flex-1 ${
|
||||
activeTab === 'salty-face' ? '' :
|
||||
['eventlog', 'control', 'imu'].includes(activeTab) ? 'flex flex-col' : 'overflow-y-auto'
|
||||
['eventlog', 'control', 'imu', 'teleop-webui'].includes(activeTab) ? 'flex flex-col' : 'overflow-y-auto'
|
||||
} ${activeTab === 'salty-face' ? '' : 'p-4'}`}>
|
||||
{activeTab === 'teleop-webui' && <TeleopWebUI subscribe={subscribe} publish={publishFn} />}
|
||||
|
||||
{activeTab === 'salty-face' && <SaltyFace subscribe={subscribe} />}
|
||||
|
||||
{activeTab === 'status' && <StatusPanel subscribe={subscribe} />}
|
||||
|
||||
480
ui/social-bot/src/components/TeleopWebUI.jsx
Normal file
480
ui/social-bot/src/components/TeleopWebUI.jsx
Normal file
@ -0,0 +1,480 @@
|
||||
/**
|
||||
* TeleopWebUI.jsx — Browser-based remote control with live video (Issue #534).
|
||||
*
|
||||
* Features:
|
||||
* - Live camera stream via rosbridge (sensor_msgs/CompressedImage)
|
||||
* - Camera selector: front / rear / left / right
|
||||
* - Virtual joystick (touch + mouse) for mobile-friendly control
|
||||
* - WASD / arrow-key keyboard fallback
|
||||
* - Speed presets: SLOW (20%), NORMAL (50%), FAST (100%)
|
||||
* - E-stop button (latching, prominent)
|
||||
* - Real-time linear/angular velocity display
|
||||
* - Mobile-responsive split layout
|
||||
*
|
||||
* Topics:
|
||||
* /camera/<name>/image_raw/compressed sensor_msgs/CompressedImage (subscribe)
|
||||
* /cmd_vel geometry_msgs/Twist (publish)
|
||||
*/
|
||||
|
||||
import { useEffect, useRef, useState, useCallback } from 'react';
|
||||
|
||||
// ── Constants ─────────────────────────────────────────────────────────────────
|
||||
|
||||
const MAX_LINEAR = 0.5; // m/s
|
||||
const MAX_ANGULAR = 1.0; // rad/s
|
||||
const DEADZONE = 0.10; // 10 %
|
||||
const CMD_HZ = 20; // publish rate (ms)
|
||||
|
||||
const SPEED_PRESETS = [
|
||||
{ label: 'SLOW', value: 0.20, color: 'text-green-400', border: 'border-green-800', bg: 'bg-green-950' },
|
||||
{ label: 'NORMAL', value: 0.50, color: 'text-amber-400', border: 'border-amber-800', bg: 'bg-amber-950' },
|
||||
{ label: 'FAST', value: 1.00, color: 'text-red-400', border: 'border-red-800', bg: 'bg-red-950' },
|
||||
];
|
||||
|
||||
const CAMERAS = [
|
||||
{ id: 'front', label: 'Front', topic: '/camera/front/image_raw/compressed' },
|
||||
{ id: 'rear', label: 'Rear', topic: '/camera/rear/image_raw/compressed' },
|
||||
{ id: 'left', label: 'Left', topic: '/camera/left/image_raw/compressed' },
|
||||
{ id: 'right', label: 'Right', topic: '/camera/right/image_raw/compressed' },
|
||||
];
|
||||
|
||||
// ── Utilities ─────────────────────────────────────────────────────────────────
|
||||
|
||||
function applyDeadzone(value) {
|
||||
const abs = Math.abs(value);
|
||||
if (abs < DEADZONE) return 0;
|
||||
return Math.sign(value) * ((abs - DEADZONE) / (1 - DEADZONE));
|
||||
}
|
||||
|
||||
function clamp(v, lo, hi) {
|
||||
return Math.max(lo, Math.min(hi, v));
|
||||
}
|
||||
|
||||
// ── Virtual joystick ──────────────────────────────────────────────────────────
|
||||
|
||||
function VirtualJoystick({ onMove }) {
|
||||
const canvasRef = useRef(null);
|
||||
const activeRef = useRef(false);
|
||||
const stickRef = useRef({ x: 0, y: 0 });
|
||||
|
||||
const draw = useCallback(() => {
|
||||
const canvas = canvasRef.current;
|
||||
if (!canvas) return;
|
||||
const ctx = canvas.getContext('2d');
|
||||
const W = canvas.width;
|
||||
const H = canvas.height;
|
||||
const cx = W / 2;
|
||||
const cy = H / 2;
|
||||
const R = Math.min(W, H) * 0.38;
|
||||
const kr = Math.min(W, H) * 0.14;
|
||||
|
||||
ctx.clearRect(0, 0, W, H);
|
||||
|
||||
// Base plate
|
||||
ctx.fillStyle = 'rgba(15,23,42,0.85)';
|
||||
ctx.beginPath();
|
||||
ctx.arc(cx, cy, R + kr, 0, Math.PI * 2);
|
||||
ctx.fill();
|
||||
|
||||
// Base ring
|
||||
ctx.strokeStyle = '#1e3a5f';
|
||||
ctx.lineWidth = 2;
|
||||
ctx.beginPath();
|
||||
ctx.arc(cx, cy, R, 0, Math.PI * 2);
|
||||
ctx.stroke();
|
||||
|
||||
// Deadzone ring
|
||||
ctx.strokeStyle = '#374151';
|
||||
ctx.lineWidth = 1;
|
||||
ctx.setLineDash([3, 3]);
|
||||
ctx.beginPath();
|
||||
ctx.arc(cx, cy, R * DEADZONE, 0, Math.PI * 2);
|
||||
ctx.stroke();
|
||||
ctx.setLineDash([]);
|
||||
|
||||
// Crosshair
|
||||
ctx.strokeStyle = '#1e3a5f';
|
||||
ctx.lineWidth = 1;
|
||||
ctx.beginPath();
|
||||
ctx.moveTo(cx - R * 0.3, cy); ctx.lineTo(cx + R * 0.3, cy);
|
||||
ctx.moveTo(cx, cy - R * 0.3); ctx.lineTo(cx, cy + R * 0.3);
|
||||
ctx.stroke();
|
||||
|
||||
// Knob
|
||||
const kx = cx + stickRef.current.x * R;
|
||||
const ky = cy - stickRef.current.y * R;
|
||||
const active = activeRef.current;
|
||||
|
||||
const grad = ctx.createRadialGradient(kx - kr * 0.3, ky - kr * 0.3, 0, kx, ky, kr);
|
||||
grad.addColorStop(0, active ? '#38bdf8' : '#1d4ed8');
|
||||
grad.addColorStop(1, active ? '#0369a1' : '#1e3a8a');
|
||||
ctx.fillStyle = grad;
|
||||
ctx.beginPath();
|
||||
ctx.arc(kx, ky, kr, 0, Math.PI * 2);
|
||||
ctx.fill();
|
||||
|
||||
ctx.strokeStyle = active ? '#7dd3fc' : '#3b82f6';
|
||||
ctx.lineWidth = 2;
|
||||
ctx.beginPath();
|
||||
ctx.arc(kx, ky, kr, 0, Math.PI * 2);
|
||||
ctx.stroke();
|
||||
|
||||
// Vector line
|
||||
const { x, y } = stickRef.current;
|
||||
if (Math.abs(x) > 0.02 || Math.abs(y) > 0.02) {
|
||||
ctx.strokeStyle = 'rgba(56,189,248,0.5)';
|
||||
ctx.lineWidth = 2;
|
||||
ctx.beginPath();
|
||||
ctx.moveTo(cx, cy);
|
||||
ctx.lineTo(kx, ky);
|
||||
ctx.stroke();
|
||||
}
|
||||
}, []);
|
||||
|
||||
const getPos = (clientX, clientY) => {
|
||||
const canvas = canvasRef.current;
|
||||
const rect = canvas.getBoundingClientRect();
|
||||
const R = Math.min(rect.width, rect.height) * 0.38;
|
||||
const cx = rect.left + rect.width / 2;
|
||||
const cy = rect.top + rect.height / 2;
|
||||
let x = (clientX - cx) / R;
|
||||
let y = (clientY - cy) / R;
|
||||
const mag = Math.sqrt(x * x + y * y);
|
||||
if (mag > 1) { x /= mag; y /= mag; }
|
||||
return { x: clamp(x, -1, 1), y: clamp(-y, -1, 1) };
|
||||
};
|
||||
|
||||
const onStart = useCallback((clientX, clientY) => {
|
||||
activeRef.current = true;
|
||||
const { x, y } = getPos(clientX, clientY);
|
||||
stickRef.current = { x, y };
|
||||
onMove({ x: applyDeadzone(x), y: applyDeadzone(y) });
|
||||
draw();
|
||||
}, [draw, onMove]);
|
||||
|
||||
const onContinue = useCallback((clientX, clientY) => {
|
||||
if (!activeRef.current) return;
|
||||
const { x, y } = getPos(clientX, clientY);
|
||||
stickRef.current = { x, y };
|
||||
onMove({ x: applyDeadzone(x), y: applyDeadzone(y) });
|
||||
draw();
|
||||
}, [draw, onMove]);
|
||||
|
||||
const onEnd = useCallback(() => {
|
||||
activeRef.current = false;
|
||||
stickRef.current = { x: 0, y: 0 };
|
||||
onMove({ x: 0, y: 0 });
|
||||
draw();
|
||||
}, [draw, onMove]);
|
||||
|
||||
// Mount: draw once + attach pointer events
|
||||
useEffect(() => {
|
||||
draw();
|
||||
const canvas = canvasRef.current;
|
||||
if (!canvas) return;
|
||||
|
||||
const onPD = (e) => { e.preventDefault(); onStart(e.clientX, e.clientY); };
|
||||
const onPM = (e) => { e.preventDefault(); onContinue(e.clientX, e.clientY); };
|
||||
const onPU = (e) => { e.preventDefault(); onEnd(); };
|
||||
|
||||
const onTD = (e) => { e.preventDefault(); const t = e.touches[0]; onStart(t.clientX, t.clientY); };
|
||||
const onTM = (e) => { e.preventDefault(); const t = e.touches[0]; onContinue(t.clientX, t.clientY); };
|
||||
const onTE = (e) => { e.preventDefault(); onEnd(); };
|
||||
|
||||
canvas.addEventListener('pointerdown', onPD);
|
||||
window.addEventListener('pointermove', onPM);
|
||||
window.addEventListener('pointerup', onPU);
|
||||
canvas.addEventListener('touchstart', onTD, { passive: false });
|
||||
canvas.addEventListener('touchmove', onTM, { passive: false });
|
||||
canvas.addEventListener('touchend', onTE, { passive: false });
|
||||
|
||||
return () => {
|
||||
canvas.removeEventListener('pointerdown', onPD);
|
||||
window.removeEventListener('pointermove', onPM);
|
||||
window.removeEventListener('pointerup', onPU);
|
||||
canvas.removeEventListener('touchstart', onTD);
|
||||
canvas.removeEventListener('touchmove', onTM);
|
||||
canvas.removeEventListener('touchend', onTE);
|
||||
};
|
||||
}, [onStart, onContinue, onEnd, draw]);
|
||||
|
||||
return (
|
||||
<canvas
|
||||
ref={canvasRef}
|
||||
width={200}
|
||||
height={200}
|
||||
className="rounded-full cursor-grab active:cursor-grabbing select-none"
|
||||
style={{ touchAction: 'none' }}
|
||||
/>
|
||||
);
|
||||
}
|
||||
|
||||
// ── Camera feed ───────────────────────────────────────────────────────────────
|
||||
|
||||
function CameraFeed({ subscribe, topic }) {
|
||||
const [src, setSrc] = useState(null);
|
||||
const [fps, setFps] = useState(0);
|
||||
const frameCountRef = useRef(0);
|
||||
const lastFpsTimeRef = useRef(Date.now());
|
||||
|
||||
useEffect(() => {
|
||||
setSrc(null);
|
||||
frameCountRef.current = 0;
|
||||
lastFpsTimeRef.current = Date.now();
|
||||
|
||||
const unsub = subscribe(topic, 'sensor_msgs/CompressedImage', (msg) => {
|
||||
const fmt = msg.format?.includes('png') ? 'png' : 'jpeg';
|
||||
const dataUrl = `data:image/${fmt};base64,${msg.data}`;
|
||||
setSrc(dataUrl);
|
||||
|
||||
frameCountRef.current++;
|
||||
const now = Date.now();
|
||||
const dt = now - lastFpsTimeRef.current;
|
||||
if (dt >= 1000) {
|
||||
setFps(Math.round((frameCountRef.current * 1000) / dt));
|
||||
frameCountRef.current = 0;
|
||||
lastFpsTimeRef.current = now;
|
||||
}
|
||||
});
|
||||
|
||||
return unsub;
|
||||
}, [subscribe, topic]);
|
||||
|
||||
return (
|
||||
<div className="relative w-full h-full flex items-center justify-center bg-black rounded-lg overflow-hidden">
|
||||
{src ? (
|
||||
<img
|
||||
src={src}
|
||||
alt="camera"
|
||||
className="max-w-full max-h-full object-contain"
|
||||
draggable={false}
|
||||
/>
|
||||
) : (
|
||||
<div className="flex flex-col items-center gap-2 text-gray-700">
|
||||
<div className="text-3xl">📷</div>
|
||||
<div className="text-xs tracking-widest">NO SIGNAL</div>
|
||||
</div>
|
||||
)}
|
||||
{/* FPS badge */}
|
||||
{src && (
|
||||
<div className="absolute top-2 right-2 bg-black bg-opacity-60 text-green-400 text-xs font-mono px-1.5 py-0.5 rounded">
|
||||
{fps} fps
|
||||
</div>
|
||||
)}
|
||||
</div>
|
||||
);
|
||||
}
|
||||
|
||||
// ── Main component ────────────────────────────────────────────────────────────
|
||||
|
||||
export function TeleopWebUI({ subscribe, publish }) {
|
||||
const [selectedCam, setSelectedCam] = useState('front');
|
||||
const [speedPreset, setSpeedPreset] = useState(0.50);
|
||||
const [isEstopped, setIsEstopped] = useState(false);
|
||||
const [linearVel, setLinearVel] = useState(0);
|
||||
const [angularVel, setAngularVel] = useState(0);
|
||||
|
||||
// Joystick normalized input ref [-1,1] each axis
|
||||
const joystickRef = useRef({ x: 0, y: 0 });
|
||||
// Keyboard pressed keys
|
||||
const keysRef = useRef({});
|
||||
|
||||
const currentCam = CAMERAS.find(c => c.id === selectedCam);
|
||||
|
||||
// ── Keyboard input ──────────────────────────────────────────────────────────
|
||||
useEffect(() => {
|
||||
const down = (e) => {
|
||||
const k = e.key.toLowerCase();
|
||||
if (['w','a','s','d','arrowup','arrowdown','arrowleft','arrowright',' '].includes(k)) {
|
||||
keysRef.current[k] = true;
|
||||
e.preventDefault();
|
||||
}
|
||||
};
|
||||
const up = (e) => {
|
||||
const k = e.key.toLowerCase();
|
||||
keysRef.current[k] = false;
|
||||
};
|
||||
window.addEventListener('keydown', down);
|
||||
window.addEventListener('keyup', up);
|
||||
return () => {
|
||||
window.removeEventListener('keydown', down);
|
||||
window.removeEventListener('keyup', up);
|
||||
};
|
||||
}, []);
|
||||
|
||||
// ── Publish loop ────────────────────────────────────────────────────────────
|
||||
useEffect(() => {
|
||||
const timer = setInterval(() => {
|
||||
const k = keysRef.current;
|
||||
const j = joystickRef.current;
|
||||
|
||||
// Keyboard overrides joystick if any key pressed
|
||||
let lin = j.y;
|
||||
let ang = -j.x; // right stick-x → negative angular (turn left on left)
|
||||
|
||||
const anyKey = k['w'] || k['s'] || k['a'] || k['d'] ||
|
||||
k['arrowup'] || k['arrowdown'] || k['arrowleft'] || k['arrowright'];
|
||||
|
||||
if (anyKey) {
|
||||
lin = 0;
|
||||
ang = 0;
|
||||
if (k['w'] || k['arrowup']) lin = 1;
|
||||
if (k['s'] || k['arrowdown']) lin = -1;
|
||||
if (k['a'] || k['arrowleft']) ang = 1;
|
||||
if (k['d'] || k['arrowright']) ang = -1;
|
||||
}
|
||||
|
||||
// Space = quick stop (doesn't latch e-stop)
|
||||
if (k[' ']) { lin = 0; ang = 0; }
|
||||
|
||||
const factor = isEstopped ? 0 : speedPreset;
|
||||
const finalLin = clamp(lin * MAX_LINEAR * factor, -MAX_LINEAR, MAX_LINEAR);
|
||||
const finalAng = clamp(ang * MAX_ANGULAR * factor, -MAX_ANGULAR, MAX_ANGULAR);
|
||||
|
||||
setLinearVel(finalLin);
|
||||
setAngularVel(finalAng);
|
||||
|
||||
if (publish) {
|
||||
publish('/cmd_vel', 'geometry_msgs/Twist', {
|
||||
linear: { x: finalLin, y: 0, z: 0 },
|
||||
angular: { x: 0, y: 0, z: finalAng },
|
||||
});
|
||||
}
|
||||
}, CMD_HZ);
|
||||
|
||||
return () => clearInterval(timer);
|
||||
}, [isEstopped, speedPreset, publish]);
|
||||
|
||||
// ── E-stop ──────────────────────────────────────────────────────────────────
|
||||
const handleEstop = () => {
|
||||
const next = !isEstopped;
|
||||
setIsEstopped(next);
|
||||
if (publish) {
|
||||
publish('/cmd_vel', 'geometry_msgs/Twist', {
|
||||
linear: { x: 0, y: 0, z: 0 },
|
||||
angular: { x: 0, y: 0, z: 0 },
|
||||
});
|
||||
}
|
||||
};
|
||||
|
||||
// Joystick callback
|
||||
const handleJoystick = useCallback((pos) => {
|
||||
joystickRef.current = pos;
|
||||
}, []);
|
||||
|
||||
// ── Render ──────────────────────────────────────────────────────────────────
|
||||
return (
|
||||
<div className="flex flex-col lg:flex-row h-full gap-3">
|
||||
|
||||
{/* ── Left: Camera feed ── */}
|
||||
<div className="flex flex-col gap-2 flex-1 min-h-0">
|
||||
|
||||
{/* Camera selector */}
|
||||
<div className="flex gap-1 shrink-0">
|
||||
{CAMERAS.map((cam) => (
|
||||
<button
|
||||
key={cam.id}
|
||||
onClick={() => setSelectedCam(cam.id)}
|
||||
className={`px-3 py-1 text-xs font-bold rounded border tracking-widest transition-colors ${
|
||||
selectedCam === cam.id
|
||||
? 'bg-cyan-950 border-cyan-700 text-cyan-300'
|
||||
: 'bg-gray-950 border-gray-800 text-gray-600 hover:text-gray-300'
|
||||
}`}
|
||||
>
|
||||
{cam.label.toUpperCase()}
|
||||
</button>
|
||||
))}
|
||||
<div className="flex-1" />
|
||||
<div className="text-xs text-gray-700 font-mono self-center">
|
||||
{currentCam?.topic}
|
||||
</div>
|
||||
</div>
|
||||
|
||||
{/* Video feed */}
|
||||
<div className="flex-1 min-h-[200px] lg:min-h-0">
|
||||
<CameraFeed
|
||||
subscribe={subscribe}
|
||||
topic={currentCam?.topic}
|
||||
/>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
{/* ── Right: Controls ── */}
|
||||
<div className="flex flex-col gap-3 w-full lg:w-72 shrink-0">
|
||||
|
||||
{/* E-stop */}
|
||||
<button
|
||||
onClick={handleEstop}
|
||||
className={`w-full py-4 text-sm font-black tracking-widest rounded-lg border-2 transition-all ${
|
||||
isEstopped
|
||||
? 'bg-red-900 border-red-500 text-red-200 animate-pulse'
|
||||
: 'bg-red-950 border-red-800 text-red-400 hover:bg-red-900 hover:border-red-600'
|
||||
}`}
|
||||
>
|
||||
{isEstopped ? '🛑 E-STOP — TAP TO RESUME' : '⚡ E-STOP'}
|
||||
</button>
|
||||
|
||||
{/* Speed presets */}
|
||||
<div className="bg-gray-950 rounded-lg border border-cyan-950 p-3 space-y-2">
|
||||
<div className="text-xs font-bold tracking-widest text-cyan-700">SPEED PRESET</div>
|
||||
<div className="grid grid-cols-3 gap-2">
|
||||
{SPEED_PRESETS.map((p) => (
|
||||
<button
|
||||
key={p.label}
|
||||
onClick={() => setSpeedPreset(p.value)}
|
||||
disabled={isEstopped}
|
||||
className={`py-2 text-xs font-bold rounded border tracking-widest transition-colors ${
|
||||
speedPreset === p.value && !isEstopped
|
||||
? `${p.bg} ${p.border} ${p.color}`
|
||||
: 'bg-gray-900 border-gray-800 text-gray-600 hover:text-gray-300 disabled:opacity-40 disabled:cursor-not-allowed'
|
||||
}`}
|
||||
>
|
||||
{p.label}
|
||||
<div className="text-gray-500 font-normal mt-0.5">{(p.value * 100).toFixed(0)}%</div>
|
||||
</button>
|
||||
))}
|
||||
</div>
|
||||
</div>
|
||||
|
||||
{/* Velocity display */}
|
||||
<div className="bg-gray-950 rounded-lg border border-cyan-950 p-3 grid grid-cols-2 gap-2">
|
||||
<div>
|
||||
<div className="text-gray-600 text-xs mb-1">LINEAR</div>
|
||||
<div className={`text-xl font-mono ${isEstopped ? 'text-gray-700' : 'text-cyan-300'}`}>
|
||||
{linearVel.toFixed(2)}
|
||||
</div>
|
||||
<div className="text-gray-700 text-xs">m/s</div>
|
||||
</div>
|
||||
<div>
|
||||
<div className="text-gray-600 text-xs mb-1">ANGULAR</div>
|
||||
<div className={`text-xl font-mono ${isEstopped ? 'text-gray-700' : 'text-amber-300'}`}>
|
||||
{angularVel.toFixed(2)}
|
||||
</div>
|
||||
<div className="text-gray-700 text-xs">rad/s</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
{/* Joystick */}
|
||||
<div className="bg-gray-950 rounded-lg border border-cyan-950 p-3 flex flex-col items-center gap-2">
|
||||
<div className="text-xs font-bold tracking-widest text-cyan-700">JOYSTICK</div>
|
||||
<VirtualJoystick onMove={handleJoystick} />
|
||||
<div className="text-xs text-gray-700 text-center">
|
||||
drag or use W A S D / arrow keys
|
||||
</div>
|
||||
</div>
|
||||
|
||||
{/* Key reference */}
|
||||
<div className="bg-gray-950 rounded border border-gray-800 p-2 text-xs text-gray-700 space-y-1">
|
||||
<div className="font-bold text-gray-600 mb-1">KEYBOARD</div>
|
||||
<div className="grid grid-cols-2 gap-x-3">
|
||||
<div><span className="text-gray-500">W/↑</span> Forward</div>
|
||||
<div><span className="text-gray-500">S/↓</span> Backward</div>
|
||||
<div><span className="text-gray-500">A/←</span> Turn Left</div>
|
||||
<div><span className="text-gray-500">D/→</span> Turn Right</div>
|
||||
<div className="col-span-2"><span className="text-gray-500">Space</span> Quick stop</div>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
);
|
||||
}
|
||||
Loading…
x
Reference in New Issue
Block a user