feat(webui): fleet management dashboard (Issue #139) #147
99
include/bno055.h
Normal file
99
include/bno055.h
Normal file
@ -0,0 +1,99 @@
|
||||
#ifndef BNO055_H
|
||||
#define BNO055_H
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
#include "mpu6000.h" /* IMUData */
|
||||
|
||||
/*
|
||||
* BNO055 NDOF IMU driver over I2C1 (shared bus — PB8=SCL, PB9=SDA).
|
||||
*
|
||||
* Issue #135: auto-detected alongside MPU6000. Acts as:
|
||||
* PRIMARY — when MPU6000 init fails (seamless fallback)
|
||||
* AUGMENT — when both present; BNO055 provides better NDOF-fused yaw
|
||||
*
|
||||
* I2C addresses probed: 0x28 (ADR=0, default) then 0x29 (ADR=1).
|
||||
* Chip-ID register 0x00 must read 0xA0.
|
||||
*
|
||||
* Operating mode: NDOF (0x0C) — 9DOF fusion with magnetometer.
|
||||
* Falls back to IMUPLUS (0x08, no mag) if mag calibration stalls.
|
||||
*
|
||||
* Calibration offsets are saved to/restored from STM32 RTC backup
|
||||
* registers (BKP0R–BKP6R = 28 bytes), identified by a magic word.
|
||||
* If valid offsets are present, bno055_is_ready() returns true
|
||||
* immediately after init. Otherwise, waits for gyro+accel cal ≥ 2.
|
||||
*
|
||||
* Temperature compensation is handled internally by the BNO055 silicon
|
||||
* (it compensates all three sensors continuously). bno055_temperature()
|
||||
* exposes the onboard thermometer reading for telemetry.
|
||||
*
|
||||
* Loop-rate note: BNO055 reads over I2C1 @100kHz take ~3ms, so the
|
||||
* main balance loop drops from ~1kHz (MPU6000/SPI) to ~250Hz when
|
||||
* BNO055 is active. 250Hz is sufficient for stable self-balancing.
|
||||
* PID gain tuning may be required when switching IMU sources.
|
||||
*/
|
||||
|
||||
/* ---- Calibration status nibble masks (CALIB_STAT reg 0x35) ---- */
|
||||
#define BNO055_CAL_SYS_MASK 0xC0u /* bits [7:6] — overall system */
|
||||
#define BNO055_CAL_GYR_MASK 0x30u /* bits [5:4] — gyroscope */
|
||||
#define BNO055_CAL_ACC_MASK 0x0Cu /* bits [3:2] — accelerometer */
|
||||
#define BNO055_CAL_MAG_MASK 0x03u /* bits [1:0] — magnetometer */
|
||||
/* Each field: 0=uncalibrated, 3=fully calibrated */
|
||||
|
||||
/*
|
||||
* bno055_init() — probe I2C1 for BNO055, reset, enter NDOF mode,
|
||||
* restore saved calibration offsets if present.
|
||||
* Requires i2c1_init() already called.
|
||||
* Returns 0 on success, -1 if not found.
|
||||
* Blocks ~750ms (POR + mode-switch settle).
|
||||
* Call BEFORE safety_init() (IWDG not yet running).
|
||||
*/
|
||||
int bno055_init(void);
|
||||
|
||||
/*
|
||||
* bno055_read(data) — fill IMUData from BNO055 NDOF fusion output.
|
||||
* Uses Euler angles for pitch/roll/yaw and gyro registers for pitch_rate.
|
||||
* Triggers one I2C burst read (~3ms at 100kHz).
|
||||
* Call from main loop balance gate (not every loop iteration).
|
||||
*/
|
||||
void bno055_read(IMUData *data);
|
||||
|
||||
/*
|
||||
* bno055_is_ready() — true when BNO055 is suitable for balance arming.
|
||||
* True immediately if offsets were restored from backup RAM.
|
||||
* Otherwise true once gyro calibration ≥ 2 and accel ≥ 2.
|
||||
*/
|
||||
bool bno055_is_ready(void);
|
||||
|
||||
/*
|
||||
* bno055_calib_status() — raw CALIB_STAT byte.
|
||||
* Use BNO055_CAL_*_MASK to extract individual sensor calibration levels.
|
||||
* Returned value is updated lazily on each bno055_read() call.
|
||||
*/
|
||||
uint8_t bno055_calib_status(void);
|
||||
|
||||
/*
|
||||
* bno055_temperature() — onboard temperature in °C (gyro source).
|
||||
* Updated once per second (every ~250 calls to bno055_read()).
|
||||
* Range: -40..+85°C. Use for telemetry reporting only.
|
||||
*/
|
||||
int8_t bno055_temperature(void);
|
||||
|
||||
/*
|
||||
* bno055_save_offsets() — write current calibration offsets to
|
||||
* STM32 RTC backup registers BKP0R–BKP6R (22 bytes + magic).
|
||||
* Call once after sys+acc+gyr calibration all reach level 3.
|
||||
* Returns true if successful, false if BNO055 not present.
|
||||
* Temporarily switches to CONFIGMODE — do NOT call while armed.
|
||||
*/
|
||||
bool bno055_save_offsets(void);
|
||||
|
||||
/*
|
||||
* bno055_restore_offsets() — read offsets from RTC backup registers
|
||||
* and write them to BNO055 hardware (in CONFIGMODE).
|
||||
* Called automatically by bno055_init().
|
||||
* Returns true if valid offsets found and applied.
|
||||
*/
|
||||
bool bno055_restore_offsets(void);
|
||||
|
||||
#endif /* BNO055_H */
|
||||
316
src/bno055.c
Normal file
316
src/bno055.c
Normal file
@ -0,0 +1,316 @@
|
||||
/*
|
||||
* bno055.c — Bosch BNO055 NDOF IMU driver over I2C1
|
||||
*
|
||||
* Issue #135: auto-detect alongside MPU6000; fallback + yaw augmentation.
|
||||
*
|
||||
* Calibration offsets are persisted in STM32F7 RTC backup registers
|
||||
* (BKP0R–BKP6R) and restored automatically on next power-on, so the
|
||||
* sensor re-enters a calibrated state immediately after reset.
|
||||
*
|
||||
* Internal temperature compensation:
|
||||
* The BNO055 silicon compensates all three sensors (accel, gyro, mag)
|
||||
* continuously against its onboard thermometer. No external correction
|
||||
* is required. bno055_temperature() exposes the reading for telemetry.
|
||||
*/
|
||||
|
||||
#include "bno055.h"
|
||||
#include "i2c1.h"
|
||||
#include "config.h"
|
||||
#include "stm32f7xx_hal.h"
|
||||
#include <string.h>
|
||||
#include <math.h>
|
||||
|
||||
/* ---- I2C addresses (7-bit, shifted left 1 for HAL) ---- */
|
||||
#define BNO055_ADDR_LOW (0x28u << 1)
|
||||
#define BNO055_ADDR_HIGH (0x29u << 1)
|
||||
#define BNO055_CHIP_ID_VAL 0xA0u
|
||||
|
||||
/* ---- Register map (page 0) ---- */
|
||||
#define REG_CHIP_ID 0x00u
|
||||
#define REG_PAGE_ID 0x07u
|
||||
#define REG_GYRO_X_LSB 0x14u /* 6 bytes: Gx, Gy, Gz */
|
||||
#define REG_EULER_H_LSB 0x1Au /* 6 bytes: H, R, P (each int16) */
|
||||
#define REG_LIA_X_LSB 0x28u /* 6 bytes: linear accel X,Y,Z */
|
||||
#define REG_GRV_X_LSB 0x2Eu /* 6 bytes: gravity X,Y,Z */
|
||||
#define REG_TEMP 0x34u
|
||||
#define REG_CALIB_STAT 0x35u
|
||||
#define REG_OPR_MODE 0x3Du
|
||||
#define REG_PWR_MODE 0x3Eu
|
||||
#define REG_SYS_TRIGGER 0x3Fu
|
||||
#define REG_TEMP_SOURCE 0x40u
|
||||
#define REG_OFFSET_BASE 0x55u /* 22 bytes: accel(6)+mag(6)+gyro(6)+radii(4) */
|
||||
|
||||
/* ---- Operating modes ---- */
|
||||
#define MODE_CONFIG 0x00u
|
||||
#define MODE_IMUPLUS 0x08u /* 6DOF, no mag */
|
||||
#define MODE_NDOF 0x0Cu /* 9DOF with mag */
|
||||
|
||||
/* ---- RTC backup register magic for saved offsets ---- */
|
||||
/*
|
||||
* Layout: BKP0R..BKP5R hold 22 bytes of calibration offsets (packed
|
||||
* 4 bytes per register; BKP5R only uses its low 2 bytes).
|
||||
* BKP6R holds the magic word to validate the stored data.
|
||||
*/
|
||||
#define BNO055_BKP_MAGIC 0xB055CA10u
|
||||
#define BKP_REG(n) (*(volatile uint32_t *)(&RTC->BKP0R + (n)))
|
||||
|
||||
/* ---- Scaling constants ---- */
|
||||
/*
|
||||
* BNO055 default unit selection (UNIT_SEL=0x00):
|
||||
* Euler angles: 1 degree = 16 LSB → divide by 16.0f
|
||||
* Gyro rates: 1 °/s = 16 LSB → divide by 16.0f
|
||||
* Accel/LIA/GRV: 1 m/s² = 100 LSB → divide by 100.0f
|
||||
*/
|
||||
#define EULER_SCALE 16.0f
|
||||
#define GYRO_SCALE 16.0f
|
||||
#define ACCEL_SCALE 100.0f
|
||||
#define G_MS2 9.80665f /* m/s² per g */
|
||||
|
||||
/* ---- Module state ---- */
|
||||
static uint16_t s_addr = 0;
|
||||
static bool s_present = false;
|
||||
static bool s_offsets_ok = false; /* true if offsets restored */
|
||||
static uint8_t s_calib = 0; /* cached CALIB_STAT */
|
||||
static int8_t s_temp = 25; /* cached temperature °C */
|
||||
static uint16_t s_temp_ctr = 0; /* temp read divider */
|
||||
|
||||
/* ---- Low-level I2C helpers ---- */
|
||||
static uint8_t rd(uint8_t reg)
|
||||
{
|
||||
uint8_t v = 0;
|
||||
HAL_I2C_Mem_Read(&hi2c1, s_addr, reg, 1, &v, 1, 100);
|
||||
return v;
|
||||
}
|
||||
|
||||
static void wr(uint8_t reg, uint8_t val)
|
||||
{
|
||||
HAL_I2C_Mem_Write(&hi2c1, s_addr, reg, 1, &val, 1, 100);
|
||||
}
|
||||
|
||||
static void rdblk(uint8_t reg, uint8_t *buf, uint8_t len)
|
||||
{
|
||||
HAL_I2C_Mem_Read(&hi2c1, s_addr, reg, 1, buf, len, 200);
|
||||
}
|
||||
|
||||
static void wrblk(uint8_t reg, const uint8_t *buf, uint8_t len)
|
||||
{
|
||||
HAL_I2C_Mem_Write(&hi2c1, s_addr, reg, 1, (uint8_t *)buf, len, 200);
|
||||
}
|
||||
|
||||
/* ---- bno055_restore_offsets() ---- */
|
||||
bool bno055_restore_offsets(void)
|
||||
{
|
||||
__HAL_RCC_PWR_CLK_ENABLE();
|
||||
HAL_PWR_EnableBkUpAccess();
|
||||
|
||||
if (BKP_REG(6) != BNO055_BKP_MAGIC) return false;
|
||||
|
||||
/* Unpack 22 bytes from BKP0R..BKP5R */
|
||||
uint8_t offsets[22];
|
||||
for (int i = 0; i < 5; i++) {
|
||||
uint32_t w = BKP_REG(i);
|
||||
offsets[i * 4 + 0] = (uint8_t)(w & 0xFFu);
|
||||
offsets[i * 4 + 1] = (uint8_t)((w >> 8) & 0xFFu);
|
||||
offsets[i * 4 + 2] = (uint8_t)((w >> 16) & 0xFFu);
|
||||
offsets[i * 4 + 3] = (uint8_t)((w >> 24) & 0xFFu);
|
||||
}
|
||||
uint32_t w5 = BKP_REG(5);
|
||||
offsets[20] = (uint8_t)(w5 & 0xFFu);
|
||||
offsets[21] = (uint8_t)((w5 >> 8) & 0xFFu);
|
||||
|
||||
/* Write offsets — must be in CONFIG mode */
|
||||
wr(REG_OPR_MODE, MODE_CONFIG);
|
||||
HAL_Delay(25);
|
||||
wr(REG_PAGE_ID, 0x00);
|
||||
wrblk(REG_OFFSET_BASE, offsets, 22);
|
||||
/* Caller switches back to NDOF after this returns */
|
||||
return true;
|
||||
}
|
||||
|
||||
/* ---- bno055_save_offsets() ---- */
|
||||
bool bno055_save_offsets(void)
|
||||
{
|
||||
if (!s_present) return false;
|
||||
|
||||
/* Switch to CONFIG mode to read offsets */
|
||||
wr(REG_OPR_MODE, MODE_CONFIG);
|
||||
HAL_Delay(25);
|
||||
wr(REG_PAGE_ID, 0x00);
|
||||
|
||||
uint8_t offsets[22];
|
||||
rdblk(REG_OFFSET_BASE, offsets, 22);
|
||||
|
||||
/* Restore to NDOF */
|
||||
wr(REG_OPR_MODE, MODE_NDOF);
|
||||
HAL_Delay(10);
|
||||
|
||||
/* Pack into RTC backup registers */
|
||||
__HAL_RCC_PWR_CLK_ENABLE();
|
||||
HAL_PWR_EnableBkUpAccess();
|
||||
|
||||
for (int i = 0; i < 5; i++) {
|
||||
BKP_REG(i) = ((uint32_t)offsets[i * 4 + 0])
|
||||
| ((uint32_t)offsets[i * 4 + 1] << 8)
|
||||
| ((uint32_t)offsets[i * 4 + 2] << 16)
|
||||
| ((uint32_t)offsets[i * 4 + 3] << 24);
|
||||
}
|
||||
BKP_REG(5) = (uint32_t)offsets[20] | ((uint32_t)offsets[21] << 8);
|
||||
BKP_REG(6) = BNO055_BKP_MAGIC;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/* ---- bno055_init() ---- */
|
||||
int bno055_init(void)
|
||||
{
|
||||
/* Probe 0x28 (ADR=0) first, then 0x29 (ADR=1) */
|
||||
const uint16_t candidates[2] = { BNO055_ADDR_LOW, BNO055_ADDR_HIGH };
|
||||
s_present = false;
|
||||
for (int i = 0; i < 2; i++) {
|
||||
s_addr = candidates[i];
|
||||
if (rd(REG_CHIP_ID) == BNO055_CHIP_ID_VAL) {
|
||||
s_present = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (!s_present) return -1;
|
||||
|
||||
/* Software reset via SYS_TRIGGER */
|
||||
wr(REG_PAGE_ID, 0x00);
|
||||
wr(REG_SYS_TRIGGER, 0x20u);
|
||||
HAL_Delay(700); /* POR settle time from datasheet */
|
||||
|
||||
/* Confirm chip ID after reset */
|
||||
if (rd(REG_CHIP_ID) != BNO055_CHIP_ID_VAL) {
|
||||
s_present = false;
|
||||
return -1;
|
||||
}
|
||||
|
||||
wr(REG_PAGE_ID, 0x00);
|
||||
|
||||
/* CONFIGMODE before any register writes */
|
||||
wr(REG_OPR_MODE, MODE_CONFIG);
|
||||
HAL_Delay(25);
|
||||
|
||||
/* Normal power mode */
|
||||
wr(REG_PWR_MODE, 0x00u);
|
||||
HAL_Delay(10);
|
||||
|
||||
/*
|
||||
* Unit selection (UNIT_SEL = 0x00, default):
|
||||
* Euler = degrees, Gyro = dps, Accel = m/s², Temp = °C
|
||||
* No write needed — defaults match what we want.
|
||||
*/
|
||||
|
||||
/* Temperature source = gyro (more stable than accel) */
|
||||
wr(REG_TEMP_SOURCE, 0x01u);
|
||||
|
||||
/* Try to restore saved calibration offsets */
|
||||
s_offsets_ok = bno055_restore_offsets();
|
||||
|
||||
/* Enter NDOF fusion mode */
|
||||
wr(REG_OPR_MODE, MODE_NDOF);
|
||||
HAL_Delay(20);
|
||||
|
||||
s_calib = 0;
|
||||
s_temp = 25;
|
||||
s_temp_ctr = 0;
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* ---- bno055_read() ---- */
|
||||
/*
|
||||
* Performs two I2C burst reads (~3ms total at 100kHz):
|
||||
* 1. REG_GYRO_X_LSB → 12 bytes: gyro(6) + euler(6)
|
||||
* 2. REG_LIA_X_LSB → 12 bytes: linear_accel(6) + gravity(6)
|
||||
*
|
||||
* BNO055 coordinate frame (NDOF, no remapping):
|
||||
* X: right, Y: forward, Z: up
|
||||
* Euler H: yaw (0–360°, clockwise from north)
|
||||
* Euler R: roll (–180°..+180°, right bank positive)
|
||||
* Euler P: pitch (–90°..+90°, nose up positive)
|
||||
* Gyro Y: pitch rate (deg/s, nose-up positive)
|
||||
*
|
||||
* Linear accel X (m/s²): forward-backward acceleration (gravity removed)
|
||||
* Gravity Z (m/s²): gravity component on Z axis (≈9.81 when level)
|
||||
*/
|
||||
void bno055_read(IMUData *data)
|
||||
{
|
||||
uint8_t buf[12];
|
||||
|
||||
/* ---- Burst 1: Gyro (0x14–0x19) + Euler (0x1A–0x1F) ---- */
|
||||
rdblk(REG_GYRO_X_LSB, buf, 12);
|
||||
|
||||
/* Gyro: bytes 0..5 = [Gx_L, Gx_H, Gy_L, Gy_H, Gz_L, Gz_H]
|
||||
* Gy = pitch rate (Y axis = pitch axis in BNO055 frame) */
|
||||
float pitch_rate = (float)(int16_t)((uint16_t)buf[3] << 8 | buf[2]) / GYRO_SCALE;
|
||||
|
||||
/* Euler: bytes 6..11 = [H_L, H_H, R_L, R_H, P_L, P_H] */
|
||||
float heading = (float)(int16_t)((uint16_t)buf[7] << 8 | buf[6]) / EULER_SCALE;
|
||||
float roll = (float)(int16_t)((uint16_t)buf[9] << 8 | buf[8]) / EULER_SCALE;
|
||||
float pitch = (float)(int16_t)((uint16_t)buf[11] << 8 | buf[10]) / EULER_SCALE;
|
||||
|
||||
/* ---- Burst 2: Linear accel (0x28–0x2D) + Gravity (0x2E–0x33) ---- */
|
||||
rdblk(REG_LIA_X_LSB, buf, 12);
|
||||
|
||||
/* Linear accel X (forward, m/s²) — gravity-compensated */
|
||||
float lia_x_ms2 = (float)(int16_t)((uint16_t)buf[1] << 8 | buf[0]) / ACCEL_SCALE;
|
||||
|
||||
/* Gravity Z (up axis, m/s²) — should be ≈+9.81 when level */
|
||||
float grv_z_ms2 = (float)(int16_t)((uint16_t)buf[11] << 8 | buf[10]) / ACCEL_SCALE;
|
||||
|
||||
/* Fill IMUData in same units as mpu6000_read() */
|
||||
data->pitch = pitch;
|
||||
data->pitch_rate = pitch_rate;
|
||||
data->roll = roll;
|
||||
data->yaw = heading;
|
||||
data->accel_x = lia_x_ms2 / G_MS2; /* m/s² → g */
|
||||
data->accel_z = grv_z_ms2 / G_MS2; /* m/s² → g (≈1.0 when level) */
|
||||
|
||||
/* Periodically update calibration status and temperature (~1Hz at 250Hz loop) */
|
||||
if (++s_temp_ctr >= 250u) {
|
||||
s_temp_ctr = 0;
|
||||
s_calib = rd(REG_CALIB_STAT);
|
||||
s_temp = (int8_t)rd(REG_TEMP);
|
||||
|
||||
/*
|
||||
* Auto-save offsets once when full calibration is achieved for
|
||||
* the first time this session (sys=3, acc=3, gyr=3, don't wait
|
||||
* for mag=3 as it is harder to achieve and not critical for balance).
|
||||
*/
|
||||
static bool s_saved = false;
|
||||
if (!s_saved) {
|
||||
uint8_t sys_cal = (s_calib & BNO055_CAL_SYS_MASK) >> 6;
|
||||
uint8_t acc_cal = (s_calib & BNO055_CAL_ACC_MASK) >> 2;
|
||||
uint8_t gyr_cal = (s_calib & BNO055_CAL_GYR_MASK) >> 4;
|
||||
if (sys_cal >= 1u && acc_cal >= 3u && gyr_cal >= 3u) {
|
||||
bno055_save_offsets();
|
||||
s_saved = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* ---- bno055_is_ready() ---- */
|
||||
bool bno055_is_ready(void)
|
||||
{
|
||||
if (!s_present) return false;
|
||||
/* If we restored saved offsets, trust them immediately */
|
||||
if (s_offsets_ok) return true;
|
||||
/* Otherwise require gyro ≥ 2 and accel ≥ 2 */
|
||||
uint8_t acc_cal = (s_calib & BNO055_CAL_ACC_MASK) >> 2;
|
||||
uint8_t gyr_cal = (s_calib & BNO055_CAL_GYR_MASK) >> 4;
|
||||
return (acc_cal >= 2u) && (gyr_cal >= 2u);
|
||||
}
|
||||
|
||||
/* ---- bno055_calib_status() ---- */
|
||||
uint8_t bno055_calib_status(void)
|
||||
{
|
||||
return s_calib;
|
||||
}
|
||||
|
||||
/* ---- bno055_temperature() ---- */
|
||||
int8_t bno055_temperature(void)
|
||||
{
|
||||
return s_temp;
|
||||
}
|
||||
24
src/main.c
24
src/main.c
@ -218,7 +218,7 @@ int main(void) {
|
||||
if (jlink_state.arm_req) {
|
||||
jlink_state.arm_req = 0u;
|
||||
if (!safety_remote_estop_active() &&
|
||||
mpu6000_is_calibrated() &&
|
||||
imu_calibrated() &&
|
||||
bal.state == BALANCE_DISARMED && fabsf(bal.pitch_deg) < 10.0f) {
|
||||
safety_arm_start(now);
|
||||
}
|
||||
@ -260,7 +260,7 @@ int main(void) {
|
||||
if (rc_armed_now && !s_rc_armed_prev) {
|
||||
/* Rising edge: start arm hold (motors enable after ARMING_HOLD_MS) */
|
||||
if (!safety_remote_estop_active() &&
|
||||
mpu6000_is_calibrated() &&
|
||||
imu_calibrated() &&
|
||||
bal.state == BALANCE_DISARMED && fabsf(bal.pitch_deg) < 10.0f) {
|
||||
safety_arm_start(now);
|
||||
}
|
||||
@ -277,7 +277,7 @@ int main(void) {
|
||||
if (cdc_arm_request) {
|
||||
cdc_arm_request = 0;
|
||||
if (!safety_remote_estop_active() &&
|
||||
mpu6000_is_calibrated() &&
|
||||
imu_calibrated() &&
|
||||
bal.state == BALANCE_DISARMED && fabsf(bal.pitch_deg) < 10.0f) {
|
||||
safety_arm_start(now);
|
||||
}
|
||||
@ -315,9 +315,14 @@ int main(void) {
|
||||
jetson_cmd_process();
|
||||
}
|
||||
|
||||
/* Balance PID at 1kHz — apply Jetson speed offset when active.
|
||||
* jlink takes priority; fall back to legacy CDC jetson_cmd if alive. */
|
||||
/*
|
||||
* Balance PID — 1kHz with MPU6000/SPI, ~250Hz with BNO055/I2C.
|
||||
* BNO055 read (~3ms) is placed inside the gate so it doesn't block
|
||||
* the full main loop; the gap between ticks is naturally longer.
|
||||
* jlink takes priority for Jetson speed offset; falls back to CDC.
|
||||
*/
|
||||
if (imu_ret == 0 && now - balance_tick >= 1) {
|
||||
if (bno055_active) bno055_read(&imu); /* I2C read inside gate */
|
||||
balance_tick = now;
|
||||
float base_sp = bal.setpoint;
|
||||
if (jlink_is_active(now))
|
||||
@ -397,7 +402,7 @@ int main(void) {
|
||||
}
|
||||
|
||||
/* USB telemetry at 50Hz (only when streaming enabled and calibration done) */
|
||||
if (cdc_streaming && mpu6000_is_calibrated() && now - send_tick >= 20) {
|
||||
if (cdc_streaming && imu_calibrated() && now - send_tick >= 20) {
|
||||
send_tick = now;
|
||||
if (imu_ret == 0) {
|
||||
float err = bal.setpoint - bal.pitch_deg;
|
||||
@ -458,6 +463,13 @@ int main(void) {
|
||||
if (_rs >= ESTOP_REMOTE) es = (int)_rs;
|
||||
else if (bal.state == BALANCE_TILT_FAULT) es = ESTOP_TILT;
|
||||
else es = ESTOP_CLEAR;
|
||||
/* BNO055 calibration status and temperature when active */
|
||||
if (bno055_active) {
|
||||
uint8_t cs = bno055_calib_status();
|
||||
n = snprintf(p, rem, ",\"bno_cs\":%d,\"bno_t\":%d",
|
||||
(int)cs, (int)bno055_temperature());
|
||||
p += n; rem -= n;
|
||||
}
|
||||
n = snprintf(p, rem, ",\"ja\":%d,\"jl\":%d,\"txc\":%u,\"rxc\":%u,\"es\":%d}\n",
|
||||
jetson_cmd_is_active(now) ? 1 : 0,
|
||||
jlink_is_active(now) ? 1 : 0,
|
||||
|
||||
447
test/test_bno055_data.py
Normal file
447
test/test_bno055_data.py
Normal file
@ -0,0 +1,447 @@
|
||||
"""
|
||||
test_bno055_data.py — Issue #135: BNO055 driver unit tests.
|
||||
|
||||
Tests data scaling, byte parsing, calibration status extraction,
|
||||
calibration offset packing/unpacking, and temperature handling.
|
||||
|
||||
No HAL or STM32 hardware required — pure Python logic.
|
||||
"""
|
||||
|
||||
import struct
|
||||
import pytest
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# BNO055 scaling constants (mirror bno055.c)
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
EULER_SCALE = 16.0 # 1 degree = 16 LSB
|
||||
GYRO_SCALE = 16.0 # 1 dps = 16 LSB
|
||||
ACCEL_SCALE = 100.0 # 1 m/s² = 100 LSB
|
||||
G_MS2 = 9.80665
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Calibration status bit extraction
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
BNO055_CAL_SYS_MASK = 0xC0
|
||||
BNO055_CAL_GYR_MASK = 0x30
|
||||
BNO055_CAL_ACC_MASK = 0x0C
|
||||
BNO055_CAL_MAG_MASK = 0x03
|
||||
|
||||
|
||||
def parse_calib_status(byte):
|
||||
"""Return (sys, gyr, acc, mag) calibration levels 0-3."""
|
||||
sys = (byte & BNO055_CAL_SYS_MASK) >> 6
|
||||
gyr = (byte & BNO055_CAL_GYR_MASK) >> 4
|
||||
acc = (byte & BNO055_CAL_ACC_MASK) >> 2
|
||||
mag = (byte & BNO055_CAL_MAG_MASK) >> 0
|
||||
return sys, gyr, acc, mag
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Offset packing (mirrors bno055_save_offsets / bno055_restore_offsets)
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
BNO055_BKP_MAGIC = 0xB055CA10
|
||||
|
||||
|
||||
def pack_offsets(offsets22: bytes):
|
||||
"""Pack 22 bytes into 6 x uint32 words + magic word (7 words total)."""
|
||||
assert len(offsets22) == 22
|
||||
words = []
|
||||
for i in range(5):
|
||||
w = (offsets22[i * 4 + 0]
|
||||
| (offsets22[i * 4 + 1] << 8)
|
||||
| (offsets22[i * 4 + 2] << 16)
|
||||
| (offsets22[i * 4 + 3] << 24))
|
||||
words.append(w & 0xFFFFFFFF)
|
||||
# Last word: only bytes 20 and 21
|
||||
w5 = offsets22[20] | (offsets22[21] << 8)
|
||||
words.append(w5 & 0xFFFFFFFF)
|
||||
words.append(BNO055_BKP_MAGIC)
|
||||
return words # 7 uint32 words
|
||||
|
||||
|
||||
def unpack_offsets(words):
|
||||
"""Reverse of pack_offsets. Returns 22 bytes."""
|
||||
assert len(words) == 7
|
||||
assert words[6] == BNO055_BKP_MAGIC
|
||||
result = []
|
||||
for i in range(5):
|
||||
w = words[i]
|
||||
result.append(w & 0xFF)
|
||||
result.append((w >> 8) & 0xFF)
|
||||
result.append((w >> 16) & 0xFF)
|
||||
result.append((w >> 24) & 0xFF)
|
||||
# Last word: only bytes 20 and 21
|
||||
result.append(words[5] & 0xFF)
|
||||
result.append((words[5] >> 8) & 0xFF)
|
||||
return bytes(result)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# IMUData parsing helpers (mirror bno055_read() logic)
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
def parse_euler(buf6: bytes):
|
||||
"""Parse 6-byte Euler burst (H, R, P each int16 LE) → (heading, roll, pitch) in degrees."""
|
||||
h_raw = struct.unpack_from("<h", buf6, 0)[0]
|
||||
r_raw = struct.unpack_from("<h", buf6, 2)[0]
|
||||
p_raw = struct.unpack_from("<h", buf6, 4)[0]
|
||||
return h_raw / EULER_SCALE, r_raw / EULER_SCALE, p_raw / EULER_SCALE
|
||||
|
||||
|
||||
def parse_gyro(buf6: bytes):
|
||||
"""Parse 6-byte gyro burst (Gx, Gy, Gz each int16 LE) → (gx, gy, gz) in dps."""
|
||||
gx = struct.unpack_from("<h", buf6, 0)[0] / GYRO_SCALE
|
||||
gy = struct.unpack_from("<h", buf6, 2)[0] / GYRO_SCALE
|
||||
gz = struct.unpack_from("<h", buf6, 4)[0] / GYRO_SCALE
|
||||
return gx, gy, gz
|
||||
|
||||
|
||||
def parse_lia_x(buf2: bytes):
|
||||
"""Parse 2-byte linear accel X (int16 LE) → m/s²."""
|
||||
raw = struct.unpack_from("<h", buf2, 0)[0]
|
||||
return raw / ACCEL_SCALE
|
||||
|
||||
|
||||
def parse_grv_z(buf2: bytes):
|
||||
"""Parse 2-byte gravity Z (int16 LE) → m/s²."""
|
||||
raw = struct.unpack_from("<h", buf2, 0)[0]
|
||||
return raw / ACCEL_SCALE
|
||||
|
||||
|
||||
def ms2_to_g(ms2):
|
||||
return ms2 / G_MS2
|
||||
|
||||
|
||||
# Mirroring bno055_read() 12-byte burst parsing:
|
||||
# buf[0:5] = gyro X, Y, Z
|
||||
# buf[6:11] = euler H, R, P
|
||||
def parse_burst1(buf12: bytes):
|
||||
"""Parse 12-byte burst1 (gyro 6B + euler 6B) from REG_GYRO_X_LSB."""
|
||||
gyro = buf12[0:6]
|
||||
euler = buf12[6:12]
|
||||
_, pitch_rate, _ = parse_gyro(gyro) # gy = pitch rate
|
||||
heading, roll, pitch = parse_euler(euler)
|
||||
return pitch, roll, heading, pitch_rate
|
||||
|
||||
|
||||
def parse_burst2(buf12: bytes):
|
||||
"""Parse 12-byte burst2 (LIA 6B + GRV 6B) from REG_LIA_X_LSB."""
|
||||
lia_x_ms2 = struct.unpack_from("<h", buf12, 0)[0] / ACCEL_SCALE # LIA X
|
||||
grv_z_ms2 = struct.unpack_from("<h", buf12, 10)[0] / ACCEL_SCALE # GRV Z (bytes 10-11)
|
||||
return ms2_to_g(lia_x_ms2), ms2_to_g(grv_z_ms2)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Tests: Calibration Status
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
class TestCalibStatus:
|
||||
|
||||
def test_all_uncalibrated(self):
|
||||
sys, gyr, acc, mag = parse_calib_status(0x00)
|
||||
assert sys == 0 and gyr == 0 and acc == 0 and mag == 0
|
||||
|
||||
def test_all_fully_calibrated(self):
|
||||
sys, gyr, acc, mag = parse_calib_status(0xFF)
|
||||
assert sys == 3 and gyr == 3 and acc == 3 and mag == 3
|
||||
|
||||
def test_sys_only(self):
|
||||
sys, gyr, acc, mag = parse_calib_status(0xC0)
|
||||
assert sys == 3 and gyr == 0 and acc == 0 and mag == 0
|
||||
|
||||
def test_gyr_only(self):
|
||||
sys, gyr, acc, mag = parse_calib_status(0x30)
|
||||
assert sys == 0 and gyr == 3 and acc == 0 and mag == 0
|
||||
|
||||
def test_acc_only(self):
|
||||
sys, gyr, acc, mag = parse_calib_status(0x0C)
|
||||
assert sys == 0 and gyr == 0 and acc == 3 and mag == 0
|
||||
|
||||
def test_mag_only(self):
|
||||
sys, gyr, acc, mag = parse_calib_status(0x03)
|
||||
assert sys == 0 and gyr == 0 and acc == 0 and mag == 3
|
||||
|
||||
def test_partially_calibrated(self):
|
||||
# sys=2, gyr=3, acc=1, mag=0 → 0b10_11_01_00 = 0xB4
|
||||
sys, gyr, acc, mag = parse_calib_status(0xB4)
|
||||
assert sys == 2 and gyr == 3 and acc == 1 and mag == 0
|
||||
|
||||
def test_bno055_is_ready_logic(self):
|
||||
"""bno055_is_ready: requires acc≥2 AND gyr≥2 (when no offsets restored)."""
|
||||
for calib_byte in range(256):
|
||||
_, gyr, acc, _ = parse_calib_status(calib_byte)
|
||||
expected_ready = (acc >= 2) and (gyr >= 2)
|
||||
actual_ready = (acc >= 2) and (gyr >= 2)
|
||||
assert actual_ready == expected_ready
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Tests: Euler Angle Parsing
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
class TestEulerParsing:
|
||||
|
||||
def test_zero(self):
|
||||
buf = struct.pack("<hhh", 0, 0, 0)
|
||||
h, r, p = parse_euler(buf)
|
||||
assert h == 0.0 and r == 0.0 and p == 0.0
|
||||
|
||||
def test_heading_north(self):
|
||||
buf = struct.pack("<hhh", 0, 0, 0)
|
||||
h, _, _ = parse_euler(buf)
|
||||
assert h == 0.0
|
||||
|
||||
def test_heading_east_90deg(self):
|
||||
raw = int(90 * EULER_SCALE)
|
||||
buf = struct.pack("<hhh", raw, 0, 0)
|
||||
h, _, _ = parse_euler(buf)
|
||||
assert abs(h - 90.0) < 0.001
|
||||
|
||||
def test_heading_360deg(self):
|
||||
raw = int(360 * EULER_SCALE)
|
||||
buf = struct.pack("<hhh", raw, 0, 0)
|
||||
h, _, _ = parse_euler(buf)
|
||||
assert abs(h - 360.0) < 0.001
|
||||
|
||||
def test_pitch_nose_up_5deg(self):
|
||||
raw = int(5.0 * EULER_SCALE)
|
||||
buf = struct.pack("<hhh", 0, 0, raw)
|
||||
_, _, p = parse_euler(buf)
|
||||
assert abs(p - 5.0) < 0.001
|
||||
|
||||
def test_pitch_nose_down_negative(self):
|
||||
raw = int(-12.5 * EULER_SCALE)
|
||||
buf = struct.pack("<hhh", 0, 0, raw)
|
||||
_, _, p = parse_euler(buf)
|
||||
assert abs(p - (-12.5)) < 0.001
|
||||
|
||||
def test_roll_positive(self):
|
||||
raw = int(30.0 * EULER_SCALE)
|
||||
buf = struct.pack("<hhh", 0, raw, 0)
|
||||
_, r, _ = parse_euler(buf)
|
||||
assert abs(r - 30.0) < 0.001
|
||||
|
||||
def test_fractional_degree(self):
|
||||
"""0.0625 degree resolution (1/16)."""
|
||||
raw = 1 # 1 LSB = 1/16 degree
|
||||
buf = struct.pack("<hhh", raw, 0, 0)
|
||||
h, _, _ = parse_euler(buf)
|
||||
assert abs(h - (1.0 / EULER_SCALE)) < 1e-6
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Tests: Gyro Rate Parsing
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
class TestGyroParsing:
|
||||
|
||||
def test_zero_rates(self):
|
||||
buf = struct.pack("<hhh", 0, 0, 0)
|
||||
gx, gy, gz = parse_gyro(buf)
|
||||
assert gx == 0.0 and gy == 0.0 and gz == 0.0
|
||||
|
||||
def test_pitch_rate_positive(self):
|
||||
raw = int(45.0 * GYRO_SCALE) # 45 dps nose-up
|
||||
buf = struct.pack("<hhh", 0, raw, 0)
|
||||
_, gy, _ = parse_gyro(buf)
|
||||
assert abs(gy - 45.0) < 0.001
|
||||
|
||||
def test_pitch_rate_negative(self):
|
||||
raw = int(-90.0 * GYRO_SCALE)
|
||||
buf = struct.pack("<hhh", 0, raw, 0)
|
||||
_, gy, _ = parse_gyro(buf)
|
||||
assert abs(gy - (-90.0)) < 0.001
|
||||
|
||||
def test_max_range_900dps(self):
|
||||
raw = int(900 * GYRO_SCALE)
|
||||
buf = struct.pack("<hhh", raw, 0, 0)
|
||||
gx, _, _ = parse_gyro(buf)
|
||||
assert abs(gx - 900.0) < 0.001
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Tests: Linear Accel + Gravity Parsing
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
class TestAccelParsing:
|
||||
|
||||
def test_zero_linear_accel(self):
|
||||
buf = struct.pack("<h", 0)
|
||||
assert parse_lia_x(buf) == 0.0
|
||||
|
||||
def test_forward_accel(self):
|
||||
raw = int(1.0 * ACCEL_SCALE) # 1 m/s² forward
|
||||
buf = struct.pack("<h", raw)
|
||||
assert abs(parse_lia_x(buf) - 1.0) < 0.001
|
||||
|
||||
def test_level_gravity_z(self):
|
||||
"""Level robot: gravity Z ≈ 9.80665 m/s²."""
|
||||
raw = int(G_MS2 * ACCEL_SCALE)
|
||||
buf = struct.pack("<h", raw)
|
||||
assert abs(parse_grv_z(buf) - G_MS2) < 0.02
|
||||
|
||||
def test_gravity_to_g_conversion(self):
|
||||
raw = int(G_MS2 * ACCEL_SCALE)
|
||||
buf = struct.pack("<h", raw)
|
||||
g_val = ms2_to_g(parse_grv_z(buf))
|
||||
assert abs(g_val - 1.0) < 0.002
|
||||
|
||||
def test_lia_negative(self):
|
||||
raw = int(-2.5 * ACCEL_SCALE)
|
||||
buf = struct.pack("<h", raw)
|
||||
assert abs(parse_lia_x(buf) - (-2.5)) < 0.001
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Tests: 12-Byte Burst Parsing (mirrors bno055_read internals)
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
class TestBurstParsing:
|
||||
|
||||
def _make_burst1(self, gx=0, gy=0, gz=0, heading=0.0, roll=0.0, pitch=0.0):
|
||||
gyro = struct.pack("<hhh",
|
||||
int(gx * GYRO_SCALE),
|
||||
int(gy * GYRO_SCALE),
|
||||
int(gz * GYRO_SCALE))
|
||||
euler = struct.pack("<hhh",
|
||||
int(heading * EULER_SCALE),
|
||||
int(roll * EULER_SCALE),
|
||||
int(pitch * EULER_SCALE))
|
||||
return gyro + euler
|
||||
|
||||
def _make_burst2(self, lia_x_ms2=0.0, grv_z_ms2=G_MS2):
|
||||
lia = struct.pack("<hhh", int(lia_x_ms2 * ACCEL_SCALE), 0, 0)
|
||||
grv = struct.pack("<hhh", 0, 0, int(grv_z_ms2 * ACCEL_SCALE))
|
||||
return lia + grv
|
||||
|
||||
def test_pitch_from_burst1(self):
|
||||
buf = self._make_burst1(pitch=7.5)
|
||||
pitch, _, _, _ = parse_burst1(buf)
|
||||
assert abs(pitch - 7.5) < 0.001
|
||||
|
||||
def test_yaw_from_burst1_heading(self):
|
||||
buf = self._make_burst1(heading=180.0)
|
||||
_, _, yaw, _ = parse_burst1(buf)
|
||||
assert abs(yaw - 180.0) < 0.001
|
||||
|
||||
def test_pitch_rate_from_burst1_gy(self):
|
||||
buf = self._make_burst1(gy=30.0)
|
||||
_, _, _, pitch_rate = parse_burst1(buf)
|
||||
assert abs(pitch_rate - 30.0) < 0.001
|
||||
|
||||
def test_accel_x_g_from_burst2(self):
|
||||
buf = self._make_burst2(lia_x_ms2=G_MS2) # 1g forward
|
||||
accel_x_g, _ = parse_burst2(buf)
|
||||
assert abs(accel_x_g - 1.0) < 0.002
|
||||
|
||||
def test_gravity_z_level_from_burst2(self):
|
||||
buf = self._make_burst2(grv_z_ms2=G_MS2) # level
|
||||
_, accel_z_g = parse_burst2(buf)
|
||||
assert abs(accel_z_g - 1.0) < 0.002
|
||||
|
||||
def test_tilted_gravity(self):
|
||||
"""Tilted 30°: gravity Z = cos(30°) × 9.80665 ≈ 8.495 m/s²."""
|
||||
import math
|
||||
grv_z = G_MS2 * math.cos(math.radians(30))
|
||||
buf = self._make_burst2(grv_z_ms2=grv_z)
|
||||
_, accel_z_g = parse_burst2(buf)
|
||||
expected = grv_z / G_MS2
|
||||
assert abs(accel_z_g - expected) < 0.01
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Tests: Calibration Offset Packing / Backup Register Round-trip
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
class TestOffsetPacking:
|
||||
|
||||
def _make_offsets(self, fill=0):
|
||||
return bytes([fill] * 22)
|
||||
|
||||
def test_pack_length(self):
|
||||
words = pack_offsets(self._make_offsets(0))
|
||||
assert len(words) == 7
|
||||
|
||||
def test_magic_in_last_word(self):
|
||||
words = pack_offsets(self._make_offsets(0))
|
||||
assert words[6] == BNO055_BKP_MAGIC
|
||||
|
||||
def test_round_trip_zeros(self):
|
||||
orig = self._make_offsets(0)
|
||||
words = pack_offsets(orig)
|
||||
recovered = unpack_offsets(words)
|
||||
assert recovered == orig
|
||||
|
||||
def test_round_trip_all_ones(self):
|
||||
orig = self._make_offsets(0xFF)
|
||||
words = pack_offsets(orig)
|
||||
recovered = unpack_offsets(words)
|
||||
assert recovered == orig
|
||||
|
||||
def test_round_trip_sequential(self):
|
||||
orig = bytes(range(22))
|
||||
words = pack_offsets(orig)
|
||||
recovered = unpack_offsets(words)
|
||||
assert recovered == orig
|
||||
|
||||
def test_round_trip_real_offsets(self):
|
||||
"""Simulate a realistic BNO055 calibration offset set."""
|
||||
# accel offset X/Y/Z (int16 LE), mag offset X/Y/Z, gyro offset X/Y/Z, radii
|
||||
data = struct.pack("<hhhhhhhhhhh",
|
||||
-45, 12, 980, # accel offsets (mg units)
|
||||
150, -200, 50, # mag offsets (LSB units)
|
||||
3, -7, 2, # gyro offsets (dps×16)
|
||||
1000, 800) # accel_radius, mag_radius
|
||||
assert len(data) == 22
|
||||
words = pack_offsets(data)
|
||||
recovered = unpack_offsets(words)
|
||||
assert recovered == data
|
||||
|
||||
def test_invalid_magic_rejected(self):
|
||||
words = pack_offsets(bytes(range(22)))
|
||||
words_bad = words[:6] + [0xDEADBEEF] # corrupt magic
|
||||
with pytest.raises(AssertionError):
|
||||
unpack_offsets(words_bad)
|
||||
|
||||
def test_only_22_bytes_preserved(self):
|
||||
"""Words 5's high 2 bytes are unused — should be zero after unpack."""
|
||||
orig = bytes(range(22))
|
||||
words = pack_offsets(orig)
|
||||
# High 2 bytes of word[5] should be zero (only 2 bytes used)
|
||||
assert (words[5] >> 16) == 0
|
||||
|
||||
def test_byte_order_word0(self):
|
||||
"""First 4 bytes of offsets pack into word[0] as little-endian."""
|
||||
orig = bytes([0x01, 0x02, 0x03, 0x04] + [0] * 18)
|
||||
words = pack_offsets(orig)
|
||||
assert words[0] == 0x04030201
|
||||
|
||||
def test_byte_order_word5_low(self):
|
||||
"""Bytes 20-21 pack into low 16 bits of word[5]."""
|
||||
orig = bytes([0] * 20 + [0xAB, 0xCD])
|
||||
words = pack_offsets(orig)
|
||||
assert words[5] == 0xCDAB
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Tests: Temperature
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
class TestTemperature:
|
||||
|
||||
def test_room_temp_range(self):
|
||||
"""Room temperature should be in –40..+85°C range."""
|
||||
for t in [-40, 0, 25, 85]:
|
||||
assert -40 <= t <= 85
|
||||
|
||||
def test_temperature_is_signed_byte(self):
|
||||
"""BNO055 TEMP register is signed int8 → –128..+127 but useful range –40..+85."""
|
||||
import ctypes
|
||||
for raw in [25, 0x19, 0xFF]:
|
||||
val = ctypes.c_int8(raw).value
|
||||
assert -128 <= val <= 127
|
||||
@ -1,11 +1,14 @@
|
||||
/**
|
||||
* App.jsx — Saltybot Social + Telemetry Dashboard root component.
|
||||
* App.jsx — Saltybot Social + Telemetry + Fleet Dashboard root component.
|
||||
*
|
||||
* Social tabs (issue #107):
|
||||
* Status | Faces | Conversation | Personality | Navigation
|
||||
*
|
||||
* Telemetry tabs (issue #126):
|
||||
* IMU | Battery | Motors | Map | Control | Health
|
||||
*
|
||||
* Fleet tabs (issue #139):
|
||||
* Fleet (self-contained via useFleet)
|
||||
*/
|
||||
|
||||
import { useState, useCallback } from 'react';
|
||||
@ -26,6 +29,9 @@ import { MapViewer } from './components/MapViewer.jsx';
|
||||
import { ControlMode } from './components/ControlMode.jsx';
|
||||
import { SystemHealth } from './components/SystemHealth.jsx';
|
||||
|
||||
// Fleet panel (issue #139)
|
||||
import { FleetPanel } from './components/FleetPanel.jsx';
|
||||
|
||||
const TAB_GROUPS = [
|
||||
{
|
||||
label: 'SOCIAL',
|
||||
@ -50,6 +56,13 @@ const TAB_GROUPS = [
|
||||
{ id: 'health', label: 'Health', },
|
||||
],
|
||||
},
|
||||
{
|
||||
label: 'FLEET',
|
||||
color: 'text-green-600',
|
||||
tabs: [
|
||||
{ id: 'fleet', label: 'Fleet' },
|
||||
],
|
||||
},
|
||||
];
|
||||
|
||||
const DEFAULT_WS_URL = 'ws://localhost:9090';
|
||||
@ -122,7 +135,9 @@ export default function App() {
|
||||
<span className="text-orange-500 font-bold tracking-widest text-sm">⚡ SALTYBOT</span>
|
||||
<span className="text-cyan-800 text-xs hidden sm:inline">DASHBOARD</span>
|
||||
</div>
|
||||
<ConnectionBar url={wsUrl} setUrl={setWsUrl} connected={connected} error={error} />
|
||||
{activeTab !== 'fleet' && (
|
||||
<ConnectionBar url={wsUrl} setUrl={setWsUrl} connected={connected} error={error} />
|
||||
)}
|
||||
</header>
|
||||
|
||||
{/* ── Tab Navigation ── */}
|
||||
@ -172,14 +187,22 @@ export default function App() {
|
||||
{activeTab === 'map' && <MapViewer subscribe={subscribe} />}
|
||||
{activeTab === 'control' && <ControlMode subscribe={subscribe} />}
|
||||
{activeTab === 'health' && <SystemHealth subscribe={subscribe} />}
|
||||
|
||||
{activeTab === 'fleet' && <FleetPanel />}
|
||||
</main>
|
||||
|
||||
{/* ── Footer ── */}
|
||||
<footer className="bg-[#070712] border-t border-cyan-950 px-4 py-1.5 flex items-center justify-between text-xs text-gray-700 shrink-0">
|
||||
<span>rosbridge: <code className="text-gray-600">{wsUrl}</code></span>
|
||||
<span className={connected ? 'text-green-700' : 'text-red-900'}>
|
||||
{connected ? 'CONNECTED' : 'DISCONNECTED'}
|
||||
</span>
|
||||
{activeTab !== 'fleet' ? (
|
||||
<>
|
||||
<span>rosbridge: <code className="text-gray-600">{wsUrl}</code></span>
|
||||
<span className={connected ? 'text-green-700' : 'text-red-900'}>
|
||||
{connected ? 'CONNECTED' : 'DISCONNECTED'}
|
||||
</span>
|
||||
</>
|
||||
) : (
|
||||
<span className="text-green-800">FLEET MODE — multi-robot</span>
|
||||
)}
|
||||
</footer>
|
||||
</div>
|
||||
);
|
||||
|
||||
803
ui/social-bot/src/components/FleetPanel.jsx
Normal file
803
ui/social-bot/src/components/FleetPanel.jsx
Normal file
@ -0,0 +1,803 @@
|
||||
/**
|
||||
* FleetPanel.jsx — Multi-robot fleet management dashboard.
|
||||
*
|
||||
* Sub-views (internal tab nav):
|
||||
* Robots | Map | Missions | Video | Alerts | Detail
|
||||
*
|
||||
* Uses useFleet() for multi-robot WebSocket multiplexing.
|
||||
* Robot detail re-uses single-robot panel components via subscribeRobot adapter.
|
||||
*
|
||||
* Video streaming:
|
||||
* http://<host>:<videoPort>/stream?topic=<videoTopic>&type=ros_compressed
|
||||
* (web_video_server / mjpeg_server)
|
||||
*/
|
||||
|
||||
import { useState, useRef, useCallback, useEffect } from 'react';
|
||||
import { useFleet } from '../hooks/useFleet.js';
|
||||
|
||||
// Reusable single-robot detail panels (re-used via subscribeRobot adapter)
|
||||
import { ImuPanel } from './ImuPanel.jsx';
|
||||
import { BatteryPanel } from './BatteryPanel.jsx';
|
||||
import { MotorPanel } from './MotorPanel.jsx';
|
||||
import { ControlMode } from './ControlMode.jsx';
|
||||
import { SystemHealth } from './SystemHealth.jsx';
|
||||
|
||||
// ── Constants ───────────────────────────────────────────────────────────────
|
||||
|
||||
const VARIANT_COLORS = {
|
||||
balance: 'text-cyan-400 border-cyan-800',
|
||||
rover: 'text-green-400 border-green-800',
|
||||
tank: 'text-amber-400 border-amber-800',
|
||||
social: 'text-purple-400 border-purple-800',
|
||||
};
|
||||
|
||||
const FLEET_VIEWS = [
|
||||
{ id: 'robots', label: 'Robots' },
|
||||
{ id: 'map', label: 'Fleet Map'},
|
||||
{ id: 'missions', label: 'Missions' },
|
||||
{ id: 'video', label: 'Video' },
|
||||
{ id: 'alerts', label: 'Alerts' },
|
||||
];
|
||||
|
||||
// ── Add-Robot modal ──────────────────────────────────────────────────────────
|
||||
|
||||
function AddRobotModal({ onAdd, onClose }) {
|
||||
const [form, setForm] = useState({
|
||||
name: '', variant: 'balance', host: '', port: '9090',
|
||||
videoPort: '8080', videoTopic: '/camera/panoramic/compressed',
|
||||
});
|
||||
const set = (k, v) => setForm(f => ({ ...f, [k]: v }));
|
||||
|
||||
const handleSubmit = (e) => {
|
||||
e.preventDefault();
|
||||
if (!form.host.trim()) return;
|
||||
onAdd({
|
||||
...form,
|
||||
port: Number(form.port),
|
||||
videoPort: Number(form.videoPort),
|
||||
});
|
||||
onClose();
|
||||
};
|
||||
|
||||
return (
|
||||
<div className="fixed inset-0 bg-black bg-opacity-70 flex items-center justify-center z-50 p-4">
|
||||
<div className="bg-gray-950 border border-cyan-800 rounded-lg p-5 w-full max-w-md">
|
||||
<div className="flex items-center justify-between mb-4">
|
||||
<span className="text-cyan-400 font-bold tracking-widest text-sm">ADD ROBOT</span>
|
||||
<button onClick={onClose} className="text-gray-600 hover:text-gray-300">✕</button>
|
||||
</div>
|
||||
<form onSubmit={handleSubmit} className="space-y-3">
|
||||
<div className="grid grid-cols-2 gap-3">
|
||||
<label className="col-span-2 space-y-1">
|
||||
<span className="text-gray-500 text-xs">Display Name</span>
|
||||
<input className="w-full bg-gray-900 border border-gray-700 rounded px-2 py-1.5 text-sm text-gray-200 focus:outline-none focus:border-cyan-700"
|
||||
value={form.name} onChange={e => set('name', e.target.value)} placeholder="SaltyBot #2" />
|
||||
</label>
|
||||
<label className="space-y-1">
|
||||
<span className="text-gray-500 text-xs">Variant</span>
|
||||
<select className="w-full bg-gray-900 border border-gray-700 rounded px-2 py-1.5 text-sm text-gray-200 focus:outline-none focus:border-cyan-700"
|
||||
value={form.variant} onChange={e => set('variant', e.target.value)}>
|
||||
<option value="balance">Balance</option>
|
||||
<option value="rover">Rover</option>
|
||||
<option value="tank">Tank</option>
|
||||
<option value="social">Social</option>
|
||||
</select>
|
||||
</label>
|
||||
<label className="space-y-1">
|
||||
<span className="text-gray-500 text-xs">Rosbridge Host</span>
|
||||
<input className="w-full bg-gray-900 border border-gray-700 rounded px-2 py-1.5 text-sm text-gray-200 focus:outline-none focus:border-cyan-700"
|
||||
value={form.host} onChange={e => set('host', e.target.value)} placeholder="192.168.1.101" required />
|
||||
</label>
|
||||
<label className="space-y-1">
|
||||
<span className="text-gray-500 text-xs">Rosbridge Port</span>
|
||||
<input type="number" className="w-full bg-gray-900 border border-gray-700 rounded px-2 py-1.5 text-sm text-gray-200 focus:outline-none focus:border-cyan-700"
|
||||
value={form.port} onChange={e => set('port', e.target.value)} />
|
||||
</label>
|
||||
<label className="space-y-1">
|
||||
<span className="text-gray-500 text-xs">Video Port</span>
|
||||
<input type="number" className="w-full bg-gray-900 border border-gray-700 rounded px-2 py-1.5 text-sm text-gray-200 focus:outline-none focus:border-cyan-700"
|
||||
value={form.videoPort} onChange={e => set('videoPort', e.target.value)} />
|
||||
</label>
|
||||
<label className="space-y-1">
|
||||
<span className="text-gray-500 text-xs">Video Topic</span>
|
||||
<input className="w-full bg-gray-900 border border-gray-700 rounded px-2 py-1.5 text-sm text-gray-200 focus:outline-none focus:border-cyan-700"
|
||||
value={form.videoTopic} onChange={e => set('videoTopic', e.target.value)} />
|
||||
</label>
|
||||
</div>
|
||||
<div className="flex gap-2 justify-end pt-2">
|
||||
<button type="button" onClick={onClose}
|
||||
className="px-3 py-1.5 rounded border border-gray-700 text-gray-400 hover:text-gray-200 text-sm">Cancel</button>
|
||||
<button type="submit"
|
||||
className="px-3 py-1.5 rounded bg-cyan-950 border border-cyan-700 text-cyan-300 hover:bg-cyan-900 text-sm font-bold">Add Robot</button>
|
||||
</div>
|
||||
</form>
|
||||
</div>
|
||||
</div>
|
||||
);
|
||||
}
|
||||
|
||||
// ── Sub-view: Robots List ────────────────────────────────────────────────────
|
||||
|
||||
function SocBar({ soc }) {
|
||||
const color = soc < 20 ? '#ef4444' : soc < 40 ? '#f59e0b' : '#22c55e';
|
||||
return (
|
||||
<div className="flex items-center gap-1.5 min-w-[60px]">
|
||||
<div className="flex-1 h-1.5 bg-gray-800 rounded overflow-hidden">
|
||||
<div className="h-full rounded" style={{ width: `${soc}%`, background: color }} />
|
||||
</div>
|
||||
<span className="text-xs w-7 text-right" style={{ color }}>{soc}%</span>
|
||||
</div>
|
||||
);
|
||||
}
|
||||
|
||||
function RobotCard({ robot, connection, data, onSelect, onRemove }) {
|
||||
const conn = connection ?? { connected: false, error: null };
|
||||
const d = data ?? {};
|
||||
const varColor = VARIANT_COLORS[robot.variant] ?? 'text-gray-400 border-gray-700';
|
||||
|
||||
return (
|
||||
<div
|
||||
className="bg-gray-950 border border-gray-800 rounded-lg p-3 flex flex-col gap-2 cursor-pointer hover:border-cyan-800 transition-colors"
|
||||
onClick={() => onSelect(robot.id)}
|
||||
>
|
||||
{/* Header row */}
|
||||
<div className="flex items-center gap-2">
|
||||
<div className={`w-2 h-2 rounded-full shrink-0 ${conn.connected ? 'bg-green-400' : conn.error ? 'bg-red-500 animate-pulse' : 'bg-gray-700'}`} />
|
||||
<span className="font-bold text-sm text-gray-200 flex-1 truncate">{robot.name || robot.id}</span>
|
||||
<span className={`text-xs border rounded px-1.5 py-0.5 font-mono uppercase ${varColor}`}>{robot.variant}</span>
|
||||
<button
|
||||
className="text-gray-700 hover:text-red-500 ml-1 text-xs"
|
||||
title="Remove robot"
|
||||
onClick={e => { e.stopPropagation(); onRemove(robot.id); }}
|
||||
>✕</button>
|
||||
</div>
|
||||
|
||||
{/* Address */}
|
||||
<div className="text-xs text-gray-600 font-mono">{robot.host}:{robot.port}</div>
|
||||
|
||||
{/* Telemetry row */}
|
||||
{conn.connected ? (
|
||||
<div className="grid grid-cols-2 gap-x-3 gap-y-1 text-xs">
|
||||
<div className="flex items-center justify-between">
|
||||
<span className="text-gray-600">Battery</span>
|
||||
<SocBar soc={Math.round(d.battery?.soc ?? 0)} />
|
||||
</div>
|
||||
<div className="flex items-center justify-between">
|
||||
<span className="text-gray-600">State</span>
|
||||
<span className={`font-bold ${
|
||||
d.state === 'ARMED' ? 'text-green-400' :
|
||||
d.state === 'TILT FAULT' ? 'text-red-400 animate-pulse' :
|
||||
d.state ? 'text-amber-400' : 'text-gray-600'
|
||||
}`}>{d.state ?? '—'}</span>
|
||||
</div>
|
||||
<div className="flex items-center justify-between col-span-2">
|
||||
<span className="text-gray-600">Pipeline</span>
|
||||
<span className={`font-bold ${
|
||||
d.pipeline === 'speaking' ? 'text-cyan-400' :
|
||||
d.pipeline === 'thinking' ? 'text-purple-400' :
|
||||
d.pipeline === 'listening' ? 'text-green-400' :
|
||||
'text-gray-600'
|
||||
}`}>{d.pipeline ?? '—'}</span>
|
||||
</div>
|
||||
{d.pose && (
|
||||
<div className="col-span-2 text-gray-600">
|
||||
pos <span className="text-cyan-600">{d.pose.x.toFixed(1)},{d.pose.y.toFixed(1)}</span>
|
||||
{' '}hdg <span className="text-cyan-600">{(d.pose.yaw * 180 / Math.PI).toFixed(0)}°</span>
|
||||
</div>
|
||||
)}
|
||||
</div>
|
||||
) : (
|
||||
<div className="text-xs text-gray-700">
|
||||
{conn.error ? <span className="text-red-700">⚠ {conn.error}</span> : 'Connecting…'}
|
||||
</div>
|
||||
)}
|
||||
|
||||
{d.alerts?.length > 0 && (
|
||||
<div className="flex flex-wrap gap-1">
|
||||
{d.alerts.slice(0, 3).map((a, i) => (
|
||||
<span key={i} className={`text-xs px-1 rounded border ${
|
||||
a.level >= 2 ? 'bg-red-950 border-red-800 text-red-400' : 'bg-amber-950 border-amber-800 text-amber-400'
|
||||
}`}>{a.name}</span>
|
||||
))}
|
||||
</div>
|
||||
)}
|
||||
</div>
|
||||
);
|
||||
}
|
||||
|
||||
function RobotsView({ robots, connections, robotData, onSelect, onRemove, onAdd, onScan, scanning }) {
|
||||
return (
|
||||
<div className="space-y-3">
|
||||
<div className="flex items-center gap-2 flex-wrap">
|
||||
<div className="text-cyan-700 text-xs font-bold tracking-widest">
|
||||
FLEET — {robots.length} ROBOT{robots.length !== 1 ? 'S' : ''}
|
||||
</div>
|
||||
<div className="ml-auto flex gap-2">
|
||||
<button
|
||||
onClick={onScan}
|
||||
disabled={scanning}
|
||||
className="px-3 py-1 rounded border border-gray-700 text-gray-400 hover:border-cyan-700 hover:text-cyan-300 text-xs disabled:opacity-50"
|
||||
>{scanning ? 'Scanning…' : 'Scan LAN'}</button>
|
||||
<button
|
||||
onClick={onAdd}
|
||||
className="px-3 py-1 rounded bg-cyan-950 border border-cyan-700 text-cyan-300 hover:bg-cyan-900 text-xs font-bold"
|
||||
>+ Add Robot</button>
|
||||
</div>
|
||||
</div>
|
||||
{robots.length === 0 ? (
|
||||
<div className="text-center text-gray-600 text-sm py-10 border border-dashed border-gray-800 rounded-lg">
|
||||
No robots configured. Add one or scan the LAN.
|
||||
</div>
|
||||
) : (
|
||||
<div className="grid grid-cols-1 sm:grid-cols-2 lg:grid-cols-3 gap-3">
|
||||
{robots.map(r => (
|
||||
<RobotCard
|
||||
key={r.id}
|
||||
robot={r}
|
||||
connection={connections[r.id]}
|
||||
data={robotData[r.id]}
|
||||
onSelect={onSelect}
|
||||
onRemove={onRemove}
|
||||
/>
|
||||
))}
|
||||
</div>
|
||||
)}
|
||||
</div>
|
||||
);
|
||||
}
|
||||
|
||||
// ── Sub-view: Fleet Map ──────────────────────────────────────────────────────
|
||||
|
||||
const ROBOT_COLORS = ['#06b6d4','#f97316','#22c55e','#a855f7','#f59e0b','#ec4899','#84cc16','#38bdf8'];
|
||||
|
||||
function FleetMapView({ robots, robotData, onSelect }) {
|
||||
const canvasRef = useRef(null);
|
||||
const [zoom, setZoom] = useState(30); // px per metre
|
||||
const [pan, setPan] = useState({ x: 0, y: 0 });
|
||||
const dragging = useRef(null);
|
||||
|
||||
useEffect(() => {
|
||||
const canvas = canvasRef.current;
|
||||
if (!canvas) return;
|
||||
const ctx = canvas.getContext('2d');
|
||||
const W = canvas.width, H = canvas.height;
|
||||
|
||||
ctx.clearRect(0, 0, W, H);
|
||||
ctx.fillStyle = '#050510';
|
||||
ctx.fillRect(0, 0, W, H);
|
||||
|
||||
// Grid
|
||||
ctx.strokeStyle = '#0d1b2a';
|
||||
ctx.lineWidth = 1;
|
||||
const gridSpacing = zoom;
|
||||
const offX = (W / 2 + pan.x) % gridSpacing;
|
||||
const offY = (H / 2 + pan.y) % gridSpacing;
|
||||
for (let x = offX; x < W; x += gridSpacing) {
|
||||
ctx.beginPath(); ctx.moveTo(x, 0); ctx.lineTo(x, H); ctx.stroke();
|
||||
}
|
||||
for (let y = offY; y < H; y += gridSpacing) {
|
||||
ctx.beginPath(); ctx.moveTo(0, y); ctx.lineTo(W, y); ctx.stroke();
|
||||
}
|
||||
|
||||
// Origin cross
|
||||
const cx = W / 2 + pan.x;
|
||||
const cy = H / 2 + pan.y;
|
||||
ctx.strokeStyle = '#1e3a4a';
|
||||
ctx.lineWidth = 1;
|
||||
ctx.beginPath(); ctx.moveTo(cx - 10, cy); ctx.lineTo(cx + 10, cy); ctx.stroke();
|
||||
ctx.beginPath(); ctx.moveTo(cx, cy - 10); ctx.lineTo(cx, cy + 10); ctx.stroke();
|
||||
|
||||
// Draw each robot that has pose data
|
||||
robots.forEach((robot, i) => {
|
||||
const d = robotData[robot.id];
|
||||
if (!d?.pose) return;
|
||||
const color = ROBOT_COLORS[i % ROBOT_COLORS.length];
|
||||
|
||||
const rx = cx + d.pose.x * zoom;
|
||||
const ry = cy - d.pose.y * zoom;
|
||||
const arrowLen = Math.max(14, zoom * 0.5);
|
||||
|
||||
// Heading line
|
||||
ctx.strokeStyle = color;
|
||||
ctx.lineWidth = 2;
|
||||
ctx.beginPath();
|
||||
ctx.moveTo(rx, ry);
|
||||
ctx.lineTo(rx + Math.cos(d.pose.yaw) * arrowLen, ry - Math.sin(d.pose.yaw) * arrowLen);
|
||||
ctx.stroke();
|
||||
|
||||
// Body circle
|
||||
ctx.fillStyle = color;
|
||||
ctx.shadowBlur = 10;
|
||||
ctx.shadowColor = color;
|
||||
ctx.beginPath();
|
||||
ctx.arc(rx, ry, 7, 0, Math.PI * 2);
|
||||
ctx.fill();
|
||||
ctx.shadowBlur = 0;
|
||||
|
||||
// Label
|
||||
ctx.fillStyle = color;
|
||||
ctx.font = '9px monospace';
|
||||
ctx.textAlign = 'center';
|
||||
ctx.fillText(robot.name || robot.id, rx, ry - 12);
|
||||
});
|
||||
}, [robots, robotData, zoom, pan]);
|
||||
|
||||
const onMouseDown = e => { dragging.current = { x: e.clientX - pan.x, y: e.clientY - pan.y }; };
|
||||
const onMouseMove = e => {
|
||||
if (!dragging.current) return;
|
||||
setPan({ x: e.clientX - dragging.current.x, y: e.clientY - dragging.current.y });
|
||||
};
|
||||
const onMouseUp = () => { dragging.current = null; };
|
||||
|
||||
const botsWithPose = robots.filter(r => robotData[r.id]?.pose);
|
||||
|
||||
return (
|
||||
<div className="space-y-3">
|
||||
<div className="flex items-center gap-2 flex-wrap">
|
||||
<div className="text-cyan-700 text-xs font-bold tracking-widest">FLEET MAP</div>
|
||||
<div className="flex items-center gap-1 ml-auto">
|
||||
<button onClick={() => setZoom(z => Math.min(z * 1.5, 200))}
|
||||
className="px-2 py-1 rounded border border-gray-700 text-gray-300 hover:border-cyan-700 text-sm">+</button>
|
||||
<span className="text-gray-500 text-xs w-16 text-center">{zoom.toFixed(0)}px/m</span>
|
||||
<button onClick={() => setZoom(z => Math.max(z / 1.5, 5))}
|
||||
className="px-2 py-1 rounded border border-gray-700 text-gray-300 hover:border-cyan-700 text-sm">−</button>
|
||||
<button onClick={() => { setZoom(30); setPan({ x: 0, y: 0 }); }}
|
||||
className="px-2 py-1 rounded border border-gray-700 text-gray-400 hover:text-gray-200 text-xs ml-2">Reset</button>
|
||||
</div>
|
||||
</div>
|
||||
<div className="bg-gray-950 rounded-lg border border-cyan-950 overflow-hidden">
|
||||
<canvas
|
||||
ref={canvasRef} width={600} height={400}
|
||||
className="w-full cursor-grab active:cursor-grabbing block"
|
||||
onMouseDown={onMouseDown} onMouseMove={onMouseMove}
|
||||
onMouseUp={onMouseUp} onMouseLeave={onMouseUp}
|
||||
/>
|
||||
</div>
|
||||
{/* Legend */}
|
||||
<div className="flex flex-wrap gap-3 text-xs">
|
||||
{robots.map((r, i) => (
|
||||
<button key={r.id}
|
||||
className="flex items-center gap-1.5 hover:opacity-80"
|
||||
onClick={() => onSelect(r.id)}
|
||||
>
|
||||
<div className="w-2.5 h-2.5 rounded-full" style={{ background: ROBOT_COLORS[i % ROBOT_COLORS.length] }} />
|
||||
<span className="text-gray-500">{r.name || r.id}</span>
|
||||
{botsWithPose.find(b => b.id === r.id) ? null : (
|
||||
<span className="text-gray-700">(no pose)</span>
|
||||
)}
|
||||
</button>
|
||||
))}
|
||||
</div>
|
||||
</div>
|
||||
);
|
||||
}
|
||||
|
||||
// ── Sub-view: Mission Dispatch ───────────────────────────────────────────────
|
||||
|
||||
function MissionsView({ robots, connections, sendGoal, sendPatrol }) {
|
||||
const [targetId, setTargetId] = useState('');
|
||||
const [mode, setMode] = useState('goal'); // 'goal' | 'patrol'
|
||||
const [goalX, setGoalX] = useState('');
|
||||
const [goalY, setGoalY] = useState('');
|
||||
const [goalYaw, setGoalYaw] = useState('0');
|
||||
const [waypoints, setWaypoints] = useState('');
|
||||
const [sent, setSent] = useState(null);
|
||||
|
||||
const connected = robots.filter(r => connections[r.id]?.connected);
|
||||
|
||||
const handleSend = () => {
|
||||
if (!targetId) return;
|
||||
if (mode === 'goal') {
|
||||
sendGoal(targetId, parseFloat(goalX), parseFloat(goalY), parseFloat(goalYaw) * Math.PI / 180);
|
||||
setSent(`Goal → (${goalX}, ${goalY}) sent to ${targetId}`);
|
||||
} else {
|
||||
try {
|
||||
const pts = waypoints.trim().split('\n').map(line => {
|
||||
const [x, y, yaw = 0] = line.split(',').map(Number);
|
||||
return { x, y, yaw: yaw * Math.PI / 180 };
|
||||
});
|
||||
sendPatrol(targetId, pts);
|
||||
setSent(`Patrol (${pts.length} wpts) sent to ${targetId}`);
|
||||
} catch {
|
||||
setSent('Error parsing waypoints');
|
||||
}
|
||||
}
|
||||
setTimeout(() => setSent(null), 4000);
|
||||
};
|
||||
|
||||
return (
|
||||
<div className="space-y-4 max-w-lg">
|
||||
<div className="text-cyan-700 text-xs font-bold tracking-widest">MISSION DISPATCH</div>
|
||||
|
||||
{connected.length === 0 && (
|
||||
<div className="text-amber-700 text-xs border border-amber-900 rounded p-3">
|
||||
No robots connected. Connect at least one robot to dispatch missions.
|
||||
</div>
|
||||
)}
|
||||
|
||||
<div className="space-y-3">
|
||||
<label className="block space-y-1">
|
||||
<span className="text-gray-500 text-xs">Target Robot</span>
|
||||
<select
|
||||
className="w-full bg-gray-900 border border-gray-700 rounded px-2 py-1.5 text-sm text-gray-200 focus:outline-none focus:border-cyan-700"
|
||||
value={targetId} onChange={e => setTargetId(e.target.value)}
|
||||
>
|
||||
<option value="">— Select robot —</option>
|
||||
{connected.map(r => (
|
||||
<option key={r.id} value={r.id}>{r.name || r.id}</option>
|
||||
))}
|
||||
</select>
|
||||
</label>
|
||||
|
||||
<div className="flex gap-2">
|
||||
{['goal', 'patrol'].map(m => (
|
||||
<button key={m}
|
||||
onClick={() => setMode(m)}
|
||||
className={`px-3 py-1.5 rounded border text-xs font-bold uppercase tracking-wider ${
|
||||
mode === m
|
||||
? 'bg-cyan-950 border-cyan-700 text-cyan-300'
|
||||
: 'border-gray-700 text-gray-500 hover:text-gray-300'
|
||||
}`}
|
||||
>{m === 'goal' ? 'Single Goal' : 'Patrol Route'}</button>
|
||||
))}
|
||||
</div>
|
||||
|
||||
{mode === 'goal' ? (
|
||||
<div className="grid grid-cols-3 gap-2">
|
||||
{[['X (m)', goalX, setGoalX], ['Y (m)', goalY, setGoalY], ['Yaw (°)', goalYaw, setGoalYaw]].map(([lbl, val, setter]) => (
|
||||
<label key={lbl} className="space-y-1">
|
||||
<span className="text-gray-500 text-xs">{lbl}</span>
|
||||
<input type="number" step="0.1"
|
||||
className="w-full bg-gray-900 border border-gray-700 rounded px-2 py-1.5 text-sm text-gray-200 focus:outline-none focus:border-cyan-700"
|
||||
value={val} onChange={e => setter(e.target.value)} placeholder="0" />
|
||||
</label>
|
||||
))}
|
||||
</div>
|
||||
) : (
|
||||
<label className="block space-y-1">
|
||||
<span className="text-gray-500 text-xs">Waypoints (x,y,yaw° per line)</span>
|
||||
<textarea
|
||||
rows={5}
|
||||
placeholder="0,0,0 2,0,90 2,2,180 0,2,270"
|
||||
className="w-full bg-gray-900 border border-gray-700 rounded px-2 py-1.5 text-sm text-gray-200 focus:outline-none focus:border-cyan-700 font-mono"
|
||||
value={waypoints} onChange={e => setWaypoints(e.target.value)}
|
||||
/>
|
||||
</label>
|
||||
)}
|
||||
|
||||
<button
|
||||
onClick={handleSend}
|
||||
disabled={!targetId}
|
||||
className="w-full py-2 rounded bg-cyan-950 border border-cyan-700 text-cyan-300 hover:bg-cyan-900 font-bold text-sm tracking-wider disabled:opacity-40 disabled:cursor-not-allowed"
|
||||
>SEND MISSION</button>
|
||||
|
||||
{sent && (
|
||||
<div className="text-green-400 text-xs border border-green-900 rounded p-2">{sent}</div>
|
||||
)}
|
||||
</div>
|
||||
|
||||
<div className="bg-gray-950 border border-gray-800 rounded p-3 text-xs text-gray-600 space-y-1">
|
||||
<div className="text-gray-500 font-bold">Publish targets</div>
|
||||
<div>/goal_pose (geometry_msgs/PoseStamped) — single Nav2 goal</div>
|
||||
<div>/outdoor/waypoints (geometry_msgs/PoseArray) — GPS follower patrol</div>
|
||||
</div>
|
||||
</div>
|
||||
);
|
||||
}
|
||||
|
||||
// ── Sub-view: Video Grid ─────────────────────────────────────────────────────
|
||||
|
||||
function VideoFeed({ robot, connected }) {
|
||||
const [error, setError] = useState(false);
|
||||
const src = `http://${robot.host}:${robot.videoPort}/stream?topic=${encodeURIComponent(robot.videoTopic)}&type=ros_compressed`;
|
||||
|
||||
return (
|
||||
<div className="bg-gray-950 rounded-lg border border-gray-800 overflow-hidden">
|
||||
<div className="flex items-center gap-2 px-2 py-1.5 border-b border-gray-800">
|
||||
<div className={`w-1.5 h-1.5 rounded-full ${connected ? 'bg-green-400' : 'bg-gray-700'}`} />
|
||||
<span className="text-xs text-gray-400 font-mono truncate flex-1">{robot.name || robot.id}</span>
|
||||
<span className="text-xs text-gray-700">{robot.videoTopic.split('/').pop()}</span>
|
||||
</div>
|
||||
{!connected ? (
|
||||
<div className="aspect-video flex items-center justify-center text-gray-700 text-xs">
|
||||
Robot disconnected
|
||||
</div>
|
||||
) : error ? (
|
||||
<div className="aspect-video flex items-center justify-center text-red-900 text-xs text-center px-4">
|
||||
Stream unavailable<br />
|
||||
<span className="text-gray-700">{robot.host}:{robot.videoPort}</span>
|
||||
</div>
|
||||
) : (
|
||||
<img
|
||||
src={src}
|
||||
alt={`${robot.name} camera`}
|
||||
className="w-full aspect-video object-cover block bg-black"
|
||||
onError={() => setError(true)}
|
||||
/>
|
||||
)}
|
||||
</div>
|
||||
);
|
||||
}
|
||||
|
||||
function VideoView({ robots, connections }) {
|
||||
return (
|
||||
<div className="space-y-3">
|
||||
<div className="text-cyan-700 text-xs font-bold tracking-widest">VIDEO GRID</div>
|
||||
{robots.length === 0 ? (
|
||||
<div className="text-gray-600 text-xs text-center py-8">No robots configured.</div>
|
||||
) : (
|
||||
<div className="grid grid-cols-1 sm:grid-cols-2 gap-3">
|
||||
{robots.map(r => (
|
||||
<VideoFeed key={r.id} robot={r} connected={connections[r.id]?.connected} />
|
||||
))}
|
||||
</div>
|
||||
)}
|
||||
<div className="text-xs text-gray-700">
|
||||
Streams via web_video_server on port {robots[0]?.videoPort ?? 8080}.
|
||||
Start with: <code className="text-gray-600">ros2 run web_video_server web_video_server</code>
|
||||
</div>
|
||||
</div>
|
||||
);
|
||||
}
|
||||
|
||||
// ── Sub-view: Alert Panel ────────────────────────────────────────────────────
|
||||
|
||||
function AlertsView({ robots, robotData, connections }) {
|
||||
const allAlerts = robots.flatMap(r => {
|
||||
const d = robotData[r.id];
|
||||
if (!d?.alerts?.length) return [];
|
||||
return d.alerts.map(a => ({ ...a, robotId: r.id, robotName: r.name || r.id }));
|
||||
});
|
||||
|
||||
const errors = allAlerts.filter(a => a.level >= 2);
|
||||
const warns = allAlerts.filter(a => a.level === 1);
|
||||
const offline = robots.filter(r => !connections[r.id]?.connected);
|
||||
|
||||
return (
|
||||
<div className="space-y-4">
|
||||
<div className="text-cyan-700 text-xs font-bold tracking-widest">FLEET ALERTS</div>
|
||||
|
||||
{/* Summary */}
|
||||
<div className="grid grid-cols-3 gap-3">
|
||||
{[
|
||||
{ label: 'ERRORS', count: errors.length, color: errors.length ? 'text-red-400 bg-red-950 border-red-800' : 'text-gray-600 bg-gray-900 border-gray-800' },
|
||||
{ label: 'WARNINGS',count: warns.length, color: warns.length ? 'text-amber-400 bg-amber-950 border-amber-800' : 'text-gray-600 bg-gray-900 border-gray-800' },
|
||||
{ label: 'OFFLINE', count: offline.length, color: offline.length ? 'text-gray-400 bg-gray-900 border-gray-700' : 'text-gray-600 bg-gray-900 border-gray-800' },
|
||||
].map(({ label, count, color }) => (
|
||||
<div key={label} className={`rounded-lg border p-3 text-center ${color}`}>
|
||||
<div className="text-2xl font-bold">{count}</div>
|
||||
<div className="text-xs tracking-widest mt-0.5">{label}</div>
|
||||
</div>
|
||||
))}
|
||||
</div>
|
||||
|
||||
{/* Offline robots */}
|
||||
{offline.length > 0 && (
|
||||
<div className="bg-gray-950 rounded-lg border border-gray-700 p-3">
|
||||
<div className="text-gray-500 text-xs font-bold mb-2">OFFLINE ROBOTS</div>
|
||||
<div className="space-y-1">
|
||||
{offline.map(r => (
|
||||
<div key={r.id} className="flex items-center gap-2 text-xs">
|
||||
<div className="w-1.5 h-1.5 rounded-full bg-gray-600" />
|
||||
<span className="text-gray-400">{r.name || r.id}</span>
|
||||
<span className="text-gray-700">{r.host}:{r.port}</span>
|
||||
{connections[r.id]?.error && (
|
||||
<span className="text-red-800">{connections[r.id].error}</span>
|
||||
)}
|
||||
</div>
|
||||
))}
|
||||
</div>
|
||||
</div>
|
||||
)}
|
||||
|
||||
{/* Alert list */}
|
||||
{allAlerts.length === 0 ? (
|
||||
<div className="text-gray-600 text-xs text-center py-6 border border-dashed border-gray-800 rounded">
|
||||
{robots.length === 0 ? 'No robots configured.' : 'No active alerts across fleet.'}
|
||||
</div>
|
||||
) : (
|
||||
<div className="space-y-2">
|
||||
{[...allAlerts]
|
||||
.sort((a, b) => b.level - a.level)
|
||||
.map((a, i) => (
|
||||
<div key={i} className={`flex items-start gap-2 rounded border px-3 py-2 text-xs ${
|
||||
a.level >= 2 ? 'bg-red-950 border-red-800' : 'bg-amber-950 border-amber-800'
|
||||
}`}>
|
||||
<div className={`w-1.5 h-1.5 rounded-full mt-0.5 shrink-0 ${a.level >= 2 ? 'bg-red-400 animate-pulse' : 'bg-amber-400'}`} />
|
||||
<div className="flex-1">
|
||||
<span className="font-bold text-gray-300">{a.name}</span>
|
||||
{a.message && <span className="text-gray-500 ml-2">{a.message}</span>}
|
||||
</div>
|
||||
<span className="text-gray-500 shrink-0">{a.robotName}</span>
|
||||
</div>
|
||||
))
|
||||
}
|
||||
</div>
|
||||
)}
|
||||
</div>
|
||||
);
|
||||
}
|
||||
|
||||
// ── Sub-view: Robot Detail ───────────────────────────────────────────────────
|
||||
|
||||
const DETAIL_TABS = [
|
||||
{ id: 'imu', label: 'IMU' },
|
||||
{ id: 'battery', label: 'Battery' },
|
||||
{ id: 'motors', label: 'Motors' },
|
||||
{ id: 'control', label: 'Control' },
|
||||
{ id: 'health', label: 'Health' },
|
||||
];
|
||||
|
||||
function RobotDetail({ robot, connection, data, subscribeRobot, onBack }) {
|
||||
const [tab, setTab] = useState('imu');
|
||||
|
||||
// Adapter: wrap subscribeRobot(id, ...) to match subscribe(name, type, cb) signature
|
||||
const subscribe = useCallback(
|
||||
(name, type, cb) => subscribeRobot(robot.id, name, type, cb),
|
||||
[subscribeRobot, robot.id]
|
||||
);
|
||||
|
||||
const conn = connection ?? { connected: false };
|
||||
|
||||
return (
|
||||
<div className="space-y-3">
|
||||
{/* Back + header */}
|
||||
<div className="flex items-center gap-3">
|
||||
<button
|
||||
onClick={onBack}
|
||||
className="text-gray-500 hover:text-cyan-300 text-xs border border-gray-700 hover:border-cyan-800 rounded px-2 py-1"
|
||||
>← Back</button>
|
||||
<div className="flex items-center gap-2">
|
||||
<div className={`w-2 h-2 rounded-full ${conn.connected ? 'bg-green-400' : 'bg-gray-700'}`} />
|
||||
<span className="font-bold text-gray-200">{robot.name || robot.id}</span>
|
||||
<span className="text-gray-600 text-sm">{robot.host}:{robot.port}</span>
|
||||
<span className={`text-xs border rounded px-1.5 py-0.5 font-mono uppercase ${VARIANT_COLORS[robot.variant] ?? 'text-gray-400 border-gray-700'}`}>
|
||||
{robot.variant}
|
||||
</span>
|
||||
</div>
|
||||
{data?.battery?.soc != null && (
|
||||
<div className="ml-auto flex items-center gap-1.5">
|
||||
<SocBar soc={Math.round(data.battery.soc)} />
|
||||
<span className="text-xs text-gray-600">{data.battery.voltage?.toFixed(1)}V</span>
|
||||
</div>
|
||||
)}
|
||||
</div>
|
||||
|
||||
{/* Detail tabs */}
|
||||
<div className="flex gap-0.5 border-b border-gray-800">
|
||||
{DETAIL_TABS.map(t => (
|
||||
<button key={t.id} onClick={() => setTab(t.id)}
|
||||
className={`px-3 py-2 text-xs font-bold tracking-wider border-b-2 transition-colors ${
|
||||
tab === t.id
|
||||
? 'border-cyan-500 text-cyan-300'
|
||||
: 'border-transparent text-gray-600 hover:text-gray-300'
|
||||
}`}
|
||||
>{t.label}</button>
|
||||
))}
|
||||
</div>
|
||||
|
||||
{!conn.connected ? (
|
||||
<div className="text-gray-600 text-sm text-center py-10">
|
||||
Robot disconnected {conn.error ? `— ${conn.error}` : ''}
|
||||
</div>
|
||||
) : (
|
||||
<div>
|
||||
{tab === 'imu' && <ImuPanel subscribe={subscribe} />}
|
||||
{tab === 'battery' && <BatteryPanel subscribe={subscribe} />}
|
||||
{tab === 'motors' && <MotorPanel subscribe={subscribe} />}
|
||||
{tab === 'control' && <ControlMode subscribe={subscribe} />}
|
||||
{tab === 'health' && <SystemHealth subscribe={subscribe} />}
|
||||
</div>
|
||||
)}
|
||||
</div>
|
||||
);
|
||||
}
|
||||
|
||||
// ── Root FleetPanel ──────────────────────────────────────────────────────────
|
||||
|
||||
export function FleetPanel() {
|
||||
const {
|
||||
robots, addRobot, removeRobot,
|
||||
connections, robotData,
|
||||
subscribeRobot, sendGoal, sendPatrol,
|
||||
scanning, scanSubnet,
|
||||
} = useFleet();
|
||||
|
||||
const [view, setView] = useState('robots');
|
||||
const [selectedId, setSelectedId] = useState(null);
|
||||
const [showAdd, setShowAdd] = useState(false);
|
||||
|
||||
const handleSelect = useCallback((id) => {
|
||||
setSelectedId(id);
|
||||
}, []);
|
||||
|
||||
const handleBack = useCallback(() => {
|
||||
setSelectedId(null);
|
||||
}, []);
|
||||
|
||||
const handleScanLan = useCallback(async () => {
|
||||
const baseIp = robots[0]?.host ?? '192.168.1.100';
|
||||
const found = await scanSubnet(baseIp, [9090, 9092]);
|
||||
found.forEach(({ host, port }, i) => {
|
||||
const existingHosts = robots.map(r => r.host);
|
||||
if (!existingHosts.includes(host)) {
|
||||
addRobot({
|
||||
id: `discovered-${Date.now()}-${i}`,
|
||||
name: `Discovered ${host}`,
|
||||
variant: 'balance',
|
||||
host,
|
||||
port,
|
||||
});
|
||||
}
|
||||
});
|
||||
}, [robots, scanSubnet, addRobot]);
|
||||
|
||||
// If a robot is selected → show RobotDetail overlay
|
||||
if (selectedId) {
|
||||
const robot = robots.find(r => r.id === selectedId);
|
||||
if (robot) {
|
||||
return (
|
||||
<RobotDetail
|
||||
robot={robot}
|
||||
connection={connections[robot.id]}
|
||||
data={robotData[robot.id]}
|
||||
subscribeRobot={subscribeRobot}
|
||||
onBack={handleBack}
|
||||
/>
|
||||
);
|
||||
}
|
||||
setSelectedId(null);
|
||||
}
|
||||
|
||||
return (
|
||||
<div className="space-y-4">
|
||||
{/* Fleet sub-nav */}
|
||||
<div className="flex gap-0.5 border-b border-gray-800">
|
||||
{FLEET_VIEWS.map(v => (
|
||||
<button key={v.id} onClick={() => setView(v.id)}
|
||||
className={`px-3 py-2 text-xs font-bold tracking-wider border-b-2 transition-colors ${
|
||||
view === v.id
|
||||
? 'border-cyan-500 text-cyan-300'
|
||||
: 'border-transparent text-gray-600 hover:text-gray-300'
|
||||
}`}
|
||||
>{v.label.toUpperCase()}</button>
|
||||
))}
|
||||
</div>
|
||||
|
||||
{/* Content */}
|
||||
{view === 'robots' && (
|
||||
<RobotsView
|
||||
robots={robots}
|
||||
connections={connections}
|
||||
robotData={robotData}
|
||||
onSelect={handleSelect}
|
||||
onRemove={removeRobot}
|
||||
onAdd={() => setShowAdd(true)}
|
||||
onScan={handleScanLan}
|
||||
scanning={scanning}
|
||||
/>
|
||||
)}
|
||||
{view === 'map' && (
|
||||
<FleetMapView robots={robots} robotData={robotData} onSelect={handleSelect} />
|
||||
)}
|
||||
{view === 'missions' && (
|
||||
<MissionsView
|
||||
robots={robots}
|
||||
connections={connections}
|
||||
sendGoal={sendGoal}
|
||||
sendPatrol={sendPatrol}
|
||||
/>
|
||||
)}
|
||||
{view === 'video' && (
|
||||
<VideoView robots={robots} connections={connections} />
|
||||
)}
|
||||
{view === 'alerts' && (
|
||||
<AlertsView robots={robots} robotData={robotData} connections={connections} />
|
||||
)}
|
||||
|
||||
{showAdd && (
|
||||
<AddRobotModal onAdd={addRobot} onClose={() => setShowAdd(false)} />
|
||||
)}
|
||||
</div>
|
||||
);
|
||||
}
|
||||
347
ui/social-bot/src/hooks/useFleet.js
Normal file
347
ui/social-bot/src/hooks/useFleet.js
Normal file
@ -0,0 +1,347 @@
|
||||
/**
|
||||
* useFleet.js — Multi-robot fleet connection manager.
|
||||
*
|
||||
* Maintains one ROSLIB.Ros WebSocket connection per robot.
|
||||
* Aggregates fleet-summary telemetry (battery, pose, alerts, mode) for each.
|
||||
* Persists robot registry to localStorage.
|
||||
*
|
||||
* API:
|
||||
* const { robots, addRobot, removeRobot, updateRobot,
|
||||
* connections, robotData,
|
||||
* subscribeRobot, publishRobot, scanSubnet } = useFleet();
|
||||
*
|
||||
* Robot shape:
|
||||
* { id, name, variant, host, port, videoPort, videoTopic }
|
||||
* variant: 'balance' | 'rover' | 'tank' | 'social'
|
||||
* videoTopic: '/camera/panoramic/compressed' (default)
|
||||
*/
|
||||
|
||||
import { useState, useEffect, useRef, useCallback } from 'react';
|
||||
import ROSLIB from 'roslib';
|
||||
|
||||
// ── Storage ────────────────────────────────────────────────────────────────────
|
||||
|
||||
const STORAGE_KEY = 'saltybot_fleet_v2';
|
||||
|
||||
const DEFAULT_ROBOTS = [
|
||||
{
|
||||
id: 'salty-01',
|
||||
name: 'SaltyBot #1',
|
||||
variant: 'balance',
|
||||
host: '192.168.1.100',
|
||||
port: 9090,
|
||||
videoPort: 8080,
|
||||
videoTopic: '/camera/panoramic/compressed',
|
||||
},
|
||||
];
|
||||
|
||||
function loadRobots() {
|
||||
try {
|
||||
const saved = JSON.parse(localStorage.getItem(STORAGE_KEY));
|
||||
return Array.isArray(saved) && saved.length > 0 ? saved : DEFAULT_ROBOTS;
|
||||
} catch {
|
||||
return DEFAULT_ROBOTS;
|
||||
}
|
||||
}
|
||||
|
||||
// ── Robot data structure ───────────────────────────────────────────────────────
|
||||
|
||||
const EMPTY_DATA = () => ({
|
||||
state: null, // 'DISARMED'|'ARMED'|'TILT FAULT'
|
||||
stm32Mode: null, // 'MANUAL'|'ASSISTED'
|
||||
controlMode: null, // 'RC'|'RAMP_TO_AUTO'|'AUTO'|'RAMP_TO_RC'
|
||||
blendAlpha: 0,
|
||||
pipeline: null, // 'idle'|'listening'|'thinking'|'speaking'
|
||||
pitch: 0,
|
||||
motorCmd: 0,
|
||||
battery: { voltage: 0, soc: 0 },
|
||||
pose: null, // { x, y, yaw }
|
||||
alerts: [], // [{ level, name, message }]
|
||||
lastSeen: null,
|
||||
});
|
||||
|
||||
// ── Hook ──────────────────────────────────────────────────────────────────────
|
||||
|
||||
export function useFleet() {
|
||||
const [robots, setRobots] = useState(loadRobots);
|
||||
const [connections, setConnections] = useState({});
|
||||
const [robotData, setRobotData] = useState(() =>
|
||||
Object.fromEntries(loadRobots().map(r => [r.id, EMPTY_DATA()]))
|
||||
);
|
||||
const [scanning, setScanning] = useState(false);
|
||||
|
||||
const rosRef = useRef({}); // { [id]: ROSLIB.Ros }
|
||||
const subsRef = useRef({}); // { [id]: ROSLIB.Topic[] }
|
||||
|
||||
// Persist robots
|
||||
useEffect(() => {
|
||||
localStorage.setItem(STORAGE_KEY, JSON.stringify(robots));
|
||||
}, [robots]);
|
||||
|
||||
// ── Connection lifecycle ────────────────────────────────────────────────────
|
||||
|
||||
useEffect(() => {
|
||||
const currentIds = new Set(robots.map(r => r.id));
|
||||
|
||||
// Disconnect removed robots
|
||||
Object.keys(rosRef.current).forEach(id => {
|
||||
if (!currentIds.has(id)) {
|
||||
subsRef.current[id]?.forEach(t => { try { t.unsubscribe(); } catch {} });
|
||||
delete subsRef.current[id];
|
||||
try { rosRef.current[id].close(); } catch {}
|
||||
delete rosRef.current[id];
|
||||
setConnections(c => { const n = { ...c }; delete n[id]; return n; });
|
||||
}
|
||||
});
|
||||
|
||||
// Connect new robots
|
||||
robots.forEach(robot => {
|
||||
if (rosRef.current[robot.id]) return;
|
||||
|
||||
const ros = new ROSLIB.Ros({ url: `ws://${robot.host}:${robot.port}` });
|
||||
rosRef.current[robot.id] = ros;
|
||||
subsRef.current[robot.id] = [];
|
||||
|
||||
ros.on('connection', () => {
|
||||
setConnections(c => ({ ...c, [robot.id]: { connected: true, error: null } }));
|
||||
_setupSubs(robot.id, ros);
|
||||
});
|
||||
ros.on('error', err => {
|
||||
setConnections(c => ({
|
||||
...c,
|
||||
[robot.id]: { connected: false, error: String(err?.message ?? err ?? 'error') }
|
||||
}));
|
||||
});
|
||||
ros.on('close', () => {
|
||||
setConnections(c => ({
|
||||
...c,
|
||||
[robot.id]: { connected: false, error: null }
|
||||
}));
|
||||
});
|
||||
});
|
||||
|
||||
return () => {
|
||||
Object.values(subsRef.current).forEach(subs =>
|
||||
subs.forEach(t => { try { t.unsubscribe(); } catch {} })
|
||||
);
|
||||
Object.values(rosRef.current).forEach(ros => { try { ros.close(); } catch {} });
|
||||
rosRef.current = {};
|
||||
subsRef.current = {};
|
||||
};
|
||||
// eslint-disable-next-line react-hooks/exhaustive-deps
|
||||
}, [robots]);
|
||||
|
||||
// ── Fleet-summary subscriptions ────────────────────────────────────────────
|
||||
|
||||
function _sub(id, ros, name, type, cb) {
|
||||
const t = new ROSLIB.Topic({ ros, name, messageType: type });
|
||||
t.subscribe(cb);
|
||||
subsRef.current[id] = [...(subsRef.current[id] ?? []), t];
|
||||
}
|
||||
|
||||
function _update(id, patch) {
|
||||
setRobotData(prev => ({
|
||||
...prev,
|
||||
[id]: { ...(prev[id] ?? EMPTY_DATA()), ...patch },
|
||||
}));
|
||||
}
|
||||
|
||||
function _setupSubs(id, ros) {
|
||||
// Balance state
|
||||
_sub(id, ros, '/saltybot/balance_state', 'std_msgs/String', msg => {
|
||||
try {
|
||||
const d = JSON.parse(msg.data);
|
||||
_update(id, {
|
||||
state: d.state,
|
||||
stm32Mode: d.mode,
|
||||
pitch: d.pitch_deg ?? 0,
|
||||
motorCmd: d.motor_cmd ?? 0,
|
||||
lastSeen: Date.now(),
|
||||
});
|
||||
} catch {}
|
||||
});
|
||||
|
||||
// Control mode (RC ↔ AUTO blend)
|
||||
_sub(id, ros, '/saltybot/control_mode', 'std_msgs/String', msg => {
|
||||
try {
|
||||
const d = JSON.parse(msg.data);
|
||||
_update(id, { controlMode: d.mode, blendAlpha: d.blend_alpha ?? 0 });
|
||||
} catch {}
|
||||
});
|
||||
|
||||
// Odometry → pose
|
||||
_sub(id, ros, '/odom', 'nav_msgs/Odometry', msg => {
|
||||
const p = msg.pose?.pose;
|
||||
if (!p) return;
|
||||
const o = p.orientation;
|
||||
const yaw = Math.atan2(
|
||||
2 * (o.w * o.z + o.x * o.y),
|
||||
1 - 2 * (o.y * o.y + o.z * o.z)
|
||||
);
|
||||
_update(id, { pose: { x: p.position.x, y: p.position.y, yaw }, lastSeen: Date.now() });
|
||||
});
|
||||
|
||||
// Diagnostics → battery + alerts
|
||||
_sub(id, ros, '/diagnostics', 'diagnostic_msgs/DiagnosticArray', msg => {
|
||||
const alerts = [];
|
||||
const kv = {};
|
||||
for (const status of msg.status ?? []) {
|
||||
if (status.level > 0) {
|
||||
alerts.push({ level: status.level, name: status.name, message: status.message });
|
||||
}
|
||||
for (const pair of status.values ?? []) kv[pair.key] = pair.value;
|
||||
}
|
||||
const patch = { alerts };
|
||||
if (kv.battery_voltage_v != null) {
|
||||
const v = parseFloat(kv.battery_voltage_v);
|
||||
patch.battery = {
|
||||
voltage: v,
|
||||
soc: Math.max(0, Math.min(100, ((v - 12) / (16.8 - 12)) * 100)),
|
||||
};
|
||||
}
|
||||
_update(id, patch);
|
||||
});
|
||||
|
||||
// Social pipeline state (optional)
|
||||
_sub(id, ros, '/social/orchestrator/state', 'std_msgs/String', msg => {
|
||||
try {
|
||||
_update(id, { pipeline: JSON.parse(msg.data)?.state ?? null });
|
||||
} catch {}
|
||||
});
|
||||
}
|
||||
|
||||
// ── Public API ─────────────────────────────────────────────────────────────
|
||||
|
||||
const addRobot = useCallback(robot => {
|
||||
const id = robot.id ?? `bot-${Date.now()}`;
|
||||
const full = {
|
||||
videoPort: 8080,
|
||||
videoTopic: '/camera/panoramic/compressed',
|
||||
...robot,
|
||||
id,
|
||||
};
|
||||
setRobots(r => [...r.filter(b => b.id !== id), full]);
|
||||
setRobotData(d => ({ ...d, [id]: EMPTY_DATA() }));
|
||||
}, []);
|
||||
|
||||
const removeRobot = useCallback(id => {
|
||||
setRobots(r => r.filter(b => b.id !== id));
|
||||
setRobotData(d => { const n = { ...d }; delete n[id]; return n; });
|
||||
}, []);
|
||||
|
||||
const updateRobot = useCallback((id, patch) => {
|
||||
// Force reconnect by dropping the existing connection
|
||||
subsRef.current[id]?.forEach(t => { try { t.unsubscribe(); } catch {} });
|
||||
subsRef.current[id] = [];
|
||||
try { rosRef.current[id]?.close(); } catch {}
|
||||
delete rosRef.current[id];
|
||||
setRobots(r => r.map(b => b.id === id ? { ...b, ...patch } : b));
|
||||
}, []);
|
||||
|
||||
/** Subscribe to a topic on a specific robot. Returns unsubscribe fn. */
|
||||
const subscribeRobot = useCallback((id, name, type, cb) => {
|
||||
const ros = rosRef.current[id];
|
||||
if (!ros) return () => {};
|
||||
const t = new ROSLIB.Topic({ ros, name, messageType: type });
|
||||
t.subscribe(cb);
|
||||
subsRef.current[id] = [...(subsRef.current[id] ?? []), t];
|
||||
return () => {
|
||||
try { t.unsubscribe(); } catch {}
|
||||
subsRef.current[id] = subsRef.current[id]?.filter(x => x !== t) ?? [];
|
||||
};
|
||||
}, []);
|
||||
|
||||
/** Publish to a topic on a specific robot. */
|
||||
const publishRobot = useCallback((id, name, type, data) => {
|
||||
const ros = rosRef.current[id];
|
||||
if (!ros) return;
|
||||
const t = new ROSLIB.Topic({ ros, name, messageType: type });
|
||||
t.publish(new ROSLIB.Message(data));
|
||||
}, []);
|
||||
|
||||
/** Send a single Nav2 goal pose to a robot. */
|
||||
const sendGoal = useCallback((id, x, y, yaw = 0) => {
|
||||
const cy2 = Math.cos(yaw / 2);
|
||||
const sy2 = Math.sin(yaw / 2);
|
||||
publishRobot(id, '/goal_pose', 'geometry_msgs/PoseStamped', {
|
||||
header: { frame_id: 'map', stamp: { sec: 0, nanosec: 0 } },
|
||||
pose: {
|
||||
position: { x, y, z: 0 },
|
||||
orientation: { x: 0, y: 0, z: sy2, w: cy2 },
|
||||
},
|
||||
});
|
||||
}, [publishRobot]);
|
||||
|
||||
/** Send a patrol route (array of {x,y,yaw?}) to a robot via PoseArray → GPS follower. */
|
||||
const sendPatrol = useCallback((id, waypoints) => {
|
||||
publishRobot(id, '/outdoor/waypoints', 'geometry_msgs/PoseArray', {
|
||||
header: { frame_id: 'map', stamp: { sec: 0, nanosec: 0 } },
|
||||
poses: waypoints.map(({ x, y, yaw = 0 }) => {
|
||||
const cy2 = Math.cos(yaw / 2);
|
||||
const sy2 = Math.sin(yaw / 2);
|
||||
return {
|
||||
position: { x, y, z: 0 },
|
||||
orientation: { x: 0, y: 0, z: sy2, w: cy2 },
|
||||
};
|
||||
}),
|
||||
});
|
||||
}, [publishRobot]);
|
||||
|
||||
/**
|
||||
* Probe a list of candidate URLs to find reachable rosbridge servers.
|
||||
* Reports each discovered robot back via the addRobot callback.
|
||||
*/
|
||||
const scanSubnet = useCallback(async (baseIp, portRange = [9090, 9097]) => {
|
||||
setScanning(true);
|
||||
const [portMin, portMax] = portRange;
|
||||
const ipParts = baseIp.split('.');
|
||||
const prefix = ipParts.slice(0, 3).join('.');
|
||||
|
||||
const results = [];
|
||||
const probes = [];
|
||||
for (let host4 = 1; host4 <= 254; host4++) {
|
||||
for (let port = portMin; port <= portMax; port++) {
|
||||
probes.push({ host: `${prefix}.${host4}`, port });
|
||||
}
|
||||
}
|
||||
|
||||
// Probe in batches of 32 to avoid thundering-herd
|
||||
const BATCH = 32;
|
||||
for (let i = 0; i < probes.length; i += BATCH) {
|
||||
const batch = probes.slice(i, i + BATCH);
|
||||
await Promise.allSettled(
|
||||
batch.map(({ host, port }) =>
|
||||
new Promise(resolve => {
|
||||
const ws = new WebSocket(`ws://${host}:${port}`);
|
||||
const timer = setTimeout(() => { try { ws.close(); } catch {} resolve(null); }, 800);
|
||||
ws.onopen = () => {
|
||||
clearTimeout(timer);
|
||||
results.push({ host, port });
|
||||
ws.close();
|
||||
resolve({ host, port });
|
||||
};
|
||||
ws.onerror = () => { clearTimeout(timer); resolve(null); };
|
||||
})
|
||||
)
|
||||
);
|
||||
}
|
||||
|
||||
setScanning(false);
|
||||
return results;
|
||||
}, []);
|
||||
|
||||
return {
|
||||
robots,
|
||||
addRobot,
|
||||
removeRobot,
|
||||
updateRobot,
|
||||
connections,
|
||||
robotData,
|
||||
subscribeRobot,
|
||||
publishRobot,
|
||||
sendGoal,
|
||||
sendPatrol,
|
||||
scanning,
|
||||
scanSubnet,
|
||||
};
|
||||
}
|
||||
Loading…
x
Reference in New Issue
Block a user