Linear Battery model (#207)

* Added linear battery plugin and msg integration Signed-off-by: Aditya <[email protected]> Co-authored-by: braanan <[email protected]>
osrf · Jun 15, 2022 · 7efbba3 · 7efbba3
1 parent fdb0322
commit 7efbba3
Show file tree

Hide file tree

Showing 11 changed files with 1,137 additions and 1 deletion.
diff --git a/lrauv_description/models/tethys_equipped/model.sdf b/lrauv_description/models/tethys_equipped/model.sdf
@@ -340,6 +340,31 @@
         <namespace>tethys</namespace>
       </plugin>
 
+      <!-- The consists of 62 batteries of NH2034HD34 Lithium Ion batteries. -->
+      <!-- Datasheet: https://github.com/osrf/lrauv/files/8614587/NH2034HD34_spec_v1.5.pdf -->
+      <!-- Reference comment : https://github.com/osrf/lrauv/issues/201#issuecomment-1116729239 -->
+      <plugin filename="ignition-gazebo-linearbatteryplugin-system"
+        name="gz::sim::systems::LinearBatteryPlugin">
+        <battery_name>linear_battery</battery_name>
+        <voltage>14.4</voltage>
+        <open_circuit_voltage_constant_coef>14.4</open_circuit_voltage_constant_coef>
+        <fix_issue_225>true</fix_issue_225>
+        <!-- Initial charge in Ah -->
+        <initial_charge>400</initial_charge>
+        <capacity>400</capacity>
+        <!-- 190mOhm / 62 -->
+        <resistance>0.003064</resistance>
+        <!-- Power consumption in Watts: 14.4V * 2 A -->
+        <power_load>28.8</power_load>
+        <!-- Recharging the battery -->
+        <!-- Avg discharge current = (max + min) / 2 -->
+        <!--  = (15 + 4) / 2 = 9.5 A -->
+        <!-- Charging time: 400 Ah/( 9.5 A ) = 42.1 h -->
+        <enable_recharge>true</enable_recharge>
+        <recharge_by_topic>true</recharge_by_topic>
+        <charging_time>42.1</charging_time>
+      </plugin>
+
     </include>
   </model>
 </sdf>
diff --git a/lrauv_ignition_plugins/proto/lrauv_ignition_plugins/lrauv_state.proto b/lrauv_ignition_plugins/proto/lrauv_ignition_plugins/lrauv_state.proto
@@ -167,4 +167,16 @@ message LRAUVState
   // 0: Data collected from Chlorophyll sensor. Unit: ug / L
   // 1: Pressure calculated from current depth and latitude. Unit: Pa
   repeated float values_          = 32;
+
+  /// \brief Voltage of the battery.
+  double batteryVoltage_           = 33;
+
+  /// \brief Current supplied by the battery in A.
+  double batteryCurrent_           = 34;
+
+  /// \brief Current charge in the battery in Ah.
+  double batteryCharge_            = 35;
+
+  /// \brief Percentage of battery remaining.
+  double batteryPercentage_         = 36;
 }
diff --git a/lrauv_ignition_plugins/src/TethysCommPlugin.cc b/lrauv_ignition_plugins/src/TethysCommPlugin.cc
@@ -350,6 +350,15 @@ void TethysCommPlugin::SetupControlTopics(const std::string &_ns)
       << this->temperatureTopic << "]. " << std::endl;
   }
 
+  this->batteryTopic = gz::transport::TopicUtils::AsValidTopic(
+    "/model/" + _ns + "/" + this->batteryTopic);
+  if (!this->node.Subscribe(this->batteryTopic,
+      &TethysCommPlugin::BatteryCallback, this))
+  {
+    ignerr << "Error subscribing to topic " << "["
+      << this->batteryTopic << "]. " << std::endl;
+  }
+
   this->chlorophyllTopic = gz::transport::TopicUtils::AsValidTopic(
     "/model/" + _ns + "/" + this->chlorophyllTopic);
   if (!this->node.Subscribe(this->chlorophyllTopic,
@@ -489,6 +498,15 @@ void TethysCommPlugin::TemperatureCallback(
   this->latestTemperature.SetCelsius(_msg.data());
 }
 
+void TethysCommPlugin::BatteryCallback(
+  const gz::msgs::BatteryState &_msg)
+{
+  this->latestBatteryVoltage = _msg.voltage();
+  this->latestBatteryCurrent = _msg.current();
+  this->latestBatteryCharge = _msg.charge();
+  this->latestBatteryPercentage = _msg.percentage();
+}
+
 void TethysCommPlugin::ChlorophyllCallback(
   const gz::msgs::Float &_msg)
 {
@@ -632,6 +650,12 @@ void TethysCommPlugin::PostUpdate(
   stateMsg.set_temperature_(this->latestTemperature.Celsius());
   stateMsg.add_values_(this->latestChlorophyll);
 
+  // Battery data
+  stateMsg.set_batteryvoltage_(this->latestBatteryVoltage);
+  stateMsg.set_batterycurrent_(this->latestBatteryCurrent);
+  stateMsg.set_batterycharge_(this->latestBatteryCharge);
+  stateMsg.set_batterypercentage_(this->latestBatteryPercentage);
+
   // Set Ocean Density
   stateMsg.set_density_(this->oceanDensity);
 
@@ -676,7 +700,11 @@ void TethysCommPlugin::PostUpdate(
       << "\tTemperature (C): " << stateMsg.temperature_() << std::endl
       << "\tSalinity (PSU): " << stateMsg.salinity_() << std::endl
       << "\tChlorophyll (ug/L): " << stateMsg.values_(0) << std::endl
-      << "\tPressure (Pa): " << stateMsg.values_(1) << std::endl;
+      << "\tPressure (Pa): " << stateMsg.values_(1) << std::endl
+      << "\tBattery Voltage (V): " << stateMsg.batteryvoltage_() << std::endl
+      << "\tBattery Current (A): " << stateMsg.batterycurrent_() << std::endl
+      << "\tBattery Charge (Ah): " << stateMsg.batterycharge_() << std::endl
+      << "\tBattery Percentage (unitless): " << stateMsg.batterypercentage_() << std::endl;
 
     this->prevPubPrintTime = _info.simTime;
   }

diff --git a/lrauv_ignition_plugins/src/TethysCommPlugin.hh b/lrauv_ignition_plugins/src/TethysCommPlugin.hh
@@ -71,6 +71,11 @@ namespace tethys
     public: void TemperatureCallback(
                 const gz::msgs::Double &_msg);
 
+    /// Callback function for battery data.
+    /// \param[in] _msg Battery data
+    public: void BatteryCallback(
+                const gz::msgs::BatteryState &_msg);
+
     /// Callback function for chlorophyll sensor data.
     /// \param[in] _msg Sensor data
     public: void ChlorophyllCallback(
@@ -140,6 +145,10 @@ namespace tethys
     private: std::string temperatureTopic
       {"temperature"};
 
+    /// Topic to subscribe to battery data
+    private: std::string batteryTopic
+      {"battery/linear_battery/state"};
+
     /// Topic to subscribe to chlorophyll data
     private: std::string chlorophyllTopic
       {"chlorophyll"};
@@ -179,6 +188,18 @@ namespace tethys
     /// Latest temperature data received from sensor. NaN if not received.
     private: gz::math::Temperature latestTemperature{std::nanf("")};
 
+    /// Latest battery voltage data received, Nan if not received.
+    private: double latestBatteryVoltage{std::nanf("")};
+
+    /// Latest battery current data received, Nan if not received.
+    private: double latestBatteryCurrent{std::nanf("")};
+
+    /// Latest battery charge data received, Nan if not received.
+    private: double latestBatteryCharge{std::nanf("")};
+
+    /// Latest battery percentage data received, Nan if not received.
+    private: double latestBatteryPercentage{std::nanf("")};
+
     /// Latest chlorophyll data received from sensor. NaN if not received.
     private: float latestChlorophyll{std::nanf("")};
 

diff --git a/lrauv_system_tests/include/lrauv_system_tests/Subscription.hh b/lrauv_system_tests/include/lrauv_system_tests/Subscription.hh
@@ -86,6 +86,32 @@ class Subscription
     return std::move(this->messageHistory);
   }
 
+  /// Current number of messages stored.
+  /// \return number of messages stored in the
+  /// messageHistory container.
+  public: int MessageHistorySize()
+  {
+    std::lock_guard<std::mutex> lock(this->mutex);
+    return this->messageHistory.size();
+  }
+
+  /// Read the message according to the index.
+  /// \return message in the messageHistory container
+  /// based on its index.
+  public: MessageT GetMessageByIndex(int _index)
+  {
+    std::lock_guard<std::mutex> lock(this->mutex);
+    return this->messageHistory[_index];
+  }
+
+  /// Reset the messageHistory container by clearing
+  /// existing messages.
+  public: void ResetMessageHistory()
+  {
+    std::lock_guard<std::mutex> lock(this->mutex);
+    this->messageHistory.clear();
+  }
+
   /// Read last message received.
   /// \note This is a destructive operation.
   /// \throws std::runtime_error if there is

diff --git a/lrauv_system_tests/test/vehicle/CMakeLists.txt b/lrauv_system_tests/test/vehicle/CMakeLists.txt
@@ -18,6 +18,9 @@ target_link_libraries(test_ahrs
 gtest_discover_tests(test_ahrs)
 
 foreach(_test
+    test_battery_full_charge
+    test_battery_half_charge
+    test_battery_low_charge
     test_buoyancy_action
     test_drop_weight
     test_elevator_action

diff --git a/lrauv_system_tests/test/vehicle/test_battery_full_charge.cc b/lrauv_system_tests/test/vehicle/test_battery_full_charge.cc
@@ -0,0 +1,69 @@
+/*
+ * 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.
+ *
+ */
+
+/*
+ * Development of this module has been funded by the Monterey Bay Aquarium
+ * Research Institute (MBARI) and the David and Lucile Packard Foundation
+ */
+
+#include <gtest/gtest.h>
+
+#include <gz/msgs/battery_state.pb.h>
+
+#include <chrono>
+
+#include "lrauv_system_tests/TestFixture.hh"
+
+using namespace gz;
+using namespace std::literals::chrono_literals;
+
+//////////////////////////////////////////////////
+/// Test if the battery discharges with time with the specified
+/// disacharge power rate, when starting with full charge.
+TEST(BatteryTest, TestDischargeFullCharged)
+{
+  using TestFixture = lrauv_system_tests::TestFixtureWithVehicle;
+  TestFixture fixture("buoyant_tethys.sdf", "tethys");
+  uint64_t iterations = fixture.Step(100u);
+
+  transport::Node node;
+  lrauv_system_tests::Subscription<msgs::BatteryState> batterySubscription;
+  batterySubscription.Subscribe(node, "/model/tethys/battery/linear_battery/state");
+
+  fixture.Step(1000u);
+
+  ASSERT_TRUE(batterySubscription.WaitForMessages(5, 10s));
+  int n = batterySubscription.MessageHistorySize() - 1;
+  auto initialMessage = batterySubscription.GetMessageByIndex(0);
+  double initialCharge = initialMessage.charge();
+  double initialVoltage = initialMessage.voltage();
+  double initialTime = initialMessage.header().stamp().sec() + 
+    initialMessage.header().stamp().nsec()/1000000000.0;
+
+  /* Plugin started with 100% charge */
+  EXPECT_NEAR(initialCharge, 400, 0.2);
+
+  auto finalMessage = batterySubscription.GetMessageByIndex(n);
+  double finalCharge = finalMessage.charge();
+  double finalVoltage = finalMessage.voltage();
+  double finalTime = finalMessage.header().stamp().sec() + 
+    finalMessage.header().stamp().nsec()/1000000000.0;
+
+  double dischargePower =  (finalVoltage + initialVoltage) * 0.5 *
+    (finalCharge - initialCharge) * 3600 / (finalTime - initialTime);
+  EXPECT_NEAR(dischargePower, -28.8, 0.5);
+}
diff --git a/lrauv_system_tests/test/vehicle/test_battery_half_charge.cc b/lrauv_system_tests/test/vehicle/test_battery_half_charge.cc
@@ -0,0 +1,97 @@
+/*
+ * 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.
+ *
+ */
+
+/*
+ * Development of this module has been funded by the Monterey Bay Aquarium
+ * Research Institute (MBARI) and the David and Lucile Packard Foundation
+ */
+
+#include <gtest/gtest.h>
+
+#include <gz/msgs/battery_state.pb.h>
+
+#include <chrono>
+
+#include "lrauv_system_tests/TestFixture.hh"
+
+using namespace gz;
+using namespace std::literals::chrono_literals;
+
+//////////////////////////////////////////////////
+/// Test if the battery discharges with time with the specified
+/// disacharge power rate, when starting with half charge.
+/// Send recharge start/stop commands and verify the battery behaves
+/// accordingly.
+TEST(BatteryTest, TestDischargeHalfCharged)
+{
+  using TestFixture = lrauv_system_tests::TestFixtureWithVehicle;
+  TestFixture fixture("buoyant_tethys_half_battery.sdf", "tethys");
+  uint64_t iterations = fixture.Step(100u);
+
+  transport::Node node;
+  lrauv_system_tests::Subscription<msgs::BatteryState> batterySubscription;
+  batterySubscription.Subscribe(node, "/model/tethys/battery/linear_battery/state");
+
+  fixture.Step(1000u);
+
+  ASSERT_TRUE(batterySubscription.WaitForMessages(5, 10s));
+  int n = batterySubscription.MessageHistorySize() - 1;
+  auto initialMessage = batterySubscription.GetMessageByIndex(0);
+  double initialCharge = initialMessage.charge();
+  double initialVoltage = initialMessage.voltage();
+  double initialTime = initialMessage.header().stamp().sec() + 
+    initialMessage.header().stamp().nsec()/1000000000.0;
+
+  /* Plugin started wiht 50% charge */
+  EXPECT_NEAR(initialCharge, 200, 0.2);
+
+  auto finalMessage = batterySubscription.GetMessageByIndex(n);
+  double finalCharge = finalMessage.charge();
+  double finalVoltage = finalMessage.voltage();
+  double finalTime = finalMessage.header().stamp().sec() + 
+    finalMessage.header().stamp().nsec()/1000000000.0;
+
+  /* Check discharge rate when battery is 50% discharged */
+  double dischargePower =  (finalVoltage + initialVoltage) * 0.5 *
+    (finalCharge - initialCharge) * 3600 / (finalTime - initialTime);
+  EXPECT_NEAR(dischargePower, -28.8, 0.5);
+
+  batterySubscription.ResetMessageHistory();
+
+  /* Test battery recharge command */
+  /* Start charging and check if charge increases with time */
+  const unsigned int timeout{5000};
+  bool result;
+  msgs::Boolean req;
+  msgs::Empty rep;
+  EXPECT_TRUE(node.Request("/model/tethys/battery/linear_battery/recharge/start", req, timeout, rep, result));
+
+  fixture.Step(1000u);
+
+  ASSERT_TRUE(batterySubscription.WaitForMessages(5, 10s));
+  n = batterySubscription.MessageHistorySize() - 1;
+  EXPECT_GT(batterySubscription.GetMessageByIndex(n).charge(), finalCharge);
+  batterySubscription.ResetMessageHistory();
+
+  /* Stop charging, charge should decrease with time */
+  EXPECT_TRUE(node.Request("/model/tethys/battery/linear_battery/recharge/stop", req, timeout, rep, result));
+  fixture.Step(1000u);
+  ASSERT_TRUE(batterySubscription.WaitForMessages(5, 10s));
+  n = batterySubscription.MessageHistorySize() - 1;
+  EXPECT_LT(batterySubscription.GetMessageByIndex(n).charge(),
+      batterySubscription.GetMessageByIndex(0).charge());
+}