官方手册参考链接：https://docs.px4.io/master/zh/ros/mavros_offboard.html

0. 准备工作

0.1 安装 ROS Melodic 和 PX4

wget https://raw.githubusercontent.com/PX4/Devguide/master/build_scripts/ubuntu_sim_ros_melodic.sh
bash ubuntu_sim_ros_melodic.sh
  • ROS Melodic 已经默认跟 Gazebo9 一起安装了。
  • 你的 catkin（ROS编译系统）工作空间已经创建在~/catkin_ws/中。
  • 如果发生报错，可以执行如下命令解决：
sudo pip install future
sudo apt install python3-pip
pip3 install pyulog

0.2 Daily Builds 安装

注意：该版本为 QGC 的非稳定版本，稳定版本的尚不支持一些新飞控的固件烧写。

官方手册参考链接：Daily Builds

下载后得到如下图的文件，双击运行即可：

image-20210819224125429

0.3 克隆固件

# 新终端
git clone https://github.com/PX4/PX4-Autopilot.git
# 如果熟悉 github 想使用 git 来管理项目，可以先 fork 到自己的账户再克隆

0.4 Gazebo 仿真

pip3 install --user toml
pip3 install --user empy
pip3 install --user packaging
pip3 install --user jinja2

sudo apt install libgstreamer1.0-dev
sudo apt install gstreamer1.0-plugins-good
sudo apt install gstreamer1.0-plugins-bad
sudo apt install gstreamer1.0-plugins-ugly

make px4_sitl gazebo # 加载完成后，按下回车键
commander arm
commander takeoff
commander land

上条命令可能会碰到 3 个报错，解决方法在下面这个链接。

Bug 解决方法：make px4_sitl gazebo 时出现 CMake Error: The following variables are used in this project

1. C++ 实现

# 创建工作空间 catkin_ws ，不再赘述
cd ~/catkin_ws/src # 进入 src 文件夹
catkin_create_pkg offboard roscpp mavros_msgs # 创建功能包名为 offboard ，依赖为 roscpp ，mavros_msgs

1.1 CMakeLists.txt 的修改

新建 offb_node.cpp ，并在 CMakeLists.txt 中添加：

add_executable(offb_node src/offb_node.cpp)
target_link_libraries(offb_node ${catkin_LIBRARIES)}

1.2 节点文件 offb_node.cpp

代码的注释在官方手册中已经给出了，需要 ROS 基础才能看懂。

/**
 * @file offb_node.cpp
 * @brief Offboard control example node, written with MAVROS version 0.19.x, PX4 Pro Flight
 * Stack and tested in Gazebo SITL
 */

#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>

mavros_msgs::State current_state; // 创建全局变量
// 订阅无人机状态的回调函数将状态信息赋值给全局变量
void state_cb(const mavros_msgs::State::ConstPtr& msg){
    current_state = *msg;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "offb_node");
    ros::NodeHandle nh;

    ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
            ("mavros/state", 10, state_cb);
    ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>
            ("mavros/setpoint_position/local", 10);
    ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
            ("mavros/cmd/arming");
    ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
            ("mavros/set_mode");

    //the setpoint publishing rate MUST be faster than 2Hz
    ros::Rate rate(20.0);

    // wait for FCU connection
    while(ros::ok() && !current_state.connected){
        ros::spinOnce();
        rate.sleep();
    }

    geometry_msgs::PoseStamped pose;
    pose.pose.position.x = 0;
    pose.pose.position.y = 0;
    pose.pose.position.z = 2;

    // send a few setpoints before starting
      // 在切换到offboard模式之前，你必须先发送一些期望点信息到飞控中。 不然飞控会拒绝切换到offboard模式。
    for(int i = 100; ros::ok() && i > 0; --i){
        local_pos_pub.publish(pose);
        ros::spinOnce();
        rate.sleep();
    }

    mavros_msgs::SetMode offb_set_mode;
    offb_set_mode.request.custom_mode = "OFFBOARD";

    mavros_msgs::CommandBool arm_cmd;
    arm_cmd.request.value = true;

    ros::Time last_request = ros::Time::now();

    while(ros::ok()){
        if( current_state.mode != "OFFBOARD" &&
            (ros::Time::now() - last_request > ros::Duration(5.0))){
            if( set_mode_client.call(offb_set_mode) &&
                offb_set_mode.response.mode_sent){
                ROS_INFO("Offboard enabled");
            }
            last_request = ros::Time::now();
        } else {
            if( !current_state.armed &&
                (ros::Time::now() - last_request > ros::Duration(5.0))){
                if( arming_client.call(arm_cmd) &&
                    arm_cmd.response.success){
                    ROS_INFO("Vehicle armed");
                }
                last_request = ros::Time::now();
            }
        }

        local_pos_pub.publish(pose);

        ros::spinOnce();
        rate.sleep();
    }

    return 0;
}

1.3 编译测试

catkin_make # 在工作空间根目录下编译

# 新终端
# 进入 px4 固件的根目录
make px4_sitl gazebo

# 新终端
roslaunch mavros px4.launch fcu_url:="udp://:14540@127.0.0.1:14557"

# 新终端
rosrun offboard offb_node # 启动节点

image-20210818220156479

mavros offboard 模式仿真

2. Python2 实现

参考链接：PX4 + MAVROS —- 实现第一个 offboard control 程序

在功能包下新建文件夹 script ，文件夹下新建文件 offb_node.py 。

2.1 节点文件 offb_node.py

#!/usr/bin/env python
import rospy
from geometry_msgs.msg import Twist
import sys, select, termios, tty
import rospy
from mavros_msgs.msg import PositionTarget, State, HomePosition
from mavros_msgs.srv import CommandBool, CommandTOL, SetMode
from geometry_msgs.msg import PoseStamped, Twist
from sensor_msgs.msg import Imu, NavSatFix
from std_msgs.msg import Float32, String
import time
import math


msg = """
$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$
%%%%%%%%%%%%%%%%%%%%%%%
offboard_cotrol
%%%%%%%%%%%%%%%%%%%%%%%
---------------------------
CTRL-C to quit

"""
cur_Position_Target = PositionTarget()
mavros_state = State()
armServer = rospy.ServiceProxy('/mavros/cmd/arming', CommandBool)
setModeServer = rospy.ServiceProxy('/mavros/set_mode', SetMode)
local_target_pub = rospy.Publisher('/mavros/setpoint_raw/local', PositionTarget, queue_size=10)
def __init__():
    rospy.init_node('PX4_AuotFLy' ,anonymous=True)
    rospy.Subscriber("/mavros/state", State, mavros_state_callback)
        #rospy.Subscriber("/mavros/local_position/pose",HomePosition, mavros_pos_callback)
    print("Initialized")

def mavros_state_callback(msg):
    global mavros_state
    mavros_state = msg
#    if mavros_state.armed == 0 :
#        print("disarm")

def mavros_pos_callback(msg):
        global mavros_pos
        mavros_pos = msg

def Intarget_local():
    set_target_local = PositionTarget()
    set_target_local.type_mask = 0b100111111000  
        set_target_local.coordinate_frame = 1
        set_target_local.position.x = 0
        set_target_local.position.y = 0
        set_target_local.position.z = 2
        set_target_local.yaw = 0
    return set_target_local

def run_state_update():
    if mavros_state.mode != 'OFFBOARD':
                setModeServer(custom_mode='OFFBOARD')
                local_target_pub.publish(cur_Position_Target)
            print("wait offboard")
        else: 
            local_target_pub.publish(cur_Position_Target)
        print("local_target_pub.publish")

global cur_Position_Target
cur_Position_Target = Intarget_local()

if __name__=="__main__":
    settings = termios.tcgetattr(sys.stdin)
    print (msg)
    __init__()
    if armServer(True) :
            print("Vehicle arming succeed!")
        if setModeServer(custom_mode='OFFBOARD'):
            print("Vehicle offboard succeed!")
    else:
            print("Vehicle offboard failed!")
    while(1):
                run_state_update()
                time.sleep(0.2)

2.2 测试

# 新终端
# 进入 px4 固件的根目录
make px4_sitl gazebo

# 新终端
roslaunch mavros px4.launch fcu_url:="udp://:14540@127.0.0.1:14557"

# 新终端
rosrun offboard offb_node.py # 启动节点