PX4无人机 - 键盘控制飞行代码_px4飞控源代码

PX4无人机 - 键盘控制飞行代码

仿真效果

实机效果
由于图片限制5M以内，只能上传一小段了，整段视频请点击链接 Pixhawk 6c | 无人机 | 键盘控制无人机 | Offboard模式

核心： 发布 mavros/setpoint_velocity/cmd_vel_unstamped 话题，控制x y z三个方向的速度
运行前先运行PX4自带仿真，例如

roslaunch px4 mavros_posix_sitl.launch
1

接着运行以下代码（根据WHEELTEC麦克纳姆轮小车的键盘控制代码改写）

注意：

空格：降落

5 ：开启offboard模式

6 ：解锁，准备起飞

7 ：起飞

控制顺序：
先按 5 开启offboard 模式
再按 6 解锁，会看到浆液开始转动
再按 7 起飞 （这里起飞后就不在 offboard 模式）
再按一次 5 切换到 offboard 模式（之后就可以通过键盘控制前后运动，左右旋转了）

控制运动键如下
i 键：前进
K 键：停止运动
, 键：后退
J 键：向左转
L键：向右转
R键：上升
F键：下降

运动速度调整键如下
常用键如下：
W 键：增加运动线速度
X 键：减少运动线速度
E 键：增加旋转角速度
C 键：减少旋转角速度

降落
空格键

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

import rospy

from geometry_msgs.msg import Twist
from geometry_msgs.msg import PoseStamped
from mavros_msgs.srv import *
from mavros_msgs.msg import State
import math
import sys, select, termios, tty

# 空格：降落
# 5  ：开启offboard模式
# 6  ：解锁，准备起飞
# 7  ：起飞

msg = """
Control Your Turtlebot!
---------------------------
Moving around:
   u    i    o
   j    k    l
   m    ,    .

q/z : increase/decrease max speeds by 10%
w/x : increase/decrease only linear speed by 10%
e/c : increase/decrease only angular speed by 10%
space key, k : force stop
anything else : stop smoothly
b : switch to OmniMode/CommonMode
CTRL-C to quit
"""
Omni = 0 #全向移动模式

#键值对应移动/转向方向
moveBindings = {
        'i':( 1, 0),
        'o':( 1,-1),
        'j':( 0, 1),
        'l':( 0,-1),
        'u':( 1, 1),
        ',':(-1, 0),
        '.':(-1, 1),
        'm':(-1,-1),
           }

#键值对应速度增量
speedBindings={
        'q':(1.1,1.1),
        'z':(0.9,0.9),
        'w':(1.1,1),
        'x':(0.9,1),
        'e':(1,  1.1),
        'c':(1,  0.9),
          }

#获取键值函数
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


speed = 0.2 #默认移动速度 m/s
turn  = 1   #默认转向速度 rad/s
#以字符串格式返回当前速度
def vels(speed,turn):
    return "currently:\tspeed %s\tturn %s " % (speed,turn)

sita = 0.0  # 朝向
z = 0
w = 0
zf = 1
# 回调函数:订阅无人机位姿
def pose_cb(m):
    global sita
    global z
    global w
    global zf
    z = m.pose.orientation.z
    w = m.pose.orientation.w
    # 计算朝向在x轴的上方还是下方
    if z*w > 0:
        zf = 1
    else:
        zf = -1
    sita = 2*math.acos(w)*180/math.pi
    # rospy.loginfo('%.2f\r',sita)

current_state = State()
# 回调函数：订阅mavros状态
def state_cb(state):
    global current_state
    current_state = state

#主函数
if __name__=="__main__":
    settings = termios.tcgetattr(sys.stdin) #获取键值初始化，读取终端相关属性
    
    rospy.init_node('turtlebot_teleop') #创建ROS节点
    pub = rospy.Publisher('mavros/setpoint_velocity/cmd_vel_unstamped', Twist, queue_size=5) #创建速度话题发布者
    # 订阅无人机位姿
    rospy.Subscriber('mavros/local_position/pose',PoseStamped, pose_cb)

    # 订阅mavros状态
    rospy.Subscriber('mavros/state',State,state_cb)

    # 定义起飞降落服务客户端（起飞，降落）
    setModeServer = rospy.ServiceProxy('mavros/set_mode',SetMode)

    armServer = rospy.ServiceProxy('/mavros/cmd/arming', CommandBool)

    x      = 0   #前进后退方向
    y      = 0   #左右移动方向
    z      = 0   #上下移动方向
    th     = 0   #转向/横向移动方向
    count  = 0   #键值不再范围计数
    target_speed = 0 #前进后退目标速度
    target_z_speed = 0 #上下运动目标速度
    target_turn  = 0 #转向目标速度
    control_speed = 0 #前进后退实际控制速度
    control_z_speed = 0 #上下运动实际控制速度
    control_turn  = 0 #转向实际控制速度
    try:
        print(msg) #打印控制说明
        print(vels(speed,turn)) #打印当前速度
        while(1):
            key = getKey() #获取键值

            # if key:
            #     print('key = ',key)
            
            #判断键值是否在移动/转向方向键值内
            # if key in moveBindings.keys():
            #     x  = moveBindings[key][0]
            #     th = moveBindings[key][1]
            #     count = 0

            if key == 'i':   #前进
                count = 0
                x = 1
                z = 0
            elif key == ',': #后退 
                count = 0
                x = -1
                z = 0
            elif key == 'j': #往左转
                count = 0
                th = 1
                z = 0
            elif key == 'l': #往右转
                count = 0
                th = -1
                z = 0
            elif key == 'r': #上升
                count = 0
                z = 1
            elif key == 'f': #下降
                count = 0
                z = -1
            #判断键值是否在速度增量键值内
            elif key in speedBindings.keys():
                speed = speed * speedBindings[key][0]
                turn  = turn  * speedBindings[key][1]
                count = 0
                print(vels(speed,turn)) #速度发生变化，打印出来

            #空键值/'k',相关变量置0
            elif key == 'k' :
                x  = 0
                y  = 0
                z  = 0
                th = 0
                control_speed = 0
                control_z_speed = 0
                control_turn  = 0

            # 降落
            elif key == ' ':
                print("Vehicle Land")
                setModeServer(custom_mode='AUTO.LAND')
            # 开启offboard模式
            elif key == '5':
                if current_state.mode != "OFFBOARD" :
                    setModeServer(custom_mode='OFFBOARD')
                    print("Offboard enabled")
            # 解锁，准备起飞
            elif key == '6':
                armServer(True) 
                print("Vehicle armed")
            # 起飞
            elif key == '7':
                print("Vehicle Takeoff")
                setModeServer(custom_mode='AUTO.TAKEOFF')

            #长期识别到不明键值，相关变量置0
            else:
                count = count + 1
                if count > 4:
                    x  = 0
                    y  = 0
                    z  = 0
                    th = 0
                if (key == '\x03'):
                    break

            #根据速度与方向计算目标速度
            target_speed = speed * x
            target_z_speed = speed * z
            target_turn  = turn * th

            #x方向平滑控制，计算前进后退实际控制速度
            if target_speed > control_speed:
                control_speed = min( target_speed, control_speed + 0.1 )
            elif target_speed < control_speed:
                control_speed = max( target_speed, control_speed - 0.1 )
            else:
                control_speed = target_speed
            
            #z方向平滑控制，实际控制速度
            if target_z_speed > control_z_speed:
                control_z_speed = min( target_z_speed, control_z_speed + 0.1 )
            elif target_z_speed < control_z_speed:
                control_z_speed = max( target_z_speed, control_z_speed - 0.1 )
            else:
                control_z_speed = target_z_speed

            #平滑控制，计算转向实际控制速度
            if target_turn > control_turn:
                control_turn = min( target_turn, control_turn + 0.5 )
            elif target_turn < control_turn:
                control_turn = max( target_turn, control_turn - 0.5 )
            else:
                control_turn = target_turn
         
            # 计算出y方向的sin值
            y_sita = math.sin(sita/180*math.pi)
            # 如果小于0，则改为正数
            if y_sita < 0:
                y_sita = -y_sita
            # 乘以y分量的正负（通过四元数z*w获得，z*w>0,y分量在x轴上方）
            y_sita = y_sita * zf

            twist = Twist()  #创建ROS速度话题变量
            twist.linear.x = control_speed * math.cos(sita/180*math.pi)
            twist.linear.y = control_speed * y_sita  # 朝向速度乘以y轴sin值
            twist.linear.z = control_z_speed
            twist.angular.x = 0
            twist.angular.y = 0
            twist.angular.z = control_turn

            pub.publish(twist) #ROS发布速度话题

    #运行出现问题则程序终止并打印相关错误信息
    except Exception as e:
        print(e)

    #程序结束前发布速度为0的速度话题
    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 = control_turn
        pub.publish(twist)

    #程序结束前设置终端相关属性
    termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)

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
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274

跑实机的话，就不需要运行gazebo仿真了，直接运行mavros的px4.launch文件

roslaunch mavros px4.launch
1

再运行上面代码即可控制实机（我用的飞控是 Pixhawk6C）
实机操作有风险，请各位有十足把握后再尝试实机