diff --git a/docs/uav_flight_base/README.md b/docs/uav_flight_base/README.md
new file mode 100644
index 0000000000..50dac1527b
--- /dev/null
+++ b/docs/uav_flight_base/README.md
@@ -0,0 +1,150 @@
+# uav_flight_base
+
+> 无人机基础飞行模块:键盘遥控 + 最小运动学仿真 + 状态监控
+> 作者:唐健斌 | 环境:Ubuntu 20.04 + ROS Noetic
+
+## 模块定位
+
+课程作业要求里,"人体/车辆/无人机/水下机器人"任选其一。本模块选**无人机**,
+并先解决最基础的一件事:**让一架无人机在仿真里被控制起来,并且状态可观测**。
+
+不接 Gazebo、不接 AirSim 也能跑——因为用的是自己写的一阶惯性运动学模型,
+这样在没有图形界面的服务器上也能验证"遥控 → 控制 → 状态"这条链路。
+
+## 数据流
+
+```
+键盘 (uav_teleop_key)
+ │ /uav/cmd_vel (geometry_msgs/Twist)
+ ▼
+运动学仿真 (uav_mock_sim)
+ │ /uav/odom (nav_msgs/Odometry)
+ │ TF: map -> base_link
+ ▼
+状态监控 (uav_state_monitor)
+ │ 终端打印 / CSV
+ ▼
+RViz / rqt_plot(可选)
+```
+
+## 话题
+
+| 话题 | 类型 | 方向 | 说明 |
+|---|---|---|---|
+| `/uav/cmd_vel` | `geometry_msgs/Twist` | 订阅 | 期望速度(机体系) |
+| `/uav/odom` | `nav_msgs/Odometry` | 发布 | 位置、姿态、速度 |
+| `/tf` | `tf2_msgs/TFMessage` | 发布 | `map` → `base_link` |
+
+## 运动学模型
+
+一阶惯性环节,避免"按键即瞬移"的不真实感:
+
+```
+dv/dt = (v_cmd - v) / τ
+```
+
+- `τ`(`~time_constant`,默认 0.35s)越大,响应越"肉"
+- 速度分别裁剪到 `±max_speed`
+- 高度限制在 `[z_min, z_max]`
+
+## 启动
+
+```bash
+rosrun uav_flight_base main.py --mode mock
+```
+
+或
+
+```bash
+roslaunch uav_flight_base main.launch mode:=mock teleop:=false
+```
+
+## 验证清单
+
+- [x] `rostopic hz /uav/odom` 稳定在 50Hz
+- [x] 键盘 `w` 后 `/uav/odom` 的 `x` 单调增大
+- [x] 松开按键后速度在约 1s 内衰减到 0(一阶惯性)
+- [x] `csv:=/tmp/flight.csv` 能导出轨迹
+- [ ] 接 Gazebo 模型(下一次提交)
+
+## 设计取舍
+
+1. **先不接 Gazebo**:Gazebo 的无人机模型(rotors / PX4)依赖重、装一次半小时且容易崩。
+ 先用轻量运动学模型把控制链路和文档跑通,再把后端换掉,代码接口不变。
+2. **`main.py` 只做调度**:真正的节点逻辑都在 `scripts/` 下,每个脚本可单独 `rosrun` 调试。
+3. **参数全部走 rosparam**:不写死在代码里,方便后面接不同机型。
+
+---
+
+## 任务 2(神经网络版):扫描 → 速度的端到端策略
+
+### 数据流
+
+```
+专家策略 uav_scan_avoider ──/cmd_vel──┐
+ ├─→ uav_scan_dataset.py ─→ scan_dataset.csv
+激光雷达 /uav/scan ───────────────────┘ (90 维扫描 + 动作标签)
+ │
+ ▼
+ uav_train_scan_policy.py ─→ scan_policy.npz
+ │
+ ▼
+ /uav/scan ─→ uav_scan_policy.py ─→ /cmd_vel
+```
+
+### 网络结构
+
+```
+输入 90 维激光扫描(-π..π 降采样,距离截断 3.5m,标准化)
+隐层 96 单元 ReLU
+输出 2 维(linear.x, angular.z),标准化后反变换
+损失 MSE + L2(1e-5),SGD + 动量 0.9,lr=2e-3,400 epoch
+```
+
+样本 **1757** 条,按时间顺序切分(前 85% 训练、后 15% 验证,避免相邻帧泄漏),
+最佳验证损失 **0.0728**。
+
+### 多场景评估结果
+
+| 指标 | 场景A 开阔场地 | 场景B 密集障碍 |
+|---|---|---|
+| 运行时长 | 116 s | 137 s |
+| 平均速度 | 0.61 m/s | **0.03 m/s** |
+| 平均角速度 | 0.21 rad/s | 0.12 rad/s |
+| 最近障碍距离 | 0.66 m | 0.59 m |
+| 贴障时间占比 (<0.6m) | 0.0 % | **90.5 %** |
+
+**结论**:训练数据只来自开阔场地,策略在**同类场景表现良好**(全程未贴近障碍),
+但在**未见过的密集场景中趋于保守停滞**(平均速度 0.03 m/s)——典型的行为克隆分布漂移。
+后续可通过在密集场景补采数据、或加入 DAgger 式在线纠正来改善。
+
+
+
+
+
+### 网络结构与优化器对比
+
+课程要求“同一模型比较不同算法、同一算法比较不同网络结构”。在固定数据(1757 组)、
+固定轮数(300)、固定随机种子的条件下,只改变隐层宽度与优化器,共 10 组实验。
+
+| 隐层宽度 | 参数量 | 验证损失(SGD+动量) | 验证损失(Adam) |
+|---|---|---|---|
+| 32 | 2 978 | 0.06767 | **0.06549** |
+| 64 | 5 954 | 0.06723 | 0.06803 |
+| 96 | 8 930 | 0.07226 | 0.06734 |
+| 128 | 11 906 | 0.08125 | 0.07058 |
+| 256 | 23 810 | 0.07399 | 0.06931 |
+
+复现方式:
+
+```bash
+rosrun uav_flight_base uav_policy_sweep.py # 输出 sweep_results.csv
+```
+
+**结论**:参数量从 2 978 增至 23 810(约 8 倍),验证损失并未下降;最优为宽度 32 配 Adam。
+说明在本数据集规模下网络容量已不是瓶颈,此后再加宽只会增加优化难度。
+这一结果与场景 B 的停滞现象互为印证——瓶颈在数据分布覆盖,而非模型容量。
+
+
+
+
diff --git a/docs/uav_flight_base/data/sweep_results.csv b/docs/uav_flight_base/data/sweep_results.csv
new file mode 100644
index 0000000000..adbd81e0a4
--- /dev/null
+++ b/docs/uav_flight_base/data/sweep_results.csv
@@ -0,0 +1,11 @@
+hidden,optimizer,params,val_loss,seconds
+32,sgd,2978,0.06767,1.2
+32,adam,2978,0.06549,2.0
+64,sgd,5954,0.06723,1.5
+64,adam,5954,0.06803,2.3
+96,sgd,8930,0.07226,1.7
+96,adam,8930,0.06734,2.7
+128,sgd,11906,0.08125,2.0
+128,adam,11906,0.07058,3.0
+256,sgd,23810,0.07399,5.0
+256,adam,23810,0.06931,6.5
diff --git a/docs/uav_flight_base/images/avoid_demo.gif b/docs/uav_flight_base/images/avoid_demo.gif
new file mode 100644
index 0000000000..219f5b3696
Binary files /dev/null and b/docs/uav_flight_base/images/avoid_demo.gif differ
diff --git a/docs/uav_flight_base/images/camera_frame.png b/docs/uav_flight_base/images/camera_frame.png
new file mode 100644
index 0000000000..27f8f57958
Binary files /dev/null and b/docs/uav_flight_base/images/camera_frame.png differ
diff --git a/docs/uav_flight_base/images/nav_result.png b/docs/uav_flight_base/images/nav_result.png
new file mode 100644
index 0000000000..b40987b94b
Binary files /dev/null and b/docs/uav_flight_base/images/nav_result.png differ
diff --git a/docs/uav_flight_base/images/policy_compare.png b/docs/uav_flight_base/images/policy_compare.png
new file mode 100644
index 0000000000..9e726a8d03
Binary files /dev/null and b/docs/uav_flight_base/images/policy_compare.png differ
diff --git a/docs/uav_flight_base/images/policy_sceneA.png b/docs/uav_flight_base/images/policy_sceneA.png
new file mode 100644
index 0000000000..a85257850c
Binary files /dev/null and b/docs/uav_flight_base/images/policy_sceneA.png differ
diff --git a/docs/uav_flight_base/images/policy_sceneB.png b/docs/uav_flight_base/images/policy_sceneB.png
new file mode 100644
index 0000000000..c9868fec20
Binary files /dev/null and b/docs/uav_flight_base/images/policy_sceneB.png differ
diff --git a/docs/uav_flight_base/images/policy_width_sweep.png b/docs/uav_flight_base/images/policy_width_sweep.png
new file mode 100644
index 0000000000..7840e88e77
Binary files /dev/null and b/docs/uav_flight_base/images/policy_width_sweep.png differ
diff --git a/docs/uav_flight_base/images/servo_demo.gif b/docs/uav_flight_base/images/servo_demo.gif
new file mode 100644
index 0000000000..47f4c8f922
Binary files /dev/null and b/docs/uav_flight_base/images/servo_demo.gif differ
diff --git a/src/uav_flight_base/CMakeLists.txt b/src/uav_flight_base/CMakeLists.txt
new file mode 100644
index 0000000000..e0aff52cde
--- /dev/null
+++ b/src/uav_flight_base/CMakeLists.txt
@@ -0,0 +1,24 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(uav_flight_base)
+
+find_package(catkin REQUIRED COMPONENTS
+ rospy
+ geometry_msgs
+ nav_msgs
+ tf2_ros
+)
+
+catkin_package(
+ CATKIN_DEPENDS rospy geometry_msgs nav_msgs tf2_ros
+)
+
+catkin_install_python(PROGRAMS
+ scripts/uav_teleop_key.py
+ scripts/uav_mock_sim.py
+ scripts/uav_state_monitor.py
+ DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+install(DIRECTORY launch
+ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
diff --git a/src/uav_flight_base/README.md b/src/uav_flight_base/README.md
new file mode 100644
index 0000000000..f2c23eaf8f
--- /dev/null
+++ b/src/uav_flight_base/README.md
@@ -0,0 +1,143 @@
+# uav_flight_base
+
+无人机仿真模块(ROS1 Noetic + Gazebo 11),对应课程要求的四个任务。
+
+## 统一入口
+
+所有任务都由 `main.py` 分发,一条命令即可运行:
+
+```bash
+python3 main.py --list # 查看全部任务
+python3 main.py --task 1 # 键盘遥控 + 运动学仿真
+python3 main.py --task 2 --mode lidar # 激光雷达感知 + 反应式避障
+python3 main.py --task 2 --mode vision # 相机颜色检测 + 视觉伺服
+python3 main.py --task 3 # 栅格建图 + A* 导航
+python3 main.py --task 4 --stage collect # 端到端网络:采集
+python3 main.py --task 4 --stage train # 端到端网络:训练
+python3 main.py --task 4 --stage drive # 端到端网络:飞行
+```
+
+| 任务 | 感知 | 控制 | 对应 launch |
+|---|---|---|---|
+| 1 键盘遥控 | — | 键盘 → Twist | `main.launch` |
+| 2 传感器感知 | 360 线激光 | 反应式避障 | `avoid.launch` |
+| 2 传感器感知 | 相机(HSV) | PID 视觉伺服 | `servo.launch` |
+| 3 建图导航 | 激光 → 占据栅格 | A* + 纯追踪 | `nav.launch` |
+| 4 端到端 | 相机图像 | 神经网络 | `main.py --task 4` |
+
+> 无图形环境(纯 SSH)时,用
+> `xvfb-run -a -s "-screen 0 1024x768x24" python3 main.py --task 2 --mode vision`
+> 给 Gazebo 相机提供虚拟显示器。
+
+## 模块说明(第一部分:基础链路)
+
+提供**键盘遥控**、**最小运动学仿真**、**状态监控**三部分,
+不依赖 Gazebo / AirSim 即可完整跑通,用于验证"遥控 → 控制 → 状态反馈"这条链路。
+
+> 这是 10 次提交中的第 1 次:只做骨架与可运行的最小闭环。后续按任务 2/3/4 逐步加感知、建图、端到端网络。
+
+## 目录结构
+
+```
+uav_flight_base/
+├── main.py # 入口(仓库约定:入口以 main. 开头)
+├── CMakeLists.txt
+├── package.xml
+├── launch/
+│ └── main.launch # ROS 启动配置(仓库约定:main.launch)
+└── scripts/
+ ├── uav_teleop_key.py # 键盘 → /uav/cmd_vel
+ ├── uav_mock_sim.py # 运动学仿真 → /uav/odom + TF
+ └── uav_state_monitor.py # 状态打印 / CSV 记录
+```
+
+## 环境
+
+| 项目 | 版本 |
+|---|---|
+| 系统 | Ubuntu 20.04 |
+| ROS | Noetic |
+| Python | 3.8 |
+| 依赖 | rospy / geometry_msgs / nav_msgs / tf2_ros(ROS 自带) |
+
+## 安装
+
+```bash
+# 1. 放进 catkin 工作空间
+cp -r uav_flight_base ~/catkin_ws/src/
+
+# 2. 编译
+cd ~/catkin_ws
+catkin_make
+source devel/setup.bash
+```
+
+## 运行
+
+**方式一:看效果(需要图形界面)**
+
+```bash
+rosrun uav_flight_base main.py --mode mock
+```
+
+会启动仿真节点 + 状态监控 + 键盘遥控窗口(xterm)。
+
+**方式二:只跑链路(不需要键盘)**
+
+```bash
+roslaunch uav_flight_base main.launch mode:=mock teleop:=false
+```
+
+另开一个终端手动发速度指令:
+
+```bash
+rostopic pub -r 10 /uav/cmd_vel geometry_msgs/Twist \
+ '{linear: {x: 1.0, y: 0.0, z: 0.5}, angular: {z: 0.3}}'
+```
+
+**方式三:记录飞行数据**
+
+```bash
+roslaunch uav_flight_base main.launch mode:=mock teleop:=false csv:=/tmp/flight.csv
+```
+
+## 键盘操作
+
+| 按键 | 动作 |
+|---|---|
+| `w` / `s` | 前进 / 后退 |
+| `a` / `d` | 左移 / 右移 |
+| `r` / `f` | 上升 / 下降 |
+| `q` / `e` | 左转 / 右转 |
+| `空格` | 悬停(速度清零) |
+| `Ctrl+C` | 退出 |
+
+## 预期输出
+
+```
+[uav_flight_base] mode = mock
+[INFO] [uav_state_monitor]: [ 0.5s] pos=( 0.42, 0.00, 1.50) yaw= 0.0° v=0.61 m/s
+[INFO] [uav_state_monitor]: [ 1.0s] pos=( 0.88, 0.12, 1.62) yaw= 6.9° v=0.95 m/s
+```
+
+## 参数
+
+| 参数 | 默认 | 说明 |
+|---|---|---|
+| `~rate` | 50.0 | 仿真频率 Hz |
+| `~time_constant` | 0.35 | 一阶惯性时间常数,越大越"肉" |
+| `~max_speed` | 5.0 | 最大速度 m/s |
+| `~z_min` / `~z_max` | 0.2 / 30.0 | 高度限制 m |
+| `~cmd_topic` | `/uav/cmd_vel` | 速度指令话题 |
+| `~odom_topic` | `/uav/odom` | 里程计话题 |
+
+## 后续计划
+
+- [ ] 接 Gazebo 无人机模型(`mode:=gazebo`)
+- [ ] 深度图避障
+- [ ] 栅格建图 + A* 导航
+- [ ] 端到端 CNN(图像 → 控制)
+
+## 许可
+
+MIT
diff --git a/src/uav_flight_base/launch/avoid.launch b/src/uav_flight_base/launch/avoid.launch
new file mode 100644
index 0000000000..c514c2fbca
--- /dev/null
+++ b/src/uav_flight_base/launch/avoid.launch
@@ -0,0 +1,18 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/uav_flight_base/launch/gazebo.launch b/src/uav_flight_base/launch/gazebo.launch
new file mode 100644
index 0000000000..4128e29fcb
--- /dev/null
+++ b/src/uav_flight_base/launch/gazebo.launch
@@ -0,0 +1,29 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/uav_flight_base/launch/main.launch b/src/uav_flight_base/launch/main.launch
new file mode 100644
index 0000000000..494eb89c7b
--- /dev/null
+++ b/src/uav_flight_base/launch/main.launch
@@ -0,0 +1,48 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/uav_flight_base/launch/nav.launch b/src/uav_flight_base/launch/nav.launch
new file mode 100644
index 0000000000..44702a964b
--- /dev/null
+++ b/src/uav_flight_base/launch/nav.launch
@@ -0,0 +1,20 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/uav_flight_base/launch/servo.launch b/src/uav_flight_base/launch/servo.launch
new file mode 100644
index 0000000000..d2bf13e22f
--- /dev/null
+++ b/src/uav_flight_base/launch/servo.launch
@@ -0,0 +1,29 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/uav_flight_base/main.py b/src/uav_flight_base/main.py
new file mode 100644
index 0000000000..92f591c869
--- /dev/null
+++ b/src/uav_flight_base/main.py
@@ -0,0 +1,144 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""uav_flight_base 模块统一入口。
+
+课程要求:入口文件名为 main.*,所有功能支持 launch 启动。
+本文件把 4 个任务收敛到一个命令:
+
+ python3 main.py --list 查看全部任务
+ python3 main.py --task 1 键盘遥控(运动学仿真)
+ python3 main.py --task 2 --mode lidar 激光雷达感知 + 反应式避障
+ python3 main.py --task 2 --mode vision 相机颜色检测 + 视觉伺服
+ python3 main.py --task 3 栅格建图 + A* 导航
+ python3 main.py --task 4 --stage collect|train|drive 端到端网络
+
+所有任务都在 Ubuntu 20.04 + ROS Noetic + Gazebo 11 上验证通过。
+"""
+
+import argparse
+import os
+import subprocess
+import sys
+
+PKG = "uav_flight_base"
+
+TASKS = {
+ 1: {
+ "name": "键盘遥控 + 运动学仿真",
+ "desc": "键盘输入 → /uav/cmd_vel → 运动学模型 → /uav/odom + TF",
+ "launch": "main.launch",
+ "args": {"mode": "mock"},
+ },
+ 2: {
+ "name": "传感器感知 + 运动控制",
+ "desc": "lidar: 360 线激光 → 扇区最近距离 → 反应式避障;"
+ "vision: HSV 颜色检测 → PID 视觉伺服",
+ "launch": None, # 按 mode 选择
+ "args": {},
+ },
+ 3: {
+ "name": "建图 + 导航",
+ "desc": "激光 ray casting 占据栅格 → 8 邻域 A* → 纯追踪飞行",
+ "launch": "nav.launch",
+ "args": {},
+ },
+ 4: {
+ "name": "端到端模型(图像 → 控制)",
+ "desc": "专家采数据 → 训练网络 → 网络实时飞行",
+ "launch": None,
+ "args": {},
+ },
+}
+
+LAUNCH_2 = {"lidar": "avoid.launch", "vision": "servo.launch"}
+
+
+def show_list():
+ print("=" * 66)
+ print(" uav_flight_base —— 无人机仿真模块(任务 1~4)")
+ print("=" * 66)
+ for k in sorted(TASKS):
+ t = TASKS[k]
+ print(" [任务 %d] %s" % (k, t["name"]))
+ print(" %s" % t["desc"])
+ if t["launch"]:
+ print(" 启动: roslaunch %s %s" % (PKG, t["launch"]))
+ elif k == 2:
+ print(" 启动: roslaunch %s avoid.launch | servo.launch" % PKG)
+ else:
+ print(" 启动: stage=collect / train / drive(见 README)")
+ print("")
+ print(" 用法示例: python3 main.py --task 3")
+ print("=" * 66)
+
+
+def run_launch(launch, extra, dry):
+ cmd = ["roslaunch", PKG, launch] + extra
+ print("[main] 执行: " + " ".join(cmd))
+ if dry:
+ print("[main] --dry-run:只打印命令,不启动")
+ return 0
+ try:
+ return subprocess.call(cmd)
+ except FileNotFoundError:
+ sys.stderr.write("[main] 找不到 roslaunch,请先 source /opt/ros/noetic/setup.bash\n")
+ return 127
+
+
+def run_task4(stage, dry):
+ home = os.path.expanduser("~")
+ data = os.path.join(home, "bc_data")
+ scripts = os.path.join(home, "catkin_ws/src", PKG, "scripts")
+
+ if stage == "collect":
+ cmd = ["rosrun", PKG, "uav_expert_collect.py"]
+ elif stage == "train":
+ cmd = [sys.executable, os.path.join(scripts, "uav_train_np.py"), data, "300"]
+ elif stage == "drive":
+ cmd = ["rosrun", PKG, "uav_np_driver.py", os.path.join(data, "weights.npz")]
+ else:
+ sys.stderr.write("[main] --stage 只能是 collect / train / drive\n")
+ return 2
+
+ print("[main] 执行: " + " ".join(cmd))
+ if dry:
+ print("[main] --dry-run:只打印命令,不启动")
+ return 0
+ return subprocess.call(cmd)
+
+
+def main():
+ ap = argparse.ArgumentParser(description="无人机仿真模块统一入口")
+ ap.add_argument("--task", type=int, choices=[1, 2, 3, 4], help="要运行的任务编号")
+ ap.add_argument("--mode", default="lidar", choices=["lidar", "vision"],
+ help="任务2 的感知方式")
+ ap.add_argument("--stage", default="collect", choices=["collect", "train", "drive"],
+ help="任务4 的阶段")
+ ap.add_argument("--gui", action="store_true", help="显示 Gazebo 界面")
+ ap.add_argument("--dry-run", action="store_true", help="只打印命令不执行")
+ ap.add_argument("--list", action="store_true", help="列出全部任务")
+ args, extra = ap.parse_known_args()
+
+ if args.list or args.task is None:
+ show_list()
+ return 0
+
+ gui = "true" if args.gui else "false"
+
+ if args.task == 1:
+ # main.launch 没有 gui 参数,只有 teleop / csv
+ return run_launch(TASKS[1]["launch"],
+ ["mode:=mock"] + extra, args.dry_run)
+
+ if args.task == 2:
+ launch = LAUNCH_2[args.mode]
+ return run_launch(launch, ["gui:=%s" % gui] + extra, args.dry_run)
+
+ if args.task == 3:
+ return run_launch(TASKS[3]["launch"], ["gui:=%s" % gui] + extra, args.dry_run)
+
+ return run_task4(args.stage, args.dry_run)
+
+
+if __name__ == "__main__":
+ sys.exit(main())
diff --git a/src/uav_flight_base/package.xml b/src/uav_flight_base/package.xml
new file mode 100644
index 0000000000..a735d704de
--- /dev/null
+++ b/src/uav_flight_base/package.xml
@@ -0,0 +1,18 @@
+
+
+ uav_flight_base
+ 0.1.0
+ 无人机基础飞行模块:键盘遥控、最小运动学仿真、状态监控(ROS1 Noetic)
+
+ 唐健斌
+ MIT
+
+ catkin
+
+ rospy
+ geometry_msgs
+ nav_msgs
+ tf2_ros
+
+
+
diff --git a/src/uav_flight_base/scripts/uav_camera_capture.py b/src/uav_flight_base/scripts/uav_camera_capture.py
new file mode 100644
index 0000000000..ab4c8d7cdf
--- /dev/null
+++ b/src/uav_flight_base/scripts/uav_camera_capture.py
@@ -0,0 +1,83 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""抓取一帧 Gazebo 相机图像并存盘(用于无图形界面环境下的验证)。
+
+用法:
+ rosrun uav_flight_base uav_camera_capture.py --topic /uav_cam/image_raw --out /tmp/frame.ppm --timeout 20
+"""
+
+import argparse
+import sys
+
+import numpy as np
+import rospy
+from sensor_msgs.msg import Image
+
+
+def save_ppm(path, arr):
+ h, w, _ = arr.shape
+ with open(path, "wb") as f:
+ f.write(b"P6\n%d %d\n255\n" % (w, h))
+ f.write(arr.tobytes())
+
+
+def save_png(path, arr):
+ try:
+ from PIL import Image as PILImage
+ except ImportError:
+ return False
+ PILImage.fromarray(arr).save(path)
+ return True
+
+
+class Grabber(object):
+ def __init__(self, topic, out, timeout):
+ self.out = out
+ self.got = False
+ self.sub = rospy.Subscriber(topic, Image, self.cb, queue_size=1)
+ self.deadline = rospy.Time.now() + rospy.Duration(timeout)
+
+ def cb(self, msg):
+ if self.got:
+ return
+ w, h, step = msg.width, msg.height, msg.step
+ enc = msg.encoding.lower()
+ raw = np.frombuffer(msg.data, dtype=np.uint8)
+ if enc in ("rgb8", "bgr8"):
+ img = raw.reshape(h, step)[:, :w * 3].reshape(h, w, 3)
+ if enc == "bgr8":
+ img = img[:, :, ::-1]
+ elif enc in ("mono8", "8uc1"):
+ img = np.repeat(raw.reshape(h, step)[:, :w][:, :, None], 3, axis=2)
+ else:
+ rospy.logwarn("暂不支持的编码: %s", msg.encoding)
+ return
+ img = np.ascontiguousarray(img)
+ base = self.out.rsplit(".", 1)[0]
+ save_ppm(base + ".ppm", img)
+ ok = save_png(base + ".png", img)
+ rospy.loginfo("已保存 %s%s (%dx%d, %s)", base + ".ppm",
+ " 和 .png" if ok else "", w, h, msg.encoding)
+ self.got = True
+
+
+def main():
+ ap = argparse.ArgumentParser()
+ ap.add_argument("--topic", default="/uav_cam/image_raw")
+ ap.add_argument("--out", default="/tmp/uav_frame.ppm")
+ ap.add_argument("--timeout", type=float, default=20.0)
+ args, _ = ap.parse_known_args(sys.argv)
+
+ rospy.init_node("uav_camera_capture", anonymous=True)
+ g = Grabber(args.topic, args.out, args.timeout)
+ r = rospy.Rate(5)
+ while not rospy.is_shutdown() and not g.got:
+ if rospy.Time.now() > g.deadline:
+ rospy.logerr("超时未收到图像: %s", args.topic)
+ return 1
+ r.sleep()
+ return 0
+
+
+if __name__ == "__main__":
+ sys.exit(main())
diff --git a/src/uav_flight_base/scripts/uav_camera_record.py b/src/uav_flight_base/scripts/uav_camera_record.py
new file mode 100644
index 0000000000..6353e1a1e1
--- /dev/null
+++ b/src/uav_flight_base/scripts/uav_camera_record.py
@@ -0,0 +1,82 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""按固定间隔录制若干帧相机图像,用于生成演示 GIF。
+
+用法:
+ rosrun uav_flight_base uav_camera_record.py --outdir /tmp/seq --count 24 --interval 0.25
+"""
+
+import argparse
+import os
+import sys
+
+import numpy as np
+import rospy
+from sensor_msgs.msg import Image
+
+
+def to_rgb(msg):
+ w, h, step = msg.width, msg.height, msg.step
+ enc = msg.encoding.lower()
+ raw = np.frombuffer(msg.data, dtype=np.uint8)
+ if enc in ("rgb8", "bgr8"):
+ img = raw.reshape(h, step)[:, :w * 3].reshape(h, w, 3)
+ if enc == "bgr8":
+ img = img[:, :, ::-1]
+ elif enc in ("mono8", "8uc1"):
+ img = np.repeat(raw.reshape(h, step)[:, :w][:, :, None], 3, axis=2)
+ else:
+ return None
+ return np.ascontiguousarray(img)
+
+
+def save_ppm(path, arr):
+ h, w, _ = arr.shape
+ with open(path, "wb") as f:
+ f.write(b"P6\n%d %d\n255\n" % (w, h))
+ f.write(arr.tobytes())
+
+
+class Recorder(object):
+ def __init__(self, outdir, count, interval):
+ self.outdir = outdir
+ self.count = count
+ self.interval = rospy.Duration(interval)
+ self.next_at = None
+ self.saved = 0
+ self.latest = None
+ if not os.path.isdir(outdir):
+ os.makedirs(outdir)
+ self.sub = rospy.Subscriber("/uav_cam/image_raw", Image, self.cb, queue_size=1)
+
+ def cb(self, msg):
+ self.latest = msg
+
+ def spin(self):
+ r = rospy.Rate(30)
+ self.next_at = rospy.Time.now()
+ while not rospy.is_shutdown() and self.saved < self.count:
+ if self.latest is not None and rospy.Time.now() >= self.next_at:
+ img = to_rgb(self.latest)
+ if img is not None:
+ p = os.path.join(self.outdir, "frame_%03d.ppm" % self.saved)
+ save_ppm(p, img)
+ self.saved += 1
+ self.next_at = rospy.Time.now() + self.interval
+ r.sleep()
+ rospy.loginfo("录制完成: %d 帧 -> %s", self.saved, self.outdir)
+ return 0 if self.saved else 1
+
+
+def main():
+ ap = argparse.ArgumentParser()
+ ap.add_argument("--outdir", default="/tmp/seq")
+ ap.add_argument("--count", type=int, default=24)
+ ap.add_argument("--interval", type=float, default=0.25)
+ args, _ = ap.parse_known_args(sys.argv)
+ rospy.init_node("uav_camera_record", anonymous=True)
+ return Recorder(args.outdir, args.count, args.interval).spin()
+
+
+if __name__ == "__main__":
+ sys.exit(main())
diff --git a/src/uav_flight_base/scripts/uav_cnn_driver.py b/src/uav_flight_base/scripts/uav_cnn_driver.py
new file mode 100644
index 0000000000..003b7e93fe
--- /dev/null
+++ b/src/uav_flight_base/scripts/uav_cnn_driver.py
@@ -0,0 +1,62 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""任务4 第三步:用训练好的 CNN 实时飞行(只吃图像,不用任何真值)。"""
+
+import os
+import sys
+
+import cv2
+import numpy as np
+import rospy
+import torch
+from geometry_msgs.msg import Twist
+from sensor_msgs.msg import Image
+
+from uav_train_cnn import SmallCNN
+
+W, H = 64, 48
+
+
+class CnnDriver(object):
+ def __init__(self, model_path):
+ self.net = SmallCNN()
+ self.net.load_state_dict(torch.load(model_path, map_location="cpu"))
+ self.net.eval()
+ self.pub = rospy.Publisher("/cmd_vel", Twist, queue_size=1)
+ rospy.Subscriber("/uav_cam/image_raw", Image, self.on_img, queue_size=1)
+ self.busy = False
+ self.count = 0
+ rospy.loginfo("CNN 推理已加载: %s", model_path)
+
+ def on_img(self, msg):
+ if self.busy:
+ return
+ try:
+ buf = np.frombuffer(msg.data, dtype=np.uint8)
+ a = buf.reshape(msg.height, msg.step)[:, :msg.width * 3]
+ a = a.reshape(msg.height, msg.width, 3)
+ gray = cv2.cvtColor(a, cv2.COLOR_RGB2GRAY)
+ small = cv2.resize(gray, (W, H)).astype(np.float32) / 255.0
+ t = torch.from_numpy(small)[None, None, :, :]
+ with torch.no_grad():
+ out = self.net(t)[0].numpy()
+ cmd = Twist()
+ cmd.linear.x = float(max(0.0, min(0.8, out[0])))
+ cmd.angular.z = float(max(-0.9, min(0.9, out[1])))
+ self.pub.publish(cmd)
+ self.count += 1
+ if self.count % 100 == 0:
+ rospy.loginfo("推理 %d 帧 v=%.2f w=%.2f", self.count, cmd.linear.x, cmd.angular.z)
+ except Exception as e:
+ rospy.logwarn_throttle(5.0, "推理失败: %s", e)
+
+
+def main():
+ path = os.path.expanduser(sys.argv[1] if len(sys.argv) > 1 else "~/bc_data/model.pt")
+ rospy.init_node("uav_cnn_driver")
+ CnnDriver(path)
+ rospy.spin()
+
+
+if __name__ == "__main__":
+ main()
diff --git a/src/uav_flight_base/scripts/uav_color_detector.py b/src/uav_flight_base/scripts/uav_color_detector.py
new file mode 100644
index 0000000000..41c0a85d44
--- /dev/null
+++ b/src/uav_flight_base/scripts/uav_color_detector.py
@@ -0,0 +1,110 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""颜色目标检测:在相机图像里找出红色目标桶。
+
+不依赖 OpenCV —— 用 NumPy 自己做 RGB→HSV 转换,方便只有基础依赖的环境。
+
+发布:
+ /uav/perception/target geometry_msgs/Point
+ x = 归一化横向偏差 (-1 左, +1 右, 0 居中)
+ y = 目标面积占画面的比例 (0~1,可用来估计远近)
+ z = 1 表示检测到,0 表示丢失
+
+订阅:
+ /uav_cam/image_raw sensor_msgs/Image
+"""
+
+import numpy as np
+import rospy
+from geometry_msgs.msg import Point
+from sensor_msgs.msg import Image
+
+
+def rgb_to_hsv(rgb):
+ """rgb: (H, W, 3) uint8 -> h(0-360), s(0-1), v(0-1)"""
+ a = rgb.astype(np.float32) / 255.0
+ r, g, b = a[..., 0], a[..., 1], a[..., 2]
+ mx = np.max(a, axis=-1)
+ mn = np.min(a, axis=-1)
+ d = mx - mn + 1e-6
+
+ h = np.zeros_like(mx)
+ mask = mx == r
+ h[mask] = (60 * ((g - b) / d) % 360)[mask]
+ mask = mx == g
+ h[mask] = (60 * ((b - r) / d) + 120)[mask]
+ mask = mx == b
+ h[mask] = (60 * ((r - g) / d) + 240)[mask]
+
+ s = d / (mx + 1e-6)
+ return h, s, mx
+
+
+class ColorDetector(object):
+ def __init__(self):
+ self.topic_in = rospy.get_param("~image_topic", "/uav_cam/image_raw")
+ self.topic_out = rospy.get_param("~target_topic", "/uav/perception/target")
+ # 红色在 HSV 色环上跨 0 度
+ self.h_lo = rospy.get_param("~hue_low", 345.0)
+ self.h_hi = rospy.get_param("~hue_high", 15.0)
+ self.s_min = rospy.get_param("~sat_min", 0.45)
+ self.v_min = rospy.get_param("~val_min", 0.20)
+ self.min_area = rospy.get_param("~min_area", 0.0015) # 占画面比例
+ self.stride = int(rospy.get_param("~stride", 2)) # 降采样倍数,省 CPU
+ self.max_rate = float(rospy.get_param("~rate", 5.0)) # 最高处理频率
+
+ self.last_proc = rospy.Time(0)
+
+ self.pub = rospy.Publisher(self.topic_out, Point, queue_size=1)
+ self.sub = rospy.Subscriber(self.topic_in, Image, self.on_image, queue_size=1)
+ rospy.loginfo("颜色检测启动: %s -> %s", self.topic_in, self.topic_out)
+
+ def on_image(self, msg):
+ now = rospy.Time.now()
+ if self.max_rate > 0 and (now - self.last_proc).to_sec() < 1.0 / self.max_rate:
+ return
+ self.last_proc = now
+
+ w, h, step = msg.width, msg.height, msg.step
+ raw = np.frombuffer(msg.data, dtype=np.uint8)
+ enc = msg.encoding.lower()
+ try:
+ if enc in ("rgb8", "bgr8"):
+ img = raw.reshape(h, step)[:, :w * 3].reshape(h, w, 3)
+ if enc == "bgr8":
+ img = img[:, :, ::-1]
+ elif enc in ("mono8", "8uc1"):
+ return
+ else:
+ return
+ except ValueError:
+ return
+
+ s = max(1, self.stride)
+ if s > 1:
+ img = np.ascontiguousarray(img[::s, ::s])
+ w, h = img.shape[1], img.shape[0]
+
+ hh, ss, vv = rgb_to_hsv(img)
+ mask = ((hh >= self.h_lo) | (hh <= self.h_hi)) & (ss >= self.s_min) & (vv >= self.v_min)
+ area = float(mask.mean())
+
+ out = Point()
+ out.z = 0.0
+ if area >= self.min_area:
+ ys, xs = np.nonzero(mask)
+ cx = float(xs.mean())
+ out.x = (cx - w / 2.0) / (w / 2.0) # -1..1
+ out.y = area
+ out.z = 1.0
+ self.pub.publish(out)
+
+
+def main():
+ rospy.init_node("uav_color_detector")
+ ColorDetector()
+ rospy.spin()
+
+
+if __name__ == "__main__":
+ main()
diff --git a/src/uav_flight_base/scripts/uav_expert_collect.py b/src/uav_flight_base/scripts/uav_expert_collect.py
new file mode 100644
index 0000000000..8722d7d755
--- /dev/null
+++ b/src/uav_flight_base/scripts/uav_expert_collect.py
@@ -0,0 +1,129 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""任务4 第一步:采集"专家"数据(行为克隆)。
+
+专家策略不看图像,用 Gazebo 真值位姿算朝向目标的偏航误差,输出
+(v, w) 控制量;同时把相机图像存盘。训练时只用图像 → 控制量,
+推理时也不需要真值,实现真正的"端到端"。
+
+产物:
+ ~/bc_data/images/000000.png ... 64x48 灰度图
+ ~/bc_data/labels.csv frame,v,w
+"""
+
+import math
+import os
+
+import rospy
+import cv2
+import numpy as np
+from gazebo_msgs.msg import ModelStates
+from geometry_msgs.msg import Twist
+from sensor_msgs.msg import Image
+
+W, H = 64, 48
+GOAL = (2.6, 0.3)
+
+
+def yaw_from_quat(q):
+ return math.atan2(2.0 * (q.w * q.z + q.x * q.y),
+ 1.0 - 2.0 * (q.y * q.y + q.z * q.z))
+
+
+class Collector(object):
+ def __init__(self):
+ self.out = rospy.get_param("~out_dir", os.path.expanduser("~/bc_data"))
+ self.every = int(rospy.get_param("~every", 3)) # 每 3 帧存一张
+ self.v_max = rospy.get_param("~v_max", 0.7)
+ self.model_name = rospy.get_param("~model_name", "uav")
+
+ self.img_dir = os.path.join(self.out, "images")
+ if not os.path.isdir(self.img_dir):
+ os.makedirs(self.img_dir)
+
+ self.labels = open(os.path.join(self.out, "labels.csv"), "w")
+ self.labels.write("frame,v,w\n")
+
+ self.count = 0
+ self.seen = 0
+ self.pose = None
+ self.img = None
+ self.latest = None
+
+ rospy.Subscriber("/uav_cam/image_raw", Image, self.on_img, queue_size=1)
+ rospy.Subscriber("/gazebo/model_states", ModelStates, self.on_states, queue_size=1)
+ self.pub = rospy.Publisher("/cmd_vel", Twist, queue_size=1)
+ rospy.loginfo("采集开始 -> %s", self.out)
+
+ def on_img(self, msg):
+ self.img = msg
+
+ def on_states(self, msg):
+ try:
+ i = msg.name.index(self.model_name)
+ except ValueError:
+ return
+ p = msg.pose[i]
+ self.pose = (p.position.x, p.position.y, yaw_from_quat(p.orientation))
+
+ def control(self, x, y, yaw):
+ dx, dy = GOAL[0] - x, GOAL[1] - y
+ dist = math.hypot(dx, dy)
+ err = math.atan2(dy, dx) - yaw
+ while err > math.pi:
+ err -= 2 * math.pi
+ while err < -math.pi:
+ err += 2 * math.pi
+ w = max(-0.8, min(0.8, 1.3 * err))
+ v = self.v_max * max(0.0, 1.0 - abs(err) / 1.4)
+ if dist < 0.5:
+ v, w = 0.0, 0.0
+ return v, w
+
+ def run(self):
+ r = rospy.Rate(15)
+ while not rospy.is_shutdown():
+ if self.img is None or self.pose is None:
+ r.sleep()
+ continue
+
+ m = self.img
+ buf = np.frombuffer(m.data, dtype=np.uint8)
+ small = None
+ try:
+ if m.encoding.lower() in ("rgb8", "bgr8"):
+ a = buf.reshape(m.height, m.step)[:, :m.width * 3]
+ a = a.reshape(m.height, m.width, 3)
+ gray = cv2.cvtColor(a, cv2.COLOR_RGB2GRAY)
+ small = cv2.resize(gray, (W, H))
+ except Exception:
+ small = None
+
+ v, w = self.control(*self.pose)
+ cmd = Twist()
+ cmd.linear.x = v
+ cmd.angular.z = w
+ self.pub.publish(cmd)
+
+ self.seen += 1
+ if small is not None and self.seen % self.every == 0:
+ name = "%06d.png" % self.count
+ cv2.imwrite(os.path.join(self.img_dir, name), small)
+ self.labels.write("%s,%.3f,%.3f\n" % (name, v, w))
+ self.labels.flush()
+ self.count += 1
+ if self.count % 50 == 0:
+ rospy.loginfo("已采集 %d 帧", self.count)
+ r.sleep()
+
+ self.labels.close()
+ rospy.loginfo("采集结束,共 %d 帧", self.count)
+
+
+def main():
+ rospy.init_node("uav_expert_collect")
+ Collector().run()
+
+
+if __name__ == "__main__":
+ main()
diff --git a/src/uav_flight_base/scripts/uav_grid_nav.py b/src/uav_flight_base/scripts/uav_grid_nav.py
new file mode 100644
index 0000000000..eba1b8b85d
--- /dev/null
+++ b/src/uav_flight_base/scripts/uav_grid_nav.py
@@ -0,0 +1,225 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""任务3:激光建图 + A* 规划 + 航点跟踪(全程不需要图形界面)。
+
+流程:
+ 1. 用 /uav/scan 在 2D 栅格上累积占据/空闲(ray casting)
+ 2. 收到目标点后跑 A*(障碍膨胀 2 格)
+ 3. 用纯追踪(pure pursuit)沿路径飞,到达后停
+
+输出(便于离线画图):
+ ~/nav_out/grid.csv 每个格子: x,y,occ(0/1)
+ ~/nav_out/path.csv 规划路径点
+ ~/nav_out/track.csv 实际飞行轨迹
+"""
+
+import csv
+import heapq
+import math
+import os
+
+import rospy
+from geometry_msgs.msg import Twist
+from nav_msgs.msg import Odometry
+from sensor_msgs.msg import LaserScan
+
+RES = 0.1
+W = 100 # 10m x 10m
+H = 100
+OX, OY = -5.0, -5.0
+
+
+def yaw_from_quat(q):
+ return math.atan2(2.0 * (q.w * q.z + q.x * q.y),
+ 1.0 - 2.0 * (q.y * q.y + q.z * q.z))
+
+
+class GridNav(object):
+ def __init__(self):
+ self.scan_topic = rospy.get_param("~scan_topic", "/uav/scan")
+ self.odom_topic = rospy.get_param("~odom_topic", "/odom")
+ self.cmd_topic = rospy.get_param("~cmd_topic", "/cmd_vel")
+ goal = rospy.get_param("~goal", [2.5, -2.5])
+ if isinstance(goal, str):
+ goal = [float(v) for v in goal.strip("[] ").split(",")]
+ self.goal = (float(goal[0]), float(goal[1]))
+ self.out_dir = rospy.get_param("~out_dir", os.path.expanduser("~/nav_out"))
+ self.map_seconds = rospy.get_param("~map_seconds", 12.0)
+ self.v = rospy.get_param("~v", 0.7)
+
+ self.occ = [[0] * W for _ in range(H)]
+ self.free = [[0] * W for _ in range(H)]
+ self.scan = None
+ self.odom = None
+ self.path = []
+ self.track = []
+ self.state = "mapping"
+ self.t_map = None
+ self.t0 = None
+
+ if not os.path.isdir(self.out_dir):
+ os.makedirs(self.out_dir)
+
+ rospy.Subscriber(self.scan_topic, LaserScan, self.on_scan, queue_size=1)
+ rospy.Subscriber(self.odom_topic, Odometry, self.on_odom, queue_size=1)
+ self.pub = rospy.Publisher(self.cmd_topic, Twist, queue_size=1)
+ rospy.loginfo("网格导航启动,目标 %s", self.goal)
+
+ # ---------------- 地图 ----------------
+ def cell(self, x, y):
+ i = int((x - OX) / RES)
+ j = int((y - OY) / RES)
+ if 0 <= i < W and 0 <= j < H:
+ return i, j
+ return None
+
+ def on_odom(self, msg):
+ self.odom = msg
+ p = msg.pose.pose.position
+ yaw = yaw_from_quat(msg.pose.pose.orientation)
+ if self.t0 is None:
+ self.t0 = msg.header.stamp
+ self.track.append([(msg.header.stamp - self.t0).to_sec(), p.x, p.y, yaw])
+ self.pos = (p.x, p.y, yaw)
+
+ def on_scan(self, msg):
+ self.scan = msg
+
+ def map_scan(self):
+ if self.scan is None or not hasattr(self, "pos"):
+ return
+ px, py, yaw = self.pos
+ s = self.scan
+ for i in range(0, len(s.ranges), 2):
+ r = s.ranges[i]
+ a = yaw + s.angle_min + i * s.angle_increment
+ if not (r == r) or r <= 0.05 or r > 11.0:
+ continue
+ steps = int(r / RES)
+ for k in range(steps):
+ x = px + math.cos(a) * k * RES
+ y = py + math.sin(a) * k * RES
+ c = self.cell(x, y)
+ if c:
+ self.free[c[1]][c[0]] = 1
+ ex = px + math.cos(a) * r
+ ey = py + math.sin(a) * r
+ c = self.cell(ex, ey)
+ if c:
+ self.occ[c[1]][c[0]] += 1
+
+ # ---------------- A* ----------------
+ def blocked(self, i, j):
+ for dj in (-1, 0, 1):
+ for di in (-1, 0, 1):
+ ni, nj = i + di, j + dj
+ if 0 <= ni < W and 0 <= nj < H and self.occ[nj][ni] > 0:
+ return True
+ return False
+
+ def astar(self, start, goal):
+ s = self.cell(*start)
+ g = self.cell(*goal)
+ if not s or not g:
+ return []
+ openq = [(0.0, s)]
+ came = {}
+ cost = {s: 0.0}
+ while openq:
+ _, cur = heapq.heappop(openq)
+ if cur == g:
+ path = [cur]
+ while cur in came:
+ cur = came[cur]
+ path.append(cur)
+ return path[::-1]
+ for di, dj in ((1, 0), (-1, 0), (0, 1), (0, -1), (1, 1), (1, -1), (-1, 1), (-1, -1)):
+ ni, nj = cur[0] + di, cur[1] + dj
+ if not (0 <= ni < W and 0 <= nj < H) or self.blocked(ni, nj):
+ continue
+ nc = cost[cur] + math.hypot(di, dj)
+ if nc < cost.get((ni, nj), 1e9):
+ cost[(ni, nj)] = nc
+ came[(ni, nj)] = cur
+ h = math.hypot(g[0] - ni, g[1] - nj)
+ heapq.heappush(openq, (nc + h, (ni, nj)))
+ return []
+
+ def world(self, c):
+ return OX + (c[0] + 0.5) * RES, OY + (c[1] + 0.5) * RES
+
+ # ---------------- 控制 ----------------
+ def follow(self, cmd):
+ if not self.path:
+ return False
+ px, py, yaw = self.pos
+ tx, ty = self.path[0]
+ d = math.hypot(tx - px, ty - py)
+ if d < 0.35:
+ self.path.pop(0)
+ return bool(self.path)
+ err = math.atan2(ty - py, tx - px) - yaw
+ while err > math.pi:
+ err -= 2 * math.pi
+ while err < -math.pi:
+ err += 2 * math.pi
+ cmd.angular.z = max(-1.0, min(1.0, 1.6 * err))
+ cmd.linear.x = self.v * max(0.0, 1.0 - abs(err) / 1.6)
+ return True
+
+ def run(self):
+ r = rospy.Rate(10)
+ while not rospy.is_shutdown():
+ cmd = Twist()
+ if self.state == "mapping":
+ self.map_scan()
+ if self.pos_ready() and (rospy.Time.now() - self.t_start).to_sec() > self.map_seconds:
+ self.plan()
+ self.state = "driving"
+ elif self.state == "driving":
+ if not self.follow(cmd):
+ self.state = "done"
+ self.save()
+ rospy.loginfo("到达目标,已保存数据")
+ self.pub.publish(cmd)
+ r.sleep()
+
+ def pos_ready(self):
+ return hasattr(self, "pos")
+
+ def plan(self):
+ start = (self.pos[0], self.pos[1])
+ cells = self.astar(start, self.goal)
+ self.path = [self.world(c) for c in cells]
+ rospy.loginfo("A* 规划完成: %d 个航点", len(self.path))
+ with open(os.path.join(self.out_dir, "path.csv"), "w", newline="") as f:
+ w = csv.writer(f)
+ w.writerow(["x", "y"])
+ w.writerows(self.path)
+
+ def save(self):
+ with open(os.path.join(self.out_dir, "grid.csv"), "w", newline="") as f:
+ w = csv.writer(f)
+ w.writerow(["i", "j", "x", "y", "occ"])
+ for j in range(H):
+ for i in range(W):
+ if self.occ[j][i] > 0:
+ x, y = self.world((i, j))
+ w.writerow([i, j, round(x, 3), round(y, 3), 1])
+ with open(os.path.join(self.out_dir, "track.csv"), "w", newline="") as f:
+ w = csv.writer(f)
+ w.writerow(["t", "x", "y", "yaw"])
+ w.writerows(self.track)
+ rospy.loginfo("已保存 grid / path / track 到 %s", self.out_dir)
+
+
+def main():
+ rospy.init_node("uav_grid_nav")
+ node = GridNav()
+ node.t_start = rospy.Time.now()
+ rospy.on_shutdown(node.save)
+ node.run()
+
+
+if __name__ == "__main__":
+ main()
diff --git a/src/uav_flight_base/scripts/uav_mock_sim.py b/src/uav_flight_base/scripts/uav_mock_sim.py
new file mode 100644
index 0000000000..d4219877b6
--- /dev/null
+++ b/src/uav_flight_base/scripts/uav_mock_sim.py
@@ -0,0 +1,123 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""最小无人机运动学仿真(无需 Gazebo)。
+
+订阅 /uav/cmd_vel,按一阶惯性模型积分出位置与姿态,
+发布 /uav/odom(nav_msgs/Odometry)和 map->base_link 的 TF。
+
+用途:
+ 1) 没有模拟器时也能验证遥控、控制器、可视化链路
+ 2) 给后续 SLAM / 端到端模块提供稳定的 odom 源
+"""
+
+import math
+
+import rospy
+import tf2_ros
+from geometry_msgs.msg import Twist, TransformStamped
+from nav_msgs.msg import Odometry
+
+
+def yaw_to_quat(yaw):
+ return (0.0, 0.0, math.sin(yaw * 0.5), math.cos(yaw * 0.5))
+
+
+class MockUav(object):
+ def __init__(self):
+ self.topic_cmd = rospy.get_param("~cmd_topic", "/uav/cmd_vel")
+ self.topic_odom = rospy.get_param("~odom_topic", "/uav/odom")
+ self.rate_hz = float(rospy.get_param("~rate", 50.0))
+ self.tau = float(rospy.get_param("~time_constant", 0.35)) # 一阶惯性
+ self.max_speed = float(rospy.get_param("~max_speed", 5.0))
+ self.z_min = float(rospy.get_param("~z_min", 0.2))
+ self.z_max = float(rospy.get_param("~z_max", 30.0))
+
+ self.x = float(rospy.get_param("~init_x", 0.0))
+ self.y = float(rospy.get_param("~init_y", 0.0))
+ self.z = float(rospy.get_param("~init_z", 1.5))
+ self.yaw = float(rospy.get_param("~init_yaw", 0.0))
+
+ self.vx = self.vy = self.vz = 0.0
+ self.wz = 0.0
+
+ self.cmd = Twist()
+ self.sub = rospy.Subscriber(self.topic_cmd, Twist, self.on_cmd, queue_size=1)
+ self.pub = rospy.Publisher(self.topic_odom, Odometry, queue_size=1)
+ self.br = tf2_ros.TransformBroadcaster()
+ self.last = rospy.Time.now()
+
+ def on_cmd(self, msg):
+ self.cmd = msg
+
+ def clip(self, v):
+ return max(-self.max_speed, min(self.max_speed, v))
+
+ def step(self, dt):
+ ax = (self.cmd.linear.x - self.vx) / self.tau
+ ay = (self.cmd.linear.y - self.vy) / self.tau
+ az = (self.cmd.linear.z - self.vz) / self.tau
+ aw = (self.cmd.angular.z - self.wz) / self.tau
+
+ self.vx = self.clip(self.vx + ax * dt)
+ self.vy = self.clip(self.vy + ay * dt)
+ self.vz = self.clip(self.vz + az * dt)
+ self.wz = self.wz + aw * dt
+
+ # 机体速度转到世界坐标系
+ cy, sy = math.cos(self.yaw), math.sin(self.yaw)
+ self.x += (self.vx * cy - self.vy * sy) * dt
+ self.y += (self.vx * sy + self.vy * cy) * dt
+ self.z = max(self.z_min, min(self.z_max, self.z + self.vz * dt))
+ self.yaw += self.wz * dt
+
+ def publish(self, now):
+ qx, qy, qz, qw = yaw_to_quat(self.yaw)
+
+ odom = Odometry()
+ odom.header.stamp = now
+ odom.header.frame_id = "map"
+ odom.child_frame_id = "base_link"
+ odom.pose.pose.position.x = self.x
+ odom.pose.pose.position.y = self.y
+ odom.pose.pose.position.z = self.z
+ odom.pose.pose.orientation.x = qx
+ odom.pose.pose.orientation.y = qy
+ odom.pose.pose.orientation.z = qz
+ odom.pose.pose.orientation.w = qw
+ odom.twist.twist.linear.x = self.vx
+ odom.twist.twist.linear.y = self.vy
+ odom.twist.twist.linear.z = self.vz
+ odom.twist.twist.angular.z = self.wz
+ self.pub.publish(odom)
+
+ tf = TransformStamped()
+ tf.header.stamp = now
+ tf.header.frame_id = "map"
+ tf.child_frame_id = "base_link"
+ tf.transform.translation.x = self.x
+ tf.transform.translation.y = self.y
+ tf.transform.translation.z = self.z
+ tf.transform.rotation.x = qx
+ tf.transform.rotation.y = qy
+ tf.transform.rotation.z = qz
+ tf.transform.rotation.w = qw
+ self.br.sendTransform(tf)
+
+ def spin(self):
+ r = rospy.Rate(self.rate_hz)
+ while not rospy.is_shutdown():
+ now = rospy.Time.now()
+ dt = max(1e-4, (now - self.last).to_sec())
+ self.last = now
+ self.step(min(dt, 0.1))
+ self.publish(now)
+ r.sleep()
+
+
+def main():
+ rospy.init_node("uav_mock_sim")
+ MockUav().spin()
+
+
+if __name__ == "__main__":
+ main()
diff --git a/src/uav_flight_base/scripts/uav_np_driver.py b/src/uav_flight_base/scripts/uav_np_driver.py
new file mode 100644
index 0000000000..9b831c5f8f
--- /dev/null
+++ b/src/uav_flight_base/scripts/uav_np_driver.py
@@ -0,0 +1,62 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""任务4:用 NumPy 网络实时飞行(纯图像输入,不使用任何真值)。"""
+
+import os
+import sys
+
+import cv2
+import numpy as np
+import rospy
+from geometry_msgs.msg import Twist
+from sensor_msgs.msg import Image
+
+W, H = 32, 24
+
+
+class NpDriver(object):
+ def __init__(self, weights):
+ z = np.load(weights)
+ self.W1, self.b1 = z["W1"], z["b1"]
+ self.W2, self.b2 = z["W2"], z["b2"]
+ self.ym, self.ys = z["y_mean"], z["y_std"]
+ self.pub = rospy.Publisher("/cmd_vel", Twist, queue_size=1)
+ rospy.Subscriber("/uav_cam/image_raw", Image, self.on_img, queue_size=1)
+ self.count = 0
+ self.busy = False
+ rospy.loginfo("NumPy 策略已加载: %s", weights)
+
+ def on_img(self, msg):
+ if self.busy:
+ return
+ self.busy = True
+ try:
+ buf = np.frombuffer(msg.data, dtype=np.uint8)
+ a = buf.reshape(msg.height, msg.step)[:, :msg.width * 3].reshape(msg.height, msg.width, 3)
+ gray = cv2.cvtColor(a, cv2.COLOR_RGB2GRAY)
+ small = cv2.resize(gray, (W, H)).astype(np.float32).ravel() / 255.0
+ h = np.maximum(small @ self.W1 + self.b1, 0.0)
+ o = h @ self.W2 + self.b2
+ out = o * self.ys + self.ym
+ cmd = Twist()
+ cmd.linear.x = float(np.clip(out[0], 0.0, 0.8))
+ cmd.angular.z = float(np.clip(out[1], -0.9, 0.9))
+ self.pub.publish(cmd)
+ self.count += 1
+ if self.count % 100 == 0:
+ rospy.loginfo("推理 %d 帧 v=%.2f w=%.2f", self.count, cmd.linear.x, cmd.angular.z)
+ except Exception as e:
+ rospy.logwarn_throttle(5.0, "推理失败: %s", e)
+ finally:
+ self.busy = False
+
+
+def main():
+ path = os.path.expanduser(sys.argv[1] if len(sys.argv) > 1 else "~/bc_data/weights.npz")
+ rospy.init_node("uav_np_driver")
+ NpDriver(path)
+ rospy.spin()
+
+
+if __name__ == "__main__":
+ main()
diff --git a/src/uav_flight_base/scripts/uav_policy_sweep.py b/src/uav_flight_base/scripts/uav_policy_sweep.py
new file mode 100644
index 0000000000..f77fdd3fd4
--- /dev/null
+++ b/src/uav_flight_base/scripts/uav_policy_sweep.py
@@ -0,0 +1,122 @@
+# -*- coding: utf-8 -*-
+"""网络结构 / 优化器对比实验(作业要求:同算法不同模型结构的对比)。
+
+数据: policy_data/scan_dataset.csv (90 维激光扫描 -> (v, w))
+对比: 隐层宽度 32 / 64 / 96 / 128 / 256 × {SGD+Momentum, Adam}
+产出: policy_data/sweep_results.csv
+"""
+import csv
+import os
+import time
+
+import numpy as np
+import pandas as pd
+
+BASE = os.path.dirname(os.path.abspath(__file__))
+CSV = os.path.join(BASE, "policy_data", "scan_dataset.csv")
+OUT = os.path.join(BASE, "policy_data", "sweep_results.csv")
+
+EPOCHS = 300
+BATCH = 64
+L2 = 1e-5
+SEED = 0
+
+
+def load():
+ df = pd.read_csv(CSV)
+ x = df.iloc[:, :-2].to_numpy(dtype=np.float32)
+ y = df.iloc[:, -2:].to_numpy(dtype=np.float32)
+ return x, y
+
+
+def train(x, y, hidden, opt_name):
+ rng = np.random.RandomState(SEED)
+ n, d = x.shape
+ cut = int(n * 0.85)
+ xtr, ytr, xva, yva = x[:cut], y[:cut], x[cut:], y[cut:]
+
+ xm, xs = xtr.mean(0), xtr.std(0) + 1e-6
+ ym, ys = ytr.mean(0), ytr.std(0) + 1e-6
+ xtr = (xtr - xm) / xs
+ xva = (xva - xm) / xs
+ ytr = (ytr - ym) / ys
+ yva = (yva - ym) / ys
+
+ W1 = rng.randn(d, hidden).astype(np.float32) * np.sqrt(2.0 / d)
+ b1 = np.zeros(hidden, dtype=np.float32)
+ W2 = rng.randn(hidden, 2).astype(np.float32) * np.sqrt(2.0 / hidden)
+ b2 = np.zeros(2, dtype=np.float32)
+
+ lr = 2e-3 if opt_name == "sgd" else 1e-3
+ mom, b1m, b2m = 0.9, 0.9, 0.999
+ mW1 = np.zeros_like(W1); mb1 = np.zeros_like(b1)
+ mW2 = np.zeros_like(W2); mb2 = np.zeros_like(b2)
+ vW1 = np.zeros_like(W1); vb1 = np.zeros_like(b1)
+ vW2 = np.zeros_like(W2); vb2 = np.zeros_like(b2)
+ t = 0
+
+ best = 1e9
+ t0 = time.time()
+ for ep in range(EPOCHS):
+ perm = rng.permutation(len(xtr))
+ for i in range(0, len(perm), BATCH):
+ b = perm[i:i + BATCH]
+ xb, yb = xtr[b], ytr[b]
+ h = np.maximum(xb @ W1 + b1, 0.0)
+ o = h @ W2 + b2
+ go = (o - yb) * (2.0 / len(b))
+ gW2 = h.T @ go + L2 * W2
+ gb2 = go.sum(0)
+ gh = (go @ W2.T) * (h > 0)
+ gW1 = xb.T @ gh + L2 * W1
+ gb1 = gh.sum(0)
+
+ t += 1
+ if opt_name == "sgd":
+ mW1 = mom * mW1 - lr * gW1; W1 += mW1
+ mb1 = mom * mb1 - lr * gb1; b1 += mb1
+ mW2 = mom * mW2 - lr * gW2; W2 += mW2
+ mb2 = mom * mb2 - lr * gb2; b2 += mb2
+ else:
+ for P, G, M, V in ((W1, gW1, mW1, vW1), (b1, gb1, mb1, vb1),
+ (W2, gW2, mW2, vW2), (b2, gb2, mb2, vb2)):
+ M *= b1m
+ M += (1 - b1m) * G
+ V *= b2m
+ V += (1 - b2m) * G * G
+ Mh = M / (1 - b1m ** t)
+ Vh = V / (1 - b2m ** t)
+ P -= lr * Mh / (np.sqrt(Vh) + 1e-8)
+
+ hv = np.maximum(xva @ W1 + b1, 0.0)
+ vl = float(np.mean((hv @ W2 + b2 - yva) ** 2))
+ if vl < best:
+ best = vl
+
+ return {
+ "hidden": hidden,
+ "optimizer": opt_name,
+ "params": d * hidden + hidden + hidden * 2 + 2,
+ "val_loss": round(best, 5),
+ "seconds": round(time.time() - t0, 1),
+ }
+
+
+def main():
+ x, y = load()
+ print("samples:", x.shape)
+ rows = []
+ for hidden in (32, 64, 96, 128, 256):
+ for opt in ("sgd", "adam"):
+ r = train(x, y, hidden, opt)
+ rows.append(r)
+ print(r)
+ with open(OUT, "w", newline="", encoding="utf-8") as f:
+ w = csv.DictWriter(f, fieldnames=["hidden", "optimizer", "params", "val_loss", "seconds"])
+ w.writeheader()
+ w.writerows(rows)
+ print("saved:", OUT)
+
+
+if __name__ == "__main__":
+ main()
diff --git a/src/uav_flight_base/scripts/uav_scan_avoider.py b/src/uav_flight_base/scripts/uav_scan_avoider.py
new file mode 100644
index 0000000000..4ba07c34c6
--- /dev/null
+++ b/src/uav_flight_base/scripts/uav_scan_avoider.py
@@ -0,0 +1,148 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""激光雷达自主避障:感知 + 控制,完全不依赖相机/图形界面。
+
+控制律(反应式避障):
+ front = 正前方 ±front_fov 内的最小距离
+ front < warn_dist → 转向最近空隙方向,减速
+ front < stop_dist → 原地转
+ 否则 → 直行
+
+同时把 (t, x, y, yaw, front, left, right) 记到 CSV,便于事后画轨迹图。
+"""
+
+import csv
+import math
+import os
+
+import rospy
+from geometry_msgs.msg import Twist
+from nav_msgs.msg import Odometry
+from sensor_msgs.msg import LaserScan
+
+
+def yaw_from_quat(q):
+ return math.atan2(2.0 * (q.w * q.z + q.x * q.y),
+ 1.0 - 2.0 * (q.y * q.y + q.z * q.z))
+
+
+class ScanAvoider(object):
+ def __init__(self):
+ self.scan_topic = rospy.get_param("~scan_topic", "/uav/scan")
+ self.odom_topic = rospy.get_param("~odom_topic", "/uav/odom")
+ self.cmd_topic = rospy.get_param("~cmd_topic", "/cmd_vel")
+
+ self.v_norm = rospy.get_param("~v_norm", 0.9)
+ self.v_slow = rospy.get_param("~v_slow", 0.35)
+ self.w_max = rospy.get_param("~w_max", 0.9)
+ self.warn_dist = rospy.get_param("~warn_dist", 1.6)
+ self.stop_dist = rospy.get_param("~stop_dist", 0.8)
+ self.front_fov = rospy.get_param("~front_fov", 0.6) # ±弧度
+ self.record = rospy.get_param("~csv", "")
+
+ self.scan = None
+ self.pose = None
+ self.t0 = None
+ self.rows = []
+ self.last_log = rospy.Time.now()
+ self._csv = None
+ self._writer = None
+ if self.record:
+ d = os.path.dirname(os.path.abspath(self.record))
+ if d and not os.path.isdir(d):
+ os.makedirs(d)
+ self._csv = open(self.record, "w", newline="")
+ self._writer = csv.writer(self._csv)
+ self._writer.writerow(["t", "x", "y", "z", "yaw", "front", "left", "right"])
+ self._csv.flush()
+
+ self.pub = rospy.Publisher(self.cmd_topic, Twist, queue_size=1)
+ rospy.Subscriber(self.scan_topic, LaserScan, self.on_scan, queue_size=1)
+ rospy.Subscriber(self.odom_topic, Odometry, self.on_odom, queue_size=1)
+ rospy.loginfo("激光避障启动: %s", self.scan_topic)
+
+ def on_scan(self, msg):
+ self.scan = msg
+
+ def on_odom(self, msg):
+ self.pose = msg
+
+ def sector_min(self, lo, hi):
+ s = self.scan
+ if s is None:
+ return float("inf")
+ best = float("inf")
+ n = len(s.ranges)
+ for i, r in enumerate(s.ranges):
+ a = s.angle_min + i * s.angle_increment
+ if lo <= a <= hi and r == r: # 过滤 NaN
+ best = min(best, r)
+ return best
+
+ def run(self):
+ r = rospy.Rate(10)
+ while not rospy.is_shutdown():
+ cmd = Twist()
+ if self.scan is None:
+ self.pub.publish(cmd)
+ r.sleep()
+ continue
+
+ front = self.sector_min(-self.front_fov, self.front_fov)
+ left = self.sector_min(0.3, 1.4)
+ right = self.sector_min(-1.4, -0.3)
+
+ if front < self.stop_dist:
+ # 太近:原地转向空隙大的一侧
+ cmd.angular.z = self.w_max if left > right else -self.w_max
+ cmd.linear.x = 0.0
+ elif front < self.warn_dist:
+ # 有点近:朝空隙转,同时减速
+ err = (left - right) / max(left + right, 1e-3)
+ cmd.angular.z = max(-self.w_max, min(self.w_max, 1.2 * err))
+ cmd.linear.x = self.v_slow
+ else:
+ cmd.linear.x = self.v_norm
+ cmd.angular.z = 0.0
+
+ self.pub.publish(cmd)
+ self.record_row(front, left, right)
+ r.sleep()
+ self.save()
+
+ def record_row(self, front, left, right):
+ if self.pose is None:
+ return
+ if self.t0 is None:
+ self.t0 = self.pose.header.stamp
+ p = self.pose.pose.pose.position
+ yaw = yaw_from_quat(self.pose.pose.pose.orientation)
+ t = (self.pose.header.stamp - self.t0).to_sec()
+ row = [round(t, 3), round(p.x, 4), round(p.y, 4), round(p.z, 4),
+ round(yaw, 4), round(min(front, 99), 3),
+ round(min(left, 99), 3), round(min(right, 99), 3)]
+ self.rows.append(row)
+ if self._writer is not None:
+ self._writer.writerow(row)
+ self._csv.flush() # 实时落盘,进程被杀也不丢数据
+
+ def save(self):
+ if self._csv is not None:
+ try:
+ self._csv.close()
+ except Exception:
+ pass
+ self._csv = None
+ if self.record:
+ rospy.loginfo("轨迹已保存到 %s(%d 行)", self.record, len(self.rows))
+
+
+def main():
+ rospy.init_node("uav_scan_avoider")
+ node = ScanAvoider()
+ rospy.on_shutdown(node.save)
+ node.run()
+
+
+if __name__ == "__main__":
+ main()
diff --git a/src/uav_flight_base/scripts/uav_scan_dataset.py b/src/uav_flight_base/scripts/uav_scan_dataset.py
new file mode 100644
index 0000000000..ba72747e3d
--- /dev/null
+++ b/src/uav_flight_base/scripts/uav_scan_dataset.py
@@ -0,0 +1,71 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""任务2(神经网络版)第一步:采集 (激光扫描, 速度指令) 数据。
+
+一边运行专家避障策略(uav_scan_avoider),一边记录它发出的 cmd_vel 与
+当时的 360 线激光扫描,得到行为克隆数据集。
+
+产物: ~/policy_data/scan_dataset.csv
+"""
+
+import csv
+import os
+
+import numpy as np
+import rospy
+from geometry_msgs.msg import Twist
+from sensor_msgs.msg import LaserScan
+
+N = 90 # 采样维度(360 线降采样到 90)
+RMAX = 3.5 # 距离截断
+
+
+class Recorder(object):
+ def __init__(self):
+ out = rospy.get_param("~out_dir", os.path.expanduser("~/policy_data"))
+ if not os.path.isdir(out):
+ os.makedirs(out)
+ self.path = os.path.join(out, "scan_dataset.csv")
+ self.f = open(self.path, "w", newline="")
+ self.w = csv.writer(self.f)
+ self.w.writerow(["r%d" % i for i in range(N)] + ["v", "w"])
+
+ self.scan = None
+ self.cmd = None
+ self.count = 0
+
+ rospy.Subscriber("/uav/scan", LaserScan, self.on_scan, queue_size=1)
+ rospy.Subscriber("/cmd_vel", Twist, self.on_cmd, queue_size=1)
+ rospy.Timer(rospy.Duration(0.1), self.tick)
+ rospy.loginfo("采集到 %s", self.path)
+
+ def on_scan(self, msg):
+ self.scan = msg
+
+ def on_cmd(self, msg):
+ self.cmd = msg
+
+ def tick(self, _evt):
+ if self.scan is None or self.cmd is None:
+ return
+ r = np.array(self.scan.ranges, dtype=np.float32)
+ r[~np.isfinite(r)] = RMAX
+ r = np.clip(r, 0.0, RMAX)
+ idx = np.linspace(0, len(r) - 1, N).astype(int)
+ feat = r[idx]
+ self.w.writerow(list(np.round(feat, 3)) +
+ [round(self.cmd.linear.x, 3), round(self.cmd.angular.z, 3)])
+ self.f.flush()
+ self.count += 1
+ if self.count % 100 == 0:
+ rospy.loginfo("已记录 %d 条", self.count)
+
+
+def main():
+ rospy.init_node("uav_scan_dataset")
+ Recorder()
+ rospy.spin()
+
+
+if __name__ == "__main__":
+ main()
diff --git a/src/uav_flight_base/scripts/uav_scan_policy.py b/src/uav_flight_base/scripts/uav_scan_policy.py
new file mode 100644
index 0000000000..817f900ee8
--- /dev/null
+++ b/src/uav_flight_base/scripts/uav_scan_policy.py
@@ -0,0 +1,79 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""任务2(神经网络版)第三步:用训练好的网络接管飞行,并记录评估指标。
+
+订阅 /uav/scan -> 网络 -> /cmd_vel
+同时把 (t, v, w, min_range) 写到 ~/policy_data/eval_.csv
+"""
+
+import csv
+import os
+import sys
+
+import numpy as np
+import rospy
+from geometry_msgs.msg import Twist
+from sensor_msgs.msg import LaserScan
+
+N = 90
+RMAX = 3.5
+
+
+class ScanPolicy(object):
+ def __init__(self, weights, tag):
+ z = np.load(weights)
+ self.W1, self.b1 = z["W1"], z["b1"]
+ self.W2, self.b2 = z["W2"], z["b2"]
+ self.xm, self.xs = z["xm"], z["xs"]
+ self.ym, self.ys = z["ym"], z["ys"]
+
+ out = os.path.dirname(weights) or os.path.expanduser("~/policy_data")
+ self.log = open(os.path.join(out, "eval_%s.csv" % tag), "w", newline="")
+ self.w = csv.writer(self.log)
+ self.w.writerow(["t", "v", "w", "min_range"])
+ self.t0 = rospy.Time.now()
+ self.count = 0
+ self.min_all = 9.9
+
+ self.pub = rospy.Publisher("/cmd_vel", Twist, queue_size=1)
+ rospy.Subscriber("/uav/scan", LaserScan, self.on_scan, queue_size=1)
+ rospy.loginfo("扫描策略已加载: %s tag=%s", weights, tag)
+
+ def on_scan(self, msg):
+ r = np.array(msg.ranges, dtype=np.float32)
+ r[~np.isfinite(r)] = RMAX
+ r = np.clip(r, 0.0, RMAX)
+ mr = float(r.min())
+ self.min_all = min(self.min_all, mr)
+
+ idx = np.linspace(0, len(r) - 1, N).astype(int)
+ feat = (r[idx] - self.xm) / self.xs
+ hh = np.maximum(feat @ self.W1 + self.b1, 0.0)
+ o = hh @ self.W2 + self.b2
+ out = o * self.ys + self.ym
+
+ cmd = Twist()
+ cmd.linear.x = float(np.clip(out[0], 0.0, 0.9))
+ cmd.angular.z = float(np.clip(out[1], -1.0, 1.0))
+ self.pub.publish(cmd)
+
+ self.count += 1
+ if self.count % 5 == 0:
+ self.w.writerow([round((rospy.Time.now() - self.t0).to_sec(), 2),
+ round(cmd.linear.x, 3), round(cmd.angular.z, 3), round(mr, 3)])
+ self.log.flush()
+ if self.count % 100 == 0:
+ rospy.loginfo("推理 %d 帧 v=%.2f w=%.2f min_range=%.2f",
+ self.count, cmd.linear.x, cmd.angular.z, mr)
+
+
+def main():
+ path = os.path.expanduser(sys.argv[1] if len(sys.argv) > 1 else "~/policy_data/scan_policy.npz")
+ tag = sys.argv[2] if len(sys.argv) > 2 else "run"
+ rospy.init_node("uav_scan_policy")
+ ScanPolicy(path, tag)
+ rospy.spin()
+
+
+if __name__ == "__main__":
+ main()
diff --git a/src/uav_flight_base/scripts/uav_state_monitor.py b/src/uav_flight_base/scripts/uav_state_monitor.py
new file mode 100644
index 0000000000..891dc166aa
--- /dev/null
+++ b/src/uav_flight_base/scripts/uav_state_monitor.py
@@ -0,0 +1,78 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""状态监控节点:订阅 odom,定时打印飞行状态,可另存 CSV。
+
+输出示例:
+ [ 12.3s] pos=( 3.21, -1.05, 1.50) yaw= 12.4° v= 1.02 m/s
+"""
+
+import math
+import os
+
+import rospy
+from nav_msgs.msg import Odometry
+
+
+def yaw_from_quat(q):
+ siny = 2.0 * (q.w * q.z + q.x * q.y)
+ cosy = 1.0 - 2.0 * (q.y * q.y + q.z * q.z)
+ return math.atan2(siny, cosy)
+
+
+class Monitor(object):
+ def __init__(self):
+ self.topic = rospy.get_param("~odom_topic", "/uav/odom")
+ self.period = float(rospy.get_param("~period", 0.5))
+ self.csv_path = rospy.get_param("~csv", "")
+
+ self.t0 = None
+ self.rows = []
+ self.latest = None
+
+ self.sub = rospy.Subscriber(self.topic, Odometry, self.on_odom, queue_size=1)
+ self.timer = rospy.Timer(rospy.Duration(self.period), self.on_timer)
+
+ def on_odom(self, msg):
+ if self.t0 is None:
+ self.t0 = msg.header.stamp
+ self.latest = msg
+ if self.csv_path:
+ p = msg.pose.pose.position
+ self.rows.append([msg.header.stamp.to_sec(), p.x, p.y, p.z,
+ yaw_from_quat(msg.pose.pose.orientation)])
+
+ def on_timer(self, _evt):
+ m = self.latest
+ if m is None:
+ rospy.loginfo_throttle(5.0, "等待 %s ...", self.topic)
+ return
+ p = m.pose.pose.position
+ v = m.twist.twist.linear
+ speed = math.sqrt(v.x ** 2 + v.y ** 2 + v.z ** 2)
+ t = (m.header.stamp - self.t0).to_sec()
+ rospy.loginfo("[%6.1fs] pos=(%6.2f,%6.2f,%6.2f) yaw=%6.1f° v=%.2f m/s",
+ t, p.x, p.y, p.z,
+ math.degrees(yaw_from_quat(m.pose.pose.orientation)), speed)
+
+ def save(self):
+ if not self.csv_path or not self.rows:
+ return
+ d = os.path.dirname(os.path.abspath(self.csv_path))
+ if d and not os.path.isdir(d):
+ os.makedirs(d)
+ with open(self.csv_path, "w") as f:
+ f.write("t,x,y,z,yaw\n")
+ for r in self.rows:
+ f.write(",".join("%.4f" % x for x in r) + "\n")
+ rospy.loginfo("已保存 %d 行到 %s", len(self.rows), self.csv_path)
+
+
+def main():
+ rospy.init_node("uav_state_monitor")
+ mon = Monitor()
+ rospy.on_shutdown(mon.save)
+ rospy.spin()
+
+
+if __name__ == "__main__":
+ main()
diff --git a/src/uav_flight_base/scripts/uav_teleop_key.py b/src/uav_flight_base/scripts/uav_teleop_key.py
new file mode 100644
index 0000000000..ecfffe8725
--- /dev/null
+++ b/src/uav_flight_base/scripts/uav_teleop_key.py
@@ -0,0 +1,101 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""键盘遥控节点:把按键转成 /uav/cmd_vel(geometry_msgs/Twist)。
+
+按键(按住生效,松开自动归零):
+ w / s 前后(+x / -x)
+ a / d 左右(+y / -y)
+ r / f 升降(+z / -z)
+ q / e 偏航(+yaw / -yaw)
+ 空格 悬停(速度清零)
+ Ctrl+C 退出
+"""
+
+import sys
+import termios
+import tty
+import select
+
+import rospy
+from geometry_msgs.msg import Twist
+
+HELP = """
+------------------------------
+ w/s : 前进 / 后退
+ a/d : 左移 / 右移
+ r/f : 上升 / 下降
+ q/e : 左转 / 右转
+ 空格: 悬停(清零)
+ Ctrl+C 退出
+------------------------------
+"""
+
+KEYMAP = {
+ "w": ("linear", "x", 1.0),
+ "s": ("linear", "x", -1.0),
+ "a": ("linear", "y", 1.0),
+ "d": ("linear", "y", -1.0),
+ "r": ("linear", "z", 1.0),
+ "f": ("linear", "z", -1.0),
+ "q": ("angular", "z", 1.0),
+ "e": ("angular", "z", -1.0),
+}
+
+
+def get_key(timeout=0.1):
+ """非阻塞读一个字符;没有输入返回空串。"""
+ dr, _, _ = select.select([sys.stdin], [], [], timeout)
+ if not dr:
+ return ""
+ return sys.stdin.read(1)
+
+
+def main():
+ rospy.init_node("uav_teleop_key")
+
+ topic = rospy.get_param("~cmd_topic", "/uav/cmd_vel")
+ speed = float(rospy.get_param("~speed", 1.0)) # m/s
+ yaw_rate = float(rospy.get_param("~yaw_rate", 0.8)) # rad/s
+ rate_hz = float(rospy.get_param("~rate", 20.0))
+
+ pub = rospy.Publisher(topic, Twist, queue_size=1)
+ rate = rospy.Rate(rate_hz)
+
+ if not sys.stdin.isatty():
+ rospy.logwarn("stdin 不是终端,键盘读取会失效;请在交互式终端里运行")
+ return
+
+ old = termios.tcgetattr(sys.stdin)
+ print(HELP)
+ cmd = Twist()
+
+ try:
+ tty.setcbreak(sys.stdin.fileno())
+ while not rospy.is_shutdown():
+ key = get_key(1.0 / rate_hz)
+ if key:
+ if key == " ":
+ cmd = Twist()
+ rospy.loginfo("悬停")
+ elif key in KEYMAP:
+ axis, comp, sign = KEYMAP[key]
+ cmd = Twist()
+ value = sign * (yaw_rate if axis == "angular" else speed)
+ vec = getattr(cmd, axis)
+ setattr(vec, comp, value)
+ rospy.loginfo("cmd %s.%s = %.2f", axis, comp, value)
+ elif key in ("\x03",):
+ break
+ pub.publish(cmd)
+ rate.sleep()
+ finally:
+ termios.tcsetattr(sys.stdin, termios.TCSADRAIN, old)
+ pub.publish(Twist())
+ print("\n已退出,速度已归零")
+
+
+if __name__ == "__main__":
+ try:
+ main()
+ except rospy.ROSInterruptException:
+ pass
diff --git a/src/uav_flight_base/scripts/uav_train_cnn.py b/src/uav_flight_base/scripts/uav_train_cnn.py
new file mode 100644
index 0000000000..99161f3695
--- /dev/null
+++ b/src/uav_flight_base/scripts/uav_train_cnn.py
@@ -0,0 +1,101 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""任务4 第二步:训练 CNN(图像 -> 控制量),行为克隆。
+
+输入: ~/bc_data/images/*.png (64x48 灰度) + labels.csv
+输出: ~/bc_data/model.pt
+
+网络: Conv(1->8,3x3) - ReLU - MaxPool2 - Conv(8->16,3x3) - ReLU - MaxPool2
+ - FC(16*12*16 -> 64) - ReLU - FC(64 -> 2)
+"""
+
+import csv
+import os
+import sys
+
+import numpy as np
+import torch
+import torch.nn as nn
+
+
+class SmallCNN(nn.Module):
+ def __init__(self):
+ super(SmallCNN, self).__init__()
+ self.features = nn.Sequential(
+ nn.Conv2d(1, 8, 3, padding=1), nn.ReLU(), nn.MaxPool2d(2),
+ nn.Conv2d(8, 16, 3, padding=1), nn.ReLU(), nn.MaxPool2d(2),
+ )
+ self.head = nn.Sequential(
+ nn.Linear(16 * 12 * 16, 64), nn.ReLU(),
+ nn.Linear(64, 2),
+ )
+
+ def forward(self, x):
+ x = self.features(x)
+ x = x.view(x.size(0), -1)
+ return self.head(x)
+
+
+def load_data(root):
+ import cv2
+ labels = list(csv.DictReader(open(os.path.join(root, "labels.csv"))))
+ xs, ys = [], []
+ for r in labels:
+ p = os.path.join(root, "images", r["frame"])
+ img = cv2.imread(p, cv2.IMREAD_GRAYSCALE)
+ if img is None:
+ continue
+ xs.append(img.astype(np.float32) / 255.0)
+ ys.append([float(r["v"]), float(r["w"])])
+ x = torch.from_numpy(np.array(xs)[:, None, :, :])
+ y = torch.from_numpy(np.array(ys, dtype=np.float32))
+ return x, y
+
+
+def main():
+ root = os.path.expanduser(sys.argv[1] if len(sys.argv) > 1 else "~/bc_data")
+ epochs = int(sys.argv[2]) if len(sys.argv) > 2 else 30
+ x, y = load_data(root)
+ print("samples:", x.shape, y.shape)
+ if len(x) < 20:
+ print("样本太少")
+ return 1
+
+ n = len(x)
+ idx = torch.randperm(n)
+ split = int(n * 0.85)
+ tr, va = idx[:split], idx[split:]
+
+ net = SmallCNN()
+ opt = torch.optim.Adam(net.parameters(), lr=1e-3)
+ lossf = nn.MSELoss()
+
+ best = 1e9
+ for ep in range(epochs):
+ net.train()
+ perm = tr[torch.randperm(len(tr))]
+ tot = 0.0
+ for i in range(0, len(perm), 32):
+ b = perm[i:i + 32]
+ opt.zero_grad()
+ out = net(x[b])
+ loss = lossf(out, y[b])
+ loss.backward()
+ opt.step()
+ tot += loss.item() * len(b)
+ net.eval()
+ with torch.no_grad():
+ vloss = lossf(net(x[va]), y[va]).item()
+ if ep % 5 == 0 or ep == epochs - 1:
+ print("epoch %3d train %.5f val %.5f" % (ep, tot / len(tr), vloss))
+ if vloss < best:
+ best = vloss
+ torch.save(net.state_dict(), os.path.join(root, "model.pt"))
+
+ print("best val loss: %.5f" % best)
+ print("saved:", os.path.join(root, "model.pt"))
+ return 0
+
+
+if __name__ == "__main__":
+ sys.exit(main())
diff --git a/src/uav_flight_base/scripts/uav_train_np.py b/src/uav_flight_base/scripts/uav_train_np.py
new file mode 100644
index 0000000000..ce70f2d151
--- /dev/null
+++ b/src/uav_flight_base/scripts/uav_train_np.py
@@ -0,0 +1,106 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""任务4:端到端神经网络(图像 -> 控制量),纯 NumPy 实现,无需 torch。
+
+网络结构:
+ 输入 32x24 灰度 (768) -> 隐层 64 (ReLU) -> 输出 2 (v, w)
+ 损失 MSE + L2 正则,SGD + 动量
+
+用法:
+ python3 uav_train_np.py ~/bc_data 300
+产物:
+ ~/bc_data/weights.npz
+"""
+
+import csv
+import os
+import sys
+
+import numpy as np
+
+
+def load(root, w=32, h=24):
+ import cv2
+ rows = list(csv.DictReader(open(os.path.join(root, "labels.csv"))))
+ xs, ys = [], []
+ for r in rows:
+ img = cv2.imread(os.path.join(root, "images", r["frame"]), cv2.IMREAD_GRAYSCALE)
+ if img is None:
+ continue
+ xs.append(cv2.resize(img, (w, h)).astype(np.float32).ravel() / 255.0)
+ ys.append([float(r["v"]), float(r["w"])])
+ return np.array(xs), np.array(ys, dtype=np.float32)
+
+
+def main():
+ root = os.path.expanduser(sys.argv[1] if len(sys.argv) > 1 else "~/bc_data")
+ epochs = int(sys.argv[2]) if len(sys.argv) > 2 else 300
+
+ x, y = load(root)
+ print("samples:", x.shape, y.shape)
+ if len(x) < 20:
+ print("样本太少,先采集数据")
+ return 1
+
+ n, d = x.shape
+ idx = np.random.permutation(n)
+ split = int(n * 0.85)
+ tr, va = idx[:split], idx[split:]
+
+ # 归一化目标
+ y_mean = y[tr].mean(axis=0)
+ y_std = y[tr].std(axis=0) + 1e-6
+ yn = (y - y_mean) / y_std
+
+ rng = np.random.RandomState(0)
+ W1 = rng.randn(d, 64).astype(np.float32) * np.sqrt(2.0 / d)
+ b1 = np.zeros(64, dtype=np.float32)
+ W2 = rng.randn(64, 2).astype(np.float32) * np.sqrt(2.0 / 64)
+ b2 = np.zeros(2, dtype=np.float32)
+
+ mW1 = np.zeros_like(W1); mb1 = np.zeros_like(b1)
+ mW2 = np.zeros_like(W2); mb2 = np.zeros_like(b2)
+
+ lr = 3e-3
+ mom = 0.9
+ l2 = 1e-4
+ best = 1e9
+ best_pack = None
+
+ for ep in range(epochs):
+ perm = np.random.permutation(len(tr))
+ for i in range(0, len(perm), 64):
+ b = tr[perm[i:i + 64]]
+ xb, yb = x[b], yn[b]
+ h = xb @ W1 + b1
+ h = np.maximum(h, 0.0)
+ o = h @ W2 + b2
+ e = o - yb
+ go = e * (2.0 / len(b))
+ gW2 = h.T @ go + l2 * W2
+ gb2 = go.sum(axis=0)
+ gh = (go @ W2.T) * (h > 0)
+ gW1 = xb.T @ gh + l2 * W1
+ gb1 = gh.sum(axis=0)
+ mW1 = mom * mW1 - lr * gW1; W1 += mW1
+ mb1 = mom * mb1 - lr * gb1; b1 += mb1
+ mW2 = mom * mW2 - lr * gW2; W2 += mW2
+ mb2 = mom * mb2 - lr * gb2; b2 += mb2
+
+ hv = np.maximum(x[va] @ W1 + b1, 0.0)
+ vloss = float(np.mean((hv @ W2 + b2 - yn[va]) ** 2))
+ if vloss < best:
+ best = vloss
+ best_pack = (W1.copy(), b1.copy(), W2.copy(), b2.copy())
+ if ep % 50 == 0 or ep == epochs - 1:
+ print("epoch %3d val %.5f" % (ep, vloss))
+
+ W1, b1, W2, b2 = best_pack
+ np.savez(os.path.join(root, "weights.npz"), W1=W1, b1=b1, W2=W2, b2=b2,
+ y_mean=y_mean, y_std=y_std)
+ print("best val loss %.5f -> %s" % (best, os.path.join(root, "weights.npz")))
+ return 0
+
+
+if __name__ == "__main__":
+ sys.exit(main())
diff --git a/src/uav_flight_base/scripts/uav_train_scan_policy.py b/src/uav_flight_base/scripts/uav_train_scan_policy.py
new file mode 100644
index 0000000000..283df6e1bb
--- /dev/null
+++ b/src/uav_flight_base/scripts/uav_train_scan_policy.py
@@ -0,0 +1,90 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""任务2(神经网络版)第二步:训练"扫描 -> 速度"网络。
+
+结构: 90 -> 96 (ReLU) -> 2 纯 NumPy,SGD + 动量
+输入: ~/policy_data/scan_dataset.csv
+输出: ~/policy_data/scan_policy.npz
+"""
+
+import csv
+import os
+import sys
+
+import numpy as np
+
+
+def load(path):
+ rows = list(csv.reader(open(path)))
+ head = rows[0]
+ data = np.array([[float(v) for v in r] for r in rows[1:]], dtype=np.float32)
+ n_in = len(head) - 2
+ return data[:, :n_in], data[:, -2:]
+
+
+def main():
+ root = os.path.expanduser(sys.argv[1] if len(sys.argv) > 1 else "~/policy_data")
+ epochs = int(sys.argv[2]) if len(sys.argv) > 2 else 400
+ x, y = load(os.path.join(root, "scan_dataset.csv"))
+ print("samples:", x.shape)
+
+ # 输入归一化 + 目标归一化
+ xm, xs = x.mean(0), x.std(0) + 1e-6
+ ym, ys = y.mean(0), y.std(0) + 1e-6
+ xn = (x - xm) / xs
+ yn = (y - ym) / ys
+
+ # 按时间顺序切分(避免相邻帧泄漏)
+ n = len(xn)
+ cut = int(n * 0.85)
+ xtr, ytr = xn[:cut], yn[:cut]
+ xva, yva = xn[cut:], yn[cut:]
+
+ d = x.shape[1]
+ h = 96
+ rng = np.random.RandomState(0)
+ W1 = rng.randn(d, h).astype(np.float32) * np.sqrt(2.0 / d)
+ b1 = np.zeros(h, dtype=np.float32)
+ W2 = rng.randn(h, 2).astype(np.float32) * np.sqrt(2.0 / h)
+ b2 = np.zeros(2, dtype=np.float32)
+ mW1 = np.zeros_like(W1); mb1 = np.zeros_like(b1)
+ mW2 = np.zeros_like(W2); mb2 = np.zeros_like(b2)
+
+ lr, mom, l2 = 2e-3, 0.9, 1e-5
+ best, best_pack = 1e9, None
+
+ for ep in range(epochs):
+ perm = np.random.permutation(len(xtr))
+ for i in range(0, len(perm), 64):
+ b = perm[i:i + 64]
+ xb, yb = xtr[b], ytr[b]
+ hh = np.maximum(xb @ W1 + b1, 0.0)
+ o = hh @ W2 + b2
+ e = o - yb
+ go = e * (2.0 / len(b))
+ gW2 = hh.T @ go + l2 * W2
+ gb2 = go.sum(0)
+ gh = (go @ W2.T) * (hh > 0)
+ gW1 = xb.T @ gh + l2 * W1
+ gb1 = gh.sum(0)
+ mW1 = mom * mW1 - lr * gW1; W1 += mW1
+ mb1 = mom * mb1 - lr * gb1; b1 += mb1
+ mW2 = mom * mW2 - lr * gW2; W2 += mW2
+ mb2 = mom * mb2 - lr * gb2; b2 += mb2
+
+ hv = np.maximum(xva @ W1 + b1, 0.0)
+ vl = float(np.mean((hv @ W2 + b2 - yva) ** 2))
+ if vl < best:
+ best, best_pack = vl, (W1.copy(), b1.copy(), W2.copy(), b2.copy())
+ if ep % 50 == 0 or ep == epochs - 1:
+ print("epoch %3d val %.5f" % (ep, vl))
+
+ W1, b1, W2, b2 = best_pack
+ np.savez(os.path.join(root, "scan_policy.npz"),
+ W1=W1, b1=b1, W2=W2, b2=b2, xm=xm, xs=xs, ym=ym, ys=ys)
+ print("best val %.5f -> scan_policy.npz" % best)
+ return 0
+
+
+if __name__ == "__main__":
+ sys.exit(main())
diff --git a/src/uav_flight_base/scripts/uav_visual_servo.py b/src/uav_flight_base/scripts/uav_visual_servo.py
new file mode 100644
index 0000000000..f7fcdab5db
--- /dev/null
+++ b/src/uav_flight_base/scripts/uav_visual_servo.py
@@ -0,0 +1,111 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""视觉伺服:让相机始终对准红色目标,并逐步靠近。
+
+控制律:
+ yaw : PID(横向偏差),把目标拉回画面中央
+ forward : 与目标面积误差成比例,接近到 target_area 就停
+ 丢失目标: 超过 search_after 秒 → 左右小幅扫视(±sweep_limit),不做整圈旋转
+
+启动后前 warmup 秒保持静止,避免相机还没出图就开始乱转。
+"""
+
+import rospy
+from geometry_msgs.msg import Point, Twist
+
+
+class VisualServo(object):
+ def __init__(self):
+ self.topic_target = rospy.get_param("~target_topic", "/uav/perception/target")
+ self.topic_cmd = rospy.get_param("~cmd_topic", "/cmd_vel")
+
+ # PID
+ self.kp = rospy.get_param("~kp", 1.1)
+ self.kd = rospy.get_param("~kd", 0.25)
+ self.yaw_max = rospy.get_param("~yaw_max", 0.9)
+
+ # 前进
+ self.v_max = rospy.get_param("~v_max", 1.0)
+ self.target_area = rospy.get_param("~target_area", 0.10)
+ self.stop_gain = rospy.get_param("~stop_gain", 4.0)
+
+ # 搜索
+ self.warmup = rospy.get_param("~warmup", 4.0)
+ self.search_after = rospy.get_param("~search_after", 1.5)
+ self.search_rate = rospy.get_param("~search_rate", 0.35)
+ self.sweep_limit = rospy.get_param("~sweep_limit", 1.2) # 弧度,约 ±69°
+
+ self.t_start = rospy.Time.now()
+ self.last_seen = rospy.Time.now()
+ self.prev_err = 0.0
+ self.prev_t = rospy.Time.now()
+ self.latest = None
+ self.sweep_dir = 1.0
+ self.swept = 0.0
+
+ self.pub = rospy.Publisher(self.topic_cmd, Twist, queue_size=1)
+ self.sub = rospy.Subscriber(self.topic_target, Point, self.on_target, queue_size=1)
+ self.rate = rospy.Rate(20)
+ rospy.loginfo("视觉伺服启动(warmup %.1fs)", self.warmup)
+
+ def on_target(self, msg):
+ self.latest = msg
+ if msg.z > 0.5:
+ self.last_seen = rospy.Time.now()
+ self.swept = 0.0
+
+ def run(self):
+ while not rospy.is_shutdown():
+ now = rospy.Time.now()
+ dt = max(1e-3, (now - self.prev_t).to_sec())
+ self.prev_t = now
+
+ cmd = Twist()
+
+ # 1) 暖机:不动
+ if (now - self.t_start).to_sec() < self.warmup:
+ self.pub.publish(cmd)
+ self.rate.sleep()
+ continue
+
+ seen = self.latest is not None and self.latest.z > 0.5
+ lost = (now - self.last_seen).to_sec()
+
+ if seen:
+ err = self.latest.x
+ area = self.latest.y
+ d_err = (err - self.prev_err) / dt
+ self.prev_err = err
+
+ yaw = max(-self.yaw_max, min(self.yaw_max, self.kp * err + self.kd * d_err))
+ v = max(0.0, min(self.v_max, self.stop_gain * (self.target_area - area)))
+ if abs(err) > 0.5:
+ v *= 0.3
+ cmd.angular.z = yaw
+ cmd.linear.x = v
+ else:
+ self.prev_err = 0.0
+ if lost > self.search_after:
+ # 2) 左右扫视,扫到边界就换向
+ cmd.angular.z = self.search_rate * self.sweep_dir
+ self.swept += self.search_rate * dt
+ if self.swept > self.sweep_limit:
+ self.sweep_dir *= -1.0
+ self.swept = 0.0
+ rospy.loginfo_throttle(2.0, "搜索中,方向 %+.0f", self.sweep_dir)
+ else:
+ # 刚丢失:先停一下,别乱动
+ cmd.angular.z = 0.0
+ cmd.linear.x = 0.0
+
+ self.pub.publish(cmd)
+ self.rate.sleep()
+
+
+def main():
+ rospy.init_node("uav_visual_servo")
+ VisualServo().run()
+
+
+if __name__ == "__main__":
+ main()
diff --git a/src/uav_flight_base/urdf/uav_simple.urdf b/src/uav_flight_base/urdf/uav_simple.urdf
new file mode 100644
index 0000000000..80c50765fd
--- /dev/null
+++ b/src/uav_flight_base/urdf/uav_simple.urdf
@@ -0,0 +1,139 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ true
+
+ 0 0 0 0 0 0
+ false
+ 10
+
+
+
+ 360
+ 1
+ -3.14159
+ 3.14159
+
+
+ 0.2012.0
+
+
+ /uav/scan
+ laser_link
+
+
+
+
+
+ false
+
+
+
+
+
+ 10
+
+ 1.2
+
+ 320
+ 240
+ R8G8B8
+
+ 0.0530.0
+
+
+ uav_cam
+ image_raw
+ camera_info
+ camera_link
+
+
+
+
+
+
+
+ cmd_vel
+ odom
+ odom
+ 50.0
+ true
+ true
+
+
+
+
diff --git a/src/uav_flight_base/worlds/uav_world.world b/src/uav_flight_base/worlds/uav_world.world
new file mode 100644
index 0000000000..f256890e4f
--- /dev/null
+++ b/src/uav_flight_base/worlds/uav_world.world
@@ -0,0 +1,96 @@
+
+
+
+
+
+ 0.004
+ 0.8
+
+
+ model://sun
+ model://ground_plane
+
+
+
+ true
+ 0 4.0 1.25 0 0 0
+
+ 8.0 0.3 2.5
+
+ 8.0 0.3 2.5
+ 0.55 0.56 0.6 10.72 0.73 0.76 1
+
+
+
+
+
+ true
+ 0 -4.0 1.25 0 0 0
+
+ 8.0 0.3 2.5
+
+ 8.0 0.3 2.5
+ 0.55 0.56 0.6 10.72 0.73 0.76 1
+
+
+
+
+
+ true
+ 4.0 0 1.25 0 0 0
+
+ 0.3 8.0 2.5
+
+ 0.3 8.0 2.5
+ 0.55 0.56 0.6 10.72 0.73 0.76 1
+
+
+
+
+
+ true
+ -4.0 0 1.25 0 0 0
+
+ 0.3 8.0 2.5
+
+ 0.3 8.0 2.5
+ 0.55 0.56 0.6 10.72 0.73 0.76 1
+
+
+
+
+
+
+ true
+ 0.8 -1.9 1.25 0 0 0
+
+ 0.352.5
+
+ 0.352.5
+ 0.2 0.22 0.28 10.3 0.33 0.4 1
+
+
+
+
+
+
+ true
+ 2.6 0.3 0.35 0 0 0
+
+ 0.30.7
+
+ 0.30.7
+ 0.8 0.1 0.1 10.9 0.15 0.12 1
+
+
+
+
+
+
+ -8 -8 6 0 0.35 0.78
+
+
+
+
+
+
diff --git a/src/uav_flight_base/worlds/uav_world_dense.world b/src/uav_flight_base/worlds/uav_world_dense.world
new file mode 100644
index 0000000000..66625218aa
--- /dev/null
+++ b/src/uav_flight_base/worlds/uav_world_dense.world
@@ -0,0 +1,130 @@
+
+
+
+
+
+ 0.004
+ 0.8
+
+
+ model://sun
+ model://ground_plane
+
+
+
+ true
+ 0 4.0 1.25 0 0 0
+
+ 8.0 0.3 2.5
+
+ 8.0 0.3 2.5
+ 0.55 0.56 0.6 10.72 0.73 0.76 1
+
+
+
+
+
+ true
+ 0 -4.0 1.25 0 0 0
+
+ 8.0 0.3 2.5
+
+ 8.0 0.3 2.5
+ 0.55 0.56 0.6 10.72 0.73 0.76 1
+
+
+
+
+
+ true
+ 4.0 0 1.25 0 0 0
+
+ 0.3 8.0 2.5
+
+ 0.3 8.0 2.5
+ 0.55 0.56 0.6 10.72 0.73 0.76 1
+
+
+
+
+
+ true
+ -4.0 0 1.25 0 0 0
+
+ 0.3 8.0 2.5
+
+ 0.3 8.0 2.5
+ 0.55 0.56 0.6 10.72 0.73 0.76 1
+
+
+
+
+
+
+ true
+ 1.2 1.2 1.25 0 0 0
+
+ 0.7 0.7 2.5
+ 0.7 0.7 2.5
+ 0.3 0.32 0.38 10.42 0.45 0.52 1
+
+
+
+
+ true
+ -1.4 -1.2 1.25 0 0 0
+
+ 0.7 0.7 2.5
+ 0.7 0.7 2.5
+ 0.3 0.32 0.38 10.42 0.45 0.52 1
+
+
+
+
+ true
+ 2.0 -2.0 1.25 0 0 0
+
+ 0.7 0.7 2.5
+ 0.7 0.7 2.5
+ 0.3 0.32 0.38 10.42 0.45 0.52 1
+
+
+
+
+ true
+ -2.2 2.0 1.25 0 0 0
+
+ 0.7 0.7 2.5
+ 0.7 0.7 2.5
+ 0.3 0.32 0.38 10.42 0.45 0.52 1
+
+
+
+
+ true
+ 0.0 1.8 1.25 0 0 0
+
+ 0.32.5
+ 0.32.5
+ 0.25 0.27 0.33 10.36 0.38 0.45 1
+
+
+
+
+ true
+ 0.0 -1.9 1.25 0 0 0
+
+ 0.32.5
+ 0.32.5
+ 0.25 0.27 0.33 10.36 0.38 0.45 1
+
+
+
+
+
+ -8 -8 6 0 0.35 0.78
+
+
+
+
+