node.cc中还注册了很多回调函数，以HandleOdometryMessage处理里程计信息函数为例，需要传入轨迹id，传感器id以及传感器的信息。首先还是对数据上锁，判断是否为暂停状态(是否使用数据)，如果Pulse函数不是暂停状态(使用所有数据)，返回true，则继续往下执行，若是暂停状态则返回，不对数据进行处理。
void Node::HandleOdometryMessage(const int trajectory_id,
const std::string& sensor_id,
const nav_msgs::Odometry::ConstPtr& msg) {
absl::MutexLock lock(&mutex_);
if (!sensor_samplers_.at(trajectory_id).odometry_sampler.Pulse()) {
return;
}
auto sensor_bridge_ptr = map_builder_bridge_.sensor_bridge(trajectory_id);
auto odometry_data_ptr = sensor_bridge_ptr->ToOdometryData(msg);
// extrapolators_使用里程计数据进行位姿预测
if (odometry_data_ptr != nullptr) {
extrapolators_.at(trajectory_id).AddOdometryData(*odometry_data_ptr);
}
sensor_bridge_ptr->HandleOdometryMessage(sensor_id, msg);
}通过map_builder_bridge_中的sensor_bridge函数传入轨迹id得到sensor_bridge_ptr指针，再通过这个指针去调用ToOdometryData()函数对ros格式的msg进行一个转换从而得到odometry_data_ptr，后文介绍了两种走向，第一种是传入PoseExtrapolator,用于位姿预测，第二种则是传入SensorBridge,使用其传感器处理函数进行里程计数据处理。剩下几类回调函数同理不过多介绍。其中map_builder_bridge_这个对象很重要，转到其定义处(node.h)：
MapBuilderBridge map_builder_bridge_ GUARDED_BY(mutex_);前一篇文章cartographer学习之node类_就不告诉你噢的博客-CSDN博客主要介绍了node类，它主要是实现Topic的订阅与发布提供Service。可以发现ROS系统与Cartographer内核之间的信息交换都是通过对象map_builder_bridge_来完成的。在对node类进行构造时也提到了这个对象：
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)转到构造函数(map_builder_bridge.cc)：
MapBuilderBridge::MapBuilderBridge(
const NodeOptions& node_options,
std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder,
tf2_ros::Buffer* const tf_buffer)
: node_options_(node_options),
map_builder_(std::move(map_builder)),
tf_buffer_(tf_buffer) {}首先构造这个MapBuilderBridge这个函数需要传入三个参数：node_options配置文件中的相关参数，通过node_main.cc传入进来的地图构建器map_builder以及tf_buffer监听缓存。接着就是对这三个传入的数据进行初始化，传入的三个参数在node_main.cc中已经构建好了。
一.AddTrajectory函数
map_builder_bridge_变量是node对象的一个成员，它的初始化需要调用AddTrajectory函数之后才能触发。AddTrajectory是map_builder_bridge_对象的成员函数，具体代码如下：
int MapBuilderBridge::AddTrajectory(
const std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>&
expected_sensor_ids,
const TrajectoryOptions& trajectory_options) {
// Step: 1 开始一条新的轨迹, 返回新轨迹的id,需要传入一个函数
const int trajectory_id = map_builder_->AddTrajectoryBuilder(
expected_sensor_ids, trajectory_options.trajectory_builder_options,
// lambda表达式 local_slam_result_callback_
[this](const int trajectory_id,
const ::cartographer::common::Time time,
const Rigid3d local_pose,
::cartographer::sensor::RangeData range_data_in_local,
const std::unique_ptr<
const ::cartographer::mapping::TrajectoryBuilderInterface::
InsertionResult>) {
// 保存local slam 的结果数据 5个参数实际只用了4个
OnLocalSlamResult(trajectory_id, time, local_pose, range_data_in_local);
});
LOG(INFO) << "Added trajectory with ID '" << trajectory_id << "'.";
// Make sure there is no trajectory with 'trajectory_id' yet.
CHECK_EQ(sensor_bridges_.count(trajectory_id), 0);
// Step: 2 为这个新轨迹 添加一个SensorBridge
sensor_bridges_[trajectory_id] = absl::make_unique<SensorBridge>(
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
// Step: 3 保存轨迹的参数配置
auto emplace_result =
trajectory_options_.emplace(trajectory_id, trajectory_options);
CHECK(emplace_result.second == true);
return trajectory_id;
}通过map_builder_对象添加一条新的轨迹，同时将构建成功的索引返回保存在局部变量trajectory_id中。 传入的三个参数分别为传感器id，轨迹跟踪器的一些配置信息以及一个回调函数OnLocalSlamResult(通过lamda表达式表示)。
1.OnLocalSlamResult函数
void MapBuilderBridge::OnLocalSlamResult(
const int trajectory_id, const ::cartographer::common::Time time,
const Rigid3d local_pose,
::cartographer::sensor::RangeData range_data_in_local) {
std::shared_ptr<const LocalTrajectoryData::LocalSlamData> local_slam_data =
std::make_shared<LocalTrajectoryData::LocalSlamData>(
LocalTrajectoryData::LocalSlamData{time, local_pose,
std::move(range_data_in_local)});
// 保存结果数据
absl::MutexLock lock(&mutex_);
local_slam_data_[trajectory_id] = std::move(local_slam_data);
}该函数构建需要四个参数。分别为轨迹id，更新子图的时间，前端的位置以及范围传感器中的数据，它有三个字段，origin描述了传感器的坐标，returns和misses都是点云数据，分别表示有物体反射和空闲区域。local_slam_data是MapBuilderBridge类中定义的结构体，用于记录局部SLAM反馈的状态。代码段如下：
struct LocalTrajectoryData {
// Contains the trajectory data received from local SLAM, after
// it had processed accumulated 'range_data_in_local' and estimated
// current 'local_pose' at 'time'.
// LocalSlamData中包含了local slam的一些数据, 包含当前时间, 当前估计的位姿, 以及累计的所有雷达数据
struct LocalSlamData {
::cartographer::common::Time time;
::cartographer::transform::Rigid3d local_pose;
::cartographer::sensor::RangeData range_data_in_local;
};
std::shared_ptr<const LocalSlamData> local_slam_data;
cartographer::transform::Rigid3d local_to_map; // local frame 到 global frame间的坐标变换
// published_frame 到 tracking_frame 间的坐标变换
std::unique_ptr<cartographer::transform::Rigid3d> published_to_tracking;
TrajectoryOptions trajectory_options;
// c++11: std::shared_ptr 主要的用途就是方便资源的管理, 自动释放没有指针引用的资源
// 使用引用计数来标识是否有其余指针指向该资源.(注意, shart_ptr本身指针会占1个引用)
// 引用计数是分配在动态分配的, std::shared_ptr支持拷贝, 新的指针获可以获取前引用计数个数
}; 最后设置互斥锁，并将数据记录到local_slam_data中。回到AddTrajectory函数：
LOG(INFO) << "Added trajectory with ID '" << trajectory_id << "'.";
// Make sure there is no trajectory with 'trajectory_id' yet.
CHECK_EQ(sensor_bridges_.count(trajectory_id), 0);
// Step: 2 为这个新轨迹 添加一个SensorBridge
sensor_bridges_[trajectory_id] = absl::make_unique<SensorBridge>(
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
// Step: 3 保存轨迹的参数配置
auto emplace_result =
trajectory_options_.emplace(trajectory_id, trajectory_options);
CHECK(emplace_result.second == true);
return trajectory_id;当map_builder_完成一个局部SLAM或者说是成功构建了一个子图的事件，打印信息。接着需要确保轨迹id没有重复，并为它创建一个SensorBridge对象。SensorBridge是cartographer_ros中对于传感器的一个封装， 用于将ROS的消息转换成Cartographer中的传感器数据类型。最后将轨迹相关的配置保存到trajectory_options_中并检查后，返回刚刚生成的轨迹id。
2.SensorBridge对象
class SensorBridge {
public:
explicit SensorBridge(
int num_subdivisions_per_laser_scan, const std::string& tracking_frame,
double lookup_transform_timeout_sec, tf2_ros::Buffer* tf_buffer,
::cartographer::mapping::TrajectoryBuilderInterface* trajectory_builder);
SensorBridge(const SensorBridge&) = delete;
SensorBridge& operator=(const SensorBridge&) = delete;
std::unique_ptr<::cartographer::sensor::OdometryData> ToOdometryData(
const nav_msgs::Odometry::ConstPtr& msg);
void HandleOdometryMessage(const std::string& sensor_id,
const nav_msgs::Odometry::ConstPtr& msg);
void HandleNavSatFixMessage(const std::string& sensor_id,
const sensor_msgs::NavSatFix::ConstPtr& msg);
void HandleLandmarkMessage(
const std::string& sensor_id,
const cartographer_ros_msgs::LandmarkList::ConstPtr& msg);
std::unique_ptr<::cartographer::sensor::ImuData> ToImuData(
const sensor_msgs::Imu::ConstPtr& msg);
void HandleImuMessage(const std::string& sensor_id,
const sensor_msgs::Imu::ConstPtr& msg);
void HandleLaserScanMessage(const std::string& sensor_id,
const sensor_msgs::LaserScan::ConstPtr& msg);
void HandleMultiEchoLaserScanMessage(
const std::string& sensor_id,
const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg);
void HandlePointCloud2Message(const std::string& sensor_id,
const sensor_msgs::PointCloud2::ConstPtr& msg);
const TfBridge& tf_bridge() const;
private:
void HandleLaserScan(
const std::string& sensor_id, ::cartographer::common::Time start_time,
const std::string& frame_id,
const ::cartographer::sensor::PointCloudWithIntensities& points);
void HandleRangefinder(const std::string& sensor_id,
::cartographer::common::Time time,
const std::string& frame_id,
const ::cartographer::sensor::TimedPointCloud& ranges);
const int num_subdivisions_per_laser_scan_;
std::map<std::string, cartographer::common::Time>
sensor_to_previous_subdivision_time_;
const TfBridge tf_bridge_;
::cartographer::mapping::TrajectoryBuilderInterface* const
trajectory_builder_;
absl::optional<::cartographer::transform::Rigid3d> ecef_to_local_frame_;
}; 这个对象需要五个参数，前面四个参数分别为激光传感器的分段数量，参考坐标系，tf坐标变换查询超时设置，tf坐标变换缓存， 以及轨迹构建器trajectory_builder，它是Cartographer的一个核心对象，通过sensor_bridge对象转换后的数据都是通过它喂给Cartographer的， 在map_builder_bridge_的成员函数AddTrajectory中通过下代码获得。
map_builder_->GetTrajectoryBuilder(trajectory_id)
427
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
