二、数据分发
接收sensor数据 ——(node.cc中的sensor回调函数)
// 接受轮速计数据
void Node::HandleOdometryMessage(const int trajectory_id,
const std::string& sensor_id,
const nav_msgs::Odometry::ConstPtr& msg) {
// 锁
absl::MutexLock lock(&mutex_);
if (!sensor_samplers_.at(trajectory_id).odometry_sampler.Pulse()) {
return;
}
// 获取该条轨迹的sensor_bridge
auto sensor_bridge_ptr = map_builder_bridge_.sensor_bridge(trajectory_id);
// 转换格式
auto odometry_data_ptr = sensor_bridge_ptr->ToOdometryData(msg);
if (odometry_data_ptr != nullptr) {
// Node位姿外推器加入数据,推算位姿
extrapolators_.at(trajectory_id).AddOdometryData(*odometry_data_ptr);
}
// 传入sensor_bridge
sensor_bridge_ptr->HandleOdometryMessage(sensor_id, msg);
}
// 接受
void Node::HandleNavSatFixMessage(const int trajectory_id,
const std::string& sensor_id,
const sensor_msgs::NavSatFix::ConstPtr& msg) {
absl::MutexLock lock(&mutex_);
// 采样器
if (!sensor_samplers_.at(trajectory_id).fixed_frame_pose_sampler.Pulse()) {
return;
}
map_builder_bridge_.sensor_bridge(trajectory_id)
->HandleNavSatFixMessage(sensor_id, msg);
}
void Node::HandleLandmarkMessage(
const int trajectory_id, const std::string& sensor_id,
const cartographer_ros_msgs::LandmarkList::ConstPtr& msg) {
absl::MutexLock lock(&mutex_);
if (!sensor_samplers_.at(trajectory_id).landmark_sampler.Pulse()) {
return;
}
map_builder_bridge_.sensor_bridge(trajectory_id)
->HandleLandmarkMessage(sensor_id, msg);
}
// 接受IMU数据
void Node::HandleImuMessage(const int trajectory_id,
const std::string& sensor_id,
const sensor_msgs::Imu::ConstPtr& msg) {
absl::MutexLock lock(&mutex_);
// 采样器
if (!sensor_samplers_.at(trajectory_id).imu_sampler.Pulse()) {
return;
}
// 获取该条轨迹的sensor_bridge
auto sensor_bridge_ptr = map_builder_bridge_.sensor_bridge(trajectory_id);
// 转换格式
auto imu_data_ptr = sensor_bridge_ptr->ToImuData(msg);
if (imu_data_ptr != nullptr) {
// Node位姿外推器加入数据,推算位姿
extrapolators_.at(trajectory_id).AddImuData(*imu_data_ptr);
}
// 传入sensor_bridge
sensor_bridge_ptr->HandleImuMessage(sensor_id, msg);
}
//
void Node::HandleLaserScanMessage(const int trajectory_id,
const std::string& sensor_id,
const sensor_msgs::LaserScan::ConstPtr& msg) {
absl::MutexLock lock(&mutex_);//锁,避免各个传感器之间数据修改
// 传感器帧数采样
if (!sensor_samplers_.at(trajectory_id).rangefinder_sampler.Pulse()) {
return;
}
// 把ROS中的数据类型送到数据转换器sensor_bridge中转换成cartographer接受的雷达数据类型,
// 使用HandleLaserScanMessage函数处理数据(sensor_bridge.cc)
map_builder_bridge_.sensor_bridge(trajectory_id)
->HandleLaserScanMessage(sensor_id, msg);
}
void Node::HandleMultiEchoLaserScanMessage(
const int trajectory_id, const std::string& sensor_id,
const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) {
absl::MutexLock lock(&mutex_);
if (!sensor_samplers_.at(trajectory_id).rangefinder_sampler.Pulse()) {
return;
}
map_builder_bridge_.sensor_bridge(trajectory_id)
->HandleMultiEchoLaserScanMessage(sensor_id, msg);
}
注:在Node.cc中,将数据通过trajecotry_id传进相应的SensorBridge中进行处理。
点击处理数据的函数HandleImuMessage、HandleLaserScanMessage、HandleMultiEchoLaserScanMessage等函数转到定义,发现这些函数都是SensorBridge类的函数,点击SensorBridge类的定义
SensorBridge类(sensor_bridge.h)
class SensorBridge {
public:
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);
SensorBridge(const SensorBridge&) = delete;
SensorBridge& operator=(const SensorBridge&) = delete;
// 转换数据类型
std::unique_ptr<::cartographer::sensor::OdometryData> ToOdometryData(
const nav_msgs::Odometry::ConstPtr& msg);
void HandleOdometryMessage(const std::string& sensor_id,
const nav_msgs::Odometry::ConstPtr& msg);
void HandleNavSatFixMessage(const std::string& sensor_id,
const sensor_msgs::NavSatFix::ConstPtr& msg);
void HandleLandmarkMessage(
const std::string& sensor_id,
const cartographer_ros_msgs::LandmarkList::ConstPtr& msg);
// 转换数据类型
std::unique_ptr<::cartographer::sensor::ImuData> ToImuData(
const sensor_msgs::Imu::ConstPtr& msg);
void HandleImuMessage(const std::string& sensor_id,
const sensor_msgs::Imu::ConstPtr& msg);
void HandleLaserScanMessage(const std::string& sensor_id,
const sensor_msgs::LaserScan::ConstPtr& msg);
void HandleMultiEchoLaserScanMessage(
const std::string& sensor_id,
const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg);
void HandlePointCloud2Message(const std::string& sensor_id,
const sensor_msgs::PointCloud2::ConstPtr& msg);
const TfBridge& tf_bridge() const;
private:
void HandleLaserScan(
const std::string& sensor_id, ::cartographer::common::Time start_time,
const std::string& frame_id,
const ::cartographer::sensor::PointCloudWithIntensities& points);
void HandleRangefinder(const std::string& sensor_id,
::cartographer::common::Time time,
const std::string& frame_id,
const ::cartographer::sensor::TimedPointCloud& ranges);
const int num_subdivisions_per_laser_scan_;
std::map<std::string, cartographer::common::Time>
sensor_to_previous_subdivision_time_;
const TfBridge tf_bridge_;
::cartographer::mapping::TrajectoryBuilderInterface* const
trajectory_builder_;
absl::optional<::cartographer::transform::Rigid3d> ecef_to_local_frame_;
};
重点关注:
处理数据 ——(Node.cc)
点击HandleLaserScan()进入sensor_brideg.cc
HandleLaserScanMessage()
void SensorBridge::HandleLaserScanMessage(
const std::string& sensor_id, const sensor_msgs::LaserScan::ConstPtr& msg) {
// 定义一个这个类型(带强度的点云)的变量point_cloud,在carto中,全局搜索,可以查看
carto::sensor::PointCloudWithIntensities point_cloud;
carto::common::Time time;
// 把LaserScan类型转换为cartographer自定义的PointCloudWithIntensities类型
// 点击ToPointCloudWithIntensities()转到声明,进入msg_conversion.h
std::tie(point_cloud, time) = ToPointCloudWithIntensities(*msg);
// 将LaserScan转换成点云后送到下面函数HandleLaserScan()去处理
HandleLaserScan(sensor_id, time, msg->header.frame_id, point_cloud);
}
void SensorBridge::HandleMultiEchoLaserScanMessage(
const std::string& sensor_id,
const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) {
carto::sensor::PointCloudWithIntensities point_cloud;
carto::common::Time time;
std::tie(point_cloud, time) = ToPointCloudWithIntensities(*msg);
HandleLaserScan(sensor_id, time, msg->header.frame_id, point_cloud);
}
点击ToPointCloudWithIntensities()转到声明,进入msg_conversion.h
// 点击转到定义,msg_conversion.cc
ToPointCloudWithIntensities(const sensor_msgs::LaserScan& msg);
点击转到定义,进入 msg_conversion.cc
std::tuple<::cartographer::sensor::PointCloudWithIntensities,
::cartographer::common::Time>
ToPointCloudWithIntensities(const sensor_msgs::LaserScan& msg) {
return LaserScanToPointCloudWithIntensities(msg);
}
std::tuple<::cartographer::sensor::PointCloudWithIntensities,
::cartographer::common::Time>
ToPointCloudWithIntensities(const sensor_msgs::MultiEchoLaserScan& msg) {
return LaserScanToPointCloudWithIntensities(msg);
}
点击LaserScanToPointCloudWithIntensities()转到其定义(msg_conversion.cc),正式处理,将laserscan转成pointcloudwithIntensities
LaserScanToPointCloudWithIntensities(const LaserMessageType& msg) {
CHECK_GE(msg.range_min, 0.f);//检查,最小距离大于0
CHECK_GE(msg.range_max, msg.range_min);//最大距离大于最小距离
if (msg.angle_increment > 0.f) {
CHECK_GT(msg.angle_max, msg.angle_min);//angle_increment,点云之间的角度增量;angle_max是最后一个点的角度,angle_min是第一个点的角度
} else {
CHECK_GT(msg.angle_min, msg.angle_max);
}
PointCloudWithIntensities point_cloud;//定义变量point_cloud,最后作为返回值返回
float angle = msg.angle_min;//第一个点
for (size_t i = 0; i < msg.ranges.size(); ++i) {
const auto& echoes = msg.ranges[i];//每个测距的echoes激光回波
if (HasEcho(echoes)) {
const float first_echo = GetFirstEcho(echoes);//一般取激光最强返回值,也就是第一次回波,返回类型为float
//接下来将距离值转换成坐标值,使用轴角方式转换
if (msg.range_min <= first_echo && first_echo <= msg.range_max) {
const Eigen::AngleAxisf rotation(angle, Eigen::Vector3f::UnitZ());//定义一个轴角:以z轴为轴,以角度angle为角度
//定义一个点point,将其算出并push_back到point_cloud下的points中,作为返回值返回
const cartographer::sensor::TimedRangefinderPoint point{
rotation * (first_echo * Eigen::Vector3f::UnitX()),
i * msg.time_increment};// Eigen::Vector3f::UnitX()表示X轴上的单位向量*第一次回波距离值
//i是一帧激光的总点数,而time_increment是点与点之间的时间增量,i * msg.time_increment,就是一帧激光的时间间隔duration
point_cloud.points.push_back(point);
//将每个点的强度push_back到point_cloud中
if (msg.intensities.size() > 0) {
CHECK_EQ(msg.intensities.size(), msg.ranges.size());
const auto& echo_intensities = msg.intensities[i];
CHECK(HasEcho(echo_intensities));
point_cloud.intensities.push_back(GetFirstEcho(echo_intensities));
} else {
point_cloud.intensities.push_back(0.f);
}
}
}
//转到下一个点point
angle += msg.angle_increment;
}
//timestamp是定义的carto中的时间戳,但刚开始拿到的是ROS中的时间戳,所以需要做个计算换成carto中的时间戳
::cartographer::common::Time timestamp = FromRos(msg.header.stamp);//ROS中laser-scan的的时间戳是第一个点的时间戳
//cartographer中一帧激光的时间戳是最后一个点的时间戳,故需转换
//激光雷达每一帧数据都有时间戳,激光雷达需与主机或者其他传感器(GPS)时间同步,
if (!point_cloud.points.empty()) {
const double duration = point_cloud.points.back().time;//一帧激光的最后一个点的时间
//此时,carto中的时间戳timestamp是ROS中的第一个点+duration,故,变成了一帧激光的最后一个点
timestamp += cartographer::common::FromSeconds(duration);//一帧数据的每一个点都要加上这一帧数据的duration
for (auto& point : point_cloud.points) {
//相对时间戳
point.time -= duration;//cartographer中时间戳是最后第一个点,所以每个点减去总时长duration,最后一个点前的点都是负值
}
}
return std::make_tuple(point_cloud, timestamp);
}
至此,返回了带时间戳的点云,回到sensor_brage.cc的HandleLaserScan()函数中,接着将LaserScan转换成点云后送到下面函数HandleLaserScan()去处理,处理点云数据,在sensor_bridge.cc里
void SensorBridge::HandleLaserScan(
const std::string& sensor_id, const carto::common::Time time,
const std::string& frame_id,
const carto::sensor::PointCloudWithIntensities& points) {
if (points.points.empty()) {
return;
}
// 检查:点云中的最后一个点的时间小于等于0
CHECK_LE(points.points.back().time, 0.f);
// TODO(gaschler): Use per-point time instead of subdivisions.
// 把一帧点云分成好几份,方便畸变处理
// 以backpack2D为例,找到其配置文件backpack_2D.lua查看num_subdivisions_per_laser_scan是10
// 就是把一帧数据分成了10份
for (int i = 0; i != num_subdivisions_per_laser_scan_; ++i) {
// 初始索引 = 一帧激光所有点 / 一小份激光的点 × i = 一份的点云数量 × i = 每一份的第一个点
const size_t start_index =
points.points.size() * i / num_subdivisions_per_laser_scan_;
// 结束索引 = 一帧激光所有点 / 一小份激光点 × (i+1) = 一份的点云数量 × (i+1) = 每一份的最后一个点
const size_t end_index =
points.points.size() * (i + 1) / num_subdivisions_per_laser_scan_;
// 得到一小块点云的数据subdivision
carto::sensor::TimedPointCloud subdivision(
points.points.begin() + start_index, points.points.begin() + end_index);
// 判断一帧激光数据是否完结,跳出循环
if (start_index == end_index) {
continue;
}
// 得到一小块数据的最后一个点的时间,相对时间戳(相对一最后一个点的时间,是负值)。
const double time_to_subdivision_end = subdivision.back().time;
// `subdivision_time` is the end of the measurement so sensor::Collator will
// send all other sensor data first.
// time 指的是整帧激光的绝对时间戳;time_to_subdivision_end是一小段激光的相对时间戳
// subdivision_time是一小段激光的最后一个点的绝对时间戳 = time + 负值
const carto::common::Time subdivision_time =
time + carto::common::FromSeconds(time_to_subdivision_end);
// 查看 sensor_to_previous_subdivision_time_的定义(sensor_bridge.h),是一个map类型(带to的要么是方向,要么是map类型)
// 表示上一次记录的子块的时间戳
auto it = sensor_to_previous_subdivision_time_.find(sensor_id);
// 这种情况出现在一台机器同时有两台激光雷达,且激光雷达的topic名称一样,很多原因导致(份数、雷达扫的点数)数据交替
// 判断条件,it:上次记录,找到了但不等于上一次的时间end && it(上一次)的绝对时间 >= 当前子块的绝对时间
// 此时说明当前帧的当前子块的点云被另一帧的某个子块(上一个subdivision)全部包含,古可将当前子块舍弃
if (it != sensor_to_previous_subdivision_time_.end() &&
it->second >= subdivision_time) {
// 打印出log报waring:忽视当前sensorId的当前帧的subdivision小块,因为另一帧上一subdivision小块的绝对时间大于这一帧此小块的绝对时间
LOG(WARNING) << "Ignored subdivision of a LaserScan message from sensor "
<< sensor_id << " because previous subdivision time "
<< it->second << " is not before current subdivision time "
<< subdivision_time;
continue;
}
sensor_to_previous_subdivision_time_[sensor_id] = subdivision_time;
for (auto& point : subdivision) {
// time_to_subdivision_end 是这一小块最后时间与这一帧最后时间的相对时间,
// point.time表示当前点相对于这一帧数据的相对时间
// 如 : time_to_subdivision_end=-7 point.time=-8 则计算后point.time变成了相对于子块最后时间的相对时间(-8 -(-7) = -1)
point.time -= time_to_subdivision_end;
}
// 检查每个小分块的最后时间小于等于0
CHECK_EQ(subdivision.back().time, 0.f);
// 送去此HandleRangefinder函数处理(sensor_bridge.cc)
HandleRangefinder(sensor_id, subdivision_time, frame_id, subdivision);
}
}
// 将数据从激光雷达坐标系转换到tracking坐标系,并将转换后的数据传给SensorBridge的trajectory_builder_,
// 该trajectory_builder_是MapBuiler中的CollectorTrajectoryBuilder
void SensorBridge::HandleRangefinder(
const std::string& sensor_id, const carto::common::Time time,
const std::string& frame_id, const carto::sensor::TimedPointCloud& ranges) {
if (!ranges.empty()) {
// 检查每个小分块的最后的相对时间戳是不是小于等于0
CHECK_LE(ranges.back().time, 0.f);
}
// sensor_to_tracking表示当前传感器要转换到tracking下,tracking指的是SLAM实际跟踪的坐标系,比如base_link
const auto sensor_to_tracking =
tf_bridge_.LookupToTracking(time, CheckNoLeadingSlash(frame_id));
if (sensor_to_tracking != nullptr) {
// sensor_to_tracking不空,说明,有传感器数据,则需要给轨迹构建器添加sensor数据
// sensor_bridge.cc中的trajectory_builder_来自哪里?
// trajectory_builder_在carto中,去到,给trajectory_builder_添加数据
// 在carto中搜索collated_Trajectory_builder.h
trajectory_builder_->AddSensorData(
sensor_id, carto::sensor::TimedPointCloudData{
time, sensor_to_tracking->translation().cast<float>(),
carto::sensor::TransformTimedPointCloud(
ranges, sensor_to_tracking->cast<float>())});
}
}
查看trajectory_builder_定义
// TrajectoryBuilderInterface是一个虚基类(抽象类),TrajectoryBuilder继承与此虚基类
// 而trajectory_builder_是此类的实例化对象,可以调用此类的成员函数和变量
::cartographer::mapping::TrajectoryBuilderInterface* const
trajectory_builder_;
trajectory_builder_类型为TrajectoryBuilderInterface,是一个虚基类(抽象类),TrajectoryBuilder继承与此虚基类,故在carto中搜索collated_Trajectory_builder.h,查看其是否有成员函数AddSensorData()
数据传入carto ——(trajectory_builder_->AddSensorData)
在cartographer_ros中接受的sensor数据经过一定处理,传入到cartographer中使用(collated_trajectory_builder)
collated_trajectory_builder.h
// 由sensor_bridge的HandleRangefinder函数调用,将sensor_bridge中处理好的激光数据传给CollatedTrajectoryBuilder
void AddSensorData(
const std::string& sensor_id,
const sensor::TimedPointCloudData& timed_point_cloud_data) override {
// MakeDispatchable是什么?使分发
// 数据经过makedispatchable之后可以给到轨迹构建器,AddData数据同步器,查看定义
AddData(sensor::MakeDispatchable(sensor_id, timed_point_cloud_data));
}
// 由sensor_bridge的HandleImuMessage函数调用,将sensor_bridge中处理好的IMU数据传给CollatedTrajectoryBuilder
void AddSensorData(const std::string& sensor_id,
const sensor::ImuData& imu_data) override {
AddData(sensor::MakeDispatchable(sensor_id, imu_data));
}
// 由sensor_bridge的HandleOdometryMessage函数调用,将sensor_bridge中处理好的轮速计数据传给CollatedTrajectoryBuilder
void AddSensorData(const std::string& sensor_id,
const sensor::OdometryData& odometry_data) override {
AddData(sensor::MakeDispatchable(sensor_id, odometry_data));
}
查看MakeDispatchable(),进去dispatchable.h
数据统一组织 —— MakeDispatchable()
template <typename DataType>
// Dispatchable类
class Dispatchable : public Data {
public:
Dispatchable(const std::string &sensor_id, const DataType &data)
: Data(sensor_id), data_(data) {}
common::Time GetTime() const override { return data_.time; }
// 重要函数:sensor数据经过数据同步之后,添加到地图构建器
void AddToTrajectoryBuilder(
mapping::TrajectoryBuilderInterface *const trajectory_builder) override {
trajectory_builder->AddSensorData(sensor_id_, data_);
}
const DataType &data() const { return data_; }
private:
const DataType data_;//数据本身data_
};
template <typename DataType>
std::unique_ptr<Dispatchable<DataType>> MakeDispatchable(
const std::string &sensor_id, const DataType &data) {
//以data为实参,构建了一个类Dispatchable
return absl::make_unique<Dispatchable<DataType>>(sensor_id, data);
}
查看collated_trajectory_builder.h数据同一调用接口AddData(),将数据放入MapBuilder的数据分发器中
void
// 传入的数据是makedispatched的数据
CollatedTrajectoryBuilder::AddData(std::unique_ptr<sensor::Data> data) {
// sensor数据等通过该函数统一将数据送入MapBuilder数据分发器sensor_collator_中
// sensor_collator_的类型为sensor::collator(继承与虚基类CollatorInterface),进入collator.h中查看
// sensor_collator_数据收集器,添加完数据进行同步,同步完了进行分发dispatch,分发给谁?
// 一种传感器数据都有一个队列,每个队列都有callback,开启一条轨迹,sensor_collator_会管制一条轨迹
sensor_collator_->AddSensorData(trajectory_id_, std::move(data));
}
collator.h查看sensor_collator_的类型
class Collator : public CollatorInterface {
public:
Collator() {}
Collator(const Collator&) = delete;
Collator& operator=(const Collator&) = delete;
// CollatedTrajectoryBuilder的HandleCollatedSensorData作为回调函数callback传进来
void AddTrajectory(
int trajectory_id,
const absl::flat_hash_set<std::string>& expected_sensor_ids,
const Callback& callback) override;
void FinishTrajectory(int trajectory_id) override;
// 数据传入接口
void AddSensorData(int trajectory_id, std::unique_ptr<Data> data) override;
void Flush() override;
absl::optional<int> GetBlockingTrajectoryId() const override;
private:
// Queue keys are a pair of trajectory ID and sensor identifier.
// 队列里存放着所有的数据,包括轨迹的轮速计数据和激光数据,并且根据QueueKey分发到不同的轨迹中,并执行相应的回调函数
OrderedMultiQueue queue_;//有序的添加队列
// Map of trajectory ID to all associated QueueKeys.
// 队列queue_keys_里管理着<trajectory_id, <trajectory_id, sensor_id>>
absl::flat_hash_map<int, std::vector<QueueKey>> queue_keys_;
};
查看AddSensorData()函数,进入collated.cc
void Collator::AddSensorData(const int trajectory_id,
std::unique_ptr<Data> data) {
// 构造QueueKey
QueueKey queue_key{trajectory_id, data->GetSensorId()};
// 将sensor::Dispatchable及其对应的QueueKey放入queue_(OrderedMultiQueue::Add)里
// 点击queue_查看其定义
queue_.Add(std::move(queue_key), std::move(data));
}
点击queue_查看其定义,进入collated.h,是collator类的私有成员变量
private:
// Queue keys are a pair of trajectory ID and sensor identifier.
// 队列里存放着所有的数据,包括轨迹的轮速计数据和激光数据,并且根据QueueKey分发到不同的轨迹中,并执行相应的回调函数
OrderedMultiQueue queue_;//有序的添加队列
queue_是OrderedMultiQueue类型的对象,查看OrderedMultiQueue类型,进入ordered_multi_queue.h
class OrderedMultiQueue {
public:
using Callback = std::function<void(std::unique_ptr<Data>)>;
OrderedMultiQueue();
OrderedMultiQueue(OrderedMultiQueue&& queue) = default;
~OrderedMultiQueue();
// Adds a new queue with key 'queue_key' which must not already exist.
// 'callback' will be called whenever data from this queue can be dispatched.
void AddQueue(const QueueKey& queue_key, Callback callback);
// Marks a queue as finished, i.e. no further data can be added. The queue
// will be removed once the last piece of data from it has been dispatched.
void MarkQueueAsFinished(const QueueKey& queue_key);
// Adds 'data' to a queue with the given 'queue_key'. Data must be added
// sorted per queue.
void Add(const QueueKey& queue_key, std::unique_ptr<Data> data);
// Dispatches all remaining values in sorted order and removes the underlying
// queues.
void Flush();
// Must only be called if at least one unfinished queue exists. Returns the
// key of a queue that needs more data before the OrderedMultiQueue can
// dispatch data.
QueueKey GetBlocker() const;
private:
struct Queue {
common::BlockingQueue<std::unique_ptr<Data>> queue;
Callback callback;
bool finished = false;
};
加入数据 —— OrderedMultiQueue::Add()
在collator.cc中查看有此成员函数Add()
void OrderedMultiQueue::Add(const QueueKey& queue_key,
std::unique_ptr<Data> data) {
auto it = queues_.find(queue_key);
if (it == queues_.end()) {
LOG_EVERY_N(WARNING, 1000)
<< "Ignored data for queue: '" << queue_key << "'";
return;
}
it->second.queue.Push(std::move(data));
Dispatch();
}
将数据的回调函数及queue_key加入队列:
// 将轨迹中QueueKey和对应的回调函数,放到queues_里
void OrderedMultiQueue::AddQueue(const QueueKey& queue_key, Callback callback) {
CHECK_EQ(queues_.count(queue_key), 0);
queues_[queue_key].callback = std::move(callback);
}
分发数据 —— OrderedMultiQueue::Dispatch()
查看dispatch()函数,进入ordered_multi_queue.cc
void OrderedMultiQueue::Dispatch() {
while (true) {
const Data* next_data = nullptr;
Queue* next_queue = nullptr;
QueueKey next_queue_key;
// 遍历所有的队列,找到所有队列中最老的数据next_data
for (auto it = queues_.begin(); it != queues_.end();) {
const auto* data = it->second.queue.Peek<Data>();
if (data == nullptr) {
// 删除finished队列,不再给finished的trajectory分发数据
if (it->second.finished) {
queues_.erase(it++);
continue;
}
// 标记没有收到数据
CannotMakeProgress(it->first);
return;
}
if (next_data == nullptr || data->GetTime() < next_data->GetTime()) {
next_data = data;
next_queue = &it->second;
next_queue_key = it->first;
}
// 必须是时间有序的数据,否则会报错
CHECK_LE(last_dispatched_time_, next_data->GetTime())
<< "Non-sorted data added to queue: '" << it->first << "'";
++it;
}
// 队列为空的情况
if (next_data == nullptr) {
CHECK(queues_.empty());
return;
}
// 分发最老的数据next_data
// If we haven't dispatched any data for this trajectory yet, fast forward
// all queues of this trajectory until a common start time has been reached.
const common::Time common_start_time =
GetCommonStartTime(next_queue_key.trajectory_id);
// 如果待分发数据时间戳大于等于起始时间,则正常分发
if (next_data->GetTime() >= common_start_time) {
// Happy case, we are beyond the 'common_start_time' already.
last_dispatched_time_ = next_data->GetTime();
// 执行queue的callbac函数,该callback即是在OrderedMultiQueue::AddQueue函数中加入的
next_queue->callback(next_queue->queue.Pop());
} else if (next_queue->queue.Size() < 2) {
// 待分发数据时间戳小于等于起始时间,只有一个数据的情况,不分发数据
if (!next_queue->finished) {
// We cannot decide whether to drop or dispatch this yet.
CannotMakeProgress(next_queue_key);
return;
}
last_dispatched_time_ = next_data->GetTime();
next_queue->callback(next_queue->queue.Pop());
} else {
// 待分发数据时间戳小于等于起始时间,如果有多个数据,就分发大于起始时间的数据
// We take a peek at the time after next data. If it also is not beyond
// 'common_start_time' we drop 'next_data', otherwise we just found the
// first packet to dispatch from this queue.
std::unique_ptr<Data> next_data_owner = next_queue->queue.Pop();
if (next_queue->queue.Peek<Data>()->GetTime() > common_start_time) {
last_dispatched_time_ = next_data->GetTime();
next_queue->callback(std::move(next_data_owner));
}
}
}
}
至此,从node的sensor数据经过处理,按照时间分发到相应的GlobalTrajectoryBuilder,并通过GlobalTrajectoryBuilder的AddSensorData()函数,将数据传到前端和后端
sensor数据传入前后端
GlobalTrajectoryBuilder.cc中的AddSensorData,将相关sensor数据传入前后端
// 激光数据接口
void AddSensorData(
const std::string& sensor_id,
const sensor::TimedPointCloudData& timed_point_cloud_data) override {
CHECK(local_trajectory_builder_)
<< "Cannot add TimedPointCloudData without a LocalTrajectoryBuilder.";
// 将激光数据送入LocalTrajectoryBuilder进行CSM匹配,并生成submap
std::unique_ptr<typename LocalTrajectoryBuilder::MatchingResult>
matching_result = local_trajectory_builder_->AddRangeData(
sensor_id, timed_point_cloud_data);
if (matching_result == nullptr) {
// The range data has not been fully accumulated yet.
return;
}
kLocalSlamMatchingResults->Increment();
std::unique_ptr<InsertionResult> insertion_result;
if (matching_result->insertion_result != nullptr) {
kLocalSlamInsertionResults->Increment();
// 将前端计算的Node传入后端PoseGraph中计算回环和参与优化
auto node_id = pose_graph_->AddNode(
matching_result->insertion_result->constant_data, trajectory_id_,
matching_result->insertion_result->insertion_submaps);
CHECK_EQ(node_id.trajectory_id, trajectory_id_);
insertion_result = absl::make_unique<InsertionResult>(InsertionResult{
node_id, matching_result->insertion_result->constant_data,
std::vector<std::shared_ptr<const Submap>>(
matching_result->insertion_result->insertion_submaps.begin(),
matching_result->insertion_result->insertion_submaps.end())});
}
if (local_slam_result_callback_) {
local_slam_result_callback_(
trajectory_id_, matching_result->time, matching_result->local_pose,
std::move(matching_result->range_data_in_local),
std::move(insertion_result));
}
}
// IMU数据接口
void AddSensorData(const std::string& sensor_id,
const sensor::ImuData& imu_data) override {
if (local_trajectory_builder_) {
// 前端接收IMU数据,放在位姿外推器中,校正运动畸变和提供CSM初值
local_trajectory_builder_->AddImuData(imu_data);
}
// 后端接收IMU数据,在3DSLAM里,会构建IMU残差,参与优化,而在2D里,不会参与优化
pose_graph_->AddImuData(trajectory_id_, imu_data);
}
// 轮速里程计数据接口
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_) {
// 前端接收轮速计数据,放在位姿外推器中,校正运动畸变和提供CSM初值
local_trajectory_builder_->AddOdometryData(odometry_data);
}
// TODO(MichaelGrupp): Instead of having an optional filter on this level,
// odometry could be marginalized between nodes in the pose graph.
// Related issue: cartographer-project/cartographer/#1768
if (pose_graph_odometry_motion_filter_.has_value() &&
pose_graph_odometry_motion_filter_.value().IsSimilar(
odometry_data.time, odometry_data.pose)) {
return;
}
// 后端接收轮速计数据,用于构建轮速计边参与优化
pose_graph_->AddOdometryData(trajectory_id_, odometry_data);
}
参考链接: https://blog.csdn.net/yeluohanchan/article/details/108673253.