LinearBatteryPlugin stability improvement (#2725)

Signed-off-by: Maksim Derbasov <[email protected]>

Author: ntfshard

src/systems/battery_plugin/LinearBatteryPlugin.cc

@@ -97,13 +97,6 @@ class gz::sim::systems::LinearBatteryPluginPrivate
   /// \brief Pointer to battery contained in link.
   public: common::BatteryPtr battery;
 
-  /// \brief Whether warning that battery has drained has been printed once.
-  public: bool drainPrinted{false};
-
-  /// \brief Battery consumer identifier.
-  /// Current implementation limits one consumer (Model) per battery.
-  public: int32_t consumerId;
-
   /// \brief Battery entity
   public: Entity batteryEntity{kNullEntity};
 
@@ -136,15 +129,11 @@ class gz::sim::systems::LinearBatteryPluginPrivate
   /// \brief State of charge [0, 1].
   public: double soc{1.0};
 
-  /// \brief Recharge status
-  public: std::atomic_bool startCharging{false};
-
   /// \brief Hours taken to fully charge battery
   public: double tCharge{0.0};
 
-  /// \TODO(caguero) Remove this flag in Gazebo Dome.
-  /// \brief Flag to enable some battery fixes.
-  public: bool fixIssue225{false};
+  /// \brief Initial power load set trough config
+  public: double initialPowerLoad{0.0};
 
   /// \TODO(caguero) Remove in Gazebo Dome.
   /// \brief Battery current for a historic time window
@@ -157,16 +146,6 @@ class gz::sim::systems::LinearBatteryPluginPrivate
   /// \brief Simulation time handled during a single update.
   public: std::chrono::steady_clock::duration stepSize;
 
-  /// \brief Flag on whether the battery should start draining
-  public: bool startDraining = false;
-
-  /// \brief The start time when battery starts draining in seconds
-  public: int drainStartTime = -1;
-
-  /// \brief Book keep the last time printed, so as to not pollute dbg messages
-  /// in minutes
-  public: int lastPrintTime = -1;
-
   /// \brief Model interface
   public: Model model{kNullEntity};
 
@@ -176,11 +155,32 @@ class gz::sim::systems::LinearBatteryPluginPrivate
   /// \brief Battery state of charge message publisher
   public: transport::Node::Publisher statePub;
 
-  /// \brief Initial power load set trough config
-  public: double initialPowerLoad = 0.0;
+  /// \brief Battery consumer identifier.
+  /// Current implementation limits one consumer (Model) per battery.
+  public: int32_t consumerId;
+
+  /// \brief The start time when battery starts draining in seconds
+  public: int drainStartTime{-1};
+
+  /// \brief Book keep the last time printed, so as to not pollute dbg messages
+  /// in minutes
+  public: int lastPrintTime{-1};
+
+  /// \brief Recharge status
+  public: std::atomic_bool startCharging{false};
+
+  /// \brief Flag on whether the battery should start draining
+  public: std::atomic_bool startDraining{false};
+
+  /// \brief Whether warning that battery has drained has been printed once.
+  public: bool drainPrinted{false};
 
   /// \brief Flag to invert the current sign
   public: bool invertCurrentSign{false};
+
+  /// \TODO(caguero) Remove this flag in Gazebo Dome.
+  /// \brief Flag to enable some battery fixes.
+  public: bool fixIssue225{false};
 };
 
 /////////////////////////////////////////////////

test/integration/battery_plugin.cc

@@ -400,23 +400,26 @@ TEST_F(BatteryPluginTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(PowerDrainTopic))
   // Send a message on one of the <power_draining_topic> topics, which will
   // start the battery draining when the server starts again.
   gz::transport::Node node;
-  auto pub = node.Advertise<msgs::StringMsg>("/battery/discharge");
+  auto dischargePub = node.Advertise<msgs::StringMsg>("/battery/discharge");
   msgs::StringMsg msg;
-  pub.Publish(msg);
+  dischargePub.Publish(msg);
 
   // Run the server again.
   server.Run(true, 100, false);
 
+  // Send a message on one of the <stop_power_draining_topic> topics, which
+  // will stop the battery draining when the server starts again.
+  auto stopPub = node.Advertise<msgs::StringMsg>("/battery/stop_discharge");
+  stopPub.Publish(msg);
+
+  // Run the server a little bit to allow msg be propagated inside plugin
+  server.Run(true, 50, false);
+
   // The state of charge should be <1, since the battery has started
   // draining.
-  double stateOfCharge = batComp->Data();
+  const double stateOfCharge = batComp->Data();
   EXPECT_LT(batComp->Data(), 1.0);
 
-  // Send a message on one of the <stop_power_draining_topic> topics, which
-  // will stop the battery draining when the server starts again.
-  auto pub2 = node.Advertise<msgs::StringMsg>("/battery/stop_discharge");
-  pub2.Publish(msg);
-
   // Run the server again.
   server.Run(true, 100, false);