本人讲解关于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() 函数的实现:
/**
* @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 连用,其他暂未深入了解。
/**
* @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});
}
}
讲解完 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 名字。