描述
Run()函数中的函数AddTrajectory()源码继续解析
int Node::AddTrajectory(const TrajectoryOptions& options) {
const std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>
expected_sensor_ids = ComputeExpectedSensorIds(options);
const int trajectory_id =
map_builder_bridge_.AddTrajectory(expected_sensor_ids, options);
AddExtrapolator(trajectory_id, options);
AddSensorSamplers(trajectory_id, options);
LaunchSubscribers(options, trajectory_id);
wall_timers_.push_back(node_handle_.createWallTimer(
::ros::WallDuration(kTopicMismatchCheckDelaySec),
&Node::MaybeWarnAboutTopicMismatch, this, /*oneshot=*/true));
for (const auto& sensor_id : expected_sensor_ids) {
subscribed_topics_.insert(sensor_id.id);
}
return trajectory_id;
}
核心语句包括以下几个步骤,展开来分析,今天这篇分析3、4、5、6
- ComputeExpectedSensorIds 确定传感器topic【已分析】
- AddTrajectory 增加轨迹 【已分析】
- AddExtrapolator
- AddSensorSamplers
- LaunchSubscribers
- createWallTimer
解析
1. AddExtrapolator
void Node::AddExtrapolator(const int trajectory_id,
const TrajectoryOptions& options) {
constexpr double kExtrapolationEstimationTimeSec = 0.001; // 1 ms
CHECK(extrapolators_.count(trajectory_id) == 0);
const double gravity_time_constant =
node_options_.map_builder_options.use_trajectory_builder_3d()
? options.trajectory_builder_options.trajectory_builder_3d_options()
.imu_gravity_time_constant()
: options.trajectory_builder_options.trajectory_builder_2d_options()
.imu_gravity_time_constant();
extrapolators_.emplace(
std::piecewise_construct, std::forward_as_tuple(trajectory_id),
std::forward_as_tuple(
::cartographer::common::FromSeconds(kExtrapolationEstimationTimeSec),
gravity_time_constant));
}
constexpr double kExtrapolationEstimationTimeSec = 0.001; // 1 ms
没什么可说的,定义了一个double型变量,从名字上来看,叫做“ 推断估计时间(秒)”
extrapolators_
声明在了node.h
中
std::map<int, ::cartographer::mapping::PoseExtrapolator> extrapolators_;
PoseExtrapolator
声明在pose_extrapolator.h
中
CHECK(extrapolators_.count(trajectory_id) == 0);
显然,这句话的意思就是在找map中,是否有trajectory_id
的PoseExtrapolator
const double gravity_time_constant =
node_options_.map_builder_options.use_trajectory_builder_3d()
? options.trajectory_builder_options.trajectory_builder_3d_options()
.imu_gravity_time_constant()
: options.trajectory_builder_options.trajectory_builder_2d_options()
.imu_gravity_time_constant();
没有时间去扒代码了,这句话的意思很明显。
一个double型的重力时间常量,如果现在是3D建图,就用3D的参数,否则使用2D的参数
extrapolators_.emplace(
std::piecewise_construct, std::forward_as_tuple(trajectory_id),
std::forward_as_tuple(
::cartographer::common::FromSeconds(kExtrapolationEstimationTimeSec),
gravity_time_constant));
piecewise_construct
的作用是,结合std::forward_as_tuple
使用,避免把整个std::forward_as_tuple(trajectory_id)
当成key
而::cartographer::common::FromSeconds(kExtrapolationEstimationTimeSec), gravity_time_constant
正是::cartographer::mapping::PoseExtrapolator
类的构造函数参数
std::forward_as_tuple
的作用,就是让它的参数成为一个tuple
显然这句话就是向extrapolators_
推送一个新的键值对
2. AddSensorSamplers
void Node::AddSensorSamplers(const int trajectory_id,
const TrajectoryOptions& options) {
CHECK(sensor_samplers_.count(trajectory_id) == 0);
sensor_samplers_.emplace(
std::piecewise_construct, std::forward_as_tuple(trajectory_id),
std::forward_as_tuple(
options.rangefinder_sampling_ratio, options.odometry_sampling_ratio,
options.fixed_frame_pose_sampling_ratio, options.imu_sampling_ratio,
options.landmarks_sampling_ratio));
}
sensor_samplers_
依旧定义在node.h
中,和上面一节extrapolators_
一样,是个和trajectory_id
相关的map类型
std::unordered_map<int, TrajectorySensorSamplers> sensor_samplers_;
一眼看过去和上一小节很像,这段代码的功能,就是向sensor_samplers_
推送一个键值对
3. LaunchSubscribers
本篇的分析重点
源码在node.cc
中
void Node::LaunchSubscribers(const TrajectoryOptions& options,
const int trajectory_id) {
for (const std::string& topic :
ComputeRepeatedTopicNames(kLaserScanTopic, options.num_laser_scans)) {
subscribers_[trajectory_id].push_back(
{SubscribeWithHandler<sensor_msgs::LaserScan>(
&Node::HandleLaserScanMessage, trajectory_id, topic, &node_handle_,
this),
topic});
}
for (const std::string& topic : ComputeRepeatedTopicNames(
kMultiEchoLaserScanTopic, options.num_multi_echo_laser_scans)) {
subscribers_[trajectory_id].push_back(
{SubscribeWithHandler<sensor_msgs::MultiEchoLaserScan>(
&Node::HandleMultiEchoLaserScanMessage, trajectory_id, topic,
&node_handle_, this),
topic});
}
for (const std::string& topic :
ComputeRepeatedTopicNames(kPointCloud2Topic, options.num_point_clouds)) {
subscribers_[trajectory_id].push_back(
{SubscribeWithHandler<sensor_msgs::PointCloud2>(
&Node::HandlePointCloud2Message, trajectory_id, topic,
&node_handle_, this),
topic});
}
// For 2D SLAM, subscribe to the IMU if we expect it. For 3D SLAM, the IMU is
// required.
if (node_options_.map_builder_options.use_trajectory_builder_3d() ||
(node_options_.map_builder_options.use_trajectory_builder_2d() &&
options.trajectory_builder_options.trajectory_builder_2d_options()
.use_imu_data())) {
subscribers_[trajectory_id].push_back(
{SubscribeWithHandler<sensor_msgs::Imu>(&Node::HandleImuMessage,
trajectory_id, kImuTopic,
&node_handle_, this),
kImuTopic});
}
if (options.use_odometry) {
subscribers_[trajectory_id].push_back(
{SubscribeWithHandler<nav_msgs::Odometry>(&Node::HandleOdometryMessage,
trajectory_id, kOdometryTopic,
&node_handle_, this),
kOdometryTopic});
}
if (options.use_nav_sat) {
subscribers_[trajectory_id].push_back(
{SubscribeWithHandler<sensor_msgs::NavSatFix>(
&Node::HandleNavSatFixMessage, trajectory_id, kNavSatFixTopic,
&node_handle_, this),
kNavSatFixTopic});
}
if (options.use_landmarks) {
subscribers_[trajectory_id].push_back(
{SubscribeWithHandler<cartographer_ros_msgs::LandmarkList>(
&Node::HandleLandmarkMessage, trajectory_id, kLandmarkTopic,
&node_handle_, this),
kLandmarkTopic});
}
}
总体来说,这段代码分为两大部分吧
第一部分
就是前三个for循环,分别对应:kLaserScanTopic、kMultiEchoLaserScanTopic、kPointCloud2Topic三个topic类型
for循环的变量,就是这三个topic类型通过ComputeRepeatedTopicNames()函数得到的“话题_id”字符串。
subscribers_
声明在node.h
中,依旧是一个map类型
std::unordered_map<int, std::vector<Subscriber>> subscribers_;
SubscribeWithHandler
就定义在node.cc
中,源码在下
template <typename MessageType>
::ros::Subscriber SubscribeWithHandler(
void (Node::*handler)(int, const std::string&,
const typename MessageType::ConstPtr&),
const int trajectory_id, const std::string& topic,
::ros::NodeHandle* const node_handle, Node* const node) {
return node_handle->subscribe<MessageType>(
topic, kInfiniteSubscriberQueueSize,
boost::function<void(const typename MessageType::ConstPtr&)>(
[node, handler, trajectory_id,
topic](const typename MessageType::ConstPtr& msg) {
(node->*handler)(trajectory_id, topic, msg);
}));
}
SubscribeWithHandler
的函数功能:使用’node_handle’订阅’trajectory_id’的’topic’,并在’node’上调用’handler’来处理消息。返回订阅者subscriber。
第二部分
和第一部分类似,只不过第一部分应该是针对点云的,slam中点云数据是必须的
在这一部分,通过LaunchSubscribers
传入的const TrajectoryOptions& options
,也就是通过参数文件设置的一些选项设定,来决定是否订阅对应消息。
options.trajectory_builder_options.trajectory_builder_2d_options().use_imu_data()
、options.use_odometry
、options.use_nav_sat
、options.use_landmarks
这几个参数,来决定是否向subscribers_
推送对应订阅信息。
4. createWallTimer
wall_timers_.push_back(node_handle_.createWallTimer(
::ros::WallDuration(kTopicMismatchCheckDelaySec),
&Node::MaybeWarnAboutTopicMismatch, this, /*oneshot=*/true));
// We have to keep the timer handles of ::ros::WallTimers around, otherwise
// they do not fire.
std::vector<::ros::WallTimer> wall_timers_;
我们必须保留::ros::WallTimers的定时器句柄,否则它们不会触发。
向这个wall_timers_
的vector中推送::ros::WallTimer
这实际上会涉及到以后多传感器数据的时间戳对齐问题,目前暂不进一步分析,因为以后肯定还会再翻回来继续理解。