PX4实战（一）Square 飞行【矩阵飞行】

于 2024-01-18 23:09:19 发布
阅读量543 收藏 14
点赞数 15
文章标签： c++ linux 机器学习
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/yeah111_/article/details/135683789
版权

PX4矩阵飞行仿真

根据官方提供的offboard程序，在此基础上进行修改实现了对矩阵进行一个飞行。在此之前我们需要先了解mavros的话题以及消息类型，可以参考这篇文章：PX4自动控制中常用的几个话题_px4的话题类型-CSDN博客

 首先创建我们的pacakge，运行以下指令

cd catkin_ws/src 
catkin_create_pkg quater_pkg rospy rospp std_msgs

创建一个新的quater_node.cpp文件

 以下是对我们整个代码的一个解说：整体代码思路很简单：切换到offboard模式——arm解锁桨叶——飞四个点位【进行一个误差的判断】——到最后一个点位进行降落。

 首先是对我们头文件的一个包含，是消息这个类下面的某个属性的.h文件

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

然后在主函数内对我们的ROS进行初始化以及定义飞机状态、飞机位置话题的订阅，定义两个服务类型分别是offboard模式飞行以及请求解锁桨叶的服务指令。以及定一个发布者，发布位置信息，给予飞控接受指令。

    ros::init(argc, argv, "square_node");//初始化ROS
    ros::NodeHandle nh;//创建节点句柄
 		//创建订阅者，订阅话题mavros/state
    ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
            ("mavros/state", 10, state_cb);
    //创建一个发布者，发布"mavros/setpoint_position/local"话题
    ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>
            ("mavros/setpoint_position/local", 10);
    //创建一个客户端，该客户端用来请求PX4无人机的解锁         
    ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
            ("mavros/cmd/arming");
    //创建一个客户端，该客户端用来请求进入offboard模式
    ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
            ("mavros/set_mode");
    //创建一个订阅者，订阅位置信息
    ros::Subscriber position_receive=nh.subscribe<geometry_msgs::PoseStamped>
    ("mavros/local_position/pose",10,pose_cb);

接着等待飞控的连接

    while(ros::ok() && !current_state.connected){
        ros::spinOnce();
        rate.sleep();
    }

在开始飞行之前给无人机发布一定的位置信息

    for(int i = 100; ros::ok() && i > 0; --i){
        local_pos_pub.publish(pose);
        ros::spinOnce();
        rate.sleep();
    }

 设置两个客户端分别是OFFBOARD与ARM解锁模式，并将当前系统时间记录下来

 mavros_msgs::SetMode offb_set_mode;
    offb_set_mode.request.custom_mode = "OFFBOARD";
 mavros_msgs::CommandBool arm_cmd;
    arm_cmd.request.value = true;//并将其bool返回值赋为1，解锁为真
ros::Time last_request = ros::Time::now();//记录当前系统时间

请求将飞机飞行模式切换为OFFBOARD模式，并且判断是否解锁，解锁成功则跳出循环【解锁成功之后必须再一次发布位置信息不然飞机将不会正常进行飞行】。

    while(ros::ok()){
        //if语句循环请求进入OFFBOARD模式，进入以后则会进入else语句请求对无人机解锁。
        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 
        {//已经进入OFFBOARD模式，判断是否解锁，若解锁则break；
            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");
                    break;
                }
                last_request = ros::Time::now();
            }
        }
//解锁之后再一次发布位置指令，若不发布位置质量，则飞机便会一直呆在（0，0，0）点。
        pose_goal(0,0,1.5);
        local_pos_pub.publish(pose);
        ros::spinOnce();
        rate.sleep();
    }

 定义两个回调函数分别是飞机状态的回调与位置消息的回调函数用两个常量指针作为参数进行传递。

//定义消息类型变量
mavros_msgs::State current_state;
geometry_msgs::PoseStamped pose;
//接受飞机状态的回调函数
void state_cb(const mavros_msgs::State::ConstPtr& msg){
    current_state = *msg;
}
//接受飞机位置的回调函数；
double pose_position[3];//定义全局变量的数组
void pose_cb(const geometry_msgs::PoseStamped::ConstPtr& msg)
{
    pose_position[0]=msg->pose.position.x;
    pose_position[1]=msg->pose.position.y;
    pose_position[2]=msg->pose.position.z;
}

定义绝对值函数与传入目标值的函数，简化编程

//设定目标位置
void pose_goal(double x,double y,double z)
{
    pose.pose.position.x = x;
    pose.pose.position.y = y;
    pose.pose.position.z = z;
}
//编写误差函数，返回值类型为double
double fab_return[3];//定义返回误差的数组
double err;//定义一个全局变量
int count=0;//定义一个标志位
double fabs_pose(double pose_goal,double pose_real)
{    
    err= fabs(pose_goal-pose_real);
    return err; 
}

编写main函数中选择发布位置的函数，通过判断目标值与实际值的误差来进行下一个目标点的飞行

while (ros::ok())
        {
        fab_return[0]=fabs_pose(pose.pose.position.x,pose_position[0]);
        fab_return[1]=fabs_pose(pose.pose.position.y,pose_position[1]);
        fab_return[2]=fabs_pose(pose.pose.position.z,pose_position[2]);
       if (fab_return[0]<0.1&&fab_return[1]<0.1&&fab_return[2]<0.1&&((ros::Time::now() - last_request )> ros::Duration(3.0)))
       {
        ROS_INFO("当前位置x:%.2f,y:%.2f,z:%.2f\n",pose.pose.position.x,pose.pose.position.y,pose.pose.position.z);
            count++;
            last_request = ros::Time::now();
        switch (count)
        {
        //case 0:pose_goal(0,0,1.5);count ++;break;
        case 0:pose_goal(0,1.5,1.5);break;
        case 1:pose_goal(1.5,1.5,1.5);break;
        case 2:pose_goal(1.5,0,1.5);break;
        case 3:pose_goal(0,0,1.5);break;
        case 4:pose_goal(0,0,0);break;
        default:
            break;
        }
       }
       else
       {
        local_pos_pub.publish(pose);//一直发布这个目标值
        // ROS_WARN("当前位置x:%.2f,y:%.2f,z:%.2f\n",pose.pose.position.x,pose.pose.position.y,pose.pose.position.z);
       }
            
        ros::spinOnce();
        rate.sleep();
        }

整体代码如下：

//相关头文件包含
#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;
geometry_msgs::PoseStamped pose;
//设定目标位置
void pose_goal(double x,double y,double z)
{
    pose.pose.position.x = x;
    pose.pose.position.y = y;
    pose.pose.position.z = z;
}
//接受飞机状态的回调函数
void state_cb(const mavros_msgs::State::ConstPtr& msg){
    current_state = *msg;
}
//接受飞机位置的回调函数；
double pose_position[3];
double fab_return[3];
double err;
int count=0;
void pose_cb(const geometry_msgs::PoseStamped::ConstPtr& msg)
{
    pose_position[0]=msg->pose.position.x;
    pose_position[1]=msg->pose.position.y;
    pose_position[2]=msg->pose.position.z;
}
double fabs_pose(double pose_goal,double pose_real)
{    
    err= fabs(pose_goal-pose_real);
    return err; 
}

int main(int argc, char **argv)
{
    
    setlocale(LC_ALL,"");
	//初始化节点
    ros::init(argc, argv, "square_node");
    //创建节点句柄
    ros::NodeHandle nh;
 		//创建订阅者，订阅话题mavros/state
    ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
            ("mavros/state", 10, state_cb);
    //创建一个发布者，发布"mavros/setpoint_position/local"话题
    ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>
            ("mavros/setpoint_position/local", 10);
    //创建一个客户端，该客户端用来请求PX4无人机的解锁         
    ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
            ("mavros/cmd/arming");
    //创建一个客户端，该客户端用来请求进入offboard模式
    ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
            ("mavros/set_mode");
    //创建一个订阅者，订阅位置信息
    ros::Subscriber position_receive=nh.subscribe<geometry_msgs::PoseStamped>
    ("mavros/local_position/pose",10,pose_cb);
    //搭配rate.sleep()函数，实现50Hz频率的循环;
    ros::Rate rate(50.0);

    // 等待飞控连接
    while(ros::ok() && !current_state.connected){
        ros::spinOnce();
        rate.sleep();
    }
    //预发布期望位置信息，给无人机目标点
    //send a few setpoints before starting
    for(int i = 100; ros::ok() && i > 0; --i){
        local_pos_pub.publish(pose);
        ros::spinOnce();
        rate.sleep();
    }
    //设置客户端请求无人机进入“OFFBOARD”模式
    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语句循环请求进入OFFBOARD模式，进入以后则会进入else语句请求对无人机解锁。
        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");
                    break;
                }
                last_request = ros::Time::now();
            }
        }
        pose_goal(0,0,1.5);
        local_pos_pub.publish(pose);
        ros::spinOnce();
        rate.sleep();
    }
    last_request =ros::Time::now();
    while (ros::ok())
        {
        fab_return[0]=fabs_pose(pose.pose.position.x,pose_position[0]);
        fab_return[1]=fabs_pose(pose.pose.position.y,pose_position[1]);
        fab_return[2]=fabs_pose(pose.pose.position.z,pose_position[2]);
       if (fab_return[0]<0.1&&fab_return[1]<0.1&&fab_return[2]<0.1&&((ros::Time::now() - last_request )> ros::Duration(3.0)))
       {
        ROS_INFO("当前位置x:%.2f,y:%.2f,z:%.2f\n",pose.pose.position.x,pose.pose.position.y,pose.pose.position.z);
            count++;
            last_request = ros::Time::now();
        switch (count)
        {
        //case 0:pose_goal(0,0,1.5);count ++;break;
        case 0:pose_goal(0,1.5,1.5);break;
        case 1:pose_goal(1.5,1.5,1.5);break;
        case 2:pose_goal(1.5,0,1.5);break;
        case 3:pose_goal(0,0,1.5);break;
        case 4:pose_goal(0,0,0);break;
        default:
            break;
        }
       }
       else
       {
        local_pos_pub.publish(pose);
        // ROS_WARN("当前位置x:%.2f,y:%.2f,z:%.2f\n",pose.pose.position.x,pose.pose.position.y,pose.pose.position.z);
       }
            
        ros::spinOnce();
        rate.sleep();
        }
    return 0;
}

最后别忘了添加编译环境

stupdf
关注
探索未来飞行：PX4-Autopilot - 开源无人机控制系统
gitblog_00020的博客
03-20 957
探索未来飞行：PX4-Autopilot - 开源无人机控制系统 项目地址:https://gitcode.com/gh_mirrors/px/PX4-Autopilot PX4-Autopilot 是一个强大的开源无人机及飞行器控制系统，它为开发者、爱好者和专业应用提供了自由度极高的飞行解决方案。该项目由全球各地的贡献者维护，并在 上进行托管。 项目简介 PX4-Autopilot 使用模块化...
【PX4代码】PX4 filght mode 飞行模式分析
RTK学习者
04-18 1564
飞行模式在vehicle_status.msg中，该消息中有一段内容： # Navigation state, i.e. "what should vehicle do". uint8 NAVIGATION_STATE_MANUAL = 0 # Manual mode uint8 NAVIGATION_STATE_ALTCTL = 1 # Altitude control mode uint8...
PX4实战之旅（三）：通过OFFBOARD模式控制无人机自主飞行
超维空间科技的博客
10-28 7063
固件：PX4 1.13.1在目录下新建文件夹在文件夹下新建下面四个文件新建SRCS)新建Kconfigdefault n新建{return 0;}{/*if (!return 1;}if (!return 0;}{int ch;break;case '?':break;}}}}}{}{
带你玩转PX4无人机仿真(1) —— 运行官方案例（C++）
weixin_55944949的博客
08-29 4432
本次教程是官方提供的 MAVROS Offboard (板外) 控制示例
px4自主飞行教程
m0_50848587的博客
04-24 470
1、转自https://github.com/amov-lab/Amov-wiki/blob/master/px4_command/source/docs/%E8%87%AA%E4%B8%BB%E9%A3%9E%E8%A1%8C%E4%B9%8B%E5%85%A5%E9%97%A8.rst
基于ROS的PX4+Gazebo仿真——PX4一键起飞及飞行控制
m0_64730542的博客
05-11 1万+
一键起飞和飞行控制
ROS+PX4无人机室内多点飞行代码
06-13
整体流程如下：无人机起飞后请求进入offboard模式，紧接着请求解锁，解锁后飞行至0.3米高，紧接着逆时针飞行边长为0.5米的正方形，每个边长飞行8秒钟。完成正方形后自动进入降落模式，全程无需手动。已经在实体...
PX4源码解读
09-30
- **实战项目**：尝试设计并实现一个简单的飞行任务，如自动巡检或定点悬停，以此加深对 PX4 工作原理的理解。 总的来说，《PX4源码解读》是一份宝贵的资料，无论你是无人机爱好者还是专业开发者，都能从中受益，...
室内多点飞行的ROS+PX4无人机控制代码教程
- PX4提供了一系列高级飞行模式和安全特性，如自动起飞、飞行计划、避障、定位和导航等。 - PX4与QGroundControl地面控制站软件配合使用，提供直观的飞行任务规划和飞行数据监控。 3. ROS与PX4的整合: - ROS和PX...
PX4 的 ECL EKF 公式推导及代码解析.pdf
09-05
PX4无人机系统是一种开源的飞行控制软件，它使用ECL（Estimation and Control Library）进行状态估计和控制，而ECL中利用EKF（扩展卡尔曼滤波器）进行传感器信息的融合。EKF是一种用于非线性动态系统的状态估计方法...
PX4实战之旅（五）：利用T265实现室内定点飞行
超维空间科技的博客
11-10 9641
硬件：Intel NUC软件：PX4 1.13.0Ros NeoticMavros视觉惯性里程计测距（VIO）是一种计算机视觉技术，用于估算3D姿态（本地位置和方向），相对于本地起始位置的移动的机体速度。它通常用于在GPS不存在或不可靠的情况下（例如室内或在桥下飞行时）给载具导航。VIO 使用视觉里程计（Visual Odometry）从相机图像中估计机身姿态，并结合机身IMU的惯性测量以校正。
【UAV仿真】在ROS中实现无人机沿自定义圆形轨迹飞行（PX4+MAVROS+Gazebo）
最新发布
xuzhengzhe的博客
01-13 1964
本博客将介绍如何在ROS中，结合MAVROS包，创建一个无人机控制节点，使无人机能够沿着预设轨迹（如圆形）飞行。本教程假设您已经安装了ROS和MAVROS包。
PX4飞行模式-多旋翼
xiaoshuai537的博客
06-06 5493
PX4飞行模式
无人机系统PX4视频教程：飞行PID参数调试
热门推荐
AMOVLAB(阿木实验室)
12-12 1万+
PID参数调试一直是比较头疼的问题，在此给大家带来如下的教程！ 完整文章(建议PC下观看，手机的排版有问题)视频教程下载地址： 链接：http://pan.baidu.com/s/1bptl6BP 密码：2zbe 获取更多免费资料或技术咨询请搜索微信公众号：阿木实验室    官网：www.amovauto.com PIXHAWk无人机PID参数
PX4飞行模式
冯顺的博客
01-30 3469
地面站解释： Stabilized/Main(稳定/主模式): 飞行员拥有飞行飞行器全部的控制权，只有姿态是稳定的。主模式开关必须被关联到一个通道，飞机才能飞行。 Assist(辅助): 如果Position模式是放在与主模式分离的通道上，主开关上增加了一个Assist模式。为了使姿态控制/位置控制开关有效，主开关必须处于辅助模式。 Auto(自动): 如果Loiter模式是放在主模式分离的
PX4模块设计之九：PX4飞行模式简介
lida2003的专栏
07-15 2884
PX4的飞行模式从官网介绍上看感觉非常多且复杂，那是因为专业。为了方便理解，梳理一下有助于后续研读代码逻辑。
PX4自主飞行
r1ch4rd的博客
08-28 1334
**PX4自主飞行**
PX4自主飞行相关问题
FC的博客
10-03 3134
调试入坑赶在回去之前把10月1日新校区试飞相关问题记录一下。首先是调试相关问题。调试具体流程 在校准遥控器时经常出现校准一半就停止的问题，期初认为是固件问题，换了1.6.5,1.6.3，1.5.5三个固件依然不行，最后发现是由于在校准时必须先将roll改变再进行pitch回中，如果调试再遇到问题，将左右摇杆的校准顺序换一下就行了。一般校准完上电后，电机的电调会慢慢的响，这是正常的，因为电调的信
PX4自动任务飞行AUTO_MISSION的实现流程
SIR_wkp的博客
06-22 5849
第一步，进入方式。    与之前分析的POSCTL模式类似，主要有遥控器开关和指令两种模式，实现过程完全相同。    最终都是调用了main_state_transition()函数将新模式写入到internal_state.main_state中。第二步，导航模式和控制模式的选择。    在commander的主循环中，set_nav_state()来设定导航模式。由于自动任务飞行需要使用各种传...
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

stupdf

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付：¥1
获取中
扫码支付

您的余额不足，请更换扫码支付或充值

打赏作者

实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值