之前看了不少的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如何工作.
看一个理论和大项目的方法，大多都推荐，先看个大概的理论框架是什么跟什么。
我是看了一些从顶层介绍的文章，然后还是云里雾里。
所以，我还是习惯从底层慢慢看。因为，本身可能数学基础就不太好，没有那么好的抽象思维。
根据我自己编程的习惯就是，数据怎么来，怎么处理，得出什么结果这样开始再看一次到底怎么做。
沿着这中思路，下一章写什么？
等我写好了下一章的草稿。。。
我再来补这里。
来源：oschina
链接：https://my.oschina.net/u/4293959/blog/3409972