C++?Cartographer的入口node main詳細講解
啃一下谷歌優(yōu)秀的激光SLAM開源框架-Cartographer. 這個框架算法簡單,但是程序部分太多需要學習的地方了.不論是整體框架的結構,還是數(shù)據(jù)的使用,都是非常優(yōu)美的.不愧是大公司啊.接下來記錄一下每天學習的內容和心得,督促自己堅持下去!
node_main.cc是整個Cartographer程序的入口,用來調用整個Cartographer進程。以最基礎的單線雷達和輪速計為例。
整體的代碼開始是在Run函數(shù)中實現(xiàn)的。
Run函數(shù)
void Run() {
constexpr double kTfBufferCacheTimeInSeconds = 10.;
tf2_ros::Buffer tf_buffer{::ros::Duration(kTfBufferCacheTimeInSeconds)};
// 開啟監(jiān)聽tf的獨立線程
tf2_ros::TransformListener tf(tf_buffer);
NodeOptions node_options;
TrajectoryOptions trajectory_options;
// c++11: std::tie()函數(shù)可以將變量連接到一個給定的tuple上,生成一個元素類型全是引用的tuple
// 讀取Lua文件內容,把Lua文件內容給到node_options和trajectory_options
std::tie(node_options, trajectory_options) =
LoadOptions(FLAGS_configuration_directory, FLAGS_configuration_basename);
// MapBuilder類是完整的SLAM算法類
// 包含前端(TrajectoryBuilders,scan to submap) 與 后端(用于查找回環(huán)的PoseGraph)
auto map_builder =
cartographer::mapping::CreateMapBuilder(node_options.map_builder_options);//在map_builder.cc中實現(xiàn),工廠函數(shù)
//在這里,實例化一個MapBuilder, 而MapBuilder是MapBuilderInterface的子類 //MapBuilder的AddTrajectoryBuilder實例化了CollatedTrajectoryBuilder
// c++11: std::move 是將對象的狀態(tài)或者所有權從一個對象轉移到另一個對象,
// 只是轉移, 沒有內存的搬遷或者內存拷貝所以可以提高利用效率,改善性能..
// 右值引用是用來支持轉移語義的.轉移語義可以將資源 ( 堆, 系統(tǒng)對象等 ) 從一個對象轉移到另一個對象,
// 這樣能夠減少不必要的臨時對象的創(chuàng)建、拷貝以及銷毀, 能夠大幅度提高 C++ 應用程序的性能.
// 臨時對象的維護 ( 創(chuàng)建和銷毀 ) 對性能有嚴重影響.
// Node類的初始化, 開啟訂閱,發(fā)布topic和service,將ROS的topic傳入SLAM, 也就是MapBuilder
Node node(node_options, std::move(map_builder), &tf_buffer,
FLAGS_collect_metrics);
// 如果加載了pbstream文件, 就執(zhí)行這個函數(shù),為定位
if (!FLAGS_load_state_filename.empty()) {
node.LoadState(FLAGS_load_state_filename, FLAGS_load_frozen_state);
}
// 使用默認topic 開始軌跡
if (FLAGS_start_trajectory_with_default_topics) {
node.StartTrajectoryWithDefaultTopics(trajectory_options);
}
::ros::spin();
// 結束所有處于活動狀態(tài)的軌跡
node.FinishAllTrajectories();
// 當所有的軌跡結束時, 再執(zhí)行一次全局優(yōu)化
node.RunFinalOptimization();
// 如果save_state_filename非空, 就保存pbstream文件
if (!FLAGS_save_state_filename.empty()) {
node.SerializeState(FLAGS_save_state_filename,
true /* include_unfinished_submaps */);
}
}
} // namespace
} // namespace cartographer_rosRun函數(shù)主要做了一下幾件事:
- 讀取Lua配置文件中的內容,確定節(jié)點構造的方式和軌跡構造的方式與參數(shù)。
- 實例化map_builder,map_builder是完整的SLAM算法類,包含了前端和后端。具體時間方式是通過工廠模式。
- 初始化Node,通過初始化Node,開啟訂閱,發(fā)布topic與service,還將topic帶的傳感器數(shù)據(jù)傳入MapBuilder。
- 判斷是否為定位還是建圖,并開啟軌跡
- 死循環(huán),不停地接受topic并運行Cartographer
- 結束時停止所用傳感器數(shù)據(jù)的訂閱,并且執(zhí)行一次全局優(yōu)化,保存pbstream地圖文件
讀取配置參數(shù)
其中std::tie很有意思,可以實現(xiàn)多個不同類型的返回值. 很多時候我們想通過一個函數(shù)丟出去多個結果,但一個函數(shù)只能有一個返回值,于是我們可以用std::make_tuple把多個返回值打包成std::tuple類型的數(shù)據(jù),這時候返回值只是tuple類型了,所以沒有違反只能返回一個返回值的規(guī)定.這點很類似Python中的pickle和tuple,啥都可以裝在一起丟出去. 實現(xiàn)文件在node_options.cc
/**
* @brief 加載lua配置文件中的參數(shù)
*
* @param[in] configuration_directory 配置文件所在目錄
* @param[in] configuration_basename 配置文件的名字
* @return std::tuple<NodeOptions, TrajectoryOptions> 返回節(jié)點的配置與軌跡的配置
*/
std::tuple<NodeOptions, TrajectoryOptions> LoadOptions(
const std::string& configuration_directory,
const std::string& configuration_basename) {
// 獲取配置文件所在的目錄
auto file_resolver =
absl::make_unique<cartographer::common::ConfigurationFileResolver>(
std::vector<std::string>{configuration_directory});
// 讀取配置文件內容到code中
const std::string code =
file_resolver->GetFileContentOrDie(configuration_basename);
// 根據(jù)給定的字符串, 生成一個lua字典
cartographer::common::LuaParameterDictionary lua_parameter_dictionary(
code, std::move(file_resolver));
// 創(chuàng)建元組tuple,元組定義了一個有固定數(shù)目元素的容器, 其中的每個元素類型都可以不相同
// 將配置文件的內容填充進NodeOptions與TrajectoryOptions, 并返回
return std::make_tuple(CreateNodeOptions(&lua_parameter_dictionary),
CreateTrajectoryOptions(&lua_parameter_dictionary));
}構建地圖構建器
Cartographer_ros和Cartographer是兩個部分,一個是數(shù)據(jù)處理與分配,一個才是真正的Cartographer算法代碼的部分,代碼上把ros和算法庫分得很開,讓我們移植和開發(fā)很容易.那么如何讓ros數(shù)據(jù)和Cartographer算法建立聯(lián)系呢?第一步就是地圖構建器.
地圖構建器的大致作用是調用Cartographer的算法.
地圖構建器通過配置文件中node_options中map_builder_options部分去初始化一個地圖.這個地圖構建器的作用以后再說.先來看看他是怎么實現(xiàn)的.
由node_main.cc調用map_builder中的CreateMapBuilder函數(shù),這個函數(shù)只有一個參數(shù),就是上一行從lua中讀取的配置文件內容. 進入map_builder.cc中:
// 工廠函數(shù),生成接口API
std::unique_ptr<MapBuilderInterface> CreateMapBuilder(
const proto::MapBuilderOptions& options) {
return absl::make_unique<MapBuilder>(options);
}發(fā)現(xiàn)這個就是一個接口函數(shù). 但這個函數(shù)也有用到一些cpp的技巧,值得學習:
返回值是一個unique_ptr的MapBuilder類型的類,而返回類型卻定于為MapBuilder的父類MapBuilderInterface類,這在cpp中是允許的,而且這樣做更能讓返回值類型更加有包容性,實現(xiàn)工廠模式.
MapBuilder這個類是SLAM算法的入口類十分重要,用來初始化pose_graph,創(chuàng)建軌跡等.會在另一篇中詳細介紹.
Node類的初始化
Node類的作用主要是傳感器數(shù)據(jù)的獲取和處理,讓數(shù)據(jù)與MapBuilder構建聯(lián)系,從而使獲取的raw sensor data能夠灌入Cartographer算法庫,實現(xiàn)定位建圖等功能.
在node_main.cc中初始化方式如下:
// Node類的初始化, 開啟訂閱,發(fā)布topic和service,將ROS的topic傳入SLAM, 也就是MapBuilder
Node node(node_options, std::move(map_builder), &tf_buffer,
FLAGS_collect_metrics);這一行代碼也有值得學習的地方,就是std::move這個函數(shù),他通過把某個實例化的類變?yōu)橛抑狄萌缓笾苯愚D移給某個對象,從而實現(xiàn)高效的"轉移".
舉個簡單的不太恰當?shù)睦?你想要我的西瓜,有兩種方式,一個是我不遠千里坐車給你,還有一種是給西瓜貼上你的名字,別人問我就說我說了不算,問你去. std::move就是后者(如有錯請指出哈).所以這樣可以直接從一個對象轉移到另一對象(貼名字),取消了不必要的臨時對象的創(chuàng)建拷貝與銷毀(運輸西瓜需要位子還要搬上搬下). 對于占用很大的類的轉移就很節(jié)約開銷(一億噸西瓜咋運啊).大致就這個意思.
Node類的內容在node.cc中,主要作用是實現(xiàn)傳感器數(shù)據(jù)的訂閱發(fā)布以及初始處理, 以及傳遞給mapbuilder.具體內容在后面會詳細介紹.
開始軌跡與結束軌跡
在上面實例化了Node類之后,我們就可以調用node中的方法去建圖. 建圖就不用加載地圖了,畢竟是建圖,所以直接調用node開始軌跡,然后在進入ros中的死循環(huán),不停地接受新的數(shù)據(jù),處理并運算,輸出結果, 直到按下ctrl+c去終止程序,跳出死循環(huán),執(zhí)行結束輸入數(shù)據(jù)和進行最終優(yōu)化.
其實看程序就可以知道,Cartographer的建圖和定位是一樣的,只是建圖的時候不加載地圖并且在結束的時候保存地圖,定位的時候加載地圖,可以不保存地圖,也可不進行最終優(yōu)化.其實我測試的不進行最終優(yōu)化也是可以的,畢竟定位是實時的,就算最終優(yōu)化使之前的定位結果有變化,機器人也回不去了.所以我認為是可以去掉的.
// 如果加載了pbstream文件, 就執(zhí)行這個函數(shù),為定位
if (!FLAGS_load_state_filename.empty()) {
node.LoadState(FLAGS_load_state_filename, FLAGS_load_frozen_state);
}
// 使用默認topic 開始軌跡
if (FLAGS_start_trajectory_with_default_topics) {
node.StartTrajectoryWithDefaultTopics(trajectory_options);
}
::ros::spin();
// 結束所有處于活動狀態(tài)的軌跡
node.FinishAllTrajectories();
// 當所有的軌跡結束時, 再執(zhí)行一次全局優(yōu)化
node.RunFinalOptimization();
// 如果save_state_filename非空, 就保存pbstream文件
if (!FLAGS_save_state_filename.empty()) {
node.SerializeState(FLAGS_save_state_filename,
true /* include_unfinished_submaps */);
}LoadState作用是加載地圖文件.這個地圖不同于可以可視化的地圖,這個地圖里面包含了位姿圖pose_graph,傳感器數(shù)據(jù)和landmark_pose等其他信息,不單單是一個地形圖一樣的地圖.調用的最終函數(shù)是Cartographer算法部分的map_builder.cc中的同名函數(shù),調用流程一環(huán)套一環(huán)(Cartographer整體框架就是這樣,復雜但都是必要的).調用的流程如下:

只有最后一層的map_builder.cc才是Cartographer算法部分的內容,才是真正實現(xiàn)加載地圖的功能. 這部分程序又臭又長,大家可以自己看看,實現(xiàn)功能加載posegraph和舊地圖的傳感器數(shù)據(jù)與landmark.
StartTrajectoryWithDefaultTopics實際上是調用了node.cc的AddTrajectory,去讓map_builder創(chuàng)建一個軌跡,并且新增位姿估計器,傳感器數(shù)據(jù)采樣器,訂閱topic以及調用回調函數(shù)的功能. 這個函數(shù)建立了數(shù)據(jù)與算法的統(tǒng)一. 詳細會在Node中解析.
FinishAllTrajectories調用node.cc中的FinishTrajectoryUnderLock去結束傳感器訂閱,然后調用map_builder的FinishTrajectory()進行軌跡的結束
node::RunFinalOptimization調用map_builder的pose_graph的RunFinalOptimization實現(xiàn)結束建圖后所有位姿圖的最終優(yōu)化.
由此可見, Node類通過類方法,實現(xiàn)了傳感器數(shù)據(jù)的處理與使用.具體的方式是用了sensor_bridge和map_builder_bridge,把傳感器數(shù)據(jù)轉換并且給了Cartographer的算法部分, 實現(xiàn)了建圖與定位.
到此這篇關于C++ Cartographer的入口node_main詳細講解的文章就介紹到這了,更多相關C++ node_main內容請搜索腳本之家以前的文章或繼續(xù)瀏覽下面的相關文章希望大家以后多多支持腳本之家!

