源码阅读:使用Cartogrpaher进行毫米波雷达栅格SLAM(二)Nodecc

cartographer_ros是一个Package, 包含多个node,每一个node也都有自己的入口main函数。但cartographer_node是其中最重要的一个node。所以,我们从这个node的入口函数开始。该文件路径为:/src/cartographer_ros/cartographer_ros/cartographer_ros下


2.node_main.cc中的run函数


这里注意:Node在/cartographer_ros/cartographer_ros/cartographer/node.h中定义;在该构造函数中订阅了很多传感器的topic。收集传感器数据。

该段代码前面部分都是在定义一些变量类型,然后调用LoadOptions函数读取目录下的配置文件,将一些参数赋给node_options, trajectory_options

接下来就是比较重要的一句


打开Node类,该类在/src/cartographer_ros/cartographer_ros/cartographer/node.h中定义,由.../node.cc实现:


对我们比较有用的信息就是这个node发布了如下Topic:

在发布Service时,我们为每个Service绑定了一个函数句柄。从名字我们也能才能每个函数是用来做什么的。其中比较重要的是:


而AddTrgkector是


最重要的 里面的LaunchSubscribers这个函数:


这里代码虽长,但结构差不多,都是在订阅传感器发布的消息。可以看到有Laser, MultiEchoLaser, PointCloud2, IMU, Odometry, NavSatFixMessage, Landmark等,其中后三项是用if来判断该传感器使用情况。所以不同的应用场景需要不同地修改配置文件

前三项是根据配置文件里点云输入的类型(scan 多回波 pointCloud

后面是是否有IMU ODO 等


回到Node的构造函数。可以看到接下来是为几个Topic设置了定时器,以及定时器函数。猜测这几个定时器函数里就是定时往Topic上发布消息。


以第一个Topic的定时器函数Node::PublishSubmapList为例:



对cartographer_node做一个总结

cartographer启动以后,做了如下的工作:

1. 注册并发布了5个Topic, 并为5个Topic分别设置了定时器函数,在定时器函数中定期向Topic上广播数据:

|===1) Topic 1: kSubmapListTopic: 广播构建出来的submap的list

|-----------发布数据的函数:Node::PublishSubmapList

|-----------调用函数:map_builder_bridge_.GetSubmapList();

|===2) Topic 2: kTrajectoryNodeListTopic:发布trajectory

|-----------发布数据的函数:Node::PublishTrajectoryNodeList Node::PublishTrajectoryStates:

|-----------调用函数:map_builder_bridge_.GetTrajectoryNodeList();

|===3) Topic3: kLandmarkPoseListTopic

|-----------发布数据的函数:Node::PublishLandmarkPosesList

|-----------调用的函数:map_builder_bridge_.GetLandmarkPoseList();

|===4) Topic4: kConstraintListTopic

|-----------发布数据的函数:Node::PublishConstraintList

|-----------调用的函数:map_builder_bridge_.GetConstraintList();

|===5)Topic5: kScanMatchedPointCloudTopic

|-----------发布数据的函数:Node::PublishTrajectoryStates

|-----------调用的函数:map_builder_bridge_.GetTrajectoryStates();

|-----------这个函数名与Topic名对应不上,但是查看Node::PublishTrajectoryStates不难发现,函数中有一句:

scan_matched_point_cloud_publisher_.publish(ToPointCloud2Message(

            carto::common::ToUniversal(trajectory_state.local_slam_data->time),

            node_options_.map_frame,

            carto::sensor::TransformTimedPointCloud(

                point_cloud, trajectory_state.local_to_map.cast<float>())));

2. 发布了4个Service,并为4个Service分别设置了句柄函数,而句柄函数也是通过调用map_builder_bridge_的成员函数来处理的。

|===1)Service 1: kSubmapQueryServiceName

|-----------句柄函数:Node::HandleSubmapQuery

|-----------实际处理的函数:map_builder_bridge_.HandleSubmapQuery

|===2) Service 2: kStartTrajectoryServiceName

|-----------句柄函数:Node::HandleStartTrajectory

|-----------实际处理的函数:map_builder_bridge_.AddTrajectory等;

|-----------这里需要额外注意的是Node::LaunchSubscribers这个函数。这个函数负责处理各个传感器函数。仔细读其中的每个处理函数,比如处理IMU的Node::HandleImuMessage函数,发现其实际调用的是map_builder_bridge_中的一个成员类sensor_bridge_ptr的函数来处理:sensor_bridge_ptr->HandleImuMessage。

|===3) Service 3: kFinishTrajectoryServiceName

|------------句柄函数:Node::HandleFinishTrajectory

|------------实际处理的函数:map_builder_bridge_.FinishTrajectory

|===4) Service 4: kWriteStateServiceName

|------------句柄函数:Node::HandleWriteState

|------------实际处理的函数:map_builder_bridge_.SerializeState

可以看到,cartographer_ros这个节点启动后所有东西都交给了map__builder_bridge_去处理。所以,之后我们得去研究这个类:MapBuilderBridge了。 这个点我们就留到下篇文章再开始。

最后编辑于
©著作权归作者所有,转载或内容合作请联系作者
  • 序言:七十年代末,一起剥皮案震惊了整个滨河市,随后出现的几起案子,更是在滨河造成了极大的恐慌,老刑警刘岩,带你破解...
    沈念sama阅读 215,539评论 6 497
  • 序言:滨河连续发生了三起死亡事件,死亡现场离奇诡异,居然都是意外死亡,警方通过查阅死者的电脑和手机,发现死者居然都...
    沈念sama阅读 91,911评论 3 391
  • 文/潘晓璐 我一进店门,熙熙楼的掌柜王于贵愁眉苦脸地迎上来,“玉大人,你说我怎么就摊上这事。” “怎么了?”我有些...
    开封第一讲书人阅读 161,337评论 0 351
  • 文/不坏的土叔 我叫张陵,是天一观的道长。 经常有香客问我,道长,这世上最难降的妖魔是什么? 我笑而不...
    开封第一讲书人阅读 57,723评论 1 290
  • 正文 为了忘掉前任,我火速办了婚礼,结果婚礼上,老公的妹妹穿的比我还像新娘。我一直安慰自己,他们只是感情好,可当我...
    茶点故事阅读 66,795评论 6 388
  • 文/花漫 我一把揭开白布。 她就那样静静地躺着,像睡着了一般。 火红的嫁衣衬着肌肤如雪。 梳的纹丝不乱的头发上,一...
    开封第一讲书人阅读 50,762评论 1 294
  • 那天,我揣着相机与录音,去河边找鬼。 笑死,一个胖子当着我的面吹牛,可吹牛的内容都是我干的。 我是一名探鬼主播,决...
    沈念sama阅读 39,742评论 3 416
  • 文/苍兰香墨 我猛地睁开眼,长吁一口气:“原来是场噩梦啊……” “哼!你这毒妇竟也来了?” 一声冷哼从身侧响起,我...
    开封第一讲书人阅读 38,508评论 0 271
  • 序言:老挝万荣一对情侣失踪,失踪者是张志新(化名)和其女友刘颖,没想到半个月后,有当地人在树林里发现了一具尸体,经...
    沈念sama阅读 44,954评论 1 308
  • 正文 独居荒郊野岭守林人离奇死亡,尸身上长有42处带血的脓包…… 初始之章·张勋 以下内容为张勋视角 年9月15日...
    茶点故事阅读 37,247评论 2 331
  • 正文 我和宋清朗相恋三年,在试婚纱的时候发现自己被绿了。 大学时的朋友给我发了我未婚夫和他白月光在一起吃饭的照片。...
    茶点故事阅读 39,404评论 1 345
  • 序言:一个原本活蹦乱跳的男人离奇死亡,死状恐怖,灵堂内的尸体忽然破棺而出,到底是诈尸还是另有隐情,我是刑警宁泽,带...
    沈念sama阅读 35,104评论 5 340
  • 正文 年R本政府宣布,位于F岛的核电站,受9级特大地震影响,放射性物质发生泄漏。R本人自食恶果不足惜,却给世界环境...
    茶点故事阅读 40,736评论 3 324
  • 文/蒙蒙 一、第九天 我趴在偏房一处隐蔽的房顶上张望。 院中可真热闹,春花似锦、人声如沸。这庄子的主人今日做“春日...
    开封第一讲书人阅读 31,352评论 0 21
  • 文/苍兰香墨 我抬头看了看天上的太阳。三九已至,却和暖如春,着一层夹袄步出监牢的瞬间,已是汗流浃背。 一阵脚步声响...
    开封第一讲书人阅读 32,557评论 1 268
  • 我被黑心中介骗来泰国打工, 没想到刚下飞机就差点儿被人妖公主榨干…… 1. 我叫王不留,地道东北人。 一个月前我还...
    沈念sama阅读 47,371评论 2 368
  • 正文 我出身青楼,却偏偏与公主长得像,于是被迫代替她去往敌国和亲。 传闻我的和亲对象是个残疾皇子,可洞房花烛夜当晚...
    茶点故事阅读 44,292评论 2 352