転載元：https：//blog.csdn.net/SIR_wkp/article/details/87861658
MAVROS ソースコード分析
SIR_wkp 2019-02-21 17 ： 52：532778コレクション34
分類列：PX4コード分析
著作権表示：この記事はブロガーの元の記事であり、CC 4.0 BY-SAの著作権表示に従います。転載するには、元のソースへのリンクとこの文を添付してください。
この記事へのリンク：https：//blog.csdn.net/SIR_wkp/article/details/87861658
著作権
1つは、MAVROSをインストールする
オンボードコンピューターを使用してOFFBOARDでPX4フライトコントローラーを制御するには、オンボードコンピューターにROSのMAVROSパッケージをインストールする必要があります。インストール方法は、PX4開発者のWebサイトを参照できます。バイナリファイルのインストールとソースコードのインストールには2つの方法があり、どちらかを選択するだけです。
ソースコードをインストールすると、MAVROSのすべてのソースコードを取得できます。
以下は、MAVROSの公式wikiURLです。
注意：
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.sh2. mavrosの実行時にシリアルポートを開くことができない場合は、ターミナルで実行できます。
sudo usermod -a -G dialout $USER
sudo apt-get remove modemmanager -yアカウントからログアウト（ログアウト）して、再度ログインします。
2、MAVROS ソースコード分析
ソースコードを読み、MAVROSの実装プロセスを 簡単に分析します。便宜上、<workspace>を使用してROSワークスペースのフォルダーパスを参照します。
1.入力機能を決定します
ある2つのフォルダがmavlinkとmavros <ワークスペース> / srcディレクトリに含まれ、関連するすべてのソースコードにMAVROSを。
MAVROSを起動する通常の方法は、たとえば、roslaunchコマンドを使用してターミナルで実行することです。
roslaunch mavros px4.launchpx4.launchファイルが開始するnode.launchファイルを参照する「mavros _node」ノードに「mavros」パッケージ名「は、mavrosそれはまた、関連する『fcu_url』など、起動に必要なパラメータを定義し;」フライトコントロールの通信リンクなどへ。さらに多くのパラメータがpx4_config.yamlファイルで定義されています。px4_pluginlists.yamlファイルはロードされたpluginlibのリストであり、ホワイトリストとブラックリストにリストされています。ブラックリストのプラグインはそうではありません。ロードされます。
チェック でCMakeLists.txtファイル<ワークスペース> / srcに/ mavros / mavrosディレクトリを、あなたが見ることができる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ファイル。内容は非常に単純で、次のように主な機能にすぎません。
#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 _node」ノード「/名前のmavrosを」、およびパラメータが.launchファイルに定義されています。最後の2つのパラメーター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）すべてのプラグインをロードします
// 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_blacklistとplugin_whitelistの2つのセットと比較され、対応するクラスのインスタンスが作成され、初期化関数が実行されます。
（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);
}
};主に2つの関数、mavlink_pub_cbとplugin_route_cb。mavlink_pub_cb関数は、mavlinkメッセージをmavros _msgs :: Mavlinkタイプのrosメッセージに直接変換 してトピック「/ mavlink / from」に公開します。plugin_route_cb関数は、メッセージ処理のmsgidに基づいて、すべてのプラグインからメッセージ処理関数を検索します。解析された mavros_msgsメッセージ。
3.Mavlinkデータ受信
例として、シリアルリンクを取る場合。fcu_link = MAVConnInterface :: open_url（fcu_url、SYSTEM_ID、COMPONENT_ID）を実行するステップと、でMavRosのコンストラクタ、ステートメントがfcu_urlストリングにおけるデバイスタイプおよびデバイスパラメータが解析されることである、と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関数は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は1バイトのデータです。
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デバイスはどちらも、シリアルデバイスと同様の方法を採用しています。最終的には、mavlinkメッセージを処理するためにmessage_received_cb関数が呼び出されます。
4.プラグインを介してmavlinkメッセージをrosメッセージとして公開します
（1）プラグインをロードします
で MavRos次のように:: add_plugin機能、プラグインのインスタンスは、プラグイン名の文字列によって確立されています。
auto plugin = plugin_loader.createInstance(pl_name);
ROS_INFO_STREAM("Plugin " << pl_name << " loaded");次に、get_subscriptions関数を使用して、プラグインでメッセージを公開できる関数の情報を取得し、msgidとその他の情報を取得し、msgidとinfoを対応するアイテムに結合して、plugin_subscriptionsコレクションに追加します。次に初期化関数を実行します。 、次にプラグインをの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メッセージをプラグインに渡して、rosメッセージとして処理します
上記3から、完全なmavlinkメッセージを受信すると、message_received_cb関数が呼び出されることがわかります。まず、トピック「/ mavlink / from」のメッセージがmavlink_pub_cb関数を介して公開され、対応するplugin_route_cbを介したプラグイン。実装は次のとおりです
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に対応するプラグインがロードされると、次に示すように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)
};
}これらのハンドル関数には統一されたパラメーター形式があります。2番目のパラメーターには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」を含む2つのメッセージを公開します。
同様に、適格なすべてのmavlinkメッセージは、公開のためにrosメッセージにパッケージ化されます。
5.プラグインを介してフライトコントローラーにmavlinkメッセージを公開します
たとえば、ユーザープログラムは、トピック「/ mavros / setpoint_position / local」を含むメッセージを公開して、ローカル座標系での航空機の位置を制御します。SetpointPositionPluginクラスは、メッセージを処理するためにsetpoint_position.cppファイルで定義されます。
（1）ニュースを購読する
初期化関数で、トピック「/ 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);特定の送信実現プロセスについては、特定のデバイスの送信機能を参照してください。
6.要約：
ソースコードの分析により、MAVROSがフライトコントローラーと通信するとき、最初にリンクデバイスの独立したデータ受信スレッドを使用してフライトコントローラーからデータを取得することがわかります。データを受信すると、msgidを比較します。プラグインとmavlinkメッセージの対応するメッセージ処理機能を選択します。この機能は、mavlinkメッセージをrosメッセージに変換して公開します。ユーザーが発行した制御コマンドは、プラグインがrosメッセージを介して受信し、次のように変換します。 mavlinkメッセージであり、デバイスのデータ送信機能を介してフライトコントローラーに送信されます。
ソースコードのコメント内容は明確です。OFFBOARD関数を開発する際に、rosメッセージ処理に対応するコールバック関数を読み取ることで、明確になり、事故を防ぐことができます。