(无人机方向)ros小白学习之路(五)mavros消息的订阅和发布与offboard例程仿真与解析

---

前言

mavros消息的订阅和发布.

应用mavros控制无人机消息流

以mavros中setpoint_position/local为例子

1:确定话题的功能和消息类型

在http://wiki.ros.org/mavros下,可以看到对应的mavros消息.
找到setpoint_position/local

[1]直接阅读mavros中的说明
[2] (如果mavros说明不详细)阅读mavros源码找到对应的mavlink协议
[3] (这一步必须要做)查询对于的mavlink协议,阅读该协议的说明文档

如果快速找到对应的mavlink消息

(1):打开官网:http://mavlink.io/zh/messages/common.html
(2):按下ctrl+t会出现关键词查询

(3):输入刚刚在mavros消息下的mavlink协议(在本例子中是SET_POSITION_TARGET_GLOBAL_INT)
(4):输入进去,按回车查询

2:确定话题并最终找到转化为uorb消息

(1):打开官网:https://dev.px4.io/v1.10_noredirect/zh/concept/architecture.html

(2):打开中间件,可以查看相应的资料
(3)阅读px4的mavlink源码了解话题如何最终转化为uorb消息
(如果没有下载则去下载PX4固件)打开PX4固件下的src/modules/mavlink/mavlink_receiver.cpp和mavlink_receiver.h文件(不知道路径一不一样,不一样的话可以在文件下查询)

这个就mavlink解析数据,是根据ID判断消息类型.ID就是#后的数字,如:

然后接受完数据,就通过handle_message函数进行处理,最终转化为uorb消息

官方example offboard例程仿真与解析

这里会用到ROS消息类型中的服务,可以看我另篇文章,这里不加赘述.

确定任务需要用到的mavros话题名称与消息类型(阅读官方文档)

头文件需要包含所需要用到的消息类型.h文件

这里我的工作空间名为catkin_1,运行代码的时候改成你的工作空间名称

cd ~/catkin_1/src
catkin_create_pkg offboard_pkg roscpp rospy std_msgs geometry_msgs mavros_msgs

新建offb_node.cpp文件,include头文件

#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,应编译安装(我用的源码安装)

下载

mkdir -p ~/catkin_ws/src
cd ~/catkin_ws/src
git clone https://github.com/mavlink/mavros.git
git clone --recurse-submodules -b  release/noetic/mavlink  https://github.com/mavlink/mavlink-gbp-release.git
cd mavros
git checkout -b 1.13.0 1.13.0

编译

cd ~/catkin_ws
catkin build

mavros一定要用catkin_build,不用catkin_make
下面的操作是因为我自己的原因,可以忽略:

因为我mavros和我功能包工作空间不同,因此需要在vscode中设置IncludePath

在c_cpp_properties.json编辑:
“/home/khs/catkin_ws/devel/include/**”,

编写

转载于 参考

#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>  //发布的消息体对应的头文件，该消息体的类型为geometry_msgs：：PoseStamped
#include <mavros_msgs/CommandBool.h>  //CommandBool服务的头文件，该服务的类型为mavros_msgs：：CommandBool
#include <mavros_msgs/SetMode.h>     //SetMode服务的头文件，该服务的类型为mavros_msgs：：SetMode
#include <mavros_msgs/State.h>  //订阅的消息体的头文件，该消息体的类型为mavros_msgs：：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::init(argc, argv, "offb_node"); //ros系统的初始化，最后一个参数为节点名称
    ros::NodeHandle nh;
 
    //订阅。<>里面为模板参数，传入的是订阅的消息体类型，（）里面传入三个参数，分别是该消息体的位置、缓存大小（通常为1000）、回调函数
    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);
 
    //启动服务1，设置客户端（Client）名称为arming_client，客户端的类型为ros::ServiceClient，
    //启动服务用的函数为nh下的serviceClient<>()函数，<>里面是该服务的类型，（）里面是该服务的路径
    ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>("mavros/cmd/arming");
 
    //启动服务2，设置客户端（Client）名称为set_mode_client，客户端的类型为ros::ServiceClient，
    //启动服务用的函数为nh下的serviceClient<>()函数，<>里面是该服务的类型，（）里面是该服务的路径
    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);
 
    // 等待飞控连接mavros，current_state是我们订阅的mavros的状态，连接成功在跳出循环
    while(ros::ok() && !current_state.connected){
        ros::spinOnce();
        rate.sleep();
    }
 
 
    //先实例化一个geometry_msgs::PoseStamped类型的对象，并对其赋值，最后将其发布出去
    geometry_msgs::PoseStamped pose;
    pose.pose.position.x = 0;
    pose.pose.position.y = 0;
    pose.pose.position.z = 2;
 
    //建立一个类型为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();
 
    while(ros::ok())//进入大循环
    {
        //首先判断当前模式是否为offboard模式，如果不是，则客户端set_mode_client向服务端offb_set_mode发起请求call，
        //然后服务端回应response将模式返回，这就打开了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 //else指已经为offboard模式，然后进去判断是否解锁，如果没有解锁，则客户端arming_client向服务端arm_cmd发起请求call
            //然后服务端回应response成功解锁，这就解锁了
        {
            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();
            }
        }
 
        local_pos_pub.publish(pose); //发布位置信息，所以综上飞机只有先打开offboard模式然后解锁才能飞起来
 
        ros::spinOnce();
        rate.sleep();
    }
 
    return 0;
}