描述
主要分析cartographer处理雷达数据的代码
在代码中cout来理解
1. HandleLaserScanMessage
cartographer_ros/node.cc中的HandleLaserScanMessage()函数中,我添加了如下几个cout
std::cout<<"sensor_id: "<<sensor_id<<" time: "<<time<<" frame_id: "<<frame_id<<std::endl;
std::cout<<"num_subdivisions_per_laser_scan_: "<<num_subdivisions_per_laser_scan_<<std::endl;
std::cout<<"points.points.size() : "<<points.points.size() <<std::endl;
std::cout<<"points.points.back().time : "<<points.points.back().time <<std::endl;
std::cout<<"points.points.back().position : "<<points.points.back().position <<std::endl;
std::cout<<"points.points[1078].position : "<<points.points[1078].position <<std::endl;
std::cout<<"points.intensities[0] : "<<points.intensities[0] <<std::endl;
std::cout<<"points.points[500].position.x : "<<points.points[500].position[0] <<std::endl;
std::cout<<"points.points[500].position.y : "<<points.points[500].position[1] <<std::endl;
std::cout<<"points.points[500].position.z : "<<points.points[500].position[2] <<std::endl;
HandleLaserScanMessage()函数会被持续调用,因此我的打印也会持续输出,我随便截取两段放在下面
sensor_id: echoes time: 635682438189920596 frame_id: horizontal_laser_link
num_subdivisions_per_laser_scan_: 10
points.points.size() : 1079
points.points.back().time : 0
points.points.back().position : -2.14016
2.15901
0
points.points[1078].position : -2.14016
2.15901
0
points.intensities[0] : 3019
points.points[500].position.x : 0.67609
points.points[500].position.y : -0.116181
points.points[500].position.z : 0
------------------ 分割线,为了两段信息看起来更清楚 -----------------------------
sensor_id: echoes time: 635682438190186686 frame_id: horizontal_laser_link
num_subdivisions_per_laser_scan_: 10
points.points.size() : 1079
points.points.back().time : 0
points.points.back().position : -2.10426
2.12279
0
points.points[1078].position : -2.10426
2.12279
0
points.intensities[0] : 3001
points.points[500].position.x : 0.682004
points.points[500].position.y : -0.117197
points.points[500].position.z : 0
变量解释
sensor_id、time、frame_id // 代表着每一帧雷达数据的属性
num_subdivisions_per_laser_scan_ // 每帧雷达被划分成10份
points.points.size // 每帧雷达有1079个点
points.intensities[0] // 当前雷达帧的强度
points.points.back().position // 每帧雷达最后一个点的三维x、y、z值
points.points[1078].position // 每帧雷达第1078个点的三维x、y、z值,所以也就是最后一个点
points.points[500].position.x、y、z // 第500个点的x、y、z值
points的类型定义在cartographer/cartographer/sensor/point_cloud.h
中
using TimedPointCloud = std::vector<TimedRangefinderPoint>;
struct PointCloudWithIntensities {
TimedPointCloud points;
std::vector<float> intensities;
};
points.points的类型就是TimedPointCloud
,也就是一个vector的TimedRangefinderPoint
,定义在cartographer/cartographer/sensor/point_cloud.h
struct TimedRangefinderPoint {
Eigen::Vector3f position;
float time;
};
Eigen::Vector3f是一个三维向量,这样你应该能完全看懂我的cout了
雷达数据时间
另外,打印1079个点的time会发现,第0个点的time是-0.0186806,第1078个点的time是0,中间的点的time从-0.018依次增加直到为0
不难猜测,每一帧的点云的时间是以最后一个雷达数据点的时间为准,以我的过往经验雷达中间的转子是逆时针旋转的,第0个点在-135度,第1078个点在135度。因此第0个点是最先扫描到的,时间最小;第1078个点是最后扫描到的,时间最大。
有兴趣的话你还可以拿雷达的position去验证,打印0~1078个点的position,你会发现它的数值是以雷达为中心的,不同扫描角度时的雷达数据position值的正负,是符合雷达坐标系的。
雷达数据分段
变量中有一个这样的参数
num_subdivisions_per_laser_scan_ // 每帧雷达被划分成10份
我们可以在代码中再次cout
std::cout<<" start_index: "<<start_index<<" end_index: "<<end_index<<std::endl;
会发现输出是这样的
start_index: 0 end_index: 107
start_index: 107 end_index: 215
start_index: 215 end_index: 323
start_index: 323 end_index: 431
start_index: 431 end_index: 539
start_index: 539 end_index: 647
start_index: 647 end_index: 755
start_index: 755 end_index: 863
start_index: 863 end_index: 971
start_index: 971 end_index: 1079
显然雷达数据被分成了10份
源码层次
- cartographer_ros/node.cc
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;
}
map_builder_bridge_.sensor_bridge(trajectory_id)
->HandleLaserScanMessage(sensor_id, msg);
}
核心语句
map_builder_bridge_.sensor_bridge(trajectory_id)
->HandleLaserScanMessage(sensor_id, msg);
- cartographer_ros/sensor_bridge.cc
void SensorBridge::HandleLaserScanMessage(
const std::string& sensor_id, const sensor_msgs::LaserScan::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);
}
核心语句是
HandleLaserScan(sensor_id, time, msg->header.frame_id, point_cloud);
- cartographer_ros/sensor_bridge.cc
HandleLaserScan
仍然定义在cartographer_ros/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;
}
CHECK_LE(points.points.back().time, 0.f);
// TODO(gaschler): Use per-point time instead of subdivisions.
for (int i = 0; i != num_subdivisions_per_laser_scan_; ++i) {
const size_t start_index =
points.points.size() * i / num_subdivisions_per_laser_scan_;
const size_t end_index =
points.points.size() * (i + 1) / num_subdivisions_per_laser_scan_;
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.
const carto::common::Time subdivision_time =
time + carto::common::FromSeconds(time_to_subdivision_end);
auto it = sensor_to_previous_subdivision_time_.find(sensor_id);
if (it != sensor_to_previous_subdivision_time_.end() &&
it->second >= subdivision_time) {
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) {
point.time -= time_to_subdivision_end;
}
CHECK_EQ(subdivision.back().time, 0.f);
HandleRangefinder(sensor_id, subdivision_time, frame_id, subdivision);
}
}
核心语句是
HandleRangefinder(sensor_id, subdivision_time, frame_id, subdivision);
- cartographer_ros/sensor_bridge.cc
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()) {
CHECK_LE(ranges.back().time, 0.f);
}
const auto sensor_to_tracking =
tf_bridge_.LookupToTracking(time, CheckNoLeadingSlash(frame_id));
if (sensor_to_tracking != nullptr) {
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_->AddSensorData(
sensor_id, carto::sensor::TimedPointCloudData{
time, sensor_to_tracking->translation().cast<float>(),
carto::sensor::TransformTimedPointCloud(
ranges, sensor_to_tracking->cast<float>())});
- cartographer/mapping/internal/collated_trajectory_builder.h
void AddSensorData(
const std::string& sensor_id,
const sensor::TimedPointCloudData& timed_point_cloud_data) override {
AddData(sensor::MakeDispatchable(sensor_id, timed_point_cloud_data));
}
- cartographer/mapping/internal/collated_trajectory_builder.cc
void CollatedTrajectoryBuilder::AddData(std::unique_ptr<sensor::Data> data) {
sensor_collator_->AddSensorData(trajectory_id_, std::move(data));
}
- cartographer/sensor/internal/collator.cc
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));
}
- cartographer/sensor/internal/ordered_multi_queue.cc
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();
}
- cartographer/sensor/internal/ordered_multi_queue.cc
void OrderedMultiQueue::Dispatch() {
while (true) {
const Data* next_data = nullptr;
Queue* next_queue = nullptr;
QueueKey next_queue_key;
for (auto it = queues_.begin(); it != queues_.end();) {
const auto* data = it->second.queue.Peek<Data>();
if (data == nullptr) {
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;
}
// 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();
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));
}
}
}
}