added timeout to getPointCloud

flightlib/include/flightlib/bridges/unity_bridge.hpp

@@ -20,6 +20,7 @@
 #include "flightlib/common/quad_state.hpp"
 #include "flightlib/common/types.hpp"
 #include "flightlib/objects/quadrotor.hpp"
+#include "flightlib/objects/static_object.hpp"
 #include "flightlib/objects/unity_camera.hpp"
 #include "flightlib/sensors/rgb_camera.hpp"
 
@@ -40,14 +41,16 @@ 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);
 
   // add object
   bool addQuadrotor(std::shared_ptr<Quadrotor> quad);
   bool addCamera(std::shared_ptr<UnityCamera> unity_camera);
+  bool addStaticObject(std::shared_ptr<StaticObject> static_object);
 
   // public auxiliary functions
   inline void setPubPort(const std::string &pub_port) { pub_port_ = pub_port; };
@@ -69,6 +72,7 @@ class UnityBridge {
 
   std::vector<std::shared_ptr<Quadrotor>> unity_quadrotors_;
   std::vector<std::shared_ptr<RGBCamera>> rgb_cameras_;
+  std::vector<std::shared_ptr<StaticObject>> static_objects_;
 
   // ZMQ variables and functions
   std::string client_address_;

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"};
 };

flightlib/include/flightlib/objects/static_gate.hpp

@@ -0,0 +1,30 @@
+#pragma once
+
+#include "flightlib/objects/static_object.hpp"
+
+namespace flightlib {
+class StaticGate : public StaticObject {
+ public:
+  StaticGate(std::string id, std::string prefab_id);
+  ~StaticGate(){};
+
+  // publich set functions
+  void setPosition(const Vector<3>& position) { position_ = position; };
+  void setRotation(const Quaternion& quaternion) { quat_ = quaternion; };
+  void setSize(const Vector<3>& size) { size_ = size; };
+
+  // publich get functions
+  Vector<3> getPos(void) { return position_; };
+  Quaternion getQuat(void) { return quat_; };
+  Vector<3> getSize(void) { return size_; };
+
+ private:
+  std::string id_;
+  std::string prefab_id_;
+
+  Vector<3> position_{0.0, 0.0, 0.0};
+  Quaternion quat_{1.0, 0.0, 0.0, 0.0};
+  Vector<3> size_{1.0, 1.0, 1.0};
+};
+
+}  // namespace flightlib

flightlib/include/flightlib/objects/static_object.hpp

@@ -0,0 +1,26 @@
+#pragma once
+
+#include "flightlib/common/types.hpp"
+
+namespace flightlib {
+class StaticObject {
+ public:
+  StaticObject(std::string id, std::string prefab_id);
+  virtual ~StaticObject(){};
+
+  // publich get functions
+  virtual Vector<3> getPos(void) = 0;
+  virtual Quaternion getQuat(void) = 0;
+  virtual Vector<3> getSize(void) = 0;
+
+  // public get functions
+  const std::string getID(void) { return id_; };
+  const std::string getPrefabID(void) { return prefab_id_; };
+
+
+ protected:
+  const std::string id_;
+  const std::string prefab_id_;
+};
+
+}  // namespace flightlib

flightlib/include/flightlib/sensors/rgb_camera.hpp

@@ -30,7 +30,7 @@ class RGBCamera : SensorBase {
   bool setHeight(const int height);
   bool setFOV(const Scalar fov);
   bool setDepthScale(const Scalar depth_scale);
-  bool setPostPrecesscing(const std::vector<bool>& enabled_layers);
+  bool setPostProcesscing(const std::vector<bool>& enabled_layers);
   bool feedImageQueue(const int image_layer, const cv::Mat& image_mat);
 
   // public get functions

flightlib/src/bridges/unity_bridge.cpp

@@ -183,6 +183,19 @@ bool UnityBridge::addQuadrotor(std::shared_ptr<Quadrotor> quad) {
   return true;
 }
 
+bool UnityBridge::addStaticObject(std::shared_ptr<StaticObject> static_object) {
+  Object_t object_t;
+  object_t.ID = static_object->getID();
+  object_t.prefab_ID = static_object->getPrefabID();
+  object_t.position = positionRos2Unity(static_object->getPos());
+  object_t.rotation = quaternionRos2Unity(static_object->getQuat());
+  object_t.size = scalarRos2Unity(static_object->getSize());
+
+  static_objects_.push_back(static_object);
+  settings_.objects.push_back(object_t);
+  pub_msg_.objects.push_back(object_t);
+}
+
 bool UnityBridge::handleOutput() {
   // create new message object
   zmqpp::message msg;
@@ -231,7 +244,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 +256,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;
 }
 

flightlib/src/objects/static_gate.cpp

@@ -0,0 +1,11 @@
+#include "flightlib/objects/static_gate.hpp"
+
+namespace flightlib {
+
+StaticGate::StaticGate(std::string id, std::string prefab_id)
+  : StaticGate(id, prefab_id) {
+  id_ = id;
+  prefab_id_ = prefab_id;
+};
+
+}  // namespace flightlib

flightlib/src/sensors/rgb_camera.cpp

@@ -97,7 +97,7 @@ bool RGBCamera::setDepthScale(const Scalar depth_scale) {
   return true;
 }
 
-bool RGBCamera::setPostPrecesscing(const std::vector<bool>& enabled_layers) {
+bool RGBCamera::setPostProcesscing(const std::vector<bool>& enabled_layers) {
   if (enabled_layers_.size() != enabled_layers.size()) {
     logger_.warn(
       "Vector size does not match. The vector size should be equal to %d.",

flightlib/tests/bridges/unity_bridge.cpp

@@ -1,5 +1,6 @@
 #include "flightlib/bridges/unity_bridge.hpp"
 #include "flightlib/common/logger.hpp"
+#include "flightlib/objects/static_gate.hpp"
 
 #include <gtest/gtest.h>
 
@@ -27,19 +28,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);
 }
 
@@ -52,9 +53,7 @@ TEST(UnityBridge, HandleOutputRGB) {
   // quad->addRGBCamera(rgb);
   // unity_bridge.addQuadrotor(quad);
 
-  // bool unity_ready = unity_bridge.connectUnity(UnityScene::GARAGE);
-
-  // EXPECT_TRUE(unity_ready);
+  // EXPECT_TRUE(unity_bridge.connectUnity(UnityScene::GARAGE));
 
   // FrameID frame_id = 1;
   // unity_bridge.getRender(frame_id);
@@ -83,9 +82,7 @@ TEST(UnityBridge, HandleOutputDepth) {
   // quad->addRGBCamera(rgb);
   // unity_bridge.addQuadrotor(quad);
 
-  // bool unity_ready = unity_bridge.connectUnity(UnityScene::GARAGE);
-
-  // EXPECT_TRUE(unity_ready);
+  // EXPECT_TRUE(unity_bridge.connectUnity(UnityScene::GARAGE));
 
   // FrameID frame_id = 1;
   // unity_bridge.getRender(frame_id);
@@ -114,9 +111,7 @@ TEST(UnityBridge, HandleOutputSegmentation) {
   // quad->addRGBCamera(rgb);
   // unity_bridge.addQuadrotor(quad);
 
-  // bool unity_ready = unity_bridge.connectUnity(UnityScene::GARAGE);
-
-  // EXPECT_TRUE(unity_ready);
+  // EXPECT_TRUE(unity_bridge.connectUnity(UnityScene::GARAGE));
 
   // FrameID frame_id = 1;
   // unity_bridge.getRender(frame_id);
@@ -138,30 +133,47 @@ TEST(UnityBridge, HandleOutputSegmentation) {
 TEST(UnityBridge, HandleOutputOpticalFlow) {
   Logger logger{"Test HandleOutputOpticalFlow"};
   UnityBridge unity_bridge;
-  // QuadrotorDynamics dyn = QuadrotorDynamics(1.0, 0.2);
-  // std::shared_ptr<Quadrotor> quad = std::make_shared<Quadrotor>(dyn);
-  // std::shared_ptr<RGBCamera> rgb = std::make_shared<RGBCamera>();
-  // rgb->setPostPrecesscing(std::vector<bool>{false, false, true});
-  // quad->addRGBCamera(rgb);
-  // unity_bridge.addQuadrotor(quad);
+  QuadrotorDynamics dyn = QuadrotorDynamics(1.0, 0.2);
+  std::shared_ptr<Quadrotor> quad = std::make_shared<Quadrotor>(dyn);
+  std::shared_ptr<RGBCamera> rgb = std::make_shared<RGBCamera>();
+  rgb->setPostPrecesscing(std::vector<bool>{false, false, true});
+  quad->addRGBCamera(rgb);
+  unity_bridge.addQuadrotor(quad);
 
-  // bool unity_ready = unity_bridge.connectUnity(UnityScene::GARAGE);
+  EXPECT_TRUE(unity_bridge.connectUnity(UnityScene::GARAGE));
 
-  // EXPECT_TRUE(unity_ready);
+  FrameID frame_id = 1;
+  unity_bridge.getRender(frame_id);
+  bool handle_output = unity_bridge.handleOutput();
 
-  // FrameID frame_id = 1;
-  // unity_bridge.getRender(frame_id);
-  // bool handle_output = unity_bridge.handleOutput();
+  EXPECT_TRUE(handle_output);
 
-  // EXPECT_TRUE(handle_output);
+  cv::Mat test_img;
 
-  // cv::Mat test_img;
+  EXPECT_TRUE(rgb->getRGBImage(test_img));
+  EXPECT_FALSE(rgb->getDepthMap(test_img));
+  EXPECT_FALSE(rgb->getSegmentation(test_img));
+  EXPECT_TRUE(rgb->getOpticalFlow(test_img));
+  cv::imwrite("/tmp/optical.png", test_img);
 
-  // EXPECT_TRUE(rgb->getRGBImage(test_img));
-  // EXPECT_FALSE(rgb->getDepthMap(test_img));
-  // EXPECT_FALSE(rgb->getSegmentation(test_img));
-  // EXPECT_TRUE(rgb->getOpticalFlow(test_img));
+  // timeout flightmare
+  usleep(5 * 1e6);
+}
 
-  // // timeout flightmare
-  // usleep(5 * 1e6);
+TEST(UnityBridge, SpawnStaticGate) {
+  Logger logger{"Test SpawnStaticGate"};
+  UnityBridge unity_bridge;
+  QuadrotorDynamics dyn = QuadrotorDynamics(1.0, 0.2);
+  std::shared_ptr<Quadrotor> quad = std::make_shared<Quadrotor>(dyn);
+  std::shared_ptr<RGBCamera> rgb = std::make_shared<RGBCamera>();
+  std::string object_id = "unity_gate";
+  std::string prefab_id = "rpg_gate";
+  std::shared_ptr<StaticGate> obj =
+    std::make_shared<StaticGate>(object_id, prefab_id);
+  rgb->setPostPrecesscing(std::vector<bool>{false, false, true});
+  quad->addRGBCamera(rgb);
+  unity_bridge.addQuadrotor(quad);
+  unity_bridge.addStaticObject(obj);
+
+  EXPECT_TRUE(unity_bridge.connectUnity(UnityScene::GARAGE));
 }

flightros/CMakeLists.txt

@@ -39,6 +39,7 @@ cs_add_library(flight_pilot
 target_link_libraries(flight_pilot
   ${catkin_LIBRARIES}
   ${OpenCV_LIBRARIES}
+  stdc++fs
 )
 
 cs_add_executable(flight_pilot_node
@@ -48,6 +49,7 @@ cs_add_executable(flight_pilot_node
 target_link_libraries(flight_pilot_node
   flight_pilot
   ${OpenCV_LIBRARIES}
+  stdc++fs
 )
 
 # Finish

flightros/src/flight_pilot.cpp

@@ -68,7 +68,7 @@ void FlightPilot::poseCallback(const nav_msgs::Odometry::ConstPtr &msg) {
 
   if (unity_render_ && unity_ready_) {
     unity_bridge_ptr_->getRender(0);
-    unity_bridge_ptr_->handleOutput(unity_output_);
+    unity_bridge_ptr_->handleOutput();
   }
 }