12345qiupeng
/
cartographer_ros
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Refactor PGM writing to be more explicit. (#602)

master
Holger Rapp 2017-11-16 20:31:06 +01:00 committed by Wally B. Feed
parent 7259bb0baf
commit 10eba093cc
1 changed files with 16 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
cartographer_ros/cartographer_ros/ros_map_writing_points_processor.cc
View File

@ -26,35 +26,28 @@ namespace {
void WritePgm(const ::cartographer::io::Image& image, const double resolution,
::cartographer::io::FileWriter* file_writer) {
// Flipping the image into ROS coordinate frame.
const std::string header = "P5\n# Cartographer map; " +
std::to_string(resolution) + " m/pixel\n" +
std::to_string(image.height()) + " " +
std::to_string(image.width()) + "\n255\n";
std::to_string(image.width()) + " " +
std::to_string(image.height()) + "\n255\n";
file_writer->Write(header.data(), header.size());
for (int y = 0; y < image.height(); ++y) {
for (int x = 0; x < image.width(); ++x) {
for (int y = image.height() - 1; y >= 0; --y) {
const char color = image.GetPixel(x, y)[0];
file_writer->Write(&color, 1);
}
}
}
void WriteYaml(const ::cartographer::io::Image& image,
const ::cartographer::mapping_2d::MapLimits& limits,
const std::string& pgm_filename, const Eigen::Array2i& offset,
void WriteYaml(const double resolution, const Eigen::Vector2d& origin,
const std::string& pgm_filename,
::cartographer::io::FileWriter* file_writer) {
const double resolution = limits.resolution();
const double x_offset =
limits.max().x() - (offset.y() + image.height()) * resolution;
const double y_offset =
limits.max().y() - (offset.x() + image.width()) * resolution;
// Magic constants taken directly from ros map_saver code:
// https://github.com/ros-planning/navigation/blob/ac41d2480c4cf1602daf39a6e9629142731d92b0/map_server/src/map_saver.cpp#L114
const std::string output =
"image: " + pgm_filename + "\n" +
"resolution: " + std::to_string(resolution) + "\n" + "origin: [" +
std::to_string(x_offset) + ", " + std::to_string(y_offset) +
std::to_string(origin.x()) + ", " + std::to_string(origin.y()) +
", 0.]\nnegate: 0\noccupied_thresh: 0.65\nfree_thresh: 0.196\n";
file_writer->Write(output.data(), output.size());
}
@ -102,12 +95,18 @@ RosMapWritingPointsProcessor::Flush() {
if (image != nullptr) {
auto pgm_writer = file_writer_factory_(filestem_ + ".pgm");
const std::string pgm_filename = pgm_writer->GetFilename();
WritePgm(*image, probability_grid_.limits().resolution(), pgm_writer.get());
const auto& limits = probability_grid_.limits();
image->Rotate90DegreesClockwise();
WritePgm(*image, limits.resolution(), pgm_writer.get());
CHECK(pgm_writer->Close());
const Eigen::Vector2d origin(
limits.max().x() - (offset.y() + image->width()) * limits.resolution(),
limits.max().y() -
(offset.x() + image->height()) * limits.resolution());
auto yaml_writer = file_writer_factory_(filestem_ + ".yaml");
WriteYaml(*image, probability_grid_.limits(), pgm_filename, offset,
yaml_writer.get());
WriteYaml(limits.resolution(), origin, pgm_filename, yaml_writer.get());
CHECK(yaml_writer->Close());
}