通过mavros+gazebo+px4实现offboard模式螺旋线（胎教级别）

闫祁晨

已于 2023-07-16 19:58:25 修改

阅读量580

点赞数

分类专栏： px4与ros学习笔记文章标签：自动驾驶 vscode 算法单片机

于 2023-07-16 19:53:12 首次发布

本文链接：https://blog.csdn.net/weixin_43261469/article/details/131753105

版权

px4与ros学习笔记专栏收录该内容

1 篇文章 1 订阅

订阅专栏

1.简介

本文将介绍通过mavros+gazebo+px4实现offboard模式,无人机螺旋上升的超简单流程。很多小伙伴在刚接触offoard的时候都会遇到跑不通例程的问题，看博客时一知半解，即使博主给出代码也不知道代码怎么运行怎样build文件。

2.运行环境

2.1.版本要求

px4安装版本：https://dev.px4.io/v1.11_noredirect/en/setup/dev_env_linux_ubuntu.html

ubuntu ：20.04

ros : noetic

mavros

qgroundcountrol

安装过程不再赘述，按照官方教程，问问师兄，使用正确上网方式，基本都很好装。

3.实现流程

3.1启动 px4和gazebo节点

找到PX4的安装路径，make一下（我的在根目录）：

cd Firmware #进入自己的目录下
make px4_sitl gazebo_iris

有报错不影响，出现如下输出基本没问题，有问题可以提出在评论区。

INFO  [mavlink] MAVLink only on localhost (set param MAV_{i}_BROADCAST = 1 to enable network)
INFO  [px4] Startup script returned successfully
pxh> INFO  [mavlink] partner IP: 127.0.0.1
INFO  [commander] Ready for takeoff!

3.2 启动ros核心

新建一个端口启动ros核心

roscore   #不要关闭！！

3.3启动QGroundControl.AppImage

我测试过4.0.8是没有问题的，需要注意连接方式，一般可以自动连接。如果不能的话按下图自行建立连接。

3.4建立mavros与px4的通信

这段代码说实话，我也不是很理解，大概意思是确定了就是主机的通信端口以及软环中px4的udp端口。

roslaunch mavros px4.launch fcu_url:="udp://:14540@127.0.0.1:14557"

4. 建立功能包

4.1 创建工作空间

mkdir -p catkin_ws/src       #(必须得有 src)
cd catkin_ws
catkin_make

进入 catkin_ws 启动 vscode

cd catkin_ws
code .

vscode 中创建功能包并编译 ros：

在vscode的资料管理器中选中选定 src 右击 ---> create catkin package。

在弹出的对话框输入：

offboard_sin

回车后，再输入依赖：

roscpp std_msgs geometry_msgs mavros_msgs

建立正确的文件夹后，新建文件offboard_node.cpp的位置如图所示：

（如果这步不会可以参考http://www.autolabor.com.cn/book/ROSTutorials/chapter1/14-ros-ji-cheng-kai-fa-huan-jing-da-jian/142-an-zhuang-vscode.html以及对应的课程，该文档对应的课程在b站有公开课，从文档可以找到对应连接）

建立cpp文件后,复制如下代码到文件中，用快捷键 ctrl + shift + B 调用编译，选择:catkin_make:build：

（（根据官方offboard代码示例改写，链接：Redirecting to latest version of document (main)））

/**
 * @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 <geometry_msgs/Vector3.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>

#define PI acos(-1)

mavros_msgs::State current_state;
geometry_msgs::PoseStamped current_position;
void state_cb(const mavros_msgs::State::ConstPtr& msg){
    current_state = *msg;
}
void getpointfdb(const geometry_msgs::PoseStamped::ConstPtr& msg){
    ROS_INFO("x: [%f]", msg->pose.position.x);
    ROS_INFO("y: [%f]", msg->pose.position.y);
    ROS_INFO("z: [%f]", msg->pose.position.z);
    current_position = *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::Subscriber get_point = nh.subscribe<geometry_msgs::PoseStamped>
            ("mavros/local_position/pose", 10, getpointfdb);
            
    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.0f);

    // 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
    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";//将飞行模式设置为 "OFFBOARD"。

    mavros_msgs::CommandBool arm_cmd;//用于解锁或锁定无人机。
    arm_cmd.request.value = true;//请求解锁无人机。

    ros::Time last_request = ros::Time::now();//将当前的ROS时间赋值给变量 last_request

    while(ros::ok()){
        if( current_state.mode != "OFFBOARD" &&
            (ros::Time::now() - last_request > ros::Duration(5.0f))){
            if( set_mode_client.call(offb_set_mode) &&
                offb_set_mode.response.mode_sent){
                ROS_INFO("Offboard enabled");//確保飛行模式為offboard
            }
            last_request = ros::Time::now();
        } else {
            if( !current_state.armed &&
                (ros::Time::now() - last_request > ros::Duration(5.0f))){
                if( arming_client.call(arm_cmd) &&
                    arm_cmd.response.success){
                    ROS_INFO("Vehicle armed");//確保飛行解鎖
                }
                last_request = ros::Time::now();
            }
        }
         
        if((abs(current_position.pose.position.x-pose.pose.position.x)<0.5f)
            &&(abs(current_position.pose.position.y-pose.pose.position.y)<0.5f)
            &&(abs(current_position.pose.position.y-pose.pose.position.y)<0.5f))
        {
            pose.pose.position.x = 20*sin(pose.pose.position.z);
            pose.pose.position.y = 20*cos(pose.pose.position.z);
            pose.pose.position.z += 0.001;
        }

        local_pos_pub.publish(pose);

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

    return 0;
}

修改src/offboard_sin/CMakeLists.txt路经下的CMakeLists.txt文件，在最后加入下面两行(一定是文末)：

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

编译代码

快捷键 ctrl + shift + B 调用编译，选择:catkin_make:build

编译完成会提示如下类似内容：

[build] [1/2  50% :: 1.971] Building CXX object CMakeFiles/offboard_node.dir/src/offboard_node.cpp.o
[build] [2/2 100% :: 2.095] Linking CXX executable devel/lib/offboard_sin/offboard_node
[driver] Build completed: 00:00:02.112
[build] Build finished with exit code 0

结束啦！然后就是rosrun运行节点，发布无人机的位置话题。

5.例程实现

编译完成后，运行节点的终端里，一定记得在终端source一下。具体路径每个人不一样，可以参考之前那个连接里的公开课教程。如下是我的工作空间路径和执行的命令，不要全文复制，执行的命令是：source ./devel/setup.bash

~/catkin_ws$ source ./devel/setup.bash

把钱前面的东西都配置好以后，就可以按照我图里的终端运行啦！

运行顺序可以自行探索，注意终端命令和文件夹的匹配。我给出一个比较好理解的顺序：

不要铁头复制，$后面才是命令!前面是路径！！！

#第一个终端0
yqc@yqc-virtual-machine:~$ roscore #开启ros核心
#新建终端1
yqc@yqc-virtual-machine:~/Firmware$ make px4_sitl gazebo_iris #makepx4软环和gazebo仿真环境
#新建终端2
yqc@yqc-virtual-machine:~/catkin_ws$ rosrun offboard_sin offboard_node  #启动位置发布节点 
#新建终端3
yqc@yqc-virtual-machine:~$ roslaunch mavros px4.launch fcu_url:="udp://:14540@127.0.0.1:14557"  #建立通信

运行结果图：