目录
[1. gflag](#1. gflag)
[1.1 从launch文件开始](#1.1 从launch文件开始)
[1.2 node_main.cc的内容](#1.2 node_main.cc的内容)
[2. lua文件](#2. lua文件)
[3. 使用Lua文件](#3. 使用Lua文件)
[3.1 node_main.cc::Run()](#3.1 node_main.cc::Run())
[3.2 LoadOptions()](#3.2 LoadOptions())
[3.3 LuaParameterDictionary](#3.3 LuaParameterDictionary)
[3.4 CreateNodeOptions(&lua_parameter_dictionary)](#3.4 CreateNodeOptions(&lua_parameter_dictionary))
[3.5 CreateTrajectoryOptions(&lua_parameter_dictionary)](#3.5 CreateTrajectoryOptions(&lua_parameter_dictionary))
1. gflag
gflag 简介 : gflags 是 google 开源的命令行标记处理库
命令行标记,顾名思义就是当运行一个可执行文件时 , 由用户为其指定的标记 , 形如:
fgrep -l -f ./test ccc jjj
-l 与 -f 是 命令行标记 , 而 ccc 与 jjj 是命令行参数 , 因为这两者不是以 - 开头的 。 一般的一个可执行文件, 允许用户为其传入命令行标记以及参数 。 如上述例子 , -l 是一个不带参数的标记 , -f 是一个带了参数 ./test 的标记 , 而 gflags 可以解析这些标记以及参数并将其存储在某些数据结构中 。
gflags 主要支持的参数类型包括 bool, int32, int64, uint64, double, string 等。 定义参数通过 DEFINE_type 宏实现 , 该宏的三个参数含义分别为命令行参数名 , 参数默 认值 , 以及参数的帮助信息。
当参数被定义后, 通过 FLAGS_name 就可访问到对应的参数。
1.1 从launch文件开始
假设当前有一个test.launch文件用于启动Cartographer建图,其内容为:
XML
<launch>
<!-- bag的地址与名称 -->
<arg name="bag_filename" default="$(env HOME)/bagfiles/rslidar-outdoor-gps.bag"/>
<!-- 使用bag的时间戳 -->
<param name="/use_sim_time" value="true" />
<!-- 启动cartographer -->
<node name="cartographer_node" pkg="cartographer_ros"
type="cartographer_node" args="
-configuration_directory $(find cartographer_ros)/configuration_files
-configuration_basename test.lua"
output="screen">
<remap from="points2" to="rslidar_points" />
<remap from="scan" to="front_scan" />
<remap from="odom" to="odom_scout" />
<remap from="imu" to="imu" />
</node>
<!-- 生成ros格式的地图 -->
<node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
type="cartographer_occupancy_grid_node" args="-resolution 0.05" />
<!-- 启动rviz -->
<node name="rviz" pkg="rviz" type="rviz" required="true"
args="-d $(find cartographer_ros)/configuration_files/lx_2d.rviz" />
<!-- 启动rosbag -->
<node name="playbag" pkg="rosbag" type="play"
args="--clock $(arg bag_filename)" />
</launch>
主要看启动Cartographer节点的这一段配置:
XML
<!-- 启动cartographer -->
<node name="cartographer_node" pkg="cartographer_ros"
type="cartographer_node" args="
-configuration_directory $(find cartographer_ros)/configuration_files
-configuration_basename test.lua"
output="screen">
<remap from="points2" to="rslidar_points" />
<remap from="scan" to="front_scan" />
<remap from="odom" to="odom_scout" />
<remap from="imu" to="imu" />
</node>
这里相当于使用rosrun从cartographer_ros功能包中启动了一个cartographer_node节点。args是传入可执行程序的参数,也就是命令行标记的内容。
<remap from="points2" to="rslidar_points" /> 的作用是将Cartographer要订阅的话题名points2 映射为rslidar_points ,因为这里使用的rosbag中提供的lidar数据话题名为rslidar_points。
再回到launch文件中,其中args的内容会被传入到cartographer_node可执行程序中。在Cartographer项目中,cartographer_ros包中的node_main.cc是主程序的入口。
1.2 node_main.cc的内容
cpp
#include "absl/memory/memory.h"
#include "cartographer/mapping/map_builder.h"
#include "cartographer_ros/node.h"
#include "cartographer_ros/node_options.h"
#include "cartographer_ros/ros_log_sink.h"
#include "gflags/gflags.h"
#include "tf2_ros/transform_listener.h"
/**
* note: gflags是一套命令行参数解析工具
* DEFINE_bool在gflags.h中定义
* gflags主要支持的参数类型包括bool, int32, int64, uint64, double, string等
* 定义参数通过DEFINE_type宏实现, 该宏的三个参数含义分别为命令行参数名, 参数默认值, 以及参数的帮助信息
* 当参数被定义后, 通过FLAGS_name就可访问到对应的参数
* 命令行标记 命令行参数
* 这些参数值也可以在launch文件设置: -configuration_basename lx_rs16_2d_outdoor_localization.lua
*/
// collect_metrics :激活运行时度量的集合.如果激活, 可以通过ROS服务访问度量
DEFINE_bool(collect_metrics, false,
"Activates the collection of runtime metrics. If activated, the "
"metrics can be accessed via a ROS service.");
DEFINE_string(configuration_directory, "",
"First directory in which configuration files are searched, "
"second is always the Cartographer installation to allow "
"including files from there.");
DEFINE_string(configuration_basename, "",
"Basename, i.e. not containing any directory prefix, of the "
"configuration file.");
DEFINE_string(load_state_filename, "",
"If non-empty, filename of a .pbstream file to load, containing "
"a saved SLAM state.");
DEFINE_bool(load_frozen_state, true,
"Load the saved state as frozen (non-optimized) trajectories.");
DEFINE_bool(
start_trajectory_with_default_topics, true,
"Enable to immediately start the first trajectory with default topics.");
DEFINE_string(
save_state_filename, "",
"If non-empty, serialize state and write it to disk before shutting down.");
namespace cartographer_ros {
namespace {
void Run() {
......
}
} // namespace
} // namespace cartographer_ros
int main(int argc, char** argv) {
// note: 初始化glog库
google::InitGoogleLogging(argv[0]);
// note: 使用gflags进行参数的初始化
google::ParseCommandLineFlags(&argc, &argv, true);
/**
* @brief glog里提供的CHECK系列的宏, 检测某个表达式是否为真
* 检测expression如果不为真, 则打印后面的description和栈上的信息
* 然后退出程序, 出错后的处理过程和FATAL比较像.
*/
CHECK(!FLAGS_configuration_directory.empty())
<< "-configuration_directory is missing.";
CHECK(!FLAGS_configuration_basename.empty())
<< "-configuration_basename is missing.";
// ros节点的初始化
// note: ::ros::init() 明确表示调用全局 ros 命名空间下的 init() 函数
::ros::init(argc, argv, "cartographer_node");
// 一般不需要在自己的代码中显式调用
// 但是若想在创建任何NodeHandle实例之前启动ROS相关的线程, 网络等, 可以显式调用该函数.
::ros::start();
// 使用ROS_INFO进行glog消息的输出
cartographer_ros::ScopedRosLogSink ros_log_sink;
// 开始运行cartographer_ros
cartographer_ros::Run();
// 结束ROS相关的线程, 网络等
::ros::shutdown();
}
其中在main函数之前就使用DEFINE_bool、DEFINE_string定义了几个命令行标记的全局参数:collect_metrics 、configuration_directory 、configuration_basename。这里设置了这些参数的默认值,但这些默认值也可以修改,就是通过args中的内容传入想修改命令行标记与参数值就好了。
cpp
google::ParseCommandLineFlags(&argc, &argv, true);
args的内容会被当作字符串,通过C/C++中main函数的参数中的char** argv传入gflag进行解析。如果有定义的命令行标记与其参数值被传入,则该标记的默认参数值就会被更换为新值。
比如,launch文件中传入了" -configuration_directory (find cartographer_ros)/configuration_files -configuration_basename test.lua",则**configuration_directory** 的参数值会被修改为**(find cartographer_ros)/configuration_files** ,这就是lua文件的存放目录;configuration_basename 的参数值会被修改为test.lua,就是当前指定的lua文件。
当参数被定义后, 通过 FLAGS_name就可访问到对应的参数,数据类型就是定义时的类型。例如:
cpp
CHECK(!FLAGS_configuration_directory.empty())
<< "-configuration_directory is missing.";
CHECK(!FLAGS_configuration_basename.empty())
<< "-configuration_basename is missing.";
cpp
std::tuple<NodeOptions, TrajectoryOptions> LoadOptions(
const std::string& configuration_directory,
const std::string& configuration_basename);
std::tie(node_options, trajectory_options) =
LoadOptions(FLAGS_configuration_directory, FLAGS_configuration_basename);
FLAGS_configuration_directory、FLAGS_configuration_basename就是string类型。
2. lua文件
在 Cartographer 中,Lua 配置文件扮演着至关重要的角色,主要用于参数配置和系统行为定义。Cartographer 作为一个复杂的 SLAM系统,需要大量参数来调整其性能、精度和适配不同场景,而 Lua 文件正是这些参数的载体。通过修改 Lua 配置文件,可以在不重新编译 Cartographer 源码的情况下,灵活调整系统性能。
一个Lua文件的示例如下:
Lua
include "map_builder.lua"
include "trajectory_builder.lua"
options = {
map_builder = MAP_BUILDER, -- map_builder.lua的配置信息
trajectory_builder = TRAJECTORY_BUILDER, -- trajectory_builder.lua的配置信息
map_frame = "map", -- 地图坐标系的名字
tracking_frame = "imu_link", -- 将所有传感器数据转换到这个坐标系下
published_frame = "footprint", -- tf: map -> footprint
odom_frame = "odom", -- 里程计的坐标系名字
provide_odom_frame = false, -- 是否提供odom的tf, 如果为true,则tf树为map->odom->footprint
-- 如果为false tf树为map->footprint
publish_frame_projected_to_2d = false, -- 是否将坐标系投影到平面上
--use_pose_extrapolator = false, -- 发布tf时是使用pose_extrapolator的位姿还是前端计算出来的位姿
use_odometry = false, -- 是否使用里程计,如果使用要求一定要有odom的tf
use_nav_sat = false, -- 是否使用gps
use_landmarks = false, -- 是否使用landmark
num_laser_scans = 0, -- 是否使用单线激光数据
num_multi_echo_laser_scans = 0, -- 是否使用multi_echo_laser_scans数据
num_subdivisions_per_laser_scan = 1, -- 1帧数据被分成几次处理,一般为1
num_point_clouds = 1, -- 是否使用点云数据
lookup_transform_timeout_sec = 0.2, -- 查找tf时的超时时间
submap_publish_period_sec = 0.3, -- 发布数据的时间间隔
pose_publish_period_sec = 5e-3,
trajectory_publish_period_sec = 30e-3,
rangefinder_sampling_ratio = 1., -- 传感器数据的采样频率
odometry_sampling_ratio = 1.,
fixed_frame_pose_sampling_ratio = 1.,
imu_sampling_ratio = 1.,
landmarks_sampling_ratio = 1.,
}
MAP_BUILDER.use_trajectory_builder_2d = true
TRAJECTORY_BUILDER_2D.use_imu_data = true
TRAJECTORY_BUILDER_2D.min_range = 0.3
TRAJECTORY_BUILDER_2D.max_range = 100.
TRAJECTORY_BUILDER_2D.min_z = 0.2
--TRAJECTORY_BUILDER_2D.max_z = 1.4
--TRAJECTORY_BUILDER_2D.voxel_filter_size = 0.02
--TRAJECTORY_BUILDER_2D.adaptive_voxel_filter.max_length = 0.5
--TRAJECTORY_BUILDER_2D.adaptive_voxel_filter.min_num_points = 200.
--TRAJECTORY_BUILDER_2D.adaptive_voxel_filter.max_range = 50.
--TRAJECTORY_BUILDER_2D.loop_closure_adaptive_voxel_filter.max_length = 0.9
--TRAJECTORY_BUILDER_2D.loop_closure_adaptive_voxel_filter.min_num_points = 100
--TRAJECTORY_BUILDER_2D.loop_closure_adaptive_voxel_filter.max_range = 50.
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = false
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.occupied_space_weight = 1.
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.translation_weight = 1.
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.rotation_weight = 1.
--TRAJECTORY_BUILDER_2D.ceres_scan_matcher.ceres_solver_options.max_num_iterations = 12
--TRAJECTORY_BUILDER_2D.motion_filter.max_distance_meters = 0.1
--TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = 0.004
--TRAJECTORY_BUILDER_2D.imu_gravity_time_constant = 1.
TRAJECTORY_BUILDER_2D.submaps.num_range_data = 80.
TRAJECTORY_BUILDER_2D.submaps.grid_options_2d.resolution = 0.1
POSE_GRAPH.optimize_every_n_nodes = 160.
POSE_GRAPH.constraint_builder.sampling_ratio = 0.3
POSE_GRAPH.constraint_builder.max_constraint_distance = 15.
POSE_GRAPH.constraint_builder.min_score = 0.48
POSE_GRAPH.constraint_builder.global_localization_min_score = 0.60
return options
其中带--前缀的表示注释。
2.1 支持模块化引入(include
机制)
Lua文件可以通过include "filename.lua"
语句引入其他 Lua 配置文件,实现参数的模块化管理。如下为map_builder.lua的内容:
Lua
include "pose_graph.lua"
MAP_BUILDER = {
use_trajectory_builder_2d = false,
use_trajectory_builder_3d = false,
num_background_threads = 4,
pose_graph = POSE_GRAPH,
collate_by_trajectory = false,
}
示例中通过include "map_builder.lua"
和include "trajectory_builder.lua"
引入了基础配置,避免了重复定义,使主配置文件更简洁。
引入的文件中定义的变量(如MAP_BUILDER
、TRAJECTORY_BUILDER
)可以直接在当前文件中使用和修改,实现了配置的继承与扩展。如:
Lua
MAP_BUILDER.use_trajectory_builder_2d = true
2.2 通过全局变量实现配置聚合
Lua的核心配置通过一个全局变量(如示例中的options
)统一聚合,将所有参数组织成键值对结构,便于 Cartographer 程序解析。
options
中可以包含多种类型的参数:- 直接定义的基础参数(如
map_frame = "map"
) - 引用其他模块的配置(如
map_builder = MAP_BUILDER
,关联引入文件中的配置) - 开关型参数(如
use_odometry = false
) - 数值型参数(如
lookup_transform_timeout_sec = 0.2
)
- 直接定义的基础参数(如
2.3 支持对引入模块的参数进行重写
引入外部配置后,可以在当前文件中直接修改其参数,覆盖默认值,实现个性化配置。
- 示例中:
- 引入
map_builder.lua
后,通过MAP_BUILDER.use_trajectory_builder_2d = true
指定使用 2D 轨迹构建器。 - 引入
trajectory_builder.lua
后,通过TRAJECTORY_BUILDER_2D.use_imu_data = true
开启 IMU 数据使用,覆盖了可能的默认值。
- 引入
这种机制允许用户在不修改原始模块文件的情况下,灵活调整子模块的行为。
3. 使用Lua文件
Lua文件的路径和文件名已经在launch文件和gflag的配合下被传入到FLAGS_configuration_directory和FLAGS_configuration_basename中。接下来进行解析和读取内容:
3.1 node_main.cc::Run()
cpp
NodeOptions node_options;
TrajectoryOptions trajectory_options;
// c++11: std::tie()函数可以将变量连接到一个给定的tuple上,生成一个元素类型全是引用的tuple
// 根据Lua配置文件中的内容, 为node_options, trajectory_options 赋值
std::tie(node_options, trajectory_options) =
LoadOptions(FLAGS_configuration_directory, FLAGS_configuration_basename);
在node_main.cc::Run()函数在,Lua文件的内容被读取到NodeOptions和TrajectoryOptions两个配置结构体的对象中。两个结构体的定义为:
cpp
struct NodeOptions {
::cartographer::mapping::proto::MapBuilderOptions map_builder_options;
std::string map_frame;
double lookup_transform_timeout_sec;
double submap_publish_period_sec;
double pose_publish_period_sec;
double trajectory_publish_period_sec;
bool publish_to_tf = true;
bool publish_tracked_pose = false;
bool use_pose_extrapolator = true;
};
cpp
struct TrajectoryOptions {
::cartographer::mapping::proto::TrajectoryBuilderOptions
trajectory_builder_options;
std::string tracking_frame;
std::string published_frame;
std::string odom_frame;
bool provide_odom_frame;
bool use_odometry;
bool use_nav_sat;
bool use_landmarks;
bool publish_frame_projected_to_2d;
int num_laser_scans;
int num_multi_echo_laser_scans;
int num_subdivisions_per_laser_scan;
int num_point_clouds;
double rangefinder_sampling_ratio;
double odometry_sampling_ratio;
double fixed_frame_pose_sampling_ratio;
double imu_sampling_ratio;
double landmarks_sampling_ratio;
};
其中的成员都能在lua文件中找到对应的。
3.2 LoadOptions()
cpp
std::tuple<NodeOptions, TrajectoryOptions> LoadOptions(
const std::string& configuration_directory,
const std::string& configuration_basename);
std::tie(node_options, trajectory_options) =
LoadOptions(FLAGS_configuration_directory, FLAGS_configuration_basename);
LoadOptions()用于从配置文件路径读取其中配置的参数内容,其返回值是一个std::tuple<NodeOptions, TrajectoryOptions> 类型的元组,用于组合返回两个返回值。
std::tie() 是 C++11 标准引入的新特性。std::tie() 定义在 <tuple> 头文件中,它可以将多个变量 "打包" 成一个 tuple 右值引用,主要用于接收函数返回的 std::tuple 并解包到多个变量中,方便进行多变量赋值操作。在 C++11 之前,没有 std::tie()
函数,处理多返回值通常需要使用结构体或指针参数,相比之下 std::tie()
提供了更简洁的语法。
接下来看一下LoadOptions()的具体内容:
cpp
/**
* @brief 加载lua配置文件中的参数
*
* @param[in] configuration_directory: 配置文件所在目录
* @param[in] configuration_basename: 配置文件的名字
* @return std::tuple<NodeOptions, TrajectoryOptions>: 返回节点的配置与轨迹的配置
*/
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));
}
其中file_resolver根据配置文件路径与文件名,读取了lua文件的内容,并将其转化为std::string返回。之后将lua文件的内容转化为了一个LuaParameterDictionary类型字典,可以通过其提供的方法,根据键值对的形式,根据"键",获得值。
3.3 LuaParameterDictionary
cpp
// 从 Lua 代码加载的参数字典
class LuaParameterDictionary {
public:
// 从 Lua 表规范构造字典
LuaParameterDictionary (const std::string& code,std::unique_ptr<FileResolver> file_resolver);
// 禁止拷贝构造函数
LuaParameterDictionary (const LuaParameterDictionary&) = delete;
// 禁止赋值运算符
LuaParameterDictionary& operator=(const LuaParameterDictionary&) = delete;
// 构造不使用引用计数的 LuaParameterDictionary
static std::unique_ptr<LuaParameterDictionary> NonReferenceCounted(const std::string& code, std::unique_ptr<FileResolver> file_resolver);
~LuaParameterDictionary();
// 返回所有可用的键
std::vectorstd::string GetKeys() const;
// 如果键存在于字典中则返回 true
bool HasKey (const std::string& key) const;
// 这些方法会检查(CHECK)'key' 是否存在
std::string GetString (const std::string& key);
double GetDouble (const std::string& key);
int GetInt (const std::string& key);
bool GetBool (const std::string& key);
std::unique_ptr<LuaParameterDictionary> GetDictionary(const std::string& key);
// 从字典中获取一个整数,并检查它是非负的
int GetNonNegativeInt (const std::string& key);
// 返回此 LuaParameterDictionary 的字符串表示形式
std::string ToString () const;
// 将键 '1'、'2'、'3' 的值作为指定类型返回
std::vector<double> GetArrayValuesAsDoubles();
std::vectorstd::string GetArrayValuesAsStrings();
std::vector<std::unique_ptr<LuaParameterDictionary>>
GetArrayValuesAsDictionaries();
private:
// 引用计数的枚举类型
enum class ReferenceCount {YES, NO};
// 构造函数,带有引用计数参数
LuaParameterDictionary (const std::string& code,ReferenceCount reference_count,std::unique_ptr<FileResolver> file_resolver);
// 供 GetDictionary () 使用的构造函数
LuaParameterDictionary (lua_State* L, ReferenceCount reference_count,std::shared_ptr<FileResolver> file_resolver);
// 用于 ToString () 的递归函数,跟踪缩进
std::string DoToString (const std::string& indent) const;
// 弹出栈顶元素,并检查其类型是否正确
double PopDouble () const;
int PopInt () const;
bool PopBool () const;
// 弹出栈顶元素,并检查其是否为字符串。返回值可以是适合 Lua 解释器读取的带引号形式,也可以是不带引号的形式。
enum class Quoted {YES, NO};
std::string PopString (Quoted quoted) const;
// 从栈顶的 Lua 表创建一个 LuaParameterDictionary,可以选择是否使用引用计数
std::unique_ptr<LuaParameterDictionary> PopDictionary(ReferenceCount reference_count) const;
// 检查(CHECK)'key' 是否在字典中
void CheckHasKey (const std::string& key) const;
// 检查(CHECK)'key' 是否在字典中,并标记它已被使用
void CheckHasKeyAndReference (const std::string& key);
// 如果需要,可以在派生类的析构函数中调用。它会检查配置中定义的所有键是否都被恰好使用了一次,并重置引用计数器。
void CheckAllKeysWereUsedExactlyOnceAndReset ();
// 将文件内容读入 Lua 字符串
static int LuaRead (lua_State* L);
// 处理其他 Lua 文件的包含,并防止重复包含
static int LuaInclude (lua_State* L);
lua_State* L_; // 按 Lua 世界的惯例命名
int index_into_reference_table_;
// 与所有子字典共享
const std::shared_ptr<FileResolver> file_resolver_;
// 如果为 true,将在析构时检查所有键是否都被使用过
const ReferenceCount reference_count_;
// 每次调用 Get * 方法时都会修改,用于验证所有参数都被恰好读取一次
std::map<std::string, int> reference_counts_;
// 所有已包含文件的列表,按包含顺序排列。用于防止重复包含。
std::vectorstd::string included_files_;
};
可以看到LuaParameterDictionary类中提供了HasKey(const std::string& key)用于检查对应key是否存在字典中,GetString(const std::string& key)、GetDouble(const std::string& key)......用于获取key对应的参数值。
LuaParameterDictionary的构造函数内容如下:
cpp
/**
* @brief Construct a new Lua Parameter Dictionary:: Lua Parameter Dictionary object
*
* @param[in] code 配置文件内容
* @param[in] file_resolver FileResolver类
*/
LuaParameterDictionary::LuaParameterDictionary(
const std::string& code, std::unique_ptr<FileResolver> file_resolver)
: LuaParameterDictionary(code, ReferenceCount::YES,
std::move(file_resolver)) {}
/**
* @brief Construct a new Lua Parameter Dictionary:: Lua Parameter Dictionary object
* 根据给定的字符串, 生成一个lua字典
*
* @param[in] code 配置文件内容
* @param[in] reference_count
* @param[in] file_resolver FileResolver类
*/
LuaParameterDictionary::LuaParameterDictionary(
const std::string& code, ReferenceCount reference_count,
std::unique_ptr<FileResolver> file_resolver)
: L_(luaL_newstate()),
index_into_reference_table_(-1),
file_resolver_(std::move(file_resolver)),
reference_count_(reference_count) {
CHECK_NOTNULL(L_);
SetDictionaryInRegistry(L_, this);
luaL_openlibs(L_);
lua_register(L_, "choose", LuaChoose);
// 将LuaInclude注册为Lua的全局函数变量,使得Lua可以调用C函数
lua_register(L_, "include", LuaInclude);
lua_register(L_, "read", LuaRead);
// luaL_loadstring()函数 将一个字符串code加载为 Lua 代码块
CheckForLuaErrors(L_, luaL_loadstring(L_, code.c_str()));
CheckForLuaErrors(L_, lua_pcall(L_, 0, 1, 0));
CheckTableIsAtTopOfStack(L_);
}
可以看到其中提供了LuaInclude函数用于处理lua文件中的include。其内容如下:
cpp
// Lua 函数:在当前 Lua 环境中运行脚本文件,仅接受文件名作为唯一参数
// 功能:读取并执行指定 Lua 文件,同时处理文件中的 include 指令,确保每个文件只被包含一次
int LuaParameterDictionary::LuaInclude(lua_State* L)
{
CHECK_EQ(lua_gettop(L), 1);
CHECK(lua_isstring(L, -1)) << "include takes a filename.";
LuaParameterDictionary* parameter_dictionary = GetDictionaryFromRegistry(L);
const std::string basename = lua_tostring(L, -1);
// 通过文件解析器获取文件的完整路径,如果失败则终止程序
const std::string filename =
parameter_dictionary->file_resolver_->GetFullPathOrDie(basename);
// 检查该文件是否已被包含过(防止重复包含)
if (std::find(parameter_dictionary->included_files_.begin(),
parameter_dictionary->included_files_.end(),
filename) != parameter_dictionary->included_files_.end()) {
std::string error_msg =
"Tried to include " + filename +
" twice. Already included files in order of inclusion: ";
for (const std::string& filename : parameter_dictionary->included_files_) {
error_msg.append(filename);
error_msg.append("\n");
}
LOG(FATAL) << error_msg;
}
// 将当前文件添加到已包含文件列表中,标记为已处理
parameter_dictionary->included_files_.push_back(filename);
lua_pop(L, 1);
CHECK_EQ(lua_gettop(L), 0);
// 通过文件解析器获取文件内容,如果失败则终止程序
const std::string content =
parameter_dictionary->file_resolver_->GetFileContentOrDie(basename);
CheckForLuaErrors(
L, luaL_loadbuffer(L, content.c_str(), content.size(), filename.c_str()));
CheckForLuaErrors(L, lua_pcall(L, 0, LUA_MULTRET, 0));
return lua_gettop(L);
}
这个函数实现了一个安全的 Lua 脚本包含机制,类似于 C/C++ 中的 #include
,增加了防止重复包含的保护机制。
3.4 CreateNodeOptions(&lua_parameter_dictionary)
cpp
return std::make_tuple(CreateNodeOptions(&lua_parameter_dictionary),
CreateTrajectoryOptions(&lua_parameter_dictionary));
在LoadOptions()函数的return中,使用了CreateNodeOptions()、CreateTrajectoryOptions()两个函数根据lua字典的内容创建了两个配置结构体对象。
cpp
/**
* @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");
}
// 判断字典中是否存在key
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;
}
3.5 CreateTrajectoryOptions(&lua_parameter_dictionary)
cpp
/**
* @brief 读取lua文件内容, 将lua文件的内容赋值给TrajectoryOptions
*
* @param[in] lua_parameter_dictionary lua字典
* @return TrajectoryOptions
*/
TrajectoryOptions CreateTrajectoryOptions(
::cartographer::common::LuaParameterDictionary* const
lua_parameter_dictionary) {
TrajectoryOptions options;
options.trajectory_builder_options =
::cartographer::mapping::CreateTrajectoryBuilderOptions(
lua_parameter_dictionary->GetDictionary("trajectory_builder").get());
options.tracking_frame =
lua_parameter_dictionary->GetString("tracking_frame");
options.published_frame =
lua_parameter_dictionary->GetString("published_frame");
options.odom_frame = lua_parameter_dictionary->GetString("odom_frame");
options.provide_odom_frame =
lua_parameter_dictionary->GetBool("provide_odom_frame");
options.use_odometry = lua_parameter_dictionary->GetBool("use_odometry");
options.use_nav_sat = lua_parameter_dictionary->GetBool("use_nav_sat");
options.use_landmarks = lua_parameter_dictionary->GetBool("use_landmarks");
options.publish_frame_projected_to_2d =
lua_parameter_dictionary->GetBool("publish_frame_projected_to_2d");
options.num_laser_scans =
lua_parameter_dictionary->GetNonNegativeInt("num_laser_scans");
options.num_multi_echo_laser_scans =
lua_parameter_dictionary->GetNonNegativeInt("num_multi_echo_laser_scans");
options.num_subdivisions_per_laser_scan =
lua_parameter_dictionary->GetNonNegativeInt(
"num_subdivisions_per_laser_scan");
options.num_point_clouds =
lua_parameter_dictionary->GetNonNegativeInt("num_point_clouds");
options.rangefinder_sampling_ratio =
lua_parameter_dictionary->GetDouble("rangefinder_sampling_ratio");
options.odometry_sampling_ratio =
lua_parameter_dictionary->GetDouble("odometry_sampling_ratio");
options.fixed_frame_pose_sampling_ratio =
lua_parameter_dictionary->GetDouble("fixed_frame_pose_sampling_ratio");
options.imu_sampling_ratio =
lua_parameter_dictionary->GetDouble("imu_sampling_ratio");
options.landmarks_sampling_ratio =
lua_parameter_dictionary->GetDouble("landmarks_sampling_ratio");
CheckTrajectoryOptions(options);
return options;
}
对options的参数内容的访问可以直接通过如node_options_.publish_tracked_pose的形式访问。
由于本人对lua的使用较少,所以这里只进行简单介绍。