一.C++相关
-
c++11: =delete: 禁止编译器自动生成默认函数; =default: 要求编译器生成一个默认函数
// 禁止编译器自动生成 默认拷贝构造函数(复制构造函数) Node(const Node&) = delete; // 禁止编译器自动生成 默认赋值函数 Node& operator=(const Node&) = delete;
-
GUARDED_BY 线程安全属性
// c++11: GUARDED_BY 是在Clang Thread Safety Analysis(线程安全分析)中定义的属性 // GUARDED_BY是数据成员的属性, 该属性声明数据成员受给定功能保护. // 对数据的读操作需要共享访问, 而写操作则需要互斥访问. // 官方介绍文档: https://clang.llvm.org/docs/ThreadSafetyAnalysis.html MapBuilderBridge map_builder_bridge_ GUARDED_BY(mutex_);
3.数据结构:std::unordered_map 是采用哈希数据结构实现的, std::map是通过红黑树实现的
// c++11: std::unordered_map 是采用哈希数据结构实现的, 内部数据保存是无序的
// 相对于std::map, unordered_map的查找和插入的效率高, 时间复杂度为常数级别O(1),而额外空间复杂度则要高出许多
// 对于需要高效率查询的情况, 使用unordered_map容器, 但是unordered_map对于迭代器遍历效率并不高
// These are keyed with 'trajectory_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_;
二.Node类初始化
// Node类的初始化, 将ROS的topic传入SLAM, 也就是MapBuilder
Node node(node_options, std::move(map_builder), &tf_buffer,
FLAGS_collect_metrics);
三.Node类作用,区分为3个模块
1.话题、地图文件、轨迹,保存、检查、使用;ros服务的处理;定时发布数据2.开始轨迹、结束轨迹;处理传感器数据3.构造函数,有4个模块(声明ROS的一些topic的发布器;服务的发布器;处理之后的点云的发布器;以及将时间驱动的函数与定时器进行绑定)
/**
* @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 如果想更改就使用remap或者更改代码
//话题名在/cartographer_ros/node_constants.h里定义
constexpr char kSubmapListTopic[] = “submap_list”;
// 发布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);
//pub的定位姿态
_pose_pub = node_handle_.advertise<geometry_msgs::Pose2D>("pose_nav", 10);//100hz
// 发布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容器中,地图信息的处理
service_servers_.push_back(node_handle_.advertiseService(
kSubmapQueryServiceName, &Node::HandleSubmapQuery, this));
service_servers_.push_back(node_handle_.advertiseService(
kTrajectoryQueryServiceName, &Node::HandleTrajectoryQuery, this));
service_servers_.push_back(node_handle_.advertiseService(
kStartTrajectoryServiceName, &Node::HandleStartTrajectory, this));
service_servers_.push_back(node_handle_.advertiseService(
kFinishTrajectoryServiceName, &Node::HandleFinishTrajectory, this));
service_servers_.push_back(node_handle_.advertiseService(
kWriteStateServiceName, &Node::HandleWriteState, this));
service_servers_.push_back(node_handle_.advertiseService(
kGetTrajectoryStatesServiceName, &Node::HandleGetTrajectoryStates, this));
service_servers_.push_back(node_handle_.advertiseService(
kReadMetricsServiceName, &Node::HandleReadMetrics, this));
Step: 3 处理之后的点云的发布器
scan_matched_point_cloud_publisher_ =
node_handle_.advertise<sensor_msgs::PointCloud2>(
kScanMatchedPointCloudTopic, kLatestOnlyPublisherQueueSize);
Step: 4 进行定时器与函数的绑定, 定时发布数据
wall_timers_.push_back(node_handle_.createWallTimer(
::ros::WallDuration(node_options_.submap_publish_period_sec), // 0.3s
&Node::PublishSubmapList, this));
if (node_options_.pose_publish_period_sec > 0) {
publish_local_trajectory_data_timer_ = node_handle_.createTimer(
::ros::Duration(node_options_.pose_publish_period_sec), // 5e-3s
&Node::PublishLocalTrajectoryData, this);
}
wall_timers_.push_back(node_handle_.createWallTimer(
::ros::WallDuration(
node_options_.trajectory_publish_period_sec), // 30e-3s
&Node::PublishTrajectoryNodeList, this));
wall_timers_.push_back(node_handle_.createWallTimer(
::ros::WallDuration(
node_options_.trajectory_publish_period_sec), // 30e-3s
&Node::PublishLandmarkPosesList, this));
wall_timers_.push_back(node_handle_.createWallTimer(
::ros::WallDuration(kConstraintPublishPeriodSec), // 0.5s
&Node::PublishConstraintList, this));
// 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));
}
}