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 轨迹
+
+
+
+蓝色为沿墙跟随段,红色为墙角转向段。小车依次经过南、东、北、西四面墙,
+四个墙角都通过原地左转通过,最后回到起点附近。
+
+### 4.2 右侧墙距离
+
+
+
+直墙段距离稳定在 0.5 m 附近;经过墙角时右侧墙暂时“消失”,距离升到约 0.75 m,
+转过墙角后重新收敛到 0.5 m。
+
+### 4.3 速度指令
+
+
+
+直墙段线速度约 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