Support battery draining start via topics

src/systems/battery_plugin/LinearBatteryPlugin.cc

@@ -69,6 +69,15 @@ class ignition::gazebo::systems::LinearBatteryPluginPrivate
   /// \param[in] _req This value should be true.
   public: void OnDisableRecharge(const ignition::msgs::Boolean &_req);
 
+  /// \brief Callback connected to additional topics that can start battery
+  /// draining.
+  /// \param[in] _data Message data.
+  /// \param[in] _size Message data size.
+  /// \param[in] _info Information about the message.
+  public: void OnBatteryDrainingMsg(
+    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;
 
@@ -153,6 +162,9 @@ 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;
 };
 
 /////////////////////////////////////////////////
@@ -340,6 +352,23 @@ void LinearBatteryPlugin::Configure(const Entity &_entity,
             << "in LinearBatteryPlugin SDF" << std::endl;
   }
 
+  // Subscribe to power draining topics, if any.
+  if (_sdf->HasElement("power_draining_topic"))
+  {
+    sdf::ElementConstPtr sdfElem = _sdf->FindElement("power_draining_topic");
+    while (sdfElem)
+    {
+      const auto &topic = sdfElem->Get<std::string>();
+      this->dataPtr->node.SubscribeRaw(topic,
+          std::bind(&LinearBatteryPluginPrivate::OnBatteryDrainingMsg,
+          this->dataPtr.get(), std::placeholders::_1, std::placeholders::_2,
+          std::placeholders::_3));
+      ignmsg << "LinearBatteryPlugin subscribes to 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()
@@ -371,6 +400,7 @@ void LinearBatteryPluginPrivate::Reset()
   this->iraw = 0.0;
   this->ismooth = 0.0;
   this->q = this->q0;
+  this->startDrainingFromTopics = false;
 }
 
 /////////////////////////////////////////////////
@@ -395,13 +425,24 @@ void LinearBatteryPluginPrivate::OnDisableRecharge(
   this->startCharging = false;
 }
 
+//////////////////////////////////////////////////
+void LinearBatteryPluginPrivate::OnBatteryDrainingMsg(
+  const char *, const size_t, const ignition::transport::MessageInfo &)
+{
+  this->startDrainingFromTopics = true;
+}
+
 //////////////////////////////////////////////////
 void LinearBatteryPlugin::PreUpdate(
   const ignition::gazebo::UpdateInfo &/*_info*/,
   ignition::gazebo::EntityComponentManager &_ecm)
 {
   IGN_PROFILE("LinearBatteryPlugin::PreUpdate");
-  this->dataPtr->startDraining = false;
+
+  // \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;
   // Start draining the battery if the robot has started moving
   if (!this->dataPtr->startDraining)
   {

src/systems/battery_plugin/LinearBatteryPlugin.hh

@@ -58,6 +58,11 @@ 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/ignitionrobotics/ign-gazebo/issues/225.
+  /// - `<power_draining_topic>` A topic that is used to start battery
+  /// discharge. Any message on the specified topic will cause the batter 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.
   class LinearBatteryPlugin
       : public System,
         public ISystemConfigure,

test/integration/battery_plugin.cc

@@ -23,6 +23,7 @@
 #include <ignition/common/Console.hh>
 #include <ignition/common/Util.hh>
 #include <ignition/common/Filesystem.hh>
+#include <ignition/transport/Node.hh>
 
 #include <sdf/Root.hh>
 #include <sdf/World.hh>
@@ -134,20 +135,106 @@ TEST_F(BatteryPluginTest, SingleBattery)
   EXPECT_LT(batComp->Data(), 12.592);
 
   // Check there is a single battery matching exactly the one specified
-  int batCount = 0;
+  int linearBatCount = 0;
+  int totalBatCount = 0;
   ecm->Each<components::BatterySoC, components::Name>(
       [&](const Entity &_batEntity, components::BatterySoC *_batComp,
           components::Name *_nameComp) -> bool
       {
-        batCount++;
+        totalBatCount++;
+        if (_nameComp->Data() == "linear_battery")
+        {
+          linearBatCount++;
 
-        EXPECT_NE(kNullEntity, _batEntity);
-        EXPECT_EQ(_nameComp->Data(), "linear_battery");
+          EXPECT_NE(kNullEntity, _batEntity);
+          EXPECT_EQ(_nameComp->Data(), "linear_battery");
 
-        // Check battery component voltage data is lower than initial voltage
-        EXPECT_LT(_batComp->Data(), 12.592);
+          // Check battery component voltage data is lower than initial voltage
+          EXPECT_LT(_batComp->Data(), 12.592);
+        }
 
         return true;
       });
-  EXPECT_EQ(batCount, 1);
+  EXPECT_EQ(linearBatCount, 1);
+  EXPECT_EQ(totalBatCount, 2);
+}
+
+/////////////////////////////////////////////////
+// Battery with  power draining topics
+TEST_F(BatteryPluginTest, PowerDrainTopic)
+{
+  const auto sdfPath = common::joinPaths(std::string(PROJECT_SOURCE_PATH),
+    "test", "worlds", "battery.sdf");
+  sdf::Root root;
+  EXPECT_EQ(root.Load(sdfPath).size(), 0lu);
+  EXPECT_GT(root.WorldCount(), 0lu);
+
+  ServerConfig serverConfig;
+  serverConfig.SetSdfFile(sdfPath);
+
+  // A pointer to the ecm. This will be valid once we run the mock system
+  gazebo::EntityComponentManager *ecm = nullptr;
+  this->mockSystem->preUpdateCallback =
+    [&ecm](const gazebo::UpdateInfo &, gazebo::EntityComponentManager &_ecm)
+    {
+      ecm = &_ecm;
+
+      // Check a battery exists
+      EXPECT_TRUE(ecm->HasComponentType(components::BatterySoC::typeId));
+
+      // Find the battery entity
+      Entity batEntity = ecm->EntityByComponents(components::Name(
+        "linear_battery_topics"));
+      EXPECT_NE(kNullEntity, batEntity);
+
+      // Find the battery component
+      EXPECT_TRUE(ecm->EntityHasComponentType(batEntity,
+        components::BatterySoC::typeId));
+      auto batComp = ecm->Component<components::BatterySoC>(batEntity);
+
+      // Check voltage is never zero.
+      // This check is here to guarantee that components::BatterySoC in
+      // the LinearBatteryPlugin is not zero when created. If
+      // components::BatterySoC is zero on start, then the Physics plugin
+      // can disable a joint. This in turn can prevent the joint from
+      // rotating. See https://github.com/ignitionrobotics/ign-gazebo/issues/55
+      EXPECT_GT(batComp->Data(), 0);
+    };
+
+  // Start server
+  Server server(serverConfig);
+  server.AddSystem(this->systemPtr);
+  server.Run(true, 100, false);
+  EXPECT_NE(nullptr, ecm);
+
+  // Check a battery exists
+  EXPECT_TRUE(ecm->HasComponentType(components::BatterySoC::typeId));
+
+  // Find the battery entity
+  Entity batEntity = ecm->EntityByComponents(components::Name(
+    "linear_battery_topics"));
+  EXPECT_NE(kNullEntity, batEntity);
+
+  // Find the battery component
+  EXPECT_TRUE(ecm->EntityHasComponentType(batEntity,
+    components::BatterySoC::typeId));
+  auto batComp = ecm->Component<components::BatterySoC>(batEntity);
+
+  // Check state of charge should be 1, since the batery has not drained
+  // and the <initial_charge> is equivalent ot the <capacity>.
+  EXPECT_DOUBLE_EQ(batComp->Data(), 1.0);
+
+  // Send a message on one of the <power_draining_topic> topics, which will
+  // start the battery draining when the server starts again.
+  ignition::transport::Node node;
+  auto pub = node.Advertise<msgs::StringMsg>("/battery/discharge2");
+  msgs::StringMsg msg;
+  pub.Publish(msg);
+
+  // Run the server again.
+  server.Run(true, 100, false);
+
+  // The state of charge should be <1, since the batery has started
+  // draining.
+  EXPECT_LT(batComp->Data(), 1.0);
 }

test/integration/imu_system.cc

@@ -223,8 +223,6 @@ TEST_F(ImuTest, OrientationDisabled)
   EXPECT_FALSE(server.Running());
   EXPECT_FALSE(*server.Running(0));
 
-  const std::string sensorName = "imu_sensor";
-
   auto topic =
       "world/imu_sensor/model/imu_model/link/link/sensor/imu_sensor/imu";
 

test/worlds/battery.sdf

@@ -58,5 +58,37 @@
 
     </model>
 
+    <model name="linear_battery_demo_model_with_topics">
+      <pose>1 0 0 0 0 0</pose>
+      <static>false</static>
+      <link name="body">
+        <pose>0 0 0.5 0 0 0</pose>
+        <visual name="visual">
+          <geometry>
+            <box>
+              <size>1 1 1</size>
+            </box>
+          </geometry>
+        </visual>
+      </link>
+
+      <plugin filename="ignition-gazebo-linearbatteryplugin-system"
+        name="ignition::gazebo::systems::LinearBatteryPlugin">
+        <battery_name>linear_battery_topics</battery_name>
+        <voltage>12.592</voltage>
+        <open_circuit_voltage_constant_coef>12.694</open_circuit_voltage_constant_coef>
+        <open_circuit_voltage_linear_coef>-3.1424</open_circuit_voltage_linear_coef>
+        <initial_charge>1.2009</initial_charge>
+        <capacity>1.2009</capacity>
+        <resistance>0.061523</resistance>
+        <smooth_current_tau>1.9499</smooth_current_tau>
+        <!-- Consumer-specific -->
+        <power_load>500</power_load>  <!-- high load to quickly drain battery -->
+        <power_draining_topic>/battery/discharge1</power_draining_topic>
+        <power_draining_topic>/battery/discharge2</power_draining_topic>
+      </plugin>
+
+    </model>
+
   </world>
 </sdf>