在阅读完cartographer_ros的主函数文件（参考上一篇）后，我们了解到，在主函数中其实最终重要的是进行node类对象的构造，读取配置node_options和创立map_builder都是为了传入node类的构造函数进行构造。所以本节内容将从node.h和node.cc文件进行突破。
一、node.h文件
个人看具体类代码的习惯是先统览类包含哪些变量和成员函数，然后再看具体实现方法。一个成熟的工程项目往往会将不同功能函数的声明与实现进行区分，前者通常为.h，后者通常为.cc文件。
// cartographer_ros命名空间
namespace cartographer_ros {
// node类的主要功用是作为中间介质将ROS的各类topic信息传入底层cartographer算法进行建图定位
class Node {
public:
// 构造函数，也就是前面node_main.cc文件里面调用的，参数分别为lua配置、指向MapBuilderInterface类的智能指针、tf监听参数、决定运行时度量集合是否开启的参数
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;
// 结束所有处于活动状态的轨迹
void FinishAllTrajectories();
// 结束指定编号的轨迹，若是成功则返回true，否则返回false
bool FinishTrajectory(int trajectory_id);
// 运行最后的全局优化，在调用此函数时确保所有路径都处于非活动状态
void RunFinalOptimization();
// 以默认的话题信息开始进行轨迹的更新
void StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options);
// 根据各自的配置参数信息从多个输入数据包中分离出各自的ROS topic数据，以容器的形式返回
std::vector<
std::set<::cartographer::mapping::TrajectoryBuilderInterface::SensorId>>
ComputeDefaultSensorIdsForMultipleBags(
const std::vector<TrajectoryOptions>& bags_options) const;
// Adds a trajectory for offline processing, i.e. not listening to topics.
// 离线状态（采用数据包的形式）下的轨迹更新
int AddOfflineTrajectory(
const std::set<
cartographer::mapping::TrajectoryBuilderInterface::SensorId>&
expected_sensor_ids,
const TrajectoryOptions& options);
// 下面各类函数就是监听到各自的传感器topic数据，分别采用的不同函数进行处理，处理后加入到轨迹中
// 处理里程计信息
void HandleOdometryMessage(int trajectory_id, const std::string& sensor_id,
const nav_msgs::Odometry::ConstPtr& msg);
// 处理GPS信息
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);
// 处理IMU信息
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);
// 写入完整的节点状态信息
void SerializeState(const std::string& filename,
const bool include_unfinished_submaps);
// 从pbstream文件中加载SLAM状态
void LoadState(const std::string& state_filename, bool load_frozen_state);
// ROS的句柄
::ros::NodeHandle* node_handle();
private:
// 自定义Subscriber结构体
struct Subscriber {
// ROS的订阅者
::ros::Subscriber subscriber;
// ROS的topic名
std::string topic;
};
// 下面的函数是上层cartographer_ros启用的各类服务处理函数，调用相应服务会进入对应函数进行处理
// 子图查阅服务
bool HandleSubmapQuery(
cartographer_ros_msgs::SubmapQuery::Request& request,
cartographer_ros_msgs::SubmapQuery::Response& response);
// 轨迹查询服务
bool HandleTrajectoryQuery(
::cartographer_ros_msgs::TrajectoryQuery::Request& request,
::cartographer_ros_msgs::TrajectoryQuery::Response& response);
// 开始更新轨迹服务（此类服务是开始建图的信号，由终端输入rosservice call后跟对应参数开启）
bool HandleStartTrajectory(
cartographer_ros_msgs::StartTrajectory::Request& request,
cartographer_ros_msgs::StartTrajectory::Response& response);
// 结束更新轨迹
bool HandleFinishTrajectory(
cartographer_ros_msgs::FinishTrajectory::Request& request,
cartographer_ros_msgs::FinishTrajectory::Response& response);
// 保存对应状态信息
bool HandleWriteState(cartographer_ros_msgs::WriteState::Request& request,
cartographer_ros_msgs::WriteState::Response& response);
// 获取轨迹状态信息
bool HandleGetTrajectoryStates(
::cartographer_ros_msgs::GetTrajectoryStates::Request& request,
::cartographer_ros_msgs::GetTrajectoryStates::Response& response);
// 获取度量信息（暂时还未弄明白这个度量是啥意思，后续更新吧。。。）
bool HandleReadMetrics(
cartographer_ros_msgs::ReadMetrics::Request& request,
cartographer_ros_msgs::ReadMetrics::Response& response);
// 根据配置参数信息从单输入数据包中分离出ROS topic信息，以集合的形式返回
std::set<::cartographer::mapping::TrajectoryBuilderInterface::SensorId>
ComputeExpectedSensorIds(const TrajectoryOptions& options) const;
// 增加路径
int AddTrajectory(const TrajectoryOptions& options);
// 各类话题的订阅
void LaunchSubscribers(const TrajectoryOptions& options, int trajectory_id);
// 发布子图
void PublishSubmapList(const ::ros::WallTimerEvent& timer_event);
// 位姿估计相关函数
void AddExtrapolator(int trajectory_id, const TrajectoryOptions& options);
// 传感器采样
void AddSensorSamplers(int trajectory_id, const TrajectoryOptions& options);
// 局部建图信息
void PublishLocalTrajectoryData(const ::ros::TimerEvent& timer_event);
// 发布路径点信息
void PublishTrajectoryNodeList(const ::ros::WallTimerEvent& timer_event);
// 发布地标位姿信息
void PublishLandmarkPosesList(const ::ros::WallTimerEvent& timer_event);
// 发布节点间约束信息
void PublishConstraintList(const ::ros::WallTimerEvent& timer_event);
// 判断配置是否符合规范
bool ValidateTrajectoryOptions(const TrajectoryOptions& options);
// 判断topic命名是否符合规范
bool ValidateTopicNames(const TrajectoryOptions& options);
cartographer_ros_msgs::StatusResponse FinishTrajectoryUnderLock(
int trajectory_id) EXCLUSIVE_LOCKS_REQUIRED(mutex_);
void MaybeWarnAboutTopicMismatch(const ::ros::WallTimerEvent&);
// 发布点云地图
void PublishPointCloudMap(const ::ros::WallTimerEvent& timer_event);
// 查询id对应轨迹状态函数
cartographer_ros_msgs::StatusResponse TrajectoryStateToStatus(
int trajectory_id,
const std::set<
cartographer::mapping::PoseGraphInterface::TrajectoryState>&
valid_states);
// 节点配置
const NodeOptions node_options_;
tf2_ros::TransformBroadcaster tf_broadcaster_;
absl::Mutex mutex_;
std::unique_ptr<cartographer_ros::metrics::FamilyFactory> metrics_registry_;
// c++11: GUARDED_BY是在Clang Thread Safety Analysis（线程安全分析）中定义的属性
// GUARDED_BY是数据成员的属性, 该属性声明数据成员受给定功能保护.
// 对数据的读操作需要共享访问, 而写操作则需要互斥访问.
MapBuilderBridge map_builder_bridge_ GUARDED_BY(mutex_);
::ros::NodeHandle node_handle_;
// 各类topic信息发布者
::ros::Publisher submap_list_publisher_;
::ros::Publisher trajectory_node_list_publisher_;
::ros::Publisher landmark_poses_list_publisher_;
::ros::Publisher constraint_list_publisher_;
::ros::Publisher tracked_pose_publisher_;
::ros::Publisher scan_matched_point_cloud_publisher_;
// 储存各类服务
std::vector<::ros::ServiceServer> service_servers_;
// 控制各个传感器数据的采样频率
struct TrajectorySensorSamplers {
TrajectorySensorSamplers(const double rangefinder_sampling_ratio,
const double odometry_sampling_ratio,
const double fixed_frame_pose_sampling_ratio,
const double imu_sampling_ratio,
const double landmark_sampling_ratio)
: rangefinder_sampler(rangefinder_sampling_ratio),
odometry_sampler(odometry_sampling_ratio),
fixed_frame_pose_sampler(fixed_frame_pose_sampling_ratio),
imu_sampler(imu_sampling_ratio),
landmark_sampler(landmark_sampling_ratio) {}
::cartographer::common::FixedRatioSampler rangefinder_sampler;
::cartographer::common::FixedRatioSampler odometry_sampler;
::cartographer::common::FixedRatioSampler fixed_frame_pose_sampler;
::cartographer::common::FixedRatioSampler imu_sampler;
::cartographer::common::FixedRatioSampler landmark_sampler;
};
// 根据对应id查找相应信息的各类容器
std::map<int, ::cartographer::mapping::PoseExtrapolator> extrapolators_;
std::map<int, ::ros::Time> last_published_tf_stamps_;
std::unordered_map<int, TrajectorySensorSamplers> sensor_samplers_;
std::unordered_map<int, std::vector<Subscriber>> subscribers_;
std::unordered_set<std::string> subscribed_topics_;
std::unordered_set<int> trajectories_scheduled_for_finish_;
std::vector<::ros::WallTimer> wall_timers_;
::ros::Timer publish_local_trajectory_data_timer_;
::ros::Publisher point_cloud_map_publisher_;
absl::Mutex point_cloud_map_mutex_;
bool load_state_ = false;
size_t last_trajectory_nodes_size_ = 0;
sensor_msgs::PointCloud2 ros_point_cloud_map_;
};
} // namespace cartographer_ros上面是node.h的全部内容，部分代码的含义已有相应的注释，针对node_main.cc里面调用的对应构造函数可能有人会问，为什么是指向MapBuilderInterface类的智能指针，而不是前面MapBuilder类呢？这里提前说明下，在底层的cartographer中，MapBuilderInterface属于MapBuilder的父类，MapBuilder继承自MapBuilderInterface，如下所示为map_builder.h文件内容：
// Wires up the complete SLAM stack with TrajectoryBuilders (for local submaps)
// and a PoseGraph for loop closure.
class MapBuilder : public MapBuilderInterface 二、node.cc文件
在看完node.h文件后，我们可以先对node类的作用有个笼统性的认识，主要功用包括：（1）提供各类服务；（2）接收传感器数据并进行处理；（3）进行SLAM轨迹更新、子图绘制、跟踪位姿、节点间约束关系并将其作为话题发布。下面是注释版node.cc文件，有需要可自行复制。然后我将挑一些重要的函数进行单独讲解，有不对的地方还望多多指正。
因为原来的node.cc文件顺序不是太符合个人习惯，所以下面我根据个人意见将其中函数按照功能简单的进行归类注释。
1.非类成员函数
namespace {
// 利用ROS句柄为指定id的路径订阅相应topic名称的数据，然后进行处理并返回相应的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,
boost::function<void(const typename MessageType::ConstPtr&)>(
[node, handler, trajectory_id,
topic](const typename MessageType::ConstPtr& msg) {
(node->*handler)(trajectory_id, topic, msg);
}));
}
// 路径状态显示函数
std::string TrajectoryStateToString(const TrajectoryState trajectory_state) {
switch (trajectory_state) {
case TrajectoryState::ACTIVE:
return "ACTIVE";
case TrajectoryState::FINISHED:
return "FINISHED";
case TrajectoryState::FROZEN:
return "FROZEN";
case TrajectoryState::DELETED:
return "DELETED";
}
return "";
}
} // namespace2.构造函数与析构函数
// 关键
// node类构造函数，主要作用有：声明ROS的相关topic的发布器, 服务的发布器, 以及将时间驱动的函数与定时器进行绑定
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) {//构造函数的主体
absl::MutexLock lock(&mutex_);//设置互斥锁
// 度量信息是否开启（这个度量暂时没弄清楚是有什么作用，等后续明白了再回过头来更新吧）
if (collect_metrics) {
metrics_registry_ = absl::make_unique<metrics::FamilyFactory>();
carto::metrics::RegisterAllMetrics(metrics_registry_.get());
}
// 1.声明需要发布的topic信息
// 发布子图集合
submap_list_publisher_ =
node_handle_.advertise<::cartographer_ros_msgs::SubmapList>(
kSubmapListTopic, kLatestOnlyPublisherQueueSize);//告知master节点,我们将要向kSubmapListTopic这个Topic上发布一个::cartographer_ros_msgs::SubmapList型的message,
//而第二个参数是publishing的缓存大小; 发布的该Topic即可允许其他节点获取到我们构建的Submap的信息。
// 发布轨迹（节点集合）
trajectory_node_list_publisher_ =
node_handle_.advertise<::visualization_msgs::MarkerArray>(
kTrajectoryNodeListTopic, kLatestOnlyPublisherQueueSize);//向kTrajectoryNodeListTopic 这个 Topic 上发布了一个::visualization_msgs::MarkerArray 型的 message
// 发布地标位姿集合
landmark_poses_list_publisher_ =
node_handle_.advertise<::visualization_msgs::MarkerArray>(
kLandmarkPosesListTopic, kLatestOnlyPublisherQueueSize);//向kLandmarkPosesListTopic 这个 Topic 上发布了一个::visualization_msgs::MarkerArray 型的 message
// 发布节点间约束集合
constraint_list_publisher_ =
node_handle_.advertise<::visualization_msgs::MarkerArray>(
kConstraintListTopic, kLatestOnlyPublisherQueueSize);//向kConstraintListTopic 这个 Topic 上发布了一个::visualization_msgs::MarkerArray 型的 message
// 根据lua文件内的参数决定是否发布跟踪位姿
if (node_options_.publish_tracked_pose) {
tracked_pose_publisher_ =
node_handle_.advertise<::geometry_msgs::PoseStamped>(
kTrackedPoseTopic, kLatestOnlyPublisherQueueSize);//kTrackedPoseTopic 这个 Topic 上发布了一个::geometry_msgs::PoseStamped 型的 message
}
// 2.声明提供的ROS的service服务，并将其放入名为service_servers_的vector容器中
service_servers_.push_back(node_handle_.advertiseService(
kSubmapQueryServiceName, &Node::HandleSubmapQuery, this));//注册一个 Service,Service 的名字由 kSubmapQueryServiceName 给出。
//第二个参数是该 Service 绑定的函数句柄,即当有一个 service的 request 时,由该函数进行 response.
//注册的这个 service 就对应了 submap_query 这个 service。这是 cartographer_node 可以提供的一个 service.
service_servers_.push_back(node_handle_.advertiseService(
kTrajectoryQueryServiceName, &Node::HandleTrajectoryQuery, this));//注册Service 名字为 kTrajectoryQueryServiceName,
//函数句柄为 Node::HandleTrajectoryQuery
service_servers_.push_back(node_handle_.advertiseService(
kStartTrajectoryServiceName, &Node::HandleStartTrajectory, this));//注册Service 名字为 kStartTrajectoryServiceName,
//函数句柄为 Node::HandleStartTrajectory
service_servers_.push_back(node_handle_.advertiseService(
kFinishTrajectoryServiceName, &Node::HandleFinishTrajectory, this));//注册Service 名字为 kFinishTrajectoryServiceName,
//函数句柄为 Node::HandleFinishTrajectory
service_servers_.push_back(node_handle_.advertiseService(
kWriteStateServiceName, &Node::HandleWriteState, this));//注册Service 名字为 kWriteStateServiceName,
//函数句柄为 Node::HandleWriteState
service_servers_.push_back(node_handle_.advertiseService(
kGetTrajectoryStatesServiceName, &Node::HandleGetTrajectoryStates, this));//注册Service 名字为 kGetTrajectoryStatesServiceName,
//函数句柄为 Node::HandleGetTrajectoryStates
service_servers_.push_back(node_handle_.advertiseService(
kReadMetricsServiceName, &Node::HandleReadMetrics, this));//注册Service 名字为 kReadMetricsServiceName,
//函数句柄为 Node::HandleReadMetrics
// 3.经过处理后的点云信息的发布器
scan_matched_point_cloud_publisher_ =
node_handle_.advertise<sensor_msgs::PointCloud2>(
kScanMatchedPointCloudTopic, kLatestOnlyPublisherQueueSize);//发布了一个跟点云相关的 Topic
// 4.进行定时器与函数的绑定, 定时发布对应topic数据
//wall_timers 在 node.h 中定义,是一个存储::ros::WallTimer 类型的 vector, 以下通过 vector 的push_back 操作依次将五个::ros::WallTimer 型对象插入这个 vector 的末尾。
//简单说,这是一个定时器,这里分别为如下的五个函数设置了定时器。参数就是 node_options 里的各项参数;
wall_timers_.push_back(node_handle_.createWallTimer(
::ros::WallDuration(node_options_.submap_publish_period_sec),
&Node::PublishSubmapList, this));//调用 map_builder_bridge_.GetSubmapList()函数获取到 submap 的 list 然后用ros 的 publish 函数向 Topic 上广播这个消息。
if (node_options_.pose_publish_period_sec > 0) {
publish_local_trajectory_data_timer_ = node_handle_.createTimer(
::ros::Duration(node_options_.pose_publish_period_sec),
&Node::PublishLocalTrajectoryData, this);//调用map_builder_bridge_.GetLocalTrajectoryData()函数获取数据，然后用ros 的 publish 函数向 Topic 上广播这个消息。
}
wall_timers_.push_back(node_handle_.createWallTimer(
::ros::WallDuration(node_options_.trajectory_publish_period_sec),
&Node::PublishTrajectoryNodeList, this));//调用map_builder_bridge_.GetTrajectoryNodeList()函数获取数据，然后用ros 的 publish 函数向 Topic 上广播这个消息。
wall_timers_.push_back(node_handle_.createWallTimer(
::ros::WallDuration(node_options_.trajectory_publish_period_sec),
&Node::PublishLandmarkPosesList, this));//调用map_builder_bridge_.GetLandmarkPosesList()函数获取数据，然后用ros 的 publish 函数向 Topic 上广播这个消息。
wall_timers_.push_back(node_handle_.createWallTimer(
::ros::WallDuration(kConstraintPublishPeriodSec),
&Node::PublishConstraintList, this));//调用map_builder_bridge_.GetConstraintList()函数获取数据，然后用ros 的 publish 函数向 Topic 上广播这个消息。
}
// 析构函数，结束所有处于活动状态下的路径
Node::~Node() { FinishAllTrajectories(); }3.各类功用函数
（1）处理ROS服务
::ros::NodeHandle* Node::node_handle() { return &node_handle_; }
//处理SubmapQuery查询服务，通过ROS服务request所要求的轨迹id和子图索引submap_index来查询相应的子图
bool Node::HandleSubmapQuery(
::cartographer_ros_msgs::SubmapQuery::Request& request,
::cartographer_ros_msgs::SubmapQuery::Response& response) {
absl::MutexLock lock(&mutex_);//设置互斥锁，避免多个线程同时对该服务进行调用导致程序混乱，一旦某个线程调用，先执行关锁服务，然后进行读写操作，退出时执行开锁服务
//可以看到,这个函数有两个参数,一个参数是一个::cartographer_ros_msgs::SubmapQuery::Request 型的变量,对应是请求服务时的输入参数,
//另外一个参数是::cartographer_ros_msgs::SubmapQuery::Response 型的变量,对应的是服务响应后的返回值。
map_builder_bridge_.HandleSubmapQuery(request, response);//MapBuilderBridge 型的变量调用HandleSubmapQuery函数进行处理，ROS的上层封装是采用MapBuilderBridge跳转到底层的MapBuilder
return true;
}
//处理TrajectoryQuery查询服务，通过request指定的id来查询
bool Node::HandleTrajectoryQuery(
::cartographer_ros_msgs::TrajectoryQuery::Request& request,
::cartographer_ros_msgs::TrajectoryQuery::Response& response) {
absl::MutexLock lock(&mutex_);//设置互斥锁
// 检查对应id的轨迹是否存在, 如果存在判断一下该id轨迹的状态
response.status = TrajectoryStateToStatus(
request.trajectory_id,
{TrajectoryState::ACTIVE, TrajectoryState::FINISHED,
TrajectoryState::FROZEN} /* valid states */);
// 异常处理
if (response.status.code != cartographer_ros_msgs::StatusCode::OK) {
LOG(ERROR) << "Can't query trajectory from pose graph: "
<< response.status.message;
return true;
}
// 获取轨迹
map_builder_bridge_.HandleTrajectoryQuery(request, response);
return true;
}
// 通过服务来开始一条新的轨迹
// 处理StartTrajectory查询服务
bool Node::HandleStartTrajectory(
::cartographer_ros_msgs::StartTrajectory::Request& request,
::cartographer_ros_msgs::StartTrajectory::Response& response) {
TrajectoryOptions trajectory_options;
// 获取配置文件内容
std::tie(std::ignore, trajectory_options) = LoadOptions(
request.configuration_directory, request.configuration_basename);
// 如果给定了一个初始位姿
if (request.use_initial_pose) {
const auto pose = ToRigid3d(request.initial_pose);
if (!pose.IsValid()) {
response.status.message =
"Invalid pose argument. Orientation quaternion must be normalized.";
LOG(ERROR) << response.status.message;
response.status.code =
cartographer_ros_msgs::StatusCode::INVALID_ARGUMENT;
return true;
}
// Check if the requested trajectory for the relative initial pose exists.
// 检查 initial_pose 对应的轨迹id是否存在
response.status = TrajectoryStateToStatus(
request.relative_to_trajectory_id,
{TrajectoryState::ACTIVE, TrajectoryState::FROZEN,
TrajectoryState::FINISHED} /* valid states */);
if (response.status.code != cartographer_ros_msgs::StatusCode::OK) {
LOG(ERROR) << "Can't start a trajectory with initial pose: "
<< response.status.message;
return true;
}
::cartographer::mapping::proto::InitialTrajectoryPose
initial_trajectory_pose;
initial_trajectory_pose.set_to_trajectory_id(
request.relative_to_trajectory_id);
// 将pose转成proto格式,放进initial_trajectory_pose
*initial_trajectory_pose.mutable_relative_pose() =
cartographer::transform::ToProto(pose);
initial_trajectory_pose.set_timestamp(cartographer::common::ToUniversal(
::cartographer_ros::FromRos(ros::Time(0))));
// 将初始位姿信息加入到trajectory_options中
*trajectory_options.trajectory_builder_options
.mutable_initial_trajectory_pose() = initial_trajectory_pose;
}
//前面一些异常情况的处理,正常情况下调用 AddTrajectory 函数,增加一条 trajectory.
// 检查TrajectoryOptions是否存在2D或者3D轨迹的配置信息
if (!ValidateTrajectoryOptions(trajectory_options)) {
response.status.message = "Invalid trajectory options.";
LOG(ERROR) << response.status.message;
response.status.code = cartographer_ros_msgs::StatusCode::INVALID_ARGUMENT;
}
// 检查topic名字是否被其他轨迹使用
else if (!ValidateTopicNames(trajectory_options)) {
response.status.message = "Topics are already used by another trajectory.";
LOG(ERROR) << response.status.message;
response.status.code = cartographer_ros_msgs::StatusCode::INVALID_ARGUMENT;
}
// 检查通过, 添加一个新的轨迹
else {
response.status.message = "Success.";
response.trajectory_id = AddTrajectory(trajectory_options);
response.status.code = cartographer_ros_msgs::StatusCode::OK;
}
return true;
}（2）ROS topic相关
// 发布子图数据，包括节点的id和当前submap的节点数
void Node::PublishSubmapList(const ::ros::WallTimerEvent& unused_timer_event) {
absl::MutexLock lock(&mutex_);// 互斥锁
submap_list_publisher_.publish(map_builder_bridge_.GetSubmapList());// 通过map_builder_bridge_链接到底层的map_builder获取子图信息并进行发布
}
// 每5e-3s发布一次tf变换信息和跟踪位姿信息
void Node::PublishLocalTrajectoryData(const ::ros::TimerEvent& timer_event) {
absl::MutexLock lock(&mutex_);
for (const auto& entry : map_builder_bridge_.GetLocalTrajectoryData()) {
// entry的数据类型为std::unordered_map<int,MapBuilderBridge::LocalTrajectoryData>
// entry.first 就是轨迹的id, entry.second 就是 LocalTrajectoryData
// 获取对应轨迹id的trajectory_data
const auto& trajectory_data = entry.second;
// 获取对应轨迹id的extrapolator
auto& extrapolator = extrapolators_.at(entry.first);
// We only publish a point cloud if it has changed. It is not needed at high
// frequency, and republishing it would be computationally wasteful.
// 如果当前状态的时间与extrapolator的lastposetime（上次结束时间）不等
if (trajectory_data.local_slam_data->time !=
extrapolator.GetLastPoseTime()) {
// 有订阅才发布scan_matched_point_cloud（帧与子图的匹配）
if (scan_matched_point_cloud_publisher_.getNumSubscribers() > 0) {
// TODO(gaschler): Consider using other message without time
// information.
carto::sensor::TimedPointCloud point_cloud;
point_cloud.reserve(trajectory_data.local_slam_data->range_data_in_local
.returns.size());
// 获取local_slam_data的点云数据, 填入到point_cloud中
for (const cartographer::sensor::RangefinderPoint point :
trajectory_data.local_slam_data->range_data_in_local.returns) {
// 这里的虽然使用的是带时间戳的点云结构, 但是数据点的时间全是0.f
point_cloud.push_back(cartographer::sensor::ToTimedRangefinderPoint(
point, 0.f /* time */));
}
// 先将点云转换成ROS的格式,再发布scan_matched_point_cloud点云
scan_matched_point_cloud_publisher_.publish(ToPointCloud2Message(
carto::common::ToUniversal(trajectory_data.local_slam_data->time),
node_options_.map_frame,
// 将雷达坐标系下的点云转换成地图坐标系下的点云
carto::sensor::TransformTimedPointCloud(
point_cloud, trajectory_data.local_to_map.cast<float>())));
}
// 将当前的pose加入到extrapolator中, 更新extrapolator的时间与状态
extrapolator.AddPose(trajectory_data.local_slam_data->time,
trajectory_data.local_slam_data->local_pose);
}
geometry_msgs::TransformStamped stamped_transform;
// If we do not publish a new point cloud, we still allow time of the
// published poses to advance. If we already know a newer pose, we use its
// time instead. Since tf knows how to interpolate, providing newer
// information is better.
// 使用较新的时间戳
const ::cartographer::common::Time now = std::max(
FromRos(ros::Time::now()), extrapolator.GetLastExtrapolatedTime());
stamped_transform.header.stamp =
node_options_.use_pose_extrapolator
? ToRos(now)
: ToRos(trajectory_data.local_slam_data->time);
// Suppress publishing if we already published a transform at this time.
// Due to 2020-07 changes to geometry2, tf buffer will issue warnings for
// repeated transforms with the same timestamp.
if (last_published_tf_stamps_.count(entry.first) &&
last_published_tf_stamps_[entry.first] == stamped_transform.header.stamp)
continue;
// 保存当前的时间戳, 以防止对同一时间戳进行重复更新
last_published_tf_stamps_[entry.first] = stamped_transform.header.stamp;
// 获取局部坐标系下的3D变换
const Rigid3d tracking_to_local_3d =
node_options_.use_pose_extrapolator
? extrapolator.ExtrapolatePose(now)
: trajectory_data.local_slam_data->local_pose;
// 获取当前位姿在局部坐标系下的坐标
const Rigid3d tracking_to_local = [&] {
// 根据参数配置决定是否将变换投影到2D平面上
if (trajectory_data.trajectory_options.publish_frame_projected_to_2d) {
return carto::transform::Embed3D(
carto::transform::Project2D(tracking_to_local_3d));
}
return tracking_to_local_3d;
}();
// 求得当前位姿在map下的坐标
const Rigid3d tracking_to_map =
trajectory_data.local_to_map * tracking_to_local;
// 根据lua配置文件发布tf
if (trajectory_data.published_to_tracking != nullptr) {
if (node_options_.publish_to_tf) {
if (trajectory_data.trajectory_options.provide_odom_frame) {
// 如果需要cartographer提供odom坐标系
// 则发布 map_frame -> odom -> published_frame 的tf
std::vector<geometry_msgs::TransformStamped> stamped_transforms;
// map_frame -> odom
stamped_transform.header.frame_id = node_options_.map_frame;
stamped_transform.child_frame_id =
trajectory_data.trajectory_options.odom_frame;
// 将local坐标系作为odom坐标系
stamped_transform.transform =
ToGeometryMsgTransform(trajectory_data.local_to_map);
stamped_transforms.push_back(stamped_transform);
// odom -> published_frame
stamped_transform.header.frame_id =
trajectory_data.trajectory_options.odom_frame;
stamped_transform.child_frame_id =
trajectory_data.trajectory_options.published_frame;
// published_to_tracking 是局部坐标系下的位姿
stamped_transform.transform = ToGeometryMsgTransform(
tracking_to_local * (*trajectory_data.published_to_tracking));
stamped_transforms.push_back(stamped_transform);
// 发布 map_frame -> odom -> published_frame 的tf
tf_broadcaster_.sendTransform(stamped_transforms);
// cartographer在不需要提供odom坐标系的情况下,则发布 map_frame -> published_frame 的tf
} else {
stamped_transform.header.frame_id = node_options_.map_frame;
stamped_transform.child_frame_id =
trajectory_data.trajectory_options.published_frame;
stamped_transform.transform = ToGeometryMsgTransform(
tracking_to_map * (*trajectory_data.published_to_tracking));
// 发布 map_frame -> published_frame 的tf
tf_broadcaster_.sendTransform(stamped_transform);
}
}
// publish_tracked_pose 默认为false, 默认不发布
// 如果设置为true, 就发布一个在tracking_frame处的pose
if (node_options_.publish_tracked_pose) {
::geometry_msgs::PoseStamped pose_msg;
pose_msg.header.frame_id = node_options_.map_frame;
pose_msg.header.stamp = stamped_transform.header.stamp;
pose_msg.pose = ToGeometryMsgPose(tracking_to_map);
tracked_pose_publisher_.publish(pose_msg);
}
}
}
}
// 每30e-3s发布一次轨迹路径点数据
void Node::PublishTrajectoryNodeList(
const ::ros::WallTimerEvent& unused_timer_event) {
// 只有存在订阅者的时候才发布轨迹
if (trajectory_node_list_publisher_.getNumSubscribers() > 0) {
absl::MutexLock lock(&mutex_);
trajectory_node_list_publisher_.publish(
map_builder_bridge_.GetTrajectoryNodeList());
}
}
// 每30e-3s发布一次landmark pose 数据
void Node::PublishLandmarkPosesList(
const ::ros::WallTimerEvent& unused_timer_event) {
if (landmark_poses_list_publisher_.getNumSubscribers() > 0) {
absl::MutexLock lock(&mutex_);
landmark_poses_list_publisher_.publish(
map_builder_bridge_.GetLandmarkPosesList());
}
}
// 每0.5s发布一次约束数据
void Node::PublishConstraintList(
const ::ros::WallTimerEvent& unused_timer_event) {
if (constraint_list_publisher_.getNumSubscribers() > 0) {
absl::MutexLock lock(&mutex_);
constraint_list_publisher_.publish(map_builder_bridge_.GetConstraintList());
}
}
// 订阅话题与注册回调函数
// 根据设置参数来订阅传感器发布的相应话题，可以看到有 Laser,MultiEchoLaser, PointCloud2, IMU, Odometry, NavSatFixMessage, Landmark 等,
// 其中后三项是用 if来判断该传感器使用情况。所以不同的应用场景需要不同地修改配置文件。
void Node::LaunchSubscribers(const TrajectoryOptions& options,
const int trajectory_id) {
// 主要作用就是订阅一个以 topic(不同的传感器中的 topic 这个变量是否 for 循环体中的这一句代码赋值的
// const std::string& topic : ComputeRepeatedTopicNames(topics.laser_scan_topic,options.num_laser_scans) )为名字的 Topic,
// 然后返回了一个node_handle->subscribe<MessageType>。在 LaunchSubscribers 函数里把这个返回值压入了subscribers_[trajectory_id]列表中。
// 订阅之后的处理是在 Node::HandleLaserScanMessage,查看该代码就可以发现最后依然交给了map_builder_bridge_去处理。
// 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 的订阅与注册回调函数（3D必须要有，2D可有可无）
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 的订阅与注册回调函数
if (options.use_odometry) {
subscribers_[trajectory_id].push_back(
{SubscribeWithHandler<nav_msgs::Odometry>(&Node::HandleOdometryMessage,
trajectory_id, kOdometryTopic,
&node_handle_, this),
kOdometryTopic});
}
// GPS的订阅与注册回调函数
if (options.use_nav_sat) {
subscribers_[trajectory_id].push_back(
{SubscribeWithHandler<sensor_msgs::NavSatFix>(
&Node::HandleNavSatFixMessage, trajectory_id, kNavSatFixTopic,
&node_handle_, this),
kNavSatFixTopic});
}
// landmarks的订阅与注册回调函数
if (options.use_landmarks) {
subscribers_[trajectory_id].push_back(
{SubscribeWithHandler<cartographer_ros_msgs::LandmarkList>(
&Node::HandleLandmarkMessage, trajectory_id, kLandmarkTopic,
&node_handle_, this),
kLandmarkTopic});
}
}
// 从多个包中获取默认topic
std::vector<
std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>>
Node::ComputeDefaultSensorIdsForMultipleBags(
const std::vector<TrajectoryOptions>& bags_options) const {
using SensorId = cartographer::mapping::TrajectoryBuilderInterface::SensorId;
std::vector<std::set<SensorId>> bags_sensor_ids;
for (size_t i = 0; i < bags_options.size(); ++i) {
std::string prefix;
if (bags_options.size() > 1) {
prefix = "bag_" + std::to_string(i + 1) + "_";
}
std::set<SensorId> unique_sensor_ids;
for (const auto& sensor_id : ComputeExpectedSensorIds(bags_options.at(i))) {
unique_sensor_ids.insert(SensorId{sensor_id.type, prefix + sensor_id.id});
}
bags_sensor_ids.push_back(unique_sensor_ids);
}
return bags_sensor_ids;
}
// 检查设置的topic名字是否在ros中存在, 不存在则报错
void Node::MaybeWarnAboutTopicMismatch(
const ::ros::WallTimerEvent& unused_timer_event) {
//使用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();
}
}（3）SLAM建图定位
// 新增位姿估计器
void Node::AddExtrapolator(const int trajectory_id,
const TrajectoryOptions& options) {
constexpr double kExtrapolationEstimationTimeSec = 0.001; // 1 ms
// 新增的轨迹id不应在已有的位姿估计器容器中，检查是否存在该异常
CHECK(extrapolators_.count(trajectory_id) == 0);
// 根据map_builder.lua内是否采用3d配置来获取配置的imu_gravity_time_constant数据
// 但在默认配置中，即trajectory_builder_3d.lua和trajectory_builder_2d.lua文件中，该项数据皆为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();
// 以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));
}
// 新生成一个传感器数据采样器
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));
}
// 添加新轨迹
// 同样,AddTrajectory 函数也是通过调用 map_builder_bridge_中的 AddTrajectory 来处理。
// 同时,每增加一条轨迹,都需要给该轨迹增加必要的处理,比如添加位姿估计的 AddExtrapolator,
// 设置传感器的 AddSensorSamplers,用来订阅必要的 Topic 以接收数据的 LaunchSubscribers 等。
int Node::AddTrajectory(const TrajectoryOptions& options) {
const std::set<cartographer::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);
// 新生成一个传感器数据采样器
AddSensorSamplers(trajectory_id, options);
// 订阅话题与注册回调函数
LaunchSubscribers(options, trajectory_id);
// 创建了一个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;
}
// 结束指定id的轨迹
// 前面检查了一下是否可以关掉,指定 id 是否存在,是否已经被 Finished 了等情况后,如果一切正常,则停止订阅 Topic、清除 id 及其他与该 trajectory 相关的量。
// 最后调用 map_builder_bridge_中的FinishTrajectory 函数。
cartographer_ros_msgs::StatusResponse Node::FinishTrajectoryUnderLock(
const int trajectory_id) {
cartographer_ros_msgs::StatusResponse status_response;
// 1.对是否可以结束相应路径进行判断，并进行异常处理
if (trajectories_scheduled_for_finish_.count(trajectory_id)) {
status_response.message = absl::StrCat("Trajectory ", trajectory_id,
" already pending to finish.");
status_response.code = cartographer_ros_msgs::StatusCode::OK;
LOG(INFO) << status_response.message;
return status_response;
}
// First, check if we can actually finish the trajectory.
// 2. 检查这个轨迹是否存在, 如果存在则检查这个轨迹是否是ACTIVE状态
status_response = TrajectoryStateToStatus(
trajectory_id, {TrajectoryState::ACTIVE} /* valid states */);
// 如果不是OK状态就返回ERROR
if (status_response.code != cartographer_ros_msgs::StatusCode::OK) {
LOG(ERROR) << "Can't finish trajectory: " << status_response.message;
return status_response;
}
// Shutdown the subscribers of this trajectory.
// A valid case with no subscribers is e.g. if we just visualize states.
// 3.如果这个轨迹存在subscribers, 则先关闭subscriber
if (subscribers_.count(trajectory_id)) {
for (auto& entry : subscribers_[trajectory_id]) {
entry.subscriber.shutdown();
subscribed_topics_.erase(entry.topic);
LOG(INFO) << "Shutdown the subscriber of [" << entry.topic << "]";
}
// 在subscribers_中将这条轨迹的信息删除
CHECK_EQ(subscribers_.erase(trajectory_id), 1);
}
// 4.调用cartographer中的map_builder的FinishTrajectory()进行轨迹的结束
map_builder_bridge_.FinishTrajectory(trajectory_id);
// 将这个轨迹id放进正在结束的轨迹集合中
trajectories_scheduled_for_finish_.emplace(trajectory_id);
status_response.message =
absl::StrCat("Finished trajectory ", trajectory_id, ".");
status_response.code = cartographer_ros_msgs::StatusCode::OK;
return status_response;
}
// 使用默认topic名字开始一条轨迹,也就是开始SLAM建图和定位
void Node::StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options) {
absl::MutexLock lock(&mutex_);
// 检查TrajectoryOptions是否存在2D或者3D轨迹的配置信息
CHECK(ValidateTrajectoryOptions(options));
// 添加一条轨迹
AddTrajectory(options);
}
// 离线下增加轨迹
int Node::AddOfflineTrajectory(
const std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>&
expected_sensor_ids,
const TrajectoryOptions& options) {
absl::MutexLock lock(&mutex_);
const int trajectory_id =
map_builder_bridge_.AddTrajectory(expected_sensor_ids, options);
AddExtrapolator(trajectory_id, options);
AddSensorSamplers(trajectory_id, options);
return trajectory_id;
}
// 获取所有轨迹的状态
bool Node::HandleGetTrajectoryStates(
::cartographer_ros_msgs::GetTrajectoryStates::Request& request,
::cartographer_ros_msgs::GetTrajectoryStates::Response& response) {
using TrajectoryState =
::cartographer::mapping::PoseGraphInterface::TrajectoryState;
absl::MutexLock lock(&mutex_);
response.status.code = ::cartographer_ros_msgs::StatusCode::OK;
response.trajectory_states.header.stamp = ros::Time::now();
for (const auto& entry : map_builder_bridge_.GetTrajectoryStates()) {
// 轨迹的id
response.trajectory_states.trajectory_id.push_back(entry.first);
// 每个轨迹对应一个TrajectoryStates
switch (entry.second) {
case TrajectoryState::ACTIVE:
response.trajectory_states.trajectory_state.push_back(
::cartographer_ros_msgs::TrajectoryStates::ACTIVE);
break;
case TrajectoryState::FINISHED:
response.trajectory_states.trajectory_state.push_back(
::cartographer_ros_msgs::TrajectoryStates::FINISHED);
break;
case TrajectoryState::FROZEN:
response.trajectory_states.trajectory_state.push_back(
::cartographer_ros_msgs::TrajectoryStates::FROZEN);
break;
case TrajectoryState::DELETED:
response.trajectory_states.trajectory_state.push_back(
::cartographer_ros_msgs::TrajectoryStates::DELETED);
break;
}
}
return true;
}
// 结束一条轨迹
bool Node::HandleFinishTrajectory(
::cartographer_ros_msgs::FinishTrajectory::Request& request,
::cartographer_ros_msgs::FinishTrajectory::Response& response) {
absl::MutexLock lock(&mutex_);
response.status = FinishTrajectoryUnderLock(request.trajectory_id);
return true;
}
// 当前状态序列化为proto流文件.如果将'include_unfinished_submaps'设置为true，
// 则未完成的子图（即尚未接收到所有测距仪数据插入的子图）将包含在序列化状态中
bool Node::HandleWriteState(
::cartographer_ros_msgs::WriteState::Request& request,
::cartographer_ros_msgs::WriteState::Response& response) {
absl::MutexLock lock(&mutex_);
//通过map_builder_bridge_调用map_builder_的SerializeStateToFile()函数来进行文件的保存
if (map_builder_bridge_.SerializeState(request.filename,
request.include_unfinished_submaps)) {
response.status.code = cartographer_ros_msgs::StatusCode::OK;
response.status.message =
absl::StrCat("State written to '", request.filename, "'.");
} else {
response.status.code = cartographer_ros_msgs::StatusCode::INVALID_ARGUMENT;
response.status.message =
absl::StrCat("Failed to write '", request.filename, "'.");
}
return true;
}
// 读取metrics
bool Node::HandleReadMetrics(
::cartographer_ros_msgs::ReadMetrics::Request& request,
::cartographer_ros_msgs::ReadMetrics::Response& response) {
absl::MutexLock lock(&mutex_);
response.timestamp = ros::Time::now();
if (!metrics_registry_) {
response.status.code = cartographer_ros_msgs::StatusCode::UNAVAILABLE;
response.status.message = "Collection of runtime metrics is not activated.";
return true;
}
metrics_registry_->ReadMetrics(&response);
response.status.code = cartographer_ros_msgs::StatusCode::OK;
response.status.message = "Successfully read metrics.";
return true;
}
// 结束所有处于活动状态的轨迹
void Node::FinishAllTrajectories() {
absl::MutexLock lock(&mutex_);
for (const auto& entry : map_builder_bridge_.GetTrajectoryStates()) {
if (entry.second == TrajectoryState::ACTIVE) {
const int trajectory_id = entry.first;
CHECK_EQ(FinishTrajectoryUnderLock(trajectory_id).code,
cartographer_ros_msgs::StatusCode::OK);
}
}
}
// 结束指定id的轨迹
bool Node::FinishTrajectory(const int trajectory_id) {
absl::MutexLock lock(&mutex_);
return FinishTrajectoryUnderLock(trajectory_id).code ==
cartographer_ros_msgs::StatusCode::OK;
}
// 当所有的轨迹结束时, 执行一次全局优化
void Node::RunFinalOptimization() {
{
for (const auto& entry : map_builder_bridge_.GetTrajectoryStates()) {
const int trajectory_id = entry.first;
if (entry.second == TrajectoryState::ACTIVE) {
LOG(WARNING)
<< "Can't run final optimization if there are one or more active "
"trajectories. Trying to finish trajectory with ID "
<< std::to_string(trajectory_id) << " now.";
CHECK(FinishTrajectory(trajectory_id))
<< "Failed to finish trajectory with ID "
<< std::to_string(trajectory_id) << ".";
}
}
}
// Assuming we are not adding new data anymore, the final optimization
// can be performed without holding the mutex.
map_builder_bridge_.RunFinalOptimization();
}
void Node::PublishPointCloudMap(const ::ros::WallTimerEvent& timer_event) {
// 纯定位时不发布点云地图
if (load_state_ || point_cloud_map_publisher_.getNumSubscribers() == 0) {
return;
}
// 只发布轨迹id 0 的点云地图
constexpr int trajectory_id = 0;
// 获取优化后的节点位姿与节点的点云数据
std::shared_ptr<MapById<NodeId, TrajectoryNode>> trajectory_nodes =
map_builder_bridge_.GetTrajectoryNodes();
// 如果个数没变就不进行地图发布
size_t trajectory_nodes_size = trajectory_nodes->SizeOfTrajectoryOrZero(trajectory_id);
if (last_trajectory_nodes_size_ == trajectory_nodes_size)
return;
last_trajectory_nodes_size_ = trajectory_nodes_size;
absl::MutexLock lock(&point_cloud_map_mutex_);
pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud_map(new pcl::PointCloud<pcl::PointXYZ>());
pcl::PointCloud<pcl::PointXYZ>::Ptr node_point_cloud(new pcl::PointCloud<pcl::PointXYZ>());
// 遍历轨迹0的所有优化后的节点
auto node_it = trajectory_nodes->BeginOfTrajectory(trajectory_id);
auto end_it = trajectory_nodes->EndOfTrajectory(trajectory_id);
for (; node_it != end_it; ++node_it) {
auto& trajectory_node = trajectory_nodes->at(node_it->id);
auto& high_resolution_point_cloud = trajectory_node.constant_data->high_resolution_point_cloud;
auto& global_pose = trajectory_node.global_pose;
if (trajectory_node.constant_data != nullptr) {
node_point_cloud->clear();
node_point_cloud->resize(high_resolution_point_cloud.size());
// 遍历点云的每一个点, 进行坐标变换
for (const RangefinderPoint& point :
high_resolution_point_cloud.points()) {
RangefinderPoint range_finder_point = global_pose.cast<float>() * point;
node_point_cloud->push_back(pcl::PointXYZ(
range_finder_point.position.x(), range_finder_point.position.y(),
range_finder_point.position.z()));
}
// 将每个节点的点云组合在一起
*point_cloud_map += *node_point_cloud;
}
} // end for
ros_point_cloud_map_.data.clear();
pcl::toROSMsg(*point_cloud_map, ros_point_cloud_map_);
ros_point_cloud_map_.header.stamp = ros::Time::now();
ros_point_cloud_map_.header.frame_id = node_options_.map_frame;
LOG(INFO) << "publish point cloud map";
point_cloud_map_publisher_.publish(ros_point_cloud_map_);
}（4）传感器数据处理函数
// 处理里程计数据,里程计的数据走向有2个
// 第1个是传入PoseExtrapolator,用于位姿预测
// 第2个是传入SensorBridge,使用其传感器处理函数进行里程计数据处理
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);
}
// 调用SensorBridge的传感器处理函数进行GPS数据处理
void Node::HandleNavSatFixMessage(const int trajectory_id,
const std::string& sensor_id,
const sensor_msgs::NavSatFix::ConstPtr& msg) {
absl::MutexLock lock(&mutex_);
if (!sensor_samplers_.at(trajectory_id).fixed_frame_pose_sampler.Pulse()) {
return;
}
map_builder_bridge_.sensor_bridge(trajectory_id)
->HandleNavSatFixMessage(sensor_id, msg);
}
// 调用SensorBridge的传感器处理函数进行地标数据处理
void Node::HandleLandmarkMessage(
const int trajectory_id, const std::string& sensor_id,
const cartographer_ros_msgs::LandmarkList::ConstPtr& msg) {
absl::MutexLock lock(&mutex_);
if (!sensor_samplers_.at(trajectory_id).landmark_sampler.Pulse()) {
return;
}
map_builder_bridge_.sensor_bridge(trajectory_id)
->HandleLandmarkMessage(sensor_id, msg);
}
// 处理IMU数据,IMU的数据走向有2个
// 第1个是传入PoseExtrapolator,用于位姿预测与重力方向的确定
// 第2个是传入SensorBridge,使用其传感器处理函数进行IMU数据处理
void Node::HandleImuMessage(const int trajectory_id,
const std::string& sensor_id,
const sensor_msgs::Imu::ConstPtr& msg) {
absl::MutexLock lock(&mutex_);
if (!sensor_samplers_.at(trajectory_id).imu_sampler.Pulse()) {
return;
}
auto sensor_bridge_ptr = map_builder_bridge_.sensor_bridge(trajectory_id);
auto imu_data_ptr = sensor_bridge_ptr->ToImuData(msg);
// extrapolators_使用IMU数据进行位姿预测
if (imu_data_ptr != nullptr) {
extrapolators_.at(trajectory_id).AddImuData(*imu_data_ptr);
}
sensor_bridge_ptr->HandleImuMessage(sensor_id, msg);
}
// 调用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);
}
// 调用SensorBridge的传感器处理函数进行多激光扫描数据处理
void Node::HandleMultiEchoLaserScanMessage(
const int trajectory_id, const std::string& sensor_id,
const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) {
absl::MutexLock lock(&mutex_);
if (!sensor_samplers_.at(trajectory_id).rangefinder_sampler.Pulse()) {
return;
}
map_builder_bridge_.sensor_bridge(trajectory_id)
->HandleMultiEchoLaserScanMessage(sensor_id, msg);
}
// 调用SensorBridge的传感器处理函数进行点云数据处理
void Node::HandlePointCloud2Message(
const int trajectory_id, const std::string& sensor_id,
const sensor_msgs::PointCloud2::ConstPtr& msg) {
absl::MutexLock lock(&mutex_);
if (!sensor_samplers_.at(trajectory_id).rangefinder_sampler.Pulse()) {
return;
}
map_builder_bridge_.sensor_bridge(trajectory_id)
->HandlePointCloud2Message(sensor_id, msg);
}（5）其他功能函数
根据ID返回相应topic名称的函数
// 根据配置文件, 确定所有需要的topic的名字的集合
std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>
Node::ComputeExpectedSensorIds(const TrajectoryOptions& options) const {
// SensorId为结构体，包括传感器的种类和topic的名称
using SensorId = cartographer::mapping::TrajectoryBuilderInterface::SensorId;
using SensorType = SensorId::SensorType;
std::set<SensorId> expected_topics;
// Subscribe to all laser scan, multi echo laser scan, and point cloud topics.
// 如果只有一个传感器, 那订阅的topic就是topic
// 如果是多个传感器, 那订阅的topic就是topic_1,topic_2, 依次类推
for (const std::string& topic :
ComputeRepeatedTopicNames(kLaserScanTopic, options.num_laser_scans)) {
expected_topics.insert(SensorId{SensorType::RANGE, topic});
}
for (const std::string& topic : ComputeRepeatedTopicNames(
kMultiEchoLaserScanTopic, options.num_multi_echo_laser_scans)) {
expected_topics.insert(SensorId{SensorType::RANGE, topic});
}
for (const std::string& topic :
ComputeRepeatedTopicNames(kPointCloud2Topic, options.num_point_clouds)) {
expected_topics.insert(SensorId{SensorType::RANGE, topic});
}
// For 2D SLAM, subscribe to the IMU if we expect it. For 3D SLAM, the IMU is
// required.
// 3D SLAM必须有IMU, 2D可有可无, 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())) {
expected_topics.insert(SensorId{SensorType::IMU, kImuTopic});
}
// Odometry is optional.
// 里程计可有可无
if (options.use_odometry) {
expected_topics.insert(SensorId{SensorType::ODOMETRY, kOdometryTopic});
}
// NavSatFix is optional.
// GPS可有可无
if (options.use_nav_sat) {
expected_topics.insert(
SensorId{SensorType::FIXED_FRAME_POSE, kNavSatFixTopic});
}
// Landmark is optional.
// 地标可有可无
if (options.use_landmarks) {
expected_topics.insert(SensorId{SensorType::LANDMARK, kLandmarkTopic});
}
// 返回传感器的topic名字
return expected_topics;
}检查TrajectoryOptions是否存在2D或者3D轨迹的配置信息
bool Node::ValidateTrajectoryOptions(const TrajectoryOptions& options) {
if (node_options_.map_builder_options.use_trajectory_builder_2d()) {
return options.trajectory_builder_options
.has_trajectory_builder_2d_options();
}
if (node_options_.map_builder_options.use_trajectory_builder_3d()) {
return options.trajectory_builder_options
.has_trajectory_builder_3d_options();
}
return false;
}检查topic名字是否被其他轨迹使用
bool Node::ValidateTopicNames(const TrajectoryOptions& options) {
for (const auto& sensor_id : ComputeExpectedSensorIds(options)) {
const std::string& topic = sensor_id.id;
// 如果topic能够在subscribed_topics_中找到, 证明这个topic名字被之前的轨迹使用了
if (subscribed_topics_.count(topic) > 0) {
LOG(ERROR) << "Topic name [" << topic << "] is already used.";
return false;
}
}
return true;
}检查对应id的轨迹是否存在, 如果存在则判断这个轨迹的状态
cartographer_ros_msgs::StatusResponse Node::TrajectoryStateToStatus(
const int trajectory_id, const std::set<TrajectoryState>& valid_states) {
const auto trajectory_states = map_builder_bridge_.GetTrajectoryStates();
cartographer_ros_msgs::StatusResponse status_response;
const auto it = trajectory_states.find(trajectory_id);
// 如果没有找到对应id的轨迹, 返回NOT_FOUND
if (it == trajectory_states.end()) {
status_response.message =
absl::StrCat("Trajectory ", trajectory_id, " doesn't exist.");
status_response.code = cartographer_ros_msgs::StatusCode::NOT_FOUND;
return status_response;
}
status_response.message =
absl::StrCat("Trajectory ", trajectory_id, " is in '",
TrajectoryStateToString(it->second), "' state.");
// 轨迹的状态如果在列表中, 返回OK, 否则返回 INVALID_ARGUMENT
status_response.code =
valid_states.count(it->second)
? cartographer_ros_msgs::StatusCode::OK
: cartographer_ros_msgs::StatusCode::INVALID_ARGUMENT;
return status_response;
}保存pbstream文件
void Node::SerializeState(const std::string& filename,
const bool include_unfinished_submaps) {
absl::MutexLock lock(&mutex_);
CHECK(
map_builder_bridge_.SerializeState(filename, include_unfinished_submaps))
<< "Could not write state.";
}加载pbstream文件
void Node::LoadState(const std::string& state_filename,
const bool load_frozen_state) {
absl::MutexLock lock(&mutex_);
map_builder_bridge_.LoadState(state_filename, load_frozen_state);
}
1219
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
