MAVROS 源码分析

一、安装 MAVROS 

利用机载计算机对 PX4 飞控进行 OFFBOARD 控制，需要在机载计算机上安装 ROS 的 MAVROS 包。安装方式可以参考 PX4 开发者网站，有二进制文件安装和源码安装两种方式，选择其中一项即可。 

MAVROS 安装链接

其中源码安装之后可以获得 MAVROS 的所有源代码。

以下是 MAVROS 的官方 wiki 网址：

http://wiki.ros.org/mavros

注意：

1、安装过程中如果碰到 wget install_geographiclib_datasets.sh 文件失败，可以直接打开 链接 去复制文件内容，或者从我的 gitee 网页去下载文件。

wget https://gitee.com/SIRwkp/mavros/raw/master/mavros/scripts/install_geographiclib_datasets.sh
sudo bash ./install_geographiclib_datasets.sh

2、如果运行 mavros 无法打开串口，可以在终端中执行：

sudo usermod -a -G dialout $USER
sudo apt-get remove modemmanager -y

退出账户（logout），重新登录即可。

二、MAVROS 源码分析

阅读源码，对 MAVROS 的实现过程进行一下简要分析。方便起见我用  <workspace> 指代 ROS 工作空间的文件夹路径。

1、确定入口函数

在 <workspace>/src 目录下有 mavlink 和 mavros 两个文件夹，包含了 MAVROS 相关的所有源代码。

正常启动 MAVROS 的方法是在终端中采用 roslaunch 指令来运行， 例如 

roslaunch mavros px4.launch

其中的 px4.launch 文件又会引用 node.launch 文件，启动 "mavros" 包中的 "mavros_node" 节点，名称是 "mavros" ；还定义了启动所需的参数，包括 "fcu_url" 即与飞控通信的链路等；更多参数则定义在 px4_config.yaml 文件中；而 px4_pluginlists.yaml 文件中则是载入 pluginlib 的列表，通过白名单和黑名单的方式来进行罗列，黑名单中的 plugin 不会被载入。

查看  <workspace>/src/mavros/mavros 目录下 CMakeLists.txt 文件，可以看到生成 mavros_node 节点的是 src/mavros_node.cpp 文件。

截取自 <workspace>/src/mavros/mavros/CMakeLists.txt 文件

## Declare a cpp executable
add_executable(mavros_node
  src/mavros_node.cpp
)
add_dependencies(mavros_node
  mavros
)
target_link_libraries(mavros_node
  mavros
  ${catkin_LIBRARIES}
)

因此 MAVROS 的函数入口就在 mavros_node.cpp 文件中。其内容很简单，就一个 main 函数，如下：

#include <mavros/mavros.h>

int main(int argc, char *argv[])
{
	ros::init(argc, argv, "mavros");

	mavros::MavRos mavros;
	mavros.spin();

	return 0;
}

这里，新建了一个 mavros::MavRos 类的实例 mavros，然后运行类成员函数 spin()，进入死循环。

2、mavros::MavRos 的构造函数

mavros::MavRos 类的声明在 <workspace>/src/mavros/mavros/include/mavros.h 文件中，类成员函数的定义在 <workspace>/src/mavros/mavros/src/lib/mavros.cpp 文件中。

按实现顺序来介绍，其中的主要工作包括了：

(1) 广告 "/mavlink/from" 话题，订阅 "/mavlink/to" 话题，主要使用的代码语句有

mavlink_nh("mavlink"),		// allow to namespace it
// ROS mavlink bridge
mavlink_pub = mavlink_nh.advertise<mavros_msgs::Mavlink>("from", 100);
mavlink_sub = mavlink_nh.subscribe("to", 100, &MavRos::mavlink_sub_cb, this,
	ros::TransportHints()
		.unreliable().maxDatagramSize(1024)
		.reliable());

其中 mavlink_nh 是类成员变量， 类型是 ros::NodeHandle ，其效果就是让 "from" 和 "to" 话题具有了 "/mavlink/" 的前缀；而  "/mavlink/to" 话题的回调函数则链接到了 MavRos::mavlink_sub_cb 函数。

(2) 读取与 mavros 有直接关联的参数

	ros::NodeHandle nh("~");

	nh.param<std::string>("fcu_url", fcu_url, "serial:///dev/ttyACM0");
	nh.param<std::string>("gcs_url", gcs_url, "udp://@");
	nh.param<bool>("gcs_quiet_mode", gcs_quiet_mode, false);
	nh.param("conn/timeout", conn_timeout_d, 30.0);

	nh.param<std::string>("fcu_protocol", fcu_protocol, "v2.0");
	nh.param("system_id", system_id, 1);
	nh.param<int>("component_id", component_id, mavconn::MAV_COMP_ID_UDP_BRIDGE);
	nh.param("target_system_id", tgt_system_id, 1);
	nh.param("target_component_id", tgt_component_id, 1);
	nh.param("startup_px4_usb_quirk", px4_usb_quirk, false);
	nh.getParam("plugin_blacklist", plugin_blacklist);
	nh.getParam("plugin_whitelist", plugin_whitelist);

其中 "~" 表示当前的命名空间，也就是名称为 "/mavros" 的 "mavros_node" 节点的空间，参数在 .launch 文件中定义。最后两个参数 plugin_blacklist 和 plugin_whitelist 是字符串集合，其余都是单一字符串或者数值。

(3) 根据 fcu_url 字符串打开与飞控的通信链路

	ROS_INFO_STREAM("FCU URL: " << fcu_url);
	try {
		fcu_link = MAVConnInterface::open_url(fcu_url, system_id, component_id);
		// may be overridden by URL
		system_id = fcu_link->get_system_id();
		component_id = fcu_link->get_component_id();

		fcu_link_diag.set_mavconn(fcu_link);
		UAS_DIAG(&mav_uas).add(fcu_link_diag);
	}
	catch (mavconn::DeviceError &ex) {
		ROS_FATAL("FCU: %s", ex.what());
		ros::shutdown();
		return;
	}

成功的话，就会新建对应硬件链路形式的实例，同时建立数据接收的线程，独立进行数据接收，下面再细讲；失败的话，就会中止当前的 ros 节点，退出程序。

(4) 载入所有的 plugin

	// prepare plugin lists
	// issue #257 2: assume that all plugins blacklisted
	if (plugin_blacklist.empty() and !plugin_whitelist.empty())
		plugin_blacklist.emplace_back("*");

	for (auto &name : plugin_loader.getDeclaredClasses())
		add_plugin(name, plugin_blacklist, plugin_whitelist);

其中在 add_plugin 函数中，将每个 plugin 的名称与 plugin_blacklist 和 plugin_whitelist 两个集合进行对照，建立对应类的实例，并运行初始化函数。

(5) 定义 mavlink 消息接收回调函数

    // XXX TODO: move workers to ROS Spinner, let mavconn threads to do only IO
	fcu_link->message_received_cb = [this](const mavlink_message_t *msg, const Framing framing) {
		mavlink_pub_cb(msg, framing);
		plugin_route_cb(msg, framing);

		if (gcs_link) {
			if (this->gcs_quiet_mode && msg->msgid != mavlink::common::msg::HEARTBEAT::MSG_ID &&
				(ros::Time::now() - this->last_message_received_from_gcs > this->conn_timeout)) {
				return;
			}

			gcs_link->send_message_ignore_drop(msg);
		}
	};

主要是 mavlink_pub_cb 和 plugin_route_cb 两个函数。其中 mavlink_pub_cb 函数直接将 mavlink 消息转换成 mavros_msgs::Mavlink 类型的 ros 消息发布到话题  "/mavlink/from" 中；plugin_route_cb 函数则是根据 msgid 从所有的 plugin 中寻找可以进行消息处理的函数进行消息处理，发布经过了解析之后的 mavros_msgs 消息。

3、mavlink 数据接收

以 serial 链路为例，当在 MavRos 构造函数中执行 fcu_link = MAVConnInterface::open_url(fcu_url, system_id, component_id); 语句是， fcu_url 字符串中的设备类型、设备参数被解析，针对 serial 设备执行了 url_parse_serial 函数，如下

static MAVConnInterface::Ptr url_parse_serial(
		std::string path, std::string query,
		uint8_t system_id, uint8_t component_id, bool hwflow)
{
	std::string file_path;
	int baudrate;

	// /dev/ttyACM0:57600
	url_parse_host(path, file_path, baudrate, MAVConnSerial::DEFAULT_DEVICE, MAVConnSerial::DEFAULT_BAUDRATE);
	url_parse_query(query, system_id, component_id);

	return std::make_shared<MAVConnSerial>(system_id, component_id,
			file_path, baudrate, hwflow);
}

最后一句中，建立了一个 MAVConnSerial 类型的实例，并返回其指针。

MAVConnSerial 的构造函数中，建立了独立的线程进行数据接收，如下所示，数据接收函数入口是 MAVConnSerial::do_read

	// give some work to io_service before start
	io_service.post(std::bind(&MAVConnSerial::do_read, this));

	// run io_service for async io
	io_thread = std::thread([this] () {
				utils::set_this_thread_name("mserial%zu", conn_id);
				io_service.run();
			});

在 do_read 函数中，读取数据、进行解析，再继续 do_read；如下所示，解析函数就是 parse_buffer 。

void MAVConnSerial::do_read(void)
{
	auto sthis = shared_from_this();
	serial_dev.async_read_some(
			buffer(rx_buf),
			[sthis] (error_code error, size_t bytes_transferred) {
				if (error) {
					CONSOLE_BRIDGE_logError(PFXd "receive: %s", sthis->conn_id, error.message().c_str());
					sthis->close();
					return;
				}

				sthis->parse_buffer(PFX, sthis->rx_buf.data(), sthis->rx_buf.size(), bytes_transferred);
				sthis->do_read();
			});
}

在 MAVConnSerial 的父类 MAVConnInterface 中，定义了 MAVConnInterface::parse_buffer 函数，它会将接收的数据逐个字节填入 mavlink 数据解析函数中，并监控是否接收到一条完整的新消息。如下，c 是一个字节的数据。

auto msg_received = static_cast<Framing>(mavlink::mavlink_frame_char_buffer(&m_buffer, &m_status, c, &message, &status));

然后针对 msg_received 的不同状态进行处理，包括 校验错误、署名错误等情况。当消息接收完成后，运行

message_received_cb(&message, msg_received);

也就是在 MavRos 构造函数中定义的 mavlink 消息接收回调函数，参看以上 2(5) 的解释。

不论是 udp 设备还是 tcp 设备，都采用了与 serial 设备类似的方式；最终都会调用 message_received_cb 函数来对mavlink 消息进行处理。

4、通过 plugin 将 mavlink 消息发布为 ros 消息

(1) 载入 plugin 

在 MavRos::add_plugin 函数中，通过 plugin 名称的字符串建立了 plugin 的实例，如下：

    auto plugin = plugin_loader.createInstance(pl_name);
    
    ROS_INFO_STREAM("Plugin " << pl_name << " loaded");

然后通过 get_subscriptions 函数，获取到 plugin 中可以发布消息的函数的 info ，进而得到 msgid 等信息，将 msgid 与 info 组合成一个对应项，添加到 plugin_subscriptions 集合中；然后运行了 initialize 函数，再将 plugin 加入 loaded_plugins 中。

for (auto &info : plugin->get_subscriptions()) {
	auto msgid = std::get<0>(info);
	auto msgname = std::get<1>(info);
	auto type_hash_ = std::get<2>(info);

    ...

    auto it = plugin_subscriptions.find(msgid);
	if (it == plugin_subscriptions.end()) {
		// new entry
		ROS_DEBUG_STREAM(log_msgname << " - new element");
		plugin_subscriptions[msgid] = PluginBase::Subscriptions{{info}};
	}

    ...

    plugin->initialize(mav_uas);
    loaded_plugins.push_back(plugin);
}

(2) 将 mavlink 消息交给 plugin 来处理为 ros 消息

由以上 3 可知，当接收到一个完整的 mavlink 消息时 message_received_cb 函数会被调用，首先通过 mavlink_pub_cb 函数发布一个 "/mavlink/from" 话题的消息，然后通过 plugin_route_cb 来分发给对应的 plugin。实现方式如下

void MavRos::plugin_route_cb(const mavlink_message_t *mmsg, const Framing framing)
{
	auto it = plugin_subscriptions.find(mmsg->msgid);
	if (it == plugin_subscriptions.end())
		return;

	for (auto &info : it->second)
		std::get<3>(info)(mmsg, framing);
}

从 plugin_subscriptions 集合中寻找所因为 msgid 的项， 如果找到了这个项，就执行其对应的消息处理函数。

(3) 举例说明

当 global_position.cpp 对应的 plugin 被载入时，会调用 get_subscriptions() 函数，如下所示：

	Subscriptions get_subscriptions()
	{
		return {
				make_handler(&GlobalPositionPlugin::handle_gps_raw_int),
				// GPS_STATUS: there no corresponding ROS message, and it is not supported by APM
				make_handler(&GlobalPositionPlugin::handle_global_position_int),
				make_handler(&GlobalPositionPlugin::handle_gps_global_origin),
				make_handler(&GlobalPositionPlugin::handle_lpned_system_global_offset)
		};
	}

这些 handle 函数具有统一的参数形式，其中第二项参数就包含了 msgid、name、type_hash 的信息，而 make_handle 函数就是从这个函数的参数中提取这些信息，并作为索引项。

	template<class _C, class _T>
	HandlerInfo make_handler(void (_C::*fn)(const mavlink::mavlink_message_t*, _T&)) {
		auto bfn = std::bind(fn, static_cast<_C*>(this), std::placeholders::_1, std::placeholders::_2);
		const auto id = _T::MSG_ID;
		const auto name = _T::NAME;
		const auto type_hash_ = typeid(_T).hash_code();

		return HandlerInfo{
			id, name, type_hash_,
			[bfn](const mavlink::mavlink_message_t *msg, const mavconn::Framing framing) {
				if (framing != mavconn::Framing::ok)
					return;

				mavlink::MsgMap map(msg);
				_T obj;
				obj.deserialize(map);

				bfn(msg, obj);
			}
		};
	}

当 msgid 与 mavlink 消息中值对应是，就执行了 (info)(mmsg, framing) 函数，真正的处理函数就被调用。

例如 handle_gps_raw_int 函数就会根据 mavlink 消息的内容，发布话题为"raw/fix" 和 "raw/gps_vel" 的两个消息。

类似的， 所有符合条件的 mavlink 消息都会被包装成 ros 消息进行发布。

5、通过 plugin 对飞控发布 mavlink 消息

例如，用户程序发布了话题为 "/mavros/setpoint_position/local" 的消息用来控制飞机的在局部坐标系中的位置；在 setpoint_position.cpp 文件中定义了 SetpointPositionPlugin 类来进行该消息的处理。

(1) 订阅消息

在 initialize 函数中，订阅了话题为  "/mavros/setpoint_position/local" 的消息，回调函数是 SetpointPositionPlugin::setpoint_cb

setpoint_sub = sp_nh.subscribe("local", 10, &SetpointPositionPlugin::setpoint_cb, this);

(2) 回调函数处理 ros 消息

经过坐标变换得到与 mavlink 协议对应的格式，进行了 mavlink 消息的封装

(3) 发送 mavlink 消息

UAS_FCU(&mav_uas)->send_message_ignore_drop(&mmsg);

具体的发送实现过程可以参考具体设备的 send 函数。

6、总结：

通过对源码的分析，可以知道 MAVROS 在与飞控进行通讯中，首先利用链路设备独立的数据接收线程来获取来自飞控的数据；当数据接收后，通过比对 plugin 与 mavlink 消息的 msgid 来选择对应的消息处理函数，该函数将 mavlink 消息转换为 ros消息并发布；用户发布的控制指令通过 ros 消息被 plugin 接收到，然后转换为 mavlink 消息再通过设备的数据发送函数发送给飞控。

源码中注释内容清晰明确，在进行 OFFBOARD 功能开发时，阅读对应 ros 消息处理的回调函数可以做到心知肚明、可以避免一些事故的发生。