Added gz sim battery for testing

rox_bringup/configs/gz_bridge/gz_bridge_config.yaml

@@ -103,6 +103,39 @@
   gz_type_name: "gz.msgs.PointCloudPacked"
   direction: "GZ_TO_ROS"
 
+# Battery State
+- ros_topic_name: "/battery_state"
+  gz_topic_name: "/model/rox/battery/linear_battery/state"
+  ros_type_name: "sensor_msgs/msg/BatteryState"
+  gz_type_name: "gz.msgs.BatteryState"
+  direction: GZ_TO_ROS
+
+# Battery Power drain control
+- ros_topic_name: "/start_power_draining"
+  gz_topic_name: "/start_power_draining"
+  ros_type_name: "std_msgs/msg/Empty"
+  gz_type_name: "gz.msgs.Empty"
+  direction: "ROS_TO_GZ"
+
+- ros_topic_name: "/stop_power_draining"
+  gz_topic_name: "/stop_power_draining"
+  ros_type_name: "std_msgs/msg/Empty"
+  gz_type_name: "gz.msgs.Empty"
+  direction: "ROS_TO_GZ"
+
+# Battery recharge control
+- ros_topic_name: "/start_charging"
+  gz_topic_name: "/model/rox/battery/linear_battery/recharge/start"
+  ros_type_name: "std_msgs/msg/Bool"
+  gz_type_name: "gz.msgs.Boolean"
+  direction: "ROS_TO_GZ"
+
+- ros_topic_name: "/stop_charging"
+  gz_topic_name: "/model/rox/battery/linear_battery/recharge/stop"
+  ros_type_name: "std_msgs/msg/Bool"
+  gz_type_name: "gz.msgs.Boolean"
+  direction: "ROS_TO_GZ"
+
 # Full set of configurations
 # - ros_topic_name: "ros_chatter"
 #   gz_topic_name: "ign_chatter"

rox_bringup/launch/bringup_sim_launch.py

@@ -23,6 +23,7 @@ def execution_stage(context: LaunchContext,
                     d435_enable,
                     scanner_type,
                     ur_dc,
+                    battery_enable,
                     gripper_type,
                     headless_sim,
                     use_wall_time):
@@ -35,6 +36,7 @@ def execution_stage(context: LaunchContext,
     scanner_typ = str(scanner_type.perform(context))
     d435 = str(d435_enable.perform(context))
     imu = str(imu_enable.perform(context))
+    battery = str(battery_enable.perform(context))
     use_ur_dc = str(ur_dc.perform(context))
     headless_sim = str(headless_sim.perform(context)).lower()
     use_wall_time = str(use_wall_time.perform(context)) in ('true', 'True')
@@ -123,6 +125,7 @@ def execution_stage(context: LaunchContext,
         " ", 'joint_type:=', joint_type,
         " ", 'use_imu:=', imu,
         " ", 'use_d435:=', d435,
+        " ", 'use_battery:=', battery,
         " ", 'scanner_type:=', scanner_typ,
         " ", 'arm_type:=', arm_typ,
         " ", 'gripper_type:=', gripper_typ,
@@ -257,6 +260,11 @@ def generate_launch_description():
             description='Enable Realsense - Options: True/False'
         )
 
+    declare_battery_cmd = DeclareLaunchArgument(
+            'battery_enable', default_value='False',
+            description='Enable LFP (Lithium Iron Phosphate) Battery in Simulation - Options: True/False'
+        )
+
     declare_scanner_cmd = DeclareLaunchArgument(
             'scanner_type', default_value='nanoscan',
             choices = ['', 'nanoscan', 'psenscan'],
@@ -299,6 +307,7 @@ def generate_launch_description():
             LaunchConfiguration('d435_enable'),
             LaunchConfiguration('scanner_type'),
             LaunchConfiguration('use_ur_dc'),
+            LaunchConfiguration('battery_enable'),
             LaunchConfiguration('gripper_type'),
             LaunchConfiguration('headless_simulation'),
             LaunchConfiguration('use_wall_time')
@@ -307,6 +316,7 @@ def generate_launch_description():
     ld = LaunchDescription([
         declare_imu_cmd,
         declare_realsense_cmd,
+        declare_battery_cmd,
         declare_scanner_cmd,
         declare_arm_type_cmd,
         declare_rox_type_cmd,

rox_description/urdf/rox.urdf.xacro

@@ -10,6 +10,7 @@ Contributor: Adarsh Karan K P
   <xacro:arg name="use_gz" default="false"/>
   <xacro:arg name="use_imu" default="false"/>
   <xacro:arg name="use_d435" default="false"/>
+  <xacro:arg name="use_battery" default="false"/>
   <xacro:arg name="scanner_type" default="nanoscan"/>
   <xacro:arg name="rox_type" default="argo"/>
   <xacro:arg name="joint_type" default="fixed"/>
@@ -31,6 +32,7 @@ Contributor: Adarsh Karan K P
   <xacro:property name="gz" value="$(arg use_gz)"/>
   <xacro:property name="imu" value="$(arg use_imu)"/>
   <xacro:property name="d435" value="$(arg use_d435)"/>
+  <xacro:property name="battery" value="$(arg use_battery)"/>
   <xacro:property name="scanner" value="$(arg scanner_type)"/>
   <xacro:property name="rox_typ" value="$(arg rox_type)"/>
   <xacro:property name="joint_typ" value="$(arg joint_type)"/>

rox_description/urdf/xacros/gazebo.xacro

@@ -1,6 +1,9 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="rox">
 
+  <!-- Include battery configuration macro -->
+  <xacro:include filename="$(find rox_description)/urdf/xacros/gazebo_battery.xacro" />
+
   <xacro:macro name="gazebo_plugins">
 
     <gazebo reference="lidar_1_link">
@@ -131,6 +134,11 @@
           name="gz::sim::systems::Imu">
       </plugin>
 
+      <!-- Battery plugin for all rox platforms -->
+      <xacro:if value="${battery == True}">
+        <xacro:battery_plugin/>
+      </xacro:if>
+
       <!-- For ArgoDrive platforms -->
       <xacro:if value="${rox_typ == 'argo' or rox_typ == 'argo-trio'}">
         <plugin filename="gz-sim-velocity-control-system"

rox_description/urdf/xacros/gazebo_battery.xacro

@@ -0,0 +1,37 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="rox">
+
+  <!-- Battery configuration macro - LFP (Lithium Iron Phosphate) -->
+  <xacro:macro name="battery_plugin">
+    <plugin filename="gz-sim-linearbatteryplugin-system"
+      name="gz::sim::systems::LinearBatteryPlugin">
+      <battery_name>linear_battery</battery_name>
+      <voltage>54.0</voltage>
+      <open_circuit_voltage_constant_coef>54.0</open_circuit_voltage_constant_coef>
+
+      <!-- Initial charge set to 100% of capacity -->
+      <initial_charge>26.0</initial_charge>
+      <capacity>26.0</capacity>
+      <!-- Internal resistance of the battery (Ohm) -->
+      <resistance>0.01</resistance>
+      <!-- Coefficient for smoothing current -->
+      <smooth_current_tau>2.0</smooth_current_tau>
+
+      <!-- charging I = c / t, discharging I = P / V, 
+      charging I should be > discharging I -->
+      <enable_recharge>true</enable_recharge>
+      <recharge_by_topic>true</recharge_by_topic>
+      <!-- Charging time: 30 minutes = 0.5 hours -->
+      <charging_time>0.5</charging_time>
+
+      <!-- Robot power draw (Wh) -->
+      <power_load>260.0</power_load>
+
+      <power_draining_topic>/start_power_draining</power_draining_topic>
+      <stop_power_draining_topic>/stop_power_draining</stop_power_draining_topic>
+
+      <!-- battery_runtime (hours) = <capacity> * <voltage> / <power_load> -->
+    </plugin>
+  </xacro:macro>
+
+</robot>