PX4無人機 - 鍵盤控制飛行代碼

仿真效果

實機效果
由于圖片限制5M以內(nèi)，只能上傳一小段了，整段視頻請點擊鏈接 Pixhawk 6c | 無人機 | 鍵盤控制無人機 | Offboard模式

核心： 發(fā)布 mavros/setpoint_velocity/cmd_vel_unstamped 話題，控制x y z三個方向的速度
運行前先運行PX4自帶仿真，例如

roslaunch px4 mavros_posix_sitl.launch

接著運行以下代碼（根據(jù)WHEELTEC麥克納姆輪小車的鍵盤控制代碼改寫）

注意：

空格：降落

5 ：開啟offboard模式

6 ：解鎖，準備起飛

7 ：起飛

控制順序：
先按 5 開啟offboard 模式
再按 6 解鎖，會看到漿液開始轉(zhuǎn)動
再按 7 起飛 （這里起飛后就不在 offboard 模式）
再按一次 5 切換到 offboard 模式（之后就可以通過鍵盤控制前后運動，左右旋轉(zhuǎn)了）

控制運動鍵如下
i 鍵：前進
K 鍵：停止運動
, 鍵：后退
J 鍵：向左轉(zhuǎn)
L鍵：向右轉(zhuǎn)
R鍵：上升
F鍵：下降

運動速度調(diào)整鍵如下
常用鍵如下：
W 鍵：增加運動線速度
X 鍵：減少運動線速度
E 鍵：增加旋轉(zhuǎn)角速度
C 鍵：減少旋轉(zhuǎn)角速度

降落
空格鍵

#!/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 #全向移動模式

#鍵值對應移動/轉(zhuǎn)向方向
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),
          }

#獲取鍵值函數(shù)
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   #默認轉(zhuǎn)向速度 rad/s
#以字符串格式返回當前速度
def vels(speed,turn):
    return "currently:\tspeed %s\tturn %s " % (speed,turn)

sita = 0.0  # 朝向
z = 0
w = 0
zf = 1
# 回調(diào)函數(shù):訂閱無人機位姿
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()
# 回調(diào)函數(shù)：訂閱mavros狀態(tài)
def state_cb(state):
    global current_state
    current_state = state

#主函數(shù)
if __name__=="__main__":
    settings = termios.tcgetattr(sys.stdin) #獲取鍵值初始化，讀取終端相關(guān)屬性
    
    rospy.init_node('turtlebot_teleop') #創(chuàng)建ROS節(jié)點
    pub = rospy.Publisher('mavros/setpoint_velocity/cmd_vel_unstamped', Twist, queue_size=5) #創(chuàng)建速度話題發(fā)布者
    # 訂閱無人機位姿
    rospy.Subscriber('mavros/local_position/pose',PoseStamped, pose_cb)

    # 訂閱mavros狀態(tài)
    rospy.Subscriber('mavros/state',State,state_cb)

    # 定義起飛降落服務(wù)客戶端（起飛，降落）
    setModeServer = rospy.ServiceProxy('mavros/set_mode',SetMode)

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

    x      = 0   #前進后退方向
    y      = 0   #左右移動方向
    z      = 0   #上下移動方向
    th     = 0   #轉(zhuǎn)向/橫向移動方向
    count  = 0   #鍵值不再范圍計數(shù)
    target_speed = 0 #前進后退目標速度
    target_z_speed = 0 #上下運動目標速度
    target_turn  = 0 #轉(zhuǎn)向目標速度
    control_speed = 0 #前進后退實際控制速度
    control_z_speed = 0 #上下運動實際控制速度
    control_turn  = 0 #轉(zhuǎn)向?qū)嶋H控制速度
    try:
        print(msg) #打印控制說明
        print(vels(speed,turn)) #打印當前速度
        while(1):
            key = getKey() #獲取鍵值

            # if key:
            #     print('key = ',key)
            
            #判斷鍵值是否在移動/轉(zhuǎn)向方向鍵值內(nèi)
            # 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': #往左轉(zhuǎn)
                count = 0
                th = 1
                z = 0
            elif key == 'l': #往右轉(zhuǎn)
                count = 0
                th = -1
                z = 0
            elif key == 'r': #上升
                count = 0
                z = 1
            elif key == 'f': #下降
                count = 0
                z = -1
            #判斷鍵值是否在速度增量鍵值內(nèi)
            elif key in speedBindings.keys():
                speed = speed * speedBindings[key][0]
                turn  = turn  * speedBindings[key][1]
                count = 0
                print(vels(speed,turn)) #速度發(fā)生變化，打印出來

            #空鍵值/'k',相關(guān)變量置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')

            #長期識別到不明鍵值，相關(guān)變量置0
            else:
                count = count + 1
                if count > 4:
                    x  = 0
                    y  = 0
                    z  = 0
                    th = 0
                if (key == '\x03'):
                    break

            #根據(jù)速度與方向計算目標速度
            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

            #平滑控制，計算轉(zhuǎn)向?qū)嶋H控制速度
            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，則改為正數(shù)
            if y_sita < 0:
                y_sita = -y_sita
            # 乘以y分量的正負（通過四元數(shù)z*w獲得，z*w>0,y分量在x軸上方）
            y_sita = y_sita * zf

            twist = Twist()  #創(chuàng)建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發(fā)布速度話題

    #運行出現(xiàn)問題則程序終止并打印相關(guān)錯誤信息
    except Exception as e:
        print(e)

    #程序結(jié)束前發(fā)布速度為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)

    #程序結(jié)束前設(shè)置終端相關(guān)屬性
    termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)

跑實機的話，就不需要運行g(shù)azebo仿真了，直接運行mavros的px4.launch文件

roslaunch mavros px4.launch

再運行上面代碼即可控制實機（我用的飛控是 Pixhawk6C）
實機操作有風險，請各位有十足把握后再嘗試實機文章來源地址http://www.zghlxwxcb.cn/news/detail-600874.html

到了這里，關(guān)于PX4無人機 - 鍵盤控制飛行代碼的文章就介紹完了。如果您還想了解更多內(nèi)容，請在右上角搜索TOY模板網(wǎng)以前的文章或繼續(xù)瀏覽下面的相關(guān)文章，希望大家以后多多支持TOY模板網(wǎng)！