Started work on thrusters

src/subjugator/simulation/subjugator_description/urdf/sub9.urdf.xacro

@@ -7,6 +7,7 @@
   <xacro:include filename="$(find subjugator_description)/urdf/xacro/imu_magnetometer.xacro"/>
   <xacro:include filename="$(find subjugator_description)/urdf/xacro/new_dvl.xacro"/>
   <xacro:include filename="$(find subjugator_description)/urdf/xacro/collision.xacro"/>
+  <xacro:include filename="$(find subjugator_description)/urdf/xacro/thruster.xacro"/>
 
   <!-- KDL Parser does not like inertial base links, so use a 'fake' massless root link -->
   <link name="base_link"/>
@@ -56,6 +57,17 @@
       <nRabsR>-6.32698957</nRabsR>
       <nR>-1</nR>
     </plugin>
+
+    <plugin
+      filename="gz-sim-thruster-system"
+      name="gz::sim::systems::Thruster">
+      <namespace>thrustme</namespace>
+      <use_angvel_cmd>0</use_angvel_cmd>
+      <joint_name>thrustjoint</joint_name>
+      <thrust_coefficient>0.004422</thrust_coefficient>
+      <fluid_density>1000</fluid_density>
+      <propeller_diameter>0.2</propeller_diameter>
+    </plugin>
   </gazebo>
 
   <!-- Sensors -->
@@ -87,4 +99,5 @@
     lz="0.08825985"
   />
 
+
 </robot>

src/subjugator/simulation/subjugator_description/urdf/xacro/thruster.xacro

@@ -0,0 +1,35 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+  <xacro:macro name="mil_thruster" params="name parent xyz rpy">
+
+    <!-- Define the thruster link -->
+    <link name="${name}_link">
+      <inertial>
+        <mass value="0.1"/>  <!-- Add some mass to avoid physics issues -->
+        <origin xyz="0 0 0" />
+        <inertia ixx="0.001" iyy="0.001" izz="0.001" ixy="0" ixz="0" iyz="0"/>
+      </inertial>
+    </link>
+
+    <!-- Define the joint -->
+    <joint name="${name}_joint" type="fixed">
+      <parent link="${parent}"/>
+      <child link="${name}_link"/>
+      <origin xyz="${xyz}" rpy="${rpy}"/>
+    </joint>
+
+    <!-- Define Gazebo plugin -->
+    <gazebo>
+      <plugin filename="gz-sim-thruster-system" name="gz::sim::systems::Thruster">
+        <namespace>${name}</namespace>
+        <use_angvel_cmd>0</use_angvel_cmd>
+        <joint_name>${name}_joint</joint_name>
+        <thrust_coefficient>0.004422</thrust_coefficient>
+        <fluid_density>1000</fluid_density>
+        <propeller_diameter>0.2</propeller_diameter>
+        <debug>true</debug>
+      </plugin>
+    </gazebo>
+
+  </xacro:macro>
+</robot>