From 3fbc642a897f78e6b16a36bdbf74246dea305101 Mon Sep 17 00:00:00 2001
From: gaschler <gaschlera@gmail.com>
Date: Mon, 18 Dec 2017 10:02:57 +0100
Subject: [PATCH] PopWithTimeout for sensor data queue (#763)

PopWithTimeout is necessary to ensure the SLAM thread
in the MapBuilderServer finishes when the server is
shut down.
---
 cartographer_grpc/map_builder_server.cc | 21 +++++++++++----
 cartographer_grpc/map_builder_server.h  | 34 +++++++++++++++----------
 2 files changed, 36 insertions(+), 19 deletions(-)

diff --git a/cartographer_grpc/map_builder_server.cc b/cartographer_grpc/map_builder_server.cc
index 701b7e1..73a8c28 100644
--- a/cartographer_grpc/map_builder_server.cc
+++ b/cartographer_grpc/map_builder_server.cc
@@ -26,10 +26,17 @@
 #include "glog/logging.h"
 
 namespace cartographer_grpc {
+namespace {
+
+const cartographer::common::Duration kPopTimeout =
+    cartographer::common::FromMilliseconds(100);
+
+}  // namespace
 
 MapBuilderServer::MapBuilderContext::MapBuilderContext(
     cartographer::mapping::MapBuilder* map_builder,
-    cartographer::common::BlockingQueue<SensorData>* sensor_data_queue)
+    cartographer::common::BlockingQueue<std::unique_ptr<SensorData>>*
+        sensor_data_queue)
     : map_builder_(map_builder), sensor_data_queue_(sensor_data_queue) {}
 
 cartographer::mapping::MapBuilder&
@@ -37,7 +44,8 @@ MapBuilderServer::MapBuilderContext::map_builder() {
   return *map_builder_;
 }
 
-cartographer::common::BlockingQueue<MapBuilderServer::SensorData>&
+cartographer::common::BlockingQueue<
+    std::unique_ptr<MapBuilderServer::SensorData>>&
 MapBuilderServer::MapBuilderContext::sensor_data_queue() {
   return *sensor_data_queue_;
 }
@@ -102,9 +110,12 @@ void MapBuilderServer::Shutdown() {
 void MapBuilderServer::ProcessSensorDataQueue() {
   LOG(INFO) << "Starting SLAM thread.";
   while (!shutting_down_) {
-    SensorData sensor_data = sensor_data_queue_.Pop();
-    grpc_server_->GetContext<MapBuilderContext>()->AddSensorDataToTrajectory(
-        sensor_data);
+    std::unique_ptr<SensorData> sensor_data =
+        sensor_data_queue_.PopWithTimeout(kPopTimeout);
+    if (sensor_data) {
+      grpc_server_->GetContext<MapBuilderContext>()->AddSensorDataToTrajectory(
+          *sensor_data);
+    }
   }
 }
 
diff --git a/cartographer_grpc/map_builder_server.h b/cartographer_grpc/map_builder_server.h
index 5872c83..573a3f3 100644
--- a/cartographer_grpc/map_builder_server.h
+++ b/cartographer_grpc/map_builder_server.h
@@ -36,27 +36,32 @@ class MapBuilderServer {
   class MapBuilderContext : public framework::ExecutionContext {
    public:
     MapBuilderContext(
-        cartographer::mapping::MapBuilder *map_builder,
-        cartographer::common::BlockingQueue<SensorData> *sensor_data_queue);
-    cartographer::mapping::MapBuilder &map_builder();
-    cartographer::common::BlockingQueue<SensorData> &sensor_data_queue();
-    void AddSensorDataToTrajectory(const SensorData &sensor_data);
+        cartographer::mapping::MapBuilder* map_builder,
+        cartographer::common::BlockingQueue<std::unique_ptr<SensorData>>*
+            sensor_data_queue);
+    cartographer::mapping::MapBuilder& map_builder();
+    cartographer::common::BlockingQueue<std::unique_ptr<SensorData>>&
+    sensor_data_queue();
+    void AddSensorDataToTrajectory(const SensorData& sensor_data);
 
     template <typename SensorDataType>
-    void EnqueueSensorData(int trajectory_id, const std::string &sensor_id,
-                           const SensorDataType &sensor_data) {
-      sensor_data_queue_->Push(MapBuilderServer::SensorData{
-          trajectory_id,
-          cartographer::sensor::MakeDispatchable(sensor_id, sensor_data)});
+    void EnqueueSensorData(int trajectory_id, const std::string& sensor_id,
+                           const SensorDataType& sensor_data) {
+      sensor_data_queue_->Push(
+          cartographer::common::make_unique<MapBuilderServer::SensorData>(
+              MapBuilderServer::SensorData{
+                  trajectory_id, cartographer::sensor::MakeDispatchable(
+                                     sensor_id, sensor_data)}));
     }
 
    private:
-    cartographer::mapping::MapBuilder *map_builder_;
-    cartographer::common::BlockingQueue<SensorData> *sensor_data_queue_;
+    cartographer::mapping::MapBuilder* map_builder_;
+    cartographer::common::BlockingQueue<std::unique_ptr<SensorData>>*
+        sensor_data_queue_;
   };
 
   MapBuilderServer(
-      const proto::MapBuilderServerOptions &map_builder_server_options);
+      const proto::MapBuilderServerOptions& map_builder_server_options);
 
   // Starts the gRPC server and the SLAM thread.
   void Start();
@@ -77,7 +82,8 @@ class MapBuilderServer {
   std::unique_ptr<std::thread> slam_thread_;
   std::unique_ptr<framework::Server> grpc_server_;
   cartographer::mapping::MapBuilder map_builder_;
-  cartographer::common::BlockingQueue<SensorData> sensor_data_queue_;
+  cartographer::common::BlockingQueue<std::unique_ptr<SensorData>>
+      sensor_data_queue_;
 };
 
 }  // namespace cartographer_grpc