cartographer的代码主要包括两个部分:cartographer和cartographer_ros。
一、cartographer和cartographer_ros的关系
cartographer_ros相当于只给我们封装了一个ROS的皮,新建了一个cartographer_node节点,但其核心的任务都还是由cartographer来完成的。所以我们刚看源码时,当查看该节点的情况,追溯到map_builder_bridge_后就先暂时停止,知道这些任务最后交给了cartographer这个包中的MapBuilder_去处理就可以了。等我们研究完这个节点后再集中精力去看cartographer中的内容。
1、新建自定义node对象和 库mapbuilder_对象】,所有ros端操作由node构造函数中加载和开启,最终通过Cator库的mapbuilder_类【map_builder_bridge_的成员变量】对象 进行实质性操作. 库mapbuilder_的类是Cator库最核心的类,包含多条轨迹CollatedTrajectoryBuilder类型【含sensor_collated传感器分选器 以及 GlobalTrajectoryBuilder类型{共同poseGragh和各自前端}】
2、在自定义的node构造时调用http://node.cc中构造函数【主要:发布消息【定时器】、提供服务【其中新轨迹服务中会launch开启所有传感器订阅】,均含对应回调函数】,提供服务/发布消息的回调函数中会调用某功能函数A【例如某回调函数中会调用:函数Node::Function A,其调用的是http://map_builder_bridge_.cc的同名函数A,再在里面调用cartographer库里的http://map_builder.cc的同名函数A,至此ros封装工作结束,转到cartographer库。
举例1:
http://node.cc中服务回调函数中会调用Node::FinishTrajectory(),再调用http://map_builder_bridge.cc的同名函数map_builder_bridge_.FinishTrajectory(),再调用cartographer库中http://map_builder.cc的同名函数MapBuilder::FinishTrajectory(),再调用cartographer库中二级FinishTrajectory()
举例2:
在StartTrajectoryService中回调函数Node::HandleStartTrajectory中会调用Node::AddTrajectory,在其中会调用map_builder_bridge_.AddTrajectory,在其中会调用库函数map_builder_->AddTrajectoryBuilder。
//Node::Run函数的map_builder是该函数局部变量[我之前理解错了,以为是全局变量]【根据配置构造好后】,
//通过Node::Run函数中Node构造函数将其传给了map_builder_bridge_对象的成员变量map_builder_
//之后调用的这个map_builder_bridge_的成员变量map_builder_
3、注意:ros端主要有三个bridge_:map_builder_bridge_【Node的成员对象】 、 sensor_bridge【属于前者的成员类对象map_builder_bridge_.sensor_bridge】和tf_bridge【属于前者的成员类对象map_builder_bridge_.sensor_bridge.tf_bridge_】。
MapBuilderBridge和SensorBridge中分别主要调用了库MapBuilderInterface(构建了其对象map_builder_)和库TrajectoryBuilderInterface(构建了其对象trajectory_builder_)【Sensor信息通过回调直接调用TrajectoryBuilder库函数输入前端】这两个类的成员函数来处理
A、map_builder_bridge_:Node的成员变量。在提供服务和发布消息时会调用,该bridge最终调用库中map_builder的功能。MapBuilderBridge定义在/src/cartographer_ros/cartographer_ros/cartographer_ros/map_builder_bridge.h中,实现由...map_builder_bridge.cc完成。
B、 sensor_bridges_:map_builder_bridge_的成员变量。在StartTrajectoryService中回调函数Node::HandleStartTrajectory中会调用Node::AddTrajectory,在其中除了会调用map_builder_bridge_.AddTrajectory开启新的轨迹,同时也调用Node::LaunchSubscribers这个函数【开启消息订阅】。这个函数负责开启订阅各个传感器的消息,从而在消息回调函数中开启主要处理工作!注意:所有传感器的回调函数中会通过map_builder_bridge_.sensor_bridges_调用相关库函数。
//例如在Node::LaunchSubscribers开启订阅的激光点云回调函数//Node::HandleLaserScanMessage中会进行map_builder_bridge_.sensor_bridge(trajectory_id)
->HandleLaserScanMessage(sensor_id, msg, use_odom);
//再其中调用HandleLaserScanMessage会调用 SensorBridge::HandleLaserScan,//再其中调用SensorBridge::HandleRangefinder//再其中调用库函数trajectory_builder_->AddSensorData( trajectory_builder_->AddSensorData(
sensor_id, carto::sensor::TimedPointCloudData{
time,//时间 sensor_to_tracking->translation().cast(),//点云原点 carto::sensor::TransformTimedPointCloud( //将点云从Lidar坐标系转到body坐标系上 ranges, sensor_to_tracking->cast())});
注意:从sensor_bridge的构造函数和赋值可知:ros端的sensor_bridges_[]的成员变量trajectory_builder_是个指针并在构造sensor_bridges_[]时会指向map_builder_对象中同一trajectory_id的trajectory_builder对象。ros端不会额外维护一段map_builder_,ros端和库使用的都是同一个map_builder对象。
sensor_bridges_[trajectory_id] =
cartographer::common::make_unique(
trajectory_options.num_subdivisions_per_laser_scan,
trajectory_options.tracking_frame,
node_options_.lookup_transform_timeout_sec, tf_buffer_,
map_builder_->GetTrajectoryBuilder(trajectory_id));
//SensorBridge的构造函数
explicit SensorBridge(
int num_subdivisions_per_laser_scan, const std::string& tracking_frame,
double lookup_transform_timeout_sec, tf2_ros::Buffer* tf_buffer,
::cartographer::mapping::TrajectoryBuilderInterface* trajectory_builder);
按轨迹trajectory_id从MapBuilderBridge成员变量sensor_bridges_【unorder_map类型】中取出一个SensorBridge类的临时指针sensor_bridge_ptr。在处理传感器的数据时,都是通过调用map_builder_bridge_的一个类成员sensor_bridge_ptr来处理的,最终调用TrajectoryBuilder库函数输入进前端。
例如:
IMU:处理IMU的Node::HandleImuMessage回调函数,会进行两部分操作:
A:先传给Node.extrapolators_实现ros端IMU递推,
B:同时会调用调用的是map_builder_bridge_中的一个成员sensor_bridges_中某个trajectory_id的sensor_bridge对象临时指针sensor_bridge_ptr的成员函数来处理:sensor_bridge_ptr->HandleImuMessage,再在其中调用cartographer库中trajectory_builder_->AddSensorData函数【注意该trajectory_builder_指针尽管属于sensor_bridge的成员变量,但是他是指针且指向map_builder_对象中同一trajectory_id的trajectory_builder】【trajectory_builder_是CollatedTrajectoryBuilder类型【含sensor_collated传感器分选器 以及 GlobalTrajectoryBuilder类型{共同poseGragh和各自前端}】,故分别传给前端LocalTrajectoryBuilder2D,也传给了后端poseGragh
Lidar:处理Lidar的Node::HandleLaserScanMessage回调函数,会调用map_builder_bridge_.sensor_bridge(trajectory_id)
->HandleLaserScanMessage,深一步再调用 SensorBridge::HandleRangefinder(),再深一步利用sensor_bridge.tf_bridge_转换激光点云后 调用cartographer库中trajectory_builder_->AddSensorData函数【http://global_trajectory_builder.cc,再在其中把激光点云信息传给前端 和 后端,前端传给local_trajectory_builder_->AddRangeData得到匹配结果,后端将该帧激光数据传给pose_graph_->AddNode】