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 式在线纠正来改善。 + +![场景A](images/policy_sceneA.png) + +![场景B](images/policy_sceneB.png) + +### 网络结构与优化器对比 + +课程要求“同一模型比较不同算法、同一算法比较不同网络结构”。在固定数据(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 的停滞现象互为印证——瓶颈在数据分布覆盖,而非模型容量。 + +![结构对比](images/policy_width_sweep.png) + + 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 + + + + +