Battery updates (#183)

* Recharge services and a few battery improvements.

Signed-off-by: Carlos Aguero <[email protected]>

* Use atomic_bool.

Signed-off-by: Carlos Aguero <[email protected]>

Co-authored-by: Carlos Aguero <[email protected]>

Author: caguero

examples/worlds/linear_battery_demo.sdf

@@ -523,7 +523,7 @@
         <resistance>0.07</resistance>
         <smooth_current_tau>2.0</smooth_current_tau>
         <enable_recharge>true</enable_recharge>
-        <!-- charging I = c / t, discharging I = P / V, 
+        <!-- charging I = c / t, discharging I = P / V,
           charging I should be > discharging I -->
         <charging_time>3.0</charging_time>
         <soc_threshold>0.51</soc_threshold>

src/systems/battery_plugin/LinearBatteryPlugin.cc

@@ -16,30 +16,32 @@
  */
 
 #include <ignition/msgs/battery_state.pb.h>
+#include <ignition/msgs/boolean.pb.h>
 
-#include <string>
+#include <atomic>
 #include <functional>
+#include <string>
 
-#include <ignition/plugin/Register.hh>
-#include <ignition/transport/Node.hh>
-
-#include <ignition/common/Util.hh>
 #include <ignition/common/Battery.hh>
 #include <ignition/common/Profiler.hh>
+#include <ignition/common/Util.hh>
+
+#include <ignition/plugin/Register.hh>
+#include <ignition/transport/Node.hh>
 
 #include <sdf/Element.hh>
 #include <sdf/Physics.hh>
 #include <sdf/Root.hh>
 #include <sdf/World.hh>
 
-#include "ignition/gazebo/Model.hh"
 #include "ignition/gazebo/components/BatterySoC.hh"
-#include "ignition/gazebo/components/Name.hh"
-#include "ignition/gazebo/components/World.hh"
+#include "ignition/gazebo/components/Joint.hh"
 #include "ignition/gazebo/components/JointForceCmd.hh"
 #include "ignition/gazebo/components/JointVelocityCmd.hh"
+#include "ignition/gazebo/components/Name.hh"
 #include "ignition/gazebo/components/ParentEntity.hh"
-#include "ignition/gazebo/components/Joint.hh"
+#include "ignition/gazebo/components/World.hh"
+#include "ignition/gazebo/Model.hh"
 
 #include "LinearBatteryPlugin.hh"
 
@@ -56,6 +58,14 @@ class ignition::gazebo::systems::LinearBatteryPluginPrivate
   /// \return State of charge of the battery in range [0.0, 1.0].
   public: double StateOfCharge() const;
 
+  /// \brief Callback executed to start recharging.
+  /// \param[in] _req This value should be true.
+  public: void OnEnableRecharge(const ignition::msgs::Boolean &_req);
+
+  /// \brief Callback executed to stop recharging.
+  /// \param[in] _req This value should be true.
+  public: void OnDisableRecharge(const ignition::msgs::Boolean &_req);
+
   /// \brief Name of model, only used for printing warning when battery drains.
   public: std::string modelName;
 
@@ -102,14 +112,11 @@ class ignition::gazebo::systems::LinearBatteryPluginPrivate
   public: double soc{1.0};
 
   /// \brief Recharge status
-  public: bool startCharging{false};
+  public: std::atomic_bool startCharging{false};
 
   /// \brief Hours taken to fully charge battery
   public: double tCharge{0.0};
 
-  /// \brief SoC threshold for enabling recharge
-  public: double socThreshold{0.0};
-
   /// \brief Battery current for a historic time window
   public: std::deque<double> iList;
 
@@ -204,26 +211,6 @@ void LinearBatteryPlugin::Configure(const Entity &_entity,
   if (_sdf->HasElement("smooth_current_tau"))
     this->dataPtr->tau = _sdf->Get<double>("smooth_current_tau");
 
-  if (_sdf->HasElement("enable_recharge"))
-  {
-    auto isCharging = _sdf->Get<bool>("enable_recharge");
-    if (isCharging)
-    {
-      if (_sdf->HasElement("charging_time") &&
-        _sdf->HasElement("soc_threshold"))
-        {
-          this->dataPtr->tCharge = _sdf->Get<double>("charging_time");
-          this->dataPtr->socThreshold = _sdf->Get<double>("soc_threshold");
-        }
-      else
-      {
-        ignerr << "No <charging_time> or <soc_threshold> specified."
-                  "Both are required to enable recharge.\n";
-        return;
-      }
-    }
-  }
-
   if (_sdf->HasElement("battery_name") && _sdf->HasElement("voltage"))
   {
     auto batteryName = _sdf->Get<std::string>("battery_name");
@@ -233,7 +220,7 @@ void LinearBatteryPlugin::Configure(const Entity &_entity,
     this->dataPtr->batteryEntity = _ecm.CreateEntity();
     // Initialize with initial voltage
     _ecm.CreateComponent(this->dataPtr->batteryEntity,
-        components::BatterySoC(this->dataPtr->soc));
+      components::BatterySoC(this->dataPtr->soc));
     _ecm.CreateComponent(this->dataPtr->batteryEntity, components::Name(
       batteryName));
     _ecm.SetParentEntity(this->dataPtr->batteryEntity, _entity);
@@ -252,6 +239,31 @@ void LinearBatteryPlugin::Configure(const Entity &_entity,
     return;
   }
 
+  if (_sdf->HasElement("enable_recharge"))
+  {
+    auto isCharging = _sdf->Get<bool>("enable_recharge");
+    if (isCharging)
+    {
+      if (_sdf->HasElement("charging_time"))
+        this->dataPtr->tCharge = _sdf->Get<double>("charging_time");
+      else
+      {
+        ignerr << "No <charging_time> specified. "
+                  "Parameter required to enable recharge.\n";
+        return;
+      }
+      std::string enableRechargeService = this->dataPtr->modelName +
+        "/" + _sdf->Get<std::string>("battery_name") + "/recharge/start";
+      std::string disableRechargeService = this->dataPtr->modelName +
+        "/" + _sdf->Get<std::string>("battery_name") + "/recharge/stop";
+
+      this->dataPtr->node.Advertise(enableRechargeService,
+        &LinearBatteryPluginPrivate::OnEnableRecharge, this->dataPtr.get());
+      this->dataPtr->node.Advertise(disableRechargeService,
+        &LinearBatteryPluginPrivate::OnDisableRecharge, this->dataPtr.get());
+    }
+  }
+
   // Consumer-specific
   if (_sdf->HasElement("power_load"))
   {
@@ -307,12 +319,29 @@ double LinearBatteryPluginPrivate::StateOfCharge() const
   return this->soc;
 }
 
+//////////////////////////////////////////////////
+void LinearBatteryPluginPrivate::OnEnableRecharge(
+  const ignition::msgs::Boolean &/*_req*/)
+{
+  igndbg << "Request for start charging received" << std::endl;
+  this->startCharging = true;
+}
+
+//////////////////////////////////////////////////
+void LinearBatteryPluginPrivate::OnDisableRecharge(
+  const ignition::msgs::Boolean &/*_req*/)
+{
+  igndbg << "Request for stop charging received" << std::endl;
+  this->startCharging = false;
+}
+
 //////////////////////////////////////////////////
 void LinearBatteryPlugin::PreUpdate(
   const ignition::gazebo::UpdateInfo &/*_info*/,
   ignition::gazebo::EntityComponentManager &_ecm)
 {
   IGN_PROFILE("LinearBatteryPlugin::PreUpdate");
+  this->dataPtr->startDraining = false;
   // Start draining the battery if the robot has started moving
   if (!this->dataPtr->startDraining)
   {
@@ -370,7 +399,7 @@ void LinearBatteryPlugin::Update(const UpdateInfo &_info,
   if (_info.paused)
     return;
 
-  if (!this->dataPtr->startDraining)
+  if (!this->dataPtr->startDraining && !this->dataPtr->startCharging)
     return;
 
   // Find the time at which battery starts to drain
@@ -421,10 +450,14 @@ void LinearBatteryPlugin::PostUpdate(const UpdateInfo &_info,
   msg.set_charge(this->dataPtr->q);
   msg.set_capacity(this->dataPtr->c);
   msg.set_percentage(this->dataPtr->soc);
-  if (this->dataPtr->startDraining)
+  if (this->dataPtr->startCharging)
+    msg.set_power_supply_status(msgs::BatteryState::CHARGING);
+  else if (this->dataPtr->startDraining)
     msg.set_power_supply_status(msgs::BatteryState::DISCHARGING);
-  else
+  else if (this->dataPtr->StateOfCharge() > 0.9)
     msg.set_power_supply_status(msgs::BatteryState::FULL);
+  else
+    msg.set_power_supply_status(msgs::BatteryState::NOT_CHARGING);
   this->dataPtr->statePub.Publish(msg);
 }
 
@@ -455,14 +488,8 @@ double LinearBatteryPlugin::OnUpdateVoltage(
   // compute charging current
   auto iCharge = this->dataPtr->c / this->dataPtr->tCharge;
 
-  // charging criteria
-  if (this->dataPtr->StateOfCharge() < this->dataPtr->socThreshold)
-    this->dataPtr->startCharging = true;
-  if (this->dataPtr->StateOfCharge() >= 0.9)
-    this->dataPtr->startCharging = false;
-
   // add charging current to battery
-  if (this->dataPtr->startCharging)
+  if (this->dataPtr->startCharging && this->dataPtr->StateOfCharge() < 0.9)
     this->dataPtr->iraw -= iCharge;
 
   this->dataPtr->ismooth = this->dataPtr->ismooth + k *
@@ -500,7 +527,7 @@ double LinearBatteryPlugin::OnUpdateVoltage(
     igndbg << "PowerLoads().size(): " << _battery->PowerLoads().size()
            << std::endl;
     igndbg << "charging status: " << std::boolalpha
-      << this->dataPtr->startCharging << std::endl;
+           << this->dataPtr->startCharging << std::endl;
     igndbg << "charging current: " << iCharge << std::endl;
     igndbg << "voltage: " << voltage << std::endl;
     igndbg << "state of charge: " << this->dataPtr->StateOfCharge()

src/systems/battery_plugin/LinearBatteryPlugin.hh

@@ -40,18 +40,21 @@ namespace systems
   /// \brief A plugin for simulating battery usage
   ///
   /// This system processes the following sdf parameters:
-  /// <battery_name> name of the battery
-  /// <voltage> Initial voltage of the battery
+  /// <battery_name> name of the battery (required)
+  /// <voltage> Initial voltage of the battery (required)
   /// <open_circuit_voltage_constant_coef> Voltage at full charge
   /// <open_circuit_voltage_linear_coef> Amount of voltage decrease when no
   ///                                    charge
   /// <initial_charge> Initial charge of the battery
   /// <capacity> Total charge that the battery can hold
   /// <resistance> Internal resistance
   /// <smooth_current_tau> coefficient for smoothing current
-  /// <power_load> power load on battery
+  /// <power_load> power load on battery (required)
   /// <start_on_motion> if set to true, the battery will start draining
-  ///                  only if the robot has started moving
+  ///                   only if the robot has started moving
+  /// <enable_recharge> If true, the battery can be recharged
+  /// <charging_time> Hours taken to fully charge the battery.
+  ///                 (Required if <enable_recharge> is set to true)
   class IGNITION_GAZEBO_VISIBLE LinearBatteryPlugin
       : public System,
         public ISystemConfigure,