Pull out WritePgm and WriteYaml. (#603)

cartographer_ros/cartographer_ros/ros_map.cc

@@ -0,0 +1,49 @@
+/*
+ * Copyright 2017 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.h"
+
+namespace cartographer_ros {
+
+void WritePgm(const ::cartographer::io::Image& image, const double resolution,
+              ::cartographer::io::FileWriter* file_writer) {
+  const std::string header = "P5\n# Cartographer map; " +
+                             std::to_string(resolution) + " m/pixel\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) {
+      const char color = image.GetPixel(x, y)[0];
+      file_writer->Write(&color, 1);
+    }
+  }
+}
+
+void WriteYaml(const double resolution, const Eigen::Vector2d& origin,
+               const std::string& pgm_filename,
+               ::cartographer::io::FileWriter* file_writer) {
+  // 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(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());
+}
+
+}  // namespace cartographer_ros

cartographer_ros/cartographer_ros/ros_map.h

@@ -0,0 +1,41 @@
+/*
+ * Copyright 2017 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_H_
+#define CARTOGRAPHER_ROS_ROS_MAP_H_
+
+#include <string>
+
+#include "Eigen/Core"
+#include "cartographer/io/file_writer.h"
+#include "cartographer/io/image.h"
+#include "cartographer/mapping_2d/map_limits.h"
+
+namespace cartographer_ros {
+
+// Write 'image' as a pgm into 'file_writer'. The resolution is used in the
+// comment only'
+void WritePgm(const ::cartographer::io::Image& image, const double resolution,
+              ::cartographer::io::FileWriter* file_writer);
+
+// Write the corresponding yaml into 'file_writer'.
+void WriteYaml(const double resolution, const Eigen::Vector2d& origin,
+               const std::string& pgm_filename,
+               ::cartographer::io::FileWriter* file_writer);
+
+}  // namespace cartographer_ros
+
+#endif /* end of include guard: CARTOGRAPHER_ROS_ROS_MAP_H_ */

cartographer_ros/cartographer_ros/ros_map_writing_points_processor.cc

@@ -19,41 +19,10 @@
 #include "cartographer/common/make_unique.h"
 #include "cartographer/io/image.h"
 #include "cartographer/io/probability_grid_points_processor.h"
+#include "cartographer_ros/ros_map.h"
 
 namespace cartographer_ros {
 
-namespace {
-
-void WritePgm(const ::cartographer::io::Image& image, const double resolution,
-              ::cartographer::io::FileWriter* file_writer) {
-  const std::string header = "P5\n# Cartographer map; " +
-                             std::to_string(resolution) + " m/pixel\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) {
-      const char color = image.GetPixel(x, y)[0];
-      file_writer->Write(&color, 1);
-    }
-  }
-}
-
-void WriteYaml(const double resolution, const Eigen::Vector2d& origin,
-               const std::string& pgm_filename,
-               ::cartographer::io::FileWriter* file_writer) {
-  // 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(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());
-}
-
-}  // namespace
-
 RosMapWritingPointsProcessor::RosMapWritingPointsProcessor(
     const double resolution,
     const ::cartographer::mapping_2d::proto::RangeDataInserterOptions&