From c238b032d11cc6a5717c6bceb58995904d04311f Mon Sep 17 00:00:00 2001 From: Alaa El Jawad Date: Wed, 25 Jan 2023 17:15:11 +0100 Subject: [PATCH 01/16] add deadband to thruster Signed-off-by: Alaa El Jawad --- src/systems/thruster/Thruster.cc | 71 ++++++++++++++++++++++++++++++++ src/systems/thruster/Thruster.hh | 2 + 2 files changed, 73 insertions(+) diff --git a/src/systems/thruster/Thruster.cc b/src/systems/thruster/Thruster.cc index c8a4492210..0ce57294c1 100644 --- a/src/systems/thruster/Thruster.cc +++ b/src/systems/thruster/Thruster.cc @@ -14,6 +14,8 @@ * limitations under the License. * */ +#include +#include #include #include #include @@ -144,6 +146,15 @@ class gz::sim::systems::ThrusterPrivateData /// \brief Linear velocity of the vehicle. public: double linearVelocity = 0.0; + /// \brief deadband + public: double deadband = 0.0; + + /// \brief Flag to enable/disable deadband + public: bool enableDeadband = false; + + /// \brief Topic name used to enable/disable the deadband + public: std::string deadbandTopic = ""; + /// \brief Topic name used to control thrust. Optional public: std::string topic = ""; @@ -159,6 +170,9 @@ class gz::sim::systems::ThrusterPrivateData /// \brief Callback for handling thrust update public: void OnCmdThrust(const msgs::Double &_msg); + /// \brief Callback for handling deadband enable/disable update + public: void OnDeabandEnable(const msgs::Boolean &_msg); + /// \brief Recalculates and updates the thrust coefficient. public: void UpdateThrustCoefficient(); @@ -179,6 +193,10 @@ class gz::sim::systems::ThrusterPrivateData /// \return True if battery is charged, false otherwise. If no battery found, /// returns true. public: bool HasSufficientBattery(const EntityComponentManager &_ecm) const; + + /// \brief Applies the deadband to the thrust and angular velocity by setting + /// those values to zero if their absolute value is below the deadband + public: void ApplyDeadband(double &_thrust, double &_angVel); }; ///////////////////////////////////////////////// @@ -276,6 +294,13 @@ void Thruster::Configure( << "[thrust_coefficient] value from the SDF file." << std::endl; } } + + // Get deadband, default to 0 + if (_sdf->HasElement("deadband")) + { + this->dataPtr->deadband = _sdf->Get("deadband"); + this->dataPtr->enableDeadband = true; + } // Get a custom topic. if (_sdf->HasElement("topic")) @@ -312,6 +337,8 @@ void Thruster::Configure( // Subscribe to specified topic for force commands thrusterTopic = gz::transport::TopicUtils::AsValidTopic( ns + "/" + this->dataPtr->topic); + this->dataPtr->deadbandTopic = gz::transport::TopicUtils::AsValidTopic( + ns + "/" + this->dataPtr->topic + "/enable_deadband"); if (this->dataPtr->opmode == ThrusterPrivateData::OperationMode::ForceCmd) { this->dataPtr->node.Subscribe( @@ -347,6 +374,9 @@ void Thruster::Configure( feedbackTopic = gz::transport::TopicUtils::AsValidTopic( "/model/" + ns + "/joint/" + jointName + "/ang_vel"); + + this->dataPtr->deadbandTopic = gz::transport::TopicUtils::AsValidTopic( + "/model/" + ns + "/joint/" + jointName + "/enable_deadband"); } else { @@ -362,10 +392,19 @@ void Thruster::Configure( feedbackTopic = gz::transport::TopicUtils::AsValidTopic( "/model/" + ns + "/joint/" + jointName + "/force"); + + this->dataPtr->deadbandTopic = gz::transport::TopicUtils::AsValidTopic( + "/model/" + ns + "/joint/" + jointName + "/enable_deadband"); } gzmsg << "Thruster listening to commands on [" << thrusterTopic << "]" << std::endl; + this->dataPtr->node.Subscribe( + this->dataPtr->deadbandTopic, + &ThrusterPrivateData::OnDeabandEnable, + this->dataPtr.get()); + gzmsg << "Thruster listening to enable_deadband on [" + << this->dataPtr->deadbandTopic << "]" << std::endl; this->dataPtr->pub = this->dataPtr->node.Advertise( feedbackTopic); @@ -475,6 +514,19 @@ void ThrusterPrivateData::OnCmdThrust(const gz::msgs::Double &_msg) this->propellerAngVel = this->ThrustToAngularVec(this->thrust); } +///////////////////////////////////////////////// +void ThrusterPrivateData::OnDeabandEnable(const gz::msgs::Boolean &_msg) +{ + if (_msg.data() != this->enableDeadband) + { + if (_msg.data()) gzmsg << "Enabling deadband." << std::endl; + else gzmsg << "Disabling deadband." << std::endl; + + this->enableDeadband = _msg.data(); + } + +} + ///////////////////////////////////////////////// void ThrusterPrivateData::OnCmdAngVel(const gz::msgs::Double &_msg) { @@ -550,6 +602,16 @@ bool ThrusterPrivateData::HasSufficientBattery( return result; } +///////////////////////////////////////////////// +void ThrusterPrivateData::ApplyDeadband(double &_thrust, double &_angVel) +{ + if (abs(_thrust) < this->deadband) + { + _thrust = 0.0; + _angVel = 0.0; + } +} + ///////////////////////////////////////////////// void Thruster::PreUpdate( const gz::sim::UpdateInfo &_info, @@ -562,6 +624,9 @@ void Thruster::PreUpdate( { return; } + if (!_ecm.HasEntity(this->dataPtr->linkEntity)){ + return; + } // Init battery consumption if it was set if (!this->dataPtr->batteryName.empty() && @@ -630,6 +695,12 @@ void Thruster::PreUpdate( this->dataPtr->ThrustToAngularVec(this->dataPtr->thrust); desiredPropellerAngVel = this->dataPtr->propellerAngVel; } + + if (this->dataPtr->enableDeadband) + { + this->dataPtr->ApplyDeadband(desiredThrust, desiredPropellerAngVel); + } + msgs::Double angvel; // PID control diff --git a/src/systems/thruster/Thruster.hh b/src/systems/thruster/Thruster.hh index 81cbb12e94..af05a237ab 100644 --- a/src/systems/thruster/Thruster.hh +++ b/src/systems/thruster/Thruster.hh @@ -85,6 +85,8 @@ namespace systems /// [Optional, defaults to 1000N or 1000rad/s] /// - - Minimum input thrust or angular velocity command. /// [Optional, defaults to -1000N or -1000rad/s] + /// - - Deadband of the thruster. Absolute value below which the + /// thruster won't spin nor generate thrust /// - - Relative speed reduction between the water /// at the propeller (Va) vs behind the vessel. /// [Optional, defults to 0.2] From b5cc5c7aba747d5e4ac65200ed366e29a5012783 Mon Sep 17 00:00:00 2001 From: Alaa El Jawad Date: Tue, 11 Apr 2023 14:58:56 +0200 Subject: [PATCH 02/16] fix typo Signed-off-by: Alaa El Jawad --- src/systems/thruster/Thruster.cc | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/systems/thruster/Thruster.cc b/src/systems/thruster/Thruster.cc index 0ce57294c1..f712581c07 100644 --- a/src/systems/thruster/Thruster.cc +++ b/src/systems/thruster/Thruster.cc @@ -171,7 +171,7 @@ class gz::sim::systems::ThrusterPrivateData public: void OnCmdThrust(const msgs::Double &_msg); /// \brief Callback for handling deadband enable/disable update - public: void OnDeabandEnable(const msgs::Boolean &_msg); + public: void OnDeadbandEnable(const msgs::Boolean &_msg); /// \brief Recalculates and updates the thrust coefficient. public: void UpdateThrustCoefficient(); @@ -401,7 +401,7 @@ void Thruster::Configure( << std::endl; this->dataPtr->node.Subscribe( this->dataPtr->deadbandTopic, - &ThrusterPrivateData::OnDeabandEnable, + &ThrusterPrivateData::OnDeadbandEnable, this->dataPtr.get()); gzmsg << "Thruster listening to enable_deadband on [" << this->dataPtr->deadbandTopic << "]" << std::endl; @@ -515,7 +515,7 @@ void ThrusterPrivateData::OnCmdThrust(const gz::msgs::Double &_msg) } ///////////////////////////////////////////////// -void ThrusterPrivateData::OnDeabandEnable(const gz::msgs::Boolean &_msg) +void ThrusterPrivateData::OnDeadbandEnable(const gz::msgs::Boolean &_msg) { if (_msg.data() != this->enableDeadband) { From 98970d7127421aa2439a9ec9e0df5a6d1c581eef Mon Sep 17 00:00:00 2001 From: Alaa El Jawad Date: Tue, 11 Apr 2023 15:13:46 +0200 Subject: [PATCH 03/16] document the topic to change the deadband at runtime Signed-off-by: Alaa El Jawad --- src/systems/thruster/Thruster.hh | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/systems/thruster/Thruster.hh b/src/systems/thruster/Thruster.hh index af05a237ab..1476c75fe4 100644 --- a/src/systems/thruster/Thruster.hh +++ b/src/systems/thruster/Thruster.hh @@ -86,7 +86,10 @@ namespace systems /// - - Minimum input thrust or angular velocity command. /// [Optional, defaults to -1000N or -1000rad/s] /// - - Deadband of the thruster. Absolute value below which the - /// thruster won't spin nor generate thrust + /// thruster won't spin nor generate thrust. This value can also + /// be changed at runtime using a topic. The topic is either + /// `/model/{ns}/joint/{jointName}/enable_deadband` or + /// `{ns}/{topic}/enable_deadband` depending on other params /// - - Relative speed reduction between the water /// at the propeller (Va) vs behind the vessel. /// [Optional, defults to 0.2] From 4d524b1a7bf688b6124461eddafed95a083a3bab Mon Sep 17 00:00:00 2001 From: Alaa El Jawad Date: Tue, 11 Apr 2023 15:15:51 +0200 Subject: [PATCH 04/16] respect code style Signed-off-by: Alaa El Jawad --- src/systems/thruster/Thruster.cc | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/src/systems/thruster/Thruster.cc b/src/systems/thruster/Thruster.cc index f712581c07..ef942507d8 100644 --- a/src/systems/thruster/Thruster.cc +++ b/src/systems/thruster/Thruster.cc @@ -519,8 +519,14 @@ void ThrusterPrivateData::OnDeadbandEnable(const gz::msgs::Boolean &_msg) { if (_msg.data() != this->enableDeadband) { - if (_msg.data()) gzmsg << "Enabling deadband." << std::endl; - else gzmsg << "Disabling deadband." << std::endl; + if (_msg.data()) + { + gzmsg << "Enabling deadband." << std::endl; + } + else + { + gzmsg << "Disabling deadband." << std::endl; + } this->enableDeadband = _msg.data(); } From 420561da2d2cf68e9fd377ec4954edf37e948bcd Mon Sep 17 00:00:00 2001 From: Alaa El Jawad Date: Tue, 11 Apr 2023 15:24:16 +0200 Subject: [PATCH 05/16] fix code style Signed-off-by: Alaa El Jawad --- src/systems/thruster/Thruster.cc | 14 +++++++------- src/systems/thruster/Thruster.hh | 6 +++--- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/src/systems/thruster/Thruster.cc b/src/systems/thruster/Thruster.cc index ef942507d8..c5676628d5 100644 --- a/src/systems/thruster/Thruster.cc +++ b/src/systems/thruster/Thruster.cc @@ -193,7 +193,7 @@ class gz::sim::systems::ThrusterPrivateData /// \return True if battery is charged, false otherwise. If no battery found, /// returns true. public: bool HasSufficientBattery(const EntityComponentManager &_ecm) const; - + /// \brief Applies the deadband to the thrust and angular velocity by setting /// those values to zero if their absolute value is below the deadband public: void ApplyDeadband(double &_thrust, double &_angVel); @@ -294,7 +294,7 @@ void Thruster::Configure( << "[thrust_coefficient] value from the SDF file." << std::endl; } } - + // Get deadband, default to 0 if (_sdf->HasElement("deadband")) { @@ -374,7 +374,7 @@ void Thruster::Configure( feedbackTopic = gz::transport::TopicUtils::AsValidTopic( "/model/" + ns + "/joint/" + jointName + "/ang_vel"); - + this->dataPtr->deadbandTopic = gz::transport::TopicUtils::AsValidTopic( "/model/" + ns + "/joint/" + jointName + "/enable_deadband"); } @@ -403,7 +403,7 @@ void Thruster::Configure( this->dataPtr->deadbandTopic, &ThrusterPrivateData::OnDeadbandEnable, this->dataPtr.get()); - gzmsg << "Thruster listening to enable_deadband on [" + gzmsg << "Thruster listening to enable_deadband on [" << this->dataPtr->deadbandTopic << "]" << std::endl; this->dataPtr->pub = this->dataPtr->node.Advertise( @@ -527,7 +527,7 @@ void ThrusterPrivateData::OnDeadbandEnable(const gz::msgs::Boolean &_msg) { gzmsg << "Disabling deadband." << std::endl; } - + this->enableDeadband = _msg.data(); } @@ -701,12 +701,12 @@ void Thruster::PreUpdate( this->dataPtr->ThrustToAngularVec(this->dataPtr->thrust); desiredPropellerAngVel = this->dataPtr->propellerAngVel; } - + if (this->dataPtr->enableDeadband) { this->dataPtr->ApplyDeadband(desiredThrust, desiredPropellerAngVel); } - + msgs::Double angvel; // PID control diff --git a/src/systems/thruster/Thruster.hh b/src/systems/thruster/Thruster.hh index 1476c75fe4..5c9cdab562 100644 --- a/src/systems/thruster/Thruster.hh +++ b/src/systems/thruster/Thruster.hh @@ -85,10 +85,10 @@ namespace systems /// [Optional, defaults to 1000N or 1000rad/s] /// - - Minimum input thrust or angular velocity command. /// [Optional, defaults to -1000N or -1000rad/s] - /// - - Deadband of the thruster. Absolute value below which the - /// thruster won't spin nor generate thrust. This value can also + /// - - Deadband of the thruster. Absolute value below which the + /// thruster won't spin nor generate thrust. This value can /// be changed at runtime using a topic. The topic is either - /// `/model/{ns}/joint/{jointName}/enable_deadband` or + /// `/model/{ns}/joint/{jointName}/enable_deadband` or /// `{ns}/{topic}/enable_deadband` depending on other params /// - - Relative speed reduction between the water /// at the propeller (Va) vs behind the vessel. From addb0e227e75aa82d9559b3641e676d71b945bc2 Mon Sep 17 00:00:00 2001 From: Alaa El Jawad Date: Tue, 11 Apr 2023 15:54:31 +0200 Subject: [PATCH 06/16] document method args Signed-off-by: Alaa El Jawad --- src/systems/thruster/Thruster.cc | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/systems/thruster/Thruster.cc b/src/systems/thruster/Thruster.cc index c5676628d5..e75f6adbba 100644 --- a/src/systems/thruster/Thruster.cc +++ b/src/systems/thruster/Thruster.cc @@ -171,6 +171,8 @@ class gz::sim::systems::ThrusterPrivateData public: void OnCmdThrust(const msgs::Double &_msg); /// \brief Callback for handling deadband enable/disable update + /// \param[in] _msg boolean msg to indicate whether to enable or disable + /// the deadband public: void OnDeadbandEnable(const msgs::Boolean &_msg); /// \brief Recalculates and updates the thrust coefficient. @@ -195,7 +197,9 @@ class gz::sim::systems::ThrusterPrivateData public: bool HasSufficientBattery(const EntityComponentManager &_ecm) const; /// \brief Applies the deadband to the thrust and angular velocity by setting - /// those values to zero if their absolute value is below the deadband + /// those values to zero if the thrust absolute value is below the deadband + /// \param[in] _thrust thrust in N used for check + /// \param[in] _angvel angular velocity in rad/s public: void ApplyDeadband(double &_thrust, double &_angVel); }; From 5d3fdc8b7d0717d18fff4e99afee641c498e12d8 Mon Sep 17 00:00:00 2001 From: Alaa El Jawad Date: Thu, 11 May 2023 19:09:49 +0200 Subject: [PATCH 07/16] add unit tests for thruster deadband Signed-off-by: Alaa El Jawad --- src/systems/thruster/Thruster.cc | 2 +- test/integration/thruster.cc | 97 +++++++++++++++++++++++--- test/worlds/thruster_deadband.sdf | 112 ++++++++++++++++++++++++++++++ 3 files changed, 202 insertions(+), 9 deletions(-) create mode 100644 test/worlds/thruster_deadband.sdf diff --git a/src/systems/thruster/Thruster.cc b/src/systems/thruster/Thruster.cc index e75f6adbba..3e385a6312 100644 --- a/src/systems/thruster/Thruster.cc +++ b/src/systems/thruster/Thruster.cc @@ -146,7 +146,7 @@ class gz::sim::systems::ThrusterPrivateData /// \brief Linear velocity of the vehicle. public: double linearVelocity = 0.0; - /// \brief deadband + /// \brief deadband in newtons public: double deadband = 0.0; /// \brief Flag to enable/disable deadband diff --git a/test/integration/thruster.cc b/test/integration/thruster.cc index a43ec0816c..07b978cb50 100644 --- a/test/integration/thruster.cc +++ b/test/integration/thruster.cc @@ -17,6 +17,7 @@ #include +#include #include #include @@ -55,12 +56,16 @@ class ThrusterTest : public InternalFixture<::testing::Test> /// \param[in] _useAngVelCmd Send commands in angular velocity instead of /// force /// \param[in] _mass Mass of the body being propelled. + /// \param[in] _deadband deadband value in newtons + /// \param[in] _dp_topic deadband enable topic public: void TestWorld(const std::string &_world, const std::string &_namespace, const std::string &_topic, double _thrustCoefficient, double _density, double _diameter, double _baseTol, double _wakeFraction = 0.2, double _alpha_1 = 1, double _alpha_2 = 0, bool _calculateCoefficient = false, - bool _useAngVelCmd = false, double _mass = 100.1); + bool _useAngVelCmd = false, double _mass = 100.1, + double _deadband = 0, + const std::string &_db_topic = std::string()); }; ////////////////////////////////////////////////// @@ -68,7 +73,8 @@ void ThrusterTest::TestWorld(const std::string &_world, const std::string &_namespace, const std::string &_topic, double _thrustCoefficient, double _density, double _diameter, double _baseTol, double _wakeFraction, double _alpha1, double _alpha2, - bool _calculateCoefficient, bool _useAngVelCmd, double _mass) + bool _calculateCoefficient, bool _useAngVelCmd, double _mass, + double _deadband, const std::string &_db_topic) { // Start server ServerConfig serverConfig; @@ -152,15 +158,30 @@ void ThrusterTest::TestWorld(const std::string &_world, // Publish command and check that vehicle moved transport::Node node; auto pub = node.Advertise(_topic); + auto db_pub = node.Advertise(_db_topic); int sleep{0}; int maxSleep{30}; - for (; !pub.HasConnections() && sleep < maxSleep; ++sleep) + if (_namespace != "deadband") { - std::this_thread::sleep_for(std::chrono::milliseconds(100)); + for (; !pub.HasConnections() && sleep < maxSleep; ++sleep) { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + } + else + { + for (; + !(pub.HasConnections() && db_pub.HasConnections()) && sleep < maxSleep; + ++sleep) { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } } EXPECT_LT(sleep, maxSleep); EXPECT_TRUE(pub.HasConnections()); + if (_namespace == "deadband") + { + EXPECT_TRUE(db_pub.HasConnections()); + } // Test the cmd limits specified in the world file. These should be: // if (use_angvel_cmd && thrust_coefficient < 0): @@ -170,11 +191,19 @@ void ThrusterTest::TestWorld(const std::string &_world, // min_thrust = 0 // max_thrust = 300 double invalidCmd = (_useAngVelCmd && _thrustCoefficient < 0) ? 1000 : -1000; + if (_namespace == "deadband") + { + // an invalid command in case the deadband is enabled is a command + // below the deadband threshold + invalidCmd = _deadband / 2.0; + // note that in the deadband world, deadband is enabled by default, + // because the deadband parameter is specified. + } msgs::Double msg; msg.set_data(invalidCmd); pub.Publish(msg); - // Check no movement + // Check no movement because of invalid commands fixture.Server()->Run(true, 100, false); std::this_thread::sleep_for(std::chrono::milliseconds(100)); EXPECT_DOUBLE_EQ(0.0, modelPoses.back().Pos().X()); @@ -285,14 +314,50 @@ void ThrusterTest::TestWorld(const std::string &_world, { EXPECT_NEAR(0.0, angVel.X(), _baseTol); } + else if (_namespace == "deadband") + { + continue; + } else { - EXPECT_NEAR(omega, angVel.X(), omegaTol) << i; + ASSERT_NEAR(omega, angVel.X(), omegaTol) << i; } } EXPECT_NEAR(0.0, angVel.Y(), _baseTol); EXPECT_NEAR(0.0, angVel.Z(), _baseTol); } + + modelPoses.clear(); + propellerAngVels.clear(); + propellerLinVels.clear(); + // Make sure that when the deadband is disabled + // commands below the deadband should create a movement + auto latest_pose = modelPoses.back(); + msgs::Boolean db_msg; + if (_namespace == "deadband") + { + // disable the deadband + db_msg.set_data(false); + db_pub.Publish(db_msg); + // And we send a command that is still below the deadband threshold + msg.set_data(_deadband / 2.0); + pub.Publish(msg); + // Check movements + fixture.Server()->Run(true, 1000, false); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + EXPECT_LT(0.1, modelPoses.back().Pos().X()); + + EXPECT_EQ(1000u, modelPoses.size()); + EXPECT_EQ(1000u, propellerAngVels.size()); + EXPECT_EQ(1000u, propellerLinVels.size()); + modelPoses.clear(); + propellerAngVels.clear(); + propellerLinVels.clear(); + } + // re-enable the deadband + db_msg.set_data(true); + db_pub.Publish(db_msg); + } ///////////////////////////////////////////////// @@ -307,7 +372,7 @@ TEST_F(ThrusterTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(AngVelCmdControl)) // Tolerance is high because the joint command disturbs the vehicle body this->TestWorld(world, ns, topic, 0.005, 950, 0.2, 1e-2, 0.2, 1, 0, false, - true, 100.01); + true, 100.01); } ///////////////////////////////////////////////// @@ -372,7 +437,7 @@ TEST_F(ThrusterTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(VelocityControl)) ///////////////////////////////////////////////// TEST_F(ThrusterTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(BatteryIntegration)) { - const std::string ns = "lowbattery"; +:string ns = "lowbattery"; const std::string topic = ns + "/thrust"; auto world = common::joinPaths(std::string(PROJECT_SOURCE_PATH), "test", "worlds", "thruster_battery.sdf"); @@ -393,3 +458,19 @@ TEST_F(ThrusterTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(ThrustCoefficient)) // Tolerance is high because the joint command disturbs the vehicle body this->TestWorld(world, ns, topic, 1, 950, 0.25, 1e-2, 0.2, 0.9, 0.01, true); } + +///////////////////////////////////////////////// +TEST_F(ThrusterTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(ThrusterDeadBand)) +{ + const std::string ns = "deadband"; + const std::string topic = "/model/" + ns + + "/joint/propeller_joint/cmd_thrust"; + const std::string db_topic = "/model/" + ns + + "/joint/propeller_joint/enable_deadband"; + auto world = common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "test", "worlds", "thruster_deadband.sdf"); + + // Tolerance is high because the joint command disturbs the vehicle body + this->TestWorld(world, ns, topic, 0.005, 950, 0.25, 1e-2, 0.2, 0.9, 0.01, + false, false, 100.1, 50.0, db_topic); +} diff --git a/test/worlds/thruster_deadband.sdf b/test/worlds/thruster_deadband.sdf new file mode 100644 index 0000000000..faf9b3228a --- /dev/null +++ b/test/worlds/thruster_deadband.sdf @@ -0,0 +1,112 @@ + + + + + + + 0 + + + + 0 0 0 + + + + + + + + true + 0 0 10 0 0 0 + 1 1 1 1 + 0.5 0.5 0.5 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + + + 0 0 0 0 1.57 0 + + 100 + + 33.89 + 0 + 0 + 33.89 + 0 + 1.125 + + + + + + 2 + 0.15 + + + + + + + -1.05 0 0 0 0 0 + + 0.1 + + 0.0000354167 + 0 + 0 + 0.0000021667 + 0 + 0.0000334167 + + + + + + 0.01 0.25 0.05 + + + + + + + body + propeller + + 1 0 0 + + -1e+12 + 1e+12 + 1e6 + 1e6 + + + + + + deadband + propeller_joint + 0.005 + 950 + 0.25 + 300 + 0 + 50 + + + + + From 066f10853af3a3d17c74789456634530239dae42 Mon Sep 17 00:00:00 2001 From: Alaa El Jawad Date: Mon, 29 May 2023 10:45:11 +0200 Subject: [PATCH 08/16] subscribe only if the deadband topic is not empty Signed-off-by: Alaa El Jawad --- src/systems/thruster/Thruster.cc | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/src/systems/thruster/Thruster.cc b/src/systems/thruster/Thruster.cc index 3e385a6312..da3acfeda9 100644 --- a/src/systems/thruster/Thruster.cc +++ b/src/systems/thruster/Thruster.cc @@ -403,12 +403,16 @@ void Thruster::Configure( gzmsg << "Thruster listening to commands on [" << thrusterTopic << "]" << std::endl; - this->dataPtr->node.Subscribe( - this->dataPtr->deadbandTopic, - &ThrusterPrivateData::OnDeadbandEnable, - this->dataPtr.get()); - gzmsg << "Thruster listening to enable_deadband on [" - << this->dataPtr->deadbandTopic << "]" << std::endl; + + if (!this->dataPtr->deadbandTopic.empty()) + { + this->dataPtr->node.Subscribe( + this->dataPtr->deadbandTopic, + &ThrusterPrivateData::OnDeadbandEnable, + this->dataPtr.get()); + gzmsg << "Thruster listening to enable_deadband on [" + << this->dataPtr->deadbandTopic << "]" << std::endl; + } this->dataPtr->pub = this->dataPtr->node.Advertise( feedbackTopic); From a5f616c5dcdbfa737563171d44cca4756c7ecec2 Mon Sep 17 00:00:00 2001 From: Alaa El Jawad Date: Mon, 29 May 2023 10:45:40 +0200 Subject: [PATCH 09/16] create the publisher in the unit test only if the topic is not empty Signed-off-by: Alaa El Jawad --- test/integration/thruster.cc | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/test/integration/thruster.cc b/test/integration/thruster.cc index 07b978cb50..fce2b01f4f 100644 --- a/test/integration/thruster.cc +++ b/test/integration/thruster.cc @@ -24,6 +24,7 @@ #include #include #include +#include #include #include "gz/sim/Link.hh" @@ -158,7 +159,12 @@ void ThrusterTest::TestWorld(const std::string &_world, // Publish command and check that vehicle moved transport::Node node; auto pub = node.Advertise(_topic); - auto db_pub = node.Advertise(_db_topic); + transport::Node::Publisher db_pub; + if (!_db_topic.empty()) { + // create publisher only if topic is not empty, otherwise tests will which should be the case + // for the deadband world. + db_pub = node.Advertise(_db_topic); + } int sleep{0}; int maxSleep{30}; From 9a74b71e605b53480ea24a4f2ea721f420c9d68b Mon Sep 17 00:00:00 2001 From: Alaa El Jawad Date: Mon, 29 May 2023 10:46:43 +0200 Subject: [PATCH 10/16] check that the propeller are rotating for more robust tests + fix typo Signed-off-by: Alaa El Jawad --- test/integration/thruster.cc | 33 +++++++++++++++++++++++++-------- 1 file changed, 25 insertions(+), 8 deletions(-) diff --git a/test/integration/thruster.cc b/test/integration/thruster.cc index fce2b01f4f..14bb29646f 100644 --- a/test/integration/thruster.cc +++ b/test/integration/thruster.cc @@ -345,24 +345,41 @@ void ThrusterTest::TestWorld(const std::string &_world, // disable the deadband db_msg.set_data(false); db_pub.Publish(db_msg); - // And we send a command that is still below the deadband threshold + // And we send a command that are below the deadband threshold msg.set_data(_deadband / 2.0); pub.Publish(msg); - // Check movements + // When the deadband is disabled, any command value + // (especially values below the deadband threshold) should move the model fixture.Server()->Run(true, 1000, false); std::this_thread::sleep_for(std::chrono::milliseconds(100)); - EXPECT_LT(0.1, modelPoses.back().Pos().X()); - + + // make sure we have run a 1000 times EXPECT_EQ(1000u, modelPoses.size()); EXPECT_EQ(1000u, propellerAngVels.size()); EXPECT_EQ(1000u, propellerLinVels.size()); + + // the model should have moved. Note that the distance moved is small + // This is because we are sending small forces (deadband/2) + EXPECT_LT(0.1, modelPoses.back().Pos().X()); + + // Check that the propeller are rotating + force = _deadband / 2.0; + omega = sqrt(abs(force / (_density * _thrustCoefficient * + pow(_diameter, 4)))); + // Account for negative thrust and/or negative thrust coefficient + omega *= (force * _thrustCoefficient > 0 ? 1 : -1); + + // for (unsigned int i = 0; i < propellerAngVels.size(); ++i) + // { + // EXPECT_NEAR(omega, propellerAngVels[i].X(), omegaTol) << i; + // } modelPoses.clear(); propellerAngVels.clear(); propellerLinVels.clear(); + // re-enable the deadband + db_msg.set_data(true); + db_pub.Publish(db_msg); } - // re-enable the deadband - db_msg.set_data(true); - db_pub.Publish(db_msg); } @@ -443,7 +460,7 @@ TEST_F(ThrusterTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(VelocityControl)) ///////////////////////////////////////////////// TEST_F(ThrusterTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(BatteryIntegration)) { -:string ns = "lowbattery"; + const std::string ns = "lowbattery"; const std::string topic = ns + "/thrust"; auto world = common::joinPaths(std::string(PROJECT_SOURCE_PATH), "test", "worlds", "thruster_battery.sdf"); From b4af0ed8e733cd7da18d442ea02c49d4a877d779 Mon Sep 17 00:00:00 2001 From: Alaa El Jawad Date: Mon, 29 May 2023 11:44:45 +0200 Subject: [PATCH 11/16] fix formatting issues with codecheck Signed-off-by: Alaa El Jawad --- test/integration/thruster.cc | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/test/integration/thruster.cc b/test/integration/thruster.cc index 14bb29646f..5350eb5edc 100644 --- a/test/integration/thruster.cc +++ b/test/integration/thruster.cc @@ -161,8 +161,8 @@ void ThrusterTest::TestWorld(const std::string &_world, auto pub = node.Advertise(_topic); transport::Node::Publisher db_pub; if (!_db_topic.empty()) { - // create publisher only if topic is not empty, otherwise tests will which should be the case - // for the deadband world. + // create publisher only if topic is not empty, otherwise tests will get + // get complains db_pub = node.Advertise(_db_topic); } @@ -348,11 +348,11 @@ void ThrusterTest::TestWorld(const std::string &_world, // And we send a command that are below the deadband threshold msg.set_data(_deadband / 2.0); pub.Publish(msg); - // When the deadband is disabled, any command value + // When the deadband is disabled, any command value // (especially values below the deadband threshold) should move the model fixture.Server()->Run(true, 1000, false); std::this_thread::sleep_for(std::chrono::milliseconds(100)); - + // make sure we have run a 1000 times EXPECT_EQ(1000u, modelPoses.size()); EXPECT_EQ(1000u, propellerAngVels.size()); @@ -361,7 +361,7 @@ void ThrusterTest::TestWorld(const std::string &_world, // the model should have moved. Note that the distance moved is small // This is because we are sending small forces (deadband/2) EXPECT_LT(0.1, modelPoses.back().Pos().X()); - + // Check that the propeller are rotating force = _deadband / 2.0; omega = sqrt(abs(force / (_density * _thrustCoefficient * From 6f6e740fa78c06ca58d373967d11d1a7d7930e32 Mon Sep 17 00:00:00 2001 From: Alaa El Jawad Date: Mon, 31 Jul 2023 13:55:04 +0200 Subject: [PATCH 12/16] add mutex to protect deadband enable Signed-off-by: Alaa El Jawad --- src/systems/thruster/Thruster.cc | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/systems/thruster/Thruster.cc b/src/systems/thruster/Thruster.cc index da3acfeda9..8cb1d32784 100644 --- a/src/systems/thruster/Thruster.cc +++ b/src/systems/thruster/Thruster.cc @@ -152,6 +152,9 @@ class gz::sim::systems::ThrusterPrivateData /// \brief Flag to enable/disable deadband public: bool enableDeadband = false; + /// \brief Mutex to protect enableDeadband + public: std::mutex deadbandMutex; + /// \brief Topic name used to enable/disable the deadband public: std::string deadbandTopic = ""; @@ -525,6 +528,7 @@ void ThrusterPrivateData::OnCmdThrust(const gz::msgs::Double &_msg) ///////////////////////////////////////////////// void ThrusterPrivateData::OnDeadbandEnable(const gz::msgs::Boolean &_msg) { + std::lock_guard lock(this->deadbandMutex); if (_msg.data() != this->enableDeadband) { if (_msg.data()) From 4289a27ad7115de668e782c1eaeb261a3ca7010b Mon Sep 17 00:00:00 2001 From: Alaa El Jawad Date: Mon, 31 Jul 2023 13:56:58 +0200 Subject: [PATCH 13/16] remove unnecessary sleep Signed-off-by: Alaa El Jawad --- test/integration/thruster.cc | 3 --- 1 file changed, 3 deletions(-) diff --git a/test/integration/thruster.cc b/test/integration/thruster.cc index 5350eb5edc..5dc77c95be 100644 --- a/test/integration/thruster.cc +++ b/test/integration/thruster.cc @@ -211,7 +211,6 @@ void ThrusterTest::TestWorld(const std::string &_world, // Check no movement because of invalid commands fixture.Server()->Run(true, 100, false); - std::this_thread::sleep_for(std::chrono::milliseconds(100)); EXPECT_DOUBLE_EQ(0.0, modelPoses.back().Pos().X()); EXPECT_EQ(100u, modelPoses.size()); EXPECT_EQ(100u, propellerAngVels.size()); @@ -248,7 +247,6 @@ void ThrusterTest::TestWorld(const std::string &_world, for (sleep = 0; modelPoses.back().Pos().X() < 5.0 && sleep < maxSleep; ++sleep) { - std::this_thread::sleep_for(std::chrono::milliseconds(100)); fixture.Server()->Run(true, 100, false); } EXPECT_LT(sleep, maxSleep); @@ -351,7 +349,6 @@ void ThrusterTest::TestWorld(const std::string &_world, // When the deadband is disabled, any command value // (especially values below the deadband threshold) should move the model fixture.Server()->Run(true, 1000, false); - std::this_thread::sleep_for(std::chrono::milliseconds(100)); // make sure we have run a 1000 times EXPECT_EQ(1000u, modelPoses.size()); From 78735bf0fc08885b1f8a9a6fd4be03e0c339b9b0 Mon Sep 17 00:00:00 2001 From: Alaa El Jawad Date: Wed, 2 Aug 2023 15:59:59 +0200 Subject: [PATCH 14/16] protect deadband enable with mutex Signed-off-by: Alaa El Jawad --- src/systems/thruster/Thruster.cc | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/systems/thruster/Thruster.cc b/src/systems/thruster/Thruster.cc index 8cb1d32784..7fca9411cc 100644 --- a/src/systems/thruster/Thruster.cc +++ b/src/systems/thruster/Thruster.cc @@ -714,9 +714,12 @@ void Thruster::PreUpdate( desiredPropellerAngVel = this->dataPtr->propellerAngVel; } - if (this->dataPtr->enableDeadband) { - this->dataPtr->ApplyDeadband(desiredThrust, desiredPropellerAngVel); + std::lock_guard lock(this->dataPtr->deadbandMutex); + if (this->dataPtr->enableDeadband) + { + this->dataPtr->ApplyDeadband(desiredThrust, desiredPropellerAngVel); + } } From a52cf355cb25184f8131866d797a1cae1d98215a Mon Sep 17 00:00:00 2001 From: Alaa El Jawad Date: Wed, 2 Aug 2023 16:02:24 +0200 Subject: [PATCH 15/16] adjust pid gains to allow the desired cmd to reach 0 Signed-off-by: Alaa El Jawad --- test/integration/thruster.cc | 15 ++++++++------- test/worlds/thruster_deadband.sdf | 3 ++- 2 files changed, 10 insertions(+), 8 deletions(-) diff --git a/test/integration/thruster.cc b/test/integration/thruster.cc index 5dc77c95be..38027880fc 100644 --- a/test/integration/thruster.cc +++ b/test/integration/thruster.cc @@ -340,11 +340,12 @@ void ThrusterTest::TestWorld(const std::string &_world, msgs::Boolean db_msg; if (_namespace == "deadband") { + force = _deadband / 2.0; // disable the deadband db_msg.set_data(false); db_pub.Publish(db_msg); - // And we send a command that are below the deadband threshold - msg.set_data(_deadband / 2.0); + // And we send a command that is below the deadband threshold + msg.set_data(force); pub.Publish(msg); // When the deadband is disabled, any command value // (especially values below the deadband threshold) should move the model @@ -360,16 +361,16 @@ void ThrusterTest::TestWorld(const std::string &_world, EXPECT_LT(0.1, modelPoses.back().Pos().X()); // Check that the propeller are rotating - force = _deadband / 2.0; omega = sqrt(abs(force / (_density * _thrustCoefficient * pow(_diameter, 4)))); // Account for negative thrust and/or negative thrust coefficient omega *= (force * _thrustCoefficient > 0 ? 1 : -1); - // for (unsigned int i = 0; i < propellerAngVels.size(); ++i) - // { - // EXPECT_NEAR(omega, propellerAngVels[i].X(), omegaTol) << i; - // } + // it takes a few iteration to reach the speed + for (unsigned int i = 25; i < propellerAngVels.size(); ++i) + { + EXPECT_NEAR(omega, propellerAngVels[i].X(), omegaTol) << i; + } modelPoses.clear(); propellerAngVels.clear(); propellerLinVels.clear(); diff --git a/test/worlds/thruster_deadband.sdf b/test/worlds/thruster_deadband.sdf index faf9b3228a..b26facbba4 100644 --- a/test/worlds/thruster_deadband.sdf +++ b/test/worlds/thruster_deadband.sdf @@ -103,8 +103,9 @@ 950 0.25 300 - 0 + -300 50 + 0.05 From 824f8e50fb17b71a05af818453059405d3fb5bf4 Mon Sep 17 00:00:00 2001 From: Alaa El Jawad Date: Wed, 2 Aug 2023 16:02:43 +0200 Subject: [PATCH 16/16] rollback the removal of sleep Signed-off-by: Alaa El Jawad --- test/integration/thruster.cc | 1 + 1 file changed, 1 insertion(+) diff --git a/test/integration/thruster.cc b/test/integration/thruster.cc index 38027880fc..3a365ea69f 100644 --- a/test/integration/thruster.cc +++ b/test/integration/thruster.cc @@ -247,6 +247,7 @@ void ThrusterTest::TestWorld(const std::string &_world, for (sleep = 0; modelPoses.back().Pos().X() < 5.0 && sleep < maxSleep; ++sleep) { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); fixture.Server()->Run(true, 100, false); } EXPECT_LT(sleep, maxSleep);