【PX4】PX4第一个offborad例程

1. 什么是OFFBOARD

PX4的OFFBOARD指的是外部控制模式，飞行器根据飞行控制栈外部（如机载计算机）提供的设定值控制位置、速度、加速度、姿态以及推力/力矩。设置值可以经由 MAVLink 提供。
PX4要求外部控制器提供2Hz（至少是2Hz）连续的“有效存在”信号，该信号可由任意支持的 MAVLink 设置点消息或ROS 2 OffboardControlMode 消息提供。 PX4只有在收到该种信号超过1秒后才有效，如果该种信号停止飞行控制栈将重新获得控制权（脱离Offboard模式）。

一些注意点：

• Offboard模式通过设置位置、速度、加速、姿态、姿态角速率或力/扭矩设置点来控制飞行器的移动和姿态。
• PX4 必须能够以2Hz的速率连续收到 MAVLink 设置点消息或 ROS 2 OffboardControlMode 消息以确保外部控制器是正常运行的。 该消息必须已经持续发送1秒钟以上PX4才能在Offboard模式下解锁或在飞行中切换至Offboard模式。 如果在外部控制器给出的指令速率低于2Hz，PX4将在超时(COM_OF_LOSS_T)后退出Offboard模式，并尝试降落或执行其他一些失败保护行为。 失效保护行为取决于RC遥控器是否可用，依据参数 COM_OBL_RC_ACT 的设定。
• 当使用 MAVLink 时，设定值消息既传达了指示外部控制器"正常运行"的信号也传达了设定值本身。 Offboard模式下要保持位置，飞行器必须接收到一个包含当前位置设定值的消息指令。
• 当使用 ROS 2 时，外部控制器运行正常通过监测 OffboardControlMode 消息流确保，真实设定点由发布任一包含设定值的uORB消息提供，例如 TrajectorySetpoint。 在这种情况下保持当前位置，飞行器必须收到连续的 OffboardControlMode 消息，但只需要收到 TrajectorySetpoint 消息一次。
• 请注意，Offboard模式只支持非常有限的 MAVLink 命令和消息。 其他操作如起飞、降落、返航，最好使用适当的模式来处理。 像上传、下载任务这样的操作可以在任何模式下执行。

2. 第一个offboard例程

本教程使用 Gazebo Classic 模拟的Iris四旋翼无人机，用MAVROS Python来展示 OFFBOARD 控制的基础知识。

首先创建一个工作空间并进行初始化

mkdir -p ~/catkin_ws/src
cd ~/catkin_ws/src
catkin_init_workspace
cd ..
catkin_make

然后在这个工作空间下创建一个功能包，命名为sitl_study依赖rospy

cd ~/catkin_ws/src
catalkin_create_pkg sitl_study rospy

再在功能包中创建我们的scripts目录用于存放python脚本

roscd sitl_study
mkdir scripts
cd scripts

进入scripts目录，创建我们的可执行脚本，命名为offb_node.py，并且添加可执行权限

touch offb_node.py
chmod +x offb_node.py

然后打开offb_node.py添加下面的代码

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

import rospy
from geometry_msgs.msg import PoseStamped
from mavros_msgs.msg import State
from mavros_msgs.srv import CommandBool, CommandBoolRequest, SetMode, SetModeRequest

current_state = State()

def state_CB(msg):
    global current_state
    current_state = msg


if __name__ == "__main__":
    # initialize ros node
    rospy.init_node('offb_node_py')

    # state subscriber
    state_sub = rospy.Subscriber('/mavros/state', State, callback=state_CB)

    # pose publisher
    local_pose_pub = rospy.Publisher('/mavros/setpoint_position/local', PoseStamped, queue_size=10)
    
    # arming client
    arming_client = rospy.ServiceProxy('/mavros/cmd/arming', CommandBool)
    arming_client.wait_for_service()

    # mode client
    setmode_client = rospy.ServiceProxy('/mavros/set_mode', SetMode)
    setmode_client.wait_for_service()

    # setpoint publishing MUST be faster than 2HZ
    rate = rospy.Rate(20)

    # wait for Flight Controller connection
    while (not rospy.is_shutdown() and not current_state.connected):
        rate.sleep()
    
    # set point
    pose = PoseStamped()
    pose.pose.position.x = 0
    pose.pose.position.y = 0
    pose.pose.position.z = 2.0

    # send a few setpoints before starting
    for i in range(20):
        if(rospy.is_shutdown()):
            break
        local_pose_pub.publish(pose)
        rate.sleep()
    
    # set OFFBOARD mode
    offb_set_mode = SetModeRequest()
    offb_set_mode.custom_mode = 'OFFBOARD'

    # arm vehicle
    arm_cmd = CommandBoolRequest()
    arm_cmd.value = True

    # request time
    last_req = rospy.Time.now()

    while(not rospy.is_shutdown()):
        if (current_state.mode != "OFFBOARD" and (rospy.Time.now() - last_req) > rospy.Duration(5.0)):
            if(setmode_client.call(offb_set_mode).mode_sent == True):
                rospy.loginfo(">>>>>> OFFBOARD enabled >>>>>>")
            last_req = rospy.Time.now()
            
        else:
            if(not current_state.armed and (rospy.Time.now() - last_req) > rospy.Duration(5.0)):
                if(arming_client.call(arm_cmd).success == True):
                    rospy.loginfo(">>>>>> Vehicle armed >>>>>>>")
                last_req = rospy.Time.now()
        
        local_pose_pub.publish(pose)
        rate.sleep()

在CMakelists.txt中添加节点路径，如下所示

#############
## Install ##
#############

# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html

## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
catkin_install_python(PROGRAMS
  scripts/offb_node.py
  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

修改完毕之后，我们首先进行编译

cd ~/catkin_ws
catkin_make
source ./devel/setup.bash

然后我们就可以测试是否编写成功了，首先启动

roslaunch px4 mavros_posix_sitl.launch 

然后再启动

rosrun sitl_study offb_node.py

我们就能看到无人机先解锁然后转到OFFBORAD模式，然后起飞到两米，如下图所示

3. 编写launch文件

虽然上述的启动方式能够实现让飞机启动，但是需要我们手动输入两次命令，非常不优雅，我们可以编写一个launch文件来集成这两者。

roscd sitl_study
mkdir launch
cd launch
touch offb_start.launch

然后打开该文件，输入以下内容

<?xml version="1.0"?>
<launch>
    <!-- Include the MAVROS node with SITL and Gazebo -->
    <include file="$(find px4)/launch/mavros_posix_sitl.launch">
    </include>

    <!-- Our node to control the drone -->
    <node pkg="sitl_study" type="offb_node.py" name="offb_node_py" required="true" output="screen" />
</launch>

然后再进行一次编译

cd ~/catkin_ws
catkin_make
source ./devel/setup.bash

我们就可以在任意路径下启动了

roslaunch sitl_study offb_start.launch

Reference

PX4官网的Offobrad模式介绍
PX4官网的控制例程