【ROS】wiki教程总结以及offboard仿真实现过程

概览

本文基于wiki教程，ROS版本为melodic，使用catkin

ROS基本概念

• Node:使用 ROS 与其他节点通信的可执行文件；

• Package:功能元件，每个包都可以包含库、可执行文件、脚本或其他文件，一般是功能的最小组成；网上有很多功能包；

• Master: ROS命名系统（简单理解：帮助节点找到彼此）

• Messages：订阅或发布主题时使用的 ROS 基本数据类型；

• Topics: 节点（Nodes ）可以publish topic以发布消息，同时节点也可以subscribe topic 去接收消息；

• package.xml：用来描述包，例如：用到的依赖，作者等等；

• Cmakelist.txt：简单理解为编译链接的辅助文件，在编写源码后要进行手动添加编译信息和链接库。

• roscore: Master + rosout + parameter server (参数系统稍后介绍)

文件结构框架

在ROS中，首先要明确以下几个概念：

• workspace：工作空间，通过在这此文件夹输入catkin_init_workspace，使之成为工作空间
• src：存放源码的文件夹
• devel：开发文件夹
• build：编译空间
• install：作为成品的打包空间（ROS3好像已经取缔了这个文件夹，生成此文件夹需要在编译时添加catkin_make -install

他们之间的逻辑结构为:

其中src是在最开始最需要关注的部分，因为其中存放着功能包，所以 workspace/src 路径下的内容为：

节点(node)之间通讯

ros中通信分为两种类型：
- service和client
- publish和subscribe
这部分比较重要，在后期的开发过程中需要理解其中流程，同时需要有自定义msg并传递的能力
自定义msg

使用图形工具

在ros中有一些很好的可视化工具，可以对节点之间的topic进行直观分析，这里只列举出官方教程中的：

rqt_plot：显示主题上发布的数据的滚动时间图

rqt_graph创建系统中正在节点间的动态图：

此图中显示出两个ros节点以及之间的topic

从头创建一个offboard包，同时在PX4-gazebo下进行悬停仿真

创建工作空间

• 首先在虚拟机中新建一个文件夹作为工作空间，此处命名为2811；
• 键入catkin_init_workspace后会生成CMakeLists.txt，此时此文件夹则正式成为我们的工作空间
  

创建功能包

• 新建src文件夹以存放我们的功能包，
• 在src下键入catkin_create_pkg m_offboard std_msgs rospy roscpp进行功能包的创建，其中m_offborad为包名，后面的为这个包的依赖；
  

编写功能源码

• 在功能包m_offboard中的src下创建offboard.cpp以存放源码，以下为注释后的悬停源码

#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系统，节点命名
    ros::init(argc, argv, "offb_node"); 
    ros::NodeHandle nh;

//创建订阅Subscribe；<>里面为模板参数，传入的是订阅的消息体类型，（）里面传入三个参数，分别是该消息的位置、缓存大小（通常为10）、回调函数
    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");

//发送频率设置为2hz
    ros::Rate rate(20.0);

// 等待飞控连接mavros，其中current_state是我们订阅的mavros的状态，连接成功后再跳出循环
    while(ros::ok() && !current_state.connected){
        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;



//自定义一个geometry_msgs::PoseStamped类型的对象，在下面的循环中由publisher将其发布
    geometry_msgs::PoseStamped pose;
    pose.pose.position.x = 0;
    pose.pose.position.y = 0;
    pose.pose.position.z = 2;

//更新时间
    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();
        }
//若已经为offboard模式，则判断飞机是否解锁，如果没有解锁，则客户端arming_client向服务端arm_cmd发起call请求；然后服务端回应response成功解锁        
        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();
            }
        }
 //发布位置信息
        local_pos_pub.publish(pose);

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

    return 0;
}

修改Cmakelist.txt以正确编译链接

打开m_offboard的Cmaklists.txt文件，将以下两段添加至合适的位置；

add_executable(offboard src/offboard.cpp)

target_link_libraries(offboard${catkin_LIBRARIES})

仿真模拟悬停

回到工作空间的根目录下键入

catkin_make//编译
source devel/setup.bash

roscore//运行ros核心
rosrun m_offboard offboard//启动编写的悬停节点
roslaunch px4 mavros_posix_sitl.launch //启动gazebo和mavros

最终可以在gazebo中得到以下仿真结果，飞机将会悬停在2m处