Enable OccupancyGridNode and remove generating of assets in the node. (#436)

This will allow us to keep way less information in memory for SLAMing and fixes crashes related to multi trajectory (e.g. this fixes #413).

It also means that to get to an X-Ray or a map, users need to run the asset writer now after SLAMing, which is inconvenient.
Remove dependency on YAML and delete more dead code.

cartographer_ros/CMakeLists.txt

@@ -44,7 +44,6 @@ google_enable_testing()
 find_package(catkin REQUIRED COMPONENTS ${PACKAGE_DEPENDENCIES})
 
 include(FindPkgConfig)
-pkg_search_module(YAMLCPP REQUIRED yaml-cpp>=0.5.1)
 
 find_package(LuaGoogle REQUIRED)
 find_package(PCL REQUIRED COMPONENTS common io)
@@ -80,7 +79,6 @@ catkin_package(
     # TODO(damonkohler): This should be here but causes Catkin to abort because
     # protobuf specifies a library '-lpthread' instead of just 'pthread'.
     # CARTOGRAPHER
-    YAMLCPP
     PCL
     EIGEN3
     Boost
@@ -99,10 +97,6 @@ add_subdirectory("cartographer_ros")
 
 target_link_libraries(${PROJECT_NAME} PUBLIC cartographer)
 
-# YAML
-target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC ${YAMLCPP_INCLUDE_DIRS})
-target_link_libraries(${PROJECT_NAME} PUBLIC ${YAMLCPP_LIBRARIES})
-
 # Lua
 target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC ${LUA_INCLUDE_DIR})
 

cartographer_ros/cartographer_ros/assets_writer.cc

@@ -1,105 +0,0 @@
-/*
- * 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/assets_writer.h"
-
-#include "cartographer/common/make_unique.h"
-#include "cartographer/common/time.h"
-#include "cartographer/io/file_writer.h"
-#include "cartographer/io/null_points_processor.h"
-#include "cartographer/io/ply_writing_points_processor.h"
-#include "cartographer/io/points_processor.h"
-#include "cartographer/io/xray_points_processor.h"
-#include "cartographer/mapping/proto/trajectory.pb.h"
-#include "cartographer/mapping_2d/proto/range_data_inserter_options.pb.h"
-#include "cartographer_ros/map_writer.h"
-#include "cartographer_ros/occupancy_grid.h"
-#include "nav_msgs/OccupancyGrid.h"
-
-namespace cartographer_ros {
-
-bool HasNonTrimmedNode(
-    const std::vector<std::vector<::cartographer::mapping::TrajectoryNode>>&
-        all_trajectory_nodes) {
-  for (const auto& trajectory_nodes : all_trajectory_nodes) {
-    for (const auto& node : trajectory_nodes) {
-      if (!node.trimmed()) {
-        return true;
-      }
-    }
-  }
-  return false;
-}
-
-void Write2DAssets(
-    const std::vector<std::vector<::cartographer::mapping::TrajectoryNode>>&
-        all_trajectory_nodes,
-    const string& map_frame,
-    const ::cartographer::mapping_2d::proto::SubmapsOptions& submaps_options,
-    const std::string& stem) {
-  ::nav_msgs::OccupancyGrid occupancy_grid;
-  BuildOccupancyGrid2D(all_trajectory_nodes, map_frame, submaps_options,
-                       &occupancy_grid);
-  WriteOccupancyGridToPgmAndYaml(occupancy_grid, stem);
-}
-
-void Write3DAssets(
-    const std::vector<std::vector<::cartographer::mapping::TrajectoryNode>>&
-        all_trajectory_nodes,
-    const double voxel_size, const std::string& stem) {
-  namespace carto = ::cartographer;
-  const auto file_writer_factory = [](const string& filename) {
-    return carto::common::make_unique<carto::io::StreamFileWriter>(filename);
-  };
-
-  carto::io::NullPointsProcessor null_points_processor;
-  carto::io::XRayPointsProcessor xy_xray_points_processor(
-      voxel_size,
-      carto::transform::Rigid3f::Rotation(
-          Eigen::AngleAxisf(-M_PI / 2.f, Eigen::Vector3f::UnitY())),
-      {}, stem + "_xray_xy", file_writer_factory, &null_points_processor);
-  carto::io::XRayPointsProcessor yz_xray_points_processor(
-      voxel_size,
-      carto::transform::Rigid3f::Rotation(
-          Eigen::AngleAxisf(M_PI, Eigen::Vector3f::UnitZ())),
-      {}, stem + "_xray_yz", file_writer_factory, &xy_xray_points_processor);
-  carto::io::XRayPointsProcessor xz_xray_points_processor(
-      voxel_size,
-      carto::transform::Rigid3f::Rotation(
-          Eigen::AngleAxisf(-M_PI / 2.f, Eigen::Vector3f::UnitZ())),
-      {}, stem + "_xray_xz", file_writer_factory, &yz_xray_points_processor);
-  carto::io::PlyWritingPointsProcessor ply_writing_points_processor(
-      file_writer_factory(stem + ".ply"), &xz_xray_points_processor);
-
-  for (size_t trajectory_id = 0; trajectory_id < all_trajectory_nodes.size();
-       ++trajectory_id) {
-    for (const auto& node : all_trajectory_nodes[trajectory_id]) {
-      const carto::sensor::RangeData range_data =
-          carto::sensor::TransformRangeData(
-              carto::sensor::Decompress(node.constant_data->range_data),
-              node.pose.cast<float>());
-      auto points_batch = carto::common::make_unique<carto::io::PointsBatch>();
-      points_batch->time = node.time();
-      points_batch->origin = range_data.origin;
-      points_batch->trajectory_id = trajectory_id;
-      points_batch->points = range_data.returns;
-      ply_writing_points_processor.Process(std::move(points_batch));
-    }
-  }
-  ply_writing_points_processor.Flush();
-}
-
-}  // namespace cartographer_ros

cartographer_ros/cartographer_ros/assets_writer.h

@@ -1,51 +0,0 @@
-/*
- * 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_ASSETS_WRITER_H_
-#define CARTOGRAPHER_ROS_ASSETS_WRITER_H_
-
-#include <string>
-#include <vector>
-
-#include "cartographer/mapping/trajectory_node.h"
-#include "cartographer_ros/node_options.h"
-
-namespace cartographer_ros {
-
-// Returns 'true' if there is at least one untrimmed node for any trajectory.
-// The Write?DAssets functions expects this to be 'true'.
-bool HasNonTrimmedNode(
-    const std::vector<std::vector<::cartographer::mapping::TrajectoryNode>>&
-        all_trajectory_nodes);
-
-// Writes a trajectory proto and an occupancy grid.
-void Write2DAssets(
-    const std::vector<std::vector<::cartographer::mapping::TrajectoryNode>>&
-        all_trajectory_nodes,
-    const string& map_frame,
-    const ::cartographer::mapping_2d::proto::SubmapsOptions& submaps_options,
-    const std::string& stem);
-
-// Writes X-ray images, trajectory proto, and PLY files from the
-// 'all_trajectory_nodes'. The filenames will all start with 'stem'.
-void Write3DAssets(
-    const std::vector<std::vector<::cartographer::mapping::TrajectoryNode>>&
-        all_trajectory_nodes,
-    const double voxel_size, const std::string& stem);
-
-}  // namespace cartographer_ros
-
-#endif  // CARTOGRAPHER_ROS_ASSETS_WRITER_H_

cartographer_ros/cartographer_ros/map_builder_bridge.cc

@@ -17,10 +17,8 @@
 #include "cartographer_ros/map_builder_bridge.h"
 
 #include "cartographer/io/proto_stream.h"
-#include "cartographer_ros/assets_writer.h"
 #include "cartographer_ros/color.h"
 #include "cartographer_ros/msg_conversion.h"
-#include "cartographer_ros/occupancy_grid.h"
 
 namespace cartographer_ros {
 
@@ -70,44 +68,12 @@ void MapBuilderBridge::FinishTrajectory(const int trajectory_id) {
   sensor_bridges_.erase(trajectory_id);
 }
 
-void MapBuilderBridge::SerializeState(const std::string& stem) {
+void MapBuilderBridge::SerializeState(const std::string& filename) {
-  cartographer::io::ProtoStreamWriter writer(stem + ".pbstream");
+  cartographer::io::ProtoStreamWriter writer(filename);
   map_builder_.SerializeState(&writer);
   CHECK(writer.Close()) << "Could not write state.";
 }
 
-void MapBuilderBridge::WriteAssets(const string& stem) {
-  const auto all_trajectory_nodes =
-      map_builder_.sparse_pose_graph()->GetTrajectoryNodes();
-  if (!HasNonTrimmedNode(all_trajectory_nodes)) {
-    LOG(WARNING) << "No data was collected and no assets will be written.";
-    return;
-  }
-  // Make sure there is a trajectory with id = 0.
-  CHECK_EQ(trajectory_options_.count(0), 1);
-  LOG(INFO) << "Writing assets with stem '" << stem << "'...";
-  if (node_options_.map_builder_options.use_trajectory_builder_2d()) {
-    // We arbitrarily use the submap_options() from the first trajectory to
-    // write the 2D assets.
-    Write2DAssets(
-        all_trajectory_nodes, node_options_.map_frame,
-        trajectory_options_[0]
-            .trajectory_builder_options.trajectory_builder_2d_options()
-            .submaps_options(),
-        stem);
-  }
-
-  if (node_options_.map_builder_options.use_trajectory_builder_3d()) {
-    Write3DAssets(
-        all_trajectory_nodes,
-        trajectory_options_[0]
-            .trajectory_builder_options.trajectory_builder_3d_options()
-            .submaps_options()
-            .high_resolution(),
-        stem);
-  }
-}
-
 bool MapBuilderBridge::HandleSubmapQuery(
     cartographer_ros_msgs::SubmapQuery::Request& request,
     cartographer_ros_msgs::SubmapQuery::Response& response) {
@@ -157,28 +123,6 @@ cartographer_ros_msgs::SubmapList MapBuilderBridge::GetSubmapList() {
   return submap_list;
 }
 
-std::unique_ptr<nav_msgs::OccupancyGrid>
-MapBuilderBridge::BuildOccupancyGrid() {
-  CHECK(node_options_.map_builder_options.use_trajectory_builder_2d())
-      << "Publishing OccupancyGrids for 3D data is not yet supported.";
-  const auto all_trajectory_nodes =
-      map_builder_.sparse_pose_graph()->GetTrajectoryNodes();
-  std::unique_ptr<nav_msgs::OccupancyGrid> occupancy_grid;
-  if (HasNonTrimmedNode(all_trajectory_nodes)) {
-    occupancy_grid =
-        cartographer::common::make_unique<nav_msgs::OccupancyGrid>();
-    // Make sure there is a trajectory with id = 0.
-    CHECK_EQ(trajectory_options_.count(0), 1);
-    BuildOccupancyGrid2D(
-        all_trajectory_nodes, node_options_.map_frame,
-        trajectory_options_[0]
-            .trajectory_builder_options.trajectory_builder_2d_options()
-            .submaps_options(),
-        occupancy_grid.get());
-  }
-  return occupancy_grid;
-}
-
 std::unordered_map<int, MapBuilderBridge::TrajectoryState>
 MapBuilderBridge::GetTrajectoryStates() {
   std::unordered_map<int, TrajectoryState> trajectory_states;

cartographer_ros/cartographer_ros/map_builder_bridge.h

@@ -54,15 +54,13 @@ class MapBuilderBridge {
   int AddTrajectory(const std::unordered_set<string>& expected_sensor_ids,
                     const TrajectoryOptions& trajectory_options);
   void FinishTrajectory(int trajectory_id);
-  void SerializeState(const string& stem);
+  void SerializeState(const string& filename);
-  void WriteAssets(const string& stem);
 
   bool HandleSubmapQuery(
       cartographer_ros_msgs::SubmapQuery::Request& request,
       cartographer_ros_msgs::SubmapQuery::Response& response);
 
   cartographer_ros_msgs::SubmapList GetSubmapList();
-  std::unique_ptr<nav_msgs::OccupancyGrid> BuildOccupancyGrid();
   std::unordered_map<int, TrajectoryState> GetTrajectoryStates();
   visualization_msgs::MarkerArray GetTrajectoryNodeList();
   visualization_msgs::MarkerArray GetConstraintList();

cartographer_ros/cartographer_ros/map_writer.cc

@@ -1,89 +0,0 @@
-/*
- * 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/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

cartographer_ros/cartographer_ros/map_writer.h

@@ -1,32 +0,0 @@
-/*
- * 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_

cartographer_ros/cartographer_ros/node.cc

@@ -68,16 +68,7 @@ Node::Node(const NodeOptions& node_options, tf2_ros::Buffer* const tf_buffer)
   service_servers_.push_back(node_handle_.advertiseService(
       kFinishTrajectoryServiceName, &Node::HandleFinishTrajectory, this));
   service_servers_.push_back(node_handle_.advertiseService(
-      kWriteAssetsServiceName, &Node::HandleWriteAssets, this));
+      kWriteStateServiceName, &Node::HandleWriteState, this));
-
-  if (node_options_.map_builder_options.use_trajectory_builder_2d()) {
-    occupancy_grid_publisher_ =
-        node_handle_.advertise<::nav_msgs::OccupancyGrid>(
-            kOccupancyGridTopic, kLatestOnlyPublisherQueueSize,
-            true /* latched */);
-    occupancy_grid_thread_ =
-        std::thread(&Node::SpinOccupancyGridThreadForever, this);
-  }
 
   scan_matched_point_cloud_publisher_ =
       node_handle_.advertise<sensor_msgs::PointCloud2>(
@@ -97,15 +88,7 @@ Node::Node(const NodeOptions& node_options, tf2_ros::Buffer* const tf_buffer)
       &Node::PublishConstraintList, this));
 }
 
-Node::~Node() {
+Node::~Node() {}
-  {
-    carto::common::MutexLocker lock(&mutex_);
-    terminating_ = true;
-  }
-  if (occupancy_grid_thread_.joinable()) {
-    occupancy_grid_thread_.join();
-  }
-}
 
 ::ros::NodeHandle* Node::node_handle() { return &node_handle_; }
 
@@ -201,25 +184,6 @@ void Node::PublishConstraintList(
   }
 }
 
-void Node::SpinOccupancyGridThreadForever() {
-  for (;;) {
-    std::this_thread::sleep_for(std::chrono::milliseconds(1000));
-    {
-      carto::common::MutexLocker lock(&mutex_);
-      if (terminating_) {
-        return;
-      }
-    }
-    if (occupancy_grid_publisher_.getNumSubscribers() == 0) {
-      continue;
-    }
-    const auto occupancy_grid = map_builder_bridge_.BuildOccupancyGrid();
-    if (occupancy_grid != nullptr) {
-      occupancy_grid_publisher_.publish(*occupancy_grid);
-    }
-  }
-}
-
 std::unordered_set<string> Node::ComputeExpectedTopics(
     const TrajectoryOptions& options,
     const cartographer_ros_msgs::SensorTopics& topics) {
@@ -429,12 +393,11 @@ bool Node::HandleFinishTrajectory(
   return true;
 }
 
-bool Node::HandleWriteAssets(
+bool Node::HandleWriteState(
-    ::cartographer_ros_msgs::WriteAssets::Request& request,
+    ::cartographer_ros_msgs::WriteState::Request& request,
-    ::cartographer_ros_msgs::WriteAssets::Response& response) {
+    ::cartographer_ros_msgs::WriteState::Response& response) {
   carto::common::MutexLocker lock(&mutex_);
-  map_builder_bridge_.SerializeState(request.stem);
+  map_builder_bridge_.SerializeState(request.filename);
-  map_builder_bridge_.WriteAssets(request.stem);
   return true;
 }
 

cartographer_ros/cartographer_ros/node.h

@@ -34,7 +34,7 @@
 #include "cartographer_ros_msgs/SubmapList.h"
 #include "cartographer_ros_msgs/SubmapQuery.h"
 #include "cartographer_ros_msgs/TrajectoryOptions.h"
-#include "cartographer_ros_msgs/WriteAssets.h"
+#include "cartographer_ros_msgs/WriteState.h"
 #include "ros/ros.h"
 #include "tf2_ros/transform_broadcaster.h"
 
@@ -71,9 +71,9 @@ class Node {
   bool HandleFinishTrajectory(
       cartographer_ros_msgs::FinishTrajectory::Request& request,
       cartographer_ros_msgs::FinishTrajectory::Response& response);
-  bool HandleWriteAssets(
+  bool HandleWriteState(
-      cartographer_ros_msgs::WriteAssets::Request& request,
+      cartographer_ros_msgs::WriteState::Request& request,
-      cartographer_ros_msgs::WriteAssets::Response& response);
+      cartographer_ros_msgs::WriteState::Response& response);
   // Returns the set of topic names we want to subscribe to.
   std::unordered_set<string> ComputeExpectedTopics(
       const TrajectoryOptions& options,
@@ -113,9 +113,6 @@ class Node {
   std::unordered_map<int, std::vector<::ros::Subscriber>> subscribers_;
   std::unordered_set<std::string> subscribed_topics_;
   std::unordered_map<int, bool> is_active_trajectory_ GUARDED_BY(mutex_);
-  ::ros::Publisher occupancy_grid_publisher_;
-  std::thread occupancy_grid_thread_;
-  bool terminating_ = false GUARDED_BY(mutex_);
 
   // We have to keep the timer handles of ::ros::WallTimers around, otherwise
   // they do not fire.

cartographer_ros/cartographer_ros/node_constants.h

@@ -34,7 +34,7 @@ constexpr char kScanMatchedPointCloudTopic[] = "scan_matched_points2";
 constexpr char kSubmapListTopic[] = "submap_list";
 constexpr char kSubmapQueryServiceName[] = "submap_query";
 constexpr char kStartTrajectoryServiceName[] = "start_trajectory";
-constexpr char kWriteAssetsServiceName[] = "write_assets";
+constexpr char kWriteStateServiceName[] = "write_state";
 constexpr char kTrajectoryNodeListTopic[] = "trajectory_node_list";
 constexpr char kConstraintListTopic[] = "constraint_list";
 constexpr double kConstraintPublishPeriodSec = 0.5;

cartographer_ros/cartographer_ros/occupancy_grid.cc

@@ -1,146 +0,0 @@
-/*
- * 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/occupancy_grid.h"
-
-#include "cartographer/common/port.h"
-#include "cartographer/mapping_2d/probability_grid.h"
-#include "cartographer/mapping_2d/range_data_inserter.h"
-#include "cartographer_ros/time_conversion.h"
-#include "glog/logging.h"
-
-namespace {
-
-Eigen::AlignedBox2f ComputeMapBoundingBox2D(
-    const std::vector<std::vector<::cartographer::mapping::TrajectoryNode>>&
-        all_trajectory_nodes) {
-  Eigen::AlignedBox2f bounding_box(Eigen::Vector2f::Zero());
-  for (const auto& trajectory_nodes : all_trajectory_nodes) {
-    for (const auto& node : trajectory_nodes) {
-      if (node.trimmed()) {
-        continue;
-      }
-      const auto& data = *node.constant_data;
-      ::cartographer::sensor::RangeData range_data;
-      range_data = ::cartographer::sensor::TransformRangeData(
-          Decompress(data.range_data), node.pose.cast<float>());
-      bounding_box.extend(range_data.origin.head<2>());
-      for (const Eigen::Vector3f& hit : range_data.returns) {
-        bounding_box.extend(hit.head<2>());
-      }
-      for (const Eigen::Vector3f& miss : range_data.misses) {
-        bounding_box.extend(miss.head<2>());
-      }
-    }
-  }
-  return bounding_box;
-}
-
-}  // namespace
-
-namespace cartographer_ros {
-
-void BuildOccupancyGrid2D(
-    const std::vector<std::vector<::cartographer::mapping::TrajectoryNode>>&
-        all_trajectory_nodes,
-    const string& map_frame,
-    const ::cartographer::mapping_2d::proto::SubmapsOptions& submaps_options,
-    ::nav_msgs::OccupancyGrid* const occupancy_grid) {
-  namespace carto = ::cartographer;
-  const carto::mapping_2d::MapLimits map_limits =
-      ComputeMapLimits(submaps_options.resolution(), all_trajectory_nodes);
-  carto::mapping_2d::ProbabilityGrid probability_grid(map_limits);
-  carto::mapping_2d::RangeDataInserter range_data_inserter(
-      submaps_options.range_data_inserter_options());
-  carto::common::Time latest_time = carto::common::Time::min();
-  for (const auto& trajectory_nodes : all_trajectory_nodes) {
-    for (const auto& node : trajectory_nodes) {
-      if (node.trimmed()) {
-        continue;
-      }
-      latest_time = std::max(latest_time, node.time());
-      range_data_inserter.Insert(carto::sensor::TransformRangeData(
-                                     Decompress(node.constant_data->range_data),
-                                     node.pose.cast<float>()),
-                                 &probability_grid);
-    }
-  }
-  CHECK(latest_time != carto::common::Time::min());
-  occupancy_grid->header.stamp = ToRos(latest_time);
-  occupancy_grid->header.frame_id = map_frame;
-  occupancy_grid->info.map_load_time = occupancy_grid->header.stamp;
-
-  Eigen::Array2i offset;
-  carto::mapping_2d::CellLimits cell_limits;
-  probability_grid.ComputeCroppedLimits(&offset, &cell_limits);
-  const double resolution = probability_grid.limits().resolution();
-
-  occupancy_grid->info.resolution = resolution;
-  occupancy_grid->info.width = cell_limits.num_y_cells;
-  occupancy_grid->info.height = cell_limits.num_x_cells;
-
-  occupancy_grid->info.origin.position.x =
-      probability_grid.limits().max().x() -
-      (offset.y() + cell_limits.num_y_cells) * resolution;
-  occupancy_grid->info.origin.position.y =
-      probability_grid.limits().max().y() -
-      (offset.x() + cell_limits.num_x_cells) * resolution;
-  occupancy_grid->info.origin.position.z = 0.;
-  occupancy_grid->info.origin.orientation.w = 1.;
-  occupancy_grid->info.origin.orientation.x = 0.;
-  occupancy_grid->info.origin.orientation.y = 0.;
-  occupancy_grid->info.origin.orientation.z = 0.;
-
-  occupancy_grid->data.resize(cell_limits.num_x_cells * cell_limits.num_y_cells,
-                              -1);
-  for (const Eigen::Array2i& xy_index :
-       carto::mapping_2d::XYIndexRangeIterator(cell_limits)) {
-    if (probability_grid.IsKnown(xy_index + offset)) {
-      const int value = carto::common::RoundToInt(
-          (probability_grid.GetProbability(xy_index + offset) -
-           carto::mapping::kMinProbability) *
-          100. /
-          (carto::mapping::kMaxProbability - carto::mapping::kMinProbability));
-      CHECK_LE(0, value);
-      CHECK_GE(100, value);
-      occupancy_grid->data[(cell_limits.num_x_cells - xy_index.x()) *
-                               cell_limits.num_y_cells -
-                           xy_index.y() - 1] = value;
-    }
-  }
-}
-
-::cartographer::mapping_2d::MapLimits ComputeMapLimits(
-    const double resolution,
-    const std::vector<std::vector<::cartographer::mapping::TrajectoryNode>>&
-        all_trajectory_nodes) {
-  Eigen::AlignedBox2f bounding_box =
-      ComputeMapBoundingBox2D(all_trajectory_nodes);
-  // Add some padding to ensure all rays are still contained in the map after
-  // discretization.
-  const float kPadding = 3.f * resolution;
-  bounding_box.min() -= kPadding * Eigen::Vector2f::Ones();
-  bounding_box.max() += kPadding * Eigen::Vector2f::Ones();
-  const Eigen::Vector2d pixel_sizes =
-      bounding_box.sizes().cast<double>() / resolution;
-  return ::cartographer::mapping_2d::MapLimits(
-      resolution, bounding_box.max().cast<double>(),
-      ::cartographer::mapping_2d::CellLimits(
-          ::cartographer::common::RoundToInt(pixel_sizes.y()),
-          ::cartographer::common::RoundToInt(pixel_sizes.x())));
-}
-
-}  // namespace cartographer_ros

cartographer_ros/cartographer_ros/occupancy_grid.h

@@ -1,47 +0,0 @@
-/*
- * 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_OCCUPANCY_GRID_H_
-#define CARTOGRAPHER_ROS_OCCUPANCY_GRID_H_
-
-#include <vector>
-
-#include "Eigen/Core"
-#include "Eigen/Geometry"
-#include "cartographer/mapping/trajectory_node.h"
-#include "cartographer/mapping_2d/map_limits.h"
-#include "cartographer/mapping_2d/proto/submaps_options.pb.h"
-#include "nav_msgs/OccupancyGrid.h"
-
-namespace cartographer_ros {
-
-void BuildOccupancyGrid2D(
-    const std::vector<std::vector<::cartographer::mapping::TrajectoryNode>>&
-        all_trajectory_nodes,
-    const string& map_frame,
-    const ::cartographer::mapping_2d::proto::SubmapsOptions& submaps_options,
-    ::nav_msgs::OccupancyGrid* const occupancy_grid);
-
-// Computes MapLimits that contain the origin, and all rays (both returns and
-// misses) in the 'trajectory_nodes'.
-::cartographer::mapping_2d::MapLimits ComputeMapLimits(
-    double resolution,
-    const std::vector<std::vector<::cartographer::mapping::TrajectoryNode>>&
-        all_trajectory_nodes);
-
-}  // namespace cartographer_ros
-
-#endif  // CARTOGRAPHER_ROS_OCCUPANCY_GRID_H_

cartographer_ros/cartographer_ros/occupancy_grid_test.cc

@@ -1,54 +0,0 @@
-/*
- * 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/occupancy_grid.h"
-
-#include <memory>
-
-#include "cartographer/common/time.h"
-#include "cartographer/sensor/range_data.h"
-#include "gtest/gtest.h"
-#include "ros/ros.h"
-
-namespace cartographer_ros {
-namespace {
-
-TEST(OccupancyGridTest, ComputeMapLimits) {
-  using ::cartographer::mapping::TrajectoryNode;
-  const TrajectoryNode trajectory_node{
-      std::make_shared<TrajectoryNode::Data>(TrajectoryNode::Data{
-          ::cartographer::common::FromUniversal(52),
-          ::cartographer::sensor::Compress(::cartographer::sensor::RangeData{
-              Eigen::Vector3f::Zero(),
-              {Eigen::Vector3f(-30.f, 1.f, 0.f),
-               Eigen::Vector3f(50.f, -10.f, 0.f)},
-              {}}),
-          ::cartographer::transform::Rigid3d::Identity()}),
-      ::cartographer::transform::Rigid3d::Identity()};
-  constexpr double kResolution = 0.05;
-  const ::cartographer::mapping_2d::MapLimits limits =
-      ComputeMapLimits(kResolution, {{trajectory_node}});
-  constexpr float kPaddingAwareTolerance = 5 * kResolution;
-  EXPECT_NEAR(50.f, limits.max().x(), kPaddingAwareTolerance);
-  EXPECT_NEAR(1.f, limits.max().y(), kPaddingAwareTolerance);
-  EXPECT_LT(200, limits.cell_limits().num_x_cells);
-  EXPECT_LT(1600, limits.cell_limits().num_y_cells);
-  EXPECT_GT(400, limits.cell_limits().num_x_cells);
-  EXPECT_GT(2000, limits.cell_limits().num_y_cells);
-}
-
-}  // namespace
-}  // namespace cartographer_ros

cartographer_ros/cartographer_ros/offline_node_main.cc

@@ -273,8 +273,7 @@ void Run(const std::vector<string>& bag_filenames) {
             << (cpu_timespec.tv_sec + 1e-9 * cpu_timespec.tv_nsec) << " s";
 #endif
 
-  node.map_builder_bridge()->SerializeState(bag_filenames.front());
+  node.map_builder_bridge()->SerializeState(bag_filenames.front() + ".pbstream");
-  node.map_builder_bridge()->WriteAssets(bag_filenames.front());
 }
 
 }  // namespace

cartographer_ros/launch/backpack_2d.launch

@@ -28,4 +28,7 @@
       output="screen">
     <remap from="echoes" to="horizontal_laser_2d" />
   </node>
+
+  <node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
+      type="cartographer_occupancy_grid_node" args="-resolution 0.05" />
 </launch>

cartographer_ros/launch/backpack_3d.launch

@@ -29,4 +29,7 @@
     <remap from="points2_1" to="horizontal_laser_3d" />
     <remap from="points2_2" to="vertical_laser_3d" />
   </node>
+
+  <node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
+      type="cartographer_occupancy_grid_node" args="-resolution 0.05" />
 </launch>

cartographer_ros/launch/demo_backpack_2d_localization.launch

@@ -32,6 +32,9 @@
     <remap from="echoes" to="horizontal_laser_2d" />
   </node>
 
+  <node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
+      type="cartographer_occupancy_grid_node" args="-resolution 0.05" />
+
   <node name="rviz" pkg="rviz" type="rviz" required="true"
       args="-d $(find cartographer_ros)/configuration_files/demo_2d.rviz" />
   <node name="playbag" pkg="rosbag" type="play"

cartographer_ros/launch/demo_pr2.launch

@@ -26,6 +26,9 @@
     <remap from="scan" to="/base_scan" />
   </node>
 
+  <node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
+      type="cartographer_occupancy_grid_node" args="-resolution 0.05" />
+
   <node name="tf_remove_frames" pkg="cartographer_ros"
       type="tf_remove_frames.py">
     <remap from="tf_out" to="/tf" />

cartographer_ros/launch/demo_revo_lds.launch

@@ -25,6 +25,9 @@
     <remap from="scan" to="horizontal_laser_2d" />
   </node>
 
+  <node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
+      type="cartographer_occupancy_grid_node" args="-resolution 0.05" />
+
   <node name="rviz" pkg="rviz" type="rviz" required="true"
       args="-d $(find cartographer_ros)/configuration_files/demo_2d.rviz" />
   <node name="playbag" pkg="rosbag" type="play"

cartographer_ros/launch/offline_backpack_2d.launch

@@ -29,4 +29,7 @@
       output="screen">
     <remap from="echoes" to="horizontal_laser_2d" />
   </node>
+
+  <node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
+      type="cartographer_occupancy_grid_node" args="-resolution 0.05" />
 </launch>

cartographer_ros/launch/offline_backpack_3d.launch

@@ -30,4 +30,7 @@
     <remap from="points2_1" to="horizontal_laser_3d" />
     <remap from="points2_2" to="vertical_laser_3d" />
   </node>
+
+  <node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
+      type="cartographer_occupancy_grid_node" args="-resolution 0.05" />
 </launch>

cartographer_ros/launch/taurob_tracker.launch

@@ -23,4 +23,7 @@
     <remap from="scan" to="/spin_laser/scan" />
     <remap from="imu" to="/imu/data" />
   </node>
+
+  <node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
+      type="cartographer_occupancy_grid_node" args="-resolution 0.05" />
 </launch>

cartographer_ros/package.xml

@@ -59,7 +59,6 @@
   <depend>tf2_ros</depend>
   <depend>urdf</depend>
   <depend>visualization_msgs</depend>
-  <depend>yaml-cpp</depend>
 
   <test_depend>rosunit</test_depend>
 

cartographer_ros_msgs/CMakeLists.txt

@@ -35,7 +35,7 @@ add_service_files(
     SubmapQuery.srv
     FinishTrajectory.srv
     StartTrajectory.srv
-    WriteAssets.srv
+    WriteState.srv
 )
 
 generate_messages(

cartographer_ros_msgs/srv/WriteState.srv

@@ -12,5 +12,5 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
-string stem
+string filename
 ---

docs/source/ros_api.rst

@@ -99,10 +99,11 @@ start_trajectory (`cartographer_ros_msgs/StartTrajectory`_)
 finish_trajectory (`cartographer_ros_msgs/FinishTrajectory`_)
   Finishes the given `trajectory_id`'s trajectory by running a final optimization.
 
-write_assets (`cartographer_ros_msgs/WriteAssets`_)
+write_state (`cartographer_ros_msgs/WriteState`_)
-  Writes artifacts (e.g. the map) to disk. The `stem` argument is used as a prefix
+  Writes the current internal state to disk into `filename`. The file will
-  for the various files which are written. Files will usually end up in `~/.ros` or
+  usually end up in `~/.ros` or `ROS_HOME` if it is set. This file can be used
-  `ROS_HOME` if it is set.
+  as input to the `assets_writer_main` to generate assets like probability
+  grids, X-Rays or PLY files.
 
 Required tf Transforms
 ======================
@@ -129,7 +130,7 @@ If *provide_odom_frame* is enabled in the :doc:`configuration`, a continuous
 .. _cartographer_ros_msgs/SubmapList: https://github.com/googlecartographer/cartographer_ros/blob/master/cartographer_ros_msgs/msg/SubmapList.msg
 .. _cartographer_ros_msgs/SubmapQuery: https://github.com/googlecartographer/cartographer_ros/blob/master/cartographer_ros_msgs/srv/SubmapQuery.srv
 .. _cartographer_ros_msgs/StartTrajectory: https://github.com/googlecartographer/cartographer_ros/blob/master/cartographer_ros_msgs/srv/StartTrajectory.srv
-.. _cartographer_ros_msgs/WriteAssets: https://github.com/googlecartographer/cartographer_ros/blob/master/cartographer_ros_msgs/srv/WriteAssets.srv
+.. _cartographer_ros_msgs/WriteState: https://github.com/googlecartographer/cartographer_ros/blob/master/cartographer_ros_msgs/srv/WriteState.srv
 .. _nav_msgs/OccupancyGrid: http://docs.ros.org/api/nav_msgs/html/msg/OccupancyGrid.html
 .. _nav_msgs/Odometry: http://docs.ros.org/api/nav_msgs/html/msg/Odometry.html
 .. _sensor_msgs/Imu: http://docs.ros.org/api/sensor_msgs/html/msg/Imu.html