write YAML for ROS map_server

cartographer/io/probability_grid_points_processor.cc

@@ -18,6 +18,7 @@
 
 #include "Eigen/Core"
 #include "absl/memory/memory.h"
+#include "absl/strings/str_format.h"
 #include "cartographer/common/lua_parameter_dictionary.h"
 #include "cartographer/common/math.h"
 #include "cartographer/io/draw_trajectories.h"
@@ -84,12 +85,14 @@ ProbabilityGridPointsProcessor::ProbabilityGridPointsProcessor(
         probability_grid_range_data_inserter_options,
     const DrawTrajectories& draw_trajectories, const OutputType& output_type,
     std::unique_ptr<FileWriter> file_writer,
+    std::unique_ptr<FileWriter> yaml_file_writer,
     const std::vector<mapping::proto::Trajectory>& trajectories,
     PointsProcessor* const next)
     : draw_trajectories_(draw_trajectories),
       output_type_(output_type),
       trajectories_(trajectories),
       file_writer_(std::move(file_writer)),
+      yaml_file_writer_(std::move(yaml_file_writer)),
       next_(next),
       range_data_inserter_(probability_grid_range_data_inserter_options),
       probability_grid_(
@@ -114,14 +117,16 @@ ProbabilityGridPointsProcessor::FromDictionary(
       dictionary->HasKey("output_type")
           ? OutputTypeFromString(dictionary->GetString("output_type"))
           : OutputType::kPng;
+
+  std::string filename = dictionary->GetString("filename") +
+                         FileExtensionFromOutputType(output_type);
+
   return absl::make_unique<ProbabilityGridPointsProcessor>(
       dictionary->GetDouble("resolution"),
       mapping::CreateProbabilityGridRangeDataInserterOptions2D(
           dictionary->GetDictionary("range_data_inserter").get()),
-      draw_trajectories, output_type,
-      file_writer_factory(dictionary->GetString("filename") +
-                          FileExtensionFromOutputType(output_type)),
-      trajectories, next);
+      draw_trajectories, output_type, file_writer_factory(filename),
+      file_writer_factory(filename + ".yaml"), trajectories, next);
 }
 
 void ProbabilityGridPointsProcessor::Process(
@@ -145,6 +150,30 @@ PointsProcessor::FlushResult ProbabilityGridPointsProcessor::Flush() {
       }
       image->WritePng(file_writer_.get());
       CHECK(file_writer_->Close());
+
+      // write YAML which confirms to http://wiki.ros.org/map_server
+      const char* format =
+          "image: %s\n"
+          "resolution: %f\n"
+          "origin: [%f, %f, 0]\n"
+          "occupied_thresh: 0.65\n"
+          "free_thresh: 0.196\n"
+          "negate: 0\n";
+
+      const cartographer::mapping::MapLimits& limits =
+          probability_grid_.limits();
+      const cartographer::mapping::CellLimits& cell_limits =
+          limits.cell_limits();
+
+      double resolution = limits.resolution();
+      std::string yaml_str = absl::StrFormat(
+          format, file_writer_->GetFilename().c_str(), resolution,
+          limits.max().x() -
+              resolution * (cell_limits.num_x_cells - offset.x()),
+          limits.max().y() -
+              resolution * (cell_limits.num_y_cells - offset.y()));
+      yaml_file_writer_->Write(yaml_str.c_str(), yaml_str.length());
+      CHECK(yaml_file_writer_->Close());
     }
   } else if (output_type_ == OutputType::kPb) {
     const auto probability_grid_proto = probability_grid_.ToProto();

cartographer/io/probability_grid_points_processor.h

@@ -48,6 +48,7 @@ class ProbabilityGridPointsProcessor : public PointsProcessor {
           probability_grid_range_data_inserter_options,
       const DrawTrajectories& draw_trajectories, const OutputType& output_type,
       std::unique_ptr<FileWriter> file_writer,
+      std::unique_ptr<FileWriter> yaml_file_writer,
       const std::vector<mapping::proto::Trajectory>& trajectories,
       PointsProcessor* next);
   ProbabilityGridPointsProcessor(const ProbabilityGridPointsProcessor&) =
@@ -70,6 +71,7 @@ class ProbabilityGridPointsProcessor : public PointsProcessor {
   const OutputType output_type_;
   const std::vector<mapping::proto::Trajectory> trajectories_;
   std::unique_ptr<FileWriter> file_writer_;
+  std::unique_ptr<FileWriter> yaml_file_writer_;
   PointsProcessor* const next_;
   mapping::ProbabilityGridRangeDataInserter2D range_data_inserter_;
   mapping::ValueConversionTables conversion_tables_;

cartographer/io/probability_grid_points_processor_test.cc

@@ -44,11 +44,15 @@ std::unique_ptr<PointsBatch> CreatePointsBatch() {
 }
 
 ::cartographer::io::FileWriterFactory CreateFakeFileWriterFactory(
-    const std::string& expected_filename,
+    const std::string& expected_filename1,
+    const std::string& expected_filename2,
     std::shared_ptr<std::vector<char>> fake_file_writer_output) {
-  return [&fake_file_writer_output,
-          &expected_filename](const std::string& full_filename) {
-    EXPECT_EQ(expected_filename, full_filename);
+  return [&fake_file_writer_output, &expected_filename1,
+          &expected_filename2](const std::string& full_filename) {
+    EXPECT_PRED3(
+        [](auto str, auto s1, auto s2) { return str == s1 || str == s2; },
+        full_filename, expected_filename1, expected_filename2);
+
     return ::absl::make_unique<::cartographer::io::FakeFileWriter>(
         full_filename, fake_file_writer_output);
   };
@@ -125,10 +129,11 @@ class ProbabilityGridPointsProcessorTest : public ::testing::Test {
   ProbabilityGridPointsProcessorTest()
       : pipeline_dictionary_(CreateParameterDictionary()) {}
 
-  void Run(const std::string& expected_filename) {
+  void Run(const std::string& expected_filename1,
+           const std::string& expected_filename2) {
     const auto pipeline = CreatePipelineFromDictionary(
         pipeline_dictionary_.get(), dummy_trajectories_,
-        CreateFakeFileWriterFactory(expected_filename,
+        CreateFakeFileWriterFactory(expected_filename1, expected_filename2,
                                     fake_file_writer_output_));
     EXPECT_TRUE(pipeline.size() > 0);
 
@@ -149,7 +154,7 @@ TEST_F(ProbabilityGridPointsProcessorTest, WriteProto) {
   const auto expected_prob_grid_proto = CreateExpectedProbabilityGrid(
       CreatePointsBatch(),
       pipeline_dictionary_->GetArrayValuesAsDictionaries().front().get());
-  Run("map.pb");
+  Run("map.pb", "map.pb.yaml");
   EXPECT_THAT(*fake_file_writer_output_,
               ::testing::ContainerEq(expected_prob_grid_proto));
 }