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; diff --git a/flightlib/include/flightlib/objects/static_gate.hpp b/flightlib/include/flightlib/objects/static_gate.hpp index c5fb4a76c9..0f8417ddb3 100644 --- a/flightlib/include/flightlib/objects/static_gate.hpp +++ b/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 \ 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..1b0b8014d8 100644 --- a/flightlib/include/flightlib/objects/static_object.hpp +++ b/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 \ 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/flightlib/tests/bridges/unity_bridge.cpp b/flightlib/tests/bridges/unity_bridge.cpp index 681365fd0c..b7557343e0 100644 --- a/flightlib/tests/bridges/unity_bridge.cpp +++ b/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 obj = - std::make_shared(object_id, prefab_id); + std::shared_ptr obj = std::make_shared(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 obj = - std::make_shared(object_id, prefab_id); + std::shared_ptr obj = std::make_shared(object_id); obj->setPosition(Eigen::Vector3f(j - 12, -10 + i, -3)); unity_bridge.addStaticObject(obj); } 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 +}