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

315 Commits

Author SHA1 Message Date
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-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
sl-jetson
5add2cab51 Merge remote-tracking branch 'origin/sl-mechanical/issue-505-charging-dock' 
# Conflicts:
#	phone/INSTALL_MOTOR_TEST.md
#	phone/MOTOR_TEST_JOYSTICK.md
#	phone/motor_test_joystick.py
 2026-03-06 14:59:59 -05:00
sl-jetson
1f4929b68c Merge remote-tracking branch 'origin/sl-jetson/issue-477-urdf' 
# Conflicts:
#	jetson/config/RECOVERY_BEHAVIORS.md
 2026-03-06 11:43:31 -05:00
sl-jetson
d97fa5fab0 Merge remote-tracking branch 'origin/sl-webui/issue-482-behavior-tree' 
# Conflicts:
#	jetson/ros2_ws/src/saltybot_bringup/behavior_trees/autonomous_coordinator.xml
#	jetson/ros2_ws/src/saltybot_bringup/launch/autonomous_mode.launch.py
 2026-03-06 11:43:26 -05:00
sl-jetson
a3d3ea1471 Merge remote-tracking branch 'origin/sl-perception/issue-478-costmaps' 
# Conflicts:
#	jetson/ros2_ws/src/saltybot_bringup/config/nav2_params.yaml
 2026-03-06 11:43:11 -05:00
sl-firmware
6f3dd46285 feat: Add Issue #503 - Audio pipeline with Jabra SPEAK 810 
Implement full audio pipeline with:
- Jabra SPEAK 810 USB audio I/O (mic + speaker)
- openwakeword 'Hey Salty' wake word detection
- whisper.cpp GPU-accelerated STT (small/base/medium/large models)
- piper TTS synthesis and playback
- Audio state machine: listening → processing → speaking
- MQTT status and state reporting
- Real-time latency metrics tracking

ROS2 Topics Published:
- /saltybot/speech/transcribed_text: STT output for voice router
- /saltybot/audio/state: Current audio state
- /saltybot/audio/status: JSON metrics with latencies

MQTT Topics:
- saltybot/audio/state: Current state (listening/processing/speaking)
- saltybot/audio/status: Complete status JSON

Configuration parameters in yaml:
- device_name: Jabra device pattern
- wake_word_threshold: 0.5 (tunable)
- whisper_model: small/base/medium/large
- mqtt_enabled: true/false with broker config

Co-Authored-By: Claude Haiku 4.5 <noreply@anthropic.com>
 2026-03-06 10:30:58 -05:00
sl-jetson
062c05cac0 feat: Add Issue #502 - Headscale VPN auto-connect on Orin 
Configure Jetson Orin with Tailscale client connecting to Headscale
coordination server at tailscale.vayrette.com:8180. Device registers
as 'saltylab-orin' with persistent auth key for unattended login.

Features:
- systemd auto-start and restart on WiFi drops
- Persistent auth key storage at /opt/saltybot/tailscale-auth.key
- SSH + HTTP access over Tailscale tailnet (encrypted WireGuard)
- IP forwarding enabled for relay/exit node capability
- WiFi resilience with aggressive restart policy
- MQTT reporting of VPN status, IP, and connection type

Components added:
- jetson/scripts/setup-tailscale.sh: Tailscale package installation
- jetson/scripts/headscale-auth-helper.sh: Auth key management utility
- jetson/systemd/tailscale-vpn.service: systemd service unit
- jetson/docs/headscale-vpn-setup.md: Comprehensive setup documentation
- saltybot_cellular/vpn_status_node.py: ROS2 node for MQTT reporting

Updated:
- jetson/systemd/install_systemd.sh: Include tailscale-vpn.service
- jetson/scripts/setup-jetson.sh: Add Tailscale setup steps

Access patterns:
- SSH: ssh user@saltylab-orin.tail12345.ts.net
- HTTP: http://saltylab-orin.tail12345.ts.net:port
- Direct IP: 100.x.x.x (Tailscale allocated address)

Co-Authored-By: Claude Haiku 4.5 <noreply@anthropic.com>
 2026-03-06 10:25:04 -05:00
sl-webui
767f377120 feat: Add Issue #504 - Integration test suite with launch_testing 
Create saltybot_tests package with comprehensive automated testing:

Test Coverage:
- Node startup verification (all critical nodes within 30s)
- Topic publishing verification
- TF tree completeness (all transforms present)
- Sensor health checks (RPLIDAR, RealSense, IMU)
- Perception pipeline (person detection availability)
- Navigation stack (odometry, transforms)
- System stability (30-second no-crash test)
- Graceful shutdown verification

Features:
- launch_testing framework for automated startup tests
- NodeChecker: wait for nodes in ROS graph
- TFChecker: verify TF tree completeness
- TopicMonitor: track message rates and counts
- Follow mode tests (minimal hardware deps)
- Subsystem-specific tests for sensor health
- Comprehensive README with troubleshooting

Usage:
  pytest src/saltybot_tests/test/test_launch.py -v -s
  or
  colcon test --packages-select saltybot_tests

Performance Targets:
- Node startup: <30s (follow mode)
- RPLIDAR: 10 Hz scan rate
- RealSense: 30 Hz RGB + depth
- Person detection: 5 Hz
- System stability: 30s no-crash validation

Co-Authored-By: Claude Haiku 4.5 <noreply@anthropic.com>
 2026-03-06 10:22:38 -05:00
Sebastien Vayrette
868b453777 fix: resolve merge conflicts for voice router PR #499 (keep both docking + mission logging) 2026-03-05 19:25:23 -05:00