Adds a PointsProcessor that can write ROS maps. (#548)

Fixes #475.

cartographer_ros/cartographer_ros/CMakeLists.txt

@@ -15,6 +15,8 @@
 google_binary(cartographer_assets_writer
   SRCS
     assets_writer_main.cc
+    ros_map_writing_points_processor.h
+    ros_map_writing_points_processor.cc
 )
 
 install(TARGETS cartographer_assets_writer

cartographer_ros/cartographer_ros/assets_writer_main.cc

@@ -32,6 +32,7 @@
 #include "cartographer/sensor/range_data.h"
 #include "cartographer/transform/transform_interpolation_buffer.h"
 #include "cartographer_ros/msg_conversion.h"
+#include "cartographer_ros/ros_map_writing_points_processor.h"
 #include "cartographer_ros/split_string.h"
 #include "cartographer_ros/time_conversion.h"
 #include "cartographer_ros/urdf_reader.h"
@@ -154,6 +155,16 @@ void Run(const string& pose_graph_filename,
   carto::io::PointsProcessorPipelineBuilder builder;
   carto::io::RegisterBuiltInPointsProcessors(all_trajectories,
                                              file_writer_factory, &builder);
+  builder.Register(
+      RosMapWritingPointsProcessor::kConfigurationFileActionName,
+      [file_writer_factory](
+          ::cartographer::common::LuaParameterDictionary* const dictionary,
+          ::cartographer::io::PointsProcessor* const next)
+          -> std::unique_ptr<::cartographer::io::PointsProcessor> {
+        return RosMapWritingPointsProcessor::FromDictionary(file_writer_factory,
+                                                            dictionary, next);
+      });
+
   std::vector<std::unique_ptr<carto::io::PointsProcessor>> pipeline =
       builder.CreatePipeline(
           lua_parameter_dictionary.GetDictionary("pipeline").get());

cartographer_ros/cartographer_ros/ros_map_writing_points_processor.cc

@@ -0,0 +1,124 @@
+/*
+ * 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/ros_map_writing_points_processor.h"
+
+#include "cartographer/common/make_unique.h"
+#include "cartographer/io/image.h"
+#include "cartographer/io/probability_grid_points_processor.h"
+
+namespace cartographer_ros {
+
+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";
+  file_writer->Write(header.data(), header.size());
+  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 string& pgm_filename, const Eigen::Array2i& offset,
+               ::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) +
+      ", 0.]\nnegate: 0\noccupied_thresh: 0.65\nfree_thresh: 0.196\n";
+  file_writer->Write(output.data(), output.size());
+}
+
+}  // namespace
+
+RosMapWritingPointsProcessor::RosMapWritingPointsProcessor(
+    const double resolution,
+    const ::cartographer::mapping_2d::proto::RangeDataInserterOptions&
+        range_data_inserter_options,
+    ::cartographer::io::FileWriterFactory file_writer_factory,
+    const string& filestem, ::cartographer::io::PointsProcessor* const next)
+    : filestem_(filestem),
+      next_(next),
+      file_writer_factory_(file_writer_factory),
+      range_data_inserter_(range_data_inserter_options),
+      probability_grid_(::cartographer::io::CreateProbabilityGrid(resolution)) {
+}
+
+std::unique_ptr<RosMapWritingPointsProcessor>
+RosMapWritingPointsProcessor::FromDictionary(
+    ::cartographer::io::FileWriterFactory file_writer_factory,
+    ::cartographer::common::LuaParameterDictionary* const dictionary,
+    ::cartographer::io::PointsProcessor* const next) {
+  return ::cartographer::common::make_unique<RosMapWritingPointsProcessor>(
+      dictionary->GetDouble("resolution"),
+      ::cartographer::mapping_2d::CreateRangeDataInserterOptions(
+          dictionary->GetDictionary("range_data_inserter").get()),
+      file_writer_factory, dictionary->GetString("filestem"), next);
+}
+
+void RosMapWritingPointsProcessor::Process(
+    std::unique_ptr<::cartographer::io::PointsBatch> batch) {
+  range_data_inserter_.Insert({batch->origin, batch->points, {}},
+                              &probability_grid_);
+  next_->Process(std::move(batch));
+}
+
+::cartographer::io::PointsProcessor::FlushResult
+RosMapWritingPointsProcessor::Flush() {
+  Eigen::Array2i offset;
+  std::unique_ptr<::cartographer::io::Image> image =
+      ::cartographer::io::DrawProbabilityGrid(probability_grid_, &offset);
+  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());
+    CHECK(pgm_writer->Close());
+
+    auto yaml_writer = file_writer_factory_(filestem_ + ".yaml");
+    WriteYaml(*image, probability_grid_.limits(), pgm_filename, offset,
+              yaml_writer.get());
+    CHECK(yaml_writer->Close());
+  }
+
+  switch (next_->Flush()) {
+    case FlushResult::kRestartStream:
+      LOG(FATAL) << "ROS map writing must be configured to occur after any "
+                    "stages that require multiple passes.";
+
+    case FlushResult::kFinished:
+      return FlushResult::kFinished;
+  }
+  LOG(FATAL);
+}
+
+}  // namespace cartographer_ros

cartographer_ros/cartographer_ros/ros_map_writing_points_processor.h

@@ -0,0 +1,64 @@
+/*
+ * 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.
+ */
+
+#ifndef CARTOGRAPHER_ROS_ROS_MAP_WRITING_POINTS_PROCESSOR_H_
+#define CARTOGRAPHER_ROS_ROS_MAP_WRITING_POINTS_PROCESSOR_H_
+
+#include "cartographer/common/lua_parameter_dictionary.h"
+#include "cartographer/io/file_writer.h"
+#include "cartographer/io/points_processor.h"
+#include "cartographer/mapping_2d/proto/range_data_inserter_options.pb.h"
+#include "cartographer/mapping_2d/range_data_inserter.h"
+
+namespace cartographer_ros {
+
+// Very similar to Cartographer's ProbabilityGridPointsProcessor, but writes
+// out a PGM and YAML suitable for ROS map server to consume.
+class RosMapWritingPointsProcessor
+    : public ::cartographer::io::PointsProcessor {
+ public:
+  constexpr static const char* kConfigurationFileActionName = "write_ros_map";
+  RosMapWritingPointsProcessor(
+      double resolution,
+      const ::cartographer::mapping_2d::proto::RangeDataInserterOptions&
+          range_data_inserter_options,
+      ::cartographer::io::FileWriterFactory file_writer_factory,
+      const string& filestem, PointsProcessor* next);
+  RosMapWritingPointsProcessor(const RosMapWritingPointsProcessor&) = delete;
+  RosMapWritingPointsProcessor& operator=(const RosMapWritingPointsProcessor&) =
+      delete;
+
+  static std::unique_ptr<RosMapWritingPointsProcessor> FromDictionary(
+      ::cartographer::io::FileWriterFactory file_writer_factory,
+      ::cartographer::common::LuaParameterDictionary* dictionary,
+      PointsProcessor* next);
+
+  ~RosMapWritingPointsProcessor() override {}
+
+  void Process(std::unique_ptr<::cartographer::io::PointsBatch> batch) override;
+  FlushResult Flush() override;
+
+ private:
+  const string filestem_;
+  PointsProcessor* const next_;
+  ::cartographer::io::FileWriterFactory file_writer_factory_;
+  ::cartographer::mapping_2d::RangeDataInserter range_data_inserter_;
+  ::cartographer::mapping_2d::ProbabilityGrid probability_grid_;
+};
+
+}  // namespace cartographer_ros
+
+#endif  // CARTOGRAPHER_ROS_ROS_MAP_WRITING_POINTS_PROCESSOR_H_