在ROS中实现组合按键控制机器人移动

　　前段时间学习《ROS By Example》发现教程功能包中的键盘控制程序无法同时读取多个按键值，实现不了前进+左转或前进+右转等组合按键控制的功能，机器人只能沿直线行走或原地旋转，控制起来十分呆板。我在原有代码的基础上做了一些修改，成功实现了组合按键控制机器人的功能。
　　

一、 程序代码及效果

代码：key_teleop.py

#!/usr/bin/env python
# -*- coding: utf-8 -*-

import rospy
from geometry_msgs.msg import Twist
from evdev import InputDevice
import threading
import sys, tty, termios, select 

# 按键事件编码
CODE_I = 23 
CODE_K = 37
CODE_J = 36
CODE_L = 38
CODE_Q = 16
CODE_Z = 44
CODE_W = 17
CODE_X = 45
CODE_SPACE = 57

flag_run = True

#　按键状态：１表示按下；0表示松开
keys_status = {
                CODE_I : 0, 
                CODE_K : 0,
                CODE_J : 0,
                CODE_L : 0,
                CODE_Q : 0,
                CODE_Z : 0,
                CODE_W : 0,
                CODE_X : 0,
                CODE_SPACE : 0,
               }

# 默认速度、最大最小速度
speed = rospy.get_param("~scale_linear", 0.1) # meters per second
turn = rospy.get_param("~scale_angular", 0.5) # radians per second
speed_max = rospy.get_param("~scale_linear_max", 0.2) # meters per second
turn_max = rospy.get_param("~scale_angular_max", 1.0) # radians per second

# 提示信息
msg = """
        使用键盘控制你的机器人!
-----------------------------------
方向控制:      i(前)    
         j(左) k(后) l(右)

q/z : 线速度增加/减少 0.01 m/s
w/x : 角速度增加/减少 0.05 rad/s
空格键: 立即停止     CTRL-C: 退出
"""

def getKey():
    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, settings)
    return key 

def detectInputKey():
    dev = InputDevice('/dev/input/event2')
    rate = rospy.Rate(10)
    while(not rospy.is_shutdown()):
        if (flag_run == False): 
            break
        select.select([dev], [], [])
        for event in dev.read():
            if (event.value == 1 or event.value == 0) and event.code != 0:
                if event.code in keys_status.keys():
                    if event.value == 1:
                        keys_status[event.code] = 1
                    else:
                        keys_status[event.code] = 0
        rate.sleep()

def vels(speed,turn):
    return "当前速度:  线速度：%sm/s  角速度：%srad/s" % (speed,turn)

if __name__ == "__main__":
    settings = termios.tcgetattr(sys.stdin)

    rospy.init_node('crobot_teleop')
    pub = rospy.Publisher('cmd_vel', Twist, queue_size=5)

    #按键检测线程
    thread_key_detect = threading.Thread(target = detectInputKey)
    thread_key_detect.start()

    x = 0               # 线速度方向
    th = 0              # 角速度方向
    count = 0
    target_speed = 0
    target_turn = 0
    control_speed = 0
    control_turn = 0

    empty_count = 0 # 连续没有检测到方向按键的次数

    try:
        print(msg)
        print(vels(speed,turn))
        rate = rospy.Rate(30)
        while(not rospy.is_shutdown()):
           # 检查CTRL-C是否按下
            ch = getKey()
            if (ch == '\x03'): 
                flag_run = False
                break

            # 当有方向按键按下时
            if (keys_status[CODE_I] == 1 or keys_status[CODE_K] == 1 or 
                keys_status[CODE_J] == 1 or keys_status[CODE_L] == 1):
                empty_count = 0
                if keys_status[CODE_I] == 1: # 前进按键按下
                    x = 1
                    th = 0
                    if keys_status[CODE_J] == 1: # 前进＋左转
                        th = 1
                    elif keys_status[CODE_L] == 1: #前进＋右转
                        th = -1
                elif keys_status[CODE_K] == 1: # 后退按键按下
                    x = -1
                    th = 0
                    if keys_status[CODE_J] == 1: # 后退＋左转
                        th = 1
                    elif keys_status[CODE_L] == 1: #后退＋右转
                        th = -1
                elif keys_status[CODE_J] == 1: # 左转
                    x = 0
                    th = 1
                else: #右转
                    x = 0
                    th = -1
            else:   # 没有任何方向键按下
                x = 0
                th = 0
                empty_count = empty_count + 1

            if (keys_status[CODE_Q] == 1 or keys_status[CODE_Z] == 1 or 
                keys_status[CODE_W] == 1 or keys_status[CODE_X] == 1):
                # 更新线速度
                if keys_status[CODE_Q] == 1:
                    speed = speed + 0.01
                elif keys_status[CODE_Z] == 1:
                    speed = speed - 0.01
                # 更新角速度
                if keys_status[CODE_W] == 1:
                    turn = turn + 0.05
                elif keys_status[CODE_X] == 1:
                    turn = turn - 0.05
                if keys_status[CODE_SPACE] == 1:
                    x = 0
                    th = 0
                    control_speed = 0
                    control_turn = 0
                # 速度限制
                if speed > speed_max:
                    speed = speed_max
                elif speed < 0:
                    speed = 0
                if turn > turn_max:
                    turn = turn_max
                elif turn < 0:
                    turn = 0
                print(vels(speed,turn)) # 显示更新后的速度
                if (count == 9):
                    print(msg)
                count = (count + 1) % 10

            target_speed = speed * x
            target_turn = turn * th

            # 加速/减速过程
            if target_speed > control_speed:
                control_speed = min( target_speed, control_speed + 0.01 )
            elif target_speed < control_speed:
                control_speed = max( target_speed, control_speed - 0.01 )
            else:
                control_speed = target_speed

            if target_turn > control_turn:
                control_turn = min( target_turn, control_turn + 0.05 )
            elif target_turn < control_turn:
                control_turn = max( target_turn, control_turn - 0.05 )
            else:
                control_turn = target_turn

            if(empty_count < 20): # 防止无按键时一直发送Twist消息
                twist = Twist()
                twist.linear.x = control_speed; twist.linear.y = 0; twist.linear.z = 0
                twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = control_turn
                pub.publish(twist)

            rate.sleep()

    except Exception as e:
      print(e)

    finally:
        twist = Twist()
        twist.linear.x = 0; twist.linear.y = 0; twist.linear.z = 0
        twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0
        pub.publish(twist)

    termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)

启动机器人，打开RViz，使用上面的程序控制机器人，效果如下：

二、 实现过程

　　大致的实现过程为：程序循环检测键盘事件，每个按键都对应着一个事件编码和事件值，当按键被按下时事件的值为1，当按键松开时事件的值为0，由此可以实现组合按键的检测。
　　
　　注意：运行此程序之前必须先确定键盘事件所对的事件编号和按键事件的编码，具体过程可参考这篇文章：在Linux下如何用Python监控键盘记录。执行下面的代码可输出各种输入事件对应的事件编码：

#!/usr/bin/env python
#coding: utf-8
import os

# 源目录
deviceFilePath = '/sys/class/input/'

def showDevice():
    os.chdir(deviceFilePath)
    for i in os.listdir(os.getcwd()):
        namePath = deviceFilePath + i + '/device/name'
        if os.path.isfile(namePath):
            print "Name: %s Device: %s" % (i, file(namePath).read())

showDevice()

输出如下：

由图可知键盘事件的编号为event2，将其完整的路径填写到如下位置：

执行程序之前还要获取对/dev/input/event2的读权限：

$ sudo chmod +r /dev/input/event2

为了避免每次启动电脑都重复输入上述指令，可以在/etc/rc.local文件的exit 0 之前添加上述指令。