seb/saltylab-firmware
1
0
Fork 0
Code Issues Pull Requests 5 Actions Packages Projects Releases Wiki Activity
694 Commits 390 Branches 0 Tags
Commit Graph

323 Commits

Author SHA1 Message Date
sl-jetson
5f0affcd79 feat: Jetson Orin system monitor ROS2 node (Issue #631) 
New package saltybot_system_monitor:
- jetson_stats.py: pure-Python data layer (JetsonStats, CpuCore,
  TegrastatsParser, JtopReader, TegrastatsReader, MockReader,
  AlertChecker, AlertThresholds) — no ROS2 dependency
- system_monitor_node.py: ROS2 node publishing /saltybot/system/stats
  (JSON) and /saltybot/diagnostics (DiagnosticArray) at 1 Hz
- Alerts: CPU/GPU >85% WARN (+10% ERROR), temp >80°C, disk/RAM >90%,
  power >30 W; each alert produces a DiagnosticStatus entry
- Stats source priority: jtop > tegrastats > mock (auto-detected)
- config/system_monitor.yaml: all thresholds and rate tunable via params
- launch/system_monitor.launch.py: single-node launch with config arg
- test/test_system_monitor.py: 50+ pytest tests, ROS2-free

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
 2026-03-15 16:36:04 -04:00
sl-jetson
4c7fa938a5 Merge pull request 'feat: UWB accuracy analyzer (Issue #634)' (#641) from sl-uwb/issue-634-uwb-logger into main 2026-03-15 16:30:16 -04:00
sl-uwb
343e53081a feat: UWB position logger and accuracy analyzer (Issue #634) 
saltybot_uwb_logger_msgs (new package):
- AccuracyReport.msg: n_samples, mean/bias/std (x,y,2D), CEP50, CEP95,
  RMSE, max_error, per-anchor range stats, test_id, duration_s
- StartAccuracyTest.srv: request (truth_x/y_m, n_samples, timeout_s,
  test_id) → response (success, message, test_id)

saltybot_uwb_logger (new package):
- accuracy_stats.py: compute_stats() + RangeAccum — pure numpy, no ROS2
  CEP50/CEP95 = 50th/95th percentile of 2-D error; bias, std, RMSE, max
- logger_node.py: /uwb_logger ROS2 node
  Subscribes:
    /saltybot/pose/fused  → fused_pose_<DATE>.csv (ts, x, y, heading)
    /saltybot/uwb/pose    → uwb_pose_<DATE>.csv   (ts, x, y)
    /uwb/ranges           → uwb_ranges_<DATE>.csv (ts, anchor_id, range_m,
                                                   raw_mm, rssi, tag_id)
  Service /saltybot/uwb/start_accuracy_test:
    Collects N fused-pose samples at known (truth_x, truth_y) in background
    thread. On completion or timeout: publishes AccuracyReport on
    /saltybot/uwb/accuracy_report + writes accuracy_<test_id>.json.
    Per-anchor range stats included. CSV flushed every 5 s.

Tests: 16/16 passing (test/test_accuracy_stats.py, no ROS2/hardware)

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
 2026-03-15 14:44:21 -04:00
sl-perception
1fd935b87e feat: ArUco marker detection for docking (Issue #627) 
New package saltybot_aruco_detect — DICT_4X4_50 ArUco detection from
RealSense D435i RGB, pose estimation, PoseArray + dock target output.

aruco_math.py (pure Python, no ROS2): rot_mat_to_quat (Shepperd),
  rvec_to_quat (Rodrigues + cv2 fallback), tvec_distance, tvec_yaw_rad,
  MarkerPose dataclass with lazy-cached distance_m/yaw_rad/lateral_m/quat.

aruco_detect_node.py (ROS2 node 'aruco_detect'):
  Subscribes: /camera/color/image_raw (30Hz BGR8) + /camera/color/camera_info.
  Converts to greyscale, cv2.aruco.ArucoDetector.detectMarkers().
  estimatePoseSingleMarkers (legacy API) with solvePnP(IPPE_SQUARE) fallback.
  Dock target: closest marker in dock_marker_ids (default=[42], empty=any),
    filtered to max_dock_range_m (3.0m).
  Publishes: /saltybot/aruco/markers (PoseArray — all detected, camera frame),
    /saltybot/aruco/dock_target (PoseStamped — closest dock candidate,
    position.z=forward, position.x=lateral), /saltybot/aruco/viz (MarkerArray
    — SPHERE + TEXT per marker, dock in red), /saltybot/aruco/status (JSON
    10Hz — detected_count, dock_distance_m, dock_yaw_deg, dock_lateral_m).
  Optional debug image with drawDetectedMarkers + drawFrameAxes.
  corner_refinement=CORNER_REFINE_SUBPIX.

config/aruco_detect_params.yaml, launch/aruco_detect.launch.py.
test/test_aruco_math.py: 22 unit tests (rotation/quat math, distance,
  yaw sign/magnitude, MarkerPose accessors + caching).

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
 2026-03-15 14:37:22 -04:00
sl-jetson
fea550c851 Merge pull request 'feat: ROS2 bag recording manager (Issue #615)' (#625) from sl-jetson/issue-615-bag-recorder into main 2026-03-15 11:02:37 -04:00
sl-jetson
96d13052b4 Merge pull request 'feat: RealSense obstacle detection (Issue #611)' (#623) from sl-perception/issue-611-obstacle-detect into main 2026-03-15 11:02:29 -04:00
sl-jetson
7e12dab4ae feat: ROS2 bag recording manager (Issue #615) 
Upgrades saltybot_bag_recorder (Issue #488) with:

- Motion-triggered auto-record: subscribes /cmd_vel, starts on non-zero
  velocity, stops after 30s idle timeout (configurable)
- Auto-split at 1 GB or 10 min via subprocess restart
- USB/NVMe storage selection: ordered priority list, picks first path
  with >= 2 GB free (/media/usb0 -> /media/usb1 -> /mnt/nvme -> ~/bags)
- Disk monitoring: warns at 70%, triggers cleanup of bags >7 days at 80%
- JSON status on /saltybot/bag_recorder/status at 1 Hz
- Services: /saltybot/bag_recorder/{start,stop,split}
  (legacy /saltybot/{start,stop}_recording kept for compatibility)
- bag_policy.py: pure-Python MotionState, DiskInfo, StorageSelector,
  BagPolicy — ROS2-free, fully unit-testable
- 76 unit tests passing

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
 2026-03-15 10:12:40 -04:00
sl-perception
82ad626a94 feat: RealSense depth obstacle detection (Issue #611) 
New package saltybot_obstacle_detect — RANSAC ground plane fitting on
D435i depth images with 2D grid BFS obstacle clustering.

ground_plane.py (pure Python + numpy):
  fit_ground_plane(pts, n_iter=50, inlier_thresh_m=0.06): RANSAC over 3D
  point cloud in camera optical frame (+Z forward). Samples 3 points, fits
  plane via cross-product, counts inliers, refines via SVD on best inlier
  set. Orients normal toward -Y (upward in world). Returns (normal, d).
  height_above_plane(pts, plane): signed h = d - n·p (h>0 = above ground).
  obstacle_mask(pts, plane, min_h, max_h): min_obstacle_h_m < h < max_h.
  ground_mask(pts, plane, thresh): inlier classification.

obstacle_clusterer.py (pure Python + numpy):
  cluster_obstacles(pts, heights, cell_m=0.30, min_pts=5): projects
  obstacle 3D points onto (X,Z) bird's-eye plane, discretises into grid
  cells, runs 4-connected BFS flood-fill, returns ObstacleCluster list
  sorted by forward distance. ObstacleCluster: centroid(3), radius_m,
  height_m, n_pts + distance_m/lateral_m properties.

obstacle_detect_node.py (ROS2 node 'obstacle_detect'):
  - Subscribes: /camera/depth/camera_info (latched, once),
    /camera/depth/image_rect_raw (BEST_EFFORT, 30Hz float32 depth).
  - Pipeline: stride downsample (default 8x → 80x60) → back-project to
    3D → RANSAC ground plane (temporally blended α=0.3) → obstacle mask
    (min_h=0.05m, max_h=0.80m) → BFS clustering → alert classification.
  - Publishes:
      /saltybot/obstacles (MarkerArray): SPHERE markers colour-coded
        DANGER(red)/WARN(yellow)/CLEAR(green) + distance TEXT labels.
      /saltybot/obstacles/cloud (PointCloud2): xyz float32 non-ground pts.
      /saltybot/obstacles/alert (String JSON): alert_level, closest_m,
        obstacle_count, per-obstacle {x,y,z,radius_m,height_m,level}.
  - Safety zone integration (depth_estop_enabled=false by default):
      DANGER → zero Twist to depth_estop_topic (/cmd_vel_input) feeds
      into safety_zone's cmd_vel chain for independent depth e-stop.

config/obstacle_detect_params.yaml: all tuneable parameters with comments.
launch/obstacle_detect.launch.py: single node with params_file arg.
test/test_ground_plane.py: 10 unit tests (RANSAC correctness, normal
  orientation, height computation, inlier/obstacle classification).
test/test_obstacle_clusterer.py: 8 unit tests (single/dual cluster,
  distance sort, empty, min_pts filter, centroid accuracy, range clip).

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
 2026-03-15 10:09:23 -04:00
sl-uwb
65e0009118 feat: ESP-NOW to ROS2 serial relay (Issue #618) 
New ROS2 package saltybot_uwb_espnow_relay:
- packet.py: EspNowPacket dataclass + FrameReader stateful decoder
  - Parses 20-byte ESP-NOW packets: MAGIC, tag_id, msg_type, anchor_id,
    range_mm (int32 LE), rssi_dbm (float32), timestamp_ms, battery_pct,
    flags (bit0=estop), seq_num
  - Serial framing: STX(0x02) + LEN(0x14) + DATA[20] + XOR-CRC(1)
  - Sync recovery: re-hunts STX after bad LEN or CRC; byte-by-byte capable
- relay_node.py: /espnow_relay ROS2 node
  - Reads from USB serial in background thread (auto-reconnects on error)
  - MSG_RANGE (0x10): publishes UwbRange on /uwb/espnow/ranges
  - MSG_ESTOP (0x20): publishes std_msgs/Bool on /uwb/espnow/estop
    and /saltybot/estop (latched True for estop_latch_s after last packet)
  - MSG_HEARTBEAT (0x30): publishes EspNowHeartbeat on /uwb/espnow/heartbeat
  - Range validity gating: min_range_m / max_range_m params
- 16/16 unit tests passing (test/test_packet.py, no ROS2/hardware needed)

saltybot_uwb_msgs: add EspNowHeartbeat.msg
  (tag_id, battery_pct, seq_num, timestamp_ms + std_msgs/Header)

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
 2026-03-15 10:08:19 -04:00
sl-jetson
bb5eff1382 Merge pull request 'feat: MQTT-to-ROS2 phone sensor bridge (Issue #601)' (#605) from sl-android/issue-601-mqtt-ros2-bridge into main 2026-03-14 15:55:22 -04:00
sl-jetson
6c00d6a321 Merge pull request 'feat: UWB anchor auto-calibration via inter-anchor ranging + MDS (Issue #602)' (#608) from sl-uwb/issue-602-anchor-calibration into main 2026-03-14 15:54:56 -04:00
sl-jetson
2460ba27c7 Merge pull request 'feat: Nav2 with UWB localization (Issue #599)' (#607) from sl-jetson/issue-599-nav2-uwb into main 2026-03-14 15:54:52 -04:00
sl-uwb
587ca8a98e feat: UWB anchor auto-calibration via inter-anchor ranging + MDS (Issue #602) 
Anchor firmware (esp32/uwb_anchor/src/main.cpp):
- Add peer_range_once(peer_id) — DS-TWR initiator role toward a peer anchor
- Add AT+PEER_RANGE=<id> command: triggers inter-anchor ranging and returns
  +PEER_RANGE:<my_id>,<peer_id>,<range_mm>,<rssi_dbm> (or ERR,TIMEOUT)

ROS2 package saltybot_uwb_calibration_msgs:
- CalibrateAnchors.srv: request (anchor_ids[], n_samples) →
  response (positions_x/y/z[], residual_rms_m, anchor_positions_json)

ROS2 package saltybot_uwb_calibration:
- mds_math.py: classical MDS (double-centred D², eigendecomposition),
  anchor_frame_align() to fix anchor-0 at origin / anchor-1 on +X
- calibration_node.py: /saltybot/uwb/calibrate_anchors service —
  opens anchor serial ports, rounds-robin AT+PEER_RANGE= for all pairs,
  builds N×N distance matrix, runs MDS, returns JSON anchor positions
- 12/12 unit tests passing (test/test_mds_math.py)
- Supports ≥ 4 anchors; 5× averaged ranging per pair by default

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
 2026-03-14 15:03:53 -04:00
sl-jetson
40b0917c33 feat: Nav2 integration with UWB localization (Issue #599) 
New package saltybot_nav2_uwb replacing AMCL-based localization with
UWB-IMU EKF fused pose. Key components:

- uwb_pose_bridge_node: subscribes /saltybot/pose/fused_cov (from EKF),
  computes map→odom TF via T_map_odom = T_map_base × inv(T_odom_base),
  broadcasts at 20 Hz. Publishes /initialpose on first valid pose.
- waypoint_sequencer.py: pure-Python state machine (IDLE→RUNNING→
  SUCCEEDED/ABORTED/CANCELED) for sequential waypoint execution.
- waypoint_follower_node: action server on /saltybot/nav/follow_waypoints
  (nav2_msgs/FollowWaypoints), sends each goal to Nav2 NavigateToPose
  in sequence; JSON topic /saltybot/nav/waypoints for operator use.
- nav2_uwb_params.yaml: DWB controller capped at 1.0 m/s, global+local
  costmap with /scan (RPLIDAR), rolling-window global costmap (no static
  map needed), AMCL removed from lifecycle manager.
- nav2_uwb.launch.py: bridge (t=0) → Nav2 (t=2s) → waypoint follower
  (t=4s) with LogInfo markers.
- 65 unit tests passing (waypoint dataclass, sequencer state machine,
  2-D TF maths, progress tracking).

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
 2026-03-14 15:02:26 -04:00
sl-perception
c76d5b0dd7 feat: Multi-sensor pose fusion node (Issue #595) 
New package saltybot_pose_fusion — EKF fusing UWB+IMU absolute pose,
visual odometry velocity, and raw IMU into a single authoritative pose.

pose_fusion_ekf.py (pure Python, no ROS2 deps):
  PoseFusionEKF — state [x, y, θ, vx, vy, ω], 6-state EKF.
  - predict_imu(ax_body, ay_body, omega, dt): body-frame IMU predict step
    with Jacobian F, bias-compensated accel, process noise Q.
  - update_uwb_position(x, y, sigma_m): absolute position measurement
    (H=[1,0,0,0,0,0; 0,1,0,0,0,0]) from UWB+IMU fused stream.
  - update_uwb_heading(heading_rad, sigma_rad): heading measurement.
  - update_vo_velocity(vx_body, omega, ...): VO velocity measurement —
    body-frame vx rotated to world via cos/sin(θ), updates [vx,vy,ω] state.
  - Joseph-form covariance update for numerical stability.
  - Dual dropout clocks: uwb_dropout_s, vo_dropout_s (reset on each update).
  - Velocity damping when uwb_dropout_s > 2s.
  - Sensor weight parameters: sigma_uwb_pos_m, sigma_uwb_head_rad,
    sigma_vo_vel_m_s, sigma_vo_omega_r_s, sigma_imu_accel/gyro,
    sigma_vel_drift, dropout_vel_damp.

pose_fusion_node.py (ROS2 node 'pose_fusion'):
  - Subscribes: /imu/data (Imu, 200Hz → predict), /saltybot/pose/fused_cov
    (PoseWithCovarianceStamped, 10Hz → position+heading update, σ extracted
    from message covariance when use_uwb_covariance=true), /saltybot/visual_odom
    (Odometry, 30Hz → velocity update, σ from twist covariance).
  - Publishes: /saltybot/pose/authoritative (PoseWithCovarianceStamped),
    /saltybot/pose/status (String JSON, 10Hz).
  - TF2: map→base_link broadcast at IMU rate.
  - Suppresses output when uwb_dropout_s > uwb_dropout_max_s (10s).
  - Warns (throttled) on UWB/VO dropout.

config/pose_fusion_params.yaml: sensor weights + dropout thresholds.
launch/pose_fusion.launch.py: single node launch with params_file arg.
test/test_pose_fusion_ekf.py: 13 unit tests — init, predict, UWB/VO
  updates, dropout reset, covariance shape/convergence, sigma override.

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
 2026-03-14 15:00:54 -04:00
sl-android
c62444cc0e chore: Register mqtt_ros2_bridge entry point and paho-mqtt dep (Issue #601) 
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
 2026-03-14 14:59:41 -04:00
sl-android
dd13569413 feat: MQTT-to-ROS2 phone sensor bridge (Issue #601) 
Add saltybot_phone/mqtt_ros2_bridge_node.py — ROS2 node bridging the three
MQTT topics published by phone/sensor_dashboard.py into typed ROS2 messages:

  saltybot/phone/imu     → /saltybot/phone/imu     sensor_msgs/Imu
  saltybot/phone/gps     → /saltybot/phone/gps     sensor_msgs/NavSatFix
  saltybot/phone/battery → /saltybot/phone/battery sensor_msgs/BatteryState
  (status)               → /saltybot/phone/bridge/status std_msgs/String

Key design:
- paho-mqtt loop_start() runs in dedicated network thread; on_message
  enqueues (topic, payload) pairs into a thread-safe queue
- ROS2 timer drains queue at 50 Hz — all publishing stays on executor
  thread, avoiding any rclpy threading concerns
- Timestamp alignment: uses ROS2 wall clock by default; opt-in
  use_phone_timestamp param uses phone epoch ts when drift < warn_drift_s
- IMU: populates accel + gyro with diagonal covariance; orientation_cov[0]=-1
  (unknown per REP-145)
- GPS: NavSatStatus.STATUS_FIX for gps/fused/network providers; full 3×3
  position covariance from accuracy_m; COVARIANCE_TYPE_APPROXIMATED
- Battery: pct→percentage [0-1], temp Kelvin, health/status mapped from
  Android health strings, voltage/current=NaN (unavailable on Android)
- Input validation: finite value checks on IMU, lat/lon range on GPS,
  pct [0-100] on battery; bad messages logged at DEBUG and counted
- Status topic at 0.2 Hz: JSON {mqtt_connected, rx/pub/err counts,
  age_s per sensor, queue_depth}
- Auto-reconnect via paho reconnect_delay_set (5 s → 20 s max)

Add launch/mqtt_bridge.launch.py with args: mqtt_host, mqtt_port,
reconnect_delay_s, use_phone_timestamp, warn_drift_s, imu_accel_cov,
imu_gyro_cov.

Register mqtt_ros2_bridge console script in setup.py.
Add python3-paho-mqtt exec_depend to package.xml.

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
 2026-03-14 14:59:02 -04:00
sl-jetson
15ff5acca7 Merge pull request 'feat: Visual odometry from RealSense stereo ORB (Issue #586)' (#593) from sl-perception/issue-586-visual-odom into main 2026-03-14 13:32:56 -04:00
sl-jetson
1da1d50171 Merge pull request 'feat: Phone video bridge (Issue #585)' (#592) from sl-android/issue-585-video-bridge into main 2026-03-14 13:32:24 -04:00
sl-jetson
e9429e6177 Merge pull request 'feat: ROS2 launch orchestrator for full SaltyBot bringup (Issue #577)' (#582) from sl-jetson/issue-577-bringup-launch into main 2026-03-14 13:32:10 -04:00
sl-perception
c1b82608d5 feat: Visual odometry from RealSense stereo ORB (Issue #586) 
Adds stereo ORB-based visual odometry to saltybot_visual_odom package.

New modules:
- orb_stereo_matcher.py: ORB feature detection (cv2.ORB_create) with BFMatcher
  NORM_HAMMING + Lowe ratio test for temporal matching (infra1 prev→curr).
  Stereo scale method matches infra1↔infra2 under epipolar row constraint
  (|Δrow|≤2px), computes depth = baseline_m * fx / disparity.
- stereo_orb_node.py: StereoOrbNode subscribes to infra1+infra2+depth
  (ApproximateTimeSynchronizer 3-topic), detects/matches ORB temporally,
  estimates SE(3) via Essential matrix (5-point RANSAC) using StereoVO,
  recovers metric scale from D435i aligned depth (primary) or stereo
  baseline disparity (fallback). Publishes nav_msgs/Odometry on
  /saltybot/visual_odom and broadcasts TF2 odom→camera_link. Baseline
  auto-updated from infra2 camera_info Tx (overrides parameter).
- config/stereo_orb_params.yaml, launch/stereo_orb.launch.py
- setup.py: adds stereo_orb entrypoint, installs launch+config dirs

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
 2026-03-14 12:21:58 -04:00
sl-android
08bc23f6df feat: Phone video streaming bridge (Issue #585) 
Phone side — phone/video_bridge.py:
- MJPEG streaming server for Android/Termux phone camera
- Dual camera backends: OpenCV VideoCapture (V4L2) with automatic
  fallback to termux-camera-photo for unmodified Android
- WebSocket server (ws://0.0.0.0:8765) — binary JPEG frames + JSON
  info/error control messages; supports multiple concurrent clients
- HTTP server (http://0.0.0.0:8766):
    /stream    — multipart/x-mixed-replace MJPEG
    /snapshot  — single JPEG
    /health    — JSON stats (frame count, dropped, resolution, fps)
- Thread-safe single-slot FrameBuffer; CaptureThread rate-limited with
  wall-clock accounting for capture latency
- Flags: --ws-port, --http-port, --width, --height, --fps, --quality,
  --device, --camera-id, --no-http, --debug

Jetson side — saltybot_phone/phone_camera_node.py:
- ROS2 node: receives JPEG frames, publishes:
    /saltybot/phone/camera            sensor_msgs/Image (bgr8)
    /saltybot/phone/camera/compressed sensor_msgs/CompressedImage
    /saltybot/phone/camera/info       std_msgs/String (stream metadata)
- WebSocket client (primary); HTTP MJPEG polling fallback on WS failure
- Auto-reconnect loop (default 3 s) for both transports
- Latency warning when frame age > latency_warn_ms (default 200 ms)
- 10 s diagnostics log: received/published counts + last frame age
- Registered as phone_camera_node console script in setup.py
- Added to phone_bringup.py launch with phone_host / phone_cam_enabled args

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
 2026-03-14 12:20:28 -04:00
sl-mechanical
7b75cdad1a feat: UWB anchor mount bracket (Issue #564) 2026-03-14 12:15:12 -04:00
sl-jetson
b09017c949 Merge pull request 'feat: UWB-IMU EKF fusion for robust indoor localization (Issue #573)' (#581) from sl-uwb/issue-573-uwb-imu-fusion into main 2026-03-14 12:14:05 -04:00
sl-jetson
4035b4cfc3 feat: ROS2 launch orchestrator for full SaltyBot bringup (Issue #577) 
Adds saltybot_bringup.launch.py with ordered startup groups (drivers→
perception→navigation→UI), timer-based health gates, configurable
profiles (minimal/full/debug), and estop on Ctrl-C shutdown.

Also adds launch_profiles.py dataclass module and 53-test coverage for
profile hierarchy, timing gates, safety bounds, and to_dict serialization.

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
 2026-03-14 11:57:57 -04:00
sl-uwb
7708c63698 feat: UWB-IMU EKF fusion for robust indoor localization (Issue #573) 
EKF fusing UWB position (10Hz) with IMU accel+gyro (200Hz) for
SaltyBot indoor localization with UWB dropout resilience.

Package: saltybot_uwb_imu_fusion

- ekf_math.py: 6-state EKF [x,y,θ,vx,vy,ω], IMU predict + UWB update
  - IMU as process input: body-frame accel rotated to world via heading
  - Jacobian F for nonlinear rotation effect
  - Process noise Q from continuous white-noise model
  - UWB 2D position update, heading update from quaternion
  - Accel bias estimation (low-pass)
  - Velocity damping during UWB dropout (>2s threshold)
- ekf_node.py: ROS2 node subscribing to /imu/data (200Hz) + /saltybot/uwb/pose
  or /uwb/bearing (10Hz)
  - Publishes /saltybot/pose/fused (PoseStamped)
  - Publishes /saltybot/pose/fused_cov (PoseWithCovarianceStamped)
  - Broadcasts base_link → map TF2 at IMU rate
  - Suppresses output after max_dead_reckoning_s without UWB
- 14/14 unit tests passing (predict, update, dropout, PD covariance)
- Launch: ros2 launch saltybot_uwb_imu_fusion uwb_imu_fusion.launch.py

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
 2026-03-14 11:55:43 -04:00
sl-perception
131d85a0d3 feat: RPLIDAR safety zone detector (Issue #575) 
Add saltybot_safety_zone — ROS2 Python node that processes the RPLIDAR
A1M8 /scan into three concentric 360° safety zones, latches an e-stop
when DANGER is detected in the forward arc, and overrides /cmd_vel to
zero while the latch is active.

Zone thresholds (default):
  DANGER  < 0.30 m — latching e-stop in forward arc
  WARN    < 1.00 m — advisory (published in sector data)
  CLEAR   otherwise

Sector grid:
  36 sectors of 10° each (sector 0 = robot forward, CCW positive).
  Per-sector: angle_deg, zone, min_range_m, in_forward_arc flag.

E-stop behaviour:
  - Latches after estop_debounce_frames (2) consecutive DANGER scans
    in the forward arc (configurable ±30°, or all-arcs mode).
  - While latched: zero Twist published to /cmd_vel every scan + every
    incoming /cmd_vel_input message is blocked.
  - Clear only via service (obstacle must be gone):
    /saltybot/safety_zone/clear_estop  (std_srvs/Trigger)

Published topics:
  /saltybot/safety_zone          String/JSON  every scan
    — per-sector {sector, angle_deg, zone, min_range_m, forward}
    — estop_active, estop_reason, danger_sectors[], warn_sectors[]
  /saltybot/safety_zone/status   String/JSON  10 Hz
    — forward_zone, closest_obstacle_m, danger/warn counts
  /cmd_vel                       Twist        zero when e-stopped

Subscribed topics:
  /scan           LaserScan  — RPLIDAR A1M8
  /cmd_vel_input  Twist      — upstream velocity (pass-through / block)

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
 2026-03-14 11:54:52 -04:00
sl-jetson
35440b7463 Merge pull request 'feat: ROS2 sensor health monitor (Issue #566)' (#572) from sl-jetson/issue-566-health-monitor into main 2026-03-14 11:49:55 -04:00
sl-jetson
8e03a209be feat: ROS2 sensor health monitor (Issue #566) 
Add sensor_health_node to saltybot_health_monitor package. Monitors 8
sensor topics for staleness, publishing DiagnosticArray on
/saltybot/diagnostics and MQTT JSON on saltybot/health.

Sensors monitored (configurable thresholds):
  /camera/color/image_raw, /camera/depth/image_rect_raw,
  /camera/color/camera_info, /scan, /imu/data,
  /saltybot/uwb/range, /saltybot/battery, /saltybot/motor_daemon/status

Each sensor: OK/WARN/ERROR based on topic age vs warn_s/error_s thresholds.
Critical sensors (camera, lidar, imu, motor_daemon) escalate overall status.

Files added:
  sensor_health_node.py — SensorWatcher + SensorHealthNode
  config/sensor_health_params.yaml — per-sensor thresholds
  launch/sensor_health.launch.py
  test/test_sensor_health.py — 35 tests, all passing

setup.py/package.xml updated: sensor_msgs, diagnostic_msgs deps + new entry point.

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
 2026-03-14 11:47:01 -04:00
sl-perception
2180b61440 feat: ROS2 UWB position node (Issue #546) 
Add saltybot_uwb_position — ROS2 Python package that reads JSON range
measurements from an ESP32 DW3000 UWB tag over USB serial, trilaterates
the robot's absolute position from 3+ fixed infrastructure anchors, and
publishes position + TF2 to the rest of the stack.

Serial protocol (one JSON line per frame):
  Full frame: {"ts":…, "ranges": [{"id":0,"d_mm":1500,"rssi":-65}, …]}
  Per-anchor: {"id":0, "d_mm":1500, "rssi":-65.0}
  Accepts both "d_mm" and "range_mm" field names.

Trilateration (trilateration.py, numpy, no ROS deps):
  Linear least-squares: linearise sphere equations around anchor 0,
  solve (N-1)x2 (2D) or (N-1)x3 (3D) system via np.linalg.lstsq.
  2D mode (default): robot_z fixed, needs >=3 anchors.
  3D mode (solve_z=true): full 3D, needs >=4 anchors.

Outlier rejection:
  After initial solve, compute per-anchor residual |r_meas - r_pred|.
  Reject anchors with residual > outlier_threshold_m (0.4 m default).
  Re-solve with inliers if >= min_anchors remain.
  Track consecutive outlier strikes; flag in /status after N strikes.

Kalman filter (KalmanFilter3D, constant-velocity, 6-state, numpy):
  Predict-only coasting when anchors drop below minimum.
  Q=0.05, R=0.10 (tunable).

Topics:
  /saltybot/uwb/pose       PoseStamped  10 Hz Kalman-filtered position
  /saltybot/uwb/range/<id> UwbRange     on arrival, raw per-anchor ranges
  /saltybot/uwb/status     String/JSON  10 Hz state+residuals+flags

TF2: uwb_link -> map (identity rotation)

Anchor config: flat float arrays in YAML.
Default layout: 4-anchor 5x5m room at 2m height.

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
 2026-03-14 11:43:22 -04:00
sl-jetson
6c5ecc9e00 Merge pull request 'feat: ROS2 gimbal control node (Issue #548)' (#558) from sl-jetson/issue-548-gimbal-ros2 into main 2026-03-14 11:39:58 -04:00
sl-jetson
da6a17cdcb feat: ROS2 gimbal control node (Issue #548) 
saltybot_gimbal ROS2 Python package for pan/tilt camera head control
via JLINK binary protocol over serial to STM32 (Issue #547 C side).

- gimbal_node.py: subscribes /saltybot/gimbal/cmd (Vector3: pan, tilt,
  speed), publishes /saltybot/gimbal/state (JSON), /saltybot/gimbal/cmd_echo
- Services: /saltybot/gimbal/home (Trigger), /saltybot/gimbal/look_at
  (Trigger + /saltybot/gimbal/look_at_target PointStamped)
- jlink_gimbal.py: JLINK codec matching jlink.h — CMD_GIMBAL_POS=0x0B,
  TLM_GIMBAL_STATE=0x84, CRC16-CCITT, deg*10 encoding, speed register
- MotionAxis: trapezoidal velocity profile (configurable accel + speed)
- Configurable limits: pan ±150°, tilt ±45° (gimbal_params.yaml)
- Serial reconnect with configurable retry delay
- 48 unit tests — all passing

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
 2026-03-14 10:34:06 -04:00
sl-perception
c68b751590 feat: Person-following head tracking (Issue #549) 
Add saltybot_head_tracking — ROS2 Python node for automatic person-
following using dual-axis PID control targeting the pan/tilt camera head.

Pipeline:
  1. Subscribe to /saltybot/objects (DetectedObjectArray from YOLOv8n)
  2. Filter for class_id==0 (person); select best target by score:
       score = 0.6 * 1/(1+dist_m)  +  0.4 * confidence
     (falls back to confidence-only when distance_m==0 / unknown)
  3. Compute pixel error of bbox centre from image centre
  4. Apply dead-zone (10 px default) to suppress micro-jitter
  5. Convert pixel error to angle error via camera FOV
  6. Independent PID controllers for pan and tilt axes
  7. Accumulate PID output into absolute angle setpoint
  8. Publish geometry_msgs/Point to /saltybot/gimbal/cmd:
       x = pan_angle_deg, y = tilt_angle_deg, z = confidence

State machine:
  IDLE      -> waiting for first detection
  TRACKING  -> active PID
  LOST      -> hold last angle for hold_duration_s (3 s)
  CENTERING -> return to (0, 0) at 20 deg/s -> IDLE

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
 2026-03-14 10:28:17 -04:00
sl-jetson
14164089dc feat: Audio pipeline end-to-end (Issue #503) 
- Add VoskSTT class to audio_utils.py: offline Vosk STT backend as
  low-latency CPU alternative to Whisper for Jetson deployments
- Update audio_pipeline_node.py: stt_backend param ("whisper"/"vosk"),
  Vosk loading with Whisper fallback, CPU auto-detection for Whisper,
  dual-backend _process_utterance dispatch, STT/<backend> log prefix
- Update audio_pipeline_params.yaml: add stt_backend and vosk_model_path
- Add test/test_audio_pipeline.py: 40 unit tests covering EnergyVAD,
  PCM conversion, AudioBuffer, UtteranceSegmenter, VoskSTT, JabraAudioDevice,
  AudioMetrics, AudioState
- Integrate into full_stack.launch.py: audio_pipeline at t=5s with
  enable_audio_pipeline and audio_stt_backend args

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
 2026-03-07 10:03:31 -05:00
sl-jetson
6d316514da Merge remote-tracking branch 'origin/sl-firmware/issue-531-pid-autotune' 2026-03-07 10:03:24 -05:00
sl-firmware
19a30a1c4f feat: PID auto-tune for balance mode (Issue #531) 
Implement Ziegler-Nichols relay feedback auto-tuning with flash persistence:

Firmware (STM32F722):
- pid_flash.c/h: erase+write Kp/Ki/Kd to flash sector 7 (0x0807FFC0),
  magic-validated; load on boot to restore saved tune
- jlink.h: add JLINK_CMD_PID_SAVE (0x0A) and JLINK_TLM_PID_RESULT (0x83)
  with jlink_tlm_pid_result_t struct and pid_save_req flag in JLinkState
- jlink.c: dispatch JLINK_CMD_PID_SAVE -> pid_save_req; add
  jlink_send_pid_result() to confirm flash write outcome over USART1
- main.c: load saved PID from flash after balance_init(); handle
  pid_save_req in main loop (disarmed-only, erase stalls CPU ~1s)

Jetson ROS2 (saltybot_pid_autotune):
- pid_autotune_node.py: add Ki to Ziegler-Nichols formula (ZN PID:
  Kp=0.6Ku, Ki=1.2Ku/Tu, Kd=0.075KuTu); add JLink serial client that
  sends JLINK_CMD_PID_SET + JLINK_CMD_PID_SAVE after tuning completes
- autotune_config.yaml: add jlink_serial_port and jlink_baud_rate params

Trigger: ros2 service call /saltybot/autotune_pid std_srvs/srv/Trigger

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
 2026-03-07 09:56:19 -05:00
sl-perception
f71fdae747 feat: Depth-to-costmap plugin for RealSense D435i (Issue #532) 
Add saltybot_depth_costmap — a Nav2 costmap2d plugin that converts
D435i depth images directly into obstacle markings on both local and
global costmaps.

Pipeline:
  1. Subscribe to /camera/depth/image_rect_raw (16UC1 mm) + camera_info
  2. Back-project depth pixels to 3D using pinhole camera intrinsics
  3. Transform points to costmap global_frame via TF2
  4. Apply configurable height filter (min_height..max_height above ground)
  5. Mark obstacle cells as LETHAL_OBSTACLE
  6. Inflate neighbours within inflation_radius as INSCRIBED_INFLATED_OBSTACLE

Parameters:
  min_height: 0.05 m       — floor clearance (ignores ground returns)
  max_height: 0.80 m       — ceiling cutoff (ignores lights/ceiling)
  obstacle_range: 3.5 m    — max marking distance from camera
  clearing_range: 4.0 m    — max distance processed at all
  inflation_radius: 0.10 m — in-layer inflation (works before inflation_layer)
  downsample_factor: 4     — process 1 of N rows+cols (~19k pts @ 640×480)

Integration (#478):
  - Added depth_costmap_layer to local_costmap plugins list
  - Added depth_costmap_layer to global_costmap plugins list
  - Plugin registered via pluginlib (plugin.xml)

Files:
  jetson/ros2_ws/src/saltybot_depth_costmap/
    CMakeLists.txt, package.xml, plugin.xml
    include/saltybot_depth_costmap/depth_costmap_layer.hpp
    src/depth_costmap_layer.cpp
  jetson/ros2_ws/src/saltybot_bringup/config/nav2_params.yaml (updated)

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
 2026-03-07 09:52:18 -05:00
sl-jetson
5b7ee63d1e Merge remote-tracking branch 'origin/sl-controls/issue-522-usart6-truncation' 2026-03-06 23:34:45 -05:00
sl-webui
7141e12320 feat: Integration test suite expanded (Issue #504) - resolve conflicts 2026-03-06 23:10:42 -05:00
sl-jetson
e28f1549cb feat: Orin motor control daemon (Issue #523) 
Add saltybot_motor_daemon ROS2 package — Python daemon that subscribes
to /cmd_vel and drives the FC via W<speed>,<steer>\n over /dev/ttyTHS1
at 921600 baud.

- motor_daemon_node.py: 50 Hz fixed-rate TX, 200ms safety watchdog,
  Twist→ESC conversion (±1000 range), FC ack parsing (W:<s>,<st>),
  periodic ? status query, /diagnostics publisher, auto-reconnect
- config/motor_daemon_params.yaml: all tunable params with comments
- launch/motor_daemon.launch.py: parameterised launch file
- test/test_motor_daemon.py: 25 unit tests (all passing)

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
 2026-03-06 23:02:57 -05:00
sl-jetson
f14ce5c3ba Merge remote-tracking branch 'origin/sl-perception/issue-469-terrain-classification' 2026-03-06 17:37:27 -05:00
sl-jetson
2e2ed2d0a7 Merge remote-tracking branch 'origin/sl-controls/issue-506-launch-profiles' 2026-03-06 17:37:27 -05:00
sl-jetson
5b9e9dd412 Merge pull request 'feat: Headscale VPN auto-connect (Issue #502)' (#517) from sl-jetson/issue-502-headscale-vpn into main 2026-03-06 17:37:07 -05:00
sl-perception
8d58d5e34c feat: Terrain classification for speed adaptation (Issue #469) 
Implement multi-sensor terrain classification using RealSense D435i depth and RPLIDAR A1M8:

- saltybot_terrain_classification: New ROS2 package for terrain classification
- TerrainClassifier: Rule-based classifier matching depth variance + reflectance to terrain type
  (smooth/carpet/grass/gravel) with hysteresis + confidence scoring
- DepthExtractor: Extracts roughness from depth discontinuities and surface gradients
- LidarExtractor: Extracts reflectance from RPLIDAR scan intensities
- terrain_classification_node: 10Hz node fusing both sensors, publishes:
  - /saltybot/terrain_type (JSON with type, confidence, speed_scale)
  - /saltybot/terrain_type_string (human-readable type)
  - /saltybot/terrain_speed_scale (0.0-1.0 speed multiplier for smooth/carpet/grass/gravel)

Speed scales: smooth=1.0, carpet=0.9, grass=0.75, gravel=0.6

Co-Authored-By: Claude Haiku 4.5 <noreply@anthropic.com>
 2026-03-06 16:43:21 -05:00
sl-webui
d3eca7bebc feat: Integration test suite (Issue #504) 
Add comprehensive integration testing for complete ROS2 system stack:

Integration Tests (test_integration_full_stack.py):
  - Verifies all ROS2 nodes launch successfully
  - Checks critical topics are published (sensors, nav, control)
  - Validates system component health and stability
  - Tests launch file validity and configuration
  - Covers indoor/outdoor/follow modes

Launch Testing (test_launch_full_stack.py):
  - Validates launch file syntax and configuration
  - Verifies all required packages are installed
  - Checks launch sequence timing
  - Validates conditional logic for optional components

Test Coverage:
  ✓ SLAM/RTAB-Map (indoor mode)
  ✓ Nav2 navigation stack
  ✓ Perception (YOLOv8n person detection)
  ✓ Control (cmd_vel bridge, STM32 bridge)
  ✓ Audio pipeline and monitoring
  ✓ Sensors (LIDAR, RealSense, UWB, CSI cameras)
  ✓ Battery and temperature monitoring
  ✓ Autonomous docking behavior
  ✓ TF2 tree and odometry

Usage:
  pytest test/test_integration_full_stack.py -v
  pytest test/test_launch_full_stack.py -v

Documentation:
  See test/README_INTEGRATION_TESTS.md for detailed information.

Co-Authored-By: Claude Haiku 4.5 <noreply@anthropic.com>
 2026-03-06 16:42:38 -05:00
sl-webui
8d67d06857 feat: Integration test suite (Issue #504) 
Add comprehensive integration testing for complete ROS2 system stack:

Integration Tests (test_integration_full_stack.py):
  - Verifies all ROS2 nodes launch successfully
  - Checks critical topics are published (sensors, nav, control)
  - Validates system component health and stability
  - Tests launch file validity and configuration
  - Covers indoor/outdoor/follow modes

Launch Testing (test_launch_full_stack.py):
  - Validates launch file syntax and configuration
  - Verifies all required packages are installed
  - Checks launch sequence timing
  - Validates conditional logic for optional components

Test Coverage:
  ✓ SLAM/RTAB-Map (indoor mode)
  ✓ Nav2 navigation stack
  ✓ Perception (YOLOv8n person detection)
  ✓ Control (cmd_vel bridge, STM32 bridge)
  ✓ Audio pipeline and monitoring
  ✓ Sensors (LIDAR, RealSense, UWB, CSI cameras)
  ✓ Battery and temperature monitoring
  ✓ Autonomous docking behavior
  ✓ TF2 tree and odometry

Usage:
  pytest test/test_integration_full_stack.py -v
  pytest test/test_launch_full_stack.py -v

Documentation:
  See test/README_INTEGRATION_TESTS.md for detailed information.

Co-Authored-By: Claude Haiku 4.5 <noreply@anthropic.com>
 2026-03-06 16:42:31 -05:00
sl-controls
e5329391bc feat: Add parameter profile YAML files for Nav2 (Issue #506) 
- profile_indoor.yaml: Conservative settings (0.4 m/s, 0.35m inflation)
- profile_outdoor.yaml: Moderate settings (0.8 m/s, 0.3m inflation)
- profile_demo.yaml: Agile settings (0.6 m/s, 0.32m inflation)

Each profile customizes velocity limits, costmap inflation, and obstacle detection.
 2026-03-06 16:42:31 -05:00
sl-controls
5d17b6c501 feat: Issue #506 — Update nav2.launch.py for profile support 
Add profile argument to nav2.launch.py to accept launch profile parameter
and log profile selection for debugging/monitoring.

Changes:
- Add profile_arg declaration with choices (indoor/outdoor/demo)
- Add profile substitution and log output
- Update docstring with profile documentation

Co-Authored-By: Claude Haiku 4.5 <noreply@anthropic.com>
 2026-03-06 16:42:31 -05:00
sl-controls
b5acb32ee6 feat: Issue #506 — Update full_stack.launch.py for profile support 
Add profile argument and documentation to full_stack.launch.py for
Issue #506 launch parameter profiles. Updated to support:
- profile:=indoor (conservative)
- profile:=outdoor (moderate)
- profile:=demo (agile with tricks/social features)

Changes:
- Add profile_arg declaration
- Add profile substitution handle
- Update docstring with profile examples
- Ready for profile-based Nav2 parameter overrides

Co-Authored-By: Claude Haiku 4.5 <noreply@anthropic.com>
 2026-03-06 16:42:31 -05:00
sl-controls
bbfcd2a9d1 feat: Issue #506 — Launch parameter profiles (indoor/outdoor/demo) 
Implement profile-based parameter overrides for Nav2, costmap, and behavior
server configurations. Profiles predefine parameter sets for different
deployment scenarios.

New files:
- config/profiles/indoor.yaml: Conservative (0.2 m/s, tight geofence, no GPS)
- config/profiles/outdoor.yaml: Moderate (0.5 m/s, wide geofence, GPS-enabled)
- config/profiles/demo.yaml: Agile (0.3 m/s, tricks/social features enabled)
- saltybot_bringup/profile_loader.py: YAML loader and parameter merger utility

Supports: ros2 launch saltybot_bringup full_stack.launch.py profile:=<profile>

Co-Authored-By: Claude Haiku 4.5 <noreply@anthropic.com>
 2026-03-06 16:42:31 -05:00