Merge pull request uzh-rpg#107 from francofusco/master

New `StaticObject` API

flightlib/include/flightlib/common/types.hpp

@@ -21,15 +21,15 @@ using SceneID = size_t;
 // Define `Dynamic` matrix size.
 static constexpr int Dynamic = Eigen::Dynamic;
 
-// Using shorthand for `Matrix<ros, cols>` with scalar type.
+// Using shorthand for `Matrix<rows, cols>` with scalar type.
 template<int rows = Dynamic, int cols = Dynamic>
 using Matrix = Eigen::Matrix<Scalar, rows, cols>;
 
-// Using shorthand for `Matrix<ros, cols>` with scalar type.
+// Using shorthand for `Matrix<rows, cols>` with scalar type.
 template<int rows = Dynamic, int cols = Dynamic>
 using MatrixRowMajor = Eigen::Matrix<Scalar, rows, cols, Eigen::RowMajor>;
 
-// Using shorthand for `Vector<ros>` with scalar type.
+// Using shorthand for `Vector<rows>` with scalar type.
 template<int rows = Dynamic>
 using Vector = Matrix<rows, 1>;
 

flightlib/include/flightlib/objects/static_gate.hpp

@@ -5,26 +5,9 @@
 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};
+  StaticGate(const std::string& id, const std::string& prefab_id = "rpg_gate")
+    : StaticObject(id, prefab_id) {}
+  ~StaticGate() {}
 };
 
-}  // namespace flightlib
+}  // namespace flightlib

flightlib/include/flightlib/objects/static_object.hpp

@@ -5,23 +5,32 @@
 namespace flightlib {
 class StaticObject {
  public:
-  StaticObject(std::string id, std::string prefab_id)
+  StaticObject(const std::string& id, const std::string& prefab_id)
     : id_(id), prefab_id_(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 set functions
+  virtual void setPosition(const Vector<3>& position) { position_ = position; };
+  virtual void setQuaternion(const Quaternion& quaternion) {
+    quat_ = quaternion;
+  };
+  virtual void setSize(const Vector<3>& size) { size_ = size; };
 
   // public get functions
-  const std::string getID(void) { return id_; };
-  const std::string getPrefabID(void) { return prefab_id_; };
+  virtual Vector<3> getPosition(void) { return position_; };
+  virtual Quaternion getQuaternion(void) { return quat_; };
+  virtual Vector<3> getSize(void) { return size_; };
+  const std::string& getID(void) { return id_; };
+  const std::string& getPrefabID(void) { return prefab_id_; };
 
+ private:
+  std::string id_;
+  std::string prefab_id_;
 
  protected:
-  const std::string id_;
-  const 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
+}  // namespace flightlib

flightlib/src/bridges/unity_bridge.cpp

@@ -115,8 +115,8 @@ bool UnityBridge::getRender(const FrameID frame_id) {
 
   for (size_t idx = 0; idx < pub_msg_.objects.size(); idx++) {
     std::shared_ptr<StaticObject> gate = static_objects_[idx];
-    pub_msg_.objects[idx].position = positionRos2Unity(gate->getPos());
-    pub_msg_.objects[idx].rotation = quaternionRos2Unity(gate->getQuat());
+    pub_msg_.objects[idx].position = positionRos2Unity(gate->getPosition());
+    pub_msg_.objects[idx].rotation = quaternionRos2Unity(gate->getQuaternion());
   }
 
   // create new message object
@@ -187,8 +187,8 @@ 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.position = positionRos2Unity(static_object->getPosition());
+  object_t.rotation = quaternionRos2Unity(static_object->getQuaternion());
   object_t.size = scalarRos2Unity(static_object->getSize());
 
   static_objects_.push_back(static_object);

flightlib/tests/bridges/unity_bridge.cpp

@@ -177,9 +177,7 @@ TEST(UnityBridge, SpawnStaticGate) {
   unity_bridge.addQuadrotor(quad);
 
   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);
+  std::shared_ptr<StaticGate> obj = std::make_shared<StaticGate>(object_id);
   obj->setPosition(Eigen::Vector3f(0, -10, -3));
   unity_bridge.addStaticObject(obj);
 
@@ -208,9 +206,7 @@ TEST(UnityBridge, Spawn100StaticGate) {
   for (int i = 0; i < 4; i++) {
     for (int j = 0; j < 25; j++) {
       std::string object_id = "unity_gate" + std::to_string(25 * i + j);
-      std::string prefab_id = "rpg_gate";
-      std::shared_ptr<StaticGate> obj =
-        std::make_shared<StaticGate>(object_id, prefab_id);
+      std::shared_ptr<StaticGate> obj = std::make_shared<StaticGate>(object_id);
       obj->setPosition(Eigen::Vector3f(j - 12, -10 + i, -3));
       unity_bridge.addStaticObject(obj);
     }

flightros/src/racing/racing.cpp

@@ -46,18 +46,24 @@ int main(int argc, char *argv[]) {
   // Initialize gates
   std::string object_id = "unity_gate";
   std::string prefab_id = "rpg_gate";
-  std::shared_ptr<StaticGate> gate_1 =
-    std::make_shared<StaticGate>(object_id, prefab_id);
-  gate_1->setPosition(Eigen::Vector3f(5, 0, 2.5));
-  gate_1->setRotation(
-    Quaternion(std::cos(1 * M_PI_2), 0.0, 0.0, std::sin(1 * M_PI_2)));
+  std::shared_ptr<StaticObject> gate_1 =
+    std::make_shared<StaticObject>(object_id, prefab_id);
+  gate_1->setPosition(Eigen::Vector3f(0, 10, 2.5));
+  gate_1->setQuaternion(
+    Quaternion(std::cos(1 * M_PI_4), 0.0, 0.0, std::sin(1 * M_PI_4)));
 
   std::string object_id_2 = "unity_gate_2";
   std::shared_ptr<StaticGate> gate_2 =
-    std::make_shared<StaticGate>(object_id_2, prefab_id);
-  gate_2->setPosition(Eigen::Vector3f(-5, 0, 2.5));
-  gate_2->setRotation(
-    Quaternion(std::cos(1 * M_PI_2), 0.0, 0.0, std::sin(1 * M_PI_2)));
+    std::make_shared<StaticGate>(object_id_2);
+  gate_2->setPosition(Eigen::Vector3f(0, -10, 2.5));
+  gate_2->setQuaternion(
+    Quaternion(std::cos(1 * M_PI_4), 0.0, 0.0, std::sin(1 * M_PI_4)));
+
+  std::string object_id_3 = "moving_gate";
+  std::shared_ptr<StaticGate> gate_3 =
+    std::make_shared<StaticGate>(object_id_3);
+  gate_3->setPosition(Eigen::Vector3f(5, 0, 2.5));
+  gate_3->setQuaternion(Quaternion(0.0, 0.0, 0.0, 1.0));
 
   // Define path through gates
   std::vector<Eigen::Vector3d> way_points;
@@ -86,6 +92,7 @@ int main(int argc, char *argv[]) {
 
   unity_bridge_ptr->addStaticObject(gate_1);
   unity_bridge_ptr->addStaticObject(gate_2);
+  unity_bridge_ptr->addStaticObject(gate_3);
   unity_bridge_ptr->addQuadrotor(quad_ptr);
   // connect unity
   unity_ready = unity_bridge_ptr->connectUnity(scene_id);
@@ -106,6 +113,10 @@ int main(int argc, char *argv[]) {
 
     quad_ptr->setState(quad_state);
 
+    auto gate_position = gate_3->getPosition();
+    gate_position(0) = (Scalar)desired_pose.position.x();
+    gate_3->setPosition(gate_position);
+
     unity_bridge_ptr->getRender(frame_id);
     unity_bridge_ptr->handleOutput();
 
@@ -114,4 +125,4 @@ int main(int argc, char *argv[]) {
   }
 
   return 0;
-}
+}