diff --git a/cartographer_ros/CMakeLists.txt b/cartographer_ros/CMakeLists.txt
index 7dd43c4..cc2fb9e 100644
--- a/cartographer_ros/CMakeLists.txt
+++ b/cartographer_ros/CMakeLists.txt
@@ -81,6 +81,8 @@ link_directories(${Boost_LIBRARY_DIRS})
 
 add_executable(cartographer_node
   src/cartographer_node_main.cc
+  src/map_writer.cc
+  src/map_writer.h
   src/msg_conversion.cc
   src/msg_conversion.h
   src/node_options.cc
@@ -100,7 +102,8 @@ target_link_libraries(cartographer_node
   ${CARTOGRAPHER_LIBRARIES}
   ${PCL_LIBRARIES}
   ${catkin_LIBRARIES}
-  gflags  # TODO(whess): CMake-ify?
+  gflags
+  yaml-cpp
 )
 add_dependencies(cartographer_node
   ${catkin_EXPORTED_TARGETS}
diff --git a/cartographer_ros/package.xml b/cartographer_ros/package.xml
index 059ae4b..4ff2d3c 100644
--- a/cartographer_ros/package.xml
+++ b/cartographer_ros/package.xml
@@ -56,6 +56,7 @@
   <depend>pcl_conversions</depend>
   <depend>eigen_conversions</depend>
   <depend>message_runtime</depend>
+  <depend>yaml-cpp</depend>
 
   <test_depend>rosunit</test_depend>
 
diff --git a/cartographer_ros/src/cartographer_node_main.cc b/cartographer_ros/src/cartographer_node_main.cc
index 5e98411..d1429a5 100644
--- a/cartographer_ros/src/cartographer_node_main.cc
+++ b/cartographer_ros/src/cartographer_node_main.cc
@@ -15,7 +15,6 @@
  */
 
 #include <chrono>
-#include <fstream>
 #include <map>
 #include <queue>
 #include <string>
@@ -63,6 +62,7 @@
 #include "tf2_ros/transform_broadcaster.h"
 #include "tf2_ros/transform_listener.h"
 
+#include "map_writer.h"
 #include "msg_conversion.h"
 #include "node_options.h"
 #include "occupancy_grid.h"
@@ -479,30 +479,11 @@ bool Node::HandleFinishTrajectory(
     const auto trajectory_nodes =
         map_builder_.sparse_pose_graph()->GetTrajectoryNodes();
     if (!trajectory_nodes.empty()) {
-      std::string filename = request.stem + ".pgm";
-      LOG(INFO) << "Saving final map to '" << filename << "'...";
-      ::nav_msgs::OccupancyGrid grid;
-      BuildOccupancyGrid(trajectory_nodes, options_, &grid);
-      std::ofstream output_file(filename, std::ios::out | std::ios::binary);
-      const std::string header =
-          "P5\n# Cartographer map; " + std::to_string(grid.info.resolution) +
-          " m/pixel\n" + std::to_string(grid.info.width) + " " +
-          std::to_string(grid.info.height) + "\n255\n";
-      output_file.write(header.data(), header.size());
-      for (size_t y = 0; y < grid.info.height; ++y) {
-        for (size_t x = 0; x < grid.info.width; ++x) {
-          size_t i = x + (grid.info.height - y - 1) * grid.info.width;
-          if (grid.data[i] >= 0 && grid.data[i] <= 100) {
-            output_file.put((100 - grid.data[i]) * 254 / 100);
-          } else {
-            constexpr uint8_t kUnknownValue = 205;
-            output_file.put(kUnknownValue);
-          }
-        }
-      }
-      output_file.close();
-      CHECK(output_file) << "Writing '" << filename << "' failed.";
-      // TODO(whess): Also write a yaml file similar to map_saver?
+      ::nav_msgs::OccupancyGrid occupancy_grid;
+      BuildOccupancyGrid(trajectory_nodes, options_, &occupancy_grid);
+      WriteOccupancyGridToPgmAndYaml(occupancy_grid, request.stem);
+    } else {
+      LOG(WARNING) << "Map is empty and will not be saved.";
     }
   }
   return true;
diff --git a/cartographer_ros/src/map_writer.cc b/cartographer_ros/src/map_writer.cc
new file mode 100644
index 0000000..aae7f02
--- /dev/null
+++ b/cartographer_ros/src/map_writer.cc
@@ -0,0 +1,89 @@
+/*
+ * 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 "map_writer.h"
+
+#include <fstream>
+
+#include "glog/logging.h"
+#include "yaml-cpp/yaml.h"
+
+namespace cartographer_ros {
+
+namespace {
+
+void WriteOccupancyGridToPgm(const ::nav_msgs::OccupancyGrid& grid,
+                             const std::string& filename) {
+  LOG(INFO) << "Saving map to '" << filename << "'...";
+  std::ofstream pgm_file(filename, std::ios::out | std::ios::binary);
+  const std::string header = "P5\n# Cartographer map; " +
+                             std::to_string(grid.info.resolution) +
+                             " m/pixel\n" + std::to_string(grid.info.width) +
+                             " " + std::to_string(grid.info.height) + "\n255\n";
+  pgm_file.write(header.data(), header.size());
+  for (size_t y = 0; y < grid.info.height; ++y) {
+    for (size_t x = 0; x < grid.info.width; ++x) {
+      const size_t i = x + (grid.info.height - y - 1) * grid.info.width;
+      if (grid.data[i] >= 0 && grid.data[i] <= 100) {
+        pgm_file.put((100 - grid.data[i]) * 255 / 100);
+      } else {
+        // We choose a value between the free and occupied threshold.
+        constexpr uint8_t kUnknownValue = 128;
+        pgm_file.put(kUnknownValue);
+      }
+    }
+  }
+  pgm_file.close();
+  CHECK(pgm_file) << "Writing '" << filename << "' failed.";
+}
+
+void WriteOccupancyGridInfoToYaml(const ::nav_msgs::OccupancyGrid& grid,
+                                  const std::string& map_filename,
+                                  const std::string& yaml_filename) {
+  LOG(INFO) << "Saving map info to '" << yaml_filename << "'...";
+  std::ofstream yaml_file(yaml_filename, std::ios::out | std::ios::binary);
+  {
+    YAML::Emitter out(yaml_file);
+    out << YAML::BeginMap;
+    // TODO(whess): Use basename only?
+    out << YAML::Key << "image" << YAML::Value << map_filename;
+    out << YAML::Key << "resolution" << YAML::Value << grid.info.resolution;
+    // According to map_server documentation "many parts of the system currently
+    // ignore yaw" so it is good we use a zero value.
+    constexpr double kYawButMaybeIgnored = 0.;
+    out << YAML::Key << "origin" << YAML::Value << YAML::Flow << YAML::BeginSeq
+        << grid.info.origin.position.x << grid.info.origin.position.y
+        << kYawButMaybeIgnored << YAML::EndSeq;
+    out << YAML::Key << "occupied_thresh" << YAML::Value << 0.51;
+    out << YAML::Key << "free_thresh" << YAML::Value << 0.49;
+    out << YAML::Key << "negate" << YAML::Value << 0;
+    out << YAML::EndMap;
+    CHECK(out.good()) << out.GetLastError();
+  }
+  yaml_file.close();
+  CHECK(yaml_file) << "Writing '" << yaml_filename << "' failed.";
+}
+
+}  // namespace
+
+void WriteOccupancyGridToPgmAndYaml(
+    const ::nav_msgs::OccupancyGrid& occupancy_grid, const std::string& stem) {
+  const std::string pgm_filename = stem + ".pgm";
+  WriteOccupancyGridToPgm(occupancy_grid, pgm_filename);
+  WriteOccupancyGridInfoToYaml(occupancy_grid, pgm_filename, stem + ".yaml");
+}
+
+}  // namespace cartographer_ros
diff --git a/cartographer_ros/src/map_writer.h b/cartographer_ros/src/map_writer.h
new file mode 100644
index 0000000..f0159a1
--- /dev/null
+++ b/cartographer_ros/src/map_writer.h
@@ -0,0 +1,32 @@
+/*
+ * 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_MAP_WRITER_H_
+#define CARTOGRAPHER_ROS_MAP_WRITER_H_
+
+#include <string>
+
+#include "nav_msgs/OccupancyGrid.h"
+
+namespace cartographer_ros {
+
+// Writes the given 'occupancy_grid' as 'stem'.pgm and 'stem'.yaml.
+void WriteOccupancyGridToPgmAndYaml(
+    const ::nav_msgs::OccupancyGrid& occupancy_grid, const std::string& stem);
+
+}  // namespace cartographer_ros
+
+#endif  // CARTOGRAPHER_ROS_MAP_WRITER_H_