Add parameter for adjust current sign for ROS

Signed-off-by: Tatsuro Sakaguchi <tatsuro.sakaguchi@g.softbank.co.jp>

Author: Tacha-S

src/systems/battery_plugin/LinearBatteryPlugin.cc

@@ -178,6 +178,9 @@ class gz::sim::systems::LinearBatteryPluginPrivate
 
   /// \brief Initial power load set trough config
   public: double initialPowerLoad = 0.0;
+
+  /// \brief Adjusts the sign of the current to align with ROS conventions.
+  public: bool adjustCurrentSignForROS{false};
 };
 
 /////////////////////////////////////////////////
@@ -273,6 +276,10 @@ void LinearBatteryPlugin::Configure(const Entity &_entity,
   if (_sdf->HasElement("fix_issue_225"))
     this->dataPtr->fixIssue225 = _sdf->Get<bool>("fix_issue_225");
 
+  if (_sdf->HasElement("adjust_current_sign_for_ros"))
+    this->dataPtr->adjustCurrentSignForROS =
+      _sdf->Get<bool>("adjust_current_sign_for_ros");
+
   if (_sdf->HasElement("battery_name") && _sdf->HasElement("voltage"))
   {
     this->dataPtr->batteryName = _sdf->Get<std::string>("battery_name");
@@ -669,14 +676,22 @@ double LinearBatteryPlugin::OnUpdateVoltage(
       totalpower += powerLoad.second;
   }
 
-  this->dataPtr->iraw = totalpower / _battery->Voltage();
+  if (this->dataPtr->adjustCurrentSignForROS)
+    this->dataPtr->iraw = -totalpower / _battery->Voltage();
+  else
+    this->dataPtr->iraw = totalpower / _battery->Voltage();
 
   // compute charging current
   auto iCharge = this->dataPtr->c / this->dataPtr->tCharge;
 
   // add charging current to battery
   if (this->dataPtr->startCharging && this->dataPtr->StateOfCharge() < 0.9)
-    this->dataPtr->iraw -= iCharge;
+  {
+    if (this->dataPtr->adjustCurrentSignForROS)
+      this->dataPtr->iraw += iCharge;
+    else
+      this->dataPtr->iraw -= iCharge;
+  }
 
   this->dataPtr->ismooth = this->dataPtr->ismooth + k *
     (this->dataPtr->iraw - this->dataPtr->ismooth);
@@ -693,13 +708,23 @@ double LinearBatteryPlugin::OnUpdateVoltage(
   }
 
   // Convert dt to hours
-  this->dataPtr->q = this->dataPtr->q - ((dt * this->dataPtr->ismooth) /
-    3600.0);
+  if (this->dataPtr->adjustCurrentSignForROS)
+    this->dataPtr->q = this->dataPtr->q + ((dt * this->dataPtr->ismooth) /
+      3600.0);
+  else
+    this->dataPtr->q = this->dataPtr->q - ((dt * this->dataPtr->ismooth) /
+      3600.0);
 
   // open circuit voltage
-  double voltage = this->dataPtr->e0 + this->dataPtr->e1 * (
-    1 - this->dataPtr->q / this->dataPtr->c)
-      - this->dataPtr->r * this->dataPtr->ismooth;
+  double voltage;
+  if (this->dataPtr->adjustCurrentSignForROS)
+    voltage = this->dataPtr->e0 + this->dataPtr->e1 * (
+      1 - this->dataPtr->q / this->dataPtr->c)
+        + this->dataPtr->r * this->dataPtr->ismooth;
+  else
+    voltage = this->dataPtr->e0 + this->dataPtr->e1 * (
+      1 - this->dataPtr->q / this->c)
+        - this->dataPtr->r * this->dataPtr->ismooth;
 
   // Estimate state of charge
   if (this->dataPtr->fixIssue225)
@@ -709,7 +734,10 @@ double LinearBatteryPlugin::OnUpdateVoltage(
     double isum = 0.0;
     for (size_t i = 0; i < this->dataPtr->iList.size(); ++i)
       isum += (this->dataPtr->iList[i] * this->dataPtr->dtList[i] / 3600.0);
-    this->dataPtr->soc = this->dataPtr->soc - isum / this->dataPtr->c;
+    if (this->dataPtr->adjustCurrentSignForROS)
+      this->dataPtr->soc = this->dataPtr->soc + isum / this->dataPtr->c;
+    else
+      this->dataPtr->soc = this->dataPtr->soc - isum / this->dataPtr->c;
   }
 
   // Throttle debug messages