实现cartographer自动保存建好的地图 针对github tag 0.3.0版本
1.ros下建好的地图一般是使用命令$ rosrunmap_server map_saver -f map_name
一般使用gmapping hector建立的地图使用上述命令保存的地图是没问题的
2.但是在cartographer中使用上述命令也能保存地图但是 保存的地图有点问题，如下图
知乎上推荐 rosservice call /finish_trajectory "map" 默认会保存在.ros文件夹中 方法 会报错
改成 rosservice call /finish_trajectory 0 也会报错
3.于是要分析修改代码，在node_main.c文件添加2个自定义函
添加函数 1 FillSubmapSlice()
添加函数 2 save_to_tos_map(string map_pgm)
//map_pgm 要保存的地图路径跟名字，如"/home/aaa/map/2B_map" 地图存放的目录下
在run() 最后面对save_to_tos_map()调用即可自动保存建好的地图
整个node_main.c 文件 代码
/*
* Copyright 2016 The Cartographer Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#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"
#include <map>
#include <string>
#include "cartographer/io/proto_stream.h"
#include "cartographer/io/submap_painter.h"
#include "cartographer/mapping/proto/pose_graph.pb.h"
#include "cartographer/mapping/proto/serialization.pb.h"
#include "cartographer/mapping/proto/submap.pb.h"
#include "cartographer/mapping/submaps.h"
#include "cartographer/mapping_2d/probability_grid.h"
#include "cartographer/mapping_2d/submaps.h"
#include "cartographer/mapping_3d/submaps.h"
#include "cartographer_ros/ros_map.h"
#include "cartographer_ros/submap.h"
#include "glog/logging.h"
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(map_filename, "", "If non-empty, filename of a map to load.");
DEFINE_bool(
start_trajectory_with_default_topics, true,
"Enable to immediately start the first trajectory with default topics.");
DEFINE_string(
save_map_filename, "",
"If non-empty, serialize state and write it to disk before shutting down.");
namespace cartographer_ros {
namespace {
// 注意一定要事先 建立home/tank/map/目录 不然会找不到目录
std::string map_path_name = "/home/tank/map/sick_map_data.pbstream";
void FillSubmapSlice(
const ::cartographer::transform::Rigid3d& global_submap_pose,
const ::cartographer::mapping::proto::Submap& proto,
::cartographer::io::SubmapSlice* const submap_slice) {
::cartographer::mapping::proto::SubmapQuery::Response response;
::cartographer::transform::Rigid3d local_pose;
if (proto.has_submap_3d()) {
::cartographer::mapping_3d::Submap submap(proto.submap_3d());
local_pose = submap.local_pose();
submap.ToResponseProto(global_submap_pose, &response);
} else {
::cartographer::mapping_2d::Submap submap(proto.submap_2d());
local_pose = submap.local_pose();
submap.ToResponseProto(global_submap_pose, &response);
}
submap_slice->pose = global_submap_pose;
auto& texture_proto = response.textures(0);
const SubmapTexture::Pixels pixels = UnpackTextureData(
texture_proto.cells(), texture_proto.width(), texture_proto.height());
submap_slice->width = texture_proto.width();
submap_slice->height = texture_proto.height();
submap_slice->resolution = texture_proto.resolution();
submap_slice->slice_pose =
::cartographer::transform::ToRigid3(texture_proto.slice_pose());
submap_slice->surface =
DrawTexture(pixels.intensity, pixels.alpha, texture_proto.width(),
texture_proto.height(), &submap_slice->cairo_data);
}
void save_to_ros_map(const std::string& map_pgm)
{
double resolution = 0.05;
::cartographer::io::ProtoStreamReader reader(map_path_name);
::cartographer::mapping::proto::PoseGraph pose_graph;
CHECK(reader.ReadProto(&pose_graph));
LOG(INFO) << "Loading submap slices from serialized data. pbstream_filename"<<map_path_name;
std::map<::cartographer::mapping::SubmapId, ::cartographer::io::SubmapSlice>
submap_slices;
for (;;) {
::cartographer::mapping::proto::SerializedData proto;
if (!reader.ReadProto(&proto)) {
break;
}
if (proto.has_submap()) {
const auto& submap = proto.submap();
const ::cartographer::mapping::SubmapId id{
submap.submap_id().trajectory_id(),
submap.submap_id().submap_index()};
const ::cartographer::transform::Rigid3d global_submap_pose =
::cartographer::transform::ToRigid3(
pose_graph.trajectory(id.trajectory_id)
.submap(id.submap_index)
.pose());
FillSubmapSlice(global_submap_pose, submap, &submap_slices[id]);
}
}
CHECK(reader.eof());
LOG(INFO) << "Generating combined map image from submap slices.";
auto result =
::cartographer::io::PaintSubmapSlices(submap_slices, resolution);
::cartographer::io::StreamFileWriter pgm_writer(map_pgm + ".pgm");
::cartographer::io::Image image(std::move(result.surface));
WritePgm(image, resolution, &pgm_writer);
const Eigen::Vector2d origin(
-result.origin.x() * resolution,
(result.origin.y() - image.height()) * resolution);
::cartographer::io::StreamFileWriter yaml_writer(map_pgm + ".yaml");
WriteYaml(resolution, origin, pgm_writer.GetFilename(), &yaml_writer);
}
void Run() {
constexpr double kTfBufferCacheTimeInSeconds = 1e6;
tf2_ros::Buffer tf_buffer{::ros::Duration(kTfBufferCacheTimeInSeconds)};
tf2_ros::TransformListener tf(tf_buffer);
NodeOptions node_options;
TrajectoryOptions trajectory_options;
std::tie(node_options, trajectory_options) =
LoadOptions(FLAGS_configuration_directory, FLAGS_configuration_basename);
Node node(node_options, &tf_buffer);
if (!FLAGS_map_filename.empty()) {
LOG(ERROR)<<"-----------------LoadMap() FLAGS_map_filename:"<<FLAGS_map_filename;
node.LoadMap(FLAGS_map_filename);
}
if (FLAGS_start_trajectory_with_default_topics) {
node.StartTrajectoryWithDefaultTopics(trajectory_options);
}
::ros::spin();
LOG(ERROR)<<"-------::ros::spin();end------";
node.FinishAllTrajectories();
node.RunFinalOptimization();
if (!FLAGS_save_map_filename.empty()) {
//LOG(ERROR)<<"-------FLAGS_save_map_filename------mapname"<<FLAGS_save_map_filename;
//node.SerializeState(FLAGS_save_map_filename);
}
node.SerializeState(map_path_name);
save_to_ros_map("/home/tank/map/sick_2B_map_0201");// 注意一定要事先 建立home/tank/map/目录 不然会找不到目录
}
} // namespace
} // namespace cartographer_ros
int main(int argc, char** argv) {
google::InitGoogleLogging(argv[0]);
google::ParseCommandLineFlags(&argc, &argv, true);
CHECK(!FLAGS_configuration_directory.empty())
<< "-configuration_directory is missing.";
CHECK(!FLAGS_configuration_basename.empty())
<< "-configuration_basename is missing.";
/*
CHECK(!FLAGS_save_map_pgm.empty())
<< "-save_map_pgm is missing.";
*/
::ros::init(argc, argv, "cartographer_node");
::ros::start();
cartographer_ros::ScopedRosLogSink ros_log_sink;
cartographer_ros::Run();
::ros::shutdown();
}
4.终端启动 demo_sick_map.launch 文件 开始建图 建图图之后 终端crtl + C 结束建图
<!--
Copyright 2016 The Cartographer Authors
Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at
http://www.apache.org/licenses/LICENSE-2.0
Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
-->
<launch>
<param name="/use_sim_time" value="false" /> <!-- 不用模拟时间 -->
<!-- 启动sick 激光雷达节点 sick 10m 激光雷达建图launch文件-->
<include file="$(find sick_tim)/launch/sick_tim551_2050001.launch" />
<!-- 启动底盘 -->
<include file="$(find cartographer_ros)/launch/minimal_sick.launch"/>
<node name="cartographer_node" pkg="cartographer_ros"
type="cartographer_node" args="
-configuration_directory $(find cartographer_ros)/configuration_files
-configuration_basename revo_lds.lua"
output="screen">
<!-- <remap from="scan" to="horizontal_laser_2d" /> -->
</node>
<node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
type="cartographer_occupancy_grid_node" args="-resolution 0.05" />
<node name="rviz" pkg="rviz" type="rviz" required="true"
args="-d $(find cartographer_ros)/configuration_files/demo_2d.rviz" />
</launch>
下图 三面是透明玻璃 一面白墙 空旷地方建立的地图
