本人讲解关于slam一系列文章汇总链接:史上最全slam从零开始,针对于本栏目讲解(02)Cartographer源码无死角解析-链接如下:
(02)Cartographer源码无死角解析- (00)目录_最新无死角讲解:https://blog.csdn.net/weixin_43013761/article/details/127350885
文末正下方中心提供了本人 联系方式, 点击本人照片即可显示 W X → 官方认证 {\color{blue}{文末正下方中心}提供了本人 \color{red} 联系方式,\color{blue}点击本人照片即可显示WX→官方认证} 文末正下方中心提供了本人联系方式,点击本人照片即可显示WX→官方认证
上篇博客已经完成了 src/cartographer_ros/cartographer_ros/cartographer_ros/node_main.cc 的讲解,但是没有深入对其进行了解,现在回过头来对其中的:
// 根据Lua配置文件中的内容, 为node_options, trajectory_options 赋值
std::tie(node_options, trajectory_options) =
LoadOptions(FLAGS_configuration_directory, FLAGS_configuration_basename);
进行深入的了解,其上的 std::tie 是 c++11 新加的函数,作用和 python 比较类似,可以对多个变量同时进行赋值,但是注意,其只能接收 tuple 这个数据类型。LoadOptions() 函数实现于 src/cartographer_ros/cartographer_ros/cartographer_ros/node_options.cc 文件:
std::tuple<NodeOptions, TrajectoryOptions> LoadOptions(
const std::string& configuration_directory,
const std::string& configuration_basename) {
......
}
可以很明显看到其返回的是 std::tuple(c++11新增),其存储两个元素(结构体),元素类型分别为 NodeOptions, TrajectoryOptions。其需要传入两个参数,configuration_directory→.lua文件所在目录;configuration_basename→.lua文件名。两个结构体的
先来看看 NodeOptions 这个类,应该说结构体,如下所示(src/cartographer_ros/cartographer_ros/cartographer_ros/node_options.h):
struct NodeOptions {
//来自于src/cartographer/configuration_files/map_builder.lua
::cartographer::mapping::proto::MapBuilderOptions map_builder_options;
std::string map_frame; //地图名称
double lookup_transform_timeout_sec; //查询tf时的超时时间
double submap_publish_period_sec; //发布submap的时间间隔
double pose_publish_period_sec; //发布pose的时间间隔
double trajectory_publish_period_sec; //发布轨迹的时间间隔
bool publish_to_tf = true; //是否发布tf
bool publish_tracked_pose = false; //是否发布追踪位姿
bool use_pose_extrapolator = true; //发布tf时是使用pose_extrapolator的位姿还是前端计算出来的位姿
};
需要注意的是,最后的三个变量并不是在 .lua 文件中配置的,也就是说其为固定初值,只能通过源码进行修改。
另外TrajectoryOptions 在 src/cartographer_ros/cartographer_ros/cartographer_ros/trajectory_options.h 中定义:
// 一条轨迹的基础参数配置
struct TrajectoryOptions {
//来自于 src/cartographer/configuration_files/trajectory_builder.lua
::cartographer::mapping::proto::TrajectoryBuilderOptions
trajectory_builder_options;
std::string tracking_frame; //由SLAM算法追踪的ROS坐标系ID,如果使用IMU,应该使用其坐标系,通常选择是 “imu_link”
std::string published_frame; //用于发布位姿子坐标系的ROS坐标系ID,例如“odom”坐标系,如果一个“odom”坐标系由系统的不同部分提供,在这种情况下,map_frame中的“odom”姿势将被发布。 否则,将其设置为“base_link”可能是合适的。
std::string odom_frame; //在provide_odom_frame为真才启用,坐标系在published_frame和map_frame之间用于发布局部SLAM结果,通常是“odom”
bool provide_odom_frame; //如果启用,局部,非闭环,持续位姿会作为odom_frame发布在map_frame中发布。
bool use_odometry; //如果启用,订阅关于“odom”话题的nav_msgs/Odometry消息。里程信息会提供,这些信息包含在SLAM里
bool use_nav_sat; //是否使用gps
bool use_landmarks; //是否使用landmark
bool publish_frame_projected_to_2d; //是否将坐标系投影到平面上
int num_laser_scans; //订阅的激光扫描话题数量。在一个激光扫描仪的“scan”话题上订sensor_msgs/LaserScan或在多个激光扫描仪上订阅话题“scan_1”,“scan_2”等。
int num_multi_echo_laser_scans; //订阅的多回波激光扫描主题的数量。在一个激光扫描仪的“echoes”话题上订阅sensor_msgs/MultiEchoLaserScan,或者为多个激光扫描仪订阅话题“echoes_1”,“echoes_2”等。
int num_subdivisions_per_laser_scan; //将每个接收到的(多回波)激光扫描分成的点云数。分扫描可以在扫描仪移动时取消扫描获取的扫描。有一个相应的轨迹构建器选项可将细分扫描累积到将用于扫描匹配的点云中。
int num_point_clouds; //要订阅的点云话题的数量。在一个测距仪的“points2”话题上订阅sensor_msgs/PointCloud2或者为多个测距仪订阅话题“points2_1”,“points2_2”等。
double rangefinder_sampling_ratio; //传感器数据的采样频率
double odometry_sampling_ratio;
double fixed_frame_pose_sampling_ratio;
double imu_sampling_ratio;
double landmarks_sampling_ratio;
};
LoadOptions() 函数的实现,主要总体来来看还是比较简单的,主要分为如下四个步骤:
( 1 ) \color{blue}(1) (1) 构建一个 cartographer::common::ConfigurationFileResolver 类指针对象 file_resolver,对其中的成员 configuration_files_directories_ 进行初始化,其类型为 std::vector
//配置文件的目录,本人如下:
configuration_directory = "/my_work/ROS/work/cartographer_detailed_comments_ws/install_isolated/share/cartographer_ros/configuration_files"
//本人的如下,每个人的不一样,其是在编译的时候自动生成的
constexpr char kConfigurationFilesDirectory[] ="/my_work/ROS/work/cartographer_detailed_comments_ws/install_isolated/share/cartographer/configuration_files";
( 2 ) \color{blue}(2) (2) 在创建的时候,已经指定了配置文件的所在的目录。只需把配置文件名 configuration_basename 传入到 file_resolver->GetFileContentOrDie() 函数即可获得配置文件中的内容。
( 3 ) \color{blue}(3) (3) 调用初始化一个 cartographer::common::LuaParameterDictionary 字典(对象),对配置文件中的 code 进行解析。关于配置文件的信息基本都在这个参数字典之中。
( 4 ) \color{blue}(4) (4) 构建一个元组返回,元组存储两个类型分别 NodeOptions 与 TrajectoryOptions 类型的元素。这两个元素使用过 CreateNodeOptions(&lua_parameter_dictionary) 与 CreateTrajectoryOptions(&lua_parameter_dictionary) 函数构建的。
代码注释入如下:
/**
* @brief 加载lua配置文件中的参数
*
* @param[in] configuration_directory 配置文件所在目录
* @param[in] configuration_basename 配置文件的名字
* @return std::tuple 返回节点的配置与轨迹的配置
*/
std::tuple<NodeOptions, TrajectoryOptions> LoadOptions(
const std::string& configuration_directory,
const std::string& configuration_basename) {
//?: 获取配置文件所在的目录
auto file_resolver =
absl::make_unique<cartographer::common::ConfigurationFileResolver>(
std::vector<std::string>{configuration_directory});
// 读取配置文件内容到code中
const std::string code =
file_resolver->GetFileContentOrDie(configuration_basename);
// 根据给定的字符串, 生成一个lua字典
cartographer::common::LuaParameterDictionary lua_parameter_dictionary(
code, std::move(file_resolver));
// 创建元组tuple,元组定义了一个有固定数目元素的容器, 其中的每个元素类型都可以不相同
// 将配置文件的内容填充进NodeOptions与TrajectoryOptions, 并返回
return std::make_tuple(CreateNodeOptions(&lua_parameter_dictionary),
CreateTrajectoryOptions(&lua_parameter_dictionary));
}
} // namespace cartographer_ros
首先我们来看 CreateNodeOptions(&lua_parameter_dictionary),还函数返回一个 struct NodeOptions(前面有注解),其错入的参数为 LuaParameterDictionary 类型,暂时把它当作一个字典即可,也就是给其对象 lua_parameter_dictionary 一个键,则可获得对应的一个值。CreateNodeOptions() 函数于 src/cartographer_ros/cartographer_ros/cartographer_ros/node_options.cc 文件中实现:
/**
* @brief 读取lua文件内容, 将lua文件的内容赋值给NodeOptions
*
* @param lua_parameter_dictionary lua字典
* @return NodeOptions
*/
NodeOptions CreateNodeOptions(
::cartographer::common::LuaParameterDictionary* const
lua_parameter_dictionary) {
NodeOptions options;
// 根据lua字典中的参数, 生成protobuf的序列化数据结构 proto::MapBuilderOptions
options.map_builder_options =
::cartographer::mapping::CreateMapBuilderOptions(
lua_parameter_dictionary->GetDictionary("map_builder").get());
options.map_frame = lua_parameter_dictionary->GetString("map_frame");
options.lookup_transform_timeout_sec =
lua_parameter_dictionary->GetDouble("lookup_transform_timeout_sec");
options.submap_publish_period_sec =
lua_parameter_dictionary->GetDouble("submap_publish_period_sec");
options.pose_publish_period_sec =
lua_parameter_dictionary->GetDouble("pose_publish_period_sec");
options.trajectory_publish_period_sec =
lua_parameter_dictionary->GetDouble("trajectory_publish_period_sec");
if (lua_parameter_dictionary->HasKey("publish_to_tf")) {
options.publish_to_tf =
lua_parameter_dictionary->GetBool("publish_to_tf");
}
if (lua_parameter_dictionary->HasKey("publish_tracked_pose")) {
options.publish_tracked_pose =
lua_parameter_dictionary->GetBool("publish_tracked_pose");
}
if (lua_parameter_dictionary->HasKey("use_pose_extrapolator")) {
options.use_pose_extrapolator =
lua_parameter_dictionary->GetBool("use_pose_extrapolator");
}
return options;
}
代码没有什么特殊的地方,这里就不进行注释了,仅仅是从 lua_parameter_dictionary 获得 .lua 中对应参数进行赋值而已。需要注意的一个点是,存在如下代码:
lua_parameter_dictionary->GetDictionary("map_builder").get()
这里使用了 .get() 去获取值(智能指针使用方式),如下:
std::unique_ptr<LuaParameterDictionary> LuaParameterDictionary::GetDictionary(
const std::string& key) {
CheckHasKeyAndReference(key);
GetValueFromLuaTable(L_, key);
return PopDictionary(reference_count_);
}
从 GetDictionary() 的实现可以知道,其返回的还是 LuaParameterDictionary 类型的智能指针,也就是说明其返回的还是一个字典。
该函数位于 src/cartographer_ros/cartographer_ros/cartographer_ros/trajectory_options.cc,与CreateNodeOptions 同出一撇,所以也不进行讲解了。同样是从 lua_parameter_dictionary 中获取数据,然后进行赋值而已。其中如下函数进行简单备注:
lua_parameter_dictionary->GetDouble() //获得参数必须为浮点型,否则报错
lua_parameter_dictionary->GetBool() //获得参数必须为bool型,否则报错
......
CheckTrajectoryOptions(options);
另外,在最后还调用了 CheckTrajectoryOptions(options); 函数,其会对配置的传感器做一个校验:
void CheckTrajectoryOptions(const TrajectoryOptions& options) {
CHECK_GE(options.num_subdivisions_per_laser_scan, 1);
CHECK_GE(options.num_laser_scans + options.num_multi_echo_laser_scans +
options.num_point_clouds,
1)
<< "Configuration error: 'num_laser_scans', "
"'num_multi_echo_laser_scans' and 'num_point_clouds' are "
"all zero, but at least one is required.";
}
总的来说,单线雷达,回声波雷达,多线雷达他们数量和需要大于1。
通过该篇博客以及上一篇博客,可以了解到,在 node_main.cc 中的 Run 函数中,其会调用 LoadOptions() 函数,获得 node_options,trajectory_options 两个结构体对象,其主要存储了 .lua 文件中的配置信息,node_options,trajectory_options 主要作用如下:
// Node类的初始化, 将ROS的topic传入SLAM, 也就是MapBuilder
Node node(node_options, std::move(map_builder), &tf_buffer,
FLAGS_collect_metrics);
// 使用默认topic 开始轨迹
if (FLAGS_start_trajectory_with_default_topics) {
node.StartTrajectoryWithDefaultTopics(trajectory_options);
}
这些内容都是后续重点讲解。下一篇博客主要是对 cartographer::common::LuaParameterDictionary 进行深入分析。