ROS python 实现键盘控制 底盘移动_ros如何控制机器人底盘

需求:

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>
1
2
3
4
5
6
7

查看话题 rostopic list

/cmd_vel

此方法缺点:

1. 线速度,角速度值固定
2. 其它按键不能做功能定制

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

按键功能

---

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

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

注意:
长按 方法键移动
松开 按键停止

---

#!/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
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138

注意事项:

1. 将/move/cmd_vel 修改为自己的话题节点:
2. 空格键 -立即停止说明:
   松开按键select超时0.2s, 如果你对控制灵敏度高.
   可以修改select超时时间, 或按 空格 立即停止
3. 功能定制
   我们的项目有加速,减速功能,其它功能回调实现.
   小伙伴根据自己项目定制,实现.

---

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