# Issue #469: Terrain Classification Implementation Plan

## Context

SaltyBot currently has good sensor infrastructure (IMU, cameras, RealSense) and a robust velocity control system with the `VelocityRamp` class. However, it lacks terrain awareness for surface type detection and speed adaptation. This feature will enable:

1. **Surface detection** via IMU vibration analysis and camera texture analysis
2. **Automatic speed adaptation** based on terrain type and roughness
3. **Terrain logging** for mapping and future learning
4. **Improved robot safety** by reducing speed on rough/unstable terrain

## Architecture Overview

The implementation follows the existing ROS2 patterns:

```
IMU/Camera Data
    ↓
[terrain_classifier_node]  ← New node
    ↓
/saltybot/terrain_state (TerrainState.msg)
    ↓
[terrain_speed_adapter_node]  ← New node
    ↓
Adjusted /cmd_vel_terrain → existing cmd_vel_bridge
    ↓
Speed-adapted robot motion

Parallel: [terrain_mapper_node] logs data for mapping
```

## Implementation Components

### 1. **Message Definition: TerrainState.msg**
**File to create:** `jetson/ros2_ws/src/saltybot_social_msgs/msg/TerrainState.msg`

Fields:
- `std_msgs/Header header` — timestamp/frame_id
- `uint8 terrain_type` — enum (0=unknown, 1=pavement, 2=grass, 3=gravel, 4=sand, 5=indoor)
- `float32 roughness` — 0.0=smooth, 1.0=very rough
- `float32 confidence` — 0.0-1.0 classification confidence
- `float32 recommended_speed_ratio` — 0.1-1.0 (fraction of max speed)
- `string source` — "imu_vibration" or "camera_texture" or "fused"

**Update files:**
- `jetson/ros2_ws/src/saltybot_social_msgs/CMakeLists.txt` — add TerrainState.msg to rosidl_generate_interfaces()
- `jetson/ros2_ws/src/saltybot_social_msgs/package.xml` — no changes needed (std_msgs already a dependency)

### 2. **Terrain Classifier Node**
**File to create:** `jetson/ros2_ws/src/saltybot_bringup/saltybot_bringup/terrain_classifier_node.py`

**Purpose:** Analyze IMU and camera data to classify terrain type and estimate roughness.

**Subscribes to:**
- `/camera/imu` (sensor_msgs/Imu) — RealSense IMU at 200 Hz
- `/camera/color/image_raw` (sensor_msgs/Image) — camera RGB at 15 Hz

**Publishes:**
- `/saltybot/terrain_state` (TerrainState.msg) — at 5 Hz

**Key functions:**
- `_analyze_imu_vibration()` — FFT analysis on accel data (window: 200 samples = 1 sec)
  - Compute power spectral density in 0-50 Hz band
  - Extract features: peak frequency, energy distribution, RMS acceleration
  - Roughness = normalized RMS of high-freq components (>10 Hz)

- `_analyze_camera_texture()` — CNN-based texture analysis
  - Uses MobileNetV2 pre-trained on ImageNet as feature extractor
  - Extracts high-level texture/surface features from camera image
  - Lightweight model (~3.5M parameters, ~50-100ms inference on Jetson)
  - Outputs feature vector fed to classification layer

- `_classify_terrain()` — decision logic
  - Simple rule-based classifier (can be upgraded to CNN)
  - Input: [imu_roughness, camera_texture_variance, accel_magnitude]
  - Decision tree or thresholds to classify into 5 types
  - Output: terrain_type, roughness, confidence

**Node Parameters:**
- `imu_window_size` (int, default 200) — samples for FFT window
- `publish_rate_hz` (float, default 5.0)
- `roughness_threshold` (float, default 0.3) — FFT roughness threshold
- `terrain_timeout_s` (float, default 5.0) — how long to keep previous estimate if no new data

### 3. **Speed Adapter Node**
**File to create:** `jetson/ros2_ws/src/saltybot_bringup/saltybot_bringup/terrain_speed_adapter_node.py`

**Purpose:** Adapt cmd_vel speed based on terrain state and integration with velocity ramp.

**Subscribes to:**
- `/cmd_vel` (geometry_msgs/Twist) — raw velocity commands
- `/saltybot/terrain_state` (TerrainState.msg) — terrain classification

**Publishes:**
- `/cmd_vel_terrain` (geometry_msgs/Twist) — terrain-adapted velocity

**Logic:**
- Extract target linear velocity from cmd_vel
- Apply terrain speed ratio: `adapted_speed = target_speed × recommended_speed_ratio`
- Preserve angular velocity (steering not affected by terrain)
- Publish adapted command

**Node Parameters:**
- `enable_terrain_adaptation` (bool, default true)
- `min_speed_ratio` (float, default 0.1) — never go below 10% of requested speed
- `debug_logging` (bool, default false)

**Note:** This is a lightweight adapter. The existing `velocity_ramp_node` handles acceleration/deceleration smoothing independently.

### 4. **Terrain Mapper Node** (Logging/Mapping)
**File to create:** `jetson/ros2_ws/src/saltybot_bringup/saltybot_bringup/terrain_mapper_node.py`

**Purpose:** Log terrain detections with robot pose for future mapping.

**Subscribes to:**
- `/saltybot/terrain_state` (TerrainState.msg)
- `/odom` (nav_msgs/Odometry) — robot pose

**Publishes:**
- `/saltybot/terrain_log` (std_msgs/String) — CSV formatted log messages (optional, mainly for logging)

**Logic:**
- Store terrain observations: (timestamp, pose_x, pose_y, terrain_type, roughness, confidence)
- Log to file: `~/.ros/terrain_map_<timestamp>.csv`
- Resample to 1 Hz to avoid spam

**Node Parameters:**
- `log_dir` (string, default "~/.ros/")
- `resample_rate_hz` (float, default 1.0)

### 5. **Launch Configuration**
**File to update:** `jetson/ros2_ws/src/saltybot_bringup/launch/full_stack.launch.py`

Add terrain nodes:
```python
terrain_classifier_node = Node(
    package='saltybot_bringup',
    executable='terrain_classifier',
    name='terrain_classifier',
    parameters=[{
        'imu_window_size': 200,
        'publish_rate_hz': 5.0,
    }],
    remappings=[
        ('/imu_in', '/camera/imu'),
        ('/camera_in', '/camera/color/image_raw'),
    ],
)

terrain_speed_adapter_node = Node(
    package='saltybot_bringup',
    executable='terrain_speed_adapter',
    name='terrain_speed_adapter',
    parameters=[{
        'enable_terrain_adaptation': True,
        'min_speed_ratio': 0.1,
    }],
    remappings=[
        ('/cmd_vel_in', '/cmd_vel'),
        ('/cmd_vel_out', '/cmd_vel_terrain'),
    ],
)

terrain_mapper_node = Node(
    package='saltybot_bringup',
    executable='terrain_mapper',
    name='terrain_mapper',
)
```

**Update setup.py entry points:**
```python
'terrain_classifier = saltybot_bringup.terrain_classifier_node:main'
'terrain_speed_adapter = saltybot_bringup.terrain_speed_adapter_node:main'
'terrain_mapper = saltybot_bringup.terrain_mapper_node:main'
```

### 6. **Integration with Existing Stack**
- The existing velocity ramp (`velocity_ramp_node.py`) processes `/cmd_vel_smooth` or `/cmd_vel`
- Optionally, update cmd_vel_bridge to use `/cmd_vel_terrain` if available, else fall back to `/cmd_vel`
- Terrain classification runs independently at 5 Hz (much slower than velocity ramping at 50 Hz)

### 7. **Future CNN Enhancement**
The current implementation uses rule-based classification with IMU FFT and camera edge detection. A future enhancement could add a lightweight CNN for texture classification (e.g., MobileNet) by:
1. Creating a `terrain_classifier_cnn.py` with TensorFlow/ONNX model
2. Replacing decision logic in `terrain_classifier_node.py` with CNN inference
3. Maintaining same message interface

## Implementation Tasks

1. ✅ **Phase 1: Message Definition**
   - Create `TerrainState.msg` in saltybot_social_msgs
   - Update `CMakeLists.txt`

2. ✅ **Phase 2: Terrain Classifier Node**
   - Implement `terrain_classifier_node.py` with IMU FFT analysis
   - Implement camera texture analysis
   - Decision logic for classification

3. ✅ **Phase 3: Speed Adapter Node**
   - Implement `terrain_speed_adapter_node.py`
   - Velocity command adaptation

4. ✅ **Phase 4: Terrain Mapper Node**
   - Implement `terrain_mapper_node.py` for logging

5. ✅ **Phase 5: Integration**
   - Update `full_stack.launch.py` with new nodes
   - Update `setup.py` with entry points
   - Test integration

## Testing & Verification

**Unit Tests:**
- Test IMU FFT feature extraction with synthetic vibration data
- Test terrain classification decision logic
- Test speed ratio application
- Test CSV logging format

**Integration Tests:**
- Run full stack with simulated IMU/camera data
- Verify terrain messages published at 5 Hz
- Verify cmd_vel_terrain adapts speeds correctly
- Check terrain log file is created and properly formatted

**Manual Testing:**
- Drive robot on different surfaces
- Verify terrain detection changes appropriately
- Verify speed adaptation is smooth (no jerks from ramping)
- Check terrain log CSV has correct format

## Critical Files Summary

**To Create:**
- `jetson/ros2_ws/src/saltybot_social_msgs/msg/TerrainState.msg`
- `jetson/ros2_ws/src/saltybot_bringup/saltybot_bringup/terrain_classifier_node.py`
- `jetson/ros2_ws/src/saltybot_bringup/saltybot_bringup/terrain_speed_adapter_node.py`
- `jetson/ros2_ws/src/saltybot_bringup/saltybot_bringup/terrain_mapper_node.py`

**To Modify:**
- `jetson/ros2_ws/src/saltybot_social_msgs/CMakeLists.txt` (add TerrainState.msg)
- `jetson/ros2_ws/src/saltybot_bringup/launch/full_stack.launch.py` (add nodes)
- `jetson/ros2_ws/src/saltybot_bringup/setup.py` (add entry points)

**Key Dependencies:**
- `numpy` — FFT analysis (already available in saltybot)
- `scipy.signal` — Butterworth filter (optional, for smoothing)
- `cv2` (OpenCV) — image processing (already available)
- `tensorflow` or `tf-lite` — MobileNetV2 pre-trained model for texture CNN
- `rclpy` — ROS2 Python client

**CNN Model Details:**
- Model: MobileNetV2 pre-trained on ImageNet
- Input: 224×224 RGB image (downsampled from camera)
- Output: 1280-dim feature vector from last conv layer before classification
- Strategy: Use pre-trained features directly (transfer learning) for quick MVP, no fine-tuning needed initially
- Alternative: Pre-trained weights can be fine-tuned on terrain image dataset in future iterations
- Inference: ~50-100ms on Jetson Xavier (acceptable at 5 Hz publish rate)

## Terrain Classification Logic (IMU + CNN Fusion)

**Features extracted:**
1. `imu_roughness` = normalized RMS of high-freq (>10 Hz) accel components (0-1)
   - Computed from FFT power spectral density in 10-50 Hz band
   - Reflects mechanical vibration from surface contact

2. `cnn_texture_features` = 1280-dim feature vector from MobileNetV2
   - Pre-trained features capture texture, edge, and surface characteristics
   - Reduced to 2-3 principal components via PCA or simple aggregation

3. `accel_magnitude` = RMS of total acceleration (m/s²)
   - Helps distinguish stationary (9.81 m/s²) vs. moving

**Classification approach (Version 1):**
- Simple decision tree with IMU-dominant logic + CNN support:
  ```
  if imu_roughness < 0.2 and accel_magnitude < 9.8:
      terrain = PAVEMENT  (confidence boosted if CNN agrees)
  elif imu_roughness < 0.35 and cnn_grainy_score < 0.4:
      terrain = GRASS
  elif imu_roughness > 0.45 and cnn_granular_score > 0.5:
      terrain = GRAVEL
  elif cnn_sand_texture_score > 0.6 and imu_roughness > 0.3:
      terrain = SAND
  else:
      terrain = INDOOR
  ```

- Confidence: weighted combination of IMU and CNN agreement
- Roughness metric: `0.0 = smooth, 1.0 = very rough` derived from IMU FFT energy ratio

**Speed recommendations:**
- Pavement: 1.0 (full speed)
- Grass: 0.8 (20% slower)
- Gravel: 0.5 (50% slower)
- Sand: 0.4 (60% slower)
- Indoor: 0.7 (30% slower by default)

**Future improvement:** Replace decision tree with trained classifier (Random Forest, SVM, or small Dense net) on labeled terrain dataset once collected.

---

This plan follows SaltyBot's established patterns:
- Pure Python libraries for core logic (_terrain_analysis.py)
- ROS2 node wrappers for integration
- Parameter-based configuration in YAML
- Message-based pub/sub architecture
- Integration with existing velocity control pipeline