diff --git a/.github/workflows/diff_drive_wall_following_tests.yml b/.github/workflows/diff_drive_wall_following_tests.yml new file mode 100644 index 0000000000..4a47feb7d9 --- /dev/null +++ b/.github/workflows/diff_drive_wall_following_tests.yml @@ -0,0 +1,61 @@ +name: Diff drive wall following tests + +on: + push: + paths: + - 'src/ground/diff_drive_wall_following/**' + - 'docs/ground/diff_drive_wall_following.md' + - 'docs/img/ground/diff_drive_wall_following/**' + - '.github/workflows/diff_drive_wall_following_tests.yml' + pull_request: + paths: + - 'src/ground/diff_drive_wall_following/**' + - 'docs/ground/diff_drive_wall_following.md' + - 'docs/img/ground/diff_drive_wall_following/**' + - '.github/workflows/diff_drive_wall_following_tests.yml' + +jobs: + test: + name: Module tests (no ROS) + runs-on: ubuntu-22.04 + steps: + - name: Checkout + uses: actions/checkout@v4 + + - name: Set up Python 3.8 + uses: actions/setup-python@v5 + with: + python-version: '3.8' + + - name: Install test dependencies + run: python3 -m pip install --upgrade pip numpy + + - name: Run local tests + run: python3 src/ground/diff_drive_wall_following/test/test_diff_drive_wall_following_local.py + + - name: Byte-compile module scripts + run: | + python3 -m py_compile \ + src/ground/diff_drive_wall_following/main.py \ + src/ground/diff_drive_wall_following/wall_follower.py \ + src/ground/diff_drive_wall_following/offline_sim.py + + docs: + name: Docs build + runs-on: ubuntu-22.04 + steps: + - name: Checkout + uses: actions/checkout@v4 + + - name: Set up Python 3.11 + uses: actions/setup-python@v5 + with: + python-version: '3.11' + + - name: Install docs dependencies + run: | + python3 -m pip install --upgrade pip + python3 -m pip install -r requirements.txt mkdocs + + - name: Build docs + run: python3 -m mkdocs build diff --git a/docs/ground/diff_drive_wall_following.md b/docs/ground/diff_drive_wall_following.md new file mode 100644 index 0000000000..935da185a5 --- /dev/null +++ b/docs/ground/diff_drive_wall_following.md @@ -0,0 +1,188 @@ +# 差速小车沿墙行驶实验 + +本实验让一台带 2D 激光雷达的差速小车沿着右侧墙壁行驶:正常段保持约 0.5 m 的墙距, +遇到墙角时原地左转,转过 90° 后继续跟随下一面墙,最终绕房间一圈回到起点。 + +## 一、实验目的与环境说明 + +### 1.1 实验目的 + +1. 掌握激光雷达数据的读取与分区:从 `LaserScan` 中提取正前、正右、右前、右后、正左五路距离; +2. 理解沿墙控制的两个误差来源:离墙距离误差、车身与墙的夹角误差; +3. 用有限状态机处理墙角:直行跟随与原地转向之间的切换; +4. 掌握差速小车 `/cmd_vel` 接口和 ROS 节点的超时保护; +5. 用离线仿真在没有机器人和 Gazebo 的情况下复现并量化控制效果。 + +### 1.2 实验环境 + +| 项目 | 配置 | +| ---- | ---- | +| 操作系统 | Ubuntu 20.04(ROS 端);Windows/Linux(离线仿真) | +| ROS 发行版 | ROS Noetic(Python 3) | +| 依赖 | rospy、geometry_msgs、sensor_msgs | +| 机器人 | 带 2D 激光雷达的差速底盘(如 TurtleBot3 burger) | +| 离线仿真依赖 | Python 3、numpy、matplotlib | + +功能包目录结构: + +```text +diff_drive_wall_following/ +├── main.py # ROS 节点入口:读 LaserScan、发 Twist +├── wall_follower.py # 纯 Python 沿墙控制器(可单独测试) +├── offline_sim.py # 矩形房间离线仿真与出图 +├── launch/main.launch # roslaunch 入口 +├── config/params.yaml # 默认参数 +├── test/test_diff_drive_wall_following_local.py # 本地测试 +├── package.xml +├── CMakeLists.txt +└── README.md +``` + +## 二、实验原理 + +### 2.1 五路测距 + +节点以机器人正前方为 0°,在五个方向各取 ±12° 范围内的最小有效距离: + +| 名称 | 角度 | 用途 | +| ---- | ---- | ---- | +| `front` | 0° | 判断是否到达墙角 | +| `side` | −90° | 右侧墙距离,沿墙的主反馈 | +| `front_right` | −45° | 与 `back_right` 一起估计车身夹角 | +| `back_right` | −135° | 与 `front_right` 一起估计车身夹角 | +| `left` | +90° | 预留,用于窄通道判断 | + +### 2.2 沿墙控制律 + +记右侧墙距离为 $d$、目标距离为 $d^*$,右前与右后距离分别为 $d_{fr}$、$d_{br}$,则 + +$$ +e_d = d - d^*, \qquad e_a = d_{fr} - d_{br} +$$ + +$$ +\omega = -\left(k_d e_d + k_a e_a\right), \qquad \omega \in [-\omega_{\max}, \omega_{\max}] +$$ + +$$ +v = v_0 \cdot \max\left(0.35,\ 1 - 0.8\frac{|\omega|}{\omega_{\max}}\right) +$$ + +- $e_d > 0$ 表示离墙太远,此时 $\omega < 0$(右转靠近墙); +- $e_a > 0$ 表示车头偏向外侧(右前比右后远),同样需要右转修正; +- 角速度越大线速度越低,转弯时不会因为速度过快而撞墙。 + +默认参数:$d^* = 0.5\ \mathrm{m}$、$k_d = 1.6$、$k_a = 1.2$、$\omega_{\max} = 0.9\ \mathrm{rad/s}$、 +$v_0 = 0.35\ \mathrm{m/s}$。 + +### 2.3 墙角状态机 + +```mermaid +stateDiagram-v2 + [*] --> FOLLOW + FOLLOW --> TURN: 前方 < 0.75 m + TURN --> TURN: 前方仍 < 1.1 m + TURN --> FOLLOW: 前方 > 1.1 m +``` + +正前方小于 `front_block_distance`(0.75 m)时认为到了墙角: +停车并原地以 0.8 rad/s 左转;等到前方重新大于 `front_clear_distance`(1.1 m), +说明已经转过墙角,回到沿墙状态。 + +### 2.4 安全策略 + +- **激光超时**:超过 `scan_timeout`(0.5 s)没有新数据时持续发布零速度; +- **速度随转角降低**:角速度接近上限时线速度降到 35%,留出修正时间; +- **退出停车**:`Ctrl+C` 或异常退出都会发布零速度。 + +## 三、运行步骤 + +### 3.1 ROS + Gazebo + +```bash +# 1. 编译 +mkdir -p ~/catkin_ws/src +cp -r <本仓库>/src/ground/diff_drive_wall_following ~/catkin_ws/src/ +chmod +x ~/catkin_ws/src/diff_drive_wall_following/main.py +cd ~/catkin_ws && catkin_make +source devel/setup.bash + +# 2. 启动仿真世界(TurtleBot3 示例) +export TURTLEBOT3_MODEL=burger +roslaunch turtlebot3_gazebo turtlebot3_world.launch + +# 3. 确认激光话题正常 +rostopic hz /scan + +# 4. 启动沿墙节点 +roslaunch diff_drive_wall_following main.launch +``` + +启动前让小车离右侧墙约 0.5 m、车头与墙平行,效果最好。 + +### 3.2 离线仿真 + +```bash +cd <本仓库> +python -m pip install numpy matplotlib +python src/ground/diff_drive_wall_following/offline_sim.py +``` + +离线仿真在 6 m × 5 m 的矩形房间内按 1° 分辨率做激光射线求交, +用差速小车运动学积分(含 0.15 s 执行器滞后)复现同样的控制器, +输出三张图和一组指标。 + +## 四、实验结果 + +离线仿真的任务参数:目标墙距 0.5 m、房间 6 m × 5 m、小车从南墙附近出发,沿右侧墙逆时针绕行一圈。 + +### 4.1 轨迹 + +![沿墙轨迹](../img/ground/diff_drive_wall_following/map_trajectory.png) + +蓝色为沿墙跟随段,红色为墙角转向段。小车依次经过南、东、北、西四面墙, +四个墙角都通过原地左转通过,最后回到起点附近。 + +### 4.2 右侧墙距离 + +![右侧墙距离](../img/ground/diff_drive_wall_following/wall_distance.png) + +直墙段距离稳定在 0.5 m 附近;经过墙角时右侧墙暂时“消失”,距离升到约 0.75 m, +转过墙角后重新收敛到 0.5 m。 + +### 4.3 速度指令 + +![速度曲线](../img/ground/diff_drive_wall_following/velocity.png) + +直墙段线速度约 0.35 m/s、角速度接近 0;墙角处线速度降到 0,角速度升到 0.8 rad/s; +转过墙角后出现一次反向修正,随后回到沿墙状态。 + +### 4.4 指标 + +| 指标 | 数值 | +| ---- | ---- | +| 绕行一圈时间 | 60.95 s | +| 路径长度 | 17.06 m | +| 平均跟墙误差 | 0.031 m | +| 最大跟墙误差 | 0.273 m(墙角段) | +| 最小前方净空 | 0.698 m | +| 终点位置 | (1.007, 0.518) m | + +平均误差约 3 cm,主要来自直墙段的控制噪声;最大误差出现在墙角, +这是“转角时右侧墙暂时丢失”的必然结果。最小前方净空 0.698 m 略小于 +0.75 m 的触发阈值,是执行器滞后造成的,实际机器人需要保留更多余量。 + +## 五、讨论与限制 + +1. 控制器只有比例项,没有积分项,直墙段会残留几厘米的稳态误差; +2. 算法假设右侧始终有一面连续的墙,遇到门洞、玻璃或动态障碍会失效, + 工程上通常再叠加前向避障或局部地图; +3. 转角依赖固定的 0.75 m / 1.1 m 阈值,房间尺寸或激光安装位置变化时需要重新调整; +4. 离线仿真是理想矩形房间 + 简化运动学,不含轮子打滑、激光噪声模型和真实动力学; +5. 本机为 Windows,没有 ROS 与 Gazebo,**ROS 端的实际运行未验证**, + 需要在 ROS Noetic + TurtleBot3 仿真或实车上按 3.1 节复验。 + +## 说明 + +本文档与配套代码在整理过程中使用了 AI 助手(DeepSeek)辅助,实验参数、运行结果 +和图表均由作者实际运行核对;作者对本文内容负全部责任。 diff --git a/docs/img/ground/diff_drive_wall_following/map_trajectory.png b/docs/img/ground/diff_drive_wall_following/map_trajectory.png new file mode 100644 index 0000000000..fca6760876 Binary files /dev/null and b/docs/img/ground/diff_drive_wall_following/map_trajectory.png differ diff --git a/docs/img/ground/diff_drive_wall_following/velocity.png b/docs/img/ground/diff_drive_wall_following/velocity.png new file mode 100644 index 0000000000..76fc247b15 Binary files /dev/null and b/docs/img/ground/diff_drive_wall_following/velocity.png differ diff --git a/docs/img/ground/diff_drive_wall_following/wall_distance.png b/docs/img/ground/diff_drive_wall_following/wall_distance.png new file mode 100644 index 0000000000..06075f6ffc Binary files /dev/null and b/docs/img/ground/diff_drive_wall_following/wall_distance.png differ diff --git a/docs/index.md b/docs/index.md index 892845a340..a18ec76d05 100644 --- a/docs/index.md +++ b/docs/index.md @@ -225,6 +225,7 @@ ROS 2 Action 斐波那契数列计算示例:[chap2/fibonacci_action](./chap2/f * [激光雷达点云语义分割](./ground/carla_lidar_pointcloud_segmentation.md) * [点云地图创建](./ground/pcl_recorder.md) * [定点停车](./ground/fixed_point_stop.md) +* [差速小车沿墙行驶](./ground/diff_drive_wall_following.md) —— 从 LaserScan 取五路距离,用距离误差和车身夹角误差控制 /cmd_vel,墙角自动原地左转,附离线仿真与跟墙误差指标 * [ROS与Conda环境排错指南](./ground/ros_conda_env_guide.md) * [打靶](./ground/mujoco_shooting.md) * [Carla 交通标志识别(Windows 脚本)](./ground/carla_traffic_sign_recognition.md) - 基于 YOLOv8 的实时感知与键盘控制 diff --git a/mkdocs.yml b/mkdocs.yml index d7dbc60e0a..4d63cd5cbb 100644 --- a/mkdocs.yml +++ b/mkdocs.yml @@ -212,6 +212,7 @@ nav: - 交通信号灯识别与响应: './ground/carla_traffic_light.md' - RVIZ Carla 插件: './ground/rviz_plugin.md' - 定点停车: './ground/fixed_point_stop.md' + - 差速小车沿墙行驶: './ground/diff_drive_wall_following.md' - 网页远程遥控小车: './ground/web_remote_car.md' - 基于MuJoCo的AEB自动紧急制动仿真: './ground/mujoco_vehicle_aeb.md' - 打靶: './ground/mujoco_shooting.md' diff --git a/src/ground/diff_drive_wall_following/CMakeLists.txt b/src/ground/diff_drive_wall_following/CMakeLists.txt new file mode 100644 index 0000000000..6ce9b60791 --- /dev/null +++ b/src/ground/diff_drive_wall_following/CMakeLists.txt @@ -0,0 +1,23 @@ +cmake_minimum_required(VERSION 3.0.2) +project(diff_drive_wall_following) + +# 本模块使用 rospy、geometry_msgs(速度指令)和 sensor_msgs(激光数据) +find_package(catkin REQUIRED COMPONENTS + rospy + geometry_msgs + sensor_msgs +) + +catkin_package() + +catkin_install_python(PROGRAMS + main.py + wall_follower.py + offline_sim.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(DIRECTORY launch config + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + USE_SOURCE_PERMISSIONS +) diff --git a/src/ground/diff_drive_wall_following/README.md b/src/ground/diff_drive_wall_following/README.md new file mode 100644 index 0000000000..050f997877 --- /dev/null +++ b/src/ground/diff_drive_wall_following/README.md @@ -0,0 +1,88 @@ +# diff_drive_wall_following — 差速小车沿墙行驶 + +订阅 `/scan`(`sensor_msgs/LaserScan`),取正前、正右、右前、右后、正左五路距离, +用「右侧距离误差 + 车身与墙夹角误差」计算速度指令并发布到 `/cmd_vel` +(`geometry_msgs/Twist`)。正前方太近时进入转角状态,原地左转,转过墙角后继续跟随。 +配套教程见 [docs/ground/diff_drive_wall_following.md](../../docs/ground/diff_drive_wall_following.md)。 + +## 运行环境 + +| 项目 | 配置 | +| ---- | ---- | +| 操作系统 | Ubuntu 20.04(ROS 节点) | +| ROS | ROS Noetic(Python 3) | +| 依赖 | rospy、geometry_msgs、sensor_msgs | +| 机器人 | 带 2D 激光雷达的差速底盘(如 TurtleBot3 burger) | +| 离线验证 | Windows / Linux,Python 3 + numpy + matplotlib(不需要 ROS) | + +## 运行步骤 + +### 方式一:ROS + Gazebo(以 TurtleBot3 为例) + +```bash +# 1. 拷贝到工作空间并编译 +mkdir -p ~/catkin_ws/src +cp -r <本仓库>/src/ground/diff_drive_wall_following ~/catkin_ws/src/ +chmod +x ~/catkin_ws/src/diff_drive_wall_following/main.py +cd ~/catkin_ws && catkin_make +source devel/setup.bash + +# 2. 启动带激光雷达的仿真世界 +export TURTLEBOT3_MODEL=burger +roslaunch turtlebot3_gazebo turtlebot3_world.launch + +# 3. 确认激光话题有数据 +rostopic hz /scan + +# 4. 启动沿墙节点 +roslaunch diff_drive_wall_following main.launch +``` + +把小车放在离墙约 0.5 m、车头与墙平行的位置再启动,跟随效果最好。 + +### 方式二:离线仿真(不需要 ROS,用于快速验证控制逻辑) + +```bash +cd <本仓库> +python -m pip install numpy matplotlib +python src/ground/diff_drive_wall_following/offline_sim.py +``` + +脚本在 6 m × 5 m 的矩形房间里模拟 1° 分辨率激光和差速小车运动学, +让小车沿右侧墙逆时针绕行一圈,输出三张图和一组指标。 + +## 运行测试 + +```bash +cd <本仓库> +python3 src/ground/diff_drive_wall_following/test/test_diff_drive_wall_following_local.py +``` + +测试不需要 ROS / Gazebo,覆盖沿墙控制、墙角状态切换、速度限幅和工程文件; +GitHub Actions 会在 `src/ground/diff_drive_wall_following/**` 变更时自动运行该测试并构建文档。 + +## 参数 + +| 参数 | 默认值 | 说明 | +| ---- | ---- | ---- | +| `~target_distance` | 0.5 | 期望的右侧墙距离(m) | +| `~k_distance` | 1.6 | 距离误差到角速度的比例系数 | +| `~k_align` | 1.2 | 车身与墙夹角误差到角速度的比例系数 | +| `~max_angular` | 0.9 | 最大角速度(rad/s) | +| `~base_speed` | 0.35 | 沿墙时的基础线速度(m/s) | +| `~turn_speed` | 0.8 | 墙角原地转向角速度(rad/s) | +| `~front_block_distance` | 0.75 | 前方小于该距离判定为墙角(m) | +| `~front_clear_distance` | 1.1 | 前方大于该距离判定转角完成(m) | +| `~scan_timeout` | 0.5 | 激光超时(s),超时后停车 | + +## 话题接口 + +| 方向 | 话题 | 类型 | 说明 | +| ---- | ---- | ---- | ---- | +| 订阅 | `/scan` | `sensor_msgs/LaserScan` | 2D 激光雷达 | +| 发布 | `/cmd_vel` | `geometry_msgs/Twist` | `linear.x` 线速度、`angular.z` 角速度 | + +## 预期效果 + +小车保持右侧墙约 0.5 m 的间距前进,遇到墙角时原地左转,转过 90° 后继续跟随下一面墙, +最终绕房间一圈回到起点;离线仿真中平均跟墙误差约 3 cm(详细数据见文档)。 diff --git a/src/ground/diff_drive_wall_following/config/params.yaml b/src/ground/diff_drive_wall_following/config/params.yaml new file mode 100644 index 0000000000..e63d4c6db8 --- /dev/null +++ b/src/ground/diff_drive_wall_following/config/params.yaml @@ -0,0 +1,11 @@ +# 沿墙行驶参数(launch 中作为节点私有参数加载) +target_distance: 0.5 # 期望的右侧墙距离(m) +k_distance: 1.6 # 距离误差 -> 角速度的比例系数 +k_align: 1.2 # 车身与墙夹角误差 -> 角速度的比例系数 +max_angular: 0.9 # 最大角速度(rad/s) +base_speed: 0.35 # 沿墙时的基础线速度(m/s) +turn_speed: 0.8 # 墙角原地转向的角速度(rad/s) +front_block_distance: 0.75 # 前方小于该距离判定为墙角(m) +front_clear_distance: 1.1 # 前方大于该距离判定转角完成(m) +control_rate: 10.0 # 控制频率(Hz) +scan_timeout: 0.5 # 激光数据超时(s),超时后停车 diff --git a/src/ground/diff_drive_wall_following/launch/main.launch b/src/ground/diff_drive_wall_following/launch/main.launch new file mode 100644 index 0000000000..39ef8d3e84 --- /dev/null +++ b/src/ground/diff_drive_wall_following/launch/main.launch @@ -0,0 +1,13 @@ + + + + + + + + + + + + diff --git a/src/ground/diff_drive_wall_following/main.py b/src/ground/diff_drive_wall_following/main.py new file mode 100644 index 0000000000..4616b1cbe1 --- /dev/null +++ b/src/ground/diff_drive_wall_following/main.py @@ -0,0 +1,151 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +"""差速小车沿墙行驶 ROS 节点(模块入口 main.py)。 + +订阅 /scan(sensor_msgs/LaserScan),从激光数据中取正前、正右、右前、右后、 +正左五路距离,交给 wall_follower.py 的控制器,发布 /cmd_vel(Twist)。 + +运行: + rosrun diff_drive_wall_following main.py + roslaunch diff_drive_wall_following main.launch +""" +from __future__ import print_function + +import math + +import rospy +from geometry_msgs.msg import Twist +from sensor_msgs.msg import LaserScan + +from wall_follower import STATE_FOLLOW, STATE_NAMES, STATE_TURN, WallFollower + +# 五路测距对应的机器人坐标系角度(rad,0 为正前方,逆时针为正) +BEAM_ANGLES = { + 'front': 0.0, + 'side': -math.pi / 2, + 'front_right': -math.pi / 4, + 'back_right': -3 * math.pi / 4, + 'left': math.pi / 2, +} +WINDOW = math.radians(12) # 每路取 ±12° 内的最小距离,抗噪 + + +class WallFollowingNode(object): + + def __init__(self): + self.scan_topic = rospy.get_param('~scan_topic', '/scan') + self.cmd_topic = rospy.get_param('~cmd_topic', '/cmd_vel') + self.scan_timeout = rospy.get_param('~scan_timeout', 0.5) + + self.follower = WallFollower( + target_distance=rospy.get_param('~target_distance', 0.5), + k_distance=rospy.get_param('~k_distance', 1.6), + k_align=rospy.get_param('~k_align', 1.2), + max_angular=rospy.get_param('~max_angular', 0.9), + base_speed=rospy.get_param('~base_speed', 0.35), + turn_speed=rospy.get_param('~turn_speed', 0.8), + front_block_distance=rospy.get_param('~front_block_distance', 0.75), + front_clear_distance=rospy.get_param('~front_clear_distance', 1.1), + ) + + self.distances = None + self.last_scan_time = None + self.last_time = None + self.last_state = None + + self.pub = rospy.Publisher(self.cmd_topic, Twist, queue_size=10) + rospy.Subscriber(self.scan_topic, LaserScan, self.on_scan) + rospy.loginfo('沿墙行驶节点已启动:scan=%s -> cmd=%s', + self.scan_topic, self.cmd_topic) + + def _beam_distance(self, scan, angle): + """在 angle 附近 [-WINDOW, +WINDOW] 内取最小有效距离。""" + best = float('inf') + start = scan.angle_min + inc = scan.angle_increment + count = len(scan.ranges) + if count == 0 or inc == 0.0: + return best + + for i in range(count): + a = start + i * inc + # 把角度差归一化到 [-pi, pi] + diff = math.atan2(math.sin(a - angle), math.cos(a - angle)) + if abs(diff) > WINDOW: + continue + r = scan.ranges[i] + if r is None or r != r: # None / NaN + continue + if r < scan.range_min or r > scan.range_max: + continue + if r < best: + best = r + return best + + def on_scan(self, msg): + self.distances = dict((name, self._beam_distance(msg, angle)) + for name, angle in BEAM_ANGLES.items()) + self.last_scan_time = rospy.Time.now() + + def publish(self, v, w): + # 控制器已对缺失测距做兜底;这里再挡一层,保证 cmd_vel 不会出现 NaN/inf + if not math.isfinite(v): + v = 0.0 + if not math.isfinite(w): + w = 0.0 + msg = Twist() + msg.linear.x = v + msg.angular.z = w + self.pub.publish(msg) + + def run(self): + rate = rospy.Rate(rospy.get_param('~control_rate', 10.0)) + self.last_time = rospy.Time.now() + + while not rospy.is_shutdown(): + now = rospy.Time.now() + dt = (now - self.last_time).to_sec() + self.last_time = now + + if self.last_scan_time is None: + rate.sleep() + continue + + if (now - self.last_scan_time).to_sec() > self.scan_timeout: + self.publish(0.0, 0.0) + rospy.logwarn_throttle(2.0, '超过 %.1f 秒没有收到激光数据,已停车', + self.scan_timeout) + rate.sleep() + continue + + d = self.distances + v, w, state = self.follower.update( + d['front'], d['side'], d['front_right'], d['back_right'], + d['left'], dt) + self.publish(v, w) + + if state != self.last_state: + rospy.loginfo('状态切换:%s -> %s', + STATE_NAMES.get(self.last_state, '无'), + STATE_NAMES.get(state, state)) + self.last_state = state + + rate.sleep() + + self.publish(0.0, 0.0) + + +def main(): + rospy.init_node('diff_drive_wall_following') + node = WallFollowingNode() + try: + node.run() + except rospy.ROSInterruptException: + pass + finally: + node.publish(0.0, 0.0) + rospy.loginfo('节点退出,已发布零速度') + + +if __name__ == '__main__': + main() diff --git a/src/ground/diff_drive_wall_following/offline_sim.py b/src/ground/diff_drive_wall_following/offline_sim.py new file mode 100644 index 0000000000..0c45dbc1f9 --- /dev/null +++ b/src/ground/diff_drive_wall_following/offline_sim.py @@ -0,0 +1,238 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +"""差速小车沿墙行驶的离线仿真与出图(不需要 ROS / Gazebo)。 + +用法(在仓库根目录执行): + python src/ground/diff_drive_wall_following/offline_sim.py + +仿真内容: +- 一个 6 m x 5 m 的矩形房间,小车从南墙附近出发,沿右侧墙壁逆时针绕行一圈; +- 按真实激光雷达的角度分辨率(1°)做射线求交,取五路最小距离; +- 差速小车运动学积分 + 执行器一阶滞后,模拟速度指令到实际速度的延迟。 + +输出: + docs/img/ground/diff_drive_wall_following/map_trajectory.png + docs/img/ground/diff_drive_wall_following/wall_distance.png + docs/img/ground/diff_drive_wall_following/velocity.png +""" +from __future__ import print_function + +import json +import math +import os +import sys + +import numpy as np + +HERE = os.path.dirname(os.path.abspath(__file__)) +REPO_ROOT = os.path.abspath(os.path.join(HERE, '..', '..', '..')) +sys.path.insert(0, HERE) + +from wall_follower import STATE_FOLLOW, STATE_TURN, WallFollower # noqa: E402 + +OUTPUT_DIR = os.path.join(REPO_ROOT, 'docs', 'img', 'ground', 'diff_drive_wall_following') + +ROOM_WIDTH = 6.0 +ROOM_HEIGHT = 5.0 +START = (1.2, 0.5, 0.0) # x, y, heading +DT = 0.05 +TAU = 0.15 # 执行器时间常数 +MAX_TIME = 120.0 +BEAM_RESOLUTION = math.radians(1.0) +MAX_RANGE = 8.0 + +BEAM_CENTERS = { + 'front': 0.0, + 'side': -math.pi / 2, + 'front_right': -math.pi / 4, + 'back_right': -3 * math.pi / 4, + 'left': math.pi / 2, +} +WINDOW = math.radians(12) + + +def ray_distance(x, y, angle): + """从 (x, y) 沿 angle 发射射线,返回到矩形房间四壁的最近距离。""" + dx, dy = math.cos(angle), math.sin(angle) + best = MAX_RANGE + if dx > 1e-9: + best = min(best, (ROOM_WIDTH - x) / dx) + elif dx < -1e-9: + best = min(best, (0.0 - x) / dx) + if dy > 1e-9: + best = min(best, (ROOM_HEIGHT - y) / dy) + elif dy < -1e-9: + best = min(best, (0.0 - y) / dy) + return max(0.0, best) + + +def scan(x, y, heading, noise_std=0.0, rng=None): + """模拟一圈 1° 激光,返回五路最小距离。""" + ranges = {} + for name, center in BEAM_CENTERS.items(): + best = MAX_RANGE + n = int(round(WINDOW / BEAM_RESOLUTION)) + for k in range(-n, n + 1): + a = heading + center + k * BEAM_RESOLUTION + d = ray_distance(x, y, a) + if noise_std > 0 and rng is not None: + d += rng.normal(0.0, noise_std) + if d < best: + best = d + ranges[name] = best + return ranges + + +def simulate(noise_std=0.01, seed=7, follower_kwargs=None): + rng = np.random.RandomState(seed) + follower = WallFollower(**(follower_kwargs or {})) + + x, y, heading = START + v = 0.0 + w = 0.0 + + times = [0.0] + xs = [x] + ys = [y] + headings = [heading] + vs = [v] + ws = [w] + sides = [np.nan] + fronts = [np.nan] + states = [follower.state] + + t = 0.0 + lap_time = None + traveled = 0.0 + while t < MAX_TIME: + ranges = scan(x, y, heading, noise_std=noise_std, rng=rng) + cmd_v, cmd_w, state = follower.update( + ranges['front'], ranges['side'], ranges['front_right'], + ranges['back_right'], ranges['left'], DT) + + # 执行器一阶滞后 + alpha = min(DT / TAU, 1.0) + v += (cmd_v - v) * alpha + w += (cmd_w - w) * alpha + + x += v * math.cos(heading) * DT + y += v * math.sin(heading) * DT + heading += w * DT + + traveled += abs(v) * DT + t += DT + + times.append(t) + xs.append(x) + ys.append(y) + headings.append(heading) + vs.append(v) + ws.append(w) + sides.append(ranges['side']) + fronts.append(ranges['front']) + states.append(state) + + if lap_time is None and t > 10.0 and traveled > 14.0: + heading_error = abs(math.atan2(math.sin(heading - START[2]), + math.cos(heading - START[2]))) + if x > 1.0 and y < 1.2 and heading_error < 0.3: + lap_time = t + break + + return { + 't': np.array(times), 'x': np.array(xs), 'y': np.array(ys), + 'heading': np.array(headings), 'v': np.array(vs), 'w': np.array(ws), + 'side': np.array(sides), 'front': np.array(fronts), + 'state': np.array(states), 'lap_time': lap_time, 'traveled': traveled, + } + + +def metrics(data): + t = data['t'] + side = data['side'] + states = data['state'] + follow = states == STATE_FOLLOW + err = np.abs(side[follow] - 0.5) + err = err[~np.isnan(err)] + return { + 'lap_time_s': data['lap_time'], + 'path_length_m': float(data['traveled']), + 'mean_wall_error_m': float(err.mean()) if len(err) else float('nan'), + 'max_wall_error_m': float(err.max()) if len(err) else float('nan'), + 'min_front_clearance_m': float(np.nanmin(data['front'])), + 'final_position_m': [float(data['x'][-1]), float(data['y'][-1])], + } + + +def plot(data): + import matplotlib + matplotlib.use('Agg') + import matplotlib.pyplot as plt + from matplotlib.patches import Rectangle + + os.makedirs(OUTPUT_DIR, exist_ok=True) + + # 1) 地图与轨迹 + fig, ax = plt.subplots(figsize=(7, 5.5)) + ax.add_patch(Rectangle((0, 0), ROOM_WIDTH, ROOM_HEIGHT, fill=False, + edgecolor='black', linewidth=2)) + colors = {STATE_FOLLOW: ('FOLLOW', '#1f77b4'), + STATE_TURN: ('TURN', '#d62728')} + for state, (label, color) in colors.items(): + mask = data['state'] == state + if mask.any(): + ax.plot(data['x'][mask], data['y'][mask], color=color, linewidth=2, + label=label) + ax.plot(START[0], START[1], 'go', markersize=8, label='start') + ax.plot(data['x'][-1], data['y'][-1], 'rx', markersize=10, label='end') + ax.set_xlim(-0.4, ROOM_WIDTH + 0.4) + ax.set_ylim(-0.4, ROOM_HEIGHT + 0.4) + ax.set_aspect('equal') + ax.set_xlabel('x (m)') + ax.set_ylabel('y (m)') + ax.set_title('Wall-following trajectory (right-hand rule)') + ax.grid(True, alpha=0.3) + ax.legend(loc='upper right', fontsize=8) + fig.tight_layout() + fig.savefig(os.path.join(OUTPUT_DIR, 'map_trajectory.png'), dpi=150) + plt.close(fig) + + # 2) 右侧墙距离 + fig, ax = plt.subplots(figsize=(8, 4.2)) + ax.plot(data['t'], data['side'], color='#1f77b4', linewidth=1.5, label='right wall distance') + ax.axhline(0.5, color='gray', linestyle='--', linewidth=1, label='target 0.5 m') + ax.set_xlabel('time (s)') + ax.set_ylabel('distance (m)') + ax.set_title('Distance to the right wall') + ax.grid(True, alpha=0.3) + ax.legend(loc='upper right') + fig.tight_layout() + fig.savefig(os.path.join(OUTPUT_DIR, 'wall_distance.png'), dpi=150) + plt.close(fig) + + # 3) 速度 + fig, ax = plt.subplots(figsize=(8, 4.2)) + ax.plot(data['t'], data['v'], label='linear velocity v (m/s)') + ax.plot(data['t'], data['w'], label='angular velocity w (rad/s)') + ax.set_xlabel('time (s)') + ax.set_ylabel('command') + ax.set_title('Velocity commands while following the wall') + ax.grid(True, alpha=0.3) + ax.legend(loc='upper right') + fig.tight_layout() + fig.savefig(os.path.join(OUTPUT_DIR, 'velocity.png'), dpi=150) + plt.close(fig) + + +def main(): + data = simulate() + result = metrics(data) + plot(data) + print(json.dumps(result, ensure_ascii=False, indent=2)) + for name in ('map_trajectory.png', 'wall_distance.png', 'velocity.png'): + print('figure:', os.path.join(OUTPUT_DIR, name)) + return 0 + + +if __name__ == '__main__': + sys.exit(main()) diff --git a/src/ground/diff_drive_wall_following/package.xml b/src/ground/diff_drive_wall_following/package.xml new file mode 100644 index 0000000000..e113a702fd --- /dev/null +++ b/src/ground/diff_drive_wall_following/package.xml @@ -0,0 +1,19 @@ + + + diff_drive_wall_following + 0.1.0 + + 差速小车沿右侧墙壁行驶模块:订阅 /scan(LaserScan),取正前、正右、 + 右前、右后、正左五路距离,用距离误差与车身夹角误差计算 /cmd_vel + (Twist),实现沿墙跟随和墙角转向。配套文档见 + docs/ground/diff_drive_wall_following.md。 + + xinyangtang91 + MIT + + catkin + + rospy + geometry_msgs + sensor_msgs + diff --git a/src/ground/diff_drive_wall_following/test/test_diff_drive_wall_following_local.py b/src/ground/diff_drive_wall_following/test/test_diff_drive_wall_following_local.py new file mode 100644 index 0000000000..9839c460d0 --- /dev/null +++ b/src/ground/diff_drive_wall_following/test/test_diff_drive_wall_following_local.py @@ -0,0 +1,145 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +"""diff_drive_wall_following 本地测试(不需要 ROS / Gazebo / 激光雷达). + +A. 沿墙控制:距离保持、夹角修正、墙角转向状态切换 +B. 限幅与丢墙保护 +C. 工程文件:package.xml / CMakeLists.txt / main.launch / README / 文档 / 图片 + +用法: python3 src/ground/diff_drive_wall_following/test/test_diff_drive_wall_following_local.py +""" +from __future__ import annotations + +import math +import os +import sys +import xml.etree.ElementTree as ET + +PASS, FAIL = [], [] + + +def check(cond, label): + (PASS if cond else FAIL).append(label) + print(" [%s] %s" % ("PASS" if cond else "FAIL", label)) + + +HERE = os.path.dirname(os.path.abspath(__file__)) +PKG = os.path.abspath(os.path.join(HERE, "..")) +ROOT = os.path.abspath(os.path.join(PKG, "..", "..", "..")) +sys.path.insert(0, PKG) + +from wall_follower import ( # noqa: E402 + STATE_FOLLOW, STATE_TURN, WallFollower, +) + + +# ================================================================= A. 沿墙控制 +def test_follow(): + print("== A. 沿墙控制 ==") + follower = WallFollower() + + v, w, state = follower.update(2.0, 0.5, 0.5, 0.5, 2.0, 0.1) + check(state == STATE_FOLLOW, "正前方无遮挡时为 FOLLOW") + check(abs(w) < 1e-6, "距离和夹角都无误差时角速度约为 0") + check(0.0 < v <= 0.4, "正常跟随时有前进速度(%.2f m/s)" % v) + + follower = WallFollower() + _, w_far, _ = follower.update(2.0, 0.8, 0.8, 0.8, 2.0, 0.1) + check(w_far < 0.0, "离墙太远时向右转(w<0,%.2f)" % w_far) + + follower = WallFollower() + _, w_near, _ = follower.update(2.0, 0.3, 0.3, 0.3, 2.0, 0.1) + check(w_near > 0.0, "离墙太近时向左转(w>0,%.2f)" % w_near) + + follower = WallFollower() + _, w_angle, _ = follower.update(2.0, 0.5, 0.6, 0.4, 2.0, 0.1) + check(w_angle < 0.0, "车头偏向外侧时向右修正(w<0,%.2f)" % w_angle) + + +def test_corner_and_limits(): + print("== B. 墙角转向、限幅与丢墙 ==") + follower = WallFollower() + v, w, state = follower.update(0.5, 0.5, 0.5, 0.5, 0.5, 0.1) + check(state == STATE_TURN, "正前方小于阈值时进入 TURN") + check(v == 0.0 and w > 0.0, "转角时停车并原地左转") + + v, w, state = follower.update(0.9, 0.5, 0.5, 0.5, 2.0, 0.1) + check(state == STATE_TURN, "前方仍不够空时保持 TURN") + + v, w, state = follower.update(1.2, 0.5, 0.5, 0.5, 2.0, 0.1) + check(state == STATE_FOLLOW, "转过墙角后回到 FOLLOW") + + follower = WallFollower(max_angular=0.6) + _, w, _ = follower.update(2.0, 3.0, 3.0, 3.0, 2.0, 0.1) + check(abs(w) <= 0.6 + 1e-9, "角速度被限制在 max_angular 以内(%.2f)" % w) + + follower = WallFollower() + v, w, _ = follower.update(2.0, 5.0, 5.0, 5.0, 2.0, 0.1) + check(v > 0.0 and w < 0.0, "右侧丢墙时低速右转寻找墙壁") + + follower = WallFollower() + v, w, _ = follower.update(2.0, 0.5, float("inf"), float("inf"), 2.0, 0.1) + check(math.isfinite(v) and math.isfinite(w), + "前右/后右同时无回波时不输出 NaN(v=%s, w=%s)" % (v, w)) + check(abs(w) < 1e-6, "两路缺失时用 side 兜底,夹角误差为 0") + + follower = WallFollower() + _, w, _ = follower.update(2.0, 0.5, 0.8, float("inf"), 2.0, 0.1) + check(math.isfinite(w) and w < 0.0, + "单路缺失时用 side 兜底并保持有限输出(w=%.2f)" % w) + + follower = WallFollower() + v, w, _ = follower.update(2.0, float("inf"), float("inf"), float("inf"), 2.0, 0.1) + check(math.isfinite(v) and math.isfinite(w), + "右侧完全丢墙时输出仍有限(v=%.2f, w=%.2f)" % (v, w)) + + +# ================================================================= C. 工程文件 +def test_project_files(): + print("== C. 工程文件 ==") + pkg = ET.parse(os.path.join(PKG, "package.xml")).getroot() + check(pkg.findtext("name").strip() == "diff_drive_wall_following", "package.xml 包名正确") + deps = [d.text.strip() for d in pkg.findall("depend")] + for dep in ("rospy", "geometry_msgs", "sensor_msgs"): + check(dep in deps, "package.xml 声明依赖 %s" % dep) + + cmake = open(os.path.join(PKG, "CMakeLists.txt"), encoding="utf-8").read() + for name in ("main.py", "wall_follower.py", "offline_sim.py"): + check(name in cmake, "CMakeLists.txt 安装 %s" % name) + + launch = ET.parse(os.path.join(PKG, "launch", "main.launch")).getroot() + nodes = [n.get("type") for n in launch.findall("node")] + check("main.py" in nodes, "main.launch 启动 main.py") + args = [a.get("name") for a in launch.findall("arg")] + for name in ("scan_topic", "cmd_topic"): + check(name in args, "main.launch 暴露参数 %s" % name) + + check(os.path.exists(os.path.join(PKG, "README.md")), "README.md 存在") + check(os.path.exists(os.path.join( + ROOT, ".github", "workflows", "diff_drive_wall_following_tests.yml")), + "CI workflow 存在") + for rel in ( + "docs/ground/diff_drive_wall_following.md", + "docs/img/ground/diff_drive_wall_following/map_trajectory.png", + "docs/img/ground/diff_drive_wall_following/wall_distance.png", + "docs/img/ground/diff_drive_wall_following/velocity.png", + ): + check(os.path.exists(os.path.join(ROOT, rel)), "文件存在:%s" % rel) + + +def main(): + test_follow() + test_corner_and_limits() + test_project_files() + print("\n==================== 结果 ====================") + print("通过 %d 项,失败 %d 项" % (len(PASS), len(FAIL))) + if FAIL: + for f in FAIL: + print(" FAIL: %s" % f) + return 1 + print("全部通过") + return 0 + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/src/ground/diff_drive_wall_following/wall_follower.py b/src/ground/diff_drive_wall_following/wall_follower.py new file mode 100644 index 0000000000..54353e8971 --- /dev/null +++ b/src/ground/diff_drive_wall_following/wall_follower.py @@ -0,0 +1,100 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +"""差速小车沿右侧墙壁行驶的控制器(纯 Python,不依赖 ROS)。 + +控制器只接收几路激光测距值,输出差速小车的线速度 v 和角速度 w: + + front 正前方最近距离 + side 正右方最近距离(跟随的目标墙) + front_right 右前方 45° 左右的距离 + back_right 右后方 45° 左右的距离 + left 正左方最近距离(预留,用于窄通道判断) + +沿墙策略: +1. 正常跟随时,用“右侧距离误差”把车拉回目标距离; +2. 用“右前 / 右后距离差”估计车身与墙的夹角,夹角偏了就转向修正; +3. 正前方太近说明到了墙角,原地左转,直到前方重新变空再继续跟随。 + +角速度约定:逆时针为正(左转为正)。 +""" +from __future__ import print_function + +import math + +STATE_FOLLOW = 'FOLLOW' +STATE_TURN = 'TURN' + +STATE_NAMES = { + STATE_FOLLOW: '沿墙', + STATE_TURN: '转角', +} + + +def _clamp(value, limit): + if value > limit: + return limit + if value < -limit: + return -limit + return value + + +class WallFollower(object): + """右侧沿墙控制器。""" + + def __init__(self, target_distance=0.5, k_distance=1.6, k_align=1.2, + max_angular=0.9, base_speed=0.35, turn_speed=0.8, + front_block_distance=0.75, front_clear_distance=1.1, + min_speed_factor=0.35, lost_range=3.0): + self.target_distance = float(target_distance) + self.k_distance = float(k_distance) + self.k_align = float(k_align) + self.max_angular = float(max_angular) + self.base_speed = float(base_speed) + self.turn_speed = float(turn_speed) + self.front_block_distance = float(front_block_distance) + self.front_clear_distance = float(front_clear_distance) + self.min_speed_factor = float(min_speed_factor) + self.lost_range = float(lost_range) + + self.state = STATE_FOLLOW + + def reset(self): + self.state = STATE_FOLLOW + + def update(self, front, side, front_right, back_right, left, dt): + """返回 (v, w, state)。dt 目前未参与计算,保留以保持接口一致。""" + if front < self.front_block_distance: + # 前方是墙:停车并原地左转 + self.state = STATE_TURN + return 0.0, self.turn_speed, self.state + + if self.state == STATE_TURN: + if front < self.front_clear_distance: + return 0.0, self.turn_speed, self.state + self.state = STATE_FOLLOW + + if not math.isfinite(side) or side >= self.lost_range: + # 右侧没有可跟随的墙,缓慢右转寻找 + return self.base_speed * self.min_speed_factor, -0.4, self.state + + distance_error = side - self.target_distance # 正:离墙太远 + + # 激光某一路没有有效回波时返回 inf;两路都缺失会得到 inf-inf=NaN。 + # 这里用侧向墙距兜底缺失的一路,仍无法兜底时跳过夹角修正。 + if not math.isfinite(front_right): + front_right = side + if not math.isfinite(back_right): + back_right = side + if math.isfinite(front_right) and math.isfinite(back_right): + align_error = front_right - back_right # 正:车头偏向外侧 + else: + align_error = 0.0 + + w = -(self.k_distance * distance_error + self.k_align * align_error) + w = _clamp(w, self.max_angular) + + speed_factor = 1.0 - 0.8 * abs(w) / self.max_angular + if speed_factor < self.min_speed_factor: + speed_factor = self.min_speed_factor + v = self.base_speed * speed_factor + return v, w, self.state