ROS python 实现键盘控制 底盘移动

需求:

1.解决手柄偶尔失灵问题, 例如:

1. 底盘移动震动导致接收断开,2-3s重连
2. 手柄没电问题

2.用键盘 替代 手柄控制.

功能实现:

方法1: 小乌龟控制键盘重映射

sim_move_ctrl.launch

<launch>
    <!--键盘控制-->
    <node pkg = "turtlesim" type = "turtle_teleop_key" name = "move_teleop_key" output = "screen">
        <remap from="/turtle1/cmd_vel" to="/cmd_vel"/>
    </node>

</launch>

查看话题 rostopic list

/cmd_vel

此方法缺点:

1. 线速度,角速度值固定
2. 其它按键，不能做更多的功能定制，如:
   加速，减速，任务开始，任务停止。

方法2: 自己实现,定制按键功能

按键功能

---

w 前进
a 右转 d 左转
s 后退

空格: 立即停止
Esc: 退出
h 帮助

注意:
长按 w\a\s\d 方向键移动
松开 按键停止

---

#!/usr/bin/env python3
#coding=utf-8

''' 
功能:
    实现输入w,s,a,d命令控制底盘移动
'''

import sys
import tty
import termios
import select
import time

# ROS
import rospy
from geometry_msgs.msg import Twist

class Noblock_terminal:
    def __init__(self):
        fd = sys.stdin.fileno()
        self.old_settings = termios.tcgetattr(fd)
        tty.setraw(sys.stdin.fileno(), termios.TCSANOW)

    def __exit__(self):
        if self.old_settings:
            self.stop_no_block()

    def get_char(self):
        ch = sys.stdin.read(1)
        # sys.stdout.write(ch)
        return ch

    def select_cmd(self):
        read_list = [sys.stdin]
        cmd = '0'
        read_ret,write_ret,err_ret = select.select(read_list,[],[],0.2)
        if read_ret:
            for fd in read_ret:
                if fd == sys.stdin:
                    cmd = sys.stdin.read(1)
                else:
                    print("unknow fd")
        else:
            # print("read timeout")
            pass

        # print("cmd:%s hex=%x"%(cmd,ord(cmd)))

        return cmd

    def stop_no_block(self):
        fd = sys.stdin.fileno()
        termios.tcsetattr(fd, termios.TCSADRAIN, self.old_settings)
        self.old_settings = None

class Sim_ctrl_move:
    def __init__(self):
        rospy.init_node('sim_ctrl_move', anonymous=True,disable_signals=True)
        self.pub_twist = rospy.Publisher('/move/cmd_vel', Twist, queue_size=10)
        self.help()
        self.last_show_help_time = time.time()

    def help(self):
        info = \
'''
\033[80D     w 前进
\033[80D a 右转      d 左转
\033[80D     s 后退 \n

\033[80D 空格: 立马停止
\033[80D Esc: 退出
\033[80D h 帮助
'''
        print(info)

    def timing_show_help(self):
        if time.time() - self.last_show_help_time > 10.0:
            self.last_show_help_time = time.time()
            self.help()

    def publisher_cmdvel(self,x:float,z:float):
        var = Twist()
        var.linear.x = x
        var.angular.z = z
        # 平面移动机器人常常 linear.y和linear.z为0, linear.x 表示前方
        #           angular.z代表平面机器人的角速度，因为此时z轴为旋转轴
        self.pub_twist.publish(var)
        
    def loop_run(self):
        nterm = Noblock_terminal()
        key_flag = True
        while True:
            # cmd = nterm.get_char()
            x:float = 0
            z:float = 0
            cmd = nterm.select_cmd()
            if cmd == 'w':
                print("前进")
                x= 0.3
            elif cmd == 's':
                print("后退")
                x= -0.3
            # elif cmd == 'q':
            #     print("右前")
            # elif cmd == 'e':
            #     print('左前')
            elif cmd == 'a':
                print('左转')
                z= 0.2
            elif cmd == 'd':
                print('右转')
                z= -0.2
            elif cmd == ' ':
                print('立即停止')
                x = 0
                z = 0
            elif cmd == 'h':
                self.help()
            elif ord(cmd) == 0x1b:   # Esc 16进制
                print('退出')
                break

            if key_flag == True and cmd == '0':
                key_flag = False
                self.publisher_cmdvel(0,0)
            elif cmd != '0':
                key_flag = True
                self.publisher_cmdvel(x,z)
            # 回到行首
            sys.stdout.write("\033[80D")
            self.timing_show_help()
        nterm.stop_no_block()

if __name__ == "__main__":
    sim_ctrl = Sim_ctrl_move()
    sim_ctrl.loop_run()

注意事项:

1. 将/move/cmd_vel 修改为自己的话题节点:
2. 空格键 -立即停止说明:
   松开方向按键，需要0.2s, 才会发速度0，设备才会停止移动。
   如果你对控制灵敏度高，可以修改select超时数值, 或按 空格 立即停止
3. 功能定制
   我们的项目有加速，减速，任务开始\暂停\继续\停止 功能，其它功能实现.
   小伙伴根据自己项目定制,实现.

---

我是一名ROS机器人, 嵌入式/中间件开发工程师.