Cartographer中的gflag与lua文件

目录

[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_metricsconfiguration_directoryconfiguration_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_BUILDERTRAJECTORY_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的使用较少,所以这里只进行简单介绍。