传感器数据分发CollatedTrajectoryBuilder

一、前言首先对前面的知识做一个回顾从 node_main.cc 文件中开始;//根据配置文件命令行参数与话题重映射订阅默认话题开始一条轨迹 node.StartTrajectoryWithDefaultTopics(trajectory_options); ---------------------- 在node.cc里面 // 使用默认topic名字开始一条轨迹,也就是开始slam void Node::StartTrajectoryWithDefaultTopics(const TrajectoryOptions options) { absl::MutexLock lock(mutex_); // 检查TrajectoryOptions是否存在2d或者3d轨迹的配置信息 CHECK(ValidateTrajectoryOptions(options)); // 添加一条轨迹 AddTrajectory(options); } -------------------------- 在node.cc int Node::AddTrajectory(const TrajectoryOptions options) { const std::setcartographer::mapping::TrajectoryBuilderInterface::SensorId expected_sensor_ids ComputeExpectedSensorIds(options); // 调用map_builder_bridge的AddTrajectory, 添加一个轨迹 const int trajectory_id map_builder_bridge_.AddTrajectory(expected_sensor_ids, options); // 新增一个位姿估计器 AddExtrapolator(trajectory_id, options); ................................. // 订阅话题与注册回调函数 LaunchSubscribers(options, trajectory_id);其上的 map_builder_bridge_.AddTrajectory 与 LaunchSubscribers() 是十分重要的两个函数1map_builder_bridge_.AddTrajectory 函数主要的核心就是构建CollatedTrajectoryBuilder对象存储于 node::map_builder_bridge_::map_builder_::trajectory_builders_变量之中然后返回一个 trajectory_id再根据 trajectory_id 构建一个 SensorBridge对象创建该对象时代码如下// 在src\cartographer_ros\cartographer_ros\cartographer_ros\map_builder_bridge.cc // 的int MapBuilderBridge::AddTrajectory函数里 // Step: 2 为这个新轨迹 添加一个SensorBridge sensor_bridges_[trajectory_id] absl::make_uniqueSensorBridge( trajectory_options.num_subdivisions_per_laser_scan, trajectory_options.tracking_frame, node_options_.lookup_transform_timeout_sec, tf_buffer_, map_builder_-GetTrajectoryBuilder(trajectory_id)); // CollatedTrajectoryBuilder注意 其上的 GetTrajectoryBuilder(trajectory_id) 就是获取 trajectory_builders_ 中的 CollatedTrajectoryBuilder 对象。然后作为参数传送给 SensorBridge 的构造函数2) LaunchSubscribers() 会根据 trajectory_id 与其对应的配置 TrajectoryOptions options进行话题的订阅同时注册回调函数。所有的回调函数实现在src\cartographer_ros\cartographer_ros\cartographer_ros\sensor_bridge.cc根据查询的tf对数据完成进行坐标系变换(变换到tracking_frame)之后最终都会调用类似如下的一段代码或: trajectory_builder_-AddSensorData(sensor_id,carto::sensor::OdometryData{odometry_data-time, odometry_data-pose}); 或: trajectory_builder_-AddSensorData(sensor_id,carto::sensor::FixedFramePoseData{time, absl::optionalRigid3d()}); 或: trajectory_builder_-AddSensorData(sensor_id, carto::sensor::FixedFramePoseData{time, absl::optionalRigid3d(Rigid3d::Translation(ecef_to_local_frame_.value() *LatLongAltToEcef(msg-latitude, msg-longitude, msg-altitude)))}); 或: trajectory_builder_-AddSensorData(sensor_id, landmark_data); ......上面只列举了一部分从上面可以看出trajectory_builder_-AddSensorData() 函数接收了各种各样的数据类型那么其定然存在很多重载函数。其上的 trajectory_builder_ 就是 CollatedTrajectoryBuilder 的实例对象指针每个 trajectory_id 都有一个与之对应的 CollatedTrajectoryBuilder 实例对象指针。二、构造时的传参根据上面的介绍可以知道 trajectory_builder_ 就是类CollatedTrajectoryBuilder的实例指针是在 src/cartographer/cartographer/mapping/map_builder.cc 文件的 MapBuilder::AddTrajectoryBuilder() 函数中实例化通过上一篇博客了解到其2D轨迹与3D轨迹的构建过程如下// CollatedTrajectoryBuilder初始化 trajectory_builders_.push_back(absl::make_uniqueCollatedTrajectoryBuilder( trajectory_options, sensor_collator_.get(), trajectory_id, expected_sensor_ids, // 将3D前端与3D位姿图打包在一起, 传入CollatedTrajectoryBuilder CreateGlobalTrajectoryBuilder3D( std::move(local_trajectory_builder), trajectory_id, static_castPoseGraph3D*(pose_graph_.get()), local_slam_result_callback, pose_graph_odometry_motion_filter))); // CollatedTrajectoryBuilder初始化 trajectory_builders_.push_back(absl::make_uniqueCollatedTrajectoryBuilder( trajectory_options, sensor_collator_.get(), trajectory_id, expected_sensor_ids, // 将2D前端与2D位姿图打包在一起, 传入CollatedTrajectoryBuilder CreateGlobalTrajectoryBuilder2D( std::move(local_trajectory_builder), trajectory_id, static_castPoseGraph2D*(pose_graph_.get()), local_slam_result_callback, pose_graph_odometry_motion_filter)));三、C多态CollatedTrajectoryBuilder 类是在 src/cartographer/cartographer/mapping/internal/collated_trajectory_builder.cc 文件中定义。这里涉及到一个多态的知识点创建的实例类型为 CollatedTrajectoryBuilder*但是在构建 SensorBridge 时 SensorBridge 构造函数需要的类型为 carto::mapping::TrajectoryBuilderInterface*从 collated_trajectory_builder.h 文件中可以看到class CollatedTrajectoryBuilder : public TrajectoryBuilderInterface故 CollatedTrajectoryBuilder 是 TrajectoryBuilderInterface 的派生类。TrajectoryBuilderInterface 在 src/cartographer/cartographer/mapping/trajectory_builder_interface.h 文件中被声明。从类名以及代码可以很明显的看出其是一个接口类定义了很多的纯虚函数。一个接口类可以派生出很多类型的子类。构建 SensorBridge 构造函数需要的参数为基类 TrajectoryBuilderInterface这样有个好处也就是由 TrajectoryBuilderInterface 派生出来子类都可以用于 SensorBridge 的构造函数。CollatedTrajectoryBuilder类的主要作用就是使用 sensor::CollatorInterface 整理传感器数据, 然后将其传递到2D和3D通用的 mapping::TrajectoryBuilderInterface。另外再介绍一下 c11中的std::function 与 using 的模板部分具体化c11: std::function 通用多态函数封装器 std::function 的实例能存储、复制及调用任何可调用 (Callable) 目标: 如函数、 lambda表达式、 bind表达式或其他函数对象, 还有指向成员函数指针和指向数据成员指针. 它也是对 C 中现有的可调用实体的一种类型安全的包裹相对来说, 函数指针的调用不是类型安全的在 trajectory_builder_interface.h 中可以看到如下一段代码// A callback which is called after local SLAM processes an accumulated // sensor::RangeData. If the data was inserted into a submap, reports the // assigned NodeId, otherwise nullptr if the data was filtered out. using LocalSlamResultCallback std::functionvoid(int /* trajectory ID */, common::Time, transform::Rigid3d /* local pose estimate */, sensor::RangeData /* in local frame */, std::unique_ptrconst InsertionResult);其上表示用 LocalSlamResultCallback 表示一个回调函数该回调函数无返回值需要传入五个参数。四、CollatedTrajectoryBuilder.h现在再来看类 CollatedTrajectoryBuilder在 collated_trajectory_builder.h 文件中可以看到 很多函数参数列表后买你都带了 override 关键字这里的 override 表示重写也就是说该函数就是重写父类函数而不是其他自身构建的函数如果该函数传入的参数或者返回值与父类不一致则会报错。那么现在就正式开始讲解首先在该类中共存在五个 void AddSensorData() 重载函数 如下所示// 处理雷达点云数据 void AddSensorData(const std::string sensor_id,const sensor::TimedPointCloudData timed_point_cloud_data) override {AddData(sensor::MakeDispatchable(sensor_id, timed_point_cloud_data));} // 处理IMU数据 void AddSensorData(const std::string sensor_id,const sensor::ImuData imu_data) override {AddData(sensor::MakeDispatchable(sensor_id, imu_data));} // 处理里程计数据 void AddSensorData(const std::string sensor_id,const sensor::OdometryData odometry_data) override {AddData(sensor::MakeDispatchable(sensor_id, odometry_data));} // 根据参数决定gps数据是否需要排序 // AddData与wrapped_trajectory_builder_-AddSensorData只能选一种 // 因为AddData最终调用的就是wrapped_trajectory_builder_-AddSensorData void AddSensorData(const std::string sensor_id,const sensor::FixedFramePoseData fixed_frame_pose_data) override {if (collate_fixed_frame_) { AddData(sensor::MakeDispatchable(sensor_id, fixed_frame_pose_data)); return; } wrapped_trajectory_builder_-AddSensorData(sensor_id,fixed_frame_pose_data); } // 根据参数决定Landmark数据是否需要排序 void AddSensorData(const std::string sensor_id,const sensor::LandmarkData landmark_data) override { if (collate_landmarks_) { AddData(sensor::MakeDispatchable(sensor_id, landmark_data)); return; } wrapped_trajectory_builder_-AddSensorData(sensor_id, landmark_data); }从上面的代码中可以看到一个共同调用的函数 AddData(sensor::MakeDispatchable(xxx))。处理GPS和Landmark数据时会对 collate_fixed_frame_ 与 collate_landmarks_ 参数进行判断然后再决定是否调用 AddData() 函数。这两个参数都是再 src/cartographer/configuration_files/trajectory_builder.lua 中设置。另外其上还可以看到wrapped_trajectory_builder_-AddSensorData(xxx)该部分类容后面进行讲解。同时在 collated_trajectory_builder.h 文件中还存在一个如下函数也是比较重要的的// 将local slam 的结果也作为一种传感器数据进行处理 void AddLocalSlamResultData(std::unique_ptrmapping::LocalSlamResultData local_slam_result_data) override { AddData(std::move(local_slam_result_data)); } private: void AddData(std::unique_ptrsensor::Data data); void HandleCollatedSensorData(const std::string sensor_id, std::unique_ptrsensor::Data data);后续会对他们进行详细的分析。五、CollatedTrajectoryBuilder.cc在大致了解了头文件之后在来看看 CollatedTrajectoryBuilder 的构造函数。首先要了解的就是该类实例化时传入的参数。这里回到前面的讲解的二、构造传参在src\cartographer\cartographer\mapping\map_builder.cc可以看到如下类似代码:trajectory_builders_.push_back(absl::make_uniqueCollatedTrajectoryBuilder( trajectory_options, sensor_collator_.get(), trajectory_id, expected_sensor_ids, // 第五个参数 将3D前端与3D位姿图打包在一起, 传入CollatedTrajectoryBuilder CreateGlobalTrajectoryBuilder3D( std::move(local_trajectory_builder), trajectory_id, static_castPoseGraph3D*(pose_graph_.get()), local_slam_result_callback, pose_graph_odometry_motion_filter)));可以看到构建 CollatedTrajectoryBuilder 对象时需要传入五个参数。特别是第五个参数时比较复杂的其返回为 GlobalTrajectoryBuilder 类对象// 第一部分3D 全局轨迹构建器创建函数 // 3d的完整的slam // 工厂函数专门创建 3D 版本的全局轨迹构建器把3D 前端和3D 后端绑定在一起 std::unique_ptrTrajectoryBuilderInterface CreateGlobalTrajectoryBuilder3D( // 参数1 3D局部SLAM前端负责实时算位姿、建子图 std::unique_ptrLocalTrajectoryBuilder3D local_trajectory_builder, // 参数2 轨迹id const int trajectory_id, // 参数3 3D 后端位姿图负责全局优化、回环闭合 mapping::PoseGraph3D* const pose_graph, // 参数4 局部SLAM结果回调 const TrajectoryBuilderInterface::LocalSlamResultCallback local_slam_result_callback, // 参数5 运动滤波器 const absl::optionalMotionFilter pose_graph_odometry_motion_filter) { // 创建并返回3D全局轨迹构建器 return absl::make_unique GlobalTrajectoryBuilderLocalTrajectoryBuilder3D, mapping::PoseGraph3D( std::move(local_trajectory_builder), trajectory_id, pose_graph, local_slam_result_callback, pose_graph_odometry_motion_filter); }注意该类继承于 TrajectoryBuilderInterface 接口。class GlobalTrajectoryBuilder : public mapping::TrajectoryBuilderInterface {CollatedTrajectoryBuilder其构造函数及其初始化列表如下src\cartographer\cartographer\mapping\internal\collated_trajectory_builder.cc/** * brief Construct a new Collated Trajectory Builder:: Collated Trajectory Builder object * * param[in] trajectory_options 轨迹的参数配置 * param[in] sensor_collator 传入的整理传感器的类,有2种类型 * param[in] trajectory_id 新生成的轨迹的id * param[in] expected_sensor_ids 所有需要的topic的名字的集合 * param[in] wrapped_trajectory_builder 完整的slam GlobalTrajectoryBuilder */ CollatedTrajectoryBuilder::CollatedTrajectoryBuilder( const proto::TrajectoryBuilderOptions trajectory_options, sensor::CollatorInterface* const sensor_collator, const int trajectory_id, const std::setSensorId expected_sensor_ids, std::unique_ptrTrajectoryBuilderInterface wrapped_trajectory_builder) : sensor_collator_(sensor_collator), // 以下两个参数在 configuration_files/trajectory_builder.lua 中 // collate_landmarks 为 false, 不要将landmark数据放入到阻塞队列中 collate_landmarks_(trajectory_options.collate_landmarks()), // collate_fixed_frame 为 true, 将gps数据放入阻塞队列中 collate_fixed_frame_(trajectory_options.collate_fixed_frame()), trajectory_id_(trajectory_id), wrapped_trajectory_builder_(std::move(wrapped_trajectory_builder)), last_logging_time_(std::chrono::steady_clock::now()) { // 获取topic的名字, 并根据参数配置决定是否加入LANDMARK与gps的topic absl::flat_hash_setstd::string expected_sensor_id_strings; for (const auto sensor_id : expected_sensor_ids) { // collate_landmarks 为 false, sensor_collator_不处理LANDMARK数据 if (sensor_id.type SensorId::SensorType::LANDMARK !collate_landmarks_) { continue; } // collate_fixed_frame 为 true, sensor_collator_处理gps数据 if (sensor_id.type SensorId::SensorType::FIXED_FRAME_POSE !collate_fixed_frame_) { continue; } expected_sensor_id_strings.insert(sensor_id.id); } // sensor::Collator的初始化 sensor_collator_-AddTrajectory( trajectory_id, expected_sensor_id_strings, [this](const std::string sensor_id, std::unique_ptrsensor::Data data) { HandleCollatedSensorData(sensor_id, std::move(data)); }); } // 将数据传入sensor_collator_的AddSensorData进行排序 void CollatedTrajectoryBuilder::AddData(std::unique_ptrsensor::Data data) { sensor_collator_-AddSensorData(trajectory_id_, std::move(data)); }初步看起来还是挺复杂的先看看其简单的部分在初始化列表中从 trajectory_options 中获取 collate_landmarks 与 collate_fixed_frame 的配置信息然后赋值给 collate_landmarks_ 与 collate_fixed_frame_。trajectory_id 赋值给成员变量 trajectory_id_。同时还把传入的 CollatedTrajectoryBuilder 实例赋值给了 成员变量 wrapped_trajectory_builder_另外使用 last_logging_time_ 记录了初始化列表完成的时间。继续接着往下分析(1):其首先对参数 expected_sensor_ids(简单理解订阅的话题) 进行遍历用所有话题的名字构建一个集合 expected_sensor_id_strings。需要注意的是如果 collate_landmarks_ 与 collate_fixed_frame_ 设置为 false则 landmark 与 GPS 的话题不会添加到该集合之中。(2):传入的参数sensor::CollatorInterface* const sensor_collator调用 sensor_collator_-AddTrajectory() 函数实际就是对 sensor::Collator 实例对象的初始化。sensor_collator_-AddTrajectory() 函数 需要传入3个参数分别为 trajectory_id, expected_sensor_id_strings 以及一个 lambda 格式的函数指针。该 lambda 函数体比较简单实际就是调用了 CollatedTrajectoryBuilder::HandleCollatedSensorData() 函数。这个在下面讲解成员函数会有六、成员函数1、AddData()分了 CollatedTrajectoryBuilder 构造函数之后在来分析一下其他的成员函数// 将数据传入sensor_collator_的AddSensorData进行排序 void CollatedTrajectoryBuilder::AddData(std::unique_ptrsensor::Data data) { sensor_collator_-AddSensorData(trajectory_id_, std::move(data)); }该成员函数就是前面提到被多个重载 CollatedTrajectoryBuilder::AddSensorData() 函数调用的 AddData 函数。该函数比较简单就是调用 sensor::Collator 实例对象 sensor_collator_ 的 AddSensorData() 函数。关于 sensor::Collator 的相关内容后续为大家进行详细的讲解。2、HandleCollatedSensorData()该函数前面的内容暂时忽略在下一篇博客中会对 std::mapstd::string, common::RateTimer rate_timers_; 进行详细的讲解该函数最后就是将排序好的数据送入 GlobalTrajectoryBuilder中的AddSensorData()函数中进行使用执行如下代码// 将排序好的数据送入 GlobalTrajectoryBuilder中的AddSensorData()函数中进行使用 >// 调用传入的trajectory_builder的AddSensorData() void AddToTrajectoryBuilder( mapping::TrajectoryBuilderInterface *const trajectory_builder) override { trajectory_builder-AddSensorData(sensor_id_, data_); }七、结语如果大家觉得比较懵逼也没有关系后续了解了其他相关类之后到时候再进行复盘一下就比较比较简单了先来对该篇博客做个总结把。1):首先在开启一条新轨迹的时候会调用 src/cartographer/cartographer/mapping/map_builder.cc 中的 MapBuilder::AddTrajectoryBuilder() 函数该函数中会构建一个 CollatedTrajectoryBuilder 对象存储于 trajectory_builders_ 之中。2): src/cartographer_ros/cartographer_ros/cartographer_ros/map_builder_bridge.cc 文件中的MapBuilderBridge::AddTrajectory() 函数会把 SensorBridge 的实例与 CollatedTrajectoryBuilder 对象绑定在一起。3):src/cartographer_ros/cartographer_ros/cartographer_ros/node.cc 的 Node::LaunchSubscribers() 函数会订阅话题然后注册回调函数所有的回调函数都会执行一句类似 trajectory_builder_-AddSensorData() 的代码。4): 实际上 trajectory_builder_-AddSensorData() 就是调用的就是 CollatedTrajectoryBuilder::AddSensorData() 函数。CollatedTrajectoryBuilder::AddSensorData()又会调用 CollatedTrajectoryBuilder::AddData() 函数。5):CollatedTrajectoryBuilder::AddData() 实际上会调用到CollatedTrajectoryBuilder初始化时传入的 sensor::Collator sensor_collator_ 变量的 sensor_collator_-AddTrajectory() 函数。最终把数据传送给了 GlobalTrajectoryBuilder绕了一大堆总的来说初始注册的回调函数整理数据之后最终都会调用到 sensor::Collator::AddTrajectory()把数据传送给了 GlobalTrajectoryBuilder