实现MAVROS与px4的自定义通讯功能（一）

一、功能

将自定义消息从mavros发送给px4，然后px4再通过mavros发送出去，实现在外部程序发送消息并接受px4回传的消息。

二、关于MAVROS二次开发

mavros的安装方法有两种：二进制安装和源码安装，相关资料自行查找。但是有二次开发需求的，建议源码安装的方式。

三、功能实现

分三步实现：实现 将自定义消息从mavros发送给px4、将自定义消息从px4发送给mavros、连接前两步。

（一） 将自定义消息从px4发送给mavros

1.自定义mavros消息

• 路径：~/catkin_ws3/src/mavros/mavros_msgs/msg（mavros的路径）
• 自定义msg文件：gs_param.msg
• 文件内容：

std_msgs/Header header
float32 gsarray

2.修改CMakeLists.txt

• 路径：~/catkin_ws3/src/mavros/mavros_msgs/
• 添加内容：

add_message_files(
  	gs_param.cpp
)

3.自定义MAVLINK消息

修改common.xml文件

• 路径：～/catkin_ws3/src/mavlink/message_definitions/v1.0/common.xml
• 添加内容：

<message id="227" name="GS_PARAM_INMAVLINK">
    <description>gs test for empty array</description>
    <field type="float" name="gsarray">float32_array</field>
  </message>

然后使用mavgenerate.py 重新生成common头文件, 在/～catkin_ws3/src/mavlink下

python mavgenerate.py

打开一个“MAVLink Generator”应用程序窗口

XML一栏选择～/catkin_ws3/src/mavlink/include/mavlink/v1.0/message_definitions/standard.xml。
输出一栏选择～/catkin_ws3/src/mavlink/include/mavlink/v1.0/
语言一栏选择C
选择1.0
勾选Validate
然后点击 Generate 按钮。 在～/catkin_ws3/src/mavlink/include/mavlink/v1.0/中会生成common和standard文件夹。

4 创建mavros消息处理插件plugin

插件的功能就是将接受到的mavlink消息分配给不同的plugin进行处理。根据我们要实现的功能，现在的数据流向是px4到mavros，所以此时插件要实现的功能是将px4发送过来的名为mavlink::common::msg::GS_PARAM_INMAVLINK的mavlink消息转换为名为“mavros_msgs::gs_param”的mavros消息。
在～/catkin_ws3/src/mavros/mavros_extras/src/plugins创建gs_param.cpp
代码理解：首先创建了一个名为“gs_param_nh/gs_test”的rostopic，handle_gs_param函数用来处理接受到的mavlink消息。

#include <mavros/mavros_plugin.h>
#include <mavros_msgs/gs_param.h>

namespace mavros {
namespace std_plugins {
class gsparamPlugin:public plugin::PluginBase{
public:
	gsparamPlugin():PluginBase(),
			gs_param_nh("~gs_param_nh")
			{}
			void initialize(UAS &uas_){
				PluginBase::initialize(uas_);
				gs_param_pub=gs_param_nh.advertise<mavros_msgs::gs_param>("gs_test",10);
			}
			Subscriptions get_subscriptions() {
				return{
					make_handler(&gsparamPlugin::handle_gs_param),
				};
			}
private:
	ros::NodeHandle gs_param_nh;
	ros::Publisher gs_param_pub;
	 //mavlink::common::msg::GS_PARAM_INMAVLINK为自动生成的消息头文件中所定义的，也是依据此来解析收到的mavlink消息。
	void handle_gs_param(const mavlink::mavlink_message_t *msg,
	 mavlink::common::msg::GS_PARAM_INMAVLINK &gs_testparam){
		 auto gs_param_msg = boost::make_shared<mavros_msgs::gs_param>();
		 //gs_param_msg->gsarray=9;
		 gs_param_msg->gsarray=gs_testparam.gsarray;
		 gs_param_pub.publish(gs_param_msg);//将解析到的消息发布至topic
	 }
};
}
}

#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(mavros::std_plugins::gsparamPlugin,mavros::plugin::PluginBase)

在自定义的插件添加到插件列表中，用于MAVROS自启动插件：
路径：～/catkin_ws3/src/mavros/mavros_extras/src/plugins/mavros_plugins.xml

<class name="gs_param" type="mavros::std_plugins::gsparamPlugin" base_class_type="mavros::plugin::PluginBase">
		<description>Handle the gs_param messages</description>
	</class>

在cmakelists添加编译信息

add_library(mavros_extras
  src/plugins/gs_param.cpp
  ）

在workspace里catkin build，然后可以运行一下mavros，看插件是否正常加载（我的pixhawk是USB连接，所以按以下指令运行，别的方式自行查询）：

source devel/setup.bash
roslaunch mavros px4.launch fcu_url:=/dev/ttyACM0

如果终端看到

plugins gs_param loaded
plugins gs_param initialized

就说明插件成功加载

至此，mavros端的配置就做完了，下面进行px4里的配置

5 px4/Firmware添加mavlink消息

步骤和3中所示方法一样,添加的代码要和3中相同(ID一定要一致)
common.xml的路径:～/px4_1.8/Firmware/mavlink/include/mavlink/v2.0/message_definitions

XML一栏选择～/px4_1.8/Firmware/mavlink/include/mavlink/v2.0/message_definitions/standard.xml。
输出一栏选择~/px4_1.8/Firmware/mavlink/include/mavlink/v2.0
语言一栏选择C
选择2.0
勾选Validate
然后点击 Generate 按钮。 在~/px4_1.8/Firmware/mavlink/include/mavlink/v2.0/中会生成common和standard文件夹。

6 添加自定义UORB消息

在Firmware/msg下添加gs.msg

float32 test_num

然后编译一下，先生成对应的gs.h头文件

make px4fmu-v5_default

7 发送自定义mavlink消息

首先，在mavlink_messages.cpp中添加uORB消息的头文件：
路径：～/px4_1.8/Firmware/src/modules/mavlink/mavlink_messages.cpp
添加头文件

#include <uORB/topics/gs.h>

然后，在mavlink_messages.cpp中新建一个消息流的类：

    class MavlinkStreamMymessage : public MavlinkStream
    {
    public:
        const char *get_name() const
        {
            return MavlinkStreamMymessage::get_name_static();
        }
        static const char *get_name_static()
        {
            return "GS_PARAM_INMAVLINK";
        }
        static uint16_t get_id_static()
        {
            return MAVLINK_MSG_ID_GS_PARAM_INMAVLINK;
        }
        uint16_t get_id()
        {
            return get_id_static();
        }
        static MavlinkStream *new_instance(Mavlink *mavlink)
        {
            return new MavlinkStreamMymessage(mavlink);
        }
        unsigned get_size()
        {
            return MAVLINK_MSG_ID_GS_PARAM_INMAVLINK_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
        }
    private:
        MavlinkOrbSubscription *_sub;
        uint64_t _gs_time;
        /* do not allow top copying this class */
        MavlinkStreamMymessage(MavlinkStreamMymessage &);
        MavlinkStreamMymessage& operator = (const MavlinkStreamMymessage &);
    protected:
        explicit MavlinkStreamMymessage(Mavlink *mavlink) : MavlinkStream(mavlink),
            _sub(_mavlink->add_orb_subscription(ORB_ID(gs))), // make sure you enter the name of your uorb topic here
            _gs_time(0)
        {}
        bool send(const hrt_abstime t)
        {
            struct gs_s _gs;    //make sure gs_s is the definition of your uorb topic
            if (_sub->update(&_gs)) {
                mavlink_gs_param_inmavlink_t msg;//make sure mavlink_gs_param_inmavlink_t is the definition of your custom mavlink message 
                
                msg.gsarray = _gs.test_num;
		       //msg.gsarray = 8;
                mavlink_msg_gs_param_inmavlink_send_struct(_mavlink->get_channel(), &msg);
            }
			return true;
        }
    };

接下来，在mavlink_messages.cpp的文件末尾，将这个消息流的类追加到treams_list

StreamListItem(&MavlinkStreamMymessage::new_instance, &MavlinkStreamMymessage::get_name_static, &MavlinkStreamMymessage::get_id_static)

8 定义一个新的UORB topic

为了省事，直接用了px4_simple_app的模板
路径：～/px4_1.8/Firmware/src/examples/px4_simple_app
打开px4_simple_app.c

#include <px4_config.h>
#include <px4_tasks.h>
#include <px4_posix.h>
#include <unistd.h>
#include <stdio.h>
#include <poll.h>
#include <string.h>
#include <math.h>

#include <uORB/uORB.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/gs.h>

__EXPORT int px4_simple_app_main(int argc, char *argv[]);

int px4_simple_app_main(int argc, char *argv[])
{
    PX4_INFO("Hello gs!");

    /*定义话题结构*/
    struct gs_s test;
    /*初始化数据*/
    memset(&test, 0, sizeof(test));

    /*公告主题*/
    /*test_pub 为handle指针*/
    orb_advert_t test_pub = orb_advertise(ORB_ID(gs), &test);

    /*test数据赋值*/
     test.test_num = 2;

    /*发布测试数据*/
    orb_publish(ORB_ID(gs), test_pub, &test);

    /*订阅数据，在copy之前，必须要订阅*/
    /*test_sub_fd为handle*/
    int test_sub_fd = orb_subscribe(ORB_ID(gs));

    struct gs_s data_copy;

    /*copy数据*/
    orb_copy(ORB_ID(gs), test_sub_fd, &data_copy);

    /*打印*/
    PX4_WARN("[px4_simple_app] GanTA:\t%f",
                         (double)data_copy.test_num
                         );

    return 0;
}

然后在/home/gs/px4_1.8/Firmware/cmake/configs/nuttx_px4fmu-v5_default.cmake里添加
examples/px4_simple_app

然后编译，上传

9测试

写一个简单的测试程序

#include <time.h>
#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include<iostream>
#include <mutex>
#include <cstdio>
#include <iomanip>
#include <stdlib.h>
#include<algorithm>
#include<chrono>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <mavros_msgs/gs_param.h>

//#include <stdio.h>
//#include"utility.h"


using namespace std;
//using namespace cv;

#define NOMINMAX

mavros_msgs::gs_param current_state;
void state_cb(const mavros_msgs::gs_param::ConstPtr &msg){
	current_state=*msg;
}

int main(int argc, char **argv){
	
	ros::init(argc, argv, "communication_node");
        ros::NodeHandle nh;
	ros::Subscriber state_sub = nh.subscribe<mavros_msgs::gs_param>
		("mavros/gs_param_nh/gs_test", 10, state_cb);
	ros::Rate rate(20.0);	
		
	mavros_msgs::gs_send gs_msg;
        
	while(ros::ok()){
       	cout<<"test result   "<<current_state.gsarray<<endl;
		ros::spinOnce();
		rate.sleep();
        }
	//ROS_INFO("print msg");

	return 0;
}

rosrun一下，懒得截图了，终端应该可以看到test result 2（因为UORB topic 里定义了test.test_num = 2;）
**提示：想要运行出结果，要在地面站的nsh端输入px4_simple_app.

nsh>>px4_simple_app

因为这个UORB topic不能自启动，需要在地面站手动输入启动一下,然后topic才会订阅我们定义的变量，之后按照数据流向，最终输出在终端上
自启动好像和rcs设置有关，以后再研究一下**

小结

看了前面的总结可能有点乱，所以首先要认识mavros的功能，mavlink和uorb消息的联系，这里总结一下：数据流向：
（1）初始化msg，UORB topic发布msg，即步骤8;
（2）在mavlink_messages.cpp里订阅了UORB topic，然后将读取到的msg的值赋给了mavlink消息，并将mavlink消息发送出去，即步骤7;
（3）外部设备接受mavlink消息，并将获取到的mavlink消息交给对应的plugin进行处理;plugin将获取到的mavlink消息转换成ros消息，即步骤4;
（4）在程序里订阅ros消息，就可以查看对应的消息，即步骤9;

目前实现了第一大块的功能

参考：mavros二次开发.