From 012beb5c0c55d6205ace53061db480113d29b306 Mon Sep 17 00:00:00 2001
From: Damon Kohler <damonkohler@google.com>
Date: Fri, 25 Nov 2016 14:13:04 +0100
Subject: [PATCH] Pulls out GetOccupancyGrid to MapBuilderBridge. (#187)

---
 cartographer_ros/cartographer_ros/CMakeLists.txt   |  2 +-
 .../cartographer_ros/map_builder_bridge.cc         | 14 ++++++++++++++
 .../cartographer_ros/map_builder_bridge.h          |  2 ++
 cartographer_ros/cartographer_ros/node_main.cc     | 13 +++----------
 4 files changed, 20 insertions(+), 11 deletions(-)

diff --git a/cartographer_ros/cartographer_ros/CMakeLists.txt b/cartographer_ros/cartographer_ros/CMakeLists.txt
index 14a885d..4733384 100644
--- a/cartographer_ros/cartographer_ros/CMakeLists.txt
+++ b/cartographer_ros/cartographer_ros/CMakeLists.txt
@@ -20,6 +20,7 @@ google_library(map_builder_bridge
     assets_writer
     msg_conversion
     node_options
+    occupancy_grid
     sensor_bridge
     tf_bridge
 )
@@ -145,7 +146,6 @@ google_binary(cartographer_node
     map_builder_bridge
     msg_conversion
     node_options
-    occupancy_grid
     ros_log_sink
     sensor_bridge
     tf_bridge
diff --git a/cartographer_ros/cartographer_ros/map_builder_bridge.cc b/cartographer_ros/cartographer_ros/map_builder_bridge.cc
index 99d375b..37e08c9 100644
--- a/cartographer_ros/cartographer_ros/map_builder_bridge.cc
+++ b/cartographer_ros/cartographer_ros/map_builder_bridge.cc
@@ -18,6 +18,7 @@
 
 #include "cartographer_ros/assets_writer.h"
 #include "cartographer_ros/msg_conversion.h"
+#include "cartographer_ros/occupancy_grid.h"
 #include "cartographer_ros_msgs/TrajectorySubmapList.h"
 
 namespace cartographer_ros {
@@ -115,4 +116,17 @@ cartographer_ros_msgs::SubmapList MapBuilderBridge::GetSubmapList() {
   return submap_list;
 }
 
+std::unique_ptr<nav_msgs::OccupancyGrid> MapBuilderBridge::BuildOccupancyGrid() {
+  const auto trajectory_nodes =
+      map_builder_.sparse_pose_graph()->GetTrajectoryNodes();
+  std::unique_ptr<nav_msgs::OccupancyGrid> occupancy_grid;
+  if (!trajectory_nodes.empty()) {
+    occupancy_grid =
+        cartographer::common::make_unique<nav_msgs::OccupancyGrid>();
+    cartographer_ros::BuildOccupancyGrid(trajectory_nodes, options_,
+                                         occupancy_grid.get());
+  }
+  return occupancy_grid;
+}
+
 }  // namespace cartographer_ros
diff --git a/cartographer_ros/cartographer_ros/map_builder_bridge.h b/cartographer_ros/cartographer_ros/map_builder_bridge.h
index dd1e226..7763322 100644
--- a/cartographer_ros/cartographer_ros/map_builder_bridge.h
+++ b/cartographer_ros/cartographer_ros/map_builder_bridge.h
@@ -25,6 +25,7 @@
 #include "cartographer_ros_msgs/SubmapEntry.h"
 #include "cartographer_ros_msgs/SubmapList.h"
 #include "cartographer_ros_msgs/SubmapQuery.h"
+#include "nav_msgs/OccupancyGrid.h"
 
 namespace cartographer_ros {
 
@@ -46,6 +47,7 @@ class MapBuilderBridge {
       cartographer_ros_msgs::FinishTrajectory::Response& response);
 
   cartographer_ros_msgs::SubmapList GetSubmapList();
+  std::unique_ptr<nav_msgs::OccupancyGrid> BuildOccupancyGrid();
 
   SensorBridge* sensor_bridge() { return sensor_bridge_.get(); }
 
diff --git a/cartographer_ros/cartographer_ros/node_main.cc b/cartographer_ros/cartographer_ros/node_main.cc
index 5daf355..8a1d969 100644
--- a/cartographer_ros/cartographer_ros/node_main.cc
+++ b/cartographer_ros/cartographer_ros/node_main.cc
@@ -36,7 +36,6 @@
 #include "cartographer_ros/map_builder_bridge.h"
 #include "cartographer_ros/msg_conversion.h"
 #include "cartographer_ros/node_options.h"
-#include "cartographer_ros/occupancy_grid.h"
 #include "cartographer_ros/ros_log_sink.h"
 #include "cartographer_ros/sensor_bridge.h"
 #include "cartographer_ros/tf_bridge.h"
@@ -48,7 +47,6 @@
 #include "cartographer_ros_msgs/TrajectorySubmapList.h"
 #include "gflags/gflags.h"
 #include "glog/logging.h"
-#include "nav_msgs/OccupancyGrid.h"
 #include "nav_msgs/Odometry.h"
 #include "ros/ros.h"
 #include "ros/serialization.h"
@@ -362,15 +360,10 @@ void Node::SpinOccupancyGridThreadForever() {
     if (occupancy_grid_publisher_.getNumSubscribers() == 0) {
       continue;
     }
-    const auto trajectory_nodes = map_builder_bridge_->map_builder()
-                                      ->sparse_pose_graph()
-                                      ->GetTrajectoryNodes();
-    if (trajectory_nodes.empty()) {
-      continue;
+    const auto occupancy_grid = map_builder_bridge_->BuildOccupancyGrid();
+    if (occupancy_grid != nullptr) {
+      occupancy_grid_publisher_.publish(*occupancy_grid);
     }
-    ::nav_msgs::OccupancyGrid occupancy_grid;
-    BuildOccupancyGrid(trajectory_nodes, options_, &occupancy_grid);
-    occupancy_grid_publisher_.publish(occupancy_grid);
   }
 }