之前看了不少的cartographer的從總體上了解cartographer的文章。但是代碼是怎么做的，代碼怎么寫的。我一點都不清楚。
所以這是一次再出發，我自己從代碼層面去看cartographer。
在從代碼層面上了解cartographer的，有知乎的一個 cartographer源碼解讀系列
在了解讀系列之后，我心里還不是很清楚。所以我自己開始自己的解讀過程的記錄。
我自己了解cartographer的一個隨筆過程。
在我寫ROS程序的時候，我自己就是一個過程思維。每一步都經歷了什么過程。
所以我在這一系列里面，嘗試着解讀這些問題。
首先是數據從哪里進去。
如果有錯誤，希望大家指出。謝謝
官方流程圖：
官方流程圖還是很清晰明了地講解了數據的輸入有哪幾個種類。
在官方流程圖的最左邊，首先說明了輸入的數據雷達的范圍數據，IMU數據以及Odom數據。
下面，我們就來了解清楚這些數據進行輸入時，代碼具體如何執行，以及Google的工程師，是如何設計了這些類。
這些都是我很好奇的地方。
第一部分：Cartographer訂閱的各種話題的流程是什么。
在Cartographer的流程圖里面，分別有三種數據，雷達數據，odom數據以及IMU數據。
這三個數據分別是雷達驅動所發出的，以及底盤驅動所發出來的Odom數據以及IMU數據。
Cartographer分別訂閱了他們所發出來的話題。
那么Cartographer在線算法部分在訂閱這些話題的主要代碼是什么呢？
下面整理的數個步驟，就是我的理解。
我僅僅列出整個過程所需要的函數，其它的會傾向於省略。
第一步：
在ros運行的時候, 入口的函數 main, 這一主函數所在的 文件是 /src/cartographer_ros/cartographer_ros/cartographer_ros/node_main.cc 下:
其中在main中, 主要的運行主要部分的函數:
int main(int argc, char** argv) { ... cartographer_ros::Run(); // 調用了同一個文件中的run函數 ... }
在main函數當中，除了初始化其它對象，如glog和cartographer這些對象之外，就是運行Run函數。
Run函數就像是主要啟動的一個函數。在這，可以看到工程師的一個想法就是，用一個函數來包含啟動需要的一些主要步驟，那這就是Run函數。
Run函數的主要代碼如下：
void Run() { ... //Node在/cartographer_ros/cartographer_ros/cartographer/node.h中定義； // 在該構造函數中訂閱了很多傳感器的topic。收集傳感器數據 // node_options 是為了node配置的一個類 // map_builder 是 cartographer里面的一個類 Node node(node_options, std::move(map_builder), &tf_buffer); .. }
在Run函數當中，主要與訂閱相關相關是實例化Node類。
ROS程序當中，主要是以節點的形式進行存在。所以工程師們想用一個類來代表這個ROS節點，進行處理ROS節點事務。
node_options，是另外一個類，是用來表示節點的選項的一個類。
map_builder，是另外一個類，是同來表示地圖構造的，雖然看過他的函數，但仍然不知道他具體的東西。
tf_buffer，是tf樹的一個緩沖區。
第二步：
Node類節點, 在 src/cartographer_ros/cartographer_ros/cartographer_ros/node.h 里面進行定義的.
根據ROS的用法習慣，如果這個類里面有訂閱的相關代碼，一定會擁有 ros::Subscriber 這個相關信息。
然后順藤摸瓜，可以得出以下跟訂閱相關的變量和函數：
namespace cartographer_ros { class Node { private: struct Subscriber { // Subscriber 構成的新的訂閱pair似的數據結構 ::ros::Subscriber subscriber; // ros訂閱用法的類 std::string topic; // 主題的名字 } .... // 因為調用 AddTrajectory bool HandleStartTrajectory( cartographer_ros_msgs::StartTrajectory::Request& request, cartographer_ros_msgs::StartTrajectory::Response& response); // 從名字看，就知道和訂閱有關。 void LaunchSubscribers(const TrajectoryOptions& options, const cartographer_ros_msgs::SensorTopics& topics, int trajectory_id); // 會調用 LaunchSubscribers int Node::AddTrajectory(const TrajectoryOptions& options, const cartographer_ros_msgs::SensorTopics& topics) .... // 用服務的形式管理所有第一個 trajectory std::vector<::ros::ServiceServer> service_servers_; // 再根據 Subscriber， 會搜索到這個變量。用一個unordered_map來進行組織 // int 類型，我看了后面知道這是一個 trajectory_id // 后面是一個vector組成Subscriber std::unordered_map<int, std::vector<Subscriber>> subscribers_; ... } }
代碼中列出了相關的代碼和注釋：
他們是從 ros::Subscriber 開始，一路順延下去進行挖掘。能夠得出這些代碼都是訂閱相關的。
在后面，會講解每個函數都做了什么，會解釋每一個函數當中與訂閱相關的代碼。
第三步：
剛才我們從定義上面知道了這些相關的變量是哪些，這是很有用的，因為，初始化函數里面的東西很多，找到相關的代碼就OK了。
但真正的流程是從Node的初始化函數開始的。
主要與訂閱數據相關的代碼如下：
Node::Node( const NodeOptions& node_options, // 選項 std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder, // 地圖匹配 tf2_ros::Buffer* const tf_buffer) // tf 樹 : node_options_(node_options), map_builder_bridge_(node_options_, std::move(map_builder), tf_buffer) { ... // node_constants.h 里面定義了下面的常量 // constexpr char kStartTrajectoryServiceName[] = "start_trajectory"; // 將 HandleStartTrajectory 作為服務的回調函數, 那么問題來了, 哪里調用了這個服務? // 這個我還不確切知道. 但以我自己的感覺, 是在Mapbuilder那邊,開始添加trajectory的時候. service_servers_.push_back(node_handle_.advertiseService( kStartTrajectoryServiceName, &Node::HandleStartTrajectory, this)); ... }
從代碼可以看出，在初始化函數里面
vector service_servers_ 進行了服務的管理，它壓入一個新增的名為kStartTrajectoryServiceName的服務。
這個服務的回調函數是HandleStartTrajectory。
所以這個服務被調用的時候，就會去啟動handleStartTrajectory這個函數。
大家有沒一個問題？為什么用一個服務呢？啟動訂閱不是一次就可以啟動，然后數據源源不斷了嗎？為什么需要一個服務，可以讓其它部件多次調用呢？
我個人的猜測，這和Trajectory是有關系的。每次開始一個Trajectory，可能都會調用這個。
至於是不是這樣，我不確切知道的哦。只是猜測。
言歸正傳，知道哪里調用了 HandleStartTrajectory 被調用了。
那么我們還得了解下 HandleStartTrajectory 做了什么。
第四步：
HandleStartTrajectory 的主要代碼如下，因為它是一個ROS規定的回調函數，它有它自己的參數格式。
request 表達的就是調用這個服務的請求的數據結構。
response 就是自定義的回復的格式。
bool Node::HandleStartTrajectory( ::cartographer_ros_msgs::StartTrajectory::Request& request, ::cartographer_ros_msgs::StartTrajectory::Response& response) { carto::common::MutexLocker lock(&mutex_); TrajectoryOptions options; // 判斷從ROS消息里面讀取出options, 並且,這個options 是否是有效的 if (!FromRosMessage(request.options, &options) || !ValidateTrajectoryOptions(options)) { // 無效的option const std::string error = "Invalid trajectory options."; LOG(ERROR) << error; response.status.code = cartographer_ros_msgs::StatusCode::INVALID_ARGUMENT; response.status.message = error; } else if (!ValidateTopicNames(request.topics, options)) { // 判斷是否無效的topic name // 無效的topic name const std::string error = "Invalid topics."; LOG(ERROR) << error; response.status.code = cartographer_ros_msgs::StatusCode::INVALID_ARGUMENT; response.status.message = error; } else { // 如果都成功, 所以就會添加這個主題 response.trajectory_id = AddTrajectory(options, request.topics); response.status.code = cartographer_ros_msgs::StatusCode::OK; response.status.message = "Success."; } return true; }
代碼中明確表示了它的邏輯就是，判斷 當前情況為正常情況之后,
就使用了AddTrajectory 來添加第一個(我不確定這是第一個阿,只是看名字才說是第一個) trajector.
在調用 AddTrajectory 的時候,可以看到, 是通過 request.topics 來定義需要調用的哪些話題.
我們又進了一步，那就是 AddTrajectory
第五步
AddTrajectory 里面的代碼如下：
int Node::AddTrajectory(const TrajectoryOptions& options, const cartographer_ros_msgs::SensorTopics& topics) { // 根據options, 得出一個SensorID的set const std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId> expected_sensor_ids = ComputeExpectedSensorIds(options, topics); // 根據sensor id 和 option, 得出 trajectory id. const int trajectory_id = map_builder_bridge_.AddTrajectory(expected_sensor_ids, options); // Extrapolator 是一個推算位置的類, 因為我看過后面, 跟我想要解決的數據流入沒關系 AddExtrapolator(trajectory_id, options); // sensor_samplers_ 同樣也是一個抽樣的類, 跟數據流入沒關系 AddSensorSamplers(trajectory_id, options); // LaunchSubscribers 訂閱所有話題的主要函數 LaunchSubscribers(options, topics, trajectory_id); is_active_trajectory_[trajectory_id] = true; for (const auto& sensor_id : expected_sensor_ids) { subscribed_topics_.insert(sensor_id.id); } return trajectory_id; }
代碼里面有相關的注釋，列出這段代碼，主要是為了讓大家看到 LaunchSubscribers 的調用，這是與訂閱話題相關的。
其它的語句的話，我清楚他們的作用，但我僅有的知識是
Extrapolator 是一個推算位置的類, 在概率機器人里面，有提到過兩種方法來推算機器人的位置，采樣法和根據當前的速度推算的方法。
明顯這個Extrapolator是根據速度來推算下一個位置的類
sensor_samplers_ ： 我也不清楚這個，名字顯示是傳感器的抽樣。但是我對LocalSLAM下面的東西還不算太清楚。所以不知道它的作用。具體是哪里。
第六步:
LaunchSubscribers 的 代碼如下
// 在這兒，訂閱了一堆東西。 void Node::LaunchSubscribers(const TrajectoryOptions& options, const cartographer_ros_msgs::SensorTopics& topics, const int trajectory_id) { // ComputeRepeatedTopicNames 作用: 確保傳進去的topic,出來之后是唯一的. for (const std::string& topic : ComputeRepeatedTopicNames( topics.laser_scan_topic, options.num_laser_scans)) { // SubscribeWithHandler 的作用: // 使用'node_handle'為'trajectory_id'訂閱'topic'，並在'node'上調用'handler'來處理消息。 返回訂閱者。 subscribers_[trajectory_id].push_back( {SubscribeWithHandler<sensor_msgs::LaserScan>( &Node::HandleLaserScanMessage, trajectory_id, topic, &node_handle_, this), topic}); } for (const std::string& topic : ComputeRepeatedTopicNames(topics.multi_echo_laser_scan_topic, options.num_multi_echo_laser_scans)) { subscribers_[trajectory_id].push_back( {SubscribeWithHandler<sensor_msgs::MultiEchoLaserScan>( &Node::HandleMultiEchoLaserScanMessage, trajectory_id, topic, &node_handle_, this), topic}); } for (const std::string& topic : ComputeRepeatedTopicNames( topics.point_cloud2_topic, options.num_point_clouds)) { subscribers_[trajectory_id].push_back( {SubscribeWithHandler<sensor_msgs::PointCloud2>( &Node::HandlePointCloud2Message, trajectory_id, topic, &node_handle_, this), topic}); } // For 2D SLAM, subscribe to the IMU if we expect it. For 3D SLAM, the IMU is // required. if (node_options_.map_builder_options.use_trajectory_builder_3d() || (node_options_.map_builder_options.use_trajectory_builder_2d() && options.trajectory_builder_options.trajectory_builder_2d_options() .use_imu_data())) { std::string topic = topics.imu_topic; subscribers_[trajectory_id].push_back( {SubscribeWithHandler<sensor_msgs::Imu>(&Node::HandleImuMessage, trajectory_id, topic, &node_handle_, this), topic}); } if (options.use_odometry) { std::string topic = topics.odometry_topic; subscribers_[trajectory_id].push_back( {SubscribeWithHandler<nav_msgs::Odometry>(&Node::HandleOdometryMessage, trajectory_id, topic, &node_handle_, this), topic}); } if (options.use_nav_sat) { std::string topic = topics.nav_sat_fix_topic; subscribers_[trajectory_id].push_back( {SubscribeWithHandler<sensor_msgs::NavSatFix>( &Node::HandleNavSatFixMessage, trajectory_id, topic, &node_handle_, this), topic}); } if (options.use_landmarks) { std::string topic = topics.landmark_topic; subscribers_[trajectory_id].push_back( {SubscribeWithHandler<cartographer_ros_msgs::LandmarkList>( &Node::HandleLandmarkMessage, trajectory_id, topic, &node_handle_, this), topic}); } }
這些代碼的目的就很明確了，根據配置所配置的名稱，來進行訂閱話題。
在這些代碼里面，工程師們的c++用法以及不清楚具體的目的，因為可能不確定里面到底輸出的是什么東西，以及輸出東西的含義。
我進行我的解讀是這樣的。它有一個相關的函數是這樣的：
std::vector<std::string> ComputeRepeatedTopicNames(const std::string& topic, const int num_topics) { CHECK_GE(num_topics, 0); if (num_topics == 1) { return {topic}; } std::vector<std::string> topics; topics.reserve(num_topics); for (int i = 0; i < num_topics; ++i) { topics.emplace_back(topic + "_" + std::to_string(i + 1)); } return topics; }
從函數里面可以看到，如果輸入的名字是一樣的，但是有多個雷達，會加上 _<num> 這樣的格式。
所以，這兒應該只是生成話題。然后進行了訂閱。
然而這寫topics是如何來的呢？
有一定的相關的默認配置，應該也是可以通過配置文件來進行配置。
默認的選項是這樣的
cartographer_ros_msgs::SensorTopics DefaultSensorTopics() { cartographer_ros_msgs::SensorTopics topics; topics.laser_scan_topic = kLaserScanTopic; topics.multi_echo_laser_scan_topic = kMultiEchoLaserScanTopic; topics.point_cloud2_topic = kPointCloud2Topic; topics.imu_topic = kImuTopic; topics.odometry_topic = kOdometryTopic; topics.nav_sat_fix_topic = kNavSatFixTopic; topics.landmark_topic = kLandmarkTopic; return topics; }
kLaserScanTopic 這些名字，定義在node_constants.h 里面。
所以,其實,我們開始提出的問題: 數據在哪進行訂閱， 代碼中如何體現
看到這里就解決了.
第七步：
第七步起始與主線關系不大，訂閱的在上面已經講清楚，這兒是我在上面有個函數每看明白，所以列出來了。
函數是 SubscribeWithHandle 這個函數：
template <typename MessageType> ::ros::Subscriber SubscribeWithHandler( void (Node::*handler)(int, const std::string&, const typename MessageType::ConstPtr&), const int trajectory_id, const std::string& topic, ::ros::NodeHandle* const node_handle, Node* const node) { return node_handle->subscribe<MessageType>( topic, kInfiniteSubscriberQueueSize, boost::function<void(const typename MessageType::ConstPtr&)>( [node, handler, trajectory_id, topic](const typename MessageType::ConstPtr& msg) { (node->*handler)(trajectory_id, topic, msg); })); }
里面的
boost::function<void(const typename MessageType::ConstPtr&)>( [node, handler, trajectory_id, topic](const typename MessageType::ConstPtr& msg) { (node->*handler)(trajectory_id, topic, msg); })
函數，之前一直不明白。我知道boost::Function對象。但是沒見過例子有 [] 這樣的用法。
首先很明確的是，這個一定是返回一個函數對象。
對象的參數信息也可以從nodehandle->subscribe里面得出：
Subscriber ros::NodeHandle::subscribe( const std::string & topic, uint32_t queue_size, const boost::function< void(const boost::shared_ptr< M const > &)> & callback, const VoidConstPtr & tracked_object = VoidConstPtr(), const TransportHints & transport_hints = TransportHints() )
不懂的時候，我喜歡看看boost文檔，然后在boost::function里面發現這樣一個定義
boost::function1<int, int> f2(f); // f 是一個函數對象
這讓我想起來， c++ lambda的用法是[]開頭的。
所以上面的函數對象的意義就明了了。
這是一個用lambda初始boost::function的用法。
OK.想明白,真簡單.
沒想明白,這都啥跟啥
其實在這里:
1. 雖然看過后面的一些文章, 但我不確切明白 Trajectory 是什么.
這些是另外的問題, 其實我現在是想知道, 數據開始流入了, LocalSLAM如何工作.
看一個理論和大項目的方法，大多都推薦，先看個大概的理論框架是什么跟什么。
我是看了一些從頂層介紹的文章，然后還是雲里霧里。
所以，我還是習慣從底層慢慢看。因為，本身可能數學基礎就不太好，沒有那么好的抽象思維。
根據我自己編程的習慣就是，數據怎么來，怎么處理，得出什么結果這樣開始再看一次到底怎么做。
沿着這中思路，下一章寫什么？
等我寫好了下一章的草稿。。。
我再來補這里。
