diff --git a/gazebo/simulator.cpp b/gazebo/simulator.cpp index 04ec3f2..ca996bb 100644 --- a/gazebo/simulator.cpp +++ b/gazebo/simulator.cpp @@ -7,13 +7,11 @@ #include #include #include -#include #include #include #include #include #include -#include #include #include @@ -55,7 +53,6 @@ public: gzmsg << "Flix plugin loaded" << endl; } -public: void OnReset() { attitude = Quaternion(); // reset estimated attitude gzmsg << "Flix plugin reset" << endl; @@ -82,12 +79,12 @@ public: control(); parseInput(); - applyMotorsThrust(); + applyMotorForces(); publishTopics(); logData(); } - void applyMotorsThrust() { + void applyMotorForces() { // thrusts const double dist = 0.035355; // motors shift from the center, m const double maxThrust = 0.03 * ONE_G; // ~30 g, https://youtu.be/VtKI4Pjx8Sk?&t=78 @@ -115,6 +112,7 @@ public: nodeHandle = transport::NodePtr(new transport::Node()); nodeHandle->Init(); string ns = "~/" + model->GetName(); + // create motors output topics for debugging and plotting motorPub[0] = nodeHandle->Advertise(ns + "/motor0"); motorPub[1] = nodeHandle->Advertise(ns + "/motor1"); motorPub[2] = nodeHandle->Advertise(ns + "/motor2");