Merge 824f8e5 into 44fbf79

src/systems/thruster/Thruster.cc

@@ -14,6 +14,8 @@
  * limitations under the License.
  *
  */
+#include <cstdlib>
+#include <gz/transport/TopicUtils.hh>
 #include <memory>
 #include <mutex>
 #include <limits>
@@ -144,6 +146,18 @@ class gz::sim::systems::ThrusterPrivateData
   /// \brief Linear velocity of the vehicle.
   public: double linearVelocity = 0.0;
 
+  /// \brief deadband in newtons
+  public: double deadband = 0.0;
+
+  /// \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 = "";
+
   /// \brief Topic name used to control thrust. Optional
   public: std::string topic = "";
 
@@ -159,6 +173,11 @@ 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
+  /// \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.
   public: void UpdateThrustCoefficient();
 
@@ -179,6 +198,12 @@ 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 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);
 };
 
 /////////////////////////////////////////////////
@@ -277,6 +302,13 @@ void Thruster::Configure(
     }
   }
 
+  // Get deadband, default to 0
+  if (_sdf->HasElement("deadband"))
+  {
+    this->dataPtr->deadband = _sdf->Get<double>("deadband");
+    this->dataPtr->enableDeadband = true;
+  }
+
   // Get a custom topic.
   if (_sdf->HasElement("topic"))
   {
@@ -312,6 +344,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 +381,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,11 +399,24 @@ 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;
 
+  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<msgs::Double>(
       feedbackTopic);
 
@@ -475,6 +525,26 @@ void ThrusterPrivateData::OnCmdThrust(const gz::msgs::Double &_msg)
   this->propellerAngVel = this->ThrustToAngularVec(this->thrust);
 }
 
+/////////////////////////////////////////////////
+void ThrusterPrivateData::OnDeadbandEnable(const gz::msgs::Boolean &_msg)
+{
+  std::lock_guard<std::mutex> lock(this->deadbandMutex);
+  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 +620,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 +642,9 @@ void Thruster::PreUpdate(
   {
     return;
   }
+  if (!_ecm.HasEntity(this->dataPtr->linkEntity)){
+    return;
+  }
 
   // Init battery consumption if it was set
   if (!this->dataPtr->batteryName.empty() &&
@@ -631,6 +714,15 @@ void Thruster::PreUpdate(
     desiredPropellerAngVel = this->dataPtr->propellerAngVel;
   }
 
+  {
+    std::lock_guard<std::mutex> lock(this->dataPtr->deadbandMutex);
+    if (this->dataPtr->enableDeadband)
+    {
+      this->dataPtr->ApplyDeadband(desiredThrust, desiredPropellerAngVel);
+    }
+  }
+
+
   msgs::Double angvel;
   // PID control
   double torque = 0.0;

src/systems/thruster/Thruster.hh

@@ -85,6 +85,11 @@ namespace systems
   ///                      [Optional, defaults to 1000N or 1000rad/s]
   /// - <min_thrust_cmd> - Minimum input thrust or angular velocity command.
   ///                      [Optional, defaults to -1000N or -1000rad/s]
+  /// - <deadband> - 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
+  ///                `{ns}/{topic}/enable_deadband` depending on other params
   /// - <wake_fraction>  - Relative speed reduction between the water
   ///                      at the propeller (Va) vs behind the vessel.
   ///                      [Optional, defults to 0.2]

test/integration/thruster.cc

@@ -17,12 +17,14 @@
 
 #include <gtest/gtest.h>
 
+#include <gz/msgs/boolean.pb.h>
 #include <gz/msgs/double.pb.h>
 
 #include <gz/common/Console.hh>
 #include <gz/common/Util.hh>
 #include <gz/math/Helpers.hh>
 #include <gz/transport/Node.hh>
+#include <gz/transport/Publisher.hh>
 #include <gz/utils/ExtraTestMacros.hh>
 
 #include "gz/sim/Link.hh"
@@ -55,20 +57,25 @@ 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());
 };
 
 //////////////////////////////////////////////////
 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 +159,35 @@ void ThrusterTest::TestWorld(const std::string &_world,
   // Publish command and check that vehicle moved
   transport::Node node;
   auto pub = node.Advertise<msgs::Double>(_topic);
+  transport::Node::Publisher db_pub;
+  if (!_db_topic.empty()) {
+    // create publisher only if topic is not empty, otherwise tests will get
+    // get complains
+    db_pub = node.Advertise<msgs::Boolean>(_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,13 +197,20 @@ 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());
   EXPECT_EQ(100u, modelPoses.size());
   EXPECT_EQ(100u, propellerAngVels.size());
@@ -285,14 +319,67 @@ 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")
+  {
+    force = _deadband / 2.0;
+    // disable the deadband
+    db_msg.set_data(false);
+    db_pub.Publish(db_msg);
+    // 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
+    fixture.Server()->Run(true, 1000, false);
+
+    // 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
+    omega = sqrt(abs(force / (_density * _thrustCoefficient *
+        pow(_diameter, 4))));
+    // Account for negative thrust and/or negative thrust coefficient
+    omega *= (force * _thrustCoefficient > 0 ? 1 : -1);
+
+    // 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();
+    // re-enable the deadband
+    db_msg.set_data(true);
+    db_pub.Publish(db_msg);
+  }
+
 }
 
 /////////////////////////////////////////////////
@@ -307,7 +394,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);
 }
 
 /////////////////////////////////////////////////
@@ -393,3 +480,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);
+}