任务动机: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