【PX4-AutoPilot教程-Offboard】MAVROS功能包控制无人机进入offboard模式飞行官方例程（C++实现）
PX4-Autopilot
PX4 Autopilot Software
项目地址：https://gitcode.com/gh_mirrors/px/PX4-Autopilot
免费下载资源
·
MAVROS功能包控制无人机进入offboard模式飞行官方例程（C++实现）
主要介绍如何通过MAVROS功能包的offboard模式控制gazebo中的飞机起飞到高度两米。
准备工作
首先建立工作空间，这里建立一个名为uav_demo_ws的工作空间。
mkdir -p uav_demo_ws/src
之后创建功能包，这里命名为offboard_run。
cd uav_demo_ws/src
catkin_create_pkg offboard_run roscpp std_msgs geometry_msgs mavros_msgs
建立C++脚本作为这个功能包的执行文件。
cd offboard_run/src
gedit offboard_run_node.cpp
把官方代码复制到这个文件里面，保存退出。
//一系列头文件
//ros库
#include <ros/ros.h>
//发布的位置消息体对应的头文件，该消息体的类型为geometry_msgs::PoseStamped
//用来进行发送目标位置
/*
ros官网上这样定义
# A Pose with reference coordinate frame and timestamp
Header header
Pose pose
实际上就是一个带有头消息和位姿的消息
*/
#include <geometry_msgs/PoseStamped.h>
//CommandBool服务的头文件，该服务的类型为mavros_msgs::CommandBool
//用来进行无人机解锁
/*
其结构如下（来源于ros wiki）
# Common type for switch commands
bool value
---
bool success
uint8 result
可以看到，发送的请求是一个bool类型的数据，为True则解锁，为False则上锁
返回的响应中success是一个bool类型的参数，表示上电/断电操作是否成功执行。
如果操作成功执行，success值为True，否则为False。
result是一个int32类型的参数，表示执行上电/断电操作的结果。
如果解锁/上锁操作成功执行，result值为0，
否则为其他值，表示执行解锁/上锁操作时发生了某种错误或异常。可以根据这个数值查看是哪种问题导致
*/
#include <mavros_msgs/CommandBool.h>
//SetMode服务的头文件，该服务的类型为mavros_msgs::SetMode
//用来设置无人机的飞行模式，切换offboard
/*
wiki上的消息定义如下
# set FCU mode
#
# Known custom modes listed here:
# http://wiki.ros.org/mavros/CustomModes
# basic modes from MAV_MODE
uint8 MAV_MODE_PREFLIGHT = 0
uint8 MAV_MODE_STABILIZE_DISARMED = 80
uint8 MAV_MODE_STABILIZE_ARMED = 208
uint8 MAV_MODE_MANUAL_DISARMED = 64
uint8 MAV_MODE_MANUAL_ARMED = 192
uint8 MAV_MODE_GUIDED_DISARMED = 88
uint8 MAV_MODE_GUIDED_ARMED = 216
uint8 MAV_MODE_AUTO_DISARMED = 92
uint8 MAV_MODE_AUTO_ARMED = 220
uint8 MAV_MODE_TEST_DISARMED = 66
uint8 MAV_MODE_TEST_ARMED = 194
uint8 base_mode # filled by MAV_MODE enum value or 0 if custom_mode != ''
string custom_mode # string mode representation or integer
---
bool success
String类型的变量custom_mode就是我们想切换的模式，有如下选择：
MANUAL，ACRO，ALTCTL，POSCTL，OFFBOARD，STABILIZED，RATTITUDE，AUTO.MISSION
AUTO.LOITER，AUTO.RTL，AUTO.LAND，AUTO.RTGS，AUTO.READY，AUTO.TAKEOFF
*/
#include <mavros_msgs/SetMode.h>
//订阅的消息体的头文件，该消息体的类型为mavros_msgs::State
//查看无人机的状态
/*
wiki上是这样的
std_msgs/Header header
bool connected
bool armed
bool guided
bool manual_input
string mode
uint8 system_status
解析如下：
header：消息头，包含时间戳和框架信息；
connected：表示是否连接到了 mavros 节点；
armed：表示无人机当前是否上锁；
guided：表示无人机当前是否处于 GUIDED 模式；
mode：表示当前无人机所处的模式，包括以下几种：
MANUAL，ACRO，ALTCTL，POSCTL，OFFBOARD，STABILIZED，RATTITUDE，AUTO.MISSION
AUTO.LOITER，AUTO.RTL，AUTO.LAND，AUTO.RTGS，AUTO.READY，AUTO.TAKEOFF
*/
#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系统的初始化，argc和argv在后期节点传值会使用，最后一个参数offb_node为节点名称
ros::init(argc, argv, "offb_node");
//实例化ROS句柄，这个ros::NodeHandle类封装了ROS中的一些常用功能
ros::NodeHandle nh;
//这是一个订阅者对象，可以订阅无人机的状态信息（状态信息来源为MAVROS发布），用来储存无人机的状态，在回调函数中会不断更新这个状态变量
//<>里面为模板参数，传入的是订阅的消息体类型，（）里面传入三个参数，分别是该消息体的位置、缓存大小（通常为1000）、回调函数
ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>("mavros/state", 10, state_cb);
//这是一个发布者对象，用来在本地坐标系下发布目标点，后面会以20Hz频率发布目标点
//<>里面为模板参数，传入的是发布的消息体类型，（）里面传入两个参数，分别是该消息体的位置、缓存大小（通常为1000）
ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>("mavros/setpoint_position/local", 10);
//一个客户端，用来解锁无人机，这是因为无人机如果降落后一段时间没有收到信号输入，会自动上锁来保障安全
//启动服务的函数为nh下的serviceClient<>()函数，<>里面是该服务的类型，（）里面是该服务的路径
ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>("mavros/cmd/arming");
//一个客户端，用来切换飞行模式
//启动服务的函数为nh下的serviceClient<>()函数，<>里面是该服务的类型，（）里面是该服务的路径
ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>("mavros/set_mode");
//官方要求local_pos_pub发布速率必须快于2Hz，这里设置为20Hz
//PX4在两个Offboard命令之间有一个500ms的延时，如果超过此延时，系统会将回到无人机进入Offboard模式之前的最后一个模式。
ros::Rate rate(20.0);
// 等待飞控和MAVROS建立连接，current_state是我订阅的MAVROS的状态，在收到心跳包之后连接成功跳出循环
while(ros::ok() && !current_state.connected){
ros::spinOnce();
rate.sleep();
}
//实例化一个geometry_msgs::PoseStamped类型的对象，并对其赋值，最后将其发布出去
//尽管PX4在航空航天常用的NED坐标系下操控飞机，但MAVROS将自动将该坐标系切换至常规的ENU坐标系下
geometry_msgs::PoseStamped pose;
pose.pose.position.x = 0;
pose.pose.position.y = 0;
pose.pose.position.z = 2;
//在进入Offboard模式之前，必须已经启动了local_pos_pub数据流，否则模式切换将被拒绝。
//这里的100可以被设置为任意数
for(int i = 100; ros::ok() && i > 0; --i){
local_pos_pub.publish(pose);
ros::spinOnce();
rate.sleep();
}
//建立一个类型为SetMode的服务端offb_set_mode，并将其中的模式mode设为"OFFBOARD"，作用便是用于后面的客户端与服务端之间的通信（服务）
mavros_msgs::SetMode offb_set_mode;
offb_set_mode.request.custom_mode = "OFFBOARD";
//建立一个类型为CommandBool的服务端arm_cmd，并将其中的是否解锁设为"true"，作用便是用于后面的客户端与服务端之间的通信（服务）
mavros_msgs::CommandBool arm_cmd;
arm_cmd.request.value = true;
//更新时间
ros::Time last_request = ros::Time::now();
//大循环，只要节点还在ros::ok()的值就为正
while(ros::ok())
{
//首先判断当前模式是否为offboard模式，如果不是，则进入if语句内部
//这里是5秒钟进行一次判断，避免飞控被大量的请求阻塞
if( current_state.mode != "OFFBOARD" && (ros::Time::now() - last_request > ros::Duration(5.0)))
{
//客户端set_mode_client向服务端offb_set_mode发起请求call，然后服务端回应response将模式返回，这就打开了offboard模式
if( set_mode_client.call(offb_set_mode) && offb_set_mode.response.mode_sent)
{
//打开Offboard模式后在终端打印信息
ROS_INFO("Offboard enabled");
}
//更新时间
last_request = ros::Time::now();
}
//else指已经为offboard模式，则进入if语句内部
else
{
//判断当前状态是否解锁，如果没有解锁，则进入if语句内部
//这里是5秒钟进行一次判断，避免飞控被大量的请求阻塞
if( !current_state.armed && (ros::Time::now() - last_request > ros::Duration(5.0)))
{
//客户端arming_client向服务端arm_cmd发起请求call，然后服务端回应response成功解锁，则解锁成功
if( arming_client.call(arm_cmd) && arm_cmd.response.success)
{
//解锁后在终端打印信息
ROS_INFO("Vehicle armed");
}
//更新时间
last_request = ros::Time::now();
}
}
//发布位置信息，所以综上飞机只有先打开offboard模式然后解锁才能飞起来
local_pos_pub.publish(pose);
//当spinOnce函数被调用时，会调用回调函数队列中第一个回调函数，这里回调函数是state_cb函数
ros::spinOnce();
//根据前面ros::Rate rate(20.0);制定的发送频率自动休眠 休眠时间 = 1/频率
rate.sleep();
}
return 0;
}
补充内容：
mavros_msgs库中的消息文件可以在以下网址找到（比如State）：
https://docs.ros.org/en/noetic/api/mavros_msgs/html/msg/
mavros_msgs库中的服务文件可以在以下网址找到（比如CommandBool、SetMode）：
https://docs.ros.org/en/noetic/api/mavros_msgs/html/srv/
打开我们创建的offboard_run功能包里面的CMakeLists.txt文件。
cd ..
gedit CMakeLists.txt
在最后添加下面两行代码，然后保存关闭。
add_executable(${PROJECT_NAME}_node src/offboard_run_node.cpp)
target_link_libraries(${PROJECT_NAME}_node
${catkin_LIBRARIES}
)
到uav_demo_ws目录下对工作空间进行编译。
cd ~/uav_demo_ws
catkin_make
运行过程
打开一个终端，在PX4源码处启动无人机gazebo仿真。
cd Firmware
make px4_sitl gazebo
新打开另一个终端，运行MAVROS。
roslaunch mavros px4.launch fcu_url:="udp://:14540@127.0.0.1:14557"
再新打开另一个终端，运行ROS节点，这里注意需要先使用source命令配置好运行环境。
cd ~/uav_demo_ws
source ./devel/setup.bash
rosrun offboard_run offboard_run_node
如果觉得每次先使用
source命令配置好运行环境麻烦，也可以将环境配置到~/.bashrc中echo "source ~/uav_demo_ws/devel/setup.bash" >> ~/.bashrc
如果一切顺利，此时在gazebo中飞机已经飞到了两米的高度。
运行过程中可能出现的BUG
在运行过程中，可能会出现一个BUG，就是无人机无法起飞。
PX4源码处无人机gazebo仿真终端显示如下并不断重复。
INFO [commander] Failsafe mode deactivated
INFO [commander] Failsafe mode activated
MAVROS终端显示如下并不断重复。
CMD: Unexpected command 176, result 0
ROS节点终端显示如下并不断重复。
Offboard enabled
这时打开QGC地面站，可以看到无人机处于Not Ready状态。
并且有报错如下。
Failsafe enabled: No manual control stick input
分析原因是因为程序判断没有接入遥控器，认为不安全，所以不允许起飞。
解决方法为：
在前面的三个终端运行的时候，打开QGC地面站，在参数中搜索COM_RCL_EXCEPT。
将COM_RCL_EXCEPT参数改为4并保存。
这时回到gazebo窗口会发现飞机已经正常起飞了。
参考资料：
GitHub 加速计划 / px / PX4-Autopilot
13
2
下载
PX4 Autopilot Software
最近提交(Master分支：2 个月前 )
8ed3489b
- the module documentation parser can only resolve defines from the same file
- also it cannot deal with defines embeded in strings
- what board should it add for the general documentation anyways?
As a result of these issues I suggest to stay with the original hardcoded
/fs/microsd for the documentation. It's still the most common path as far as
I can see.
4 个月前
84d4ee0e
which is up to date containing "Use SO_REUSEPORT only if it exists"
and is advertised by GitHub because the commit is on a branch
4 个月前
旨在为数千万中国开发者提供一个无缝且高效的云端环境，以支持学习、使用和贡献开源项目。
更多推荐
52
0- 0
已为社区贡献6条内容
所有评论(0)