Updated StaticObject API

- Make `StaticObject` concrete (no pure virtuals). Allow subclasses to 
still change internal mechanics by keeping seters and getters virtual.
- Make `StaticGate` nothing but a shorthand to spawn the "rpg_gate" 
asset. This also solves a shadowning issue.
- Change `getPos` into `getPosition` (so that it matches formatting of 
`setPosition`).
- Return `id_` and `prefab_id_` by const reference.
- Updated `unity_bridge.cpp` to use the new API.
- Updated `racing.cpp` (from the `flightros` pkg) to show and use the 
new API.

flightlib/include/flightlib/objects/static_gate.hpp

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

flightlib/include/flightlib/objects/static_object.hpp

@@ -9,19 +9,25 @@ class StaticObject {
     : 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);

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