机载电脑发布自定义MAVROS消息教程

        自定义MAVROS消息需要首先通过源码安装MAVROS。Ubuntu18.04下MAVROS源码安装教程见https://blog.csdn.net/real86/article/details/131891303?spm=1001.2014.3001.5501

一、创建msg消息

        在源码安装的MAVROS工作空间中创建msg消息。具体的，在~/mavros_catkin_ws/src/mavros/mavros_msgs/msg文件夹下新建TargetDetection.msg文件，该消息文件是我们要传输的消息的数据类型，在文件中写入：

std_msgs/Header header

int8 targetsDetected
uint8 num_targets

        在本例中，targetsDetected为检测到目标框的标志位，num_targets为检测到目标框的数量。

        在~/mavros_catkin_ws/src/mavros/mavros_msgs目录下的CmakeLists.txt文件中添加TargetDetection.msg文件，具体如下：

add_message_files(
...
TargetDetection.msg
)

        利用catkin build在~/mavros_catkin_ws目录下编译，确保在~/mavros_catkin_ws/devel/include/mavros_msgs目录下生成了TargetDetection.h头文件。

二、添加自定义的mavros消息

        在~/mavros_catkin_ws/src/mavlink/message_definition/v1.0目录下，修改common.xml文件，添加自定义的mavros消息，具体如下：

<message id="227" name="TARGET_DETECTION">
    <description>the detection message from nx to vehicle.</description>
    <field type="int8_t" name="targetsDetected">int8_t</field>
    <field type="uint8_t" name="num_targets">uint8_t</field>
</message>

        在~/mavros_catkin_ws/src/mavlink/message_definitions目录下，使用MAVLINK的GUI代码mavgenerate.py生成MAVLINK库文件，具体如下：

python -m mavgenerate

        在弹出的GUI中选择如下：

XML: ~/mavros_catkin_ws/src/mavlink/message_definitions/v1.0/common.xml
OUT: ~/mavros_catkin_ws/src/mavlink/message_definitions/v1.0
Language: C
Protocol: 1.0
Validate: 勾选

        点击Generate后，会在~/mavros_catkin_ws/src/mavlink/message_definitions/v1.0目录下生成common、minimal、standard文件夹。

        为了将自定义的MAVROS消息发送出去，还需要编写插件。具体的，在~/mavros_catkin_ws/src/mavros/mavros_extras/src/plugins目录下创建target_detection.cpp文件，具体如下：

#include <mavros/mavros_plugin.h>
#include <pluginlib/class_list_macros.h>
#include <iostream>
#include <mavros_msgs/TargetDetection.h>

namespace mavros {
namespace extra_plugins{

class TargetDetectionPlugin : public plugin::PluginBase {
public:
     TargetDetectionPlugin() : PluginBase(),
         nh("~target_detection"){ };

     void initialize(UAS &uas_)
     {
         PluginBase::initialize(uas_);
         mavros2fcu_sub = nh.subscribe("send_data", 10, &TargetDetectionPlugin::TargetDetection_cb, this);
     };

     Subscriptions get_subscriptions()
     {
         return {/* RX disabled */ };
     }

 private:
     ros::NodeHandle nh;
     ros::Subscriber mavros2fcu_sub;

    void TargetDetection_cb(const mavros_msgs::TargetDetection::ConstPtr &req)
     {
        mavros::UAS *m_uas_ = static_cast<TargetDetectionPlugin *>(this)->m_uas;

		mavlink::common::msg::TARGET_DETECTION send_data = {}; 

		send_data.targetsDetected = req->targetsDetected;

        send_data.num_targets = req->num_targets;

        UAS_FCU(m_uas)->send_message_ignore_drop(send_data);
     }
};
}   // namespace extra_plugins
}   // namespace mavros

PLUGINLIB_EXPORT_CLASS(mavros::extra_plugins::TargetDetectionPlugin, mavros::plugin::PluginBase)

        将自定义的插件添加到插件列表中，用于MAVROS自启动插件。具体的，在~/mavros_catkin_ws/src/mavros/mavros_extras/mavros_plugins.xml中添加：

<class name="target_detection" type="mavros::extra_plugins::TargetDetectionPlugin" base_class_type="mavros::plugin::PluginBase">
    <description>Send target detection.</description>
</class>

        在~/mavros_catkin_ws/src/mavros/mavros_extras/CmakeLists.txt中添加编译信息：

add_library(mavros_extras
  src/plugins/target_detection.cpp
）

        在~/mavros_catkin_ws目录下进行编译，在终端输入：

catkin build

三、通过ROS发布自定义的MAVROS消息

        自定义的插件target_detection.cpp会订阅数据并发布。为了验证自定义MAVROS消息的正确性，需要写一个ROS节点发布供自定义的插件订阅的数据，让自定义的插件订阅到ROS节点发布的数据并发送给飞控。首先创建发布消息的ROS功能包，具体如下：

mkdir ros_test
cd ros_test
mkdir src
cd src
catkin_init_workspace
cd ~/ros_test
catkin_make
cd src
catkin_create_pkg msg_test

        在msg_test目录下创建pub_data.cpp，用于发布自定义MAVROS消息。具体如下：

#include <time.h>
#include <ros/ros.h>
#include <mavros_msgs/TargetDetection.h>

using namespace std;

mavros_msgs::TargetDetection targets_msg;

int main(int argc, char **argv){
	ros::init(argc, argv, "target_msg");
    ros::NodeHandle nh;

    targets_msg.targetsDetected = 1;
    targets_msg.num_targets = 6;
	
	ros::Publisher test;
	test = nh.advertise<mavros_msgs::TargetDetection>("/mavros/target_detection/send_data", 10);

	ros::Rate rate(20.0);
	while(ros::ok()){
		ros::spinOnce();
		rate.sleep();
		test.publish(targets_msg);
    }
	return 0;
}

        pub_data.cpp能够发布TargetDetection类型的消息，以供自定义插件target_detection.cpp订阅。

        在msg_test目录下，修改CmakeLists.txt文件。在文件中添加：

find_package(catkin REQUIRED COMPONENTS
    mavros
    roscpp
    std_msgs
    mavros_msgs
)

include_directories(
    include
    ${catkin_INCLUDE_DIRS}
)

add_executable(target_msg pub_data.cpp)
target_link_libraries(target_msg ${catkin_LIBRARIES})

        在msg_test目录下，修改package.xml文件。在文件中添加：

  <build_depend>mavros</build_depend>
  <build_depend>roscpp</build_depend>
  <build_depend>mavros_msgs</build_depend>

  <build_export_depend>mavros</build_export_depend>
  <build_export_depend>roscpp</build_export_depend>
  <build_export_depend>std_msgs</build_export_depend>

  <exec_depend>mavros</exec_depend>
  <exec_depend>roscpp</exec_depend>
  <exec_depend>mavros_msgs</exec_depend>

        最后，在ros_test目录下编译，执行catkin_make指令。

        测试流程如下：

        1）在机载电脑的终端中启动roscore；

        2）在~/ros_test/devel目录下依次执行：

source setup.bash
rosrun msg_test target_msg

        开始通过ROS节点发布自定义的MAVROS消息；

        3）在新终端中执行：

rostopic echo /mavros/target_detection/send_data

        若输出如下，代表自定义的MAVROS消息发布成功。