sl-controls c86abdd1b8 feat: Add VESC balance PID controller with tilt safety (Issue #407)
- ROS2 node for balance mode PID parameter management via pyvesc UART
- Tilt safety kill switch: ±45° pitch > 500ms triggers motor cutoff
- Startup ramp: gradual acceleration from 0 to full output over configurable duration
- IMU integration: subscribe to /imu/data for pitch/roll angle computation
- State publishing: /saltybot/balance_state with tilt angles, PID values, motor telemetry
- Data logging: /saltybot/balance_log publishes CSV-formatted IMU + motor data
- Configurable parameters: PID gains, tilt thresholds, ramp duration, control frequency
- Test suite: quaternion to Euler conversion, tilt safety checks, startup ramp

Co-Authored-By: Claude Haiku 4.5 <noreply@anthropic.com>
2026-03-04 22:40:40 -05:00

567 lines
18 KiB
JavaScript

/**
* OpsDashboard.jsx — Live operations dashboard (Issue #412)
*
* Comprehensive telemetry view combining:
* - Battery & power (10Hz)
* - Motors & PWM (10Hz)
* - IMU attitude (pitch/roll/yaw) (10Hz)
* - LIDAR polar map (1Hz)
* - Camera feed + object tracking
* - Social state (1Hz)
* - System health (temps/RAM/disk) (1Hz)
* - 2D odometry map (10Hz)
*
* Responsive grid layout, dark theme, auto-reconnect, mobile-optimized.
*/
import { useState, useEffect, useRef } from 'react';
const QUATERNION_TOPIC = '/saltybot/imu';
const BALANCE_STATE_TOPIC = '/saltybot/balance_state';
const ROVER_PWM_TOPIC = '/saltybot/rover_pwm';
const DIAGNOSTICS_TOPIC = '/diagnostics';
const SCAN_TOPIC = '/scan';
const ODOM_TOPIC = '/odom';
const SOCIAL_FACE_TOPIC = '/social/face/active';
const SOCIAL_SPEECH_TOPIC = '/social/speech/is_speaking';
// Quaternion to Euler angles
function quatToEuler(qx, qy, qz, qw) {
// Roll (x-axis rotation)
const sinr_cosp = 2 * (qw * qx + qy * qz);
const cosr_cosp = 1 - 2 * (qx * qx + qy * qy);
const roll = Math.atan2(sinr_cosp, cosr_cosp);
// Pitch (y-axis rotation)
const sinp = 2 * (qw * qy - qz * qx);
const pitch = Math.abs(sinp) >= 1 ? Math.PI / 2 * Math.sign(sinp) : Math.asin(sinp);
// Yaw (z-axis rotation)
const siny_cosp = 2 * (qw * qz + qx * qy);
const cosy_cosp = 1 - 2 * (qy * qy + qz * qz);
const yaw = Math.atan2(siny_cosp, cosy_cosp);
return {
roll: (roll * 180) / Math.PI,
pitch: (pitch * 180) / Math.PI,
yaw: (yaw * 180) / Math.PI,
};
}
// Attitude Gauge Component
function AttitudeGauge({ roll, pitch, yaw }) {
const canvasRef = useRef(null);
useEffect(() => {
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) / 2 - 10;
// Clear
ctx.fillStyle = '#020208';
ctx.fillRect(0, 0, W, H);
// Outer circle
ctx.strokeStyle = '#06b6d4';
ctx.lineWidth = 2;
ctx.beginPath();
ctx.arc(cx, cy, r, 0, 2 * Math.PI);
ctx.stroke();
// Pitch scale lines (outer ring)
ctx.strokeStyle = 'rgba(6,182,212,0.3)';
ctx.lineWidth = 1;
for (let i = -90; i <= 90; i += 30) {
const angle = (i * Math.PI) / 180;
const x1 = cx + Math.cos(angle) * r;
const y1 = cy + Math.sin(angle) * r;
const x2 = cx + Math.cos(angle) * (r - 8);
const y2 = cy + Math.sin(angle) * (r - 8);
ctx.beginPath();
ctx.moveTo(x1, y1);
ctx.lineTo(x2, y2);
ctx.stroke();
}
// Roll indicator (artificial horizon)
ctx.save();
ctx.translate(cx, cy);
ctx.rotate((roll * Math.PI) / 180);
// Horizon line
ctx.strokeStyle = '#f59e0b';
ctx.lineWidth = 2;
const horizonY = (-pitch / 90) * (r * 0.6);
ctx.beginPath();
ctx.moveTo(-r * 0.7, horizonY);
ctx.lineTo(r * 0.7, horizonY);
ctx.stroke();
ctx.restore();
// Yaw indicator (top)
ctx.fillStyle = '#06b6d4';
ctx.font = 'bold 10px monospace';
ctx.textAlign = 'center';
ctx.fillText(`YAW ${yaw.toFixed(0)}°`, cx, 15);
// Roll/Pitch labels
ctx.textAlign = 'left';
ctx.fillStyle = '#f59e0b';
ctx.fillText(`R:${roll.toFixed(0)}°`, cx - r + 5, cy + r - 5);
ctx.fillText(`P:${pitch.toFixed(0)}°`, cx - r + 5, cy + r + 8);
}, [roll, pitch, yaw]);
return <canvas ref={canvasRef} width={180} height={180} className="bg-gray-950 rounded border border-cyan-950" />;
}
// LIDAR Polar Map Component
function LidarMap({ scanMsg }) {
const canvasRef = useRef(null);
useEffect(() => {
const canvas = canvasRef.current;
if (!canvas || !scanMsg) return;
const ctx = canvas.getContext('2d');
const W = canvas.width;
const H = canvas.height;
const cx = W / 2;
const cy = H / 2;
const maxR = Math.min(W, H) / 2 - 20;
// Clear
ctx.fillStyle = '#020208';
ctx.fillRect(0, 0, W, H);
// Polar grid
ctx.strokeStyle = 'rgba(6,182,212,0.2)';
ctx.lineWidth = 0.5;
for (let d = 1; d <= 5; d++) {
const r = (d / 5) * maxR;
ctx.beginPath();
ctx.arc(cx, cy, r, 0, 2 * Math.PI);
ctx.stroke();
}
// Cardinal directions
ctx.strokeStyle = 'rgba(6,182,212,0.3)';
ctx.lineWidth = 1;
[0, Math.PI / 2, Math.PI, (3 * Math.PI) / 2].forEach((angle) => {
ctx.beginPath();
ctx.moveTo(cx, cy);
ctx.lineTo(cx + Math.cos(angle) * maxR, cy + Math.sin(angle) * maxR);
ctx.stroke();
});
// LIDAR points
const ranges = scanMsg.ranges ?? [];
const angleMin = scanMsg.angle_min ?? 0;
const angleIncrement = scanMsg.angle_increment ?? 0.01;
ctx.fillStyle = '#06b6d4';
ranges.forEach((range, idx) => {
if (range === 0 || !isFinite(range) || range > 8) return;
const angle = angleMin + idx * angleIncrement;
const r = (Math.min(range, 5) / 5) * maxR;
const x = cx + Math.cos(angle) * r;
const y = cy - Math.sin(angle) * r; // invert y
ctx.fillRect(x - 1, y - 1, 2, 2);
});
// Forward indicator
ctx.strokeStyle = '#f59e0b';
ctx.lineWidth = 2;
ctx.beginPath();
ctx.moveTo(cx, cy);
ctx.lineTo(cx, cy - maxR * 0.3);
ctx.stroke();
}, [scanMsg]);
return <canvas ref={canvasRef} width={200} height={200} className="bg-gray-950 rounded border border-cyan-950" />;
}
// Odometry 2D Map Component
function OdomMap({ odomMsg }) {
const canvasRef = useRef(null);
const trailRef = useRef([]);
useEffect(() => {
if (odomMsg?.pose?.pose?.position) {
trailRef.current.push({
x: odomMsg.pose.pose.position.x,
y: odomMsg.pose.pose.position.y,
ts: Date.now(),
});
if (trailRef.current.length > 500) trailRef.current.shift();
}
}, [odomMsg]);
useEffect(() => {
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 scale = 50; // pixels per meter
// Clear
ctx.fillStyle = '#020208';
ctx.fillRect(0, 0, W, H);
// Grid
ctx.strokeStyle = 'rgba(6,182,212,0.1)';
ctx.lineWidth = 0.5;
for (let i = -10; i <= 10; i++) {
ctx.beginPath();
ctx.moveTo(cx + i * scale, 0);
ctx.lineTo(cx + i * scale, H);
ctx.stroke();
ctx.beginPath();
ctx.moveTo(0, cy + i * scale);
ctx.lineTo(W, cy + i * scale);
ctx.stroke();
}
// Trail
if (trailRef.current.length > 1) {
ctx.strokeStyle = '#06b6d4';
ctx.lineWidth = 1;
ctx.beginPath();
trailRef.current.forEach((pt, i) => {
const x = cx + pt.x * scale;
const y = cy - pt.y * scale;
i === 0 ? ctx.moveTo(x, y) : ctx.lineTo(x, y);
});
ctx.stroke();
}
// Robot position
if (odomMsg?.pose?.pose?.position) {
const x = cx + odomMsg.pose.pose.position.x * scale;
const y = cy - odomMsg.pose.pose.position.y * scale;
ctx.fillStyle = '#f59e0b';
ctx.beginPath();
ctx.arc(x, y, 5, 0, 2 * Math.PI);
ctx.fill();
}
}, [odomMsg, trailRef.current.length]);
return <canvas ref={canvasRef} width={250} height={250} className="bg-gray-950 rounded border border-cyan-950" />;
}
// Battery Widget
function BatteryWidget({ batteryData }) {
const voltage = batteryData?.voltage ?? 0;
const current = batteryData?.current ?? 0;
const soc = batteryData?.soc ?? 0;
let color = '#22c55e';
if (soc < 20) color = '#ef4444';
else if (soc < 50) color = '#f59e0b';
return (
<div className="bg-gray-950 rounded-lg border border-cyan-950 p-4">
<div className="text-xs font-bold text-cyan-700 tracking-widest mb-3">BATTERY</div>
<div className="flex justify-between items-center mb-3">
<div className="text-3xl font-bold" style={{ color }}>
{soc.toFixed(0)}%
</div>
<div className="text-xs text-gray-500 text-right">
<div>{voltage.toFixed(1)}V</div>
<div>{current.toFixed(1)}A</div>
</div>
</div>
<div className="w-full h-2 bg-gray-900 rounded overflow-hidden border border-gray-800">
<div
className="h-full transition-all duration-500"
style={{ width: `${soc}%`, background: color }}
/>
</div>
</div>
);
}
// Motor Widget
function MotorWidget({ motorData }) {
const left = motorData?.left ?? 0;
const right = motorData?.right ?? 0;
const dutyBar = (norm) => {
const pct = Math.abs(norm) * 50;
const color = norm > 0 ? '#f97316' : '#3b82f6';
const left = norm >= 0 ? '50%' : `${50 - pct}%`;
return { pct, color, left };
};
const leftBar = dutyBar(left);
const rightBar = dutyBar(right);
return (
<div className="bg-gray-950 rounded-lg border border-cyan-950 p-4">
<div className="text-xs font-bold text-cyan-700 tracking-widest mb-3">MOTORS</div>
<div className="space-y-2">
{['L', 'R'].map((label, idx) => {
const val = idx === 0 ? left : right;
const bar = idx === 0 ? leftBar : rightBar;
return (
<div key={label}>
<div className="flex justify-between text-xs mb-1">
<span className="text-gray-600">{label}:</span>
<span className="text-orange-400 font-bold">{(val * 100).toFixed(0)}%</span>
</div>
<div className="relative h-2 bg-gray-900 rounded border border-gray-800 overflow-hidden">
<div className="absolute inset-y-0 left-1/2 w-px bg-gray-700" />
<div
className="absolute inset-y-0 transition-all duration-100"
style={{ left: bar.left, width: `${bar.pct}%`, background: bar.color }}
/>
</div>
</div>
);
})}
</div>
</div>
);
}
// System Health Widget
function SystemWidget({ sysData }) {
const cpuTemp = sysData?.cpuTemp ?? 0;
const gpuTemp = sysData?.gpuTemp ?? 0;
const ramPct = sysData?.ramPct ?? 0;
const diskPct = sysData?.diskPct ?? 0;
const tempColor = (t) => {
if (t > 80) return '#ef4444';
if (t > 60) return '#f59e0b';
return '#22c55e';
};
return (
<div className="bg-gray-950 rounded-lg border border-cyan-950 p-4">
<div className="text-xs font-bold text-cyan-700 tracking-widest mb-3">SYSTEM</div>
<div className="grid grid-cols-2 gap-3">
<div className="text-center">
<div className="text-xs text-gray-600">CPU</div>
<div className="text-lg font-bold" style={{ color: tempColor(cpuTemp) }}>
{cpuTemp.toFixed(0)}°C
</div>
</div>
<div className="text-center">
<div className="text-xs text-gray-600">GPU</div>
<div className="text-lg font-bold" style={{ color: tempColor(gpuTemp) }}>
{gpuTemp.toFixed(0)}°C
</div>
</div>
<div>
<div className="text-xs text-gray-600 mb-1">RAM {ramPct.toFixed(0)}%</div>
<div className="h-1.5 bg-gray-900 rounded overflow-hidden border border-gray-800">
<div
className="h-full bg-cyan-500 transition-all duration-500"
style={{ width: `${Math.min(ramPct, 100)}%` }}
/>
</div>
</div>
<div>
<div className="text-xs text-gray-600 mb-1">Disk {diskPct.toFixed(0)}%</div>
<div className="h-1.5 bg-gray-900 rounded overflow-hidden border border-gray-800">
<div
className="h-full bg-amber-500 transition-all duration-500"
style={{ width: `${Math.min(diskPct, 100)}%` }}
/>
</div>
</div>
</div>
</div>
);
}
// Social Status Widget
function SocialWidget({ isSpeaking, faceId }) {
return (
<div className="bg-gray-950 rounded-lg border border-cyan-950 p-4">
<div className="text-xs font-bold text-cyan-700 tracking-widest mb-3">SOCIAL</div>
<div className="space-y-2">
<div className="flex items-center gap-2">
<div
className={`w-3 h-3 rounded-full ${isSpeaking ? 'bg-green-400 animate-pulse' : 'bg-gray-700'}`}
/>
<span className="text-xs">
{isSpeaking ? 'Speaking' : 'Silent'}
</span>
</div>
<div className="text-xs text-gray-600">
Face: <span className="text-gray-400">{faceId || 'none'}</span>
</div>
</div>
</div>
);
}
export function OpsDashboard({ subscribe }) {
const [imu, setImu] = useState({ roll: 0, pitch: 0, yaw: 0 });
const [battery, setBattery] = useState({ voltage: 0, current: 0, soc: 0 });
const [motors, setMotors] = useState({ left: 0, right: 0 });
const [system, setSystem] = useState({ cpuTemp: 0, gpuTemp: 0, ramPct: 0, diskPct: 0 });
const [scan, setScan] = useState(null);
const [odom, setOdom] = useState(null);
const [social, setSocial] = useState({ isSpeaking: false, faceId: null });
// IMU subscription
useEffect(() => {
const unsub = subscribe(QUATERNION_TOPIC, 'sensor_msgs/Imu', (msg) => {
const q = msg.orientation;
const euler = quatToEuler(q.x, q.y, q.z, q.w);
setImu(euler);
});
return unsub;
}, [subscribe]);
// Diagnostics subscription (battery, system temps)
useEffect(() => {
const unsub = subscribe(DIAGNOSTICS_TOPIC, 'diagnostic_msgs/DiagnosticArray', (msg) => {
for (const status of msg.status ?? []) {
const kv = {};
for (const pair of status.values ?? []) kv[pair.key] = pair.value;
// Battery
if (kv.battery_voltage_v !== undefined) {
setBattery((prev) => ({
...prev,
voltage: parseFloat(kv.battery_voltage_v),
soc: parseFloat(kv.battery_soc_pct) || 0,
current: parseFloat(kv.battery_current_a) || 0,
}));
}
// System temps/resources
if (kv.cpu_temp_c !== undefined || kv.gpu_temp_c !== undefined) {
setSystem((prev) => ({
...prev,
cpuTemp: parseFloat(kv.cpu_temp_c) || prev.cpuTemp,
gpuTemp: parseFloat(kv.gpu_temp_c) || prev.gpuTemp,
ramPct: parseFloat(kv.ram_pct) || prev.ramPct,
diskPct: parseFloat(kv.disk_pct) || prev.diskPct,
}));
}
}
});
return unsub;
}, [subscribe]);
// Balance state subscription (motors)
useEffect(() => {
const unsub = subscribe(BALANCE_STATE_TOPIC, 'std_msgs/String', (msg) => {
try {
const state = JSON.parse(msg.data);
const cmd = state.motor_cmd ?? 0;
const norm = Math.max(-1, Math.min(1, cmd / 1000));
setMotors({ left: norm, right: norm });
} catch {
/* ignore */
}
});
return unsub;
}, [subscribe]);
// LIDAR subscription
useEffect(() => {
const unsub = subscribe(SCAN_TOPIC, 'sensor_msgs/LaserScan', (msg) => {
setScan(msg);
});
return unsub;
}, [subscribe]);
// Odometry subscription
useEffect(() => {
const unsub = subscribe(ODOM_TOPIC, 'nav_msgs/Odometry', (msg) => {
setOdom(msg);
});
return unsub;
}, [subscribe]);
// Social speech subscription
useEffect(() => {
const unsub = subscribe(SOCIAL_SPEECH_TOPIC, 'std_msgs/Bool', (msg) => {
setSocial((prev) => ({ ...prev, isSpeaking: msg.data }));
});
return unsub;
}, [subscribe]);
// Social face subscription
useEffect(() => {
const unsub = subscribe(SOCIAL_FACE_TOPIC, 'std_msgs/String', (msg) => {
setSocial((prev) => ({ ...prev, faceId: msg.data }));
});
return unsub;
}, [subscribe]);
return (
<div className="flex flex-col h-full gap-4 overflow-y-auto p-2 sm:p-4">
{/* Header */}
<div className="text-center mb-2">
<h2 className="text-lg sm:text-2xl font-bold text-orange-400 tracking-wider"> OPERATIONS DASHBOARD</h2>
<p className="text-xs text-gray-600 mt-1">Real-time telemetry 10Hz critical 1Hz system</p>
</div>
{/* 3-column responsive grid */}
<div className="grid grid-cols-1 md:grid-cols-2 lg:grid-cols-3 gap-4 auto-rows-max">
{/* Left column: Critical data */}
<BatteryWidget batteryData={battery} />
<MotorWidget motorData={motors} />
<SocialWidget {...social} />
{/* Center column: Attitude & maps */}
<div className="flex justify-center">
<AttitudeGauge roll={imu.roll} pitch={imu.pitch} yaw={imu.yaw} />
</div>
<div className="flex justify-center">
<LidarMap scanMsg={scan} />
</div>
<SystemWidget sysData={system} />
{/* Right column: Odometry */}
<div className="flex justify-center lg:col-span-1">
<OdomMap odomMsg={odom} />
</div>
</div>
{/* Footer stats */}
<div className="mt-4 grid grid-cols-2 sm:grid-cols-4 gap-2 text-xs text-gray-600 bg-gray-950 border border-cyan-950 rounded p-3">
<div>
<span className="text-gray-500">Battery</span>
<br />
<span className="text-green-400 font-bold">{battery.soc.toFixed(0)}% {battery.voltage.toFixed(1)}V</span>
</div>
<div>
<span className="text-gray-500">Motors</span>
<br />
<span className="text-orange-400 font-bold">
L:{(motors.left * 100).toFixed(0)}% R:{(motors.right * 100).toFixed(0)}%
</span>
</div>
<div>
<span className="text-gray-500">Attitude</span>
<br />
<span className="text-blue-400 font-bold">R:{imu.roll.toFixed(0)}° Y:{imu.yaw.toFixed(0)}°</span>
</div>
<div>
<span className="text-gray-500">System</span>
<br />
<span className="text-cyan-400 font-bold">CPU:{system.cpuTemp.toFixed(0)}°C RAM:{system.ramPct.toFixed(0)}%</span>
</div>
</div>
</div>
);
}