Adding battery consumers and extra fixes (#1811)

Signed-off-by: Marco A. Gutierrez <marco@openrobotics.org>

Author: Marco A. Gutierrez

include/ignition/gazebo/components/BatteryPowerLoad.hh

@@ -0,0 +1,52 @@
+/*
+ * Copyright (C) 2022 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+#ifndef IGNITION_GAZEBO_COMPONENTS_BATTERYPOWERLOAD_HH_
+#define IGNITION_GAZEBO_COMPONENTS_BATTERYCONSUMPTION_HH_
+
+#include <ignition/gazebo/components/Factory.hh>
+#include <ignition/gazebo/components/Component.hh>
+#include <ignition/gazebo/config.hh>
+
+namespace ignition
+{
+namespace gazebo
+{
+// Inline bracket to help doxygen filtering.
+inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
+namespace components
+{
+  /// \brief Data structure to hold the consumer power load
+  /// and the name of the battery it uses.
+  struct BatteryPowerLoadInfo
+  {
+    /// \brief Entity of the battery to use.
+    Entity batteryId;
+    /// \brief Battery power load (W) to add to the battery.
+    double batteryPowerLoad;
+  };
+
+  /// \brief A component that indicates the total consumption of a battery.
+  using BatteryPowerLoad =
+    Component<BatteryPowerLoadInfo, class BatteryPowerLoadTag>;
+  IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.BatteryPowerLoad",
+                                BatteryPowerLoad)
+}
+}
+}
+}
+
+#endif

src/systems/battery_plugin/LinearBatteryPlugin.cc

@@ -39,6 +39,7 @@
 #include <sdf/Root.hh>
 #include <sdf/World.hh>
 
+#include "ignition/gazebo/components/BatteryPowerLoad.hh"
 #include "ignition/gazebo/components/BatterySoC.hh"
 #include "ignition/gazebo/components/Joint.hh"
 #include "ignition/gazebo/components/JointForceCmd.hh"
@@ -78,9 +79,21 @@ class ignition::gazebo::systems::LinearBatteryPluginPrivate
     const char *_data, const size_t _size,
     const ignition::transport::MessageInfo &_info);
 
+  /// \brief Callback connected to additional topics that can stop battery
+  /// draining.
+  /// \param[in] _data Message data.
+  /// \param[in] _size Message data size.
+  /// \param[in] _info Information about the message.
+  public: void OnBatteryStopDrainingMsg(
+    const char *_data, const size_t _size,
+    const ignition::transport::MessageInfo &_info);
+
   /// \brief Name of model, only used for printing warning when battery drains.
   public: std::string modelName;
 
+  /// \brief Name that identifies a battery.
+  public: std::string batteryName;
+
   /// \brief Pointer to battery contained in link.
   public: common::BatteryPtr battery;
 
@@ -145,7 +158,7 @@ class ignition::gazebo::systems::LinearBatteryPluginPrivate
   public: std::chrono::steady_clock::duration stepSize;
 
   /// \brief Flag on whether the battery should start draining
-  public: bool startDraining = true;
+  public: bool startDraining = false;
 
   /// \brief The start time when battery starts draining in seconds
   public: int drainStartTime = -1;
@@ -163,8 +176,8 @@ class ignition::gazebo::systems::LinearBatteryPluginPrivate
   /// \brief Battery state of charge message publisher
   public: transport::Node::Publisher statePub;
 
-  /// \brief Whether a topic has received any battery-draining command.
-  public: bool startDrainingFromTopics = false;
+  /// \brief Initial power load set trough config
+  public: double initialPowerLoad = 0.0;
 };
 
 /////////////////////////////////////////////////
@@ -262,21 +275,18 @@ void LinearBatteryPlugin::Configure(const Entity &_entity,
 
   if (_sdf->HasElement("battery_name") && _sdf->HasElement("voltage"))
   {
-    auto batteryName = _sdf->Get<std::string>("battery_name");
+    this->dataPtr->batteryName = _sdf->Get<std::string>("battery_name");
     auto initVoltage = _sdf->Get<double>("voltage");
 
-    // Create battery entity and component
+    // Create battery entity and some components
     this->dataPtr->batteryEntity = _ecm.CreateEntity();
-    // Initialize with initial voltage
-    _ecm.CreateComponent(this->dataPtr->batteryEntity,
-      components::BatterySoC(this->dataPtr->soc));
     _ecm.CreateComponent(this->dataPtr->batteryEntity, components::Name(
-      batteryName));
+      this->dataPtr->batteryName));
     _ecm.SetParentEntity(this->dataPtr->batteryEntity, _entity);
 
     // Create actual battery and assign update function
-    this->dataPtr->battery = std::make_shared<common::Battery>(batteryName,
-      initVoltage);
+    this->dataPtr->battery = std::make_shared<common::Battery>(
+      this->dataPtr->batteryName, initVoltage);
     this->dataPtr->battery->Init();
     this->dataPtr->battery->SetUpdateFunc(
       std::bind(&LinearBatteryPlugin::OnUpdateVoltage, this,
@@ -339,10 +349,10 @@ void LinearBatteryPlugin::Configure(const Entity &_entity,
   // Consumer-specific
   if (_sdf->HasElement("power_load"))
   {
-    auto powerLoad = _sdf->Get<double>("power_load");
+    this->dataPtr->initialPowerLoad = _sdf->Get<double>("power_load");
     this->dataPtr->consumerId = this->dataPtr->battery->AddConsumer();
     bool success = this->dataPtr->battery->SetPowerLoad(
-      this->dataPtr->consumerId, powerLoad);
+      this->dataPtr->consumerId, this->dataPtr->initialPowerLoad);
     if (!success)
       ignerr << "Failed to set consumer power load." << std::endl;
   }
@@ -352,6 +362,9 @@ void LinearBatteryPlugin::Configure(const Entity &_entity,
             << "in LinearBatteryPlugin SDF" << std::endl;
   }
 
+  if (_sdf->HasElement("start_draining"))
+    this->dataPtr->startDraining = _sdf->Get<bool>("start_draining");
+
   // Subscribe to power draining topics, if any.
   if (_sdf->HasElement("power_draining_topic"))
   {
@@ -369,12 +382,33 @@ void LinearBatteryPlugin::Configure(const Entity &_entity,
     }
   }
 
+  // Subscribe to stop power draining topics, if any.
+  if (_sdf->HasElement("stop_power_draining_topic"))
+  {
+    sdf::ElementConstPtr sdfElem =
+        _sdf->FindElement("stop_power_draining_topic");
+    while (sdfElem)
+    {
+      const auto &topic = sdfElem->Get<std::string>();
+      this->dataPtr->node.SubscribeRaw(topic,
+          std::bind(&LinearBatteryPluginPrivate::OnBatteryStopDrainingMsg,
+          this->dataPtr.get(), std::placeholders::_1, std::placeholders::_2,
+          std::placeholders::_3));
+      ignmsg << "LinearBatteryPlugin subscribes to stop power draining topic ["
+             << topic << "]." << std::endl;
+      sdfElem = sdfElem->GetNextElement("power_draining_topic");
+    }
+  }
+
   ignmsg << "LinearBatteryPlugin configured. Battery name: "
          << this->dataPtr->battery->Name() << std::endl;
   igndbg << "Battery initial voltage: " << this->dataPtr->battery->InitVoltage()
          << std::endl;
 
   this->dataPtr->soc = this->dataPtr->q / this->dataPtr->c;
+  // Initialize battery with initial calculated state of charge
+  _ecm.CreateComponent(this->dataPtr->batteryEntity,
+      components::BatterySoC(this->dataPtr->soc));
 
   // Setup battery state topic
   std::string stateTopic{"/model/" + this->dataPtr->model.Name(_ecm) +
@@ -400,7 +434,7 @@ void LinearBatteryPluginPrivate::Reset()
   this->iraw = 0.0;
   this->ismooth = 0.0;
   this->q = this->q0;
-  this->startDrainingFromTopics = false;
+  this->startDraining = false;
 }
 
 /////////////////////////////////////////////////
@@ -429,7 +463,14 @@ void LinearBatteryPluginPrivate::OnDisableRecharge(
 void LinearBatteryPluginPrivate::OnBatteryDrainingMsg(
   const char *, const size_t, const ignition::transport::MessageInfo &)
 {
-  this->startDrainingFromTopics = true;
+  this->startDraining = true;
+}
+
+//////////////////////////////////////////////////
+void LinearBatteryPluginPrivate::OnBatteryStopDrainingMsg(
+  const char *, const size_t, const ignition::transport::MessageInfo &)
+{
+  this->startDraining = false;
 }
 
 //////////////////////////////////////////////////
@@ -439,10 +480,26 @@ void LinearBatteryPlugin::PreUpdate(
 {
   IGN_PROFILE("LinearBatteryPlugin::PreUpdate");
 
-  // \todo(anyone) Add in the ability to stop the battery from draining
-  // after it has been started by a topic. See this comment:
-  // https://github.com/ignitionrobotics/ign-gazebo/pull/1255#discussion_r770223092
-  this->dataPtr->startDraining = this->dataPtr->startDrainingFromTopics;
+  // Recalculate the total power load among consumers
+  double total_power_load = this->dataPtr->initialPowerLoad;
+  _ecm.Each<components::BatteryPowerLoad>(
+    [&](const Entity & /*_entity*/,
+        const components::BatteryPowerLoad *_batteryPowerLoadInfo)->bool
+    {
+      if (_batteryPowerLoadInfo->Data().batteryId ==
+          this->dataPtr->batteryEntity)
+      {
+        total_power_load = total_power_load +
+            _batteryPowerLoadInfo->Data().batteryPowerLoad;
+      }
+      return true;
+    });
+
+  bool success = this->dataPtr->battery->SetPowerLoad(
+      this->dataPtr->consumerId, total_power_load);
+  if (!success)
+      ignerr << "Failed to set consumer power load." << std::endl;
+
   // Start draining the battery if the robot has started moving
   if (!this->dataPtr->startDraining)
   {

src/systems/battery_plugin/LinearBatteryPlugin.hh

@@ -58,11 +58,17 @@ namespace systems
   ///                 (Required if `<enable_recharge>` is set to true)
   /// - `<fix_issue_225>` True to change the battery behavior to fix some issues
   /// described in https://github.com/gazebosim/gz-sim/issues/225.
+  /// - `<start_drainign>` Whether to start draining the battery right away.
+  /// False by default.
   /// - `<power_draining_topic>` A topic that is used to start battery
-  /// discharge. Any message on the specified topic will cause the batter to
+  /// discharge. Any message on the specified topic will cause the battery to
   /// start draining. This element can be specified multiple times if
   /// multiple topics should be monitored. Note that this mechanism will
   /// start the battery draining, and once started will keep drainig.
+  /// - `<stop_power_draining_topic>` A topic that is used to stop battery
+  /// discharge. Any message on the specified topic will cause the battery to
+  /// stop draining.
+
   class LinearBatteryPlugin
       : public System,
         public ISystemConfigure,

src/systems/thruster/Thruster.cc

@@ -29,10 +29,12 @@
 
 #include "ignition/gazebo/components/AngularVelocity.hh"
 #include "ignition/gazebo/components/BatterySoC.hh"
+#include "ignition/gazebo/components/BatteryPowerLoad.hh"
 #include "ignition/gazebo/components/ChildLinkName.hh"
 #include "ignition/gazebo/components/JointAxis.hh"
 #include "ignition/gazebo/components/JointVelocityCmd.hh"
 #include "ignition/gazebo/components/LinearVelocity.hh"
+#include "ignition/gazebo/components/Name.hh"
 #include "ignition/gazebo/components/Pose.hh"
 #include "ignition/gazebo/components/World.hh"
 #include "ignition/gazebo/Link.hh"
@@ -65,6 +67,9 @@ class ignition::gazebo::systems::ThrusterPrivateData
   /// \brief The link entity which will spin
   public: ignition::gazebo::Entity linkEntity;
 
+  /// \brief Battery consumer entity
+  public: Entity consumerEntity;
+
   /// \brief Axis along which the propeller spins. Expressed in the joint
   /// frame. Addume this doesn't change during simulation.
   public: ignition::math::Vector3d jointAxis;
@@ -126,6 +131,15 @@ class ignition::gazebo::systems::ThrusterPrivateData
   /// \brief Topic name used to control thrust.
   public: std::string topic = "";
 
+  /// \brief Battery entity used by the thruster to consume power.
+  public: std::string batteryName = "";
+
+  /// \brief Battery power load of the thruster.
+  public: double powerLoad = 0.0;
+
+  /// \brief Has the battery consumption being initialized.
+  public: bool batteryInitialized = false;
+
   /// \brief Callback for handling thrust update
   public: void OnCmdThrust(const msgs::Double &_msg);
 
@@ -363,6 +377,22 @@ void Thruster::Configure(
   {
     igndbg << "Using velocity control for propeller joint." << std::endl;
   }
+
+  // Get power load and battery name info
+  if (_sdf->HasElement("power_load"))
+  {
+    if (!_sdf->HasElement("battery_name"))
+    {
+      ignerr << "Specified a <power_load> but missing <battery_name>."
+          "Specify a battery name so the power load can be assigned to it."
+          << std::endl;
+    }
+    else
+    {
+      this->dataPtr->powerLoad = _sdf->Get<double>("power_load");
+      this->dataPtr->batteryName = _sdf->Get<std::string>("battery_name");
+    }
+  }
 }
 
 /////////////////////////////////////////////////
@@ -443,6 +473,52 @@ void Thruster::PreUpdate(
     return;
   }
 
+  // Intit battery consumption if it was set
+  if (!this->dataPtr->batteryName.empty() &&
+      !this->dataPtr->batteryInitialized)
+  {
+    this->dataPtr->batteryInitialized = true;
+
+    // Check that a battery exists with the specified name
+    Entity batteryEntity;
+    int numBatteriesWithName = 0;
+    _ecm.Each<components::BatterySoC, components::Name>(
+      [&](const Entity &_entity,
+        const components::BatterySoC */*_BatterySoC*/,
+        const components::Name *_name)->bool
+      {
+        if (this->dataPtr->batteryName == _name->Data())
+        {
+          ++numBatteriesWithName;
+          batteryEntity = _entity;
+        }
+        return true;
+      });
+    if (numBatteriesWithName == 0)
+    {
+      ignerr << "Can't assign battery consumption to battery: ["
+             << this->dataPtr->batteryName << "]. No batteries"
+             "were found with the given name." << std::endl;
+      return;
+    }
+    if (numBatteriesWithName > 1)
+    {
+      ignerr << "More than one battery found with name: ["
+             << this->dataPtr->batteryName << "]. Please make"
+             "sure battery names are unique within the system."
+             << std::endl;
+      return;
+    }
+
+    // Create the battery consumer entity and its component
+    this->dataPtr->consumerEntity = _ecm.CreateEntity();
+    components::BatteryPowerLoadInfo batteryPowerLoadInfo{
+        batteryEntity, this->dataPtr->powerLoad};
+    _ecm.CreateComponent(this->dataPtr->consumerEntity,
+        components::BatteryPowerLoad(batteryPowerLoadInfo));
+    _ecm.SetParentEntity(this->dataPtr->consumerEntity, batteryEntity);
+  }
+
   ignition::gazebo::Link link(this->dataPtr->linkEntity);
 
   auto pose = worldPose(this->dataPtr->linkEntity, _ecm);