讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解(02)Cartographer源码无死角解析-链接如下:
(02)Cartographer源码无死角解析- (00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/127350885
文
末
正
下
方
中
心
提
供
了
本
人
联
系
方
式
，
点
击
本
人
照
片
即
可
显
示
W
X
→
官
方
认
证
{\color{blue}{文末正下方中心}提供了本人 \color{red} 联系方式，\color{blue}点击本人照片即可显示WX→官方认证}
文末正下方中心提供了本人联系方式，点击本人照片即可显示WX→官方认证
一、前言
继续前面的内容继续简介，在 Node::AddTrajectory() 函数中，其调用了 LaunchSubscribers(options, trajectory_id) 函数，其主要的作用就是订阅话题与注册回调函数。其主要根据传感器的类型与数目注册回调函数，但是注意，同一类型的位姿传感器共用一个回调函数。如：
Node::HandleLaserScanMessage() //所有单线雷达topic回调函数
Node::HandleMultiEchoLaserScanMessage() //所有回声波雷达topoc回调函数
Node::HandlePointCloud2Message() //所有多线雷达topic回调函数
Node::HandleImuMessage() //所有IMU topic回调函数
Node::HandleOdometryMessage() //所有里程计 topic
Node::HandleNavSatFixMessage() //GPU
Node::HandleLandmarkMessage()//Landmar 回调函数
LaunchSubscribers() 函数初步看起来是比较复杂的，不过没有关系，一步步对其进行分析即可。在讲解之前，这里提及一下 C++11 中的 lambda 表达式，其编程规则如下：
[捕捉列表](参数)mutable—>返回值类型{ 函数体 }
1.
[
捕
捉
列
表
]
\color{blue}1.[捕捉列表]
1.[捕捉列表] 该列表总是出现在lambda表达式的起始位置，编译器根据[]来判断接下来的代码是否为lambda表达式，捕捉列表能够捕捉当前作用域中的变量，供lambda函数使用。
[val]： 表示以传值方式捕捉变量val
[=]： 表示以传值方式捕捉当前作用域中的变量，包括this指针。
[&val]： 表示以引用方式传递捕捉变量val。
[&]： 表示以引用方式传递捕捉当前作用域中的所有变量，包括this指针。
[this]： 表示以传值方式捕捉当前的this指针。
简单的说，就是你书写的这个函数需要那些变量，你可以通过指定具体的变量名，也可以通过=或者&号捕获作用域的所有变量等。
2.
(
参
数
)
\color{blue}2.(参数)
2.(参数)与普通函数参数列表使用相同。如果不需要传递参数，可以连同"()"一起省略。
3.
m
u
t
a
b
l
e
\color{blue}3.mutable
3.mutable 除了 mutable 还可以声明 exception，默认情况下，lambda函数总是一个const函数，捕捉的传值参数具有常性，mutable可以取消常性。使用mutable修饰符时，参数列表不能省略，即使参数为空。
4.
→
返
回
值
类
型
\color{blue}4.→返回值类型
4.→返回值类型 使用追踪返回类型形式声明函数的返回值类型，没有返回值此部分可省略。返回值类型明确的情况下，也可省略，由编译器推导。
5.
函
数
体
\color{blue}5.函数体
5.函数体 在函数体内除了可以使用参数外，还能使用捕捉的变量，其余的变量都是部可以使用的。
示
例
\color{red}示例
示例
//捕捉当前作用域的变量，没有参数，编译器推导返回值类型。
int a = 1;
int b = 2;
[=]{return a + b; };
auto fun1 = [&](int c){b = a + c; }; //使用和仿函数差不多
fun1(10);
auto add1 = [&x, y]()->int{ x *= 2;//捕捉传递引用不具有常性
return x + y; };
cout << add1() << endl;
//传值捕捉
int x = 1;
int y = 2;
auto add0 = [x, y]()mutable->int{ x *= 2;//捕捉传递传值具有常性
return x + y; };
上面都是列举的一些简单例子，有兴趣的朋友可以通过其他的资料查阅进行更加深入的了解。
二、语法讲解
首先 LaunchSubscribers() 函数调用了如下一段代码：
// laser_scan 的订阅与注册回调函数, 多个laser_scan 的topic 共用同一个回调函数
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});
}
关于 ComputeRepeatedTopicNames 的内容在上一篇博客中已经进行过讲解，其主要功能就是根据配置参数→传感器默认 Topic name 以及该类传感的数目 num_xxx 生成新的 Topics name。然后会进行循环，对每个新生成的 topic 都注册会调函数。
先来看看代码中的 subscribers_(订阅者)变量，从名字都能够看出来，其应该是存储 ROS 中的订阅者对象，定义如下：
std::unordered_map<int, std::vector<Subscriber>> subscribers_;
可以看到，其是一个 map 类型，key 其实就是 trajectory_id，value 为 Subscriber 类型，该类型在同样在 node.h 中定义：
struct Subscriber {
::ros::Subscriber subscriber;
std::string topic;
};
注意看清楚，这里重新定义了一个 Subscriber，与 ::ros::Subscriber 不要搞混淆了。也就是说，如果往 subscribers_ 添加元素，其需要需要一个 trajectory_id 以及一个 Subscriber 对象。Subscriber 对象中有两个成员，分别为 ::ros::Subscriber subscriber 与 std::string topic。
源码中在添加元素时，使用花括号{}进行初始化，也就是 {::ros::Subscriber，std::string} 这种形式，源码如下：
{SubscribeWithHandler<sensor_msgs::LaserScan>(
&Node::HandleLaserScanMessage, trajectory_id, topic, &node_handle_,
this),
topic}
topic 就是 std::string 类型，SubscribeWithHandler() 函数返回的实际就是 ::ros::Subscriber 类型，定义在 node.cc 中如下。
三、SubscribeWithHandler()
初步了解代码执行原理之后，下面看详细看看 SubscribeWithHandler() 函数的实现：
/**
* @brief 在node_handle中订阅topic,并与传入的回调函数进行注册
*
* @tparam MessageType 模板参数,消息的数据类型
* @param[in] handler 函数指针, 接受传入的函数的地址
* @param[in] trajectory_id 轨迹id
* @param[in] topic 订阅的topic名字
* @param[in] node_handle ros的node_handle
* @param[in] node node类的指针
* @return ::ros::Subscriber 订阅者
*/
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, // kInfiniteSubscriberQueueSize = 0
// 使用boost::function构造回调函数,被subscribe注册
boost::function<void(const typename MessageType::ConstPtr&)>(
// c++11: lambda表达式
[node, handler, trajectory_id, topic](const typename MessageType::ConstPtr& msg) {
(node->*handler)(trajectory_id, topic, msg);
}));
}
首先第一参数，是一个函数指针，如下面的整个代码，其表示的是一个参数(参数名为handler)：
void (Node::*handler)(int, const std::string&,const typename MessageType::ConstPtr&),
其实也是比较好理解的，首先该函数必须是 Node 类中实现，返回值为 void 类型。该函数的第一个参数为 int 类型、第二个为 const std::string& 类型、第三个为 const typename MessageType::ConstPtr& 类型。
SubscribeWithHandler() 第二个参数为 const int trajectory_id，第三个参数为 const std::string& topic 都是比好好理解的，第四个 ::ros::NodeHandle* const node_handle 是前面提到的节点句柄，第五个为 Node* 类型，一般为this。
了解 SubscribeWithHandler() 的参数之后，可以看到其还是一个模板函数，回到之前的代码，来看看该模板函数其是如和调用的：
SubscribeWithHandler<sensor_msgs::LaserScan>(
&Node::HandleLaserScanMessage, trajectory_id, topic, &node_handle_, this)
首先模板需要指定消息类型，这里 sensor_msgs::LaserScan 说明其为单线雷达消息类型，指定了消息类型之后，应该如何去订阅的话题呢？
根据对ROS的了解，订阅话题一般使用 NodeHandle::subscribe() 注册回调函数，同时其会返回一个 ROS::Subscriber 类型，NodeHandle::subscribe() 函数至少需要传入三个参数，分别为→订阅话题的名称、消息队列大小、回调函数。回到 SubscribeWithHandler() 函数如下：
return node_handle->subscribe<MessageType>(
topic, kInfiniteSubscriberQueueSize, // kInfiniteSubscriberQueueSize = 0
// 使用boost::function构造回调函数,被subscribe注册
boost::function<void(const typename MessageType::ConstPtr&)>(
// c++11: lambda表达式
[node, handler, trajectory_id, topic](const typename MessageType::ConstPtr& msg) {
(node->*handler)(trajectory_id, topic, msg);
}));
现在能够看明白，通过 node_handle->subscribe 创建一个订阅者 ，消息类型为 MessageType(模板参数)，订阅的话题为 topic，消息队列大小为 kInfiniteSubscriberQueueSize，那么现在还缺少一个参数，那就是回调函数。前面传入了一个 函数指针 handler，那么我们如何去使用呢？
总的来说：
boost::function<void(const typename MessageType::ConstPtr&)>(
// c++11: lambda表达式
[node, handler, trajectory_id, topic](const typename MessageType::ConstPtr& msg) {
(node->*handler)(trajectory_id, topic, msg);
})
其表达就是一个函数，先来看看其 lambda表达式 部分(在文章开头有初步的讲解)。该 lambda 函数需要使用的到 [node, handler, trajectory_id, topic] 四个变量，传递的参数为 const typename MessageType::ConstPtr& 类型，其函数内容就是调用了 node->*handler 函数(传递trajectory_id, topic, msg三个参数)。
注意，也就是说利用 lambda 表达式，原本的NodeHandle::subscribe()即node_handle->subscribe()函数注册的回调函数只能只能传入一个参数。但是现在却可以传入三个函数了，这就是 lambda 表达式捕获代码的优势，也就是源码中这样书写的主要原因。
然后再来看看 boost::function，个人感觉该函数在这里没有起到什么作用，或许可以加快程序的运行吧，本人尝试把 boost::function 注释掉之后，依然可以编译运行。不过通过查阅资料，大概的意思就是回调函数都喜欢与 boost::function 连用，其他暂未深入了解。
四、LaunchSubscribers()注释
/**
* @brief 订阅话题与注册回调函数
*
* @param[in] options 配置参数
* @param[in] trajectory_id 轨迹id
*/
void Node::LaunchSubscribers(const TrajectoryOptions& options,
const int trajectory_id) {
// laser_scan 的订阅与注册回调函数, 多个laser_scan 的topic 共用同一个回调函数
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});
}
// multi_echo_laser_scans的订阅与注册回调函数
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});
}
// point_clouds 的订阅与注册回调函数
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.
// imu 的订阅与注册回调函数,只有一个imu的topic
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});
}
// odometry 的订阅与注册回调函数,只有一个odometry的topic
if (options.use_odometry) {
subscribers_[trajectory_id].push_back(
{SubscribeWithHandler<nav_msgs::Odometry>(&Node::HandleOdometryMessage,
trajectory_id, kOdometryTopic,
&node_handle_, this),
kOdometryTopic});
}
// gps 的订阅与注册回调函数,只有一个gps的topic
if (options.use_nav_sat) {
subscribers_[trajectory_id].push_back(
{SubscribeWithHandler<sensor_msgs::NavSatFix>(
&Node::HandleNavSatFixMessage, trajectory_id, kNavSatFixTopic,
&node_handle_, this),
kNavSatFixTopic});
}
// landmarks 的订阅与注册回调函数,只有一个landmarks的topic
if (options.use_landmarks) {
subscribers_[trajectory_id].push_back(
{SubscribeWithHandler<cartographer_ros_msgs::LandmarkList>(
&Node::HandleLandmarkMessage,
trajectory_id,
kLandmarkTopic,
&node_handle_,
this),
kLandmarkTopic});
}
}
五、MaybeWarnAboutTopicMismatch
讲解完 LaunchSubscribers() 函数之后，回到 Node::AddTrajectory() 其还执行了如下代码：
// 创建了一个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;
创建了一个定时器，间隔 kTopicMismatchCheckDelaySec 秒执行一次 MaybeWarnAboutTopicMismatch 函数，由于 由于oneshot=true, 所以只执行一次。进入 Node::MaybeWarnAboutTopicMismatch() 函数，主要功能检查设置的 topic 名字是否在ros中存在, 不存在则报错。
其主要执行步骤如下：
(
1
)
:
\color{blue}(1):
(1): 首先使用ros的master的api进行topic名字的获取，如本人显示如下：
因为是订阅话题，所以需要订阅的话题存在于ros的master之中。对所有 master 中的话题进行循环，获得其全局名称。如 “blah” => “/namespace/blah”。
( 2 ) : \color{blue}(2): (2): 循环遍历获取实际订阅的topic名字，如果设置的topic名字,在ros中不存在,则报错。注意实际订阅的topic名字按照 trajectory_id 存储于 subscribers_ 变量中，所以有两层循环，第一层是遍历轨迹，第二层遍历轨迹下的所有topic名字。
( 3 ) : \color{blue}(3): (3): 如果报错，还会打印信息告诉订阅者那些 topic 可用。
源码注释如下：
// 检查设置的topic名字是否在ros中存在, 不存在则报错
void Node::MaybeWarnAboutTopicMismatch(
const ::ros::WallTimerEvent& unused_timer_event) {
// note: 使用ros的master的api进行topic名字的获取
::ros::master::V_TopicInfo ros_topics;
::ros::master::getTopics(ros_topics);
std::set<std::string> published_topics;
std::stringstream published_topics_string;
// 获取ros中的实际topic的全局名称,resolveName()是获取全局名称
for (const auto& it : ros_topics) {
std::string resolved_topic = node_handle_.resolveName(it.name, false);
published_topics.insert(resolved_topic);
published_topics_string << resolved_topic << ",";
}
bool print_topics = false;
for (const auto& entry : subscribers_) {
int trajectory_id = entry.first;
for (const auto& subscriber : entry.second) {
// 获取实际订阅的topic名字
std::string resolved_topic = node_handle_.resolveName(subscriber.topic);
// 如果设置的topic名字,在ros中不存在,则报错
if (published_topics.count(resolved_topic) == 0) {
LOG(WARNING) << "Expected topic \"" << subscriber.topic
<< "\" (trajectory " << trajectory_id << ")"
<< " (resolved topic \"" << resolved_topic << "\")"
<< " but no publisher is currently active.";
print_topics = true;
}
}
}
// 告诉使用者哪些topic可用
if (print_topics) {
LOG(WARNING) << "Currently available topics are: "
<< published_topics_string.str();
}
}
六、结语
在 Node::AddTrajectory 函数的最后，还会执行如下代码：
for (const auto& sensor_id : expected_sensor_ids) {
subscribed_topics_.insert(sensor_id.id);
}
其是将topic名字保存下来,用于之后的新建轨迹时检查topic名字是否重复。也就是说 subscribed_topics_ 存储的是当前已经订阅过的 topic 名字。
527
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
