From 383a68e6517c4e6049e2b9bbaf249d13c4e72aa4 Mon Sep 17 00:00:00 2001
From: Matteo Del Seppia <71389092+matteodelseppia@users.noreply.github.com>
Date: Tue, 4 Jun 2024 19:08:54 +0200
Subject: [PATCH] Fixed uuv_bluerov2_heavy SDF model, adding jinja for multi
 vehicle simulations (#1042)

* Adding IMU link and joint to uuv_bluerov2_heavy

Adding the iMU link and joint the uuv_bluerov2_heavy model, since it is currently unusable.

* Adding uuv_bluerov2_heavy.sdf.jinja for multi vehicle UUV simulations

* Delete models/uuv_bluerov2_heavy/uuv_bluerov2_heavy.sdf

Deleting the SDF model in favor of the jinja template
---
 ...heavy.sdf => uuv_bluerov2_heavy.sdf.jinja} | 99 +++++++++++++------
 1 file changed, 70 insertions(+), 29 deletions(-)
 rename models/uuv_bluerov2_heavy/{uuv_bluerov2_heavy.sdf => uuv_bluerov2_heavy.sdf.jinja} (90%)

diff --git a/models/uuv_bluerov2_heavy/uuv_bluerov2_heavy.sdf b/models/uuv_bluerov2_heavy/uuv_bluerov2_heavy.sdf.jinja
similarity index 90%
rename from models/uuv_bluerov2_heavy/uuv_bluerov2_heavy.sdf
rename to models/uuv_bluerov2_heavy/uuv_bluerov2_heavy.sdf.jinja
index e99e8199a1..794320870c 100644
--- a/models/uuv_bluerov2_heavy/uuv_bluerov2_heavy.sdf
+++ b/models/uuv_bluerov2_heavy/uuv_bluerov2_heavy.sdf.jinja
@@ -1,6 +1,6 @@
 <?xml version='1.0'?>
 <sdf version='1.6'>
-    <model name="BlueROV2 Heavy">
+    <model name="uuv_bluerov2_heavy">
         <pose>0 0 0 0 0 0</pose>
 
         <link name="base_link">
@@ -30,22 +30,59 @@
             </visual>
 
         </link>
+        
 
+        <!-- IMU Link & Joint -->
+        <link name='uuv_bluerov2_heavy/imu_link'>
+            <pose>0 0 0 0 0 0</pose>
+            <inertial>
+                <pose>0 0 0 0 0 0</pose>
+                <mass>0.015</mass>
+                <inertia>
+                    <ixx>1e-05</ixx>
+                    <ixy>0</ixy>
+                    <ixz>0</ixz>
+                    <iyy>1e-05</iyy>
+                    <iyz>0</iyz>
+                    <izz>1e-05</izz>
+                </inertia>
+            </inertial>
+        </link>
 
-<!-- GPS for getting the local pose -->
-    <include>
-      <uri>model://gps</uri>
-      <pose>0 0 0 0 0 0</pose>
-      <name>gps</name>
-    </include>
-    <joint name='gps_joint' type='fixed'>
-      <parent>base_link</parent>
-      <child>gps::link</child>
-    </joint>
 
+        <joint name='uuv_bluerov2_heavy/imu_joint' type='revolute'>
+            <child>uuv_bluerov2_heavy/imu_link</child>
+            <parent>base_link</parent>
+            <axis>
+                <xyz>1 0 0</xyz>
+                <limit>
+                    <lower>0</lower>
+                    <upper>0</upper>
+                    <effort>0</effort>
+                    <velocity>0</velocity>
+                </limit>
+                <dynamics>
+                    <spring_reference>0</spring_reference>
+                    <spring_stiffness>0</spring_stiffness>
+                </dynamics>
+                <use_parent_model_frame>1</use_parent_model_frame>
+            </axis>
+        </joint>
+
+
+        <!-- GPS -->
+        <include>
+            <uri>model://gps</uri>
+            <pose>0 0 0 0 0 0</pose>
+            <name>gps</name>
+        </include>
+        <joint name='gps_joint' type='fixed'>
+            <parent>base_link</parent>
+            <child>gps::link</child>
+        </joint>
 
 
-<!-- Start of Thrusters  -->
+        <!-- Start of Thrusters  -->
         <link name="thruster1">
             <pose>0.14 -0.10 0 0 1.570796 0.78539815</pose>
             <inertial>
@@ -526,18 +563,22 @@
             <motorSpeedPubTopic>/motor_speed/7</motorSpeedPubTopic>
             <rotorVelocitySlowdownSim>0.025</rotorVelocitySlowdownSim>
         </plugin>
-
-        <plugin name="magnetometer_plugin" filename="libgazebo_magnetometer_plugin.so">
-            <robotNamespace />
+    
+        <plugin name='magnetometer_plugin' filename='libgazebo_magnetometer_plugin.so'>
+            <robotNamespace/>
             <pubRate>100</pubRate>
             <noiseDensity>0.0004</noiseDensity>
             <randomWalk>6.4e-06</randomWalk>
             <biasCorrelationTime>600</biasCorrelationTime>
             <magTopic>/mag</magTopic>
         </plugin>
-
-        <plugin name="barometer_plugin" filename="libgazebo_barometer_plugin.so">
-            <robotNamespace />
+    
+        <plugin name='groundtruth_plugin' filename='libgazebo_groundtruth_plugin.so'>
+            <robotNamespace/>
+        </plugin>
+    
+        <plugin name='barometer_plugin' filename='libgazebo_barometer_plugin.so'>
+            <robotNamespace/>
             <pubRate>50</pubRate>
             <baroTopic>/baro</baroTopic>
         </plugin>
@@ -564,20 +605,20 @@
             <magSubTopic>/mag</magSubTopic>
             <baroSubTopic>/baro</baroSubTopic>
             <mavlink_addr>INADDR_ANY</mavlink_addr>
-            <mavlink_udp_port>14560</mavlink_udp_port>
-            <mavlink_tcp_port>4560</mavlink_tcp_port>
-            <serialEnabled>0</serialEnabled>
-            <serialDevice>/dev/ttyACM0</serialDevice>
-            <baudRate>921600</baudRate>
+            <mavlink_udp_port>{{ mavlink_udp_port }}</mavlink_udp_port>
+            <mavlink_tcp_port>{{ mavlink_tcp_port }}</mavlink_tcp_port>
+            <serialEnabled>{{ serial_enabled }}</serialEnabled>
+            <serialDevice>{{ serial_device }}</serialDevice>
+            <baudRate>{{ serial_baudrate }}</baudRate>
             <qgc_addr>INADDR_ANY</qgc_addr>
             <qgc_udp_port>14550</qgc_udp_port>
             <sdk_addr>INADDR_ANY</sdk_addr>
             <sdk_udp_port>14540</sdk_udp_port>
-            <hil_mode>false</hil_mode>
+            <hil_mode>{{ hil_mode }}</hil_mode>
             <hil_state_level>false</hil_state_level>
             <vehicle_is_tailsitter>false</vehicle_is_tailsitter>
             <send_vision_estimation>0</send_vision_estimation>
-            <send_odometry>0</send_odometry>
+            <send_odometry>1</send_odometry>
             <enable_lockstep>true</enable_lockstep>
             <use_tcp>true</use_tcp>
             <motorSpeedCommandPubTopic>/gazebo/command/motor_speed</motorSpeedCommandPubTopic>
@@ -658,10 +699,10 @@
                 </channel>
             </control_channels>
         </plugin>
-
-        <plugin name="gazebo_imu_plugin" filename="libgazebo_imu_plugin.so">
-            <robotNamespace />
-            <linkName>base_link</linkName>
+        
+        <plugin name='gazebo_imu_plugin' filename='libgazebo_imu_plugin.so'>
+            <robotNamespace/>
+            <linkName>uuv_bluerov2_heavy/imu_link</linkName>
             <imuTopic>/imu</imuTopic>
             <gyroscopeNoiseDensity>0.0003394</gyroscopeNoiseDensity>
             <gyroscopeRandomWalk>3.8785e-05</gyroscopeRandomWalk>