本文小编为大家详细介绍“C++Node类Cartographer开始轨迹的处理深度源码分析”，内容详细，步骤清晰，细节处理妥当，希望这篇“C++Node类Cartographer开始轨迹的处理深度源码分析”文章能帮助大家解决疑惑，下面跟着小编的思路慢慢深入，一起来学习新知识吧。
Node.h头文件
在Node.h中,包含了以下几个部分(程序太大,就不贴了):
构造,析构,拷贝构造,赋值函数的构建
Node(const NodeOptions& node_options, std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder, tf2_ros::Buffer* tf_buffer, bool collect_metrics); ~Node(); // c++11: =delete: 禁止编译器自动生成默认函数; =default: 要求编译器生成一个默认函数 // 禁止编译器自动生成 默认拷贝构造函数(复制构造函数) Node(const Node&) = delete; // 禁止编译器自动生成 默认赋值函数 Node& operator=(const Node&) = delete;
轨迹有关部分
// Finishes all yet active trajectories. void FinishAllTrajectories(); // Finishes a single given trajectory. Returns false if the trajectory did not // exist or was already finished. bool FinishTrajectory(int trajectory_id); // Runs final optimization. All trajectories have to be finished when calling. void RunFinalOptimization(); // Starts the first trajectory with the default topics. void StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options);
传感器数据部分
// The following functions handle adding sensor data to a trajectory. void HandleOdometryMessage(int trajectory_id, const std::string& sensor_id, const nav_msgs::Odometry::ConstPtr& msg); void HandleNavSatFixMessage(int trajectory_id, const std::string& sensor_id, const sensor_msgs::NavSatFix::ConstPtr& msg); void HandleLandmarkMessage( int trajectory_id, const std::string& sensor_id, const cartographer_ros_msgs::LandmarkList::ConstPtr& msg); void HandleImuMessage(int trajectory_id, const std::string& sensor_id, const sensor_msgs::Imu::ConstPtr& msg); void HandleLaserScanMessage(int trajectory_id, const std::string& sensor_id, const sensor_msgs::LaserScan::ConstPtr& msg); void HandleMultiEchoLaserScanMessage( int trajectory_id, const std::string& sensor_id, const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg); void HandlePointCloud2Message(int trajectory_id, const std::string& sensor_id, const sensor_msgs::PointCloud2::ConstPtr& msg);
其他部分.
比如传感器采样设置,位姿推测器等部分.
Node类的构造函数
/**
* @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容器中
...
// Step: 3 处理之后的点云的发布器
...
// Step: 4 进行定时器与函数的绑定, 定时发布数据
...
// 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));
}
}Node构造函数最重要的两个传入变量是node_options和map_builder. 这两个在上一节中已经详细说过了,map_builder是Cartographer算法部分,包含了前端和后端.
构造函数通过使用初始化列表去初始化一些私有变量(node_options_, map_builder_bridge_), 然后使用MutexLock上锁. 初始化列表和智能锁比较基础就不详细介绍了.
总之,这个构造函数使用node_options初始化了map_builder_bridge, 而map_builder_bridge又调用Cartographer算法部分的map_builder(前,后端), 同时还确定了要发布的topic,和可视化所需的topic.
开始一条轨迹
现在介绍一下Node::AddTrajectory。这块函数是这一节的重中之重了,是node.cc中的核心部分. 它维护了传感器列表, 添加了一条轨迹,新增了一个位姿估计器,传感器数据采样器,还有订阅所需的topic和注册对应的回调函数. 为了确保topic没有重复,他还保存了注册topic的键值对,以供查询是否重复.
添加一条轨迹
添加轨迹的函数是AddTrajectory
/** * @brief 添加一个新的轨迹 * * @param[in] options 轨迹的参数配置 * @return int 新生成的轨迹的id */ int Node::AddTrajectory(const TrajectoryOptions& options);
添加传感器维护功能:
调用了Node的函数ComputeExpectedSensorIds, 作用是根据配置文件,去返回一个传感器列表:
std::set<SensorId> expected_topics;
在看传感器列表之前咱们先看一下Cartographer中传感器类型的定义:
enum class SensorType {
RANGE = 0,
IMU,
ODOMETRY,
FIXED_FRAME_POSE,
LANDMARK,
LOCAL_SLAM_RESULT
};
struct SensorId {
SensorType type; // 传感器的种类
std::string id; // topic的名字
......
};它规定了一个传感器的类型与一个对应的topic的名字. 传感器的类型是一个限域枚举(枚举类). 总之作用就是联系topic与topic对应的传感器类型, 以便后续维护.
回到之前的代码,不难看出, 一个轨迹的传感器的列表有一下命名规则:
如果只有一个传感器, 那订阅的topic就是topic
如果是多个传感器, 那订阅的topic就是topic_1,topic_2, 依次类推(多个超声雷达)
3d slam必须有imu, 2d可有可无, imu的topic的个数只能有一个
里程计可有可无, topic的个数只能有一个
gps可有可无, topic的个数只能有一个
Landmark可有可无, topic的个数只能有一个
添加一个轨迹
接下来就是最重要的函数, AddTrajectory, Cartographer的核心, 传感器数据和Cartographer算法库连接处的大门, 调用了ros部分的map_builder_bridge和算法部分的map_builder产生联系. 联系的实现方法相当复杂, 将在另一节详细说
// 调用map_builder_bridge的AddTrajectory, 添加一个轨迹 const int trajectory_id = map_builder_bridge_.AddTrajectory(expected_sensor_ids, options);
这一行调用了Map_builder_bridge_的AddTrajectory添加一条轨迹. 传入的参数有一个std::set<...Sensor_id>类型的变量,std::set是一个容器,可以简单理解为键值对,而键就是值,值就是键.(比较基础不细说啦).另一个就是从node_main.cc就跟着我们的TrajectoryOptions. 也就是配置文件读取的内容. 返回值很简单,就是新建轨迹的编号. Cartographer允许有多个轨迹同时维护,而且后面我们会发现, Cartographer定位其实就是把建好的地图和定位作为两个不同的轨迹实现的. 这个函数是整个Cartographer的功能实现, 方法很复杂, 将会在以后MapBuilder部分详细说.
新增位姿估计器
这个函数功能是通过IMU和里程计(轮编码器)去预估下一次可能的位姿,给定位一个初始值
/**
* @brief 新增一个位姿估计器
*
* @param[in] trajectory_id 轨迹id
* @param[in] options 参数配置
*/
void Node::AddExtrapolator(const int trajectory_id,
const TrajectoryOptions& options) {
constexpr double kExtrapolationEstimationTimeSec = 0.001; // 1 ms
// 新生成的轨迹的id 不应该在extrapolators_中
CHECK(extrapolators_.count(trajectory_id) == 0);
// imu_gravity_time_constant在2d, 3d中都是10
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();
// c++11: map::emplace() 用于通过在容器中插入新元素来扩展map容器
// 元素是直接构建的（既不复制也不移动）.仅当键不存在时才进行插入
// c++11: std::forward_as_tuple tuple的完美转发
// 该 tuple 在以右值为参数时拥有右值引用数据成员, 否则拥有左值引用数据成员
// c++11: std::piecewise_construct 分次生成tuple的标志常量
// 在map::emplace()中使用forward_as_tuple时必须要加piecewise_construct,不加就报错
// https://www.cnblogs.com/guxuanqing/p/11396511.html
// 以1ms, 以及重力常数10, 作为参数构造PoseExtrapolator
extrapolators_.emplace(
std::piecewise_construct,
std::forward_as_tuple(trajectory_id),
std::forward_as_tuple(
::cartographer::common::FromSeconds(kExtrapolationEstimationTimeSec),
gravity_time_constant));
}这里面有个重点变量:
std::map<int, ::cartographer::mapping::PoseExtrapolator> extrapolators_;
看PoseExtrapolator这个类, 发现功能是使用IMU和/或里程计数据（如果有）来改善预测估计速度与运动. 因为机器人运动,角速度和线速度都不可能变化特别大, 所以可以用上一次的速度去预测下一次的位姿,然后用预测的位姿为初始值去进行优化, 这样的好处是可以以较小的迭代次数获得更好的结果,而且不容易陷入局部最小值.
数据采样器
这一部分很简单, 通过使用配置文件, 去给某条轨迹的各个传感器得到的值进行采样,
/**
* @brief 新生成一个传感器数据采样器
*
* @param[in] trajectory_id 轨迹id
* @param[in] options 参数配置
*/
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_, 定义如下
std::unordered_map<int, TrajectorySensorSamplers> sensor_samplers_;
看看TrajectorySensorSamplers, 发现作用只有控制各个传感器的采样频率.
订阅话题与注册回调函数
LaunchSubscribers(options, trajectory_id);
这个函数就挺有意思的, 有值得学的的编程技巧. 同样, 传入的参数只有配置文件和轨迹ID.
咱们进入到node.cc里看这个函数本身, 发现它是首先通过配置options判断了是否用了某个传感器, 然后把SubscribeWithHandler压入subscribers_里. 现在, 这里有两个疑问, subscribers_是啥, SubscribeWithHandler又是啥. 咱们先看subscribers_
std::unordered_map<int, std::vector<Subscriber>> subscribers_;
subscribers_是一个无序表, 键是轨迹id, 值是一个数组, 里面放的都是Subscriber:
struct Subscriber {
::ros::Subscriber subscriber;
// ::ros::Subscriber::getTopic() does not necessarily return the same
// std::string
// it was given in its constructor. Since we rely on the topic name as the
// unique identifier of a subscriber, we remember it ourselves.
std::string topic;
};Subscriber是一个结构体, 里面是ros的订阅器和对应的topic名字.
所以subscribers_表示某条轨迹的ros订阅器以及订阅topic的名字.
然后就是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);
}));
}这个地方有点晦涩, 不过特别优美, 也是值得学习的部分, 通过SubscribeWithHandler这个函数, 实现了所有传感器的订阅,回调函数注册,以及订阅器的维护,简洁明了.
首先, 定义了一个模板, 用来实现同一个函数适应不同的传感器类型. 返回值是ros的Subscriber类. 对于第一个参数, 这是一个函数指针,也就是函数的地址. 第二个和第三个就是轨迹id和message的topic, 第四个是ros的nodehandle, 掌控ros节点的开启与关闭,让我们能使用ros的Subscribe, 不重要. 最后一个参数就是当前node类本身,让我们可以在后续使用node类相关的方法和变量.
咱们以LaserScan为例看看
subscribers_[trajectory_id].push_back(
{SubscribeWithHandler<sensor_msgs::LaserScan>(
&Node::HandleLaserScanMessage, trajectory_id, topic, &node_handle_,
this),
topic});整体看来,就是通过SubscribeWithHandler这个模板函数,把LaserScan类型的message, 回调函数, 轨迹id, 订阅话题名称等打包成ros的subscriber, 连同订阅话题名称一起压入某条轨迹的订阅维护器.
看看这个回调函数咋传的: 参数就是回调函数指针(地址), 而回调函数指针的定义在这里定义:
void (Node::*handler)(int, const std::string&, const typename MessageType::ConstPtr&)
可以看到,这个函数是无返回值,函数指针名叫handler的, 传入参数类型为int, string,和当前模板的传感器类常量指针.
在看看SubscribeWithHandler. 第一个参数就是接收到LaserScan这个类型的message之后执行的回调函数的地址,让ros的Subscribe有回调函数调用, 后面的参数在上面已经提到, 就不多说了. 把SubscribeWithHandler的定义和使用对照着看, 发现返回值是一个ros的subscribe, 写法上和我们自己写最基础的ros订阅是一样的, topic+数字+回调函数. 回调函数用的是boost::function, 作用和std::function差不多, 用来构造函数. 函数本身又是一个lambda表达式:
[node, handler, trajectory_id, topic](const typename MessageType::ConstPtr& msg)
{(node->*handler)(trajectory_id, topic, msg);}这个lambda捕获了外部的node类本身,也就是this, 轨迹id, topic名字, 传入了当前message的类型, 执行了该回调函数(HandleLaserScanMessage), 注意啊,现在node->*handler就是HandleLaserScanMessage了, 因为在传进SubscribeWithHandler的时候就确定了这个函数指针是啥. 所以再来看看HandleLaserScanMessage
// 调用SensorBridge的传感器处理函数进行数据处理
void Node::HandleLaserScanMessage(const int trajectory_id,
const std::string& sensor_id,
const sensor_msgs::LaserScan::ConstPtr& msg) {
absl::MutexLock lock(&mutex_);
// 根据配置,是否将传感器数据跳过
if (!sensor_samplers_.at(trajectory_id).rangefinder_sampler.Pulse()) {
return;
}
map_builder_bridge_.sensor_bridge(trajectory_id)
->HandleLaserScanMessage(sensor_id, msg);
}和定义的函数指针一样传入参数类型为int, string,和当前模板的传感器类常量指针. 然后把这些数据MapBuilder的HandleLaserScanMessage进行处理.