刚下好ROS，如何从零开始实现px4的无人机仿真

刚刚入门ROS开发，作为刚刚挺过艰难的ubuntu安装、ros安装的萌新来说，你应该正处于ROS开发的初始过渡阶段。一方面，你庆幸你解决了许多安装新系统的许多bug，跨过了阻拦了无数人入门ROS的一道坎，另一方面，你开始惆怅，面对着空荡荡的新系统的新桌面，你不知道接下来的一步该怎么走，该如何学习。

如果你想学习无人机仿真，可以接着往下看，我带你从下好ROS之后空空如也的新系统，一步一步地实现无人机的仿真飞行。

第一步：配置仿真平台

我们要依次进行gazebo安装（仿真平台）、mavros安装（无人机通信模块）、px4配置、地面站QGroundControl安装。

此处仿真平台的搭建比较复杂，上链接:仿真平台基础配置 · 语雀

链接的步骤介绍已经十分详细，如果还有哪里不懂，可以跟着b站的一个up主的视频一步一步慢慢搭建

【【XTDrone】无人机仿真平台基础配置（基于ROS和PX4的无人机仿真平台的基础配置搭建）-哔哩哔哩】 https://b23.tv/txNJO6P

第二步：创建工作空间与功能包，写入飞行代码

1、创建工作空间

#创建工作空间
midir catkin_ws/

cd catkin_ws/

midir src

cd src/

catkin_init_workspace

#编译工作空间
cd..

catkin_make

#设置环境变量
source devel/setup.bash

#检查环境变量
echo $ROS_PACKAGE_PATH

2、创建功能包

功能包是放在工作空间src文件夹中实现具体功能的特殊文件夹，是放置ROS源码的最小单元

#创建功能包(offboard_sin是功能包名)
$ cd ~/catkin_ws/src

$ catkin_create_pkg offboard_sin std_msgs rospy roscpp

#编译功能包
$ cd ~/catkin_ws

$ catkin_make

$ source ~/catkin_ws/devel/setup.bash

3、写入飞行代码

在功能包的src路径下创建offboard_sin_node.cpp文件，并写入如下沿着sin路径飞行的代码 

 （根据官方offboard代码示例改写）

/**
 * @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 = 3;
    
    
    

    //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";

    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.0f))){
            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.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 += 5;
            pose.pose.position.y = 20*sin(pose.pose.position.x/40*PI);
            pose.pose.position.z = 3;
        }

        local_pos_pub.publish(pose);

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

    return 0;
}

 4、修改CMakeLists.txt文件

加入下面两行

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

第三步：实现飞行仿真

打开终端，输入

roslaunch px4 mavros_posix_sitl.launch

此launch文件会打开你的gazebo启动无人机模型并建立好mavros通信

同时，打开你的QGroundControl，其会自动与你gazebo里的无人机进行通信连接

然后打开新终端，输入

rosrun offboard_sin offboard_sin_node

#rosrun+功能包+功能包内节点文件

实现效果 

 至此，第一次简单的无人机飞行仿真结束。

参考文章：(8条消息) 使用PX4+mavros+gazebo实现无人机offboard控制仿真_sdhdwyx的博客-CSDN博客_offboard 无人机https://blog.csdn.net/qq_42680785/article/details/118853000?utm_source=app&app_version=5.3.0&code=app_1562916241&uLinkId=usr1mkqgl919blen