MAVROS 源码分析

2023-12-18 05:18
文章标签 分析 源码 mavros

本文主要是介绍MAVROS 源码分析，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

转载自：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_nodesrc/mavros_node.cpp
)
add_dependencies(mavros_nodemavros
)
target_link_libraries(mavros_nodemavros${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 URLsystem_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 blacklistedif (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 IOfcu_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:57600url_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 startio_service.post(std::bind(&MAVConnSerial::do_read, this));// run io_service for async ioio_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 entryROS_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 APMmake_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 消息处理的回调函数可以做到心知肚明、可以避免一些事故的发生。

这篇关于MAVROS 源码分析的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！


http://www.chinasem.cn/article/507162。

相关文章

性能分析之MySQL索引实战案例

性能分析之MySQL索引实战案例

文章目录 一、前言二、准备三、MySQL索引优化四、MySQL 索引知识回顾五、总结 一、前言 在上一讲性能工具之 JProfiler 简单登录案例分析实战中已经发现SQL没有建立索引问题，本文将一起从代码层去分析为什么没有建立索引？ 开源ERP项目地址：https://gitee.com/jishenghua/JSH_ERP 二、准备 打开IDEA找到登录请求资源路径位置
阅读更多...
JAVA智听未来一站式有声阅读平台听书系统小程序源码

JAVA智听未来一站式有声阅读平台听书系统小程序源码

智听未来，一站式有声阅读平台听书系统 🌟&nbsp;开篇：遇见未来，从“智听”开始 在这个快节奏的时代，你是否渴望在忙碌的间隙，找到一片属于自己的宁静角落？是否梦想着能随时随地，沉浸在知识的海洋，或是故事的奇幻世界里？今天，就让我带你一起探索“智听未来”——这一站式有声阅读平台听书系统，它正悄悄改变着我们的阅读方式，让未来触手可及！ 📚&nbsp;第一站：海量资源，应有尽有 走进“智听
阅读更多...
Java ArrayList扩容机制 （源码解读）

Java ArrayList扩容机制 （源码解读）

结论：初始长度为10，若所需长度小于1.5倍原长度，则按照1.5倍扩容。若不够用则按照所需长度扩容。 一. 明确类内部重要变量含义         1：数组默认长度         2:这是一个共享的空数组实例，用于明确创建长度为0时的ArrayList ，比如通过 new ArrayList<>(0)，ArrayList 内部的数组 elementData 会指向这个 EMPTY_EL
阅读更多...
如何在Visual Studio中调试.NET源码

如何在Visual Studio中调试.NET源码

今天偶然在看别人代码时，发现在他的代码里使用了Any判断List<T>是否为空。 我一般的做法是先判断是否为null，再判断Count。 看了一下Count的源码如下： 1 [__DynamicallyInvokable]2 public int Count3 {4 [__DynamicallyInvokable]5 get
阅读更多...
SWAP作物生长模型安装教程、数据制备、敏感性分析、气候变化影响、R模型敏感性分析与贝叶斯优化、Fortran源代码分析、气候数据降尺度与变化影响分析

SWAP作物生长模型安装教程、数据制备、敏感性分析、气候变化影响、R模型敏感性分析与贝叶斯优化、Fortran源代码分析、气候数据降尺度与变化影响分析

查看原文>>>全流程SWAP农业模型数据制备、敏感性分析及气候变化影响实践技术应用 SWAP模型是由荷兰瓦赫宁根大学开发的先进农作物模型，它综合考虑了土壤-水分-大气以及植被间的相互作用；是一种描述作物生长过程的一种机理性作物生长模型。它不但运用Richard方程，使其能够精确的模拟土壤中水分的运动，而且耦合了WOFOST作物模型使作物的生长描述更为科学。 本文让更多的科研人员和农业工作者
阅读更多...
MOLE 2.5 分析分子通道和孔隙

MOLE 2.5 分析分子通道和孔隙

软件介绍 生物大分子通道和孔隙在生物学中发挥着重要作用，例如在分子识别和酶底物特异性方面。 我们介绍了一种名为 MOLE 2.5 的高级软件工具，该工具旨在分析分子通道和孔隙。 与其他可用软件工具的基准测试表明，MOLE 2.5 相比更快、更强大、功能更丰富。作为一项新功能，MOLE 2.5 可以估算已识别通道的物理化学性质。 软件下载 https://pan.quark.cn/s/57
阅读更多...
工厂ERP管理系统实现源码（JAVA）

工厂ERP管理系统实现源码（JAVA）

工厂进销存管理系统是一个集采购管理、仓库管理、生产管理和销售管理于一体的综合解决方案。该系统旨在帮助企业优化流程、提高效率、降低成本，并实时掌握各环节的运营状况。 在采购管理方面，系统能够处理采购订单、供应商管理和采购入库等流程，确保采购过程的透明和高效。仓库管理方面，实现库存的精准管理，包括入库、出库、盘点等操作，确保库存数据的准确性和实时性。 生产管理模块则涵盖了生产计划制定、物料需求计划、
阅读更多...
衡石分析平台使用手册-单机安装及启动

衡石分析平台使用手册-单机安装及启动

单机安装及启动​ 本文讲述如何在单机环境下进行 HENGSHI SENSE 安装的操作过程。 在安装前请确认网络环境，如果是隔离环境，无法连接互联网时，请先按照 离线环境安装依赖的指导进行依赖包的安装，然后按照本文的指导继续操作。如果网络环境可以连接互联网，请直接按照本文的指导进行安装。 准备工作​ 请参考安装环境文档准备安装环境。 配置用户与安装目录。 在操作前请检查您是否有 sud
阅读更多...
线性因子模型 - 独立分量分析（ICA）篇

线性因子模型 - 独立分量分析（ICA）篇

序言 线性因子模型是数据分析与机器学习中的一类重要模型，它们通过引入潜变量（ latent variables \text{latent variables} latent variables）来更好地表征数据。其中，独立分量分析（ ICA \text{ICA} ICA）作为线性因子模型的一种，以其独特的视角和广泛的应用领域而备受关注。 ICA \text{ICA} ICA旨在将观察到的复杂信号
阅读更多...
Spring 源码解读：自定义实现Bean定义的注册与解析

Spring 源码解读：自定义实现Bean定义的注册与解析

引言 在Spring框架中，Bean的注册与解析是整个依赖注入流程的核心步骤。通过Bean定义，Spring容器知道如何创建、配置和管理每个Bean实例。本篇文章将通过实现一个简化版的Bean定义注册与解析机制，帮助你理解Spring框架背后的设计逻辑。我们还将对比Spring中的BeanDefinition和BeanDefinitionRegistry，以全面掌握Bean注册和解析的核心原理。
阅读更多...

Copyright China编程 23002807@qq.com

沪ICP备2023033072号-2