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