# VLA 视觉避障 3D 导航（MuJoCo + ROS1）

基于 MuJoCo 与 ROS1 的 **全向小车视觉避障导航** 仿真项目，支持仿真深度图输入，并可直接对接 RealSense / Orbbec / ZED 等深感相机落地。

---

## 特性

- **全向移动**：vx, vy, wz 车体系速度 → 世界系执行器
- **仿真深度图**：mj_ray 网格模拟深感相机，便于替换为真实 depth/pointcloud
- **DDA 避障**：Directional Distance Avoidance，基于方向距离的避障
- **目标点导航**：按键设置目标，大角度先转后走，小角度边走边调
- **转向策略**：大角度（>35°）优先转向，小角度时前进并微调朝向

---

## 安装

```bash
pip install -r requirements.txt
```

依赖：`mujoco>=3.0.0`、`numpy`

---

## 快速开始

```bash
python scripts/run_simulation.py
```

### 操作说明

| 按键 | 功能 |
|------|------|
| **G** | 目标 = 车头前方 2m |
| **C** | 目标 = 相机视线与地面交点 |
| **ESC** | 退出 |

小车会按设定目标行驶，到达后停止，可继续按键设置新目标。目标点有边界限制（距原点 ≤ 5m）。

---

## 目录结构

```
├── README.md
├── OUTLINE.md              # 项目大纲与里程碑
├── requirements.txt
├── mujoco_scenes/          # MuJoCo 场景
│   └── cylinder_obstacles.xml
├── scripts/
│   └── run_simulation.py   # 仿真主程序
└── docs/
    └── DDA_KNOWLEDGE.md    # DDA 知识点与 ROS1 落地说明
```

---

## 控制流程

```
仿真深度图 (mj_ray) / 真实 depth
    → depth_to_sectors()  →  dists, angles
    → dda_avoidance_from_depth()  →  vx_a, vy_a, wz_a
    → goal_attraction_vel()  →  vx_g, vy_g, wz_g
    → blend_and_clamp()  →  融合 + 前方扇区限速
    → 滤波 → 车体系→世界系 → 执行器
```

---

## 主要参数（`NavCfg`）

| 参数 | 默认 | 说明 |
|------|------|------|
| `hfov_deg` | 86 | 水平视场角（如 RealSense D435）|
| `num_sectors` | 61 | 扇区数 |
| `obstacle_threshold` | 0.45 | 避障停止距离 (m) |
| `safe_distance` | 0.65 | 安全距离 (m) |
| `yaw_turn_first_deg` | 35 | 大角度先转阈值 (°) |
| `goal_ahead_dist` | 2.0 | G 键目标前方距离 (m) |
| `goal_max_r` | 5.0 | 目标点最大半径 (m) |

---

## ROS1 + 深感相机落地

`depth_to_sectors()` 的输入为 H×W 深度图（米），与真实相机接口一致。在 ROS1 中订阅深度图：

```python
# 伪代码
rospy.Subscriber('/camera/depth/image_rect', Image, depth_cb)
# depth_cb 中：depth_to_sectors(depth, hfov_rad, num_sectors, ...) → dists, angles
```

详见 `docs/DDA_KNOWLEDGE.md`。

---

## 引用

- [MuJoCo](https://mujoco.readthedocs.io/)：物理仿真
- [OUTLINE.md](OUTLINE.md)：项目规划与 VLA 目标