[Cartographer]传感器数据流
[Cartographer]初始化
[Cartographer]传感器数据流
传感器数据流
A.Node
- 开始建图
node.StartTrajectoryWithDefaultTopics(trajectory_options);
- 添加轨迹，订阅话题
int Node::AddTrajectory(const TrajectoryOptions& options)
{
....
LaunchSubscribers(options, trajectory_id);
}
- 在
LaunchSubscribers中调用传感器的回调函数
void Node::HandleLaserScanMessage(const int trajectory_id,
const std::string& sensor_id,
const sensor_msgs::LaserScan::ConstPtr& msg)
void Node::HandleImuMessage(const int trajectory_id,
const std::string& sensor_id,
const sensor_msgs::Imu::ConstPtr& msg)
void Node::HandleOdometryMessage(const int trajectory_id,
const std::string& sensor_id,
const nav_msgs::Odometry::ConstPtr& msg)
title: IMU/Odom数据处理
1. 将ROS msg->carto::sensor::OdometryData/carto::sensor::ImuData，送到PoseExtrapolator中
2. 将ROS msg直接给SensorBridge
B.SensorBridge
- 处理Scan、IMU、Odom数据
void SensorBridge::HandleOdometryMessage(
const std::string& sensor_id, const nav_msgs::Odometry::ConstPtr& msg)
void SensorBridge::HandleImuMessage(
const std::string& sensor_id, const sensor_msgs::Imu::ConstPtr& msg)
void SensorBridge::HandleLaserScanMessage(
const std::string& sensor_id, const sensor_msgs::LaserScan::ConstPtr& msg)
C.CollatedTrajectoryBuilder
- 处理完后，调用
CollatedTrajectoryBuilder::AddSensorData()，在函数中，通过sensor::MakeDispatchable()将sensor::ImuData转换为sensor::Dispatchable，方便后续调用统一接口Dispatchable::AddToTrajectoryBuilder()
void AddSensorData(const std::string& sensor_id,
const sensor::ImuData& imu_data) override {
AddData(sensor::MakeDispatchable(sensor_id, imu_data));
}
// 将数据传入sensor_collator_的AddSensorData进行排序
void CollatedTrajectoryBuilder::AddData(std::unique_ptr<sensor::Data> data) {
sensor_collator_->AddSensorData(trajectory_id_, std::move(data));
}
D.Collator
- 将转换后的Data传递给
sensor_collator_添加传感器数据
// 向数据队列中添加 传感器数据
void Collator::AddSensorData(const int trajectory_id,
std::unique_ptr<Data> data) {
QueueKey queue_key{trajectory_id, data->GetSensorId()};
queue_.Add(std::move(queue_key), std::move(data));
}
E.OrderedMultiQueue
- 然后将数据加入到队列中，再分发
// 向数据队列中添加数据
void OrderedMultiQueue::Add(const QueueKey& queue_key,
std::unique_ptr<Data> data) {
....
// 传感器数据的分发处理
Dispatch();
}
- 在分发过程中，调用回调函数
callback
sensor_collator_->AddTrajectory(
trajectory_id, expected_sensor_id_strings,
[this](const std::string& sensor_id, std::unique_ptr<sensor::Data> data) {
HandleCollatedSensorData(sensor_id, std::move(data));
});
title: 回调函数
回调函数在CollatedTrajectoryBuilder的构造函数中，通过sensor collator->AddTrajectory进行添加
F.CollatedTrajectoryBuilder
- 回调函数：HandleCollatedSensorData
void CollatedTrajectoryBuilder::HandleCollatedSensorData(
const std::string& sensor_id, std::unique_ptr<sensor::Data> data) {
// 将排序好的数据送入 GlobalTrajectoryBuilder中的AddSensorData()函数中进行使用
data->AddToTrajectoryBuilder(wrapped_trajectory_builder_.get());
}
G.Dispatchable
- 通过统一的调度接口
Dispatchable::AddToTrajectoryBuilder()传递给CollatedTrajectoryBuilder::AddSensorData()
void AddToTrajectoryBuilder(
mapping::TrajectoryBuilderInterface *const trajectory_builder) override {
trajectory_builder->AddSensorData(sensor_id_, data_);
}
H.GlobalTrajectoryBuilder
scan
传递给LocalTrajectoryBuilder::AddRangeData
std::unique_ptr<typename LocalTrajectoryBuilder::MatchingResult>
matching_result = local_trajectory_builder_->AddRangeData(
sensor_id, timed_point_cloud_data);
IMU
数据传递给前端local_trajectory_builder_和位姿图pose_graph_
void AddSensorData(const std::string& sensor_id,
const sensor::ImuData& imu_data) override {
if (local_trajectory_builder_) {
local_trajectory_builder_->AddImuData(imu_data);
}
pose_graph_->AddImuData(trajectory_id_, imu_data);
}
Odom
数据传递给前端local_trajectory_builder_和位姿图pose_graph_
void AddSensorData(const std::string& sensor_id,
const sensor::OdometryData& odometry_data) override {
CHECK(odometry_data.pose.IsValid()) << odometry_data.pose;
if (local_trajectory_builder_) {
local_trajectory_builder_->AddOdometryData(odometry_data);
}
pose_graph_->AddOdometryData(trajectory_id_, odometry_data);
}