Cartographer源码阅读---开始轨迹的相关处理
上一节我们看了一下node_main.cc的内容,发现其中最重要的部分就是Node类,这个类吃下了map_builder类,且里面实现了传感器数据的处理与分发(sensor_bridge),还有整个Cartographer算法的调用(map_builder与map_builder_bridge). 这一节我们重点看一下Node类到底做了些什么.
Node.h头文件
在Node.h中,包含了以下几个部分(程序太大,就不贴了):
构造,析构,拷贝构造,赋值函数的构建
Node(const NodeOptions& node_options,std::unique_ptr 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 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();carto::metrics::RegisterAllMetrics(metrics_registry_.get());}// Step: 1 声明需要发布的topic// 发布SubmapListsubmap_list_publisher_ =node_handle_.advertise<::cartographer_ros_msgs::SubmapList>(kSubmapListTopic, kLatestOnlyPublisherQueueSize);// 发布轨迹trajectory_node_list_publisher_ =node_handle_.advertise<::visualization_msgs::MarkerArray>(kTrajectoryNodeListTopic, kLatestOnlyPublisherQueueSize);// 发布landmark_poselandmark_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 addif (node_options_.map_builder_options.use_trajectory_builder_3d()) {point_cloud_map_publisher_ =node_handle_.advertise(kPointCloudMapTopic, kLatestOnlyPublisherQueueSize, true);}// Step: 2 声明发布对应名字的ROS服务, 并将服务的发布器放入到vector容器中
...// Step: 3 处理之后的点云的发布器
...// Step: 4 进行定时器与函数的绑定, 定时发布数据
...// lx addif (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的键值对,以供查询是否重复.
3.1 添加一条轨迹
添加轨迹的函数是AddTrajectory
/*** @brief 添加一个新的轨迹** @param[in] options 轨迹的参数配置* @return int 新生成的轨迹的id*/
int Node::AddTrajectory(const TrajectoryOptions& options);3.1.1 添加传感器维护功能:
调用了Node的函数ComputeExpectedSensorIds, 作用是根据配置文件,去返回一个传感器列表:
std::set 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的个数只能有一个
3.1.2 添加一个轨迹
接下来就是最重要的函数, Cartographer的核心
// 调用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部分详细说.
3.1.3 新增位姿估计器
这个函数功能是通过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中都是10const 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, 作为参数构造PoseExtrapolatorextrapolators_.emplace(std::piecewise_construct, std::forward_as_tuple(trajectory_id),std::forward_as_tuple(::cartographer::common::FromSeconds(kExtrapolationEstimationTimeSec),gravity_time_constant));
}这里面有个重点变量:
std::map extrapolators_; 看PoseExtrapolator这个类, 发现功能是使用IMU和/或里程计数据（如果有）来改善预测估计速度与运动. 因为机器人运动,角速度和线速度都不可能变化特别大, 所以可以用上一次的速度去预测下一次的位姿,然后用预测的位姿为初始值去进行优化, 这样的好处是可以以较小的迭代次数获得更好的结果,而且不容易陷入局部最小值.
3.1.4 数据采样器
这一部分很简单, 通过使用配置文件, 去给某条轨迹的各个传感器得到的值进行采样,
/*** @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 sensor_samplers_; 看看TrajectorySensorSamplers, 发现作用只有控制各个传感器的采样频率.
3.1.5 订阅话题与注册回调函数
LaunchSubscribers(options, trajectory_id);这个函数就挺有意思的, 有值得学的的编程技巧. 同样, 传入的参数只有配置文件和轨迹ID.
咱们进入到node.cc里看这个函数本身, 发现它是首先通过配置options判断了是否用了某个传感器, 然后把SubscribeWithHandler压入subscribers_里. 现在, 这里有两个疑问, subscribers_是啥, SubscribeWithHandler又是啥. 咱们先看subscribers_
std::unordered_map> 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
::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(topic, kInfiniteSubscriberQueueSize, // kInfiniteSubscriberQueueSize = 0// 使用boost::function构造回调函数,被subscribe注册boost::function(// 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(&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进行处理. 这一部分内容较多,还是要单独讲.
这就是整个添加一条轨迹的方法, 创作不易, 纯手打, 求点赞!