Add PointCloud generator to unity_bridge with test

captjulian · Sep 7, 2020 · b549ec1 · b549ec1
1 parent 2454e1a
commit b549ec1
Show file tree

Hide file tree

Showing 5 changed files with 71 additions and 23 deletions.
diff --git a/flightlib/CMakeLists.txt b/flightlib/CMakeLists.txt
@@ -199,7 +199,8 @@ else()
     ${OpenCV_LIBRARIES}
     yaml-cpp
     zmq
-    zmqpp)
+    zmqpp
+    stdc++fs)
   set(LIBRARY_NAME ${PROJECT_NAME})
   set_target_properties(${PROJECT_NAME} PROPERTIES POSITION_INDEPENDENT_CODE TRUE)
 endif()

diff --git a/flightlib/include/flightlib/bridges/unity_bridge.hpp b/flightlib/include/flightlib/bridges/unity_bridge.hpp
@@ -3,11 +3,13 @@
 // std libs
 #include <unistd.h>
 #include <chrono>
+#include <experimental/filesystem>
 #include <fstream>
 #include <map>
 #include <string>
 #include <unordered_map>
 
+
 // Include ZMQ bindings for communications with Unity.
 #include <zmqpp/zmqpp.hpp>
 
@@ -38,6 +40,7 @@ class UnityBridge {
   // public get functions
   bool getRender(const FrameID frame_id);
   bool handleOutput(RenderMessage_t &output);
+  bool getPointCloud(PointCloudMessage_t &pointcloud_msg);
 
   // 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
@@ -112,6 +112,14 @@ struct SubMessage_t {
   std::vector<Sub_Vehicle_t> sub_vehicles;
 };
 
+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};
+  std::string path{"point_clouds_data/"};
+  std::string file_name{"default"};
+};
+
 /*********************
  * JSON constructors *
  *********************/
@@ -184,6 +192,14 @@ inline void from_json(const json &j, SubMessage_t &o) {
   o.sub_vehicles = j.at("pub_vehicles").get<std::vector<Sub_Vehicle_t>>();
 }
 
+inline void to_json(json &j, const PointCloudMessage_t &o) {
+  j = json{{"range", o.range},
+           {"origin", o.origin},
+           {"resolution", o.resolution},
+           {"path", o.path},
+           {"file_name", o.file_name}};
+}
+
 // Struct for outputting parsed received messages to handler functions
 struct RenderMessage_t {
   SubMessage_t sub_msg;

diff --git a/flightlib/src/bridges/unity_bridge.cpp b/flightlib/src/bridges/unity_bridge.cpp
@@ -200,4 +200,26 @@ bool UnityBridge::handleOutput(RenderMessage_t& output) {
   return true;
 }
 
+bool UnityBridge::getPointCloud(PointCloudMessage_t& pointcloud_msg) {
+  // create new message object
+  zmqpp::message msg;
+  // add topic header
+  msg << "PointCloud";
+  // create JSON object for initial settings
+  json json_msg = pointcloud_msg;
+  msg << json_msg.dump();
+  // send message without blocking
+  pub_.send(msg, true);
+
+  std::cout << "Generate PointCloud" << std::endl;
+  std::cout << "[";
+  while (!std::experimental::filesystem::exists(
+    pointcloud_msg.path + pointcloud_msg.file_name + ".ply")) {
+    std::cout << ".";
+    usleep(1 * 1e6);
+  }
+  std::cout << "]";
+  return true;
+}
+
 }  // namespace flightlib
diff --git a/flightlib/tests/bridges/unity_bridge.cpp b/flightlib/tests/bridges/unity_bridge.cpp
@@ -8,28 +8,34 @@ using namespace flightlib;
 TEST(UnityBridge, Constructor) {
   Logger logger{"Test Unity Bridge"};
   UnityBridge unity_bridge;
-  // unity_bridge.initializeConnections();
-  // Scalar time_out_count = 0;
-  // Scalar sleep_useconds = 0.2 * 1e5;
   // bool unity_ready = false;
-  // logger.info("Trying to Connect Unity.");
-  // std::cout << "[";
-  // while (!unity_ready) {
-  //   // connect unity
-  //   unity_ready = unity_bridge.connectUnity();
-  //   if (time_out_count / 1e6 > 10) {
-  //     std::cout << "]" << std::endl;
-  //     logger.warn(
-  //       "Unity Connection time out! Make sure that Unity Standalone "
-  //       "or Unity Editor is running the Flightmare.");
-  //     break;
-  //   }
-  //   // sleep
-  //   usleep(sleep_useconds);
-  //   // incread time out counter
-  //   time_out_count += sleep_useconds;
-  //   std::cout << ".";
-  //   std::cout.flush();
-  // }
+
+  // // 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_ready = unity_bridge.connectUnity(UnityScene::GARAGE);
+
   // if (unity_ready) logger.info("Unity Rendering is connected");
+  // EXPECT_TRUE(unity_ready);
+}
+
+TEST(UnityBridge, PointCloud) {
+  Logger logger{"Test PointCloud"};
+  UnityBridge unity_bridge;
+
+  // 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);
+  // 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));
+  // std::experimental::filesystem::remove(pointcloud_msg.path +
+  //                                       pointcloud_msg.file_name + ".ply");
+  // unity_bridge.disconnectUnity();
 }