PX4中offboard模式

最新推荐文章于 2024-07-30 17:39:02 发布
最新推荐文章于 2024-07-30 17:39:02 发布
阅读量538 收藏 8
点赞数 4
文章标签： c++ 开源
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/kjb655/article/details/131492215
版权 
  • /*
头文件，包括常见的geometry_msgs和mavros通信需要的mavros_msgs，添加上就行
*/
#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>


/*
current_state表示的是无人机的状态，在主函数中订阅了对应的话题，这个状态就会不断更新，表示无人机当前的状态。state_cb就是对应的回调函数，会不断的执行，更新状态。现在先不用管为什么需要这个状态，后面的代码会解释。
*/
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);
    
    /* 
    发布一个geometry_msgs::PoseStamped的消息，需要知道的是，这个消息是控制无人机的一种方式，将指定坐标包裹进这个消息，然后发布出去，无人机就能自动飞行到指定的坐标地点
    */ 
    ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>
            ("mavros/setpoint_position/local", 10);
    
    /*
    无人机有一个锁，如果不解锁，无人机虽然接受了命令但是不会动被锁住了，只有解锁了才能对无人机进行控制，下面这个服务调用就是用来请求解锁无人机。上面的current_state就包含了无人机是否解锁的信息，若没解锁就需要解锁，否则就不用，其用途在这就体现出来
    */
    ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
            ("mavros/cmd/arming");
    
    /*
    无人机飞行有很多种模式，如果需要用代码操控无人机，我们就需要切换到OFFBOARD模式。上面的current_state也包含了无人机当前的飞行模式，若不是OFFBOARD就需要切换到该模式。下面的这个服务调用就是用来请求切换无人机飞行模式。
    */
    ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
            ("mavros/set_mode");

    //the setpoint publishing rate MUST be faster than 2Hz
    /*
    在OFFBOARD模式下，需要以>=2HZ的频率向无人机发送消息，否则无人机会回退到OFFBOARD模式之前所在的模式，因此这里的rate需要设置的比2大就行
    */
    ros::Rate rate(20.0);
	
    // wait for FCU connection
    /*
    等待无人机与控制站连接（代码的方式就是代理），只有连接了才能发送消息
    */
    while(ros::ok() && !current_state.connected){
        ros::spinOnce();
        rate.sleep();
    }

    /*
    pose就是坐标，本实例是让无人机在2m处悬空，因此z设置为2，z表示的就是高度
    */
    geometry_msgs::PoseStamped pose;
    pose.pose.position.x = 0;
    pose.pose.position.y = 0;
    pose.pose.position.z = 1;

    // 下面这个感觉有没有都无所谓
    //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";

    // 请求解锁的消息，arm表示解锁，设置为true，disarm是上锁
    mavros_msgs::CommandBool arm_cmd;
    arm_cmd.request.value = true;

    // 记录上次请求的时间
    ros::Time last_request = ros::Time::now();

    while(ros::ok()){
        // 如果无人机模式不是OFFBOARD并且离上次操作时间大于5秒就发送请求切换，这里的5s是为了演示清楚设置的延时
        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 {
            // 如果当前未解锁且与请求时间大于5s，就发送请求解锁
            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();
            }
        }
		
        // 不断发送位置消息，但是只有解锁后才能真正开始运动，如果不发送就会退出OFFBOARD模式，因为请求发送速度要>=2HZ
        local_pos_pub.publish(pose);

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

    return 0;
}

/**
 * @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>
#include <nav_msgs/Odometry.h>//里程计信息格式

int flag  = 1;

mavros_msgs::State current_state;
nav_msgs::Odometry local_pos;

void local_pos_cb(const nav_msgs::Odometry::ConstPtr& msg);
void state_cb(const mavros_msgs::State::ConstPtr& msg);


int main(int argc, char **argv)
{
    ros::init(argc, argv, "offboard_multi_position");

    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::Subscriber local_pos_sub = nh.subscribe<nav_msgs::Odometry>("/mavros/local_position/odom", 10, local_pos_cb);

    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 =   0.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();
 
 	//此处满足一次请求进入offboard模式即可，官方例成循环切入offboard会导致无人机无法使用遥控器控制
    while(ros::ok())
    {
    	//请求进入OFFBOARD模式
        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();
			}
		}
	    
	    if(ros::Time::now() - last_request > ros::Duration(5.0))
	    {
	    	break;
	    }	
	    
		local_pos_pub.publish(pose);
        ros::spinOnce();
        rate.sleep();
    }   
    
    
    while(ros::ok())
    {
        if((flag == 1)  && (ros::Time::now() - last_request > ros::Duration(5.0)))
		{ 
			ROS_INFO("Position_1");
            pose.pose.position.x =  0;
    		pose.pose.position.y =  0;
    		pose.pose.position.z =  0.3;                        
			last_request = ros::Time::now();
            flag=2;
        }

		if((flag ==2) && (ros::Time::now() - last_request > ros::Duration(8.0)))
		{
			ROS_INFO("Position_2 ");
		    pose.pose.position.x =  0.5;
			pose.pose.position.y =  0;
			pose.pose.position.z =  0.3;
			last_request = ros::Time::now();
			flag=3;
		}
		                   
		if((flag ==3) && (ros::Time::now() - last_request > ros::Duration(8.0)))
		{
		    ROS_INFO("Position_3 ");
		    pose.pose.position.x =  0.5;
		    pose.pose.position.y =  0.5;
            pose.pose.position.z =  0.3;   
		    last_request = ros::Time::now();
			flag=4;
		}

		if((flag ==4) && (ros::Time::now() - last_request > ros::Duration(8.0)))
		{
		    ROS_INFO("Position_4 ");
		    pose.pose.position.x =  0;
            pose.pose.position.y =  0.5;
		    pose.pose.position.z =  0.3;
		    last_request = ros::Time::now();
			flag=5;
		}

		if((flag ==5) && (ros::Time::now() - last_request > ros::Duration(8.0)))
		{
            ROS_INFO("Position_1 ");
            pose.pose.position.x =  0;
            pose.pose.position.y =  0;
            pose.pose.position.z =  0.3;  		
            last_request = ros::Time::now();
			flag=6;
        }
        if((flag ==6) && (ros::Time::now() - last_request > ros::Duration(8.0)))
        {
        	break;
        }
        local_pos_pub.publish(pose);
        ros::spinOnce();
        rate.sleep();
    }
    
    ROS_INFO("AUTO.LAND");
    offb_set_mode.request.custom_mode = "AUTO.LAND";
    set_mode_client.call(offb_set_mode);
    return 0;
}

void state_cb(const mavros_msgs::State::ConstPtr& msg)
{
    current_state = *msg;
}

void local_pos_cb(const nav_msgs::Odometry::ConstPtr& msg)
{
    local_pos = *msg;
}

心里有只小鹿
关注
  • 4
    点赞
  • 踩
  • 8
    收藏
    觉得还不错? 一键收藏
  • 1
    评论
px4 运行mavros offboard仿真例程
超维空间科技的博客
05-24 6353
roslaunch mavros px4.launch fcu_url:=“udp://:14540@127.0.0.1:14557”
PX4 如何切换到offboard 模式
xiaochengyexiao的博客
04-23 6211
最近研究px4的offboard模式，如何切换到offboard模式一筹莫展，便跟踪源代码至commander module。 一、 handle_command函数   从dronekit发送设定offboard模式的mavlink信息至飞控，相关命令经mavlink模块解析得到关于vehicle_command的主题。 由handle_command函数中
PX4位置控制offboard模式说明
热门推荐
Gen_Ye的博客
03-13 2万+
offboard模式的开发及应用 一、px4固件的模式 px4固件支持10几种飞行模式，从代码结构上分析，分为基本模式、自定义模式和自定义子模式。 1.基本模式 基本模式又分为，位置控制模式、自稳模式、手动模式，通常情况下，自驾仪处于自定义模式，目前还没有见过处于基本模式工作的代码，所以暂时不讨论基本模式。                              地面站
P3：PX4的offboard模式
m0_53394161的博客
07-18 455
PX4的offboard模式
PX4+Offboard模式+代码控制无人机起飞（Gazebo）
晨少的博客
01-24 7734
在src目录下的CMakeLists.txt文件中，在Build的最后（Install的上面）添加下面两行代码。想要无人机降落可以通过代码控制，也可以在运行px4的终端中输入。在src目录下新建offboard.cpp文件。再回到px4_offboard_ws下进行编译。可以看到gazebo中的无人机已经起飞。进入src目录，创建ros功能包。px4仿真的教程在我上一篇博客中。编译成功后运行先px4仿真。（需要先终止ros程序。可以看到无人机已经降落。接着进入文件编译一下。接着运行ros功能包。
【PX4-AutoPilot教程-Offboard】MAVROS功能包控制无人机进入offboard模式飞行官方例程（C++实现）
在后厂村路努力科研搬砖的无人机控制方向练习生一名～
12-21 6682
主要介绍如何通过MAVROS功能包使用PX4飞控中的offboard模式控制gazebo中的飞机起飞到高度两米，代码用C++实现。在运行过程中，可能会出现一个BUG，就是无人机无法起飞，文章中简述了解决方法。
PX4实战之旅（三）：通过OFFBOARD模式控制无人机自主飞行
超维空间科技的博客
10-28 5974
固件：PX4 1.13.1在目录下新建文件夹在文件夹下新建下面四个文件新建SRCS)新建Kconfigdefault n新建{return 0;}{/*if (!return 1;}if (!return 0;}{int ch;break;case '?':break;}}}}}{}{
Pixhawk原生固件PX4之offboard
FantasyJXF的博客
09-15 9685
想毕业 。。。 想的睡不着 。。。
无人机 PX4 飞控 | Offboard 模式详解与gazebo仿真测试
最新发布
月照银海似蛟龙的博客
07-30 1147
在该模式下，飞行器根据飞行控制栈外部（如机载计算机）提供的设定值控制位置、速度、加速度、姿态以及推力/力矩。设定值可以通过MAVLink协议提供给飞控PX4要求外部控制器提供2Hz连续的“有效存在”信号，该信号可由任意支持的 MAVLink 设置点消息提供。PX4只有在收到该种信号超过1秒后才有效，如果该种信号停止飞行控制栈将重新获得控制权（脱离Offboard模式）。此模式需要位置或位姿信息的提供 - 例如GPS、光流、视觉惯性里程计、mocap等。
px4_indoor_navigation:使用PX4自动驾驶仪将ROS节点打Kong进行室内导航，主要是在OFFBOARD模式下
05-18
px4_indoor_navigation 使用PX4自动驾驶仪和indoot定位系统（例如OptiTrack）进行室内导航的ROS节点集。
PX4无人机offboard开发之旅
InTereSTingHE的博客
12-14 681
（慢慢更）无人机OFFBOARD开发
在PX4中如何使用offboard模式以及对c_uart_interface_example程序的分析
weixin_39472686的博客
10-21 1892
这个程序（mavlink-examles/c_uart_interface_example)演示了如何用c语言调用mavlink API做offboard控制。 这个程序仅针对串口和udp口通讯，目前pixhawk飞控计算机与外部计算机的通讯只支持串口，而udp口是针对PX4放在仿真环境里时的通讯接口，本文只针对uart进行分析。 c_uart_interface_example在打开串口（port.start())后，就启动了2个线程，一个是读线程，一个是写线程，顾名思义，写线..........
无人机仿真—PX4编译，gazebo仿真及简单off board控制模式下无人机起飞
LI2582957686的博客
03-18 2473
无人机PX4编译offboard模式简单起飞
PX4官方手册的MAVROS-offboard给的示范代码详细注释
诗筱涵的博客
08-07 2418
PX4官方手册的MAVROS-offboard给的示范代码详细注释 摘自：https://blog.csdn.net/zouxu634866/article/details/104095648 #include <ros/ros.h> #include <geometry_msgs/PoseStamped.h> //发布的消息体对应的头文件，该消息体的类型为geometry_msgs：：PoseStamped #include <mavros_msgs/Comm..
PX4无法进入offboard模式
qq_44695258的博客
03-25 3896
固件版本为v1.11.1。 遥控器切入offboard模式时，QGC提示reject。 找了一两天，发现是因为GPS没起作用。 打开QGC的mavlink消息检测，发现有GPS_RAW_INT消息，但是没有GLOBAL_POSITION_INT消息。 GPS_RAW_INT是原始GPS数据，GLOBAL_POSITION_INT是经过卡尔曼滤波后的数据。而后者的存在才是offboard的进入条件之一。 经探索，发现是因为GPS数据不达标才没有GLOBAL_POSITION_INT消息。 如果是测试PX4能不
5.ROS&PX4--offboard模式多航点代码编写
qq_45926844的博客
05-31 1242
ROS&PX4--offboard模式多航点代码编写
通过mavros+gazebo+px4实现offboard模式螺旋线（胎教级别）
weixin_43261469的博客
07-16 659
px4中offboard例子简单实现，快速上手无人机px4在gazebo中的轨迹跟踪的仿真
小白PX4无人机仿真—OFFBOARD控制资料汇总
weixin_41097249的博客
10-30 1695
基于PX4的无人机仿真环境搭建与基础控制节点实现，本文更多的是新学过程中对所学知识整体实现流程的顺序资料整理，另加入了一些个人理解，如有问题欢迎大家指正
PX4 & gazebo仿真 offboard模式无法起飞解决思路 2023.5.19更新
private_Jack的博客
12-28 3366
我用最新版PX4固件，offboard模式飞机无法起飞，经过一番尝试，最后顺利解决。
offboard模式px4控制
07-10
offboard模式是PX4飞控系统中的一种工作模式，允许外部设备通过MAVLink协议与飞控进行通信控制。在offboard模式下，飞控将不再遵循预设的飞行计划，而是接受外部指令进行控制。 要将PX4飞控切换到offboard模式，需要通过MAVLink发送相应的指令。具体步骤如下： 1. 连接到PX4飞控系统的地面站软件（如QGroundControl）。 2. 将飞控系统连接到计算机上，通过串口或网络连接。 3. 在地面站软件中，选择正确的串口或网络连接。 4. 进入“Flight Modes”（飞行模式）选项。 5. 选择“Offboard”（或类似的选项）作为期望的飞行模式。 6. 在地面站软件中，发送对应的MAVLink指令，来控制飞行器的姿态、位置或其他相关参数。 需要注意的是，切换到offboard模式前，必须确保飞行器已经解锁且处于安全的状态。另外，为了确保安全，建议在使用offboard模式时，使用遥控器或其他手动控制方式作为备用控制手段，以便在需要时能够快速切换回手动控制模式。
评论 1
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

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

余额充值