From 00b952c14a90b61ce3b8be1dabfac2fec27e8fee Mon Sep 17 00:00:00 2001
From: rafal-gorecki <rafal.gorecki@husarion.com>
Date: Tue, 23 Apr 2024 15:06:02 +0200
Subject: [PATCH] Fix battery discharge

---
 panther_description/urdf/gazebo.urdf.xacro    | 29 +++++++------------
 .../config/battery_plugin_config.yaml         | 25 +++++++++-------
 2 files changed, 25 insertions(+), 29 deletions(-)

diff --git a/panther_description/urdf/gazebo.urdf.xacro b/panther_description/urdf/gazebo.urdf.xacro
index 7dbbc738c..9b3713f37 100644
--- a/panther_description/urdf/gazebo.urdf.xacro
+++ b/panther_description/urdf/gazebo.urdf.xacro
@@ -6,38 +6,31 @@
 
     <!-- Read property -->
     <xacro:property name="battery_config" value="${xacro.load_yaml(config_file)}" />
-    <xacro:property name="bat_initial_charge"
-      value="${20.0*battery_config['initial_charge_percentage']/100.0}" />
-    <xacro:property name="bat_charging_time" value="${battery_config['charging_time']}" />
-
-    <xacro:if value="${battery_config['simulate_discharging']}">
-      <xacro:property name="bat_power_load" value="${battery_config['power_load']}" />
+    <xacro:if value="${namespace == ''}">
+      <xacro:property name="ns" value="" />
     </xacro:if>
-    <xacro:unless value="${battery_config['simulate_discharging']}">
-      <xacro:property name="bat_power_load" value="0.0" />
+    <xacro:unless value="${namespace == ''}">
+        <xacro:property name="ns" value="${namespace}/" />
     </xacro:unless>
 
     <!-- Sim Battery -->
     <gazebo>
       <plugin filename="ignition-gazebo-linearbatteryplugin-system"
         name="gz::sim::systems::LinearBatteryPlugin">
-        <battery_name>panther_battery</battery_name>
+        <battery_name>${ns}panther_battery</battery_name>
         <voltage>41.4</voltage>
         <open_circuit_voltage_constant_coef>42.0</open_circuit_voltage_constant_coef>
         <open_circuit_voltage_linear_coef>-10.0</open_circuit_voltage_linear_coef>
-        <initial_charge>${bat_initial_charge}</initial_charge>
-        <capacity>20.0</capacity>
+        <initial_charge>${battery_config['initial_charge_percentage']*battery_config['capacity']}</initial_charge>
+        <capacity>${battery_config['capacity']}</capacity>
         <resistance>0.15</resistance>
         <smooth_current_tau>2.0</smooth_current_tau>
         <enable_recharge>true</enable_recharge>
-        <charging_time>${bat_charging_time}</charging_time>
+        <charging_time>${battery_config['charging_time']}</charging_time>
         <soc_threshold>2.0</soc_threshold>
-        <power_load>${bat_power_load}</power_load>
-        <start_on_motion>false</start_on_motion>
-
-        <!-- https://github.com/gazebosim/gz-sim/issues/225 -->
-        <fix_issue_225>true</fix_issue_225>
-
+        <!-- Because of https://github.com/gazebosim/gz-sim/issues/225 devision by 100-->
+        <power_load>${battery_config['power_load']/100.0}</power_load>
+        <start_draining>${battery_config['simulate_discharging']}</start_draining>
         <ros>
           <namespace>${namespace}</namespace>
         </ros>
diff --git a/panther_gazebo/config/battery_plugin_config.yaml b/panther_gazebo/config/battery_plugin_config.yaml
index 665710fdc..45eaac5ed 100644
--- a/panther_gazebo/config/battery_plugin_config.yaml
+++ b/panther_gazebo/config/battery_plugin_config.yaml
@@ -1,19 +1,22 @@
-# Parameters for the Ignition LinearBatteryPlugin
+# Parameters for the Gazebo LinearBatteryPlugin
+# For more information on Panther power consumption, please refer to:
+# https://husarion.com/manuals/panther/#battery--charging
 
-# Enables battery simulation. If set to true, the battery will discharge,
-# and if it depletes completely, the robot will stop moving.
+# If set to true, the battery will discharge and if it depletes completely, the robot will stop moving.
 simulate_discharging: false
 
-# Sets the initial charge percentage of the battery.
-initial_charge_percentage: 70.0
+# Battery capacity
+# BAT01 - 20 Ah
+# BAT02 - 40 Ah
+capacity: 20.0 # [Ah]
+
+# Sets the initial charge percentage of the battery (range 0-1).
+initial_charge_percentage: 0.7
 
 # Specifies the charging time for the battery in hours.
 charging_time: 6.0 # [h]
 
-# Represents the power load during normal operation and is initially set to 120W.
-# With the default power load of 120W, the robot can operate for up to 8 hours.
-# When the 'simulate_discharging' parameter is set to false, this value defaults to 0.
-# Users are encouraged to customize this value to match their specific needs.
-# For more information on Panther power consumption, please refer to:
-# https://husarion.com/manuals/panther/#battery--charging
+# Constant power discharging the battery.
+# 120W – represents the power consumed during normal operation.
+# With this power load, the robot equipped with BAT01 should work for about 7 hours.
 power_load: 120.0 # [W]