From 3afb7135567097f82077392bf02d2a76ee608beb Mon Sep 17 00:00:00 2001 From: Franco Fusco Date: Mon, 29 Mar 2021 12:39:41 +0200 Subject: [PATCH 1/8] Fixed typo In few comments, `ros` was written instead of `rows`. --- flightlib/include/flightlib/common/types.hpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/flightlib/include/flightlib/common/types.hpp b/flightlib/include/flightlib/common/types.hpp index 69b4e17f9a..966b0b617e 100644 --- a/flightlib/include/flightlib/common/types.hpp +++ b/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` with scalar type. +// Using shorthand for `Matrix` with scalar type. template using Matrix = Eigen::Matrix; -// Using shorthand for `Matrix` with scalar type. +// Using shorthand for `Matrix` with scalar type. template using MatrixRowMajor = Eigen::Matrix; -// Using shorthand for `Vector` with scalar type. +// Using shorthand for `Vector` with scalar type. template using Vector = Matrix; From a2af4cb6a28b0396948a2274600599887ba1bdc3 Mon Sep 17 00:00:00 2001 From: Franco Fusco Date: Mon, 29 Mar 2021 12:48:37 +0200 Subject: [PATCH 2/8] 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. --- .../include/flightlib/objects/static_gate.hpp | 24 ++------------ .../flightlib/objects/static_object.hpp | 26 ++++++++++------ flightlib/src/bridges/unity_bridge.cpp | 8 ++--- flightlib/src/objects/static_gate.cpp | 11 ------- flightros/src/racing/racing.cpp | 31 +++++++++++++------ 5 files changed, 44 insertions(+), 56 deletions(-) delete mode 100644 flightlib/src/objects/static_gate.cpp diff --git a/flightlib/include/flightlib/objects/static_gate.hpp b/flightlib/include/flightlib/objects/static_gate.hpp index c5fb4a76c9..9a8301a7a6 100644 --- a/flightlib/include/flightlib/objects/static_gate.hpp +++ b/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 \ No newline at end of file +} // namespace flightlib diff --git a/flightlib/include/flightlib/objects/static_object.hpp b/flightlib/include/flightlib/objects/static_object.hpp index 6273afaefc..5da83bdadf 100644 --- a/flightlib/include/flightlib/objects/static_object.hpp +++ b/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 \ No newline at end of file +} // namespace flightlib diff --git a/flightlib/src/bridges/unity_bridge.cpp b/flightlib/src/bridges/unity_bridge.cpp index d2ed8b09ed..6c62061445 100644 --- a/flightlib/src/bridges/unity_bridge.cpp +++ b/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 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 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); diff --git a/flightlib/src/objects/static_gate.cpp b/flightlib/src/objects/static_gate.cpp deleted file mode 100644 index a658dc5b59..0000000000 --- a/flightlib/src/objects/static_gate.cpp +++ /dev/null @@ -1,11 +0,0 @@ -#include "flightlib/objects/static_gate.hpp" - -namespace flightlib { - -StaticGate::StaticGate(std::string id, std::string prefab_id) - : StaticObject(id, prefab_id) { - id_ = id; - prefab_id_ = prefab_id; -}; - -} // namespace flightlib \ No newline at end of file diff --git a/flightros/src/racing/racing.cpp b/flightros/src/racing/racing.cpp index e2873b54c4..f4eacfe6e0 100644 --- a/flightros/src/racing/racing.cpp +++ b/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 gate_1 = - std::make_shared(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 gate_1 = + std::make_shared(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 gate_2 = - std::make_shared(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(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 gate_3 = + std::make_shared(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 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; -} \ No newline at end of file +} From c51b046cd2d433060bc46332556974c898a6fa2c Mon Sep 17 00:00:00 2001 From: Franco Fusco Date: Mon, 29 Mar 2021 13:00:25 +0200 Subject: [PATCH 3/8] Make pose protected Make sure that sub-classes of `StaticObject` can change the pose and size of the object (by making the corresponding variables protected). --- flightlib/include/flightlib/objects/static_object.hpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/flightlib/include/flightlib/objects/static_object.hpp b/flightlib/include/flightlib/objects/static_object.hpp index 5da83bdadf..a745b6a194 100644 --- a/flightlib/include/flightlib/objects/static_object.hpp +++ b/flightlib/include/flightlib/objects/static_object.hpp @@ -21,10 +21,11 @@ class StaticObject { const std::string& getID(void) { return id_; }; const std::string& getPrefabID(void) { return prefab_id_; }; - private: +private: std::string id_; std::string prefab_id_; +protected: 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}; From 6742fe243e7e724895ac213705dbdf3a3c41dea6 Mon Sep 17 00:00:00 2001 From: Franco Fusco Date: Mon, 29 Mar 2021 13:52:38 +0200 Subject: [PATCH 4/8] Fixing clang-formatting --- flightlib/include/flightlib/objects/static_object.hpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/flightlib/include/flightlib/objects/static_object.hpp b/flightlib/include/flightlib/objects/static_object.hpp index a745b6a194..77a94994ca 100644 --- a/flightlib/include/flightlib/objects/static_object.hpp +++ b/flightlib/include/flightlib/objects/static_object.hpp @@ -11,7 +11,9 @@ class StaticObject { // public set functions virtual void setPosition(const Vector<3>& position) { position_ = position; }; - virtual void setQuaternion(const Quaternion& quaternion) { quat_ = quaternion; }; + virtual void setQuaternion(const Quaternion& quaternion) { + quat_ = quaternion; + }; virtual void setSize(const Vector<3>& size) { size_ = size; }; // public get functions @@ -21,11 +23,11 @@ class StaticObject { const std::string& getID(void) { return id_; }; const std::string& getPrefabID(void) { return prefab_id_; }; -private: + private: std::string id_; std::string prefab_id_; -protected: + protected: 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}; From 4c09c01f7988b0effcb795d2ddd2a711f5a59faa Mon Sep 17 00:00:00 2001 From: Franco Fusco Date: Mon, 29 Mar 2021 18:20:35 +0200 Subject: [PATCH 5/8] Fixed unity_bridge test --- flightlib/tests/bridges/unity_bridge.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/flightlib/tests/bridges/unity_bridge.cpp b/flightlib/tests/bridges/unity_bridge.cpp index 681365fd0c..49d6962ad0 100644 --- a/flightlib/tests/bridges/unity_bridge.cpp +++ b/flightlib/tests/bridges/unity_bridge.cpp @@ -177,9 +177,8 @@ TEST(UnityBridge, SpawnStaticGate) { unity_bridge.addQuadrotor(quad); std::string object_id = "unity_gate"; - std::string prefab_id = "rpg_gate"; std::shared_ptr obj = - std::make_shared(object_id, prefab_id); + std::make_shared(object_id); obj->setPosition(Eigen::Vector3f(0, -10, -3)); unity_bridge.addStaticObject(obj); @@ -208,9 +207,8 @@ 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 obj = - std::make_shared(object_id, prefab_id); + std::make_shared(object_id); obj->setPosition(Eigen::Vector3f(j - 12, -10 + i, -3)); unity_bridge.addStaticObject(obj); } From 095a6279a5ffd767e5ea8ab121b16a1ba2e810a2 Mon Sep 17 00:00:00 2001 From: Franco Fusco Date: Mon, 29 Mar 2021 18:30:14 +0200 Subject: [PATCH 6/8] Clang-format on test --- flightlib/tests/bridges/unity_bridge.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/flightlib/tests/bridges/unity_bridge.cpp b/flightlib/tests/bridges/unity_bridge.cpp index 49d6962ad0..b7557343e0 100644 --- a/flightlib/tests/bridges/unity_bridge.cpp +++ b/flightlib/tests/bridges/unity_bridge.cpp @@ -177,8 +177,7 @@ TEST(UnityBridge, SpawnStaticGate) { unity_bridge.addQuadrotor(quad); std::string object_id = "unity_gate"; - std::shared_ptr obj = - std::make_shared(object_id); + std::shared_ptr obj = std::make_shared(object_id); obj->setPosition(Eigen::Vector3f(0, -10, -3)); unity_bridge.addStaticObject(obj); @@ -207,8 +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::shared_ptr obj = - std::make_shared(object_id); + std::shared_ptr obj = std::make_shared(object_id); obj->setPosition(Eigen::Vector3f(j - 12, -10 + i, -3)); unity_bridge.addStaticObject(obj); } From bf95e622c98f0a102dd0c98b8d9fea7a357e1b00 Mon Sep 17 00:00:00 2001 From: Franco Fusco Date: Wed, 31 Mar 2021 10:23:57 +0200 Subject: [PATCH 7/8] StaticGate can choose its prefab_id --- flightlib/include/flightlib/objects/static_gate.hpp | 3 ++- flightlib/include/flightlib/objects/static_object.hpp | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/flightlib/include/flightlib/objects/static_gate.hpp b/flightlib/include/flightlib/objects/static_gate.hpp index 9a8301a7a6..689b153748 100644 --- a/flightlib/include/flightlib/objects/static_gate.hpp +++ b/flightlib/include/flightlib/objects/static_gate.hpp @@ -5,7 +5,8 @@ namespace flightlib { class StaticGate : public StaticObject { public: - StaticGate(std::string id) : StaticObject(id, "rpg_gate") {} + StaticGate(const std::string& id, const std::string& prefab_id="rpg_gate") + : StaticObject(id, prefab_id) {} ~StaticGate() {} }; diff --git a/flightlib/include/flightlib/objects/static_object.hpp b/flightlib/include/flightlib/objects/static_object.hpp index 77a94994ca..1b0b8014d8 100644 --- a/flightlib/include/flightlib/objects/static_object.hpp +++ b/flightlib/include/flightlib/objects/static_object.hpp @@ -5,7 +5,7 @@ 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(){}; From 290d42599ff9d78102e20ffb2cfcd5cd8b405e04 Mon Sep 17 00:00:00 2001 From: Franco Fusco Date: Wed, 31 Mar 2021 10:26:02 +0200 Subject: [PATCH 8/8] Fix format --- flightlib/include/flightlib/objects/static_gate.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flightlib/include/flightlib/objects/static_gate.hpp b/flightlib/include/flightlib/objects/static_gate.hpp index 689b153748..0f8417ddb3 100644 --- a/flightlib/include/flightlib/objects/static_gate.hpp +++ b/flightlib/include/flightlib/objects/static_gate.hpp @@ -5,7 +5,7 @@ namespace flightlib { class StaticGate : public StaticObject { public: - StaticGate(const std::string& id, const std::string& prefab_id="rpg_gate") + StaticGate(const std::string& id, const std::string& prefab_id = "rpg_gate") : StaticObject(id, prefab_id) {} ~StaticGate() {} };