# DDA 知识点：Directional Distance Avoidance

## 一、从「离散射线」到「深度图」的转变

| 原方案 | 新方案 |
|--------|--------|
| mj_ray 离散射线 | 深度图 / 点云（带距离的视场） |
| N 条射线直接给距离 | 从深度图按方位做 1D 距离扇区 |
| 每条射线独立 | 每个扇区取**最近障碍距离**（鲁棒统计） |

**核心**：深感相机给你的是 **Depth / PointCloud**，不需要 mj_ray 这种离散射线；先把深度转成扇区最近距离，再做避障。

---

## 二、DDA 核心思想

1. **输入**：`dist[sector]` —— 每个方向上的最近障碍距离（米）
2. **判断**：当前期望运动方向 `move_theta = atan2(vy_cmd, vx_cmd)` 是否过近
3. **动作**：
   - 若过近：**禁止/衰减**朝该方向的速度分量
   - 同时生成**侧向/偏航**的「脱离」控制，朝开阔方向

---

## 三、深度 → 扇区最近距离（depth_to_sectors）

- 视场水平角 `HFOV`（如 RealSense D435 ≈ 86°）
- 扇区数 `N`（如 31、61）
- 对每个扇区：在深度图对应列范围取**鲁棒最小值**：
  - `percentile(depth, 5%)` 或
  - 剔除 0/无效 + 中值滤波

得到：
- `angles[i]`：扇区角（车体坐标系，左正右负）
- `d[i]`：该方向最近障碍距离（米）

---

## 四、DDA 避障控制律

1. 计算期望运动方向角：`move_theta = atan2(vy_cmd, vx_cmd)`
2. 在 `move_theta` 附近取窗口内最小距离 `dmin_dir`
3. 阈值判断：
   - `dmin_dir >= safe_dist`：正常走
   - `dmin_dir < stop_dist`：禁止向前 + 强制侧移/转向
   - 中间区间：按比例衰减 + 轻微偏航
4. 选择「最开阔方向」`best_theta`，作为绕行/偏航目标

---

## 五、ROS1 + 深感相机 落地要点

### 5.1 深度图输入

- **Depth Image**：`sensor_msgs/Image`，订阅 `/camera/depth/image_rect` 等
- **PointCloud**：`sensor_msgs/PointCloud2`，转为深度图或直接投影到扇区

### 5.2 常用深感相机参数

| 相机 | HFOV | 分辨率 |
|------|------|--------|
| RealSense D435 | ~87° | 640×480 |
| Orbbec Astra | ~60° | 640×480 |
| ZED | 可调 | 多种 |
| OAK-D | ~73° | 640×400 |

### 5.3 推荐两行增强（仿真中已启用）

1. **速度自适应安全距离**：跑得越快看得越远
   ```py
   safe_dist = base_safe + k * speed
   stop_dist = base_stop + 0.5 * k * speed
   ```

2. **前方扇区限速**：只用前方 ±20° 的最小距离限速
   ```py
   front = (np.abs(angles) < np.deg2rad(20))
   d_front = np.min(dists[front])
   speed_limit = np.clip((d_front - stop_dist) / (safe_dist - stop_dist), 0, 1) * MAX_LINEAR
   ```

---

## 六、ROS1 深度输入对接（待实现）

### 6.1 Depth Image 订阅

```python
# 伪代码：ROS1 订阅
import rospy
from sensor_msgs.msg import Image
from cv_bridge import CvBridge

def depth_cb(msg):
    depth = bridge.imgmsg_to_cv2(msg, desired_encoding='passthrough')
    depth = depth.astype(np.float32) / 1000.0  # mm -> m
    dists, angles = depth_to_sectors(depth, hfov_rad, num_sectors, ...)
    # 存入共享变量，供控制回调使用

rospy.Subscriber('/camera/depth/image_rect', Image, depth_cb)
```

### 6.2 PointCloud 订阅

从 `sensor_msgs/PointCloud2` 投影到极坐标扇区，取每扇区最近距离。

---

## 七、控制流程对接

**仿真**：`mj_ray` 网格 → 仿真深度图 `(1, num_sectors)` → `depth_to_sectors()`  
**实机**：深感相机 depth/pointcloud → `depth_to_sectors()`  

```
depth (H×W) 或 pointcloud
    → depth_to_sectors() → dists, angles
    → dda_avoidance_from_depth(dists, angles, vx_g, vy_g)
    → vx_a, vy_a, wz_a
    → blend_and_clamp(vx_a, vy_a, wz_a, vx_g, vy_g, wz_g)
```

融合后继续做：滤波 → 车体系→世界系 → 执行器。