From 8b25e89ab33bcfc5b2d98b5445d34b53572fb2cf Mon Sep 17 00:00:00 2001
From: slimeth <snaji@student.ethz.ch>
Date: Tue, 8 Sep 2020 16:01:57 +0200
Subject: [PATCH] added timeout to getPointCloud

---
 .../flightlib/bridges/unity_bridge.hpp        |  3 ++-
 .../flightlib/bridges/unity_message_types.hpp |  7 ++++---
 flightlib/src/bridges/unity_bridge.cpp        | 20 +++++++++++++------
 flightlib/tests/bridges/unity_bridge.cpp      |  8 ++++----
 4 files changed, 24 insertions(+), 14 deletions(-)

diff --git a/flightlib/include/flightlib/bridges/unity_bridge.hpp b/flightlib/include/flightlib/bridges/unity_bridge.hpp
index cd33667378..289528c925 100644
--- a/flightlib/include/flightlib/bridges/unity_bridge.hpp
+++ b/flightlib/include/flightlib/bridges/unity_bridge.hpp
@@ -40,7 +40,8 @@ class UnityBridge {
   // public get functions
   bool getRender(const FrameID frame_id);
   bool handleOutput();
-  bool getPointCloud(PointCloudMessage_t &pointcloud_msg);
+  bool getPointCloud(PointCloudMessage_t &pointcloud_msg,
+                     Scalar time_out = 600.0);
 
   // public set functions
   bool setScene(const SceneID &scene_id);
diff --git a/flightlib/include/flightlib/bridges/unity_message_types.hpp b/flightlib/include/flightlib/bridges/unity_message_types.hpp
index 9db2fd991f..230d284b5d 100644
--- a/flightlib/include/flightlib/bridges/unity_message_types.hpp
+++ b/flightlib/include/flightlib/bridges/unity_message_types.hpp
@@ -113,9 +113,10 @@ struct SubMessage_t {
 };
 
 struct PointCloudMessage_t {
-  std::vector<double> range{20.0, 20.0, 20.0};
-  std::vector<double> origin{0.0, 0.0, 0.0};
-  double resolution{0.15};
+  // define point cloud box range [x, y, z] / meter
+  std::vector<Scalar> range{20.0, 20.0, 20.0};
+  std::vector<Scalar> origin{0.0, 0.0, 0.0};
+  Scalar resolution{0.15};
   std::string path{"point_clouds_data/"};
   std::string file_name{"default"};
 };
diff --git a/flightlib/src/bridges/unity_bridge.cpp b/flightlib/src/bridges/unity_bridge.cpp
index 5c541b1cfe..0659736278 100644
--- a/flightlib/src/bridges/unity_bridge.cpp
+++ b/flightlib/src/bridges/unity_bridge.cpp
@@ -231,7 +231,8 @@ bool UnityBridge::handleOutput() {
   return true;
 }
 
-bool UnityBridge::getPointCloud(PointCloudMessage_t& pointcloud_msg) {
+bool UnityBridge::getPointCloud(PointCloudMessage_t& pointcloud_msg,
+                                Scalar time_out) {
   // create new message object
   zmqpp::message msg;
   // add topic header
@@ -242,14 +243,21 @@ bool UnityBridge::getPointCloud(PointCloudMessage_t& pointcloud_msg) {
   // send message without blocking
   pub_.send(msg, true);
 
-  std::cout << "Generate PointCloud" << std::endl;
-  std::cout << "[";
+  std::cout << "Generate PointCloud: Timeout=" << (int)time_out << " seconds."
+            << std::endl;
+
+  Scalar run_time = 0.0;
   while (!std::experimental::filesystem::exists(
     pointcloud_msg.path + pointcloud_msg.file_name + ".ply")) {
-    std::cout << ".";
-    usleep(1 * 1e6);
+    if (run_time >= time_out) {
+      logger_.warn("Timeout... PointCloud was not saved within expected time.");
+      return false;
+    }
+    std::cout << "Waiting for Pointcloud: Current Runtime=" << (int)run_time
+              << " seconds." << std::endl;
+    usleep((time_out / 10.0) * 1e6);
+    run_time += time_out / 10.0;
   }
-  std::cout << "]";
   return true;
 }
 
diff --git a/flightlib/tests/bridges/unity_bridge.cpp b/flightlib/tests/bridges/unity_bridge.cpp
index 2b9fe827fa..9585d2e77d 100644
--- a/flightlib/tests/bridges/unity_bridge.cpp
+++ b/flightlib/tests/bridges/unity_bridge.cpp
@@ -27,19 +27,19 @@ TEST(UnityBridge, PointCloud) {
   Logger logger{"Test PointCloud"};
   UnityBridge unity_bridge;
 
-  // need to add a quad to connect to Flightmare
+  // // need to add a quad to connect to Flightmare
   // QuadrotorDynamics dyn = QuadrotorDynamics(1.0, 0.2);
   // std::shared_ptr<Quadrotor> quad = std::make_shared<Quadrotor>(dyn);
   // unity_bridge.addQuadrotor(quad);
 
-  // unity_bridge.connectUnity(UnityScene::GARAGE);
+  // EXPECT_TRUE(unity_bridge.connectUnity(UnityScene::GARAGE));
   // PointCloudMessage_t pointcloud_msg;
   // pointcloud_msg.path = "/tmp/";
   // pointcloud_msg.file_name = "unity-bridge" + std::to_string(::rand());
-  // EXPECT_TRUE(unity_bridge.getPointCloud(pointcloud_msg));
+  // EXPECT_TRUE(unity_bridge.getPointCloud(pointcloud_msg, 30.0));
   // std::experimental::filesystem::remove(pointcloud_msg.path +
   //                                       pointcloud_msg.file_name + ".ply");
-  // //timeout flightmare
+  // // timeout flightmare
   // usleep(5 * 1e6);
 }