MAVROS接收PX4飞控消息

MAVROS接收PX4飞控消息

px4飞控，使用板载控制器，实现实时读取飞控信息的功能。部分飞控信息已能够通过mavros接收并读取，例如"mavros/local_position/pose"，部分飞控信息则没有通过mavros发送。本篇博客针对这部分信息进行处理，以发布悬停升力hover_thrust为例。

整个流程图如下：

一、飞控信息的发布

在飞控代码中，飞控信息通过UORB发送消息，这部分代码已经存在于Firmware中，例如：~/Firmware/src/modules/mc_hover_thrust_estimator模块中发送悬停升力的消息：

uORB::Publication<hover_thrust_estimate_s> _hover_thrust_ekf_pub{ORB_ID(hover_thrust_estimate)};

...

_hover_thrust_ekf_pub.publish(status_msg);

二、MAVLink

飞控通过MAVLink模块发送接收到的UORB信息，需要在Firmware中新建自定义的MAVLink消息类型，并添加到发送流中去，具体步骤如下：

①板载控制器上源代码安装MAVLink

安装依赖项：

sudo apt-get install python-wstool python-rosinstall-generator python-catkin-tools
sudo apt-get install python3-pip
pip3 install --user future
sudo apt-get install python3-tk

源代码安装：

wstool init ~/catkin_mav/src
rosinstall_generator mavlink | tee -a /tmp/mavros.rosinstall
wstool merge -t src /tmp/mavros.rosinstall
wstool update -t src
rosdep install --from-paths src --ignore-src --rosdistro melodic
catkin build

②Firmware中自定义MAVLink消息

修改~/Firmware/mavlink/include/mavlink/v2.0/message_definitions/common.xml文件，添加自定义消息：

<message id="13000" name="MY_MESSAGE_INMAVLINK">
      <description>my message</description>
      <field type="float" name="hover_thrust">float32_array</field>
</message>

使用MAVLink的图形用户界面代码生成器mavgenerate.py生成MAVLink库文件，在MAVLink的安装目录下运行：

python3 -m mavgenerate

XML：~/Firmware/mavlink/include/mavlink/v2.0/message_definitions/standard.xml
OUT：~/Firmware/mavlink/include/mavlink/v2.0
Language：C
Protocol：2.0
Validate：勾选
点击Generate按钮，在~/Firmware/mavlink/include/mavlink/v2.0/中会生成common和standard文件夹。

在~/Firmware/mavlink/include/mavlink/v2.0/中生成的common文件夹中能找到生成的.h文件：
mavlink_msg_my_message_inmavlink.h

③添加自定义mavlink消息到发送流

首先，在~/Firmware/src/modules/mavlink/mavlink_messages.cpp中添加UORB头文件

#include <uORB/topics/hover_thrust_estimate.h>

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

class MavlinkStreamMyMessage : public MavlinkStream
{
public:
	const char *get_name() const override
	{
		return MavlinkStreamMyMessage::get_name_static();
	}

	static constexpr const char *get_name_static()
	{
		return "MY_MESSAGE";
	}

	static constexpr uint16_t get_id_static()
	{
		return MAVLINK_MSG_ID_MY_MESSAGE_INMAVLINK;
	}

	uint16_t get_id() override
	{
		return get_id_static();
	}

	static MavlinkStream *new_instance(Mavlink *mavlink)
	{
		return new MavlinkStreamMyMessage(mavlink);
	}

	unsigned get_size() override
	{
		return _hover_thrust_sub.advertised() ? (MAVLINK_MSG_ID_MY_MESSAGE_INMAVLINK_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0;
	}

private:
	uORB::Subscription _hover_thrust_sub{ORB_ID(hover_thrust_estimate)};

	/* do not allow top copying this class */
	MavlinkStreamMyMessage(MavlinkStreamMyMessage &) = delete;
	MavlinkStreamMyMessage &operator = (const MavlinkStreamMyMessage &) = delete;

protected:
	explicit MavlinkStreamMyMessage(Mavlink *mavlink) : MavlinkStream(mavlink)
	{}

	bool send(const hrt_abstime t) override
	{
		hover_thrust_estimate_s _hover_thrust_estimate;
		if (_hover_thrust_sub.update(&_hover_thrust_estimate)) {
			mavlink_my_message_inmavlink_t msg{};
			msg.hover_thrust = _hover_thrust_estimate.hover_thrust;
			mavlink_msg_my_message_inmavlink_send_struct(_mavlink->get_channel(), &msg);
			return true;
		}
		return false;
	}
};

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

create_stream_list_item<MavlinkStreamMyMessage>()

最后，在~/Firmware/src/modules/mavlink/mavlink_messages.cpp中添加消息的发布频率：

configure_stream_local("MY_MESSAGE", 1.0f);

④make

修改完成，在Firmware目录下执行命令：

make px4_sitl_default

三、MAVROS

板载控制器通过MAVROS读取飞控发布的MAVLink消息，需要板载控制器安装MAVLink，并且新建自定义的与飞控发布的MAVLink消息类型相同的MAVLink消息类型。需要板载控制器安装MAVROS，并且新建自定义MAVROS消息用于发布。

①板载控制器上源代码安装MAVROS

源代码安装：

rosinstall_generator --upstream mavros | tee /tmp/mavros.rosinstall
wstool merge -t src /tmp/mavros.rosinstall
wstool update -t src
rosdep install --from-paths src --ignore-src --rosdistro melodic
catkin build

②板载控制器中自定义MAVLink消息

修改~/catkin_ws/src/mavlink/message_definitions/v1.0/common.xml文件，添加自定义消息：

<message id="13000" name="MY_MESSAGE_INMAVLINK">
      <description>my message</description>
      <field type="float" name="hover_thrust">float32_array</field>
</message>

使用MAVLink的图形用户界面代码生成器mavgenerate.py生成MAVLink库文件，在MAVLink的安装目录下运行：

python3 -m mavgenerate

XML：~/catkin_ws/src/mavlink/message_definitions/v1.0/standard.xml
OUT：~/catkin_ws/src/mavlink/message_definitions/v1.0
Language：C
Protocol：1.0
Validate：勾选
点击Generate按钮，在~/catkin_ws/src/mavlink/message_definitions/v1.0/中会生成common和standard文件夹。

在~/catkin_ws/src/mavlink/message_definitions/v1.0/中生成的common文件夹中能找到生成的.h文件：
mavlink_msg_my_message_inmavlink.h

③板载控制器中自定义MAVROS消息

在~/catkin_ws/src/mavros/mavros_msgs/msg中自定义msg文件：
MyMessage.msg

std_msgs/Header header
float32 hover_thrust

在~/catkin_ws/src/mavros/mavros_msgs/CMakeLists.txt中添加内容：

add_message_files(
  	MyMessage.msg
)

④板载控制器中创建MAVROS消息处理插件plugin

将接受到的MAVLink消息分配给不同的plugin进行处理。在~/catkin_ws/src/mavros/mavros_extras/src/plugins创建my_message.cpp，代码如下：

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

namespace mavros {
namespace extra_plugins {
class MyMessagePlugin:public plugin::PluginBase {
public:
	MyMessagePlugin():PluginBase(),
			my_message_nh("~my_message_nh")
			{}
			void initialize(UAS &uas_) override {
				PluginBase::initialize(uas_);
				my_message_pub = my_message_nh.advertise<mavros_msgs::MyMessage>("my_message", 10);
			}
			Subscriptions get_subscriptions() override {
				return{
					make_handler(&MyMessagePlugin::handle_my_message)
				};
			}

private:
	ros::NodeHandle my_message_nh;
	ros::Publisher my_message_pub;
	// mavlink::common::msg::MY_MESSAGE_INMAVLINK为自动生成的消息头文件中所定义的，也是依据此来解析收到的mavlink消息。
	void handle_my_message(const mavlink::mavlink_message_t *msg, mavlink::common::msg::MY_MESSAGE_INMAVLINK &my_message) {
		auto tmp = boost::make_shared<mavros_msgs::MyMessage>();
		tmp->hover_thrust = my_message.hover_thrust;
		my_message_pub.publish(tmp);//将解析到的消息发布至topic
	}
};
}
}

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

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

<class name="my_message" type="mavros::extra_plugins::MyMessagePlugin" base_class_type="mavros::plugin::PluginBase">
		<description>Handle my messages</description>
</class>

在~/catkin_ws/src/mavros/mavros_extras/cmakelists添加编译信息

add_library(mavros_extras
  src/plugins/my_message.cpp
）

⑤编译

修改完成，在工作空间catkin_ws下执行命令：

catkin build

四、测试

Firmware文件夹下终端运行：

make px4_sitl_default gazebo

工作空间catkin_ws下终端运行：

source devel/setup.bash
roslaunch mavros px4.launch

在工作空间catkin_ws下另开一个终端运行：

source devel/setup.bash
rostopic list

可以看到已有消息发布：

运行

rostopic echo /mavros/my_message_nh/my_message 

即可看到发布的消息：

五、参考

PX4/Pixhawk—uORB深入理解和应用
安装 MAVLink
ubuntu 安装mavros和mavlink
实现MAVROS与px4的自定义通讯功能（一）
[pixhawk笔记]7-MAVLink添加自定义消息