转载自：https://blog.csdn.net/SIR_wkp/article/details/87861658
MAVROS 源码分析
SIR_wkp 2019-02-21 17:52:53 2778 收藏 34
分类专栏： PX4代码分析
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/SIR_wkp/article/details/87861658
版权
一、安装 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 消息处理的回调函数可以做到心知肚明、可以避免一些事故的发生。
本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)
