Cartographer Standalone移植(1)—移植思路

任务动机:Cartographer建图对硬件资源要求高,为了实现在RK3399开发板上高质量的建图,需要使用各种方法降低资源消耗,包括操作系统对资源的消耗、ROS Core对资源的消耗。本文将Cartographer与ROS剥离,目的是在不启动ROS Core的基础上正常使用Cartographer建图,并通过对ROS Bridge的支持完成对分布式ROS开发板的通信。

任务描述:根据任务动机,研究移植研发思路,形成文档。

1. 输入/输出要求

  • 输入:激光雷达/drv/laser
  • 输出:地图 .pgm/.pbstream

2. 移植思路

        只安装cartographer和ceres-solver,以cartographer_ros为示例,编写一个自己的程序。

3. 移植过程

        在分析源码的过程中,我把所有的与ROS有关的类和相关库全部过滤。这个代码写的就像拔萝卜,开始拔以后发现地下除了泥还有树根。我打算直接用硬件激光雷达测试(因为没安装ros,还要写py脚本去从rosbag中解压激光雷达数据确实有些麻烦)。因此我在此写出我的思路,并在结尾提出疑问。

3.1 建图,开始拔萝卜之看见萝卜->建图前准备:载入参数

  NodeOptions node_options; //创建NodeOptions参数结构体
  TrajectoryOptions trajectory_options; //创建TrajectoryOptions参数结构体
  //把Lua的参数加载到程序中
  std::tie(node_options, trajectory_options) =
      LoadOptions(FLAGS_configuration_directory, FLAGS_configuration_basename);

......

std::tuple<NodeOptions, TrajectoryOptions> LoadOptions( //加载参数
    const std::string& configuration_directory,         //参数所在目录
    const std::string& configuration_basename) {
  //标准库,把字符串参数所在目录放到file_resolver这个目录中
  auto file_resolver =
      absl::make_unique<cartographer::common::ConfigurationFileResolver>(
          std::vector<std::string>{configuration_directory});
  //标准库+cartographer库,目的是把文件中的字符提取出来
  const std::string code =
      file_resolver->GetFileContentOrDie(configuration_basename);
  //cartographer库,相当于configuration_directory+configuration_basename
  cartographer::common::LuaParameterDictionary lua_parameter_dictionary(
      code, std::move(file_resolver));
  //返回参数
  return std::make_tuple(CreateNodeOptions(&lua_parameter_dictionary),
                         CreateTrajectoryOptions(&lua_parameter_dictionary));
}

......

//node结构体
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 use_pose_extrapolator = true;
};

//创建node参数,只依赖标准库和cartographer库
NodeOptions CreateNodeOptions(
    ::cartographer::common::LuaParameterDictionary* const
        lua_parameter_dictionary) {
  NodeOptions options;
  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("use_pose_extrapolator")) {
    options.use_pose_extrapolator =
        lua_parameter_dictionary->GetBool("use_pose_extrapolator");
  }
  return options;
}

······

//TrajectoryOptions结构体
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;
};

//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_od
评论 2
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值