82e836ec3f
Merge pull request 'feat(social): energy+ZCR voice activity detection node (Issue #242 )' ( #247 ) from sl-jetson/issue-242-vad into main
social-bot integration tests / Lint (flake8 + pep257) (push) Failing after 10s
social-bot integration tests / Core integration tests (mock sensors, no GPU) (push) Has been skipped
social-bot integration tests / Latency profiling (GPU, Orin) (push) Has been cancelled
2026-03-02 12:46:26 -05:00
3c276b0bce
Merge pull request 'feat(controls): Smooth acceleration rate limiter (Issue #241 )' ( #246 ) from sl-controls/issue-241-smooth-accel into main
2026-03-02 12:46:22 -05:00
4919dc0bc6
feat(social): energy+ZCR voice activity detection node (Issue #242 )
...
social-bot integration tests / Lint (flake8 + pep257) (push) Failing after 2s
social-bot integration tests / Core integration tests (mock sensors, no GPU) (push) Has been skipped
social-bot integration tests / Lint (flake8 + pep257) (pull_request) Failing after 2s
social-bot integration tests / Core integration tests (mock sensors, no GPU) (pull_request) Has been skipped
social-bot integration tests / Latency profiling (GPU, Orin) (push) Has been cancelled
social-bot integration tests / Latency profiling (GPU, Orin) (pull_request) Has been cancelled
Add vad_node to saltybot_social: subscribes to /social/speech/audio_raw
(UInt8MultiArray PCM-16), computes RMS energy (dBFS) and zero-crossing
rate per chunk, applies onset/offset hysteresis (VadStateMachine), and
publishes /social/speech/is_speaking (Bool) and /social/speech/energy
(Float32 linear RMS). All thresholds configurable via ROS params:
rms_threshold_db=-35.0, zcr_min=0.01, zcr_max=0.40, onset_frames=2,
offset_frames=8, audio_topic. 69/69 tests passing.
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-03-02 12:26:01 -05:00
60d8d19342
feat(controls): Smooth acceleration rate limiter (Issue #241 )
...
Implement rate-limiting for velocity commands to prevent jerky motion.
Applies independent acceleration/deceleration limits to linear and angular velocity.
Features:
- Smooth acceleration/deceleration rate limiting
Default: 0.5 m/s² linear, 1.0 rad/s² angular (configurable)
- Independent limits for each velocity component (x, y, z linear; x, y, z angular)
- Calculates maximum change per control cycle: limit * dt
- Clamps velocity changes to stay within acceleration envelope
- 50Hz control frequency with configurable parameters
Algorithm:
- Subscribe to /cmd_vel (target velocity)
- For each component: change = target - current
- Clamp change: |change| ≤ accel_limit * period
- Apply clamped change to current velocity
- Publish smoothed /cmd_vel_smooth
Benefits:
- Prevents jerky motion from sudden velocity jumps
- Protects mechanical systems from shock loads
- Enables gradual speed/direction changes
- Smooth tracking of dynamic targets
Test Coverage:
- 30+ unit tests covering:
- Node initialization and parameter configuration
- Individual component rate limiting (linear, angular)
- Acceleration and deceleration scenarios
- Multi-component simultaneous limiting
- Reaching target velocity after multiple cycles
- Emergency stops and rapid direction changes
- Independent linear vs angular limits
- Realistic scenarios: gradual acceleration, smooth stops, turns while moving,
obstacle avoidance, continuous motion tracking, oscillating targets
Topics:
- Subscribed: /cmd_vel (geometry_msgs/Twist)
- Published: /cmd_vel_smooth (geometry_msgs/Twist)
Config: frequency=50Hz, linear_accel_limit=0.5 m/s², angular_accel_limit=1.0 rad/s²
Co-Authored-By: Claude Haiku 4.5 <noreply@anthropic.com>
2026-03-02 12:22:58 -05:00
ff34f5ac43
feat(bringup): LIDAR Euclidean object clustering + RViz visualisation (Issue #239 )
...
Adds gap-based Euclidean distance clustering of /scan LaserScan points.
Each cluster is published as a labelled semi-transparent CUBE + TEXT marker
in /saltybot/lidar_clusters (MarkerArray), sorted nearest-first. Stale
markers from shrinking cluster counts are explicitly deleted each cycle.
22/22 pure-Python tests pass (no ROS2 required).
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-03-02 12:21:35 -05:00
80a049fb0e
Merge pull request 'feat(perception): QR code reader on CSI surround frames (Issue #233 )' ( #237 ) from sl-perception/issue-233-qr-reader into main
2026-03-02 12:15:08 -05:00
b2fdc3a500
feat(perception): QR code reader on CSI surround frames (Issue #233 )
...
Adds cv2.QRCodeDetector-based QR reader that subscribes to all four IMX219
CSI camera streams, deduplicates detections with a 2 s per-payload cooldown,
and publishes /saltybot/qr_codes (QRDetectionArray) at 10 Hz. New
QRDetection / QRDetectionArray messages added to saltybot_scene_msgs.
16/16 pure-Python tests pass (no ROS2 required).
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-03-02 12:12:57 -05:00
570ebd3d22
feat(controls): Tilt-compensated compass heading node (Issue #235 )
...
Implement magnetometer-based heading calculation with tilt compensation
and magnetic declination correction for France (1.5° east).
Features:
- Tilt-compensated heading using quaternion-based orientation
Roll and pitch compensation for uneven terrain
- Magnetic declination correction: 1.5° for France
- Heading normalization to 0-360 degree range
- Publishes both Float64 (degrees) and quaternion representations
- 10Hz publishing frequency with configurable parameters
Algorithm:
- Subscribe to IMU (quaternion orientation) and magnetometer data
- Convert quaternion to roll/pitch/yaw for tilt compensation
- Project magnetometer vector onto horizontal plane using trig functions
- Apply declination correction and normalize heading
- Publish heading as Float64 degrees and quaternion (Z-axis rotation only)
Test Coverage:
- 30+ unit tests covering:
- Node initialization and parameters
- Quaternion to Euler conversion (identity, 90° rotations)
- Heading quaternion creation (0°, 90°, 180°, custom angles)
- Tilt-compensated heading with roll, pitch, combined tilts
- Declination correction application
- Sensor subscription handlers
- Heading angle normalization and wrapping
- Realistic scenarios (level, tilted uphill/sideways, 3D tilt, weak signal, continuous rotation)
Topics:
- Subscribed: /saltybot/imu/data (Imu), /saltybot/mag (MagneticField)
- Published: /saltybot/heading (Float64), /saltybot/heading_quaternion (QuaternionStamped)
Config: frequency=10Hz, declination_deg=1.5
Co-Authored-By: Claude Haiku 4.5 <noreply@anthropic.com>
2026-03-02 12:11:37 -05:00
3b352ad2c5
Merge pull request 'feat(social): 68-point Kalman landmark smoother (Issue #227 )' ( #231 ) from sl-perception/issue-227-landmark-smooth into main
social-bot integration tests / Lint (flake8 + pep257) (push) Failing after 10s
social-bot integration tests / Core integration tests (mock sensors, no GPU) (push) Has been skipped
social-bot integration tests / Latency profiling (GPU, Orin) (push) Has been cancelled
2026-03-02 12:08:53 -05:00
24340dea9b
feat(social): add 68-point Kalman landmark smoother (Issue #227 )
...
social-bot integration tests / Lint (flake8 + pep257) (pull_request) Failing after 2s
social-bot integration tests / Core integration tests (mock sensors, no GPU) (pull_request) Has been skipped
social-bot integration tests / Latency profiling (GPU, Orin) (pull_request) Has been cancelled
Adds per-person constant-velocity Kalman filter that smooths raw 68-point
facial landmarks and republishes on /social/faces/landmarks_smooth at input
rate. New FaceLandmarks / FaceLandmarksArray messages added to
saltybot_social_msgs. 21/21 pure-Python tests pass (no ROS2 required).
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-03-02 12:05:24 -05:00
173b538f44
feat(controls): Priority-based cmd_vel multiplexer (Issue #228 )
...
Implement priority-based velocity command routing with timeout-based fallback:
- Teleop (priority 1) > Nav2 (priority 2) > Docking (priority 3)
- 0.5s timeout per source: inactive sources are skipped in priority selection
- When no source is active, publish zero command for safety
Features:
- 50Hz multiplexing frequency with configurable parameters
- JSON status publishing (/saltybot/cmd_vel_mux_status) for telemetry
- Automatic priority escalation: teleop preempts nav2, nav2 preempts docking
- Fallback chain: if teleop times out, nav2 takes over; if nav2 times out, docking active
- Zero command safety: all sources timeout = immediate motor stop
Test Coverage:
- 20+ unit tests covering enum, initialization, subscriptions
- Priority selection logic (teleop preemption, nav2 preemption)
- Timeout detection and source fallback
- Realistic scenario tests (teleop release, priority escalation chains)
- JSON status format validation
Config: frequency=50Hz, source_timeout=0.5s
Co-Authored-By: Claude Haiku 4.5 <noreply@anthropic.com>
2026-03-02 12:01:00 -05:00
3a7cd3649a
Merge pull request 'feat(controls): motor current protection (Issue #223 )' ( #224 ) from sl-controls/issue-223-motor-protection into main
2026-03-02 11:57:54 -05:00
4730ea46d1
feat: Add Issue #223 - Motor current protection (I^2R thermal derating)
...
Monitors motor current, detects overcurrent (soft 5A, hard 8A), models thermal
state using I^2R heating model, and applies speed derating for safe operation.
Overcurrent Protection:
- Soft limit: 5A (warning, trigger speed derating)
- Hard limit: 8A (fault, immediate shutdown)
- Monitors both left and right motor currents
- Triggers emergency stop on hard fault
Thermal Derating (I^2R Model):
Heat generation: P_loss = I^2 * R (both motors combined)
Heat dissipation: P_cool = cooling_constant * ΔT
Temperature dynamics: dT/dt = (P_loss - P_cool) / thermal_mass
Temperature-based speed derating:
- Full speed at ambient temperature
- Linear derating from 25°C to 80°C limit
- Aggressive derating near 80°C limit
- Zero speed at or above 80°C
Combined Protection:
- Speed derate = min(current_derate, thermal_derate)
- Hard fault → 0% speed (immediate stop)
- Soft fault → gradual derating based on current
- High temperature → gradual derating approaching zero
- Provides protective action before damage
Published Topics:
- /saltybot/motor_protection (std_msgs/String) - JSON status
* state (NORMAL/SOFT_FAULT/HARD_FAULT)
* current_left, current_right, current_max (A)
* motor_temp (°C)
* soft/hard limits
- /saltybot/speed_limit (std_msgs/Float32) - Thermal derate [0, 1]
Subscribed Topics:
- /saltybot/motor_current_left (std_msgs/Float32) - Left motor (A)
- /saltybot/motor_current_right (std_msgs/Float32) - Right motor (A)
Package: saltybot_motor_protection
Entry point: motor_protection_node
Frequency: 50Hz (20ms cycle)
Thermal Model Parameters:
- Motor resistance: 1.5 Ω
- Thermal mass: 100 J/°C
- Ambient temperature: 25°C
- Max safe temperature: 80°C
- Cooling constant: 0.05 1/s
Features:
✓ Multi-motor current monitoring (worst-case approach)
✓ I^2R Joule heating with passive cooling
✓ Exponential temperature dynamics
✓ Two-level overcurrent protection
✓ Combined current+thermal derating
✓ Soft fault duration tracking
✓ Automatic recovery on current drop
✓ JSON telemetry with state and metrics
Tests: 25+ unit tests covering:
- ThermalModel initialization and parameters
- Current subscription and clamping
- Overcurrent detection (soft, hard)
- Fault recovery and state transitions
- Joule heating calculation (I^2R)
- Temperature rise and cooling
- Speed derating (normal, soft fault, thermal, hard fault)
- Derate clipping and bounds
- JSON status format
- Realistic scenarios (thermal rise, overcurrent spike, asymmetric loading)
- Combined current+thermal derating
Co-Authored-By: Claude Haiku 4.5 <noreply@anthropic.com>
2026-03-02 11:57:17 -05:00
bd4a9f094d
feat(social): hand gesture pointing direction node (Issue #221 )
...
social-bot integration tests / Lint (flake8 + pep257) (pull_request) Failing after 2s
social-bot integration tests / Core integration tests (mock sensors, no GPU) (pull_request) Has been skipped
social-bot integration tests / Latency profiling (GPU, Orin) (pull_request) Has been cancelled
saltybot_social_msgs:
- Add PointingTarget.msg: origin (INDEX_MCP), direction (unit vec), target,
range_m, person_id, confidence, coarse_direction, is_active
- Register in CMakeLists.txt
saltybot_social:
- _pointing_ray.py (pure Python, no rclpy): unproject(), sample_depth()
(median with outlier rejection), compute_pointing_ray() — reprojects
INDEX_MCP and INDEX_TIP into 3-D using D435i depth; falls back to image-
plane direction when both depths are equal; gracefully handles one-sided
missing depth
- pointing_node.py: subscribes /social/gestures + synced D435i colour+depth;
re-runs MediaPipe Hands when a 'point' gesture is cached (within
gesture_timeout_s); picks closest hand to gesture anchor; publishes
PointingTarget on /saltybot/pointing_target at 5 Hz
- setup.py: adds pointing_node entry point
- 18/18 unit tests pass
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-03-02 11:56:54 -05:00
3a34ec84e0
Merge pull request 'feat(bringup): obstacle height filter with IMU pitch compensation (Issue #211 )' ( #219 ) from sl-perception/issue-211-height-filter into main
2026-03-02 11:51:37 -05:00
b722279739
feat(bringup): scan height filter with IMU pitch compensation (Issue #211 )
...
Two files added to saltybot_bringup:
- _scan_height_filter.py: pure-Python helpers (no rclpy) —
filter_scan_by_height() projects each LIDAR ray to world-frame height
using pitch/roll from the IMU and filters ground/ceiling returns;
pitch_roll_from_accel() uses convention-agnostic atan2 formula;
AttitudeEstimator low-pass filters the accelerometer attitude.
- scan_height_filter_node.py: subscribes /scan + /camera/imu, publishes
/scan_filtered (LaserScan) for Nav2 at source rate (up to 20 Hz).
setup.py: adds scan_height_filter entry point.
18/18 unit tests pass.
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-03-02 11:50:56 -05:00
938c2a33d4
feat: Add Issue #216 - Odometry fusion node (complementary filter)
...
Fuses wheel, visual, and IMU odometry using complementary filtering. Publishes
fused /odom (nav_msgs/Odometry) and broadcasts odom→base_link TF at 50Hz.
Sensor Fusion Strategy:
- Wheel odometry: High-frequency accurate linear displacement (weight: 0.6)
- Visual odometry: Loop closure and long-term drift correction (weight: 0.3)
- IMU: High-frequency attitude and angular velocity (weight: 0.1)
Complementary Filter Architecture:
- Fast loop (IMU): High-frequency attitude updates, angular velocity
- Slow loop (Vision): Low-frequency position/orientation correction
- Integration: Velocity-based position updates with covariance weighting
- Dropout handling: Continues with available sources if sensors drop
Fusion Algorithm:
1. Extract velocities from wheel odometry (most reliable linear)
2. Apply IMU angular velocity (highest frequency rotation)
3. Update orientation from IMU with blending
4. Integrate velocities to position (wheel odometry frame)
5. Apply visual odometry drift correction (low-frequency)
6. Update covariances based on available measurements
7. Publish fused odometry with full covariance matrices
Published Topics:
- /odom (nav_msgs/Odometry) - Fused pose/twist with covariance
- /saltybot/odom_fusion_info (std_msgs/String) - JSON debug info
TF Broadcasts:
- odom→base_link - Position (x, y) and orientation (yaw)
Subscribed Topics:
- /saltybot/wheel_odom (nav_msgs/Odometry) - Wheel encoder odometry
- /rtab_map/odom (nav_msgs/Odometry) - Visual/SLAM odometry
- /imu/data (sensor_msgs/Imu) - IMU data
Package: saltybot_odom_fusion
Entry point: odom_fusion_node
Frequency: 50Hz (20ms cycle)
Features:
✓ Multi-source odometry fusion
✓ Complementary filtering with configurable weights
✓ Full covariance matrices for uncertainty tracking
✓ TF2 transform broadcasting
✓ Sensor dropout handling
✓ JSON telemetry with fusion status
✓ Configurable normalization of weights
Tests: 20+ unit tests covering:
- OdomState initialization and covariances
- Subscription handling for all three sensors
- Position integration from velocity
- Angular velocity updates
- Velocity blending from multiple sources
- Drift correction from visual odometry
- Covariance updates based on measurement availability
- Quaternion to Euler angle conversion
- Realistic fusion scenarios (straight line, circles, drift correction)
- Sensor dropout and recovery
Co-Authored-By: Claude Haiku 4.5 <noreply@anthropic.com>
2026-03-02 11:50:13 -05:00
fd7eddd44d
feat: Add Issue #213 - PID auto-tuner (Astrom-Hagglund relay oscillation)
...
Implements PID auto-tuning ROS2 node using relay feedback (Astrom-Hagglund) method.
Service-triggered relay oscillation measures ultimate gain (Ku) and ultimate period
(Tu), then computes Ziegler-Nichols PD gains. Safety abort on >25deg tilt.
Features:
- Service /saltybot/autotune_pid (std_srvs/Trigger) starts tuning
- Relay oscillation method for accurate gain measurement
- Measures Ku (ultimate gain) and Tu (ultimate period)
- Computes Z-N PD gains: Kp=0.6*Ku, Kd=0.075*Ku*Tu
- Real-time safety abort >25° tilt angle
- JSON telemetry on /saltybot/autotune_info
- Relay commands on /saltybot/autotune_cmd_vel
Tuning Process:
1. Settle phase: zero command, allow oscillations to die
2. Relay oscillation: apply +/-relay_magnitude commands
3. Measure peaks: detect zero crossings, record extrema
4. Analysis: calculate Ku from peak amplitude, Tu from period
5. Gain computation: Ziegler-Nichols formulas
6. Publish results: Ku, Tu, Kp, Kd
Safety Features:
- IMU tilt monitoring (abort >25°)
- Max tuning duration timeout
- Configurable settle time and oscillation cycles
Published Topics:
- /saltybot/autotune_info (std_msgs/String) - JSON with Ku, Tu, Kp, Kd
- /saltybot/autotune_cmd_vel (geometry_msgs/Twist) - Relay control
Subscribed Topics:
- /imu/data (sensor_msgs/Imu) - IMU tilt safety check
- /saltybot/balance_feedback (std_msgs/Float32) - Balance feedback
Package: saltybot_pid_autotune
Entry point: pid_autotune_node
Service: /saltybot/autotune_pid
Tests: 20+ unit tests covering:
- IMU tilt extraction
- Relay oscillation analysis
- Ku/Tu measurement
- Ziegler-Nichols gain computation
- Peak detection and averaging
- Safety limits (tilt, timeout)
- State machine transitions
- JSON telemetry format
Co-Authored-By: Claude Haiku 4.5 <noreply@anthropic.com>
2026-03-02 11:47:05 -05:00
d143a6d156
chore: remove accidentally committed __pycache__ dirs
...
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-03-02 11:39:21 -05:00
0d07b09949
feat(perception): add person re-ID node (Issue #201 )
...
Two new packages:
- saltybot_person_reid_msgs: PersonAppearance + PersonAppearanceArray msgs
- saltybot_person_reid: MobileNetV2 torso-crop embedder (128-dim L2-norm)
with 128-bin HSV histogram fallback, cosine-similarity gallery with EMA
identity updates and configurable age-based pruning, ROS2 node publishing
PersonAppearanceArray on /saltybot/person_reid at 5 Hz.
Pure-Python helpers (_embedding_model, _reid_gallery) importable without
rclpy — 18/18 unit tests pass.
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-03-02 11:39:21 -05:00
03e7995e66
Merge pull request 'feat(jetson): CPU/GPU thermal monitor — sysfs + /saltybot/thermal JSON (Issue #205 )' ( #209 ) from sl-jetson/issue-205-thermal into main
2026-03-02 11:39:03 -05:00
1600691ec5
Merge pull request 'feat(controls): node watchdog monitor (Issue #203 )' ( #207 ) from sl-controls/issue-203-watchdog into main
2026-03-02 11:38:55 -05:00
58bb5ef18e
feat(jetson): CPU/GPU thermal monitor — sysfs + /saltybot/thermal JSON (Issue #205 )
...
New saltybot_thermal package with thermal_node: reads all
/sys/class/thermal/thermal_zone* sysfs entries (millidegrees→°C),
publishes /saltybot/thermal JSON at 1 Hz with zones[], max_temp_c,
warn, and throttled flags. Logs ROS2 WARN at ≥75°C, ERROR at ≥85°C.
thermal_root param allows sysfs override for offline testing.
50/50 tests passing.
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-03-02 11:21:18 -05:00
e247389b07
feat: Add Issue #203 - Node watchdog monitor (20Hz heartbeat detection)
...
Implements node watchdog ROS2 node that monitors heartbeats from critical
systems and triggers safety fallback when motor driver is lost >2s.
Features:
- Monitor heartbeats from: balance, motor_driver, emergency, docking
- Alert on /saltybot/node_watchdog (JSON) if heartbeat lost >1s
- Safety fallback: zero cmd_vel if motor driver lost >2s
- Republish cmd_vel on /saltybot/cmd_vel_safe with safety checks
- 20Hz monitoring and publishing frequency
- Configurable heartbeat timeout thresholds
Heartbeat Topics:
- /saltybot/balance_heartbeat (std_msgs/UInt32)
- /saltybot/motor_driver_heartbeat (std_msgs/UInt32)
- /saltybot/emergency_heartbeat (std_msgs/UInt32)
- /saltybot/docking_heartbeat (std_msgs/UInt32)
- /cmd_vel (geometry_msgs/Twist)
Published Topics:
- /saltybot/node_watchdog (std_msgs/String) - JSON status
- /saltybot/cmd_vel_safe (geometry_msgs/Twist) - Safety-checked velocity
Package: saltybot_node_watchdog
Entry point: node_watchdog_node
Launch file: node_watchdog.launch.py
Tests: 20+ unit tests covering:
- Heartbeat reception and timeout detection
- Motor driver critical timeout (>2s)
- Safety fallback logic
- cmd_vel zeroing on motor driver loss
- Health status JSON serialization
- Multi-node failure scenarios
Co-Authored-By: Claude Haiku 4.5 <noreply@anthropic.com>
2026-03-02 11:17:19 -05:00
bfd58fc98c
Merge pull request 'feat(social): robot mesh comms — peer announce + person handoff (Issue #171 )' ( #202 ) from sl-jetson/issue-171-mesh-comms into main
social-bot integration tests / Lint (flake8 + pep257) (push) Failing after 2s
social-bot integration tests / Core integration tests (mock sensors, no GPU) (push) Has been skipped
social-bot integration tests / Latency profiling (GPU, Orin) (push) Has been cancelled
2026-03-02 11:17:03 -05:00
bc45208768
feat: Add Issue #194 - Speed limiter node (50Hz safety constraints)
...
Implements speed limiter ROS2 node that enforces safety constraints from:
- Terrain type (via /saltybot/terrain_speed_scale)
- Battery level (via /saltybot/speed_limit)
- Emergency system state (via /saltybot/emergency)
Node computes minimum scale factor and applies to /cmd_vel, forwarding
limited commands to /saltybot/cmd_vel_limited. Publishes JSON telemetry
on /saltybot/speed_limit_info at 50Hz (20ms cycle).
Features:
- Subscription to terrain, battery, and emergency constraints
- Twist velocity scaling with min-of-three constraint logic
- JSON telemetry with timestamp and all scale factors
- 50Hz timer-based publishing for real-time control
- Emergency state mapping (NOMINAL/RECOVERING/STOPPING/ESCALATED)
- Comprehensive unit tests (18 test cases)
Package: saltybot_speed_limiter
Entry point: speed_limiter_node
Launch file: speed_limiter.launch.py
Co-Authored-By: Claude Haiku 4.5 <noreply@anthropic.com>
2026-03-02 11:14:10 -05:00
e5236f781b
feat(social): robot mesh comms — peer announce + person handoff (Issue #171 )
...
social-bot integration tests / Lint (flake8 + pep257) (push) Failing after 9s
social-bot integration tests / Core integration tests (mock sensors, no GPU) (push) Has been skipped
social-bot integration tests / Lint (flake8 + pep257) (pull_request) Failing after 9s
social-bot integration tests / Core integration tests (mock sensors, no GPU) (pull_request) Has been skipped
social-bot integration tests / Latency profiling (GPU, Orin) (push) Has been cancelled
social-bot integration tests / Latency profiling (GPU, Orin) (pull_request) Has been cancelled
New MeshPeer.msg (1 Hz DDS heartbeat: robot_id, social_state, active persons,
greeted names) and MeshHandoff.msg (person context transfer on STATE_LEAVING).
mesh_comms_node subscribes to person_states and orchestrator/state, publishes
announce heartbeat, triggers handoff on LEAVING, tracks peers with timeout
cleanup, and propagates mesh-wide greeting deduplication via /social/mesh/greeted.
73/73 tests passing.
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-03-02 11:14:00 -05:00
e6065e1531
feat(jetson): camera health watchdog node (issue #198 )
...
Adds camera_health_node.py + _camera_state.py to saltybot_bringup:
• _camera_state.py — pure-Python CameraState dataclass (no ROS2):
on_frame(), age_s, fps(window_s), status(),
should_reset() + mark_reset() with 30s cooldown
• camera_health_node.py — subscribes 6 image topics (D435i color/depth
+ 4× IMX219 CSI front/right/rear/left);
1 Hz tick: WARNING at >2s silence, ERROR at
>10s + v4l2 stream-off/on reset for CSI cams;
publishes /saltybot/camera_health JSON with
per-camera status, age_s, fps, total_frames
• test/test_camera_health.py — 15 unit tests (15/15 pass, no ROS2 needed)
• setup.py — adds camera_health_monitor console_scripts entry point
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-03-02 11:11:48 -05:00
d6d7e7b75a
feat(jetson): AprilTag landmark detector — DICT_APRILTAG_36h11 (issue #191 )
...
saltybot_apriltag_msgs (ament_cmake):
• DetectedTag.msg — tag_id, family, decision_margin, corners[8],
center, 6-DOF pose + pose_valid flag
• DetectedTagArray.msg — DetectedTag[], count
saltybot_apriltag (ament_python, single node):
• _aruco_utils.py — ROS2-free: ArucoBackend (cv2 4.6/4.7+ API shim),
rvec_tvec_to_pose (Rodrigues → Shepperd quaternion)
• apriltag_node.py — 10 Hz timer; subscribes image + latched camera_info;
cv2.solvePnP IPPE_SQUARE for 6-DOF pose when K
available; publishes /saltybot/apriltags
• test/test_apriltag.py — 11 unit tests (11/11 pass, no ROS2 needed):
pose math + rendered tag detection + multi-tag
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-03-02 11:07:35 -05:00
c26293d000
feat(jetson): depth confidence filter node (issue #190 )
...
Adds depth_confidence_filter_node.py to saltybot_bringup:
- Synchronises /camera/depth/image_rect_raw + /camera/depth/confidence
via ApproximateTimeSynchronizer (10ms slop)
- Zeros pixels where confidence uint8 < threshold * 255 (default 0.5)
- Republishes filtered float32 depth on /camera/depth/filtered
- Registered as depth_confidence_filter console_scripts entry point
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-03-02 11:02:15 -05:00
90c8b427fc
feat(social): multi-language support — Whisper LID + per-lang Piper TTS (Issue #167 )
...
social-bot integration tests / Lint (flake8 + pep257) (push) Failing after 2s
social-bot integration tests / Core integration tests (mock sensors, no GPU) (push) Has been skipped
social-bot integration tests / Lint (flake8 + pep257) (pull_request) Failing after 10s
social-bot integration tests / Core integration tests (mock sensors, no GPU) (pull_request) Has been skipped
social-bot integration tests / Latency profiling (GPU, Orin) (push) Has been cancelled
social-bot integration tests / Latency profiling (GPU, Orin) (pull_request) Has been cancelled
- Add SpeechTranscript.language (BCP-47), ConversationResponse.language fields
- speech_pipeline_node: whisper_language param (""=auto-detect via Whisper LID);
detected language published in every transcript
- conversation_node: track per-speaker language; inject "[Please respond in X.]"
hint for non-English speakers; propagate language to ConversationResponse.
_LANG_NAMES: 24 BCP-47 codes -> English names. Also adds Issue #161 emotion
context plumbing (co-located in same branch for clean merge)
- tts_node: voice_map_json param (JSON BCP-47->ONNX path); lazy voice loading
per language; playback queue now carries (text, lang) tuples for voice routing
- speech_params.yaml, tts_params.yaml: new language params with docs
- 47/47 tests pass (test_multilang.py)
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-03-02 10:57:34 -05:00
728d1b0c0e
Merge pull request 'feat(webui): live camera viewer — multi-stream + detection overlays (Issue #177 )' ( #182 ) from sl-webui/issue-177-camera-viewer into main
2026-03-02 10:48:50 -05:00
57420807ca
feat(webui): live camera viewer — multi-stream + detection overlays (Issue #177 )
...
UI (src/hooks/useCamera.js, src/components/CameraViewer.jsx):
- 7 camera sources: front/left/rear/right CSI, D435i RGB/depth, panoramic
- Compressed image subscription via rosbridge (sensor_msgs/CompressedImage)
- Client-side 15fps gate (drops excess frames, reduces JS pressure)
- Per-camera FPS indicator with quality badge (FULL/GOOD/LOW/NO SIGNAL)
- Detection overlays: face boxes + names (/social/faces/detections),
gesture icons (/social/gestures), scene object labels + hazard colours
(/social/scene/objects); overlay mode selector (off/faces/gestures/objects/all)
- 360° panoramic equirect viewer with mouse/touch drag azimuth pan
- Picture-in-picture: up to 3 pinned cameras via ⊕ button
- One-click recording (MediaRecorder → MP4/WebM download)
- Snapshot to PNG with detection overlay composite + timestamp watermark
- Cameras tab added to TELEMETRY group in App.jsx
Jetson (rosbridge bringup):
- rosbridge_params.yaml: whitelist + /camera/depth/image_rect_raw/compressed,
/camera/panoramic/compressed, /social/faces/detections,
/social/gestures, /social/scene/objects
- rosbridge.launch.py: D435i colour republisher (JPEG 75%) +
depth republisher (compressedDepth/PNG16 preserving uint16 values)
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-03-02 10:47:01 -05:00
9ca0e0844c
Merge pull request 'feat(social): facial expression recognition — TRT FP16 emotion CNN (Issue #161 )' ( #180 ) from sl-jetson/issue-161-emotion into main
social-bot integration tests / Lint (flake8 + pep257) (push) Failing after 10s
social-bot integration tests / Core integration tests (mock sensors, no GPU) (push) Has been skipped
social-bot integration tests / Latency profiling (GPU, Orin) (push) Has been cancelled
2026-03-02 10:46:21 -05:00
54668536c1
Merge pull request 'feat(jetson): dynamic obstacle tracking — LIDAR motion detection, Kalman tracking, trajectory prediction, Nav2 costmap ( #176 )' ( #181 ) from sl-perception/issue-176-dynamic-obstacles into main
2026-03-02 10:45:22 -05:00
2f4540f1d3
feat(jetson): add dynamic obstacle tracking package (issue #176 )
...
Implements real-time moving obstacle detection, Kalman tracking, trajectory
prediction, and Nav2 costmap integration at 10 Hz / <50ms latency:
saltybot_dynamic_obs_msgs (ament_cmake):
• TrackedObject.msg — id, PoseWithCovariance, velocity, predicted_path,
predicted_times, speed, confidence, age, hits
• MovingObjectArray.msg — TrackedObject[], active_count, tentative_count,
detector_latency_ms
saltybot_dynamic_obstacles (ament_python):
• object_detector.py — LIDAR background subtraction (EMA occupancy grid),
foreground dilation + scipy connected-component
clustering → Detection list
• kalman_tracker.py — CV Kalman filter, state [px,py,vx,vy], Joseph-form
covariance update, predict_horizon() (non-mutating)
• tracker_manager.py — up to 20 tracks, Hungarian assignment
(scipy.optimize.linear_sum_assignment), TENTATIVE→
CONFIRMED lifecycle, miss-prune
• dynamic_obs_node.py — 10 Hz timer: detect→track→publish
/saltybot/moving_objects + MarkerArray viz
• costmap_layer_node.py — predicted paths → PointCloud2 inflation smear
→ /saltybot/dynamic_obs_cloud for Nav2 ObstacleLayer
• launch/dynamic_obstacles.launch.py + config/dynamic_obstacles_params.yaml
• test/test_dynamic_obstacles.py — 27 unit tests (27/27 pass, no ROS2 needed)
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-03-02 10:44:32 -05:00
50971c0946
feat(social): facial expression recognition — TRT FP16 emotion CNN (Issue #161 )
...
social-bot integration tests / Lint (flake8 + pep257) (push) Failing after 2s
social-bot integration tests / Core integration tests (mock sensors, no GPU) (push) Has been skipped
social-bot integration tests / Lint (flake8 + pep257) (pull_request) Failing after 2s
social-bot integration tests / Core integration tests (mock sensors, no GPU) (pull_request) Has been skipped
social-bot integration tests / Latency profiling (GPU, Orin) (push) Has been cancelled
social-bot integration tests / Latency profiling (GPU, Orin) (pull_request) Has been cancelled
- Add Expression.msg / ExpressionArray.msg ROS2 message definitions
- Add emotion_classifier.py: 7-class CNN (happy/sad/angry/surprised/fearful/disgusted/neutral)
via TensorRT FP16 with landmark-geometry fallback; EMA per-person smoothing; opt-out registry
- Add emotion_node.py: subscribes /social/faces/detections, runs TRT crop inference (<5ms),
publishes /social/faces/expressions and /social/emotion/context JSON for LLM
- Wire emotion context into conversation_node.py: emotion hint injected into LLM prompt
when speaker shows non-neutral affect; subscribes /social/emotion/context
- Add emotion_params.yaml config and emotion.launch.py launch file
- Add 67-test suite (test_emotion_classifier.py): classifier, tracker, opt-out, heuristic
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-03-02 10:40:54 -05:00
3b2f219d66
feat( #169 ): emergency behavior system — obstacle stop, fall prevention, stuck detection, recovery FSM
...
Two new packages:
- saltybot_emergency_msgs: EmergencyEvent.msg, RecoveryAction.msg
- saltybot_emergency: threat_detector, alert_manager, recovery_sequencer, emergency_fsm, emergency_node
Implements:
- ObstacleDetector: MAJOR <30cm, CRITICAL <10cm; suppressed when not moving
- FallDetector: MINOR/MAJOR/CRITICAL tilt thresholds; floor-drop edge detection
- StuckDetector: MAJOR after 3s wheel stall (cmd>threshold, actual~0)
- BumpDetector: jerk = |Δ|a||/dt with gravity removal; MAJOR/CRITICAL thresholds
- AlertManager: per-(type,level) suppression; MAJOR×N within window → CRITICAL escalation
- RecoverySequencer: REVERSING→TURNING→RETRYING FSM; max_retries before GAVE_UP
- EmergencyFSM: NOMINAL→STOPPING→RECOVERING→ESCALATED; acknowledge to clear
- EmergencyNode: 20Hz ROS2 node; /saltybot/emergency, /saltybot/e_stop, cmd_vel mux
59/59 tests passing.
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-03-02 10:39:37 -05:00
7dcdd2088a
feat(jetson): add saltybot_night_vision package (issue #168 )
...
Implements ambient-light-aware night vision mode for the D435i + IMX219
stack on the Jetson Orin Nano Super:
• light_analyser.py — histogram-based intensity FSM with hysteresis:
day → twilight → night → ir_only
• camera_controller.py — D435i IR emitter via realsense2_camera param
service + IMX219 gain/exposure via v4l2-ctl
• gpio_headlight.py — physical pin 33 headlight; Jetson.GPIO PWM
primary, sysfs on/off fallback, sim mode
• light_monitor_node.py — subscribes IMX219/IR1, publishes
/saltybot/ambient_light + /saltybot/vision_mode
• night_vision_controller_node.py — reacts to mode changes; drives
D435i emitter, IMX219 gain, headlight
• ir_slam_bridge_node.py — mono8 IR1 → rgb8 republish so RTAB-Map
keeps loop-closing in darkness
• launch/night_vision.launch.py + config/night_vision_params.yaml
• test/test_night_vision.py — 18 unit tests (18/18 pass, no ROS2 needed)
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-03-02 10:36:21 -05:00
77b3d614dc
Merge pull request 'feat( #158 ): docking station auto-return — ArUco/IR detection, visual servo, charge monitoring' ( #165 ) from sl-controls/issue-158-docking into main
2026-03-02 10:26:17 -05:00
06f72521c9
Merge pull request 'feat(vo): visual odometry fallback — CUDA optical flow + EKF fusion + slip failover (Issue #157 )' ( #164 ) from sl-perception/issue-157-visual-odom into main
2026-03-02 10:26:09 -05:00
783dedf4d4
feat( #158 ): docking station auto-return with ArUco/IR detection and visual servo
...
Two new ROS2 packages implementing Issue #158 :
saltybot_docking_msgs (ament_cmake)
- DockingStatus.msg: stamp, state, dock_detected, distance_m, lateral_error_m,
battery_pct, charging, aligned
- Dock.srv / Undock.srv: force + resume_mission flags
saltybot_docking (ament_python, 20 Hz)
- dock_detector.py: ArucoDetector (cv2.aruco PnP → DockPose) + IRBeaconDetector
(EMA envelope with amplitude threshold); both gracefully degrade if unavailable
- visual_servo.py: IBVS proportional controller — v = k_lin×(d−target),
ω = −k_ang×yaw; aligned when |lateral| < 5mm AND d < contact_distance
- charge_monitor.py: edge-triggered events (CHARGING_STARTED/STOPPED,
THRESHOLD_LOW at 15%, THRESHOLD_HIGH at 80%)
- docking_state_machine.py: 7-state FSM (IDLE→DETECTING→NAV2_APPROACH→
VISUAL_SERVO→CONTACT→CHARGING→UNDOCKING); mission_resume signal on
auto-dock cycle; contact retry on timeout; lost-detection timeout
- docking_node.py: 20Hz ROS2 node; Nav2 NavigateToPose action client (optional);
/saltybot/dock + /saltybot/undock services; /saltybot/docking_cmd_vel;
/saltybot/resume_mission; /saltybot/docking_status
- config/docking_params.yaml, launch/docking.launch.py
Tests: 64/64 passing (IRBeaconDetector, VisualServo, ChargeMonitor,
DockingStateMachine — all state transitions and guard conditions covered)
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-03-02 10:19:22 -05:00
572e578069
feat(vo): visual odometry fallback — CUDA optical flow + EKF fusion + slip failover (Issue #157 )
...
New package: saltybot_visual_odom (13 files, ~900 lines)
Nodes:
visual_odom_node — D435i IR1 stereo VO at 30 Hz
CUDA: SparsePyrLKOpticalFlow + FastFeatureDetector (GPU)
CPU fallback: calcOpticalFlowPyrLK + goodFeaturesToTrack
Essential matrix (5-pt RANSAC) + depth-aided metric scale
forward-backward consistency check on tracked points
Publishes /saltybot/visual_odom (Odometry)
odom_fusion_node — 5-state EKF [px, py, θ, v, ω] (unicycle model)
Fuses: wheel odom (/saltybot/rover_odom or tank_odom)
+ visual odom (/saltybot/visual_odom)
Slip failover: /saltybot/terrain JSON → 10× wheel noise on slip
Loop closure: /rtabmap/odom jump > 0.3m → EKF soft-correct
TF: publishes odom → base_link at 30 Hz
Publishes /saltybot/odom_fused + /saltybot/visual_odom_status
Modules:
optical_flow_tracker.py — CUDA/CPU sparse LK tracker with re-detection,
forward-backward consistency, ROI masking
stereo_vo.py — Essential matrix decomposition, camera→base_link
frame rotation, depth median scale recovery,
loop closure soft-correct, accumulated SE(3) pose
kalman_odom_filter.py — 5-state EKF: predict (unicycle), update_wheel,
update_visual, update_rtabmap (absolute pose);
Joseph-form covariance for numerical stability
Tests:
test/test_kalman_odom.py — 8 unit tests for EKF + StereoVO (no ROS deps)
Topic/TF map:
/camera/infra1/image_rect_raw → visual_odom_node
/camera/depth/image_rect_raw → visual_odom_node
/saltybot/visual_odom ← visual_odom_node (30 Hz)
/saltybot/rover_odom → odom_fusion_node
/saltybot/terrain → odom_fusion_node (slip signal)
/rtabmap/odom → odom_fusion_node (loop closure)
/saltybot/odom_fused ← odom_fusion_node (30 Hz)
odom → base_link TF ← odom_fusion_node (30 Hz)
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-03-02 10:15:32 -05:00
ce6d5ee249
feat(social): multi-camera gesture recognition — MediaPipe Hands + Pose (Issue #140 )
...
social-bot integration tests / Lint (flake8 + pep257) (push) Failing after 9s
social-bot integration tests / Core integration tests (mock sensors, no GPU) (push) Has been skipped
social-bot integration tests / Lint (flake8 + pep257) (pull_request) Failing after 2s
social-bot integration tests / Core integration tests (mock sensors, no GPU) (pull_request) Has been skipped
social-bot integration tests / Latency profiling (GPU, Orin) (push) Has been cancelled
social-bot integration tests / Latency profiling (GPU, Orin) (pull_request) Has been cancelled
Delivers Issue #140 (P2): real-time gesture detection from 4 CSI cameras via
MediaPipe Hands and Pose, publishing classified gestures on /social/gestures.
New files:
- saltybot_social_msgs/msg/Gesture.msg + GestureArray.msg — ROS2 message types
- saltybot_social/gesture_classifier.py — pure-Python geometric classifier
(stop_palm, thumbs_up/down, point, come_here, follow, wave, arms_up,
arms_spread, crouch); WaveDetector temporal sliding-window oscillation tracker
- saltybot_social/gesture_node.py — ROS2 node; round-robin multi-camera
_FrameBuffer, lazy MediaPipe init, person-ID correlation via PersonState
- saltybot_social/test/test_gesture_classifier.py — 70 unit tests, all passing
- saltybot_social/config/gesture_params.yaml — tuned defaults for Orin Nano
- saltybot_social/launch/gesture.launch.py — all params overridable at launch
Modified:
- saltybot_social_msgs/CMakeLists.txt — register Gesture + GestureArray msgs
- saltybot_social/setup.py — add gesture_node console_scripts entry point
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-03-02 10:10:54 -05:00
33007fb5ed
Merge pull request 'feat( #142 ): terrain adaptation — surface detection + dynamic speed/PID/bias' ( #154 ) from sl-controls/issue-142-terrain into main
2026-03-02 10:07:38 -05:00
fb3e9f1bf1
feat( #142 ): terrain adaptation — surface detection, speed/PID/bias adaptation
...
Two new ROS2 packages implementing Issue #142 :
saltybot_terrain_msgs (ament_cmake)
- Terrain.msg: stamp, surface_type, roughness, incline_rad, grip, slip_ratio, speed_scale
saltybot_terrain_adaptation (ament_python, 50 Hz)
- terrain_analyzer.py: IMU vibration RMS → roughness [0,1] + surface classification
(smooth/tile/carpet/grass/gravel) with hysteresis-protected switching
- incline_detector.py: quaternion pitch → first-order LPF → incline_rad; motor_bias()
- slip_detector.py: cmd_vel vs odom → slip_ratio with EMA smoothing + speed gate
- terrain_adapter.py: TerrainState, TerrainAdapter (speed_scale, pid_scales, motor_bias),
TerrainHistory (rolling deque, JSON type-change log)
- terrain_adaptation_node.py: 50 Hz node; IMU watchdog; publishes terrain JSON + typed msg,
speed_scale, pid_scales (JSON), motor_bias, terrain_history (on type change)
- config/terrain_params.yaml, launch/terrain_adaptation.launch.py
Tests: 60/60 passing (TerrainAnalyzer, SlipDetector, InclineDetector, TerrainAdapter,
TerrainHistory all covered)
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-03-02 10:04:36 -05:00
a9717cd602
feat(scene): semantic scene understanding — YOLOv8n TRT + room classification + hazards (Issue #141 )
...
New packages:
saltybot_scene_msgs — 4 msgs (SceneObject, SceneObjectArray, RoomClassification, BehaviorHint)
saltybot_scene — 3 nodes + launch + config + TRT build script
Nodes:
scene_detector_node — YOLOv8-nano TRT FP16 (target ≥15 FPS @ 640×640);
synchronized RGB+depth input; filters scene classes
(chairs, tables, doors, stairs, pets, appliances);
3D back-projection via aligned depth; depth-based hazard
scan (HazardClassifier); room classification at 2Hz;
publishes /social/scene/objects + /social/scene/hazards
+ /social/scene/room_type
behavior_adapter_node — adapts speed_limit_mps + personality_mode from room
type and hazard severity; publishes BehaviorHint on
/social/scene/behavior_hint (on-change + 1Hz heartbeat)
costmap_publisher_node — converts SceneObjectArray → PointCloud2 disc rings
for Nav2 obstacle_layer + MarkerArray for RViz;
publishes /social/scene/obstacle_cloud
Modules:
yolo_utils.py — YOLOv8 preprocess/postprocess (letterbox, cx/cy/w/h decode,
NMS), COCO+custom class table (door=80, stairs=81, wet=82),
hazard-by-class mapping
room_classifier.py — rule-based (object co-occurrence weights + softmax) with
optional MobileNetV2 TRT/ONNX backend (Places365-style 8-class)
hazard_classifier.py — depth-only hazard patterns: drop (row-mean cliff), stairs
(alternating depth bands), wet floor (depth std-dev), glass
(zero depth + strong Sobel edges in RGB)
scripts/build_scene_trt.py — export YOLOv8n → ONNX → TRT FP16; optionally build
MobileNetV2 room classifier engine; includes benchmark
Topic map:
/social/scene/objects SceneObjectArray ~15+ FPS
/social/scene/room_type RoomClassification ~2 Hz
/social/scene/hazards SceneObjectArray on hazard
/social/scene/behavior_hint BehaviorHint on-change + 1 Hz
/social/scene/obstacle_cloud PointCloud2 Nav2 obstacle_layer
/social/scene/object_markers MarkerArray RViz debug
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-03-02 09:59:53 -05:00
732045a086
Merge pull request 'feat(controls): adaptive PID balance controller with gain scheduling (Issue #136 )' ( #149 ) from sl-controls/issue-136-adaptive-pid into main
2026-03-02 09:51:19 -05:00
4cda5d0f19
Merge pull request 'feat(social): voice command NLU — 30+ intents with confirmation flow (Issue #137 )' ( #148 ) from sl-jetson/issue-137-voice-commands into main
social-bot integration tests / Lint (flake8 + pep257) (push) Failing after 47s
social-bot integration tests / Core integration tests (mock sensors, no GPU) (push) Has been skipped
social-bot integration tests / Latency profiling (GPU, Orin) (push) Has been cancelled
2026-03-02 09:51:12 -05:00
4d2b008c48
feat(controls): adaptive PID balance controller with gain scheduling (Issue #136 )
...
Pure modules (no ROS2 dep, fully unit-tested):
- pid_controller.py:
GainSet — (kp,ki,kd) with safety clamp helper
PIDController — anti-windup integral, D-on-error, output clamping
GainScheduler — 3-class weight table (empty/light/heavy), exponential
gain blending (alpha per tick), safety bounds clamping, manual
override, immediate revert-to-defaults on instability
InstabilityDetector — dual criteria: tilt threshold (>50% of window)
+ sign-reversal count (oscillation)
- weight_estimator.py:
WeightEstimator — rolling-window current→weight, steady-state gating
(|tilt|≤threshold), change detection (threshold_kg)
CalibrationRoutine — IDLE→ROCKING→SETTLING→DONE/FAILED state machine;
sinusoidal rocking output, settling current sampling, weight estimate
from avg current; abort() / restart supported
- adaptive_pid_node.py: 100 Hz ROS2 node
Sub: /saltybot/imu (Imu, pitch from quaternion), /saltybot/motor_current
Pub: /saltybot/balance_effort (Float32), /saltybot/weight_estimate,
/saltybot/pid_state (JSON: gains, class, weight_kg, flags)
Srv: /saltybot/calibrate_balance (std_srvs/Trigger)
IMU watchdog (0.5 s), dynamic reconfigure via override_enabled param,
instability → revert + PID reset, structured INFO/WARN logging
- config/adaptive_pid_params.yaml, launch/adaptive_pid.launch.py,
package.xml, setup.py, setup.cfg
- test/test_adaptive_pid.py: 68/68 unit tests passing
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-03-02 09:38:46 -05:00
