diff --git a/docs/chapter/chap1/chap1.md b/docs/chapter/chap1/chap1.md index fb6d8c3b45..ae70385935 100644 --- a/docs/chapter/chap1/chap1.md +++ b/docs/chapter/chap1/chap1.md @@ -66,3 +66,11 @@ 控制小海龟把输入的文字逐笔绘制在画布上。 [进入小海龟绘制任意文字实验](turtle_text.md) + +### 10. 小海龟键盘遥控与 ROS 基础实验 + +读取方向键或 WASD,向 `/turtle1/cmd_vel` 发布 `geometry_msgs/Twist` 控制小海龟移动, +练习 `rosnode`、`rostopic`、`rosmsg`、`rosservice` 等常用命令, +并阅读 CARLA-ROS 桥接源码,理解外部仿真器与 ROS 之间的双向桥接原理。 + +[进入小海龟键盘遥控与 ROS 基础实验](turtlesim_teleop.md) diff --git a/docs/chapter/chap1/images/turtle_teleop.png b/docs/chapter/chap1/images/turtle_teleop.png new file mode 100644 index 0000000000..a22aeed532 Binary files /dev/null and b/docs/chapter/chap1/images/turtle_teleop.png differ diff --git a/docs/chapter/chap1/turtlesim_teleop.md b/docs/chapter/chap1/turtlesim_teleop.md new file mode 100644 index 0000000000..55314d3399 --- /dev/null +++ b/docs/chapter/chap1/turtlesim_teleop.md @@ -0,0 +1,220 @@ +# 小海龟键盘遥控与 ROS 基础实验 + +本实验完成 ROS 环境验证、turtlesim 小海龟键盘遥控、常用 ROS 命令练习, +并阅读 CARLA-ROS 桥接(ros-bridge)的源码结构,理解 ROS 节点之间的话题通信方式。 + +## 一、实验目的与环境说明 + +### 1.1 实验目的 + +1. 验证 ROS Noetic 环境:`roscore` 能正常启动,`rosversion`、`rostopic` 等命令可用; +2. 掌握 turtlesim 的话题通信模型:遥控节点向 `/turtle1/cmd_vel` 发布 `geometry_msgs/Twist` 速度消息; +3. 实现一个键盘遥控节点:读取方向键 / WASD,把按键换算成线速度与角速度并发布; +4. 熟练使用 `rosnode`、`rostopic`、`rosmsg`、`rosservice`、`rqt_graph` 等常用命令查看和调试 ROS 系统; +5. 阅读 CARLA-ROS 桥接源码,理解外部仿真器与 ROS 之间“传感器数据转话题、控制指令转仿真命令”的双向桥接思路。 + +### 1.2 实验环境 + +| 项目 | 配置 | +| ---- | ---- | +| 操作系统 | Ubuntu 20.04(VMware 虚拟机) | +| ROS 发行版 | ROS Noetic(`rosversion` 输出 1.16.0) | +| 仿真器 | turtlesim | +| 编程语言 | Python 3(rospy),代码兼容 Python 2.7 | +| 功能包 | turtlesim_teleop | + +功能包目录结构如下: + +```text +turtlesim_teleop/ +├── main.py # 主程序:键盘遥控节点,发布 /turtle1/cmd_vel +├── launch/main.launch # roslaunch 入口:一键启动 turtlesim + 遥控节点 +├── package.xml # 包清单,声明 rospy、geometry_msgs、turtlesim 依赖 +├── CMakeLists.txt # 编译配置,安装 main.py 与 launch 目录 +└── README.md # 运行环境与运行步骤 +``` + +## 二、实验原理 + +### 2.1 turtlesim 的话题模型 + +`turtlesim_node` 启动后会创建一只小海龟,并订阅 `/turtle1/cmd_vel` 话题,消息类型为 +`geometry_msgs/Twist`。`Twist` 由线速度和角速度两部分组成,本实验只用到两个分量: + +- `linear.x`:沿海龟自身 x 轴(车头方向)的线速度,单位 m/s,正值为前进; +- `angular.z`:绕 z 轴的角速度,单位 rad/s,正值为逆时针左转。 + +遥控节点作为发布者,只要向该话题写入一条 `Twist`,海龟就会按消息运动。 +因此“键盘遥控”的本质是:**把按键事件映射成速度分量,再周期性或按键时发布出去**。 + +### 2.2 终端按键读取 + +普通终端是行缓冲的,按一个键后要按回车才会把内容交给程序,不适合实时遥控。 +本模块用 `termios` + `tty.setcbreak()` 把终端切换为 cbreak 模式,按一个键立即生效; +再用 `select.select()` 以 100 ms 超时非阻塞读取,没有按键时循环继续,不会卡住节点。 + +方向键不是单个字符,而是以 `ESC` 开头的 3 字符转义序列: + +| 按键 | 转义序列 | +| ---- | ---- | +| ↑ | `\x1b[A` | +| ↓ | `\x1b[B` | +| → | `\x1b[C` | +| ← | `\x1b[D` | + +程序读到 `ESC` 后再尝试读 2 个字符,拼成完整的转义序列再查表,因此方向键和 WASD 都能识别。 + +### 2.3 速度保持与安全停车 + +turtlesim 会持续应用最后一次收到的速度指令,松开按键后海龟不会自动停下。 +因此程序在按空格时发布一条全零 `Twist` 立即停车,在按 `q` 退出或 `Ctrl+C` 中断时, +在 `finally` 中先恢复终端设置、再发布零速度,避免海龟“跑飞”。 + +## 三、实验步骤 + +### 3.1 环境验证 + +打开终端,先确认 ROS 版本并启动主节点: + +```bash +rosversion -d # 输出 noetic +roscore # 启动 ROS Master,保持该终端不关闭 +``` + +`roscore` 正常启动时会打印 `started roslaunch server ...`、`ros_comm version 1.16.0` 等信息, +说明环境安装成功。 + +### 3.2 用官方节点体验键盘遥控 + +另开一个终端启动仿真器: + +```bash +rosrun turtlesim turtlesim_node +``` + +再开一个终端启动官方遥控节点(键盘输入必须在这个前台终端里): + +```bash +rosrun turtlesim turtle_teleop_key +``` + +终端会提示 `Reading from keyboard`、`Use arrow keys to move the turtle. 'q' to quit.`, +此时按方向键即可控制海龟移动,按 `q` 退出。运行截图见 3.4 节。 + +### 3.3 运行本模块的遥控节点 + +把 `src/chap1/turtlesim_teleop` 拷贝到 catkin 工作空间的 `src` 下,编译并加载环境: + +```bash +mkdir -p ~/catkin_ws/src +cp -r <本仓库>/src/chap1/turtlesim_teleop ~/catkin_ws/src/ +chmod +x ~/catkin_ws/src/turtlesim_teleop/main.py +cd ~/catkin_ws && catkin_make +source devel/setup.bash +``` + +方式一,一键启动(需要 xterm 承载键盘输入): + +```bash +sudo apt install -y xterm # 只需安装一次 +roslaunch turtlesim_teleop main.launch +``` + +方式二,手动三个终端: + +```bash +roscore +rosrun turtlesim turtlesim_node +rosrun turtlesim_teleop main.py +``` + +按键说明:方向键 / WASD 移动,空格停车,`+` / `-` 调整速度,`q` 或 `Ctrl+C` 退出。 +终端会实时打印当前线速度和角速度。 + +### 3.4 运行结果 + +下图为 ROS Noetic 虚拟机中运行小海龟仿真与键盘遥控的截图:左侧终端已经启动 +`roscore` 和 `turtle_teleop_key`,右侧为 turtlesim 窗口,小海龟在按键控制下走出了三角形轨迹。 + +![小海龟键盘遥控运行截图](images/turtle_teleop.png) + +从终端日志可以看到: + +- `roscore` 输出的参数为 `/rosdistro: noetic`、`/rosversion: 1.16.0`; +- `turtlesim_node` 输出 `Spawning turtle [turtle1] at x=[5.544445], y=[5.544445], theta=[0.000000]`; +- 遥控终端输出 `Reading from keyboard` 和方向键提示,说明键盘节点已就绪。 + +## 四、常用 ROS 命令 + +在遥控节点和小海龟运行的同时,另开终端练习以下命令: + +| 命令 | 作用 | 本实验中的观察结果 | +| ---- | ---- | ---- | +| `rosnode list` | 列出所有节点 | 能看到 `/turtlesim` 和 `/turtlesim_teleop`(或 `/turtle_teleop_key`) | +| `rosnode info /turtlesim` | 查看节点详情 | 显示它订阅 `/turtle1/cmd_vel`,发布 `/turtle1/pose` | +| `rostopic list` | 列出所有话题 | 能看到 `/turtle1/cmd_vel`、`/turtle1/pose` 等 | +| `rostopic info /turtle1/cmd_vel` | 查看话题类型和收发双方 | 类型为 `geometry_msgs/Twist` | +| `rostopic echo /turtle1/cmd_vel` | 实时打印速度消息 | 按键时打印 `linear.x` / `angular.z` 数值 | +| `rostopic hz /turtle1/pose` | 统计话题发布频率 | 观察 turtlesim 发布位姿的频率 | +| `rosmsg show geometry_msgs/Twist` | 查看消息字段定义 | 显示 `linear` 和 `angular` 两个 `Vector3` 字段 | +| `rosservice list` | 列出所有服务 | 能看到 `/clear`、`/turtle1/set_pen` 等 | +| `rosservice call /clear` | 清空画布 | 海龟走过的轨迹被擦除 | +| `rqt_graph` | 图形化显示节点与话题连接 | 可看到遥控节点 → `/turtle1/cmd_vel` → turtlesim 的连线 | +| `rosbag record -O turtle /turtle1/cmd_vel` | 录制话题数据 | 生成 `turtle.bag`,可用 `rosbag play` 回放 | + +## 五、CARLA-ROS 桥接源码阅读 + +### 5.1 桥接的作用 + +CARLA 是自动驾驶仿真器,本身不是 ROS 系统。`ros-bridge` 在两个系统之间做双向翻译: + +- **CARLA → ROS**:把仿真世界中的车辆位姿、激光雷达、相机、GNSS、IMU、交通灯、碰撞等数据 + 转换成 ROS 话题(`/carla/...`),供 ROS 节点订阅; +- **ROS → CARLA**:把 ROS 中的控制指令(转向、油门、刹车、仿真暂停等)转换成 CARLA 的 API 调用。 + +一句话概括:**桥接节点既是 CARLA 世界的观察者,也是 ROS 指令的执行者**。 + +### 5.2 源码目录与职责 + +| 文件 | 职责 | +| ---- | ---- | +| `main.py` | 键盘遥控节点:读取方向键,映射成 `geometry_msgs/Twist`,发布到 `/turtle1/cmd_vel`;退出时补发零速度 | +| `launch/main.launch` | 一键启动 `turtlesim_node` 与遥控节点,用 `xterm` 给键盘节点提供终端输入 | +| `package.xml` | 声明 `rospy`、`geometry_msgs`、`turtlesim` 依赖 | +| `CMakeLists.txt` | 安装 `main.py` 到 bin 目录、`launch/` 到 share 目录 | + +### 5.3 核心数据流 + +1. `launch/main.launch` 先启动 `turtlesim_node`,它创建小海龟并订阅 `/turtle1/cmd_vel`; +2. 遥控节点在 `xterm` 中读取方向键,把按键映射成线速度和角速度; +3. 节点以 `Twist` 消息发布到 `/turtle1/cmd_vel`,`turtlesim_node` 收到后积分位姿并刷新画面; +4. 松开按键时发布零速度,退出或 Ctrl+C 时也会补发一次零速度,避免海龟继续运动。 + +### 5.4 阅读收获 + +- 话题通信把“产生指令”和“执行运动”解耦,遥控节点不需要知道 `turtlesim` 的内部实现; +- 键盘输入依赖真实终端,`roslaunch` 场景要用 `xterm -e` 或 `launch-prefix` 提供 TTY; +- `catkin_install_python` 负责脚本,`install(DIRECTORY launch ...)` 负责 launch 文件,二者都安装后 `catkin_make install` 才是完整安装。 + +## 六、常见问题 + +1. **按方向键没有反应**:确认键盘节点是在前台终端用 `rosrun` 运行;用 `roslaunch` 启动时 + 必须加 `launch-prefix="xterm -e"`,否则节点读不到终端输入。 +2. **海龟不动**:检查 `roscore` 是否在运行、`turtlesim_node` 是否启动,并用 + `rostopic echo /turtle1/cmd_vel` 确认按键时确有消息发布。 +3. **`Unable to register with master node`**:通常是 `roscore` 没启动,或 `ROS_MASTER_URI` + 指向了别的机器,重新打开终端 `source /opt/ros/noetic/setup.bash` 后再试。 +4. **退出后海龟还在动**:本模块退出时会发布零速度;若用其他方式结束进程,可手动执行 + `rostopic pub -1 /turtle1/cmd_vel geometry_msgs/Twist "{}"` 停车。 + +## 七、总结 + +本实验验证了 ROS Noetic 环境,使用官方 `turtle_teleop_key` 和自写的 `turtlesim_teleop` +节点两种方式完成了小海龟键盘遥控,并练习了节点、话题、消息、服务相关的常用命令。 +通过阅读本模块源码,进一步理解了键盘输入、`Twist` 消息与 `turtlesim_node` 订阅者之间的 +数据流,以及 `catkin_install_python` 和 `install(DIRECTORY launch ...)` 在安装场景下的作用。 + +## 说明 + +本文档在整理过程中使用了 AI 助手(DeepSeek)辅助润色和结构化,实验环境、命令与截图 +均由作者核对;作者对本文内容负全部责任。 diff --git a/docs/index.md b/docs/index.md index 892845a340..0b9c56742a 100644 --- a/docs/index.md +++ b/docs/index.md @@ -54,6 +54,7 @@ ROS 相关资料([网盘下载地址](https://pan.baidu.com/s/1viua4SZ7tP2DtU2 ROS每章节运行代码: * [第 1 章](./chapter/chap1/chap1.md) - 认识 ROS +* [小海龟键盘遥控与 ROS 基础实验](./chapter/chap1/turtlesim_teleop.md) —— 用方向键发布 /turtle1/cmd_vel,练习节点、话题、消息与 roslaunch 一键启动 * [第 2 章](./chapter/chap2.md) - ROS 基础 * [ROS 2 话题发布订阅基础实验(talker/listener)](./chap2/ros2_pubsub_py.md) * [ROS Noetic 多传感器话题时间同步](./chap2/ros1_message_time_sync/index.md) diff --git a/mkdocs.yml b/mkdocs.yml index d7dbc60e0a..b333b1c8a9 100644 --- a/mkdocs.yml +++ b/mkdocs.yml @@ -58,6 +58,7 @@ nav: - ROS 2 Action 斐波那契数列计算示例: 'chap2/fibonacci_action.md' - ROS 教程: - 1. 认识 ROS: 'chapter/chap1/chap1.md' + - 小海龟键盘遥控与 ROS 基础实验: 'chapter/chap1/turtlesim_teleop.md' - 2. ROS 基础 - ROS 基础: - 章节首页: './chapter/chap2.md' - ROS Noetic 多传感器话题时间同步: 'chap2/ros1_message_time_sync/index.md' diff --git a/src/chap1/turtlesim_teleop/CMakeLists.txt b/src/chap1/turtlesim_teleop/CMakeLists.txt new file mode 100644 index 0000000000..9029fbaa26 --- /dev/null +++ b/src/chap1/turtlesim_teleop/CMakeLists.txt @@ -0,0 +1,27 @@ +cmake_minimum_required(VERSION 3.0.2) +project(turtlesim_teleop) + +# 查找 catkin 及本模块用到的依赖:rospy 负责节点与话题,geometry_msgs 提供 Twist 消息, +# turtlesim 提供仿真器节点。 +find_package(catkin REQUIRED COMPONENTS + rospy + geometry_msgs + turtlesim +) + +catkin_package() + +# 把键盘遥控入口脚本 main.py 安装到 bin 目录,装好后可用 +# rosrun turtlesim_teleop main.py 直接运行。 +# 注意:clone 后需保证脚本有可执行权限(chmod +x main.py)。 +catkin_install_python(PROGRAMS + main.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +# 安装 launch 目录到 share/turtlesim_teleop,保证 catkin_make install 或 +# 发布场景下 roslaunch turtlesim_teleop main.launch 能找到文件。 +install(DIRECTORY launch + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + USE_SOURCE_PERMISSIONS +) diff --git a/src/chap1/turtlesim_teleop/README.md b/src/chap1/turtlesim_teleop/README.md new file mode 100644 index 0000000000..5d67785d6d --- /dev/null +++ b/src/chap1/turtlesim_teleop/README.md @@ -0,0 +1,75 @@ +# turtlesim_teleop — 小海龟键盘遥控模块(第一章实验) + +读取方向键或 WASD,向 `/turtle1/cmd_vel` 发布 `geometry_msgs/Twist` 速度指令, +控制 turtlesim 小海龟前后移动、左右转向。配套教程见 +[docs/chapter/chap1/turtlesim_teleop.md](../../docs/chapter/chap1/turtlesim_teleop.md)。 + +## 运行环境 + +| 项目 | 配置 | +| ---- | ---- | +| 操作系统 | Ubuntu 20.04(VMware 虚拟机) | +| ROS | ROS Noetic(Python 3);代码同时兼容 Ubuntu 16.04 + Kinetic(Python 2.7) | +| 依赖功能包 | turtlesim、rospy、geometry_msgs(desktop-full 自带) | +| 其他 | roslaunch 一键启动时键盘节点需要 xterm(`sudo apt install -y xterm`) | + +## 运行步骤 + +**方式一:roslaunch 一键启动(推荐)** + +```bash +# 1. 把本模块拷贝到 catkin 工作空间 +mkdir -p ~/catkin_ws/src +cp -r <本仓库>/src/chap1/turtlesim_teleop ~/catkin_ws/src/ +chmod +x ~/catkin_ws/src/turtlesim_teleop/main.py + +# 2. 编译并加载环境 +cd ~/catkin_ws && catkin_make +source devel/setup.bash + +# 3. 一键启动:turtlesim 窗口 + 键盘遥控 xterm 窗口 +roslaunch turtlesim_teleop main.launch +``` + +若没有安装 xterm,可用 `roslaunch turtlesim_teleop main.launch use_xterm:=false`, +再另开一个终端执行 `rosrun turtlesim_teleop main.py`。 + +**方式二:手动三终端** + +```bash +# 终端 1 +roscore +# 终端 2 +rosrun turtlesim turtlesim_node +# 终端 3(键盘输入必须在这个前台终端里) +rosrun turtlesim_teleop main.py +``` + +## 按键说明 + +| 按键 | 功能 | +| ---- | ---- | +| 方向键 / WASD | 前进、后退、左转、右转 | +| 空格 | 立即停车 | +| `+` / `-` | 加速 / 减速(步长 0.5,范围 0.5~5.0) | +| `q` 或 `Ctrl+C` | 退出;退出前自动发布零速度停车 | + +## 可调参数 + +| 参数 | 默认值 | 说明 | +| ---- | ---- | ---- | +| `~linear_speed` | 2.0 | 线速度(m/s) | +| `~angular_speed` | 2.0 | 角速度(rad/s) | + +在 launch 中传参示例: + +```xml + + + +``` + +## 预期效果 + +turtlesim 窗口中的小海龟按方向键移动,终端实时打印当前速度; +松开按键后海龟保持最后一次速度指令,按空格或 `q` 退出时发布零速度停车。 diff --git a/src/chap1/turtlesim_teleop/launch/main.launch b/src/chap1/turtlesim_teleop/launch/main.launch new file mode 100644 index 0000000000..d0d1a2fd79 --- /dev/null +++ b/src/chap1/turtlesim_teleop/launch/main.launch @@ -0,0 +1,16 @@ + + + + + + + + + + + + + diff --git a/src/chap1/turtlesim_teleop/main.py b/src/chap1/turtlesim_teleop/main.py new file mode 100644 index 0000000000..ee324725a0 --- /dev/null +++ b/src/chap1/turtlesim_teleop/main.py @@ -0,0 +1,170 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +"""turtlesim 小海龟键盘遥控节点(第一章实验) + +用方向键或 WASD 控制 turtlesim 中的小海龟前后移动、左右转向, +速度指令发布到 /turtle1/cmd_vel(geometry_msgs/Twist)。 + +运行方式一(前台运行,键盘输入直接可用): + rosrun turtlesim_teleop main.py + +运行方式二(一键启动 turtlesim + 遥控节点,会弹出 xterm 窗口): + roslaunch turtlesim_teleop main.launch + +按键说明: + 方向键 / WASD 前进、后退、左转、右转 + 空格 立即停车 + + / - 加速 / 减速(步长 0.5) + q 退出(退出前自动发布零速度,避免海龟继续运动) + +兼容 Python 2.7(ROS Kinetic)与 Python 3(ROS Noetic)。 +""" +from __future__ import print_function + +import select +import sys +import termios +import tty + +import rospy +from geometry_msgs.msg import Twist + +# 方向键的转义序列 +ARROW_UP = '\x1b[A' +ARROW_DOWN = '\x1b[B' +ARROW_RIGHT = '\x1b[C' +ARROW_LEFT = '\x1b[D' + +# 按键 -> (线速度系数, 角速度系数) +# 线速度为正表示前进,角速度为正表示左转(逆时针) +KEY_COMMANDS = { + ARROW_UP: (1.0, 0.0), + ARROW_DOWN: (-1.0, 0.0), + ARROW_LEFT: (0.0, 1.0), + ARROW_RIGHT: (0.0, -1.0), + 'w': (1.0, 0.0), + 's': (-1.0, 0.0), + 'a': (0.0, 1.0), + 'd': (0.0, -1.0), + ' ': (0.0, 0.0), +} + +# 退出按键:q +QUIT_KEYS = ('q', 'Q') + + +def command_for_key(key): + """返回按键对应的速度系数 (线速度系数, 角速度系数),未知按键返回 None。 + + 该函数只做按键到速度的映射,不依赖 ROS,便于单独验证。 + """ + if key is None: + return None + return KEY_COMMANDS.get(key.lower() if len(key) == 1 else key) + + +def read_key(timeout=0.1): + """从标准输入读取一个按键(非阻塞),没有输入时返回 None。 + + 方向键会返回 3 个字符的转义序列,例如上方向键返回 '\\x1b[A'。 + """ + ready, _, _ = select.select([sys.stdin], [], [], timeout) + if not ready: + return None + + char = sys.stdin.read(1) + if char == '\x1b': + # 方向键的转义序列还剩下 2 个字符,尽量一次读完 + for _ in range(2): + ready, _, _ = select.select([sys.stdin], [], [], 0.02) + if not ready: + break + char += sys.stdin.read(1) + return char + + +def publish_cmd(pub, linear, angular): + """发布一条速度消息。""" + msg = Twist() + msg.linear.x = linear + msg.angular.z = angular + pub.publish(msg) + + +def main(): + rospy.init_node('turtlesim_teleop') + + pub = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10) + + linear_speed = rospy.get_param('~linear_speed', 2.0) # 线速度 m/s + angular_speed = rospy.get_param('~angular_speed', 2.0) # 角速度 rad/s + + # roslaunch 启动的节点拿不到终端 stdin,必须用 xterm 前台运行 + if not sys.stdin.isatty(): + rospy.logerr('当前标准输入不是终端,读不到键盘输入。' + '请用 rosrun 前台运行,或给 launch 节点加 launch-prefix="xterm -e"。') + return + + # 让发布者与 turtlesim 订阅端先建立连接,避免第一条消息丢失 + rospy.sleep(1.0) + + print('小海龟键盘遥控已启动:') + print(' 方向键 / WASD 移动') + print(' 空格 停车') + print(' + / - 加速 / 减速') + print(' q 退出') + print('当前速度:线速度 %.1f m/s,角速度 %.1f rad/s' + % (linear_speed, angular_speed)) + + old_settings = termios.tcgetattr(sys.stdin) + try: + # cbreak 模式:按键立即生效,不需要按回车 + tty.setcbreak(sys.stdin.fileno()) + + while not rospy.is_shutdown(): + key = read_key(timeout=0.1) + if key is None: + continue + + if key in QUIT_KEYS: + rospy.loginfo('收到退出按键,停车退出') + break + + if key in ('+', '='): + linear_speed = min(linear_speed + 0.5, 5.0) + angular_speed = min(angular_speed + 0.5, 5.0) + print('加速:线速度 %.1f m/s,角速度 %.1f rad/s' + % (linear_speed, angular_speed)) + continue + + if key in ('-', '_'): + linear_speed = max(linear_speed - 0.5, 0.5) + angular_speed = max(angular_speed - 0.5, 0.5) + print('减速:线速度 %.1f m/s,角速度 %.1f rad/s' + % (linear_speed, angular_speed)) + continue + + command = command_for_key(key) + if command is None: + print('未识别的按键,使用方向键、WASD、空格、+/- 或 q') + continue + + linear = command[0] * linear_speed + angular = command[1] * angular_speed + publish_cmd(pub, linear, angular) + + if command == (0.0, 0.0): + print('停车') + else: + print('运动:线速度 %+.1f m/s,角速度 %+.1f rad/s' + % (linear, angular)) + except KeyboardInterrupt: + rospy.loginfo('收到 Ctrl+C,停车退出') + finally: + # 恢复终端设置,并发布零速度让海龟停下 + termios.tcsetattr(sys.stdin, termios.TCSADRAIN, old_settings) + publish_cmd(pub, 0.0, 0.0) + + +if __name__ == '__main__': + main() diff --git a/src/chap1/turtlesim_teleop/package.xml b/src/chap1/turtlesim_teleop/package.xml new file mode 100644 index 0000000000..65a4c51dcf --- /dev/null +++ b/src/chap1/turtlesim_teleop/package.xml @@ -0,0 +1,18 @@ + + + turtlesim_teleop + 1.0.0 + + 小海龟键盘遥控模块(第一章实验):读取方向键或 WASD,向 /turtle1/cmd_vel + 发布 geometry_msgs/Twist 速度指令,控制 turtlesim 小海龟前后移动和左右转向。 + 配套文档见 docs/chapter/chap1/turtlesim_teleop.md。 + + xinyangtang91 + MIT + + catkin + + rospy + geometry_msgs + turtlesim + diff --git a/tests/test_turtlesim_teleop_local.py b/tests/test_turtlesim_teleop_local.py new file mode 100644 index 0000000000..3a625c9d82 --- /dev/null +++ b/tests/test_turtlesim_teleop_local.py @@ -0,0 +1,157 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +"""turtlesim_teleop 本地测试(桩依赖,不需要 ROS / turtlesim / 终端). + +A. 按键映射:方向键转义序列与 WASD -> 线速度/角速度系数,空格停车,未知键忽略 +B. publish_cmd:Twist 字段映射正确 +C. 工程文件:package.xml / CMakeLists.txt / main.launch + +用法: python3 tests/test_turtlesim_teleop_local.py +""" +from __future__ import annotations + +import importlib.util +import os +import sys +import types +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__)) +ROOT = os.path.abspath(os.path.join(HERE, "..")) +PKG = os.path.join(ROOT, "src", "chap1", "turtlesim_teleop") + + +# ---------------------------------------------------------------- 消息桩 +class _Vec3(object): + def __init__(self): + self.x = self.y = self.z = 0.0 + + +class _Twist(object): + def __init__(self): + self.linear = _Vec3() + self.angular = _Vec3() + + +# ---------------------------------------------------------------- rospy 桩 +rospy = types.ModuleType("rospy") +rospy.loginfo = lambda *a, **k: None +rospy.logerr = lambda *a, **k: None +rospy.init_node = lambda *a, **k: None +rospy.get_param = lambda name, default=None: default +rospy.Publisher = lambda *a, **k: None +rospy.sleep = lambda *a, **k: None +rospy.is_shutdown = lambda: False +sys.modules.setdefault("rospy", rospy) + +geometry_msgs = types.ModuleType("geometry_msgs") +geometry_msgs_msg = types.ModuleType("geometry_msgs.msg") +geometry_msgs_msg.Twist = _Twist +geometry_msgs.msg = geometry_msgs_msg +sys.modules.setdefault("geometry_msgs", geometry_msgs) +sys.modules.setdefault("geometry_msgs.msg", geometry_msgs_msg) + +# termios / tty 是 Unix 专有模块,本测试用空实现替代 +termios = types.ModuleType("termios") +termios.tcgetattr = lambda fd: [] +termios.tcsetattr = lambda fd, when, attrs: None +termios.TCSADRAIN = 1 +sys.modules.setdefault("termios", termios) +tty = types.ModuleType("tty") +tty.setcbreak = lambda fd: None +sys.modules.setdefault("tty", tty) + +spec = importlib.util.spec_from_file_location( + "turtlesim_teleop_main", os.path.join(PKG, "main.py")) +teleop = importlib.util.module_from_spec(spec) +spec.loader.exec_module(teleop) + + +# ================================================================= A. 按键映射 +def test_key_mapping(): + print("== A. 按键映射 ==") + cases = { + "\x1b[A": (1.0, 0.0), # ↑ 前进 + "\x1b[B": (-1.0, 0.0), # ↓ 后退 + "\x1b[D": (0.0, 1.0), # ← 左转 + "\x1b[C": (0.0, -1.0), # → 右转 + "w": (1.0, 0.0), + "W": (1.0, 0.0), + "s": (-1.0, 0.0), + "a": (0.0, 1.0), + "d": (0.0, -1.0), + " ": (0.0, 0.0), # 空格停车 + } + for key, expected in cases.items(): + check(teleop.command_for_key(key) == expected, + "按键 %r -> %r" % (key, expected)) + + check(teleop.command_for_key("x") is None, "未知按键返回 None") + check(teleop.command_for_key(None) is None, "空按键返回 None") + + +# ================================================================= B. 消息发布 +def test_publish_cmd(): + print("== B. 消息发布 ==") + published = [] + + class Pub(object): + def publish(self, msg): + published.append((msg.linear.x, msg.angular.z)) + + teleop.publish_cmd(Pub(), 2.0, -1.5) + check(published == [(2.0, -1.5)], "publish_cmd 正确写入 linear.x / angular.z") + + teleop.publish_cmd(Pub(), 0.0, 0.0) + check(published[-1] == (0.0, 0.0), "停车消息为全零") + + +# ================================================================= C. 工程文件 +def test_project_files(): + print("== C. 工程文件 ==") + pkg = ET.parse(os.path.join(PKG, "package.xml")).getroot() + check(pkg.findtext("name").strip() == "turtlesim_teleop", "package.xml 包名正确") + deps = [d.text.strip() for d in pkg.findall("depend")] + for dep in ("rospy", "geometry_msgs", "turtlesim"): + check(dep in deps, "package.xml 声明依赖 %s" % dep) + + cmake = open(os.path.join(PKG, "CMakeLists.txt"), encoding="utf-8").read() + check("main.py" in cmake, "CMakeLists.txt 安装 main.py") + check("catkin_install_python" in cmake, "CMakeLists.txt 使用 catkin_install_python") + check("install(DIRECTORY launch" in cmake, "CMakeLists.txt 安装 launch 目录") + + launch = ET.parse(os.path.join(PKG, "launch", "main.launch")).getroot() + nodes = [n.get("type") for n in launch.findall("node")] + check("turtlesim_node" in nodes, "main.launch 启动 turtlesim_node") + check("main.py" in nodes, "main.launch 启动 main.py") + check(os.path.exists(os.path.join(PKG, "README.md")), "README.md 存在") + check(os.path.exists(os.path.join(ROOT, "docs", "chapter", "chap1", + "turtlesim_teleop.md")), "配套文档存在") + check(os.path.exists(os.path.join(ROOT, "docs", "chapter", "chap1", "images", + "turtle_teleop.png")), "运行截图存在") + + +def main(): + test_key_mapping() + test_publish_cmd() + 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())