Pulls out GetOccupancyGrid to MapBuilderBridge. (#187)

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

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

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(); }
 

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()
+    const auto occupancy_grid = map_builder_bridge_->BuildOccupancyGrid();
-                                      ->sparse_pose_graph()
+    if (occupancy_grid != nullptr) {
-                                      ->GetTrajectoryNodes();
+      occupancy_grid_publisher_.publish(*occupancy_grid);
-    if (trajectory_nodes.empty()) {
-      continue;
     }
-    ::nav_msgs::OccupancyGrid occupancy_grid;
-    BuildOccupancyGrid(trajectory_nodes, options_, &occupancy_grid);
-    occupancy_grid_publisher_.publish(occupancy_grid);
   }
 }