尧图网站开发 
在实际机器人开发中直接通过键盘控制电机转速是一个基础但至关重要的环节。它不仅是验证机器人底层驱动是否正常工作的第一步也为后续更高级的遥控、导航和自主决策功能奠定了基础。很多初学者在搭建好ROS2环境、写好电机驱动节点后却卡在了如何将键盘指令实时、平滑地转化为电机转速这一步。本文将带你从零开始在ROS2中实现一个完整的键盘控制电机转速实验。你将理解如何创建自定义的速度消息、编写键盘监听节点、处理速度指令的发布逻辑并最终将指令发送到电机驱动节点。整个过程会涉及ROS2节点、话题、消息和服务等核心概念的实际应用适合已经了解ROS2基本概念希望进行第一个实质性交互控制的开发者。1. 理解实验目标与系统架构在开始写代码之前我们需要明确这个实验要达成的目标以及系统中各个部分是如何协作的。这能帮助你在后续步骤中清楚每一行代码的作用而不是盲目复制粘贴。1.1 实验最终目标我们的目标是实现一个功能当你在终端按下特定的按键例如W、S、A、D时机器人能够相应地改变其电机的转速。例如按下W键增加前进速度按下S键减速或后退按下A/D键实现差速转向。1.2 ROS2 系统架构设计为了实现上述目标我们需要设计至少两个ROS2节点并通过话题进行通信。键盘控制节点 (Keyboard Teleop Node)这个节点负责监听标准输入键盘将按下的键位解析为具体的速度指令。它需要持续运行实时捕获用户的输入。电机驱动节点 (Motor Driver Node)这个节点负责与实际的电机控制器如STM32、Arduino或树莓派GPIO通信接收速度指令并转化为PWM信号或CAN报文等发送给电机。本文主要聚焦于ROS2部分因此我们会模拟一个电机驱动节点它订阅速度指令并打印出来。这两个节点通过一个自定义的速度指令话题进行连接。键盘节点发布消息到这个话题电机节点订阅这个话题。这是一种典型的发布-订阅模型实现了控制逻辑与硬件驱动的解耦。数据流键盘输入-键盘控制节点-(发布) /cmd_vel 话题-(订阅) 电机驱动节点-电机硬件。注意在真实的机器人中/cmd_vel话题通常遵循geometry_msgs/msg/Twist消息类型它包含线速度和角速度。为了教学清晰我们先从自定义一个简单的速度消息开始。2. 环境准备与工作空间创建在开始编码前请确保你有一个可用的ROS2开发环境。本实验以ROS2 Humble版本在Ubuntu 22.04上为例其他版本操作类似。2.1 确认ROS2环境打开终端运行以下命令确认ROS2已正确安装并激活source /opt/ros/humble/setup.bash ros2 --version如果正确显示版本号如ros2 version 4.0.0则环境正常。2.2 创建功能包与工作空间我们将创建一个独立的工作空间来管理本项目。创建并进入工作空间目录mkdir -p ~/keyboard_motor_ws/src cd ~/keyboard_motor_ws/src创建ROS2功能包。我们的包将包含自定义消息、键盘节点和模拟电机节点。ros2 pkg create keyboard_motor_control --build-type ament_python --dependencies rclpy geometry_msgs--build-type ament_python指定创建Python包。--dependencies rclpy geometry_msgs声明依赖。rclpy是Python的ROS2客户端库geometry_msgs后续可能会用到。创建完成后进入包目录查看结构cd keyboard_motor_control ls你会看到package.xml、setup.py、setup.cfg和keyboard_motor_control目录。3. 定义自定义速度消息虽然我们可以直接使用geometry_msgs/msg/Twist但为了深入理解消息创建过程我们先定义一个更简单的自定义消息。3.1 创建消息定义文件在功能包根目录下创建msg文件夹并在其中创建MotorSpeed.msg文件。mkdir -p ~/keyboard_motor_ws/src/keyboard_motor_control/msg cd ~/keyboard_motor_ws/src/keyboard_motor_control/msg touch MotorSpeed.msg3.2 编辑消息内容用文本编辑器打开MotorSpeed.msg定义两个float32类型的字段分别代表左轮和右轮的目标转速单位rad/s 或 自定义单位。float32 left_speed float32 right_speed3.3 配置包以编译消息需要修改package.xml和setup.py来声明对消息构建的支持。编辑package.xml 在buildtool_dependament_python/buildtool_depend之后添加以下依赖build_dependrosidl_default_generators/build_depend exec_dependrosidl_default_runtime/exec_depend member_of_grouprosidl_interface_packages/member_of_group编辑setup.py 找到data_files部分通常是一个列表。修改它使其包含msg目录。修改后的部分大致如下import os from glob import glob from setuptools import setup package_name keyboard_motor_control setup( # ... 其他参数保持不变 ... data_files[ (share/ament_index/resource_index/packages, [resource/ package_name]), (share/ package_name, [package.xml]), # 添加以下两行将msg文件夹及其内容安装到指定位置 (os.path.join(share, package_name, msg), glob(msg/*.msg)), (os.path.join(share, package_name), glob(launch/*.py)), ], # ... 其他参数保持不变 ... )同时确保setup函数中的install_requires包含了setuptools。4. 编写键盘控制节点现在我们来编写核心的键盘控制节点keyboard_teleop.py。4.1 创建节点文件在keyboard_motor_control/keyboard_motor_control目录下创建该文件。cd ~/keyboard_motor_ws/src/keyboard_motor_control/keyboard_motor_control touch keyboard_teleop.py chmod x keyboard_teleop.py # 使其可执行4.2 编写节点代码以下是keyboard_teleop.py的完整代码包含详细注释#!/usr/bin/env python3 import rclpy from rclpy.node import Node from keyboard_motor_control.msg import MotorSpeed # 导入自定义消息 import sys import select import tty import termios import time class KeyboardTeleopNode(Node): def __init__(self): super().__init__(keyboard_teleop_node) # 创建发布者话题名为 /cmd_motor_speed消息类型为 MotorSpeed self.publisher_ self.create_publisher(MotorSpeed, /cmd_motor_speed, 10) self.speed_msg MotorSpeed() # 创建消息实例 self.left_speed 0.0 self.right_speed 0.0 self.speed_step 0.1 # 每次按键速度增量 self.max_speed 2.0 # 最大速度限制 self.get_logger().info(键盘控制节点已启动) self.get_logger().info(使用 W/S 控制前进/后退速度A/D 控制转向空格键停止Q 退出。) self.print_speed() # 保存终端原始设置用于恢复 self.old_settings termios.tcgetattr(sys.stdin) def get_key(self): 非阻塞获取键盘输入的函数 tty.setraw(sys.stdin.fileno()) rlist, _, _ select.select([sys.stdin], [], [], 0.1) if rlist: key sys.stdin.read(1) else: key termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.old_settings) return key def update_speed(self, key): 根据按键更新速度值 if key w or key W: # 增加前进速度 self.left_speed min(self.left_speed self.speed_step, self.max_speed) self.right_speed min(self.right_speed self.speed_step, self.max_speed) elif key s or key S: # 减少速度可变为负值后退 self.left_speed max(self.left_speed - self.speed_step, -self.max_speed) self.right_speed max(self.right_speed - self.speed_step, -self.max_speed) elif key a or key A: # 左转左轮减速右轮加速或保持 self.left_speed max(self.left_speed - self.speed_step/2, -self.max_speed) self.right_speed min(self.right_speed self.speed_step/2, self.max_speed) elif key d or key D: # 右转右轮减速左轮加速或保持 self.left_speed min(self.left_speed self.speed_step/2, self.max_speed) self.right_speed max(self.right_speed - self.speed_step/2, -self.max_speed) elif key : # 空格键停止 self.left_speed 0.0 self.right_speed 0.0 elif key q or key Q: # Q键退出 self.get_logger().info(正在退出...) return False return True def publish_speed(self): 发布当前速度到话题 self.speed_msg.left_speed self.left_speed self.speed_msg.right_speed self.right_speed self.publisher_.publish(self.speed_msg) self.print_speed() def print_speed(self): 打印当前速度到终端 self.get_logger().info(f当前速度: left{self.left_speed:.2f}, right{self.right_speed:.2f}) def run(self): 节点主循环 try: while rclpy.ok(): key self.get_key() if key: if not self.update_speed(key): break self.publish_speed() # 即使没有按键也以一定频率发布速度保证电机持续收到指令例如维持速度 # 这对于真实电机很重要否则可能因收不到指令而停止。 time.sleep(0.05) # 控制循环频率约20Hz finally: # 退出前发布一次零速确保电机停止 self.left_speed 0.0 self.right_speed 0.0 self.publish_speed() termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.old_settings) self.get_logger().info(键盘控制节点已关闭。) def main(argsNone): rclpy.init(argsargs) node KeyboardTeleopNode() node.run() node.destroy_node() rclpy.shutdown() if __name__ __main__: main()4.3 关键代码解析非阻塞键盘输入get_key函数使用select和tty.setraw实现非阻塞读取这样节点不会在等待键盘输入时卡住可以同时处理ROS2的通信。速度更新逻辑update_speed函数根据不同的按键修改成员变量left_speed和right_speed。这里实现了简单的差速转向模型。消息发布publish_speed函数将速度值赋给MotorSpeed消息实例并通过发布者发送到/cmd_motor_speed话题。资源清理在finally块中无论是否异常退出都会发布零速并恢复终端设置这是一个良好的编程习惯防止电机失控和终端状态错乱。5. 编写模拟电机驱动节点为了测试键盘节点我们需要一个订阅者来接收速度指令。创建motor_driver_sim.py。cd ~/keyboard_motor_ws/src/keyboard_motor_control/keyboard_motor_control touch motor_driver_sim.py chmod x motor_driver_sim.pymotor_driver_sim.py代码如下#!/usr/bin/env python3 import rclpy from rclpy.node import Node from keyboard_motor_control.msg import MotorSpeed class MotorDriverSimNode(Node): def __init__(self): super().__init__(motor_driver_sim_node) # 创建订阅者订阅 /cmd_motor_speed 话题 self.subscription self.create_subscription( MotorSpeed, /cmd_motor_speed, self.speed_callback, 10) self.subscription # 防止未使用变量警告 self.get_logger().info(模拟电机驱动节点已启动等待速度指令...) def speed_callback(self, msg): 收到速度指令后的回调函数 # 在实际硬件中这里会将 msg.left_speed 和 msg.right_speed # 转换为PWM占空比、CAN命令等发送给电机驱动器。 self.get_logger().info(f收到指令 - 左轮转速: {msg.left_speed:.2f}, 右轮转速: {msg.right_speed:.2f}) # 示例模拟一个简单的处理逻辑 # if abs(msg.left_speed) 1.5: # self.get_logger().warn(左轮转速过高) def main(argsNone): rclpy.init(argsargs) node MotorDriverSimNode() rclpy.spin(node) # 保持节点运行等待消息 node.destroy_node() rclpy.shutdown() if __name__ __main__: main()这个节点非常简单它只是订阅话题并将收到的速度值打印出来。在真实项目中speed_callback函数内部会包含与具体电机硬件如通过串口、PWM、CAN总线通信的代码。6. 配置与构建功能包在编译和运行之前需要确保Python能够找到我们的模块并正确声明入口点。6.1 编辑setup.py添加入口点在setup.py的setup()函数中找到entry_points部分修改如下entry_points{ console_scripts: [ keyboard_teleop keyboard_motor_control.keyboard_teleop:main, motor_driver_sim keyboard_motor_control.motor_driver_sim:main, ], },这告诉ROS2当使用ros2 run命令时keyboard_teleop对应执行keyboard_teleop.py中的main函数。6.2 安装依赖并构建工作空间回到工作空间根目录使用colcon构建工具进行构建。cd ~/keyboard_motor_ws # 安装依赖首次构建或修改package.xml后需要 rosdep install -i --from-path src --rosdistro humble -y # 构建功能包 colcon build --packages-select keyboard_motor_control构建成功后你会看到Summary: 1 package finished的提示。每次修改源代码后都需要重新运行colcon build。6.3 激活工作空间环境构建完成后需要“激活”这个工作空间的环境以便ROS2能够找到我们新创建的功能包和节点。source ~/keyboard_motor_ws/install/setup.bash你可以将此命令添加到~/.bashrc中以便每次打开终端自动激活。7. 运行与验证实验现在到了验证成果的时刻。我们将打开三个终端窗口。7.1 终端1运行模拟电机驱动节点source ~/keyboard_motor_ws/install/setup.bash ros2 run keyboard_motor_control motor_driver_sim如果一切正常你将看到输出[motor_driver_sim_node-1] 模拟电机驱动节点已启动等待速度指令...7.2 终端2运行键盘控制节点source ~/keyboard_motor_ws/install/setup.bash ros2 run keyboard_motor_control keyboard_teleop你会看到启动提示信息并且终端会等待你的键盘输入。请确保这个终端窗口是当前焦点否则键盘输入不会被捕获。7.3 验证控制效果在运行keyboard_teleop的终端终端2中尝试按下W、S、A、D、空格键。观察该终端的输出它会显示当前计算出的左右轮速度。同时观察终端1电机节点的输出它会显示每次收到的速度指令。预期行为按W左右轮速度同步增加。按S左右轮速度同步减少可能变为负值代表后退。按A左轮速度减小右轮速度增加机器人向左转。按D右轮速度减小左轮速度增加机器人向右转。按空格速度归零。按Q退出键盘控制节点。7.4 使用ros2 topic命令监控你可以打开第三个终端使用ROS2命令行工具来监控话题通信这是一个非常实用的调试手段。source ~/keyboard_motor_ws/install/setup.bash # 查看当前所有活跃的话题 ros2 topic list # 你应该能看到 /cmd_motor_speed # 实时回显该话题的消息内容 ros2 topic echo /cmd_motor_speed当你按下键盘时这个终端会实时打印出发布到/cmd_motor_speed话题上的MotorSpeed消息内容。8. 常见问题排查与进阶思考即使按照步骤操作你也可能会遇到一些问题。下面是一些常见问题的排查思路。8.1 常见问题排查表问题现象可能原因检查与解决步骤运行ros2 run提示找不到包或节点1. 工作空间未激活。2. 构建失败或未构建。3.setup.py中入口点配置错误。1. 执行source ~/keyboard_motor_ws/install/setup.bash。2. 检查colcon build输出是否有错误。3. 确认setup.py中entry_points的格式和路径正确。键盘控制节点启动后按键无反应1. 终端窗口未获得焦点。2. 键盘输入被其他程序拦截。3. 节点代码中的按键处理逻辑有误。1. 点击运行键盘节点的终端窗口。2. 关闭可能冲突的输入法或其他全局快捷键软件。3. 检查keyboard_teleop.py中get_key和update_speed函数。电机节点收不到消息1. 话题名称不匹配。2. 消息类型不匹配。3. 网络通信问题通常单机不会。1. 使用ros2 topic list确认话题是否存在。使用ros2 topic info /cmd_motor_speed查看发布者和订阅者。2. 使用ros2 interface show keyboard_motor_control/msg/MotorSpeed确认消息类型。自定义消息无法导入1.package.xml和setup.py配置错误。2. 构建后未重新激活环境。3. Python路径问题。1. 仔细核对3.3节的配置步骤。2. 重新执行colcon build和source install/setup.bash。3. 在Python代码中尝试from keyboard_motor_control.msg import MotorSpeed看是否报错。按下Q键节点未退出终端原始模式设置恢复可能有问题。检查keyboard_teleop.py中finally块是否确保执行了termios.tcsetattr。可以尝试用CtrlC强制终止。8.2 从模拟到真实电机控制本实验的电机节点是模拟的。要控制真实电机你需要硬件连接将你的主控板如树莓派、Jetson通过GPIO、I2C、PWM或CAN总线连接到电机驱动器如TB6612、DRV8833、VESC。修改电机节点在motor_driver_sim.py的speed_callback函数中将msg.left_speed和msg.right_speed转换为硬件驱动所需的信号。例如使用PWM你可能需要调用类似RPi.GPIO或pigpio库来设置PWM占空比。转换公式通常是线性的pwm_duty (speed / max_speed) * max_pwm。例如使用串口/UART你需要将速度值格式化为特定的指令帧通过serial库发送给驱动器。加入安全逻辑真实硬件必须考虑急停、堵转检测、过流保护等。可以在节点中加入状态监测和异常处理。8.3 进阶使用标准geometry_msgs/Twist消息在ROS机器人社区控制移动平台速度的标准话题是/cmd_vel消息类型是geometry_msgs/msg/Twist。它包含线速度linear.x,linear.y,linear.z和角速度angular.x,angular.y,angular.z。对于差分轮式机器人通常只使用linear.x前进速度和angular.z旋转角速度。你可以修改键盘节点发布Twist消息并编写一个twist_to_motor节点来订阅/cmd_vel然后根据机器人运动学模型差分驱动模型将linear.x和angular.z解算为左右轮的速度。这更符合ROS生态的通用做法便于集成导航如Nav2等高级功能。8.4 性能与优化建议发布频率键盘节点的发布频率time.sleep(0.05)控制的20Hz对于手动控制通常足够。但对于高速自主导航可能需要更高的频率如50Hz。消息去抖键盘输入可能存在抖动可以加入简单的滤波逻辑比如速度变化过于频繁时进行平滑处理。参数可配置将speed_step、max_speed等参数通过ROS2参数服务器declare_parameter暴露出来这样可以在启动时或运行时动态调整无需修改代码。使用launch文件创建Python或XML格式的launch文件可以方便地一键启动键盘节点和电机节点并设置参数。通过这个实验你不仅实现了一个键盘控制功能更实践了ROS2中节点间通信的完整流程。理解了这个基础框架你就可以在此基础上扩展出更复杂的机器人控制应用。 
在实际机器人开发中直接通过键盘控制电机转速是一个基础但至关重要的环节。它不仅是验证机器人底层驱动是否正常工作的第一步也为后续更高级的遥控、导航和自主决策功能奠定了基础。很多初学者在搭建好ROS2环境、写好电机驱动节点后却卡在了如何将键盘指令实时、平滑地转化为电机转速这一步。本文将带你从零开始在ROS2中实现一个完整的键盘控制电机转速实验。你将理解如何创建自定义的速度消息、编写键盘监听节点、处理速度指令的发布逻辑并最终将指令发送到电机驱动节点。整个过程会涉及ROS2节点、话题、消息和服务等核心概念的实际应用适合已经了解ROS2基本概念希望进行第一个实质性交互控制的开发者。1. 理解实验目标与系统架构在开始写代码之前我们需要明确这个实验要达成的目标以及系统中各个部分是如何协作的。这能帮助你在后续步骤中清楚每一行代码的作用而不是盲目复制粘贴。1.1 实验最终目标我们的目标是实现一个功能当你在终端按下特定的按键例如W、S、A、D时机器人能够相应地改变其电机的转速。例如按下W键增加前进速度按下S键减速或后退按下A/D键实现差速转向。1.2 ROS2 系统架构设计为了实现上述目标我们需要设计至少两个ROS2节点并通过话题进行通信。键盘控制节点 (Keyboard Teleop Node)这个节点负责监听标准输入键盘将按下的键位解析为具体的速度指令。它需要持续运行实时捕获用户的输入。电机驱动节点 (Motor Driver Node)这个节点负责与实际的电机控制器如STM32、Arduino或树莓派GPIO通信接收速度指令并转化为PWM信号或CAN报文等发送给电机。本文主要聚焦于ROS2部分因此我们会模拟一个电机驱动节点它订阅速度指令并打印出来。这两个节点通过一个自定义的速度指令话题进行连接。键盘节点发布消息到这个话题电机节点订阅这个话题。这是一种典型的发布-订阅模型实现了控制逻辑与硬件驱动的解耦。数据流键盘输入-键盘控制节点-(发布) /cmd_vel 话题-(订阅) 电机驱动节点-电机硬件。注意在真实的机器人中/cmd_vel话题通常遵循geometry_msgs/msg/Twist消息类型它包含线速度和角速度。为了教学清晰我们先从自定义一个简单的速度消息开始。2. 环境准备与工作空间创建在开始编码前请确保你有一个可用的ROS2开发环境。本实验以ROS2 Humble版本在Ubuntu 22.04上为例其他版本操作类似。2.1 确认ROS2环境打开终端运行以下命令确认ROS2已正确安装并激活source /opt/ros/humble/setup.bash ros2 --version如果正确显示版本号如ros2 version 4.0.0则环境正常。2.2 创建功能包与工作空间我们将创建一个独立的工作空间来管理本项目。创建并进入工作空间目录mkdir -p ~/keyboard_motor_ws/src cd ~/keyboard_motor_ws/src创建ROS2功能包。我们的包将包含自定义消息、键盘节点和模拟电机节点。ros2 pkg create keyboard_motor_control --build-type ament_python --dependencies rclpy geometry_msgs--build-type ament_python指定创建Python包。--dependencies rclpy geometry_msgs声明依赖。rclpy是Python的ROS2客户端库geometry_msgs后续可能会用到。创建完成后进入包目录查看结构cd keyboard_motor_control ls你会看到package.xml、setup.py、setup.cfg和keyboard_motor_control目录。3. 定义自定义速度消息虽然我们可以直接使用geometry_msgs/msg/Twist但为了深入理解消息创建过程我们先定义一个更简单的自定义消息。3.1 创建消息定义文件在功能包根目录下创建msg文件夹并在其中创建MotorSpeed.msg文件。mkdir -p ~/keyboard_motor_ws/src/keyboard_motor_control/msg cd ~/keyboard_motor_ws/src/keyboard_motor_control/msg touch MotorSpeed.msg3.2 编辑消息内容用文本编辑器打开MotorSpeed.msg定义两个float32类型的字段分别代表左轮和右轮的目标转速单位rad/s 或 自定义单位。float32 left_speed float32 right_speed3.3 配置包以编译消息需要修改package.xml和setup.py来声明对消息构建的支持。编辑package.xml 在buildtool_dependament_python/buildtool_depend之后添加以下依赖build_dependrosidl_default_generators/build_depend exec_dependrosidl_default_runtime/exec_depend member_of_grouprosidl_interface_packages/member_of_group编辑setup.py 找到data_files部分通常是一个列表。修改它使其包含msg目录。修改后的部分大致如下import os from glob import glob from setuptools import setup package_name keyboard_motor_control setup( # ... 其他参数保持不变 ... data_files[ (share/ament_index/resource_index/packages, [resource/ package_name]), (share/ package_name, [package.xml]), # 添加以下两行将msg文件夹及其内容安装到指定位置 (os.path.join(share, package_name, msg), glob(msg/*.msg)), (os.path.join(share, package_name), glob(launch/*.py)), ], # ... 其他参数保持不变 ... )同时确保setup函数中的install_requires包含了setuptools。4. 编写键盘控制节点现在我们来编写核心的键盘控制节点keyboard_teleop.py。4.1 创建节点文件在keyboard_motor_control/keyboard_motor_control目录下创建该文件。cd ~/keyboard_motor_ws/src/keyboard_motor_control/keyboard_motor_control touch keyboard_teleop.py chmod x keyboard_teleop.py # 使其可执行4.2 编写节点代码以下是keyboard_teleop.py的完整代码包含详细注释#!/usr/bin/env python3 import rclpy from rclpy.node import Node from keyboard_motor_control.msg import MotorSpeed # 导入自定义消息 import sys import select import tty import termios import time class KeyboardTeleopNode(Node): def __init__(self): super().__init__(keyboard_teleop_node) # 创建发布者话题名为 /cmd_motor_speed消息类型为 MotorSpeed self.publisher_ self.create_publisher(MotorSpeed, /cmd_motor_speed, 10) self.speed_msg MotorSpeed() # 创建消息实例 self.left_speed 0.0 self.right_speed 0.0 self.speed_step 0.1 # 每次按键速度增量 self.max_speed 2.0 # 最大速度限制 self.get_logger().info(键盘控制节点已启动) self.get_logger().info(使用 W/S 控制前进/后退速度A/D 控制转向空格键停止Q 退出。) self.print_speed() # 保存终端原始设置用于恢复 self.old_settings termios.tcgetattr(sys.stdin) def get_key(self): 非阻塞获取键盘输入的函数 tty.setraw(sys.stdin.fileno()) rlist, _, _ select.select([sys.stdin], [], [], 0.1) if rlist: key sys.stdin.read(1) else: key termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.old_settings) return key def update_speed(self, key): 根据按键更新速度值 if key w or key W: # 增加前进速度 self.left_speed min(self.left_speed self.speed_step, self.max_speed) self.right_speed min(self.right_speed self.speed_step, self.max_speed) elif key s or key S: # 减少速度可变为负值后退 self.left_speed max(self.left_speed - self.speed_step, -self.max_speed) self.right_speed max(self.right_speed - self.speed_step, -self.max_speed) elif key a or key A: # 左转左轮减速右轮加速或保持 self.left_speed max(self.left_speed - self.speed_step/2, -self.max_speed) self.right_speed min(self.right_speed self.speed_step/2, self.max_speed) elif key d or key D: # 右转右轮减速左轮加速或保持 self.left_speed min(self.left_speed self.speed_step/2, self.max_speed) self.right_speed max(self.right_speed - self.speed_step/2, -self.max_speed) elif key : # 空格键停止 self.left_speed 0.0 self.right_speed 0.0 elif key q or key Q: # Q键退出 self.get_logger().info(正在退出...) return False return True def publish_speed(self): 发布当前速度到话题 self.speed_msg.left_speed self.left_speed self.speed_msg.right_speed self.right_speed self.publisher_.publish(self.speed_msg) self.print_speed() def print_speed(self): 打印当前速度到终端 self.get_logger().info(f当前速度: left{self.left_speed:.2f}, right{self.right_speed:.2f}) def run(self): 节点主循环 try: while rclpy.ok(): key self.get_key() if key: if not self.update_speed(key): break self.publish_speed() # 即使没有按键也以一定频率发布速度保证电机持续收到指令例如维持速度 # 这对于真实电机很重要否则可能因收不到指令而停止。 time.sleep(0.05) # 控制循环频率约20Hz finally: # 退出前发布一次零速确保电机停止 self.left_speed 0.0 self.right_speed 0.0 self.publish_speed() termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.old_settings) self.get_logger().info(键盘控制节点已关闭。) def main(argsNone): rclpy.init(argsargs) node KeyboardTeleopNode() node.run() node.destroy_node() rclpy.shutdown() if __name__ __main__: main()4.3 关键代码解析非阻塞键盘输入get_key函数使用select和tty.setraw实现非阻塞读取这样节点不会在等待键盘输入时卡住可以同时处理ROS2的通信。速度更新逻辑update_speed函数根据不同的按键修改成员变量left_speed和right_speed。这里实现了简单的差速转向模型。消息发布publish_speed函数将速度值赋给MotorSpeed消息实例并通过发布者发送到/cmd_motor_speed话题。资源清理在finally块中无论是否异常退出都会发布零速并恢复终端设置这是一个良好的编程习惯防止电机失控和终端状态错乱。5. 编写模拟电机驱动节点为了测试键盘节点我们需要一个订阅者来接收速度指令。创建motor_driver_sim.py。cd ~/keyboard_motor_ws/src/keyboard_motor_control/keyboard_motor_control touch motor_driver_sim.py chmod x motor_driver_sim.pymotor_driver_sim.py代码如下#!/usr/bin/env python3 import rclpy from rclpy.node import Node from keyboard_motor_control.msg import MotorSpeed class MotorDriverSimNode(Node): def __init__(self): super().__init__(motor_driver_sim_node) # 创建订阅者订阅 /cmd_motor_speed 话题 self.subscription self.create_subscription( MotorSpeed, /cmd_motor_speed, self.speed_callback, 10) self.subscription # 防止未使用变量警告 self.get_logger().info(模拟电机驱动节点已启动等待速度指令...) def speed_callback(self, msg): 收到速度指令后的回调函数 # 在实际硬件中这里会将 msg.left_speed 和 msg.right_speed # 转换为PWM占空比、CAN命令等发送给电机驱动器。 self.get_logger().info(f收到指令 - 左轮转速: {msg.left_speed:.2f}, 右轮转速: {msg.right_speed:.2f}) # 示例模拟一个简单的处理逻辑 # if abs(msg.left_speed) 1.5: # self.get_logger().warn(左轮转速过高) def main(argsNone): rclpy.init(argsargs) node MotorDriverSimNode() rclpy.spin(node) # 保持节点运行等待消息 node.destroy_node() rclpy.shutdown() if __name__ __main__: main()这个节点非常简单它只是订阅话题并将收到的速度值打印出来。在真实项目中speed_callback函数内部会包含与具体电机硬件如通过串口、PWM、CAN总线通信的代码。6. 配置与构建功能包在编译和运行之前需要确保Python能够找到我们的模块并正确声明入口点。6.1 编辑setup.py添加入口点在setup.py的setup()函数中找到entry_points部分修改如下entry_points{ console_scripts: [ keyboard_teleop keyboard_motor_control.keyboard_teleop:main, motor_driver_sim keyboard_motor_control.motor_driver_sim:main, ], },这告诉ROS2当使用ros2 run命令时keyboard_teleop对应执行keyboard_teleop.py中的main函数。6.2 安装依赖并构建工作空间回到工作空间根目录使用colcon构建工具进行构建。cd ~/keyboard_motor_ws # 安装依赖首次构建或修改package.xml后需要 rosdep install -i --from-path src --rosdistro humble -y # 构建功能包 colcon build --packages-select keyboard_motor_control构建成功后你会看到Summary: 1 package finished的提示。每次修改源代码后都需要重新运行colcon build。6.3 激活工作空间环境构建完成后需要“激活”这个工作空间的环境以便ROS2能够找到我们新创建的功能包和节点。source ~/keyboard_motor_ws/install/setup.bash你可以将此命令添加到~/.bashrc中以便每次打开终端自动激活。7. 运行与验证实验现在到了验证成果的时刻。我们将打开三个终端窗口。7.1 终端1运行模拟电机驱动节点source ~/keyboard_motor_ws/install/setup.bash ros2 run keyboard_motor_control motor_driver_sim如果一切正常你将看到输出[motor_driver_sim_node-1] 模拟电机驱动节点已启动等待速度指令...7.2 终端2运行键盘控制节点source ~/keyboard_motor_ws/install/setup.bash ros2 run keyboard_motor_control keyboard_teleop你会看到启动提示信息并且终端会等待你的键盘输入。请确保这个终端窗口是当前焦点否则键盘输入不会被捕获。7.3 验证控制效果在运行keyboard_teleop的终端终端2中尝试按下W、S、A、D、空格键。观察该终端的输出它会显示当前计算出的左右轮速度。同时观察终端1电机节点的输出它会显示每次收到的速度指令。预期行为按W左右轮速度同步增加。按S左右轮速度同步减少可能变为负值代表后退。按A左轮速度减小右轮速度增加机器人向左转。按D右轮速度减小左轮速度增加机器人向右转。按空格速度归零。按Q退出键盘控制节点。7.4 使用ros2 topic命令监控你可以打开第三个终端使用ROS2命令行工具来监控话题通信这是一个非常实用的调试手段。source ~/keyboard_motor_ws/install/setup.bash # 查看当前所有活跃的话题 ros2 topic list # 你应该能看到 /cmd_motor_speed # 实时回显该话题的消息内容 ros2 topic echo /cmd_motor_speed当你按下键盘时这个终端会实时打印出发布到/cmd_motor_speed话题上的MotorSpeed消息内容。8. 常见问题排查与进阶思考即使按照步骤操作你也可能会遇到一些问题。下面是一些常见问题的排查思路。8.1 常见问题排查表问题现象可能原因检查与解决步骤运行ros2 run提示找不到包或节点1. 工作空间未激活。2. 构建失败或未构建。3.setup.py中入口点配置错误。1. 执行source ~/keyboard_motor_ws/install/setup.bash。2. 检查colcon build输出是否有错误。3. 确认setup.py中entry_points的格式和路径正确。键盘控制节点启动后按键无反应1. 终端窗口未获得焦点。2. 键盘输入被其他程序拦截。3. 节点代码中的按键处理逻辑有误。1. 点击运行键盘节点的终端窗口。2. 关闭可能冲突的输入法或其他全局快捷键软件。3. 检查keyboard_teleop.py中get_key和update_speed函数。电机节点收不到消息1. 话题名称不匹配。2. 消息类型不匹配。3. 网络通信问题通常单机不会。1. 使用ros2 topic list确认话题是否存在。使用ros2 topic info /cmd_motor_speed查看发布者和订阅者。2. 使用ros2 interface show keyboard_motor_control/msg/MotorSpeed确认消息类型。自定义消息无法导入1.package.xml和setup.py配置错误。2. 构建后未重新激活环境。3. Python路径问题。1. 仔细核对3.3节的配置步骤。2. 重新执行colcon build和source install/setup.bash。3. 在Python代码中尝试from keyboard_motor_control.msg import MotorSpeed看是否报错。按下Q键节点未退出终端原始模式设置恢复可能有问题。检查keyboard_teleop.py中finally块是否确保执行了termios.tcsetattr。可以尝试用CtrlC强制终止。8.2 从模拟到真实电机控制本实验的电机节点是模拟的。要控制真实电机你需要硬件连接将你的主控板如树莓派、Jetson通过GPIO、I2C、PWM或CAN总线连接到电机驱动器如TB6612、DRV8833、VESC。修改电机节点在motor_driver_sim.py的speed_callback函数中将msg.left_speed和msg.right_speed转换为硬件驱动所需的信号。例如使用PWM你可能需要调用类似RPi.GPIO或pigpio库来设置PWM占空比。转换公式通常是线性的pwm_duty (speed / max_speed) * max_pwm。例如使用串口/UART你需要将速度值格式化为特定的指令帧通过serial库发送给驱动器。加入安全逻辑真实硬件必须考虑急停、堵转检测、过流保护等。可以在节点中加入状态监测和异常处理。8.3 进阶使用标准geometry_msgs/Twist消息在ROS机器人社区控制移动平台速度的标准话题是/cmd_vel消息类型是geometry_msgs/msg/Twist。它包含线速度linear.x,linear.y,linear.z和角速度angular.x,angular.y,angular.z。对于差分轮式机器人通常只使用linear.x前进速度和angular.z旋转角速度。你可以修改键盘节点发布Twist消息并编写一个twist_to_motor节点来订阅/cmd_vel然后根据机器人运动学模型差分驱动模型将linear.x和angular.z解算为左右轮的速度。这更符合ROS生态的通用做法便于集成导航如Nav2等高级功能。8.4 性能与优化建议发布频率键盘节点的发布频率time.sleep(0.05)控制的20Hz对于手动控制通常足够。但对于高速自主导航可能需要更高的频率如50Hz。消息去抖键盘输入可能存在抖动可以加入简单的滤波逻辑比如速度变化过于频繁时进行平滑处理。参数可配置将speed_step、max_speed等参数通过ROS2参数服务器declare_parameter暴露出来这样可以在启动时或运行时动态调整无需修改代码。使用launch文件创建Python或XML格式的launch文件可以方便地一键启动键盘节点和电机节点并设置参数。通过这个实验你不仅实现了一个键盘控制功能更实践了ROS2中节点间通信的完整流程。理解了这个基础框架你就可以在此基础上扩展出更复杂的机器人控制应用。