node.cc 的数据走向有两个
成员变量
const NodeOptions node_options_;
tf2_ros::TransformBroadcaster tf_broadcaster_;
absl::Mutex mutex_;
std::unique_ptr<cartographer_ros::metrics::FamilyFactory> metrics_registry_;
MapBuilderBridge map_builder_bridge_ GUARDED_BY(mutex_);
// These are keyed with 'trajectory_id'.
std::map<int, ::cartographer::mapping::PoseExtrapolator> extrapolators_;
std::map<int, ::ros::Time> last_published_tf_stamps_;
std::unordered_map<int, TrajectorySensorSamplers> sensor_samplers_;
std::unordered_map<int, std::vector<Subscriber>> subscribers_;
std::unordered_set<std::string> subscribed_topics_;
std::unordered_set<int> trajectories_scheduled_for_finish_;
数据走向有两个
一个是 cartographer::mapping::PoseExtrapolator , 一个是 map_builder_bridge_ 的 sensor_bridge()
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;
}
auto sensor_bridge_ptr = map_builder_bridge_.sensor_bridge(trajectory_id);
auto imu_data_ptr = sensor_bridge_ptr->ToImuData(msg);
// extrapolators_使用里程计数据进行位姿预测
if (imu_data_ptr != nullptr) {
extrapolators_.at(trajectory_id).AddImuData(*imu_data_ptr);
}
sensor_bridge_ptr->HandleImuMessage(sensor_id, msg);
}
node.cc 调用 MapBuilderBridge的sensor_bridge,
MapBuilderBridge的sensor_bridge将数据传入trajectory_builder_interface
collated_trajectory_builder继承trajectory_builder_interface,
collator 继承 collated_trajectory_builder
MapBuilderBridge
成员变量
const NodeOptions node_options_;
std::unordered_map<int,
std::shared_ptr<const
tf2_ros::Buffer* const tf_buffer_;
std::unordered_map<std::string /* landmark ID */, int> landmark_to_index_;
// These are keyed with 'trajectory_id'.
std::unordered_map<int, TrajectoryOptions> trajectory_options_;
std::unordered_map<int, size_t> trajectory_to_highest_marker_id_;
std::unordered_map<int, std::unique_ptr<SensorBridge>> sensor_bridges_;
LocalTrajectoryData::LocalSlamData>> local_slam_data_ GUARDED_BY(mutex_);
std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder_;
MapBuilderBridge 构造函数
/**
* @brief 根据传入的node_options,MapBuilder,以及tf_buffer 完成三个本地变量的初始化
*
* @param[in] node_options 参数配置
* @param[in] map_builder 在node_main.cc中传入的MapBuilder
* @param[in] tf_buffer tf_buffer
*/
MapBuilderBridge::MapBuilderBridge(
const NodeOptions& node_options,
std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder,
tf2_ros::Buffer* const tf_buffer)
: node_options_(node_options),
map_builder_(std::move(map_builder)),
tf_buffer_(tf_buffer) {
}
sensor_bridges_ 的初始化
// 开始一条新轨迹
int MapBuilderBridge::AddTrajectory(
LOG(INFO) << "Added trajectory with ID '" << trajectory_id << "'.";
// Make sure there is no trajectory with 'trajectory_id' yet.
CHECK_EQ(sensor_bridges_.count(trajectory_id), 0);
// 为这个新轨迹 添加一个SensorBridge
sensor_bridges_[trajectory_id] = absl::make_unique<SensorBridge>(
trajectory_options.num_subdivisions_per_laser_scan,
trajectory_options.tracking_frame,
node_options_.lookup_transform_timeout_sec, tf_buffer_,
map_builder_->GetTrajectoryBuilder(trajectory_id));
map_builder_->GetTrajectoryBuilder(trajectory_id) 指的是 CollatedTrajectoryBuilder
trajectory_builders_.push_back(absl::make_unique<CollatedTrajectoryBuilder>(
trajectory_options, sensor_collator_.get(), trajectory_id,
expected_sensor_ids,
// 将2D前端与2D位姿图打包在一起, 传入CollatedTrajectoryBuilder
CreateGlobalTrajectoryBuilder2D(
std::move(local_trajectory_builder), trajectory_id,
static_cast<PoseGraph2D*>(pose_graph_.get()),
local_slam_result_callback, pose_graph_odometry_motion_filter)));
}
CollatedTrajectoryBuilder 是将前端local_trajectory_builder 与 后端 PoseGraph2D 结合在一起的 TrajectoryBuilder
SensorBridge
成员变量
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_;
构造函数
// 构造函数,并且初始化TfBridge
SensorBridge::SensorBridge(
const int num_subdivisions_per_laser_scan,
const std::string& tracking_frame,
const double lookup_transform_timeout_sec, tf2_ros::Buffer* const tf_buffer,
carto::mapping::TrajectoryBuilderInterface* const trajectory_builder)
: num_subdivisions_per_laser_scan_(num_subdivisions_per_laser_scan),
tf_bridge_(tracking_frame, lookup_transform_timeout_sec, tf_buffer),
trajectory_builder_(trajectory_builder) {
}
数据走向 SensorBridge -> CollatedTrajectoryBuilder::AddSensorData()
// 调用trajectory_builder_的AddSensorData进行数据的处理
void SensorBridge::HandleImuMessage(const std::string& sensor_id,
const sensor_msgs::Imu::ConstPtr& msg) {
std::unique_ptr<carto::sensor::ImuData> imu_data = ToImuData(msg);
if (imu_data != nullptr) {
trajectory_builder_->AddSensorData(
sensor_id,
carto::sensor::ImuData{
imu_data->time, imu_data->linear_acceleration,
imu_data->angular_velocity});
}
}
MapBuilder
成员变量
const proto::MapBuilderOptions options_;
common::ThreadPool thread_pool_;
std::vector<proto::TrajectoryBuilderOptionsWithSensorIds>
all_trajectory_builder_options_;
std::unique_ptr<PoseGraph> pose_graph_;
std::unique_ptr<sensor::CollatorInterface> sensor_collator_;
std::vector<std::unique_ptr<mapping::TrajectoryBuilderInterface>>
trajectory_builders_;
一些类的初始化
// pose_graph_
pose_graph_ = absl::make_unique<PoseGraph2D>(
options_.pose_graph_options(),
absl::make_unique<optimization::OptimizationProblem2D>(
options_.pose_graph_options().optimization_problem_options()),
&thread_pool_);
// sensor_collator_
sensor_collator_ = absl::make_unique<sensor::Collator>();
// local_trajectory_builder(前端)的初始化
local_trajectory_builder = absl::make_unique<LocalTrajectoryBuilder2D>(
trajectory_options.trajectory_builder_2d_options(),
SelectRangeSensorIds(expected_sensor_ids));
// tag: CollatedTrajectoryBuilder初始化
trajectory_builders_.push_back(absl::make_unique<CollatedTrajectoryBuilder>(
trajectory_options, sensor_collator_.get(), trajectory_id,
expected_sensor_ids,
// 将2D前端与2D位姿图打包在一起, 传入CollatedTrajectoryBuilder
CreateGlobalTrajectoryBuilder2D(
std::move(local_trajectory_builder), trajectory_id,
static_cast<PoseGraph2D*>(pose_graph_.get()),
local_slam_result_callback, pose_graph_odometry_motion_filter)));
数据走向
CollatedTrajectoryBuilder
sensor_bridges_ 的 AddSensorData 调用的是 CollatedTrajectoryBuilder 中的函数.
成员变量
sensor::CollatorInterface* const sensor_collator_;
const bool collate_landmarks_;
const bool collate_fixed_frame_;
const int trajectory_id_;
std::unique_ptr<TrajectoryBuilderInterface> wrapped_trajectory_builder_;
// Time at which we last logged the rates of incoming sensor data.
std::chrono::steady_clock::time_point last_logging_time_;
std::map<std::string, common::RateTimer<>> rate_timers_;
构造函数
CollatedTrajectoryBuilder::CollatedTrajectoryBuilder(
const proto::TrajectoryBuilderOptions& trajectory_options,
sensor::CollatorInterface* const sensor_collator, const int trajectory_id,
const std::set<SensorId>& expected_sensor_ids,
std::unique_ptr<TrajectoryBuilderInterface> wrapped_trajectory_builder)
: sensor_collator_(sensor_collator),
collate_landmarks_(trajectory_options.collate_landmarks()),
collate_fixed_frame_(trajectory_options.collate_fixed_frame()),
trajectory_id_(trajectory_id),
wrapped_trajectory_builder_(std::move(wrapped_trajectory_builder)),
last_logging_time_(std::chrono::steady_clock::now()) {
absl::flat_hash_set<std::string> expected_sensor_id_strings;
for (const auto& sensor_id : expected_sensor_ids) {
if (sensor_id.type == SensorId::SensorType::LANDMARK &&
!collate_landmarks_) {
continue;
}
if (sensor_id.type == SensorId::SensorType::FIXED_FRAME_POSE &&
!collate_fixed_frame_) {
continue;
}
expected_sensor_id_strings.insert(sensor_id.id);
}
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));
});
}
数据走向 AddSensorData -> AddData
void AddSensorData(
const std::string& sensor_id,
const sensor::TimedPointCloudData& timed_point_cloud_data) override {
AddData(sensor::MakeDispatchable(sensor_id, timed_point_cloud_data));
}
数据走向 AddData -> Collator::AddSensorData()
void CollatedTrajectoryBuilder::AddData(std::unique_ptr<sensor::Data> data) {
sensor_collator_->AddSensorData(trajectory_id_, std::move(data));
}
Collator : public CollatorInterface
成员变量
// Queue keys are a pair of trajectory ID and sensor identifier.
OrderedMultiQueue queue_;
// Map of trajectory ID to all associated QueueKeys.
absl::flat_hash_map<int, std::vector<QueueKey>> queue_keys_;
AddTrajectory()
传入回调函数
// cartographer/mapping/internal/collated_trajectory_builder.cc
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));
});
void Collator::AddTrajectory(
const int trajectory_id,
const absl::flat_hash_set<std::string>& expected_sensor_ids,
const Callback& callback) {
for (const auto& sensor_id : expected_sensor_ids) {
const auto queue_key = QueueKey{
trajectory_id, sensor_id};
queue_.AddQueue(queue_key,
[callback, sensor_id](std::unique_ptr<Data> data) {
callback(sensor_id, std::move(data));
});
queue_keys_[trajectory_id].push_back(queue_key);
}
}
数据走向 Collator::AddSensorData() -> OrderedMultiQueue::Add()
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));
}
将数据传入 OrderedMultiQueue 队列中 等待处理
OrderedMultiQueue类
成员变量
// Used to verify that values are dispatched in sorted order.
common::Time last_dispatched_time_ = common::Time::min();
std::map<int, common::Time> common_start_time_per_trajectory_;
std::map<QueueKey, Queue> queues_;
QueueKey blocker_;
数据走向 OrderedMultiQueue::Add() -> OrderedMultiQueue::Add()
放入了 BlockingQueue 队列中
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();
}
之后调用 Dispatch() 将数据使用 callback() 处理数据
数据走向 OrderedMultiQueue::Queue -> CollatedTrajectoryBuilder::HandleCollatedSensorData()
数据走向 CollatedTrajectoryBuilder::HandleCollatedSensorData() -> GlobalTrajectoryBuilder::AddSensorData()
void CollatedTrajectoryBuilder::HandleCollatedSensorData(
const std::string& sensor_id, std::unique_ptr<sensor::Data> data) {
auto it = rate_timers_.find(sensor_id);
if (it == rate_timers_.end()) {
it = rate_timers_
.emplace(
std::piecewise_construct, std::forward_as_tuple(sensor_id),
std::forward_as_tuple(
common::FromSeconds(kSensorDataRatesLoggingPeriodSeconds)))
.first;
}
it->second.Pulse(data->GetTime());
if (std::chrono::steady_clock::now() - last_logging_time_ >
common::FromSeconds(kSensorDataRatesLoggingPeriodSeconds)) {
for (const auto& pair : rate_timers_) {
LOG(INFO) << pair.first << " rate: " << pair.second.DebugString();
}
last_logging_time_ = std::chrono::steady_clock::now();
}
// 传入的是wrapped_trajectory_builder_
data->AddToTrajectoryBuilder(wrapped_trajectory_builder_.get());
}
// 调用传入的trajectory_builder的AddSensorData()
void AddToTrajectoryBuilder(
mapping::TrajectoryBuilderInterface *const trajectory_builder) override {
trajectory_builder->AddSensorData(sensor_id_, data_);
}
GlobalTrajectoryBuilder
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);
}