cartographer_ros主要函数的调用过程

cartographer_ros主要函数的调用过程

main()-----(node_main.cc)
---->cartographer_ros::Run()
-----> auto map_builder = absl::make_uniquecartographer::mapping::MapBuilder(node_options.map_builder_options);
-------->pose_graph_ = absl::make_unique(…….); map_builder.cc
----------> AddTrimmer(absl::make_unique(………) pose_graph_2d.cc
------------> AddWorkItem(this, trimmer_ptr LOCKS_EXCLUDED(mutex_) {……}); pose_graph_2d.cc
----------> absl::make_uniqueoptimization::OptimizationProblem2D(………) optimization_problem_2d.cc
--------> sensor_collator_ = absl::make_uniquesensor::TrajectoryCollator();/ sensor::Collator();
------>Node node(node_options, std::move(map_builder),&tf_buffer,FLAGS_collect_metrics); node.cc
--------> HandleSubmapQuery
--------> HandleTrajectoryQuery
--------> HandleFinishTrajectory
--------> HandleWriteState
--------> HandleGetTrajectoryStates
--------> HandleReadMetrics
-------->HandleStartTrajectory-----(node.cc) //开始一条轨迹
---------->AddTrajectory(trajectory_options)
------------>map_builder_bridge_.AddTrajectory(expected_sensor_ids,options) //回调函数传入LocalSlamData
-------------->map_builder_->AddTrajectoryBuilder() (map_builder_bridge.cc)
---------------->trajectory_builders_.push_back () (map_builder.cc)
------------------>CreateGlobalTrajectoryBuilder2D() (global_trajectory_builder.cc)
-------------------->GlobalTrajectoryBuilder()
------------------> absl::make_unique(…………, CreateGlobalTrajectoryBuilder2D(…));传入回调函数
--------------------> HandleCollatedSensorData(sensor_id, std::move(data));
--------------------> sensor_collator_->AddTrajectory(,HandleCollatedSensorData(),); 传入函数指针
---------------------->queue_.AddQueue()；collator.cc
------------------------->queues_[queue_key].callback = std::move(callback); ordered.multi.queue.cc
---------------------->queue_keys_[trajectory_id].push_back(queue_key);
---------------->MaybeAddPureLocalizationTrimmer()
------------------>pose_graph->AddTrimmer()
------------>LaunchSubscribers(options, trajectory_id)
-------------->LaunchSubscribers(options, trajectory_id)
---------------->HandleLaserScanMessage();
------------------>map_builder_bridge_.sensor_bridge(trajectory_id)->HandleLaserScanMessage(sensor_id,msg);
-------------------->HandleLaserScan(sensor_id,time,msg->header.frame_id,point_cloud) (senser_bridge.cc)
---------------------->HandleRangefinder(sensor_id, subdivision_time,frame_id,subdivision)
------------------------>trajectory_builder_->AddSensorData()
--------------------------> sensor::MakeDispatchable(sensor_id, timed_point_cloud_data)
----------------------------> Dispatchable() Dispatchable.cc
-------------------------->AddData(sensor::MakeDispatchable(………)) collated_trajectory_builder.cc
---------------------------->sensor_collator_->AddSensorData(trajectory_id_, std::move(data));
------------------------------>queue_.Add(std::move(queue_key), std::move(data))放数据到队列中collator.cc
---------------------------------> Dispatch(); ordered_multi_oueue.cc
-----------------------------------> CollatedTrajectoryBuilder::HandleCollatedSensorData 调用callback 函数调用
-------------------------------------> data->AddToTrajectoryBuilder(wrapped_trajectory_builder_.get()); 传入GlobalTrajectoryBuilder指针
---------------------------------------> trajectory_builder->AddSensorData(sensor_id_, data_); 进入global_trajectory_builder.cc
------------------------------------------>matching_result = local_trajectory_builder_->AddRangeData( ………);
------------------------------------------>auto node_id = pose_graph_->AddNode(……)
------------------------------------------>local_slam_result_callback_(……); 调用回调函数
--------------------------------------------->MapBuilderBridge::OnLocalSlamResult()
--------------------------------------------->TrajectoryState::LocalSlamData
------------>AddExtrapolator(trajectory_id, options);
------------>AddSensorSamplers(trajectory_id, options);
-------->PublishSubmapList
-------->PublishLocalTrajectoryData
-------->PublishTrajectoryNodeList
-------->PublishLandmarkPosesList
-------->PublishLocalTrajectoryData
-------->PublishConstraintList
AddSensorData()
->AddRangeData（）
—>AddAccumulatedRangeData（）
----->scanmatch() //扫描匹配
------->real_time_correlative_scan_matcher_.Match()
--------->GenerateExhaustiveSearchCandidates //候选值生成
--------->ScoreCandidates()//计算得分
------->ceres_scan_matcher_.Match()//前端优化
----->InsertIntoSubmap() //插入submap
------->InsertRangeData() submap_2d.cc
--------->InsertRangeData()
----------->Insert() ProbabilityGridRangeDataInserter2D
------------->CastRays()
->AddNode（）
–>AddNode() pose_graph_2d.cc
---->AppendNode()
---->ComputeConstraintsForNode() //计算约束
---->AddWorkItem //启动优化