node.cc
node.h和node.cc所包含的内容太多，只能分段解析！
01 构造函数
Node类的初始化是在node_main.cc中的Run函数中构造
// Node类的初始化, 将ROS的topic传入SLAM, 也就是MapBuilder
Node node(node_options, std::move(map_builder), &tf_buffer,
FLAGS_collect_metrics);
Node类的构造函数一共完成了四步：
-
Step 1： 声明需要发布的topic
-
Step 2： 声明发布对应名字的ROS服务, 并将服务的发布器放入到vector容器中
-
Step 3： 处理之后的点云的发布器
-
Step 4： 进行定时器与函数的绑定, 定时发布数据
/**
* @brief
* 声明ROS的一些topic的发布器, 服务的发布器, 以及将时间驱动的函数与定时器进行绑定
*
* @param[in] node_options 配置文件的内容
* @param[in] map_builder SLAM算法的具体实现
* @param[in] tf_buffer tf
* @param[in] collect_metrics 是否启用metrics,默认不启用
*/
Node::Node(
const NodeOptions& node_options,
std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder,
tf2_ros::Buffer* const tf_buffer, const bool collect_metrics)
: node_options_(node_options),
map_builder_bridge_(node_options_, std::move(map_builder), tf_buffer) {
// 将mutex_上锁, 防止在初始化时数据被更改
absl::MutexLock lock(&mutex_);
// 默认不启用
if (collect_metrics) {
metrics_registry_ = absl::make_unique<metrics::FamilyFactory>();
carto::metrics::RegisterAllMetrics(metrics_registry_.get());
}
// Step: 1 声明需要发布的topic
// 发布SubmapList
submap_list_publisher_ =
node_handle_.advertise<::cartographer_ros_msgs::SubmapList>(
kSubmapListTopic, kLatestOnlyPublisherQueueSize);
// 发布轨迹
trajectory_node_list_publisher_ =
node_handle_.advertise<::visualization_msgs::MarkerArray>(
kTrajectoryNodeListTopic, kLatestOnlyPublisherQueueSize);
// 发布landmark_pose
landmark_poses_list_publisher_ =
node_handle_.advertise<::visualization_msgs::MarkerArray>(
kLandmarkPosesListTopic, kLatestOnlyPublisherQueueSize);
// 发布约束
constraint_list_publisher_ =
node_handle_.advertise<::visualization_msgs::MarkerArray>(
kConstraintListTopic, kLatestOnlyPublisherQueueSize);
// 发布tracked_pose, 默认不发布
if (node_options_.publish_tracked_pose) {
tracked_pose_publisher_ =
node_handle_.advertise<::geometry_msgs::PoseStamped>(
kTrackedPoseTopic, kLatestOnlyPublisherQueueSize);
}
// lx add
if (node_options_.map_builder_options.use_trajectory_builder_3d()) {
point_cloud_map_publisher_ =
node_handle_.advertise<sensor_msgs::PointCloud2>(
kPointCloudMapTopic, kLatestOnlyPublisherQueueSize, true);
}
// Step: 2 声明发布对应名字的ROS服务, 并将服务的发布器放入到vector容器中
service_servers_.push_back(node_handle_.advertiseService(
kSubmapQueryServiceName, &Node::HandleSubmapQuery, this));
service_servers_.push_back(node_handle_.advertiseService(
kTrajectoryQueryServiceName, &Node::HandleTrajectoryQuery, this));
service_servers_.push_back(node_handle_.advertiseService(
kStartTrajectoryServiceName, &Node::HandleStartTrajectory, this));
service_servers_.push_back(node_handle_.advertiseService(
kFinishTrajectoryServiceName, &Node::HandleFinishTrajectory, this));
service_servers_.push_back(node_handle_.advertiseService(
kWriteStateServiceName, &Node::HandleWriteState, this));
service_servers_.push_back(node_handle_.advertiseService(
kGetTrajectoryStatesServiceName, &Node::HandleGetTrajectoryStates, this));
service_servers_.push_back(node_handle_.advertiseService(
kReadMetricsServiceName, &Node::HandleReadMetrics, this));
// Step: 3 处理之后的点云的发布器
scan_matched_point_cloud_publisher_ =
node_handle_.advertise<sensor_msgs::PointCloud2>(
kScanMatchedPointCloudTopic, kLatestOnlyPublisherQueueSize);
// Step: 4 进行定时器与函数的绑定, 定时发布数据
wall_timers_.push_back(node_handle_.createWallTimer(
::ros::WallDuration(node_options_.submap_publish_period_sec), // 0.3s
&Node::PublishSubmapList, this));
if (node_options_.pose_publish_period_sec > 0) {
publish_local_trajectory_data_timer_ = node_handle_.createTimer(
::ros::Duration(node_options_.pose_publish_period_sec), // 5e-3s
&Node::PublishLocalTrajectoryData, this);
}
wall_timers_.push_back(node_handle_.createWallTimer(
::ros::WallDuration(
node_options_.trajectory_publish_period_sec), // 30e-3s
&Node::PublishTrajectoryNodeList, this));
wall_timers_.push_back(node_handle_.createWallTimer(
::ros::WallDuration(
node_options_.trajectory_publish_period_sec), // 30e-3s
&Node::PublishLandmarkPosesList, this));
wall_timers_.push_back(node_handle_.createWallTimer(
::ros::WallDuration(kConstraintPublishPeriodSec), // 0.5s
&Node::PublishConstraintList, this));
// lx add
if (node_options_.map_builder_options.use_trajectory_builder_3d()) {
wall_timers_.push_back(node_handle_.createWallTimer(
::ros::WallDuration(kPointCloudMapPublishPeriodSec), // 10s
&Node::PublishPointCloudMap, this));
}
}
02 开始轨迹
node_main.cc的Run函数构造了node类，马上利用node类中的成员函数StartTrajectoryWithDefaultTopics开启一条默认的轨迹！
- 变量
start_trajectory_with_default_topics是在node_main.cc中，gflags定义的一些变量中定义的，默认为true
// Node类的初始化, 将ROS的topic传入SLAM, 也就是MapBuilder
Node node(node_options, std::move(map_builder), &tf_buffer,
FLAGS_collect_metrics);
// 如果加载了pbstream文件, 就执行这个函数
if (!FLAGS_load_state_filename.empty()) {
node.LoadState(FLAGS_load_state_filename, FLAGS_load_frozen_state);
}
// 使用默认topic 开始轨迹
if (FLAGS_start_trajectory_with_default_topics) {
node.StartTrajectoryWithDefaultTopics(trajectory_options);
}
在node类中的成员函数StartTrajectoryWithDefaultTopics中
- 首先检查
TrajectoryOptions是否存在2d或者3d轨迹的配置信息，存在返回true，不存在返回false - 然后调用node类中的另一个成员函数
AddTrajectory
// 使用默认topic名字开始一条轨迹,也就是开始slam
void Node::StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options) {
absl::MutexLock lock(&mutex_);
// 检查TrajectoryOptions是否存在2d或者3d轨迹的配置信息
CHECK(ValidateTrajectoryOptions(options));
// 添加一条轨迹
AddTrajectory(options);
}
在node类中的成员函数AddTrajectory中
-
首先调用node的成员函数
ComputeExpectedSensorIds中，根据配置文件, 确定所有需要的topic的名字的集合-
lua文件中，在订阅的所有激光扫描、多回波激光扫描和点云主题的基础上，如果对应的话题只有一个传感器, 那订阅的topic就是topic；如果是存在多个传感器, 那订阅的topic就是topic_1,topic_2, 依次类推；
num_laser_scans = 0, -- 是否使用单线激光数据 num_multi_echo_laser_scans = 0, -- 是否使用multi_echo_laser_scans数据 num_subdivisions_per_laser_scan = 1, -- 1帧数据被分成几次处理,一般为1 num_point_clouds = 1, -- 是否使用点云数据 -
3d slam必须有imu, 2d可有可无, imu的topic的个数只能有一个
-
Odometry（里程计）可有可无, topic的个数只能有一个
-
gps可有可无, topic的个数只能有一个
-
Landmark可有可无, topic的个数只能有一个
-
-
然后调用
MapBuilderBridge类的成员函数AddTrajectory，传入上一步获得的topic的名字的集合以及TrajectoryOptions的配置参数！- 这个函数在之后的
MapBuilderBridge类的时候会详细注释
- 这个函数在之后的
-
新增一个位姿估计器（imu和里程计的一个融合，它只是去做预测，前端匹配之前的先验！）
- 以1ms, 以及重力常数10, 作为参数构造
PoseExtrapolator
- 以1ms, 以及重力常数10, 作为参数构造
-
新添加一个传感器采样器
- 频率为1的时候，表示每一帧点云数据都使用；表示两帧使用一次点云数据
- 从lua文件中获取自己所设置的各个传感器的频率（默认所有传感器的频率都为1）来进行参数构造
TrajectorySensorSamplers
-
订阅话题与注册回调函数
-
实现了laser_scan 的订阅与注册回调函数, 多个laser_scan 的topic 共用同一个回调函数
-
multi_echo_laser_scans的订阅与注册回调函数
-
point_clouds 的订阅与注册回调函数
-
imu 的订阅与注册回调函数,只有一个imu的topic
-
odometry 的订阅与注册回调函数,只有一个odometry的topic
-
gps 的订阅与注册回调函数,只有一个gps的topic
-
landmarks 的订阅与注册回调函数,只有一个landmarks的topic
这里使用的语法很复杂，用
std::unordered_map<int, std::vector<Subscriber>>的变量subscribe_来订阅与注册回调函数，将trajectory_id作为键，一个函数指针和多个其它参数传入进一个函数，用该函数的返回值加上topic一起作为的值进行写入！最后得到的键是trajectory_id，而值则是一个装有laser_scan，point_clouds，gps等Subscriber类的一个vector！
-
-
创建了一个3s执行一次的定时器
- 由于oneshot=true, 所以只执行一次，检查设置的topic名字是否在ros中存在, 不存在则报错
（出现以上图片的错误的一个解决办法就是去launch文件重新设置remap即可！）
- 由于oneshot=true, 所以只执行一次，检查设置的topic名字是否在ros中存在, 不存在则报错
-
将topic名字保存下来,用于之后的新建轨迹时检查topic名字是否重复
int Node::AddTrajectory(const TrajectoryOptions& options) {
const std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>
expected_sensor_ids = ComputeExpectedSensorIds(options);
// 调用map_builder_bridge的AddTrajectory, 添加一个轨迹
const int trajectory_id =
map_builder_bridge_.AddTrajectory(expected_sensor_ids, options);
// 新增一个位姿估计器
AddExtrapolator(trajectory_id, options);
// 新生成一个传感器数据采样器
AddSensorSamplers(trajectory_id, options);
// 订阅话题与注册回调函数
LaunchSubscribers(options, trajectory_id);
// 创建了一个3s执行一次的定时器,由于oneshot=true, 所以只执行一次
// 检查设置的topic名字是否在ros中存在, 不存在则报错
wall_timers_.push_back(node_handle_.createWallTimer(
::ros::WallDuration(kTopicMismatchCheckDelaySec), // kTopicMismatchCheckDelaySec = 3s
&Node::MaybeWarnAboutTopicMismatch, this, /*oneshot=*/true));
// 将topic名字保存下来,用于之后的新建轨迹时检查topic名字是否重复
for (const auto& sensor_id : expected_sensor_ids) {
subscribed_topics_.insert(sensor_id.id);
}
return trajectory_id;
}
03 传感器数据的走向与传递
在开始轨迹的LaunchSubscribers中，注册了各个传感器的回调函数，当每一帧传感器数据传入时，对应的回调函数就会被调用来处理传感器数据！
| 传感器 | 回调函数 | 传感器数据走向 |
|---|---|---|
| imu | Node::HandleImuMessage | 处理imu数据,imu的数据走向有2个： 1）传入PoseExtrapolator，用于位姿预测与重力方向的确定； 2）第2个是传入SensorBridge，使用其传感器处理函数进行imu数据处理 |
| 里程计 | Node::HandleOdometryMessage | 处理里程计数据,里程计的数据走向有2个 1）传入PoseExtrapolator，用于位姿预测 2）传入SensorBridge,使用其传感器处理函数进行里程计数据处理 |
| laser_scan | Node::HandleLaserScanMessage | 调用SensorBridge的传感器处理函数进行数据处理 |
| multi_echo_laser_scans | Node::HandleMultiEchoLaserScanMessage | 调用SensorBridge的传感器处理函数进行数据处理 |
| point_clouds | Node::HandlePointCloud2Message | 调用SensorBridge的传感器处理函数进行数据处理 |
| gps | Node::HandleNavSatFixMessage | 调用SensorBridge的传感器处理函数进行数据处理 |
| landmarks | Node::HandleLandmarkMessage | 调用SensorBridge的传感器处理函数进行数据处理 |
可以发现，除了imu和里程计的数据走向多了一个传入PoseExtrapolator，其他传感器都直接传入了SensorBridge类中！
这里传入的
PoseExtrapolator和前端的位姿推测器是同一个类下的不同对象，是相互独立的！这里的
PoseExtrapolator是否使用对应了lua文件的options中use_pose_extrapolator参数的设置！
这里以imu传感器的回调函数Node::HandleImuMessage为例
* @param[in] trajectory_id 轨迹id
* @param[in] sensor_id imu的topic名字
* @param[in] msg imu的ros格式的数据
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);
}
从代码可以看到，这里的sensor_bridge_ptr是由map_builder_bridge_.sensor_bridge(trajectory_id)得到的
所以在了解SensorBridge这个类之前首先得简单了解MapBuilderBridge这个类
MapBuilderBridge.h
各个传感器回调函数中比较关键的一句：
auto sensor_bridge_ptr = map_builder_bridge_.sensor_bridge(trajectory_id);
node.cc中的map_builder_bridge_从何而来？
上面的map_builder_bridge是在node.h中定义的
//GUARDED_BY(mutex_)表示使用时需要上锁
MapBuilderBridge map_builder_bridge_ GUARDED_BY(mutex_);
初始化是在node的构造函数中
Node::Node(
const NodeOptions& node_options,
std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder,
tf2_ros::Buffer* const tf_buffer, const bool collect_metrics)
: node_options_(node_options),
map_builder_bridge_(node_options_, std::move(map_builder), tf_buffer) {
// 将mutex_上锁, 防止在初始化时数据被更改
absl::MutexLock lock(&mutex_);
因此，在node_main.cc构建node时，map_builder_bridge就存在了
// Node类的初始化, 将ROS的topic传入SLAM, 也就是MapBuilder
Node node(node_options, std::move(map_builder), &tf_buffer,
FLAGS_collect_metrics);
auto sensor_bridge_ptr = map_builder_bridge_.sensor_bridge(trajectory_id)的用法？
在MapBuilderBridge.h中有定义了一个成员变量的类型为std::unordered_map<int, std::unique_ptr<SensorBridge>> ，键为int类型，值为std::unique_ptr<SensorBridge>类型
std::unordered_map<int, std::unique_ptr<SensorBridge>> sensor_bridges_;
对应的成员函数
// 获取对应轨迹id的SensorBridge的指针
SensorBridge* MapBuilderBridge::sensor_bridge(const int trajectory_id) {
return sensor_bridges_.at(trajectory_id).get();
}
因此，可以看到，sensor_bridges_.at(trajectory_id)就表示键为trajectory_id所对应的值std::unique_ptr<SensorBridge>，而值是指向SensorBridge类的智能指针，加上get()取原始指针，再将原始指针传入给sensor_bridge_ptr进行后续的传感器数据处理！
之前在开始轨迹中，在node类中的成员函数AddTrajectory中第二步，调用了MapBuilderBridge类的成员函数AddTrajectory
- 调用MapBuilder类中的成员函数
AddTrajectoryBuilder，开始一条新的轨迹，接受返回的轨迹id - 为这个新轨迹 添加一个SensorBridge
- 保存轨迹的参数配置
// 开始一条新轨迹
int MapBuilderBridge::AddTrajectory(
const std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>&
expected_sensor_ids,
const TrajectoryOptions& trajectory_options) {
// Step: 1 开始一条新的轨迹, 返回新轨迹的id,需要传入一个函数
const int trajectory_id = map_builder_->AddTrajectoryBuilder(
expected_sensor_ids, trajectory_options.trajectory_builder_options,
// lambda表达式 local_slam_result_callback_
[this](const int trajectory_id,
const ::cartographer::common::Time time,
const Rigid3d local_pose,
::cartographer::sensor::RangeData range_data_in_local,
const std::unique_ptr<
const ::cartographer::mapping::TrajectoryBuilderInterface::
InsertionResult>) {
// 保存local slam 的结果数据 5个参数实际只用了4个
OnLocalSlamResult(trajectory_id, time, local_pose, range_data_in_local);
});
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);
// Step: 2 为这个新轨迹 添加一个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)); // CollatedTrajectoryBuilder
// Step: 3 保存轨迹的参数配置
auto emplace_result =
trajectory_options_.emplace(trajectory_id, trajectory_options);
CHECK(emplace_result.second == true);
return trajectory_id;
}
预备知识—— local frame 与 global frame
carographer中存在两个地图坐标系, 分别为global frame与local frame（slam也是这样）
local frame
是表达local slam结果的坐标系, 是固定的坐标系, 不会被回环检测与位姿图优化所更改,
其每一帧位姿间的坐标变换不会改变
global frame
是表达被回环检测与位姿图优化所更改后的坐标系, 当有新的优化结果可用时, 此坐标系与任何其他坐标系之间的转换都会跳变.
它的z轴指向上方, 即重力加速度矢量指向-z方向, 即由加速度计测得的重力分量沿+z方向.
扫一扫
696
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
