控制底盘采集环境数据（一）- 键盘控制

为了进行后续的定位SLAM算法、路径跟踪等自动驾驶算法，需要提前采集一些环境数据，此处在我们建立的小车模型、巷道环境下采集激光雷达、IMU、关节等数据，使用rosbag保存用于后续分析处理。

1、之前我们使用rqt向cmd_vel话题写入数据，通过拖动进度条来控制小车

  <node name="rqt_robot_steering" pkg="rqt_robot_steering" type="rqt_robot_steering" />

2、但是这样控制不是特别方便，因此改用键盘控制

先安装上键盘控制节点,注意:melodic替换成自己使用的版本

sudo apt install ros-melodic-teleop-twist-keyboard

执行键盘控制节点，后续直接集成在启动文件

rosrun teleop_twist_keyboard teleop_twist_keyboard.py

运行之前确保ros 管理器开启,若没有开启，执行下列命令

roscore

3、经过尝试也不算很方便的控制自己的小车模型，因此参考以下文章编写自己的控制键盘https://blog.csdn.net/josiah_zhao/article/details/146525779

在之前的控制功能包下创建keyboard_control_node.py文件

("控制指令: w前进 s后退 a左转 d右转 e直行 x停止 q退出")

#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy
import sys
import tty
import termios
import select
import time
from geometry_msgs.msg import Twist

def main():
    print("=== 智能键盘控制（高频检测+10Hz发布）===")
    
    # 初始化ROS
    rospy.init_node('smart_keyboard_control', anonymous=True)
    pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10, latch=True)
    rospy.sleep(0.5)
    
    # 速度状态
    linear_x, angular_z = 0.0, 0.0
    running = True
    publish_count = 0
    
    print("控制指令: w前进 s后退 a左转 d右转 e直行 x停止 q退出")
    print("开始控制（高频检测+10Hz发布）...")
    
    # 保存终端设置
    fd = sys.stdin.fileno()
    old_settings = termios.tcgetattr(fd)
    
    # 设置非阻塞模式
    tty.setraw(fd)
    
    try:
        last_status_time = time.time()
        last_publish_time = time.time()
        
        # 关键参数
        DETECTION_INTERVAL = 0.001    # 检测频率：1000Hz（1ms）
        PUBLISH_INTERVAL = 0.1        # 发布频率：10Hz（100ms）
        
        while running and not rospy.is_shutdown():
            current_time = time.time()
            
            # ========== 高频按键检测（1000Hz） ==========
            key = None
            rlist, _, _ = select.select([sys.stdin], [], [], 0.0)  # 立即返回
            if rlist:
                try:
                    key = sys.stdin.read(1)
                except:
                    pass
            
            # 立即处理按键（无延迟）
            if key:
                if key == 'w':
                    linear_x, angular_z = 5.0, 0.0
                    print("\n🎯 前进")
                elif key == 's':
                    linear_x, angular_z = -5.0, 0.0
                    print("\n🎯 后退")
                elif key == 'a':
                    angular_z = 0.61
                    print("\n🎯 左转")
                elif key == 'd':
                    angular_z = -0.61
                    print("\n🎯 右转")
                elif key == 'e':
                    angular_z = 0.0
                    print("\n🎯 直行")
                elif key == 'x':
                    linear_x, angular_z = 0.0, 0.0
                    print("\n🎯 停止")
                elif key == 'q':
                    running = False
                    print("\n🎯 退出")
            
            # ========== 低频消息发布（10Hz） ==========
            if current_time - last_publish_time >= PUBLISH_INTERVAL:
                msg = Twist()
                msg.linear.x = linear_x
                msg.angular.z = angular_z
                pub.publish(msg)
                publish_count += 1
                last_publish_time = current_time
            
            # ========== 状态显示（0.5秒更新） ==========
            if current_time - last_status_time > 0.5:
                subscribers = pub.get_num_connections()
                status = "⚡ 发布:%d | 速度:%.1f | 转向:%.2f | 检测:1000Hz | 发布:10Hz" % (
                    publish_count, linear_x, angular_z)
                print("\r" + status + " " * 10),
                sys.stdout.flush()
                last_status_time = current_time
            
            # 保持高频检测循环
            time.sleep(DETECTION_INTERVAL)
            
    except KeyboardInterrupt:
        print("\n用户中断")
    except Exception as e:
        print("错误: %s" % e)
    finally:
        # 清理
        termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
        pub.publish(Twist())
        print("\n退出")

if __name__ == '__main__':
    main()

赋予py执行权限

sudo chmod +x keyboard_control_node.py 

并在keyboard_control_node.py 路径下打开终端，启动节点

4、启动后控制,效果如下：