$ mkdir catkin_ws
$ cd catkin_ws
$ mkdir src
$ cd src
$ catkin_init_workspace //当前文件夹初始化,变成ros工作空间
$ cd ..
$ catkin_make //编译ros,生成对应文件夹
$ cd src
$ catkin_create_pkg offboard_pkg roscpp std_msgs geometry_msgs mavros_msgs //创建offboard_pkg 功能包
$ cd offboard_pkg /src
$ touch offboard_node.cpp // 创建cpp文件,并将下方3部分的程序复制到其中
$ cd ../../.. //回到catkin_ws工作目录
$ catkin_make //编译ROS程序
添加环境变量
$ source devel/setup.bash
$ source ~/.bash
offboard_node.cpp文件内容为:
/**
* @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
#include
#include
#include
#include
//订阅 mavros/state 的回调函数
mavros_msgs::State current_state;
void state_cb(const mavros_msgs::State::ConstPtr& msg){
current_state = *msg;
}
int main(int argc, char **argv)
{
//初始化ros系统,节点命名
ros::init(argc, argv, "offb_node");
ros::NodeHandle nh;
/*
创建订阅Subscribe;<>里面为模板参数,传入的是订阅的消息体类型,
()里面传入三个参数,分别是该消息的位置、缓存大小(通常为10)、回调函数
订阅 mavrso 状态:mavros/state
*/
ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
("mavros/state", 10, state_cb);
/*
发布之前需要公告,并获取句柄,发布的消息体的类型为:geometry_msgs::PoseStamped
发布公告 mavros/setpoint_position/local
*/
ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>
("mavros/setpoint_position/local", 10);
/*
启动服务用的函数为nh下的serviceClient<>()函数,<>里面是该服务的类型,()里面是该服务的路径
启动服务1,设置客户端(Client)名称为arming_client,客户端的类型为ros::ServiceClient: 连接服务 mavros/cmd/arming
*/
ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
("mavros/cmd/arming");
/*
启动服务2,设置客户端(Client)名称为set_mode_client,客户端的类型为ros::ServiceClient:连接服务 mavros/set_mode
*/
ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
("mavros/set_mode");
//the setpoint publishing rate MUST be faster than 2Hz
//发送频率设置为2hz
ros::Rate rate(20.0);
// wait for FCU connection
// 等待飞控连接mavros,其中current_state是我们订阅的mavros的状态,
//一直等待 current_state.connected 为 true,表示连接成功,跳出循环。此时已经得到了 mavros/state 消息,飞控已经成功连接 mavros
while(ros::ok() && !current_state.connected)
{
ros::spinOnce();//执行消息回调
rate.sleep();//睡眠保证频率
}
//实例化一个 geometry_msgs::PoseStamped 类,并把 Z 轴赋予 2 米 在下面的循环中由publisher将其发布
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();
}
//建立一个类型为SetMode的服务端offb_set_mode,并将其中的模式mode设为"OFFBOARD",作用便是用于后面的客户端与服务端之间的通信(服务)
// 实例化 mavros_msgs::SetMode 类,offb_set_mode.request.custom_mode 赋 值 为“OFFBOARD”,表示我们要进入 OFFBOARD 模式
mavros_msgs::SetMode offb_set_mode;
offb_set_mode.request.custom_mode = "OFFBOARD";
//建立一个类型为CommandBool的服务端arm_cmd,并将其中的是否解锁设为"true",作用便是用于后面的客户端与服务端之间的通信(服务)
//实例化 mavros_msgs::CommandBool,arm_cmd.request.value = true;表示我们要解锁解锁 (= true 解锁 = false 加锁)
mavros_msgs::CommandBool arm_cmd;
arm_cmd.request.value = true;
//采集当前系统时间
ros::Time last_request = ros::Time::now();
while(ros::ok())
{
/*
首先判断当前模式是否为offboard模式,如果不是,
则客户端set_mode_client向服务端offb_set_mode发起请求call,然后服务端回应response将模式返回,这就打开了offboard模式
*/
//如果模式还不为 offboard,并且距离上一次执行进入 offboard 命令(或者系统首次运行)已经过去了 5秒,进入这里
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");//打开模式,打印消息 "Offboard enabled"
}
//更新当前时间
last_request = ros::Time::now();
}
//如果已经处于 offboard 模式进入这里
else
{
/*
若已经为offboard模式,则判断飞机是否解锁,如果没有解锁,
则客户端arming_client向服务端arm_cmd发起call请求;然后服务端回应response成功解锁
*/
//如果还未解锁,并且距离上次此请求解锁已经过去 5 秒,进入这里
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");//打印消息 "Vehicle armed"
}
//更新当前时间
last_request = ros::Time::now();
}
}
//发布位置消息
local_pos_pub.publish(pose);
//消息回调保持频率
ros::spinOnce();
rate.sleep();
}
return 0;
}
终端1运行:
$ cd Firmware/ && make px4_sitl gazebo //打开仿真环境
终端2运行:
$ roslaunch mavros px4.launch fcu_url:="udp://:[email protected]:14557" //MOVROS通讯
终端3运行:
$ rosrun offboard_pkg offboard_node //开启控制节点
终端4运行:
$ rostopic echo /mavros/state //查看飞机MAVROS状态
实验现象:
gazebo仿真界面的飞机飞到了指定坐标(0,0,2),然后悬停。
打开GQC地面站可以发现地面站自动连接到了仿真飞机,并显示其状态信息。
注:若您git下载PX4源码时,直接运行
$ git clone https://github.com/PX4/Firmware.git --recursive
会导致飞机无法起飞,运行rosrun offboard_pkg offboard_node时,一直输出Offboard enabled,别的终端输出
Failsafe mode deactivated
Failsafe mode activated
等,原因是建议部署旧版本代码,可解决问题:
$ git clone -b v1.12.3 https://github.com/PX4/Firmware.git --recursive