From 0135a779ea1b20c9a6e45af0ee3fe2ac6dd47e3c Mon Sep 17 00:00:00 2001
From: Ben Pratt <c.ben.pratt@gmail.com>
Date: Sun, 29 Sep 2024 17:16:31 -0400
Subject: [PATCH] renamed the models and removed wamv model

---
 .../model.config                              |    0
 .../model.sdf                                 |    0
 .../model.config                              |    0
 .../model.sdf                                 |    0
 .../model.config                              |    0
 .../model.sdf                                 |    0
 .../wildlife_encounter_test_case_1.world      | 2138 +----------------
 .../wildlife_encounter_test_case_2.world      | 2138 +----------------
 .../wildlife_encounter_test_case_3.world      | 2138 +----------------
 .../wildlife_encounter_test_case_4.world      | 2138 +----------------
 .../wildlife_encounter_test_case_5.world      | 2138 +----------------
 .../wildlife_encounter_test_case_6.world      | 2138 +----------------
 12 files changed, 36 insertions(+), 12792 deletions(-)
 rename NaviGator/simulation/navigator_gazebo/models/{polyform_a2_blue => blue_cirucular_buoy}/model.config (100%)
 rename NaviGator/simulation/navigator_gazebo/models/{polyform_a2_blue => blue_cirucular_buoy}/model.sdf (100%)
 rename NaviGator/simulation/navigator_gazebo/models/{polyform_a2_green => green_circular_buoy}/model.config (100%)
 rename NaviGator/simulation/navigator_gazebo/models/{polyform_a2_green => green_circular_buoy}/model.sdf (100%)
 rename NaviGator/simulation/navigator_gazebo/models/{polyform_a2_red => red_circular_buoy}/model.config (100%)
 rename NaviGator/simulation/navigator_gazebo/models/{polyform_a2_red => red_circular_buoy}/model.sdf (100%)

diff --git a/NaviGator/simulation/navigator_gazebo/models/polyform_a2_blue/model.config b/NaviGator/simulation/navigator_gazebo/models/blue_cirucular_buoy/model.config
similarity index 100%
rename from NaviGator/simulation/navigator_gazebo/models/polyform_a2_blue/model.config
rename to NaviGator/simulation/navigator_gazebo/models/blue_cirucular_buoy/model.config
diff --git a/NaviGator/simulation/navigator_gazebo/models/polyform_a2_blue/model.sdf b/NaviGator/simulation/navigator_gazebo/models/blue_cirucular_buoy/model.sdf
similarity index 100%
rename from NaviGator/simulation/navigator_gazebo/models/polyform_a2_blue/model.sdf
rename to NaviGator/simulation/navigator_gazebo/models/blue_cirucular_buoy/model.sdf
diff --git a/NaviGator/simulation/navigator_gazebo/models/polyform_a2_green/model.config b/NaviGator/simulation/navigator_gazebo/models/green_circular_buoy/model.config
similarity index 100%
rename from NaviGator/simulation/navigator_gazebo/models/polyform_a2_green/model.config
rename to NaviGator/simulation/navigator_gazebo/models/green_circular_buoy/model.config
diff --git a/NaviGator/simulation/navigator_gazebo/models/polyform_a2_green/model.sdf b/NaviGator/simulation/navigator_gazebo/models/green_circular_buoy/model.sdf
similarity index 100%
rename from NaviGator/simulation/navigator_gazebo/models/polyform_a2_green/model.sdf
rename to NaviGator/simulation/navigator_gazebo/models/green_circular_buoy/model.sdf
diff --git a/NaviGator/simulation/navigator_gazebo/models/polyform_a2_red/model.config b/NaviGator/simulation/navigator_gazebo/models/red_circular_buoy/model.config
similarity index 100%
rename from NaviGator/simulation/navigator_gazebo/models/polyform_a2_red/model.config
rename to NaviGator/simulation/navigator_gazebo/models/red_circular_buoy/model.config
diff --git a/NaviGator/simulation/navigator_gazebo/models/polyform_a2_red/model.sdf b/NaviGator/simulation/navigator_gazebo/models/red_circular_buoy/model.sdf
similarity index 100%
rename from NaviGator/simulation/navigator_gazebo/models/polyform_a2_red/model.sdf
rename to NaviGator/simulation/navigator_gazebo/models/red_circular_buoy/model.sdf
diff --git a/NaviGator/simulation/navigator_gazebo/worlds/wildlife/wildlife_encounter_test_case_1.world b/NaviGator/simulation/navigator_gazebo/worlds/wildlife/wildlife_encounter_test_case_1.world
index 872001ecc..f6f9265c9 100644
--- a/NaviGator/simulation/navigator_gazebo/worlds/wildlife/wildlife_encounter_test_case_1.world
+++ b/NaviGator/simulation/navigator_gazebo/worlds/wildlife/wildlife_encounter_test_case_1.world
@@ -1439,2133 +1439,7 @@
       <real_time_update_rate>1000</real_time_update_rate>
     </physics>
     <wind/>
-    <model name='wamv'>
-      <link name='wamv/base_link'>
-        <inertial>
-          <pose>0.012722 0 0.026171 0 -0 0</pose>
-          <mass>182.806</mass>
-          <inertia>
-            <ixx>128.232</ixx>
-            <ixy>-2.4178e-21</ixy>
-            <ixz>-3.85993</ixz>
-            <iyy>403.15</iyy>
-            <iyz>-9.28107e-22</iyz>
-            <izz>447.942</izz>
-          </inertia>
-        </inertial>
-        <collision name='wamv/base_link_fixed_joint_lump__front_left_cam_post_collision_collision'>
-          <pose>0.92 0.1 1.38475 0 -0 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.1765</length>
-              <radius>0.0076</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__front_left_cam_post_arm_collision_collision_1'>
-          <pose>0.95 0.1 1.473 -1.20428 -0 -1.5708</pose>
-          <geometry>
-            <cylinder>
-              <length>0.065</length>
-              <radius>0.008</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__front_left_cam_lens_collision_collision_2'>
-          <pose>0.987607 0.1 1.49626 3.14159 1.36136 3.14159</pose>
-          <geometry>
-            <cylinder>
-              <length>0.042</length>
-              <radius>0.015</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__front_left_cam_box_collision_collision_3'>
-          <pose>0.949948 0.1 1.50426 0 0.20944 0</pose>
-          <geometry>
-            <box>
-              <size>0.036 0.03 0.03</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__front_right_cam_post_collision_collision_4'>
-          <pose>0.92 -0.1 1.38475 0 -0 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.1765</length>
-              <radius>0.0076</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__front_right_cam_post_arm_collision_collision_5'>
-          <pose>0.95 -0.1 1.473 -1.20428 -0 -1.5708</pose>
-          <geometry>
-            <cylinder>
-              <length>0.065</length>
-              <radius>0.008</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__front_right_cam_lens_collision_collision_6'>
-          <pose>0.987607 -0.1 1.49626 3.14159 1.36136 3.14159</pose>
-          <geometry>
-            <cylinder>
-              <length>0.042</length>
-              <radius>0.015</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__front_right_cam_box_collision_collision_7'>
-          <pose>0.949948 -0.1 1.50426 0 0.20944 0</pose>
-          <geometry>
-            <box>
-              <size>0.036 0.03 0.03</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__velodyne_post_collision_collision_8'>
-          <pose>0.704 0 1.62325 0 -0 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.6535</length>
-              <radius>0.0076</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__velodyne_post_arm_collision_collision_9'>
-          <pose>0.734 0 1.95 -1.20428 -0 -1.5708</pose>
-          <geometry>
-            <cylinder>
-              <length>0.065</length>
-              <radius>0.008</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__velodyne_collision_collision_10'>
-          <pose>0.774 0 2 0 0.261799 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.075</length>
-              <radius>0.055</radius>
-            </cylinder>
-          </geometry>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <friction>
-              <ode/>
-              <torsional>
-                <ode/>
-              </torsional>
-            </friction>
-            <bounce/>
-          </surface>
-          <max_contacts>10</max_contacts>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__left_float_collision_11'>
-          <pose>-0.4 1.03 0.2 3.14159 1.57079 3.14159</pose>
-          <geometry>
-            <cylinder>
-              <length>4</length>
-              <radius>0.2</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__left_mid_float_collision_12'>
-          <pose>1.85 1.03 0.3 0 1.38 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.5</length>
-              <radius>0.17</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__left_front_float_collision_13'>
-          <pose>2.3 1.03 0.4 0 1.3 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.45</length>
-              <radius>0.12</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__front_left_beam_lower_collision_14'>
-          <pose>0.9 0.85 1 0.78 -0 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.5</length>
-              <radius>0.04</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__front_left_beam_upper_collision_15'>
-          <pose>0.9 0.6 1.18 1.35 -0 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.2</length>
-              <radius>0.04</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__mid_left_beam_lower_collision_16'>
-          <pose>-0.65 0.99 0.7 0.1 0.25 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.45</length>
-              <radius>0.03</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__mid_left_beam_medium_collision_17'>
-          <pose>-0.57 0.87 1.05 0.75 0.25 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.32</length>
-              <radius>0.03</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__mid_left_beam_upper_collision_18'>
-          <pose>-0.55 0.65 1.17 1.35 0.25 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.3</length>
-              <radius>0.03</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__rear_left_beam_lower_collision_19'>
-          <pose>-0.74 1.03 0.7 0 -0.15 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.45</length>
-              <radius>0.03</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__rear_left_beam_medium_collision_20'>
-          <pose>-0.79 0.91 1.05 0.75 -0.15 -0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.32</length>
-              <radius>0.03</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__rear_left_beam_upper_collision_21'>
-          <pose>-0.81 0.67 1.18 1.45 -0.15 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.3</length>
-              <radius>0.03</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__left_joint_collision_22'>
-          <pose>0.58 1.03 0.6 0 -0.6 0</pose>
-          <geometry>
-            <box>
-              <size>0.65 0.2 0.1</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__right_float_collision_23'>
-          <pose>-0.4 -1.03 0.2 3.14159 1.57079 3.14159</pose>
-          <geometry>
-            <cylinder>
-              <length>4</length>
-              <radius>0.2</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__right_mid_float_collision_24'>
-          <pose>1.85 -1.03 0.3 0 1.38 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.5</length>
-              <radius>0.17</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__right_front_float_collision_25'>
-          <pose>2.3 -1.03 0.4 0 1.3 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.45</length>
-              <radius>0.12</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__front_right_beam_lower_collision_26'>
-          <pose>0.9 -0.85 1 -0.78 0 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.5</length>
-              <radius>0.04</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__front_right_beam_upper_collision_27'>
-          <pose>0.9 -0.6 1.18 -1.35 0 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.2</length>
-              <radius>0.04</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__mid_right_beam_lower_collision_28'>
-          <pose>-0.65 -0.99 0.7 -0.1 0.25 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.45</length>
-              <radius>0.03</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__mid_right_beam_medium_collision_29'>
-          <pose>-0.57 -0.87 1.05 -0.75 0.25 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.32</length>
-              <radius>0.03</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__mid_right_beam_upper_collision_30'>
-          <pose>-0.55 -0.65 1.17 -1.35 0.25 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.3</length>
-              <radius>0.03</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__rear_right_beam_lower_collision_31'>
-          <pose>-0.74 -1.03 0.7 0 -0.15 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.45</length>
-              <radius>0.03</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__rear_right_beam_medium_collision_32'>
-          <pose>-0.79 -0.91 1.05 -0.75 -0.15 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.32</length>
-              <radius>0.03</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__rear_right_beam_upper_collision_33'>
-          <pose>-0.81 -0.67 1.18 -1.45 -0.15 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.3</length>
-              <radius>0.03</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__right_joint_collision_34'>
-          <pose>0.58 -1.03 0.6 0 -0.6 0</pose>
-          <geometry>
-            <box>
-              <size>0.65 0.2 0.1</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__top_base_collision_35'>
-          <pose>0 0 1.25 0 -0 0</pose>
-          <geometry>
-            <box>
-              <size>1.85 1 0.1</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__hydrophones_collision_collision_36'>
-          <pose>1 0 -1 0 -0 0</pose>
-          <geometry>
-            <box>
-              <size>0.1 0.1 0.1</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <visual name='wamv/base_link_fixed_joint_lump__wamv/front_left_cam_post_link_visual'>
-          <pose>0.92 0.1 1.38475 0 -0 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.1765</length>
-              <radius>0.0076</radius>
-            </cylinder>
-          </geometry>
-          <material>
-            <script>
-              <name>Gazebo/Black</name>
-              <uri>file://media/materials/scripts/gazebo.material</uri>
-            </script>
-          </material>
-        </visual>
-        <visual name='wamv/base_link_fixed_joint_lump__front_left_cam_post_arm_visual_visual_1'>
-          <pose>0.912 0.1 1.47 -1.0472 -0 -1.5708</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/vrx_gazebo/models/sensor_post/mesh/sensor_post_arm.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <visual name='wamv/base_link_fixed_joint_lump__front_left_cam_visual_visual_2'>
-          <pose>0.937721 0.1 1.50686 0.20944 -0 1.5708</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/vrx_gazebo/models/mono_camera/mesh/mono_camera.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <visual name='wamv/base_link_fixed_joint_lump__wamv/front_right_cam_post_link_visual_3'>
-          <pose>0.92 -0.1 1.38475 0 -0 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.1765</length>
-              <radius>0.0076</radius>
-            </cylinder>
-          </geometry>
-          <material>
-            <script>
-              <name>Gazebo/Black</name>
-              <uri>file://media/materials/scripts/gazebo.material</uri>
-            </script>
-          </material>
-        </visual>
-        <visual name='wamv/base_link_fixed_joint_lump__front_right_cam_post_arm_visual_visual_4'>
-          <pose>0.912 -0.1 1.47 -1.0472 -0 -1.5708</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/vrx_gazebo/models/sensor_post/mesh/sensor_post_arm.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <visual name='wamv/base_link_fixed_joint_lump__front_right_cam_visual_visual_5'>
-          <pose>0.937721 -0.1 1.50686 0.20944 -0 1.5708</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/vrx_gazebo/models/mono_camera/mesh/mono_camera.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <visual name='wamv/base_link_fixed_joint_lump__velodyne_post_link_visual_6'>
-          <pose>0.704 0 1.62325 0 -0 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.6535</length>
-              <radius>0.0076</radius>
-            </cylinder>
-          </geometry>
-          <material>
-            <script>
-              <name>Gazebo/Black</name>
-              <uri>file://media/materials/scripts/gazebo.material</uri>
-            </script>
-          </material>
-        </visual>
-        <visual name='wamv/base_link_fixed_joint_lump__velodyne_post_arm_visual_visual_7'>
-          <pose>0.696 0 1.947 -1.0472 -0 -1.5708</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/vrx_gazebo/models/sensor_post/mesh/sensor_post_arm.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <visual name='wamv/base_link_fixed_joint_lump__velodyne_visual_visual_8'>
-          <pose>0.764941 0 1.96619 0 0.261799 0</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/vrx_gazebo/models/3d_lidar/mesh/3d_lidar.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <visual name='wamv/base_link_fixed_joint_lump__wamv/dummy_link_visual_9'>
-          <pose>0 0 0 0 -0 0</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/wamv_description/models/WAM-V-Base/mesh/WAM-V-Base.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <visual name='wamv/base_link_fixed_joint_lump__wamv/hydrophones_visual_10'>
-          <pose>1 0 -1 0 -0 0</pose>
-          <geometry>
-            <box>
-              <size>0.1 0.1 0.1</size>
-            </box>
-          </geometry>
-        </visual>
-        <sensor name='front_left_cam_sensor' type='camera'>
-          <update_rate>30</update_rate>
-          <camera name='front_left_cam_camera'>
-            <horizontal_fov>1.39626</horizontal_fov>
-            <image>
-              <width>1280</width>
-              <height>720</height>
-              <format>R8G8B8</format>
-            </image>
-            <clip>
-              <near>0.05</near>
-              <far>300</far>
-            </clip>
-            <noise>
-              <type>gaussian</type>
-              <mean>0</mean>
-              <stddev>0.007</stddev>
-            </noise>
-          </camera>
-          <plugin name='camera_plugin_front_left_cam' filename='libgazebo_ros_camera.so'>
-            <alwaysOn>1</alwaysOn>
-            <updateRate>0.0</updateRate>
-            <cameraName>wamv/sensors/camera/front_left_cam</cameraName>
-            <imageTopicName>image_raw</imageTopicName>
-            <cameraInfoTopicName>camera_info</cameraInfoTopicName>
-            <frameName>wamv/front_left_cam_link_optical</frameName>
-            <hackBaseline>0.07</hackBaseline>
-            <distortionK1>0.0</distortionK1>
-            <distortionK2>0.0</distortionK2>
-            <distortionK3>0.0</distortionK3>
-            <distortionT1>0.0</distortionT1>
-            <distortionT2>0.0</distortionT2>
-            <robotNamespace>/</robotNamespace>
-          </plugin>
-          <pose>0.97 0.1 1.5 0 0.20944 0</pose>
-        </sensor>
-        <sensor name='front_right_cam_sensor' type='camera'>
-          <update_rate>30</update_rate>
-          <camera name='front_right_cam_camera'>
-            <horizontal_fov>1.39626</horizontal_fov>
-            <image>
-              <width>1280</width>
-              <height>720</height>
-              <format>R8G8B8</format>
-            </image>
-            <clip>
-              <near>0.05</near>
-              <far>300</far>
-            </clip>
-            <noise>
-              <type>gaussian</type>
-              <mean>0</mean>
-              <stddev>0.007</stddev>
-            </noise>
-          </camera>
-          <plugin name='camera_plugin_front_right_cam' filename='libgazebo_ros_camera.so'>
-            <alwaysOn>1</alwaysOn>
-            <updateRate>0.0</updateRate>
-            <cameraName>wamv/sensors/camera/front_right_cam</cameraName>
-            <imageTopicName>image_raw</imageTopicName>
-            <cameraInfoTopicName>camera_info</cameraInfoTopicName>
-            <frameName>wamv/front_right_cam_link_optical</frameName>
-            <hackBaseline>0.07</hackBaseline>
-            <distortionK1>0.0</distortionK1>
-            <distortionK2>0.0</distortionK2>
-            <distortionK3>0.0</distortionK3>
-            <distortionT1>0.0</distortionT1>
-            <distortionT2>0.0</distortionT2>
-            <robotNamespace>/</robotNamespace>
-          </plugin>
-          <pose>0.97 -0.1 1.5 0 0.20944 0</pose>
-        </sensor>
-        <sensor name='velodyne_sensor' type='gpu_ray'>
-          <update_rate>10</update_rate>
-          <ray>
-            <scan>
-              <horizontal>
-                <samples>18750</samples>
-                <resolution>0.1</resolution>
-                <min_angle>-3.14159</min_angle>
-                <max_angle>3.14159</max_angle>
-              </horizontal>
-              <vertical>
-                <samples>16</samples>
-                <resolution>1</resolution>
-                <min_angle>-0.261799</min_angle>
-                <max_angle>0.261799</max_angle>
-              </vertical>
-            </scan>
-            <range>
-              <min>0.1</min>
-              <max>130</max>
-            </range>
-            <noise>
-              <type>gaussian</type>
-              <mean>0</mean>
-              <stddev>0.01</stddev>
-            </noise>
-          </ray>
-          <plugin name='velodyne_plugin' filename='libgazebo_ros_velodyne_gpu_laser.so'>
-            <topicName>velodyne_points</topicName>
-            <frameName>velodyne</frameName>
-            <min_intensity>0</min_intensity>
-            <robotNamespace>/</robotNamespace>
-          </plugin>
-          <pose>0.774 0 2 0 0.261799 0</pose>
-        </sensor>
-        <self_collide>0</self_collide>
-        <enable_wind>0</enable_wind>
-        <kinematic>0</kinematic>
-      </link>
-      <joint name='BL_chasis_engine_joint' type='revolute'>
-        <pose relative_to='wamv/base_link'>-1.9304 1.1 0.318237 0 -0 0.785398</pose>
-        <parent>wamv/base_link</parent>
-        <child>BL_engine_link</child>
-        <axis>
-          <xyz>0 0 1</xyz>
-          <limit>
-            <lower>-3.14159</lower>
-            <upper>3.14159</upper>
-            <effort>10</effort>
-            <velocity>10</velocity>
-          </limit>
-          <dynamics>
-            <spring_reference>0</spring_reference>
-            <spring_stiffness>0</spring_stiffness>
-          </dynamics>
-        </axis>
-      </joint>
-      <link name='BL_engine_link'>
-        <pose relative_to='BL_chasis_engine_joint'>0 0 0 0 -0 0</pose>
-        <inertial>
-          <pose>0 0 0 0 -0 0</pose>
-          <mass>15</mass>
-          <inertia>
-            <ixx>0.889245</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>0.911125</iyy>
-            <iyz>0</iyz>
-            <izz>0.078125</izz>
-          </inertia>
-        </inertial>
-        <collision name='BL_engine_link_fixed_joint_lump__BL_engine_vertical_axis_collision_collision'>
-          <pose>-0.16 0 -0.24 0 -0 0</pose>
-          <geometry>
-            <box>
-              <size>0.2 0.15 0.83</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='BL_engine_link_fixed_joint_lump__BL_engine_rear_end_collision_collision_1'>
-          <pose>-0.34 0 0.12 0 -0 0</pose>
-          <geometry>
-            <box>
-              <size>0.12 0.15 0.12</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <visual name='BL_engine_link_visual'>
-          <pose>0 0 0 0 -0 0</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/wamv_description/models/engine/mesh/engine.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <self_collide>0</self_collide>
-        <enable_wind>0</enable_wind>
-        <kinematic>0</kinematic>
-      </link>
-      <joint name='BL_engine_propeller_joint' type='revolute'>
-        <pose relative_to='BL_engine_link'>-0.278156 0 -0.509371 0 -0 0</pose>
-        <parent>BL_engine_link</parent>
-        <child>BL_propeller_link</child>
-        <axis>
-          <xyz>1 0 0</xyz>
-          <limit>
-            <effort>400</effort>
-            <velocity>100</velocity>
-            <lower>-1e+16</lower>
-            <upper>1e+16</upper>
-          </limit>
-          <dynamics>
-            <damping>0.05</damping>
-            <friction>0.05</friction>
-            <spring_reference>0</spring_reference>
-            <spring_stiffness>0</spring_stiffness>
-          </dynamics>
-        </axis>
-      </joint>
-      <link name='BL_propeller_link'>
-        <pose relative_to='BL_engine_propeller_joint'>0 0 0 0 -0 0</pose>
-        <inertial>
-          <pose>0 0 0 0 -0 0</pose>
-          <mass>0.5</mass>
-          <inertia>
-            <ixx>0.008545</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>0.008545</iyy>
-            <iyz>0</iyz>
-            <izz>0.0144</izz>
-          </inertia>
-        </inertial>
-        <collision name='BL_propeller_link_fixed_joint_lump__BL_propeller_collision_collision'>
-          <pose>-0.08 0 0 3.14159 1.57079 3.14159</pose>
-          <geometry>
-            <cylinder>
-              <length>0.18</length>
-              <radius>0.24</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <visual name='BL_propeller_link_visual'>
-          <pose>0 0 0 0 -0 0</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/wamv_description/models/propeller/mesh/propeller.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <self_collide>0</self_collide>
-        <enable_wind>0</enable_wind>
-        <kinematic>0</kinematic>
-      </link>
-      <joint name='BR_chasis_engine_joint' type='revolute'>
-        <pose relative_to='wamv/base_link'>-1.9304 -1.1 0.318237 0 0 -0.785398</pose>
-        <parent>wamv/base_link</parent>
-        <child>BR_engine_link</child>
-        <axis>
-          <xyz>0 0 1</xyz>
-          <limit>
-            <lower>-3.14159</lower>
-            <upper>3.14159</upper>
-            <effort>10</effort>
-            <velocity>10</velocity>
-          </limit>
-          <dynamics>
-            <spring_reference>0</spring_reference>
-            <spring_stiffness>0</spring_stiffness>
-          </dynamics>
-        </axis>
-      </joint>
-      <link name='BR_engine_link'>
-        <pose relative_to='BR_chasis_engine_joint'>0 0 0 0 -0 0</pose>
-        <inertial>
-          <pose>0 0 0 0 -0 0</pose>
-          <mass>15</mass>
-          <inertia>
-            <ixx>0.889245</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>0.911125</iyy>
-            <iyz>0</iyz>
-            <izz>0.078125</izz>
-          </inertia>
-        </inertial>
-        <collision name='BR_engine_link_fixed_joint_lump__BR_engine_vertical_axis_collision_collision'>
-          <pose>-0.16 0 -0.24 0 -0 0</pose>
-          <geometry>
-            <box>
-              <size>0.2 0.15 0.83</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='BR_engine_link_fixed_joint_lump__BR_engine_rear_end_collision_collision_1'>
-          <pose>-0.34 0 0.12 0 -0 0</pose>
-          <geometry>
-            <box>
-              <size>0.12 0.15 0.12</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <visual name='BR_engine_link_visual'>
-          <pose>0 0 0 0 -0 0</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/wamv_description/models/engine/mesh/engine.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <self_collide>0</self_collide>
-        <enable_wind>0</enable_wind>
-        <kinematic>0</kinematic>
-      </link>
-      <joint name='BR_engine_propeller_joint' type='revolute'>
-        <pose relative_to='BR_engine_link'>-0.278156 0 -0.509371 0 -0 0</pose>
-        <parent>BR_engine_link</parent>
-        <child>BR_propeller_link</child>
-        <axis>
-          <xyz>1 0 0</xyz>
-          <limit>
-            <effort>400</effort>
-            <velocity>100</velocity>
-            <lower>-1e+16</lower>
-            <upper>1e+16</upper>
-          </limit>
-          <dynamics>
-            <damping>0.05</damping>
-            <friction>0.05</friction>
-            <spring_reference>0</spring_reference>
-            <spring_stiffness>0</spring_stiffness>
-          </dynamics>
-        </axis>
-      </joint>
-      <link name='BR_propeller_link'>
-        <pose relative_to='BR_engine_propeller_joint'>0 0 0 0 -0 0</pose>
-        <inertial>
-          <pose>0 0 0 0 -0 0</pose>
-          <mass>0.5</mass>
-          <inertia>
-            <ixx>0.008545</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>0.008545</iyy>
-            <iyz>0</iyz>
-            <izz>0.0144</izz>
-          </inertia>
-        </inertial>
-        <collision name='BR_propeller_link_fixed_joint_lump__BR_propeller_collision_collision'>
-          <pose>-0.08 0 0 3.14159 1.57079 3.14159</pose>
-          <geometry>
-            <cylinder>
-              <length>0.18</length>
-              <radius>0.24</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <visual name='BR_propeller_link_visual'>
-          <pose>0 0 0 0 -0 0</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/wamv_description/models/propeller/mesh/propeller.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <self_collide>0</self_collide>
-        <enable_wind>0</enable_wind>
-        <kinematic>0</kinematic>
-      </link>
-      <joint name='FL_chasis_engine_joint' type='revolute'>
-        <pose relative_to='wamv/base_link'>1.46 0.54 0.49675 0 0 -0.785398</pose>
-        <parent>wamv/base_link</parent>
-        <child>FL_engine_link</child>
-        <axis>
-          <xyz>0 0 1</xyz>
-          <limit>
-            <lower>-3.14159</lower>
-            <upper>3.14159</upper>
-            <effort>10</effort>
-            <velocity>10</velocity>
-          </limit>
-          <dynamics>
-            <spring_reference>0</spring_reference>
-            <spring_stiffness>0</spring_stiffness>
-          </dynamics>
-        </axis>
-      </joint>
-      <link name='FL_engine_link'>
-        <pose relative_to='FL_chasis_engine_joint'>0 0 0 0 -0 0</pose>
-        <inertial>
-          <pose>0 0 0 0 -0 0</pose>
-          <mass>15</mass>
-          <inertia>
-            <ixx>0.889245</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>0.911125</iyy>
-            <iyz>0</iyz>
-            <izz>0.078125</izz>
-          </inertia>
-        </inertial>
-        <collision name='FL_engine_link_fixed_joint_lump__FL_engine_vertical_axis_collision_collision'>
-          <pose>-0.16 0 -0.24 0 -0 0</pose>
-          <geometry>
-            <box>
-              <size>0.2 0.15 0.83</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='FL_engine_link_fixed_joint_lump__FL_engine_rear_end_collision_collision_1'>
-          <pose>-0.34 0 0.12 0 -0 0</pose>
-          <geometry>
-            <box>
-              <size>0.12 0.15 0.12</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <visual name='FL_engine_link_visual'>
-          <pose>0 0 0 0 -0 0</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/wamv_description/models/engine/mesh/engine.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <self_collide>0</self_collide>
-        <enable_wind>0</enable_wind>
-        <kinematic>0</kinematic>
-      </link>
-      <joint name='FL_engine_propeller_joint' type='revolute'>
-        <pose relative_to='FL_engine_link'>-0.278156 0 -0.509371 0 -0 0</pose>
-        <parent>FL_engine_link</parent>
-        <child>FL_propeller_link</child>
-        <axis>
-          <xyz>1 0 0</xyz>
-          <limit>
-            <effort>400</effort>
-            <velocity>100</velocity>
-            <lower>-1e+16</lower>
-            <upper>1e+16</upper>
-          </limit>
-          <dynamics>
-            <damping>0.05</damping>
-            <friction>0.05</friction>
-            <spring_reference>0</spring_reference>
-            <spring_stiffness>0</spring_stiffness>
-          </dynamics>
-        </axis>
-      </joint>
-      <link name='FL_propeller_link'>
-        <pose relative_to='FL_engine_propeller_joint'>0 0 0 0 -0 0</pose>
-        <inertial>
-          <pose>0 0 0 0 -0 0</pose>
-          <mass>0.5</mass>
-          <inertia>
-            <ixx>0.008545</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>0.008545</iyy>
-            <iyz>0</iyz>
-            <izz>0.0144</izz>
-          </inertia>
-        </inertial>
-        <collision name='FL_propeller_link_fixed_joint_lump__FL_propeller_collision_collision'>
-          <pose>-0.08 0 0 3.14159 1.57079 3.14159</pose>
-          <geometry>
-            <cylinder>
-              <length>0.18</length>
-              <radius>0.24</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <visual name='FL_propeller_link_visual'>
-          <pose>0 0 0 0 -0 0</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/wamv_description/models/propeller/mesh/propeller.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <self_collide>0</self_collide>
-        <enable_wind>0</enable_wind>
-        <kinematic>0</kinematic>
-      </link>
-      <joint name='FR_chasis_engine_joint' type='revolute'>
-        <pose relative_to='wamv/base_link'>1.46 -0.54 0.49657 0 -0 0.785398</pose>
-        <parent>wamv/base_link</parent>
-        <child>FR_engine_link</child>
-        <axis>
-          <xyz>0 0 1</xyz>
-          <limit>
-            <lower>-3.14159</lower>
-            <upper>3.14159</upper>
-            <effort>10</effort>
-            <velocity>10</velocity>
-          </limit>
-          <dynamics>
-            <spring_reference>0</spring_reference>
-            <spring_stiffness>0</spring_stiffness>
-          </dynamics>
-        </axis>
-      </joint>
-      <link name='FR_engine_link'>
-        <pose relative_to='FR_chasis_engine_joint'>0 0 0 0 -0 0</pose>
-        <inertial>
-          <pose>0 0 0 0 -0 0</pose>
-          <mass>15</mass>
-          <inertia>
-            <ixx>0.889245</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>0.911125</iyy>
-            <iyz>0</iyz>
-            <izz>0.078125</izz>
-          </inertia>
-        </inertial>
-        <collision name='FR_engine_link_fixed_joint_lump__FR_engine_vertical_axis_collision_collision'>
-          <pose>-0.16 0 -0.24 0 -0 0</pose>
-          <geometry>
-            <box>
-              <size>0.2 0.15 0.83</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='FR_engine_link_fixed_joint_lump__FR_engine_rear_end_collision_collision_1'>
-          <pose>-0.34 0 0.12 0 -0 0</pose>
-          <geometry>
-            <box>
-              <size>0.12 0.15 0.12</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <visual name='FR_engine_link_visual'>
-          <pose>0 0 0 0 -0 0</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/wamv_description/models/engine/mesh/engine.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <self_collide>0</self_collide>
-        <enable_wind>0</enable_wind>
-        <kinematic>0</kinematic>
-      </link>
-      <joint name='FR_engine_propeller_joint' type='revolute'>
-        <pose relative_to='FR_engine_link'>-0.278156 0 -0.509371 0 -0 0</pose>
-        <parent>FR_engine_link</parent>
-        <child>FR_propeller_link</child>
-        <axis>
-          <xyz>1 0 0</xyz>
-          <limit>
-            <effort>400</effort>
-            <velocity>100</velocity>
-            <lower>-1e+16</lower>
-            <upper>1e+16</upper>
-          </limit>
-          <dynamics>
-            <damping>0.05</damping>
-            <friction>0.05</friction>
-            <spring_reference>0</spring_reference>
-            <spring_stiffness>0</spring_stiffness>
-          </dynamics>
-        </axis>
-      </joint>
-      <link name='FR_propeller_link'>
-        <pose relative_to='FR_engine_propeller_joint'>0 0 0 0 -0 0</pose>
-        <inertial>
-          <pose>0 0 0 0 -0 0</pose>
-          <mass>0.5</mass>
-          <inertia>
-            <ixx>0.008545</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>0.008545</iyy>
-            <iyz>0</iyz>
-            <izz>0.0144</izz>
-          </inertia>
-        </inertial>
-        <collision name='FR_propeller_link_fixed_joint_lump__FR_propeller_collision_collision'>
-          <pose>-0.08 0 0 3.14159 1.57079 3.14159</pose>
-          <geometry>
-            <cylinder>
-              <length>0.18</length>
-              <radius>0.24</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <visual name='FR_propeller_link_visual'>
-          <pose>0 0 0 0 -0 0</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/wamv_description/models/propeller/mesh/propeller.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <self_collide>0</self_collide>
-        <enable_wind>0</enable_wind>
-        <kinematic>0</kinematic>
-      </link>
-      <joint name='wamv/ball_shooter_base_joint' type='revolute'>
-        <pose relative_to='wamv/base_link'>0.54 0.3 1.3 0 -0 0.1</pose>
-        <parent>wamv/base_link</parent>
-        <child>wamv/ball_shooter_base_link</child>
-        <axis>
-          <xyz>0 0 1</xyz>
-          <limit>
-            <lower>0</lower>
-            <upper>0</upper>
-            <effort>1000</effort>
-            <velocity>0</velocity>
-          </limit>
-          <dynamics>
-            <spring_reference>0</spring_reference>
-            <spring_stiffness>0</spring_stiffness>
-          </dynamics>
-        </axis>
-      </joint>
-      <link name='wamv/ball_shooter_base_link'>
-        <pose relative_to='wamv/ball_shooter_base_joint'>0 0 0 0 -0 0</pose>
-        <inertial>
-          <pose>-0.02 0 0.05 0 -0 0</pose>
-          <mass>0.5</mass>
-          <inertia>
-            <ixx>0.00083</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>0.00052083</iyy>
-            <iyz>0</iyz>
-            <izz>0.00052083</izz>
-          </inertia>
-        </inertial>
-        <visual name='wamv/ball_shooter_base_link_fixed_joint_lump__ball_shooter_base_visual_visual'>
-          <pose>0 0 0 0 -0 0</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/vrx_gazebo/models/ball_shooter/meshes/ball_shooter_base.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <self_collide>0</self_collide>
-        <enable_wind>0</enable_wind>
-        <kinematic>0</kinematic>
-      </link>
-      <joint name='wamv/ball_shooter_launcher_joint' type='revolute'>
-        <pose relative_to='wamv/ball_shooter_base_link'>-0.0204 0 0.107877 0 -0.8 0</pose>
-        <parent>wamv/ball_shooter_base_link</parent>
-        <child>wamv/ball_shooter_launcher_link</child>
-        <axis>
-          <xyz>0 0 1</xyz>
-          <limit>
-            <lower>0</lower>
-            <upper>0</upper>
-            <effort>1000</effort>
-            <velocity>0</velocity>
-          </limit>
-          <dynamics>
-            <spring_reference>0</spring_reference>
-            <spring_stiffness>0</spring_stiffness>
-          </dynamics>
-        </axis>
-      </joint>
-      <link name='wamv/ball_shooter_launcher_link'>
-        <pose relative_to='wamv/ball_shooter_launcher_joint'>0 0 0 0 -0 0</pose>
-        <inertial>
-          <pose>0 0 0 3.14159 -1.57079 3.14159</pose>
-          <mass>0.5</mass>
-          <inertia>
-            <ixx>0.00177917</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>0.00177917</iyy>
-            <iyz>0</iyz>
-            <izz>0.000225</izz>
-          </inertia>
-        </inertial>
-        <collision name='wamv/ball_shooter_launcher_link_fixed_joint_lump__ball_shooter_collision_collision'>
-          <pose>0 0 0 0 0.261799 0</pose>
-          <geometry>
-            <box>
-              <size>0.3 0.3 0.2</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <visual name='wamv/ball_shooter_launcher_link_fixed_joint_lump__ball_shooter_launcher_visual_visual'>
-          <pose>0 0 0 0 0.261799 0</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/vrx_gazebo/models/ball_shooter/meshes/ball_shooter_launcher.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <self_collide>0</self_collide>
-        <enable_wind>0</enable_wind>
-        <kinematic>0</kinematic>
-      </link>
-      <joint name='wamv/imu_wamv_joint' type='revolute'>
-        <pose relative_to='wamv/base_link'>0.24 0.17 1.5 0 -0 0</pose>
-        <parent>wamv/base_link</parent>
-        <child>wamv/imu_wamv_link</child>
-        <axis>
-          <xyz>0 0 1</xyz>
-          <limit>
-            <lower>0</lower>
-            <upper>0</upper>
-            <effort>1000</effort>
-            <velocity>0</velocity>
-          </limit>
-          <dynamics>
-            <spring_reference>0</spring_reference>
-            <spring_stiffness>0</spring_stiffness>
-          </dynamics>
-        </axis>
-      </joint>
-      <link name='wamv/imu_wamv_link'>
-        <pose relative_to='wamv/imu_wamv_joint'>0 0 0 0 -0 0</pose>
-        <inertial>
-          <pose>0 0 0 0 -0 0</pose>
-          <mass>0.1</mass>
-          <inertia>
-            <ixx>8.3e-05</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>8.3e-05</iyy>
-            <iyz>0</iyz>
-            <izz>0.0125</izz>
-          </inertia>
-        </inertial>
-        <visual name='wamv/imu_wamv_link_fixed_joint_lump__imu_wamv_visual_visual'>
-          <pose>0 0 0 0 -0 0</pose>
-          <geometry>
-            <box>
-              <size>0.05 0.025 0.005</size>
-            </box>
-          </geometry>
-        </visual>
-        <self_collide>0</self_collide>
-        <enable_wind>0</enable_wind>
-        <kinematic>0</kinematic>
-      </link>
-      <joint name='wamv/ins_joint' type='revolute'>
-        <pose relative_to='wamv/base_link'>0.24 0.17 1.5 0 -0 0</pose>
-        <parent>wamv/base_link</parent>
-        <child>wamv/ins_link</child>
-        <axis>
-          <xyz>0 0 1</xyz>
-          <limit>
-            <lower>0</lower>
-            <upper>0</upper>
-            <effort>1000</effort>
-            <velocity>0</velocity>
-          </limit>
-          <dynamics>
-            <spring_reference>0</spring_reference>
-            <spring_stiffness>0</spring_stiffness>
-          </dynamics>
-        </axis>
-      </joint>
-      <link name='wamv/ins_link'>
-        <pose relative_to='wamv/ins_joint'>0 0 0 0 -0 0</pose>
-        <inertial>
-          <pose>0 0 0 0 -0 0</pose>
-          <mass>1</mass>
-          <inertia>
-            <ixx>0.006458</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>0.006458</iyy>
-            <iyz>0</iyz>
-            <izz>0.01125</izz>
-          </inertia>
-        </inertial>
-        <collision name='wamv/ins_link_fixed_joint_lump__ins_collision_base_collision'>
-          <pose>0 0 0.025 0 -0 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.05</length>
-              <radius>0.015</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/ins_link_fixed_joint_lump__ins_collision_antenna_collision_1'>
-          <pose>0 0 0.11 0 -0 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.1</length>
-              <radius>0.15</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <visual name='wamv/ins_link_fixed_joint_lump__ins_visual_visual'>
-          <pose>0 0 0 0 -0 0</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/wamv_gazebo/models/gps/mesh/gps.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <self_collide>0</self_collide>
-        <enable_wind>0</enable_wind>
-        <kinematic>0</kinematic>
-      </link>
-      <plugin name='gps_plugin_ins' filename='libhector_gazebo_ros_gps.so'>
-        <updateRate>15</updateRate>
-        <alwaysOn>1</alwaysOn>
-        <bodyName>wamv/ins_link</bodyName>
-        <frameId>wamv/ins_link</frameId>
-        <topicName>wamv/sensors/gps/gps/fix</topicName>
-        <velocityTopicName>wamv/sensors/gps/gps/fix_velocity</velocityTopicName>
-        <useWorldSphericalCoordinates>1</useWorldSphericalCoordinates>
-        <offset>0.0 0.0 0.0</offset>
-        <drift>0.0 0.0 0.0</drift>
-        <gaussianNoise>0 0 0</gaussianNoise>
-        <velocityOffset>0.0 0.0 0.0</velocityOffset>
-        <velocityDrift>0.0 0.0 0.0</velocityDrift>
-        <velocityGaussianNoise>0.0 0.0 0.0</velocityGaussianNoise>
-        <robotNamespace>/</robotNamespace>
-      </plugin>
-      <plugin name='imu_plugin_imu_wamv' filename='libhector_gazebo_ros_imu.so'>
-        <updateRate>15</updateRate>
-        <bodyName>wamv/imu_wamv_link</bodyName>
-        <topicName>wamv/sensors/imu/imu/data</topicName>
-        <serviceName>wamv/sensors/imu_service</serviceName>
-        <frameId>wamv/imu_wamv_link</frameId>
-        <alwaysOn>1</alwaysOn>
-        <accelOffset>0.0 0.0 0.0</accelOffset>
-        <accelDrift>0.0 0.0 0.0</accelDrift>
-        <accelDriftFrequency>0.00027 0.00027 0.000027</accelDriftFrequency>
-        <accelGaussianNoise>0.0 0.0 0.0</accelGaussianNoise>
-        <rateOffset>0.0 0.0 0.0</rateOffset>
-        <rateDrift>0.0 0.0 0.0</rateDrift>
-        <rateDriftFrequency>0.00027 0.00027 0.000027</rateDriftFrequency>
-        <rateGaussianNoise>0.0 0.0 0.0</rateGaussianNoise>
-        <yawOffset>0.0</yawOffset>
-        <yawDrift>0.0</yawDrift>
-        <yawDriftFrequency>0.00027</yawDriftFrequency>
-        <yawGaussianNoise>0.0</yawGaussianNoise>
-        <robotNamespace>/</robotNamespace>
-      </plugin>
-      <plugin name='ball_shooter_plugin' filename='libball_shooter_plugin.so'>
-        <projectile>
-          <model_name>blue_projectile</model_name>
-          <link_name>link</link_name>
-          <frame>wamv/ball_shooter_launcher_link</frame>
-          <pose>0.14 0 0 0 0 0</pose>
-        </projectile>
-        <num_shots>4</num_shots>
-        <shot_force>300</shot_force>
-        <topic>wamv/shooters/ball_shooter/fire</topic>
-        <robotNamespace>/</robotNamespace>
-      </plugin>
-      <plugin name='pinger_plugin' filename='libusv_gazebo_acoustic_pinger_plugin.so'>
-        <frameId>wamv/pinger</frameId>
-        <topicName>wamv/sensors/pingers/hydrophones/range_bearing</topicName>
-        <setPositionTopicName>wamv/sensors/pingers/hydrophones/set_pinger_position</setPositionTopicName>
-        <rangeNoise>
-          <noise>
-            <type>gaussian</type>
-            <mean>0.0</mean>
-            <stddev>3.0</stddev>
-          </noise>
-        </rangeNoise>
-        <bearingNoise>
-          <noise>
-            <type>gaussian</type>
-            <mean>0.0</mean>
-            <stddev>0.01</stddev>
-          </noise>
-        </bearingNoise>
-        <elevationNoise>
-          <noise>
-            <type>gaussian</type>
-            <mean>0.0</mean>
-            <stddev>0.01</stddev>
-          </noise>
-        </elevationNoise>
-        <robotNamespace>/</robotNamespace>
-      </plugin>
-      <plugin name='wamv_gazebo_thrust' filename='libusv_gazebo_thrust_plugin.so'>
-        <cmdTimeout>1.0</cmdTimeout>
-        <robotNamespace>wamv</robotNamespace>
-        <thruster>
-          <linkName>BL_propeller_link</linkName>
-          <propJointName>BL_engine_propeller_joint</propJointName>
-          <engineJointName>BL_chasis_engine_joint</engineJointName>
-          <cmdTopic>thrusters/BL_thrust_cmd</cmdTopic>
-          <angleTopic>thrusters/BL_thrust_angle</angleTopic>
-          <enableAngle>1</enableAngle>
-          <mappingType>1</mappingType>
-          <maxCmd>1.0</maxCmd>
-          <maxForceFwd>250.0</maxForceFwd>
-          <maxForceRev>-100.0</maxForceRev>
-          <maxAngle>1.5707963267948966</maxAngle>
-        </thruster>
-        <thruster>
-          <linkName>BR_propeller_link</linkName>
-          <propJointName>BR_engine_propeller_joint</propJointName>
-          <engineJointName>BR_chasis_engine_joint</engineJointName>
-          <cmdTopic>thrusters/BR_thrust_cmd</cmdTopic>
-          <angleTopic>thrusters/BR_thrust_angle</angleTopic>
-          <enableAngle>1</enableAngle>
-          <mappingType>1</mappingType>
-          <maxCmd>1.0</maxCmd>
-          <maxForceFwd>250.0</maxForceFwd>
-          <maxForceRev>-100.0</maxForceRev>
-          <maxAngle>1.5707963267948966</maxAngle>
-        </thruster>
-        <thruster>
-          <linkName>FL_propeller_link</linkName>
-          <propJointName>FL_engine_propeller_joint</propJointName>
-          <engineJointName>FL_chasis_engine_joint</engineJointName>
-          <cmdTopic>thrusters/FL_thrust_cmd</cmdTopic>
-          <angleTopic>thrusters/FL_thrust_angle</angleTopic>
-          <enableAngle>1</enableAngle>
-          <mappingType>1</mappingType>
-          <maxCmd>1.0</maxCmd>
-          <maxForceFwd>250.0</maxForceFwd>
-          <maxForceRev>-100.0</maxForceRev>
-          <maxAngle>1.5707963267948966</maxAngle>
-        </thruster>
-        <thruster>
-          <linkName>FR_propeller_link</linkName>
-          <propJointName>FR_engine_propeller_joint</propJointName>
-          <engineJointName>FR_chasis_engine_joint</engineJointName>
-          <cmdTopic>thrusters/FR_thrust_cmd</cmdTopic>
-          <angleTopic>thrusters/FR_thrust_angle</angleTopic>
-          <enableAngle>1</enableAngle>
-          <mappingType>1</mappingType>
-          <maxCmd>1.0</maxCmd>
-          <maxForceFwd>250.0</maxForceFwd>
-          <maxForceRev>-100.0</maxForceRev>
-          <maxAngle>1.5707963267948966</maxAngle>
-        </thruster>
-      </plugin>
-      <plugin name='usv_dynamics_wamv_dynamics_plugin' filename='libusv_gazebo_dynamics_plugin.so'>
-        <bodyName>wamv/base_link</bodyName>
-        <waterLevel>0</waterLevel>
-        <waterDensity>997.8</waterDensity>
-        <xDotU>0.0</xDotU>
-        <yDotV>0.0</yDotV>
-        <nDotR>0.0</nDotR>
-        <xU>51.3</xU>
-        <xUU>72.4</xUU>
-        <yV>40.0</yV>
-        <yVV>0.0</yVV>
-        <zW>500.0</zW>
-        <kP>50.0</kP>
-        <mQ>50.0</mQ>
-        <nR>400.0</nR>
-        <nRR>0.0</nRR>
-        <hullRadius>0.213</hullRadius>
-        <boatWidth>2.4</boatWidth>
-        <boatLength>4.9</boatLength>
-        <length_n>2</length_n>
-        <wave_model>ocean_waves</wave_model>
-        <robotNamespace>/</robotNamespace>
-      </plugin>
-      <frame name='wamv/front_left_cam_to_front_left_cam_link_optical_joint' attached_to='wamv/front_left_cam_link'>
-        <pose>0 0 0 -1.5708 -0 -1.5708</pose>
-      </frame>
-      <frame name='wamv/front_left_cam_link_optical' attached_to='wamv/front_left_cam_to_front_left_cam_link_optical_joint'/>
-      <frame name='wamv/front_left_cam_post_arm_to_front_left_cam_joint' attached_to='wamv/front_left_cam_post_arm_link'>
-        <pose>0.02 0 0.027 0 0.20944 0</pose>
-      </frame>
-      <frame name='wamv/front_left_cam_link' attached_to='wamv/front_left_cam_post_arm_to_front_left_cam_joint'/>
-      <frame name='wamv/front_left_cam_post_to_front_left_cam_post_arm_joint' attached_to='wamv/front_left_cam_post_link'>
-        <pose>0.03 0 0.08825 0 -0 0</pose>
-      </frame>
-      <frame name='wamv/front_left_cam_post_arm_link' attached_to='wamv/front_left_cam_post_to_front_left_cam_post_arm_joint'/>
-      <frame name='wamv/base_to_front_left_cam_post_joint' attached_to='wamv/base_link'>
-        <pose>0.92 0.1 1.38475 0 -0 0</pose>
-      </frame>
-      <frame name='wamv/front_left_cam_post_link' attached_to='wamv/base_to_front_left_cam_post_joint'/>
-      <frame name='wamv/front_right_cam_to_front_right_cam_link_optical_joint' attached_to='wamv/front_right_cam_link'>
-        <pose>0 0 0 -1.5708 -0 -1.5708</pose>
-      </frame>
-      <frame name='wamv/front_right_cam_link_optical' attached_to='wamv/front_right_cam_to_front_right_cam_link_optical_joint'/>
-      <frame name='wamv/front_right_cam_post_arm_to_front_right_cam_joint' attached_to='wamv/front_right_cam_post_arm_link'>
-        <pose>0.02 0 0.027 0 0.20944 0</pose>
-      </frame>
-      <frame name='wamv/front_right_cam_link' attached_to='wamv/front_right_cam_post_arm_to_front_right_cam_joint'/>
-      <frame name='wamv/front_right_cam_post_to_front_right_cam_post_arm_joint' attached_to='wamv/front_right_cam_post_link'>
-        <pose>0.03 0 0.08825 0 -0 0</pose>
-      </frame>
-      <frame name='wamv/front_right_cam_post_arm_link' attached_to='wamv/front_right_cam_post_to_front_right_cam_post_arm_joint'/>
-      <frame name='wamv/base_to_front_right_cam_post_joint' attached_to='wamv/base_link'>
-        <pose>0.92 -0.1 1.38475 0 -0 0</pose>
-      </frame>
-      <frame name='wamv/front_right_cam_post_link' attached_to='wamv/base_to_front_right_cam_post_joint'/>
-      <frame name='wamv/velodyne_post_arm_to_velodyne_joint' attached_to='velodyne_post_arm_link'>
-        <pose>0.04 0 0.05 0 0.261799 0</pose>
-      </frame>
-      <frame name='velodyne' attached_to='wamv/velodyne_post_arm_to_velodyne_joint'/>
-      <frame name='wamv/velodyne_post_to_velodyne_post_arm_joint' attached_to='velodyne_post_link'>
-        <pose>0.03 0 0.32675 0 -0 0</pose>
-      </frame>
-      <frame name='velodyne_post_arm_link' attached_to='wamv/velodyne_post_to_velodyne_post_arm_joint'/>
-      <frame name='wamv/base_to_velodyne_post_joint' attached_to='wamv/base_link'>
-        <pose>0.704 0 1.62325 0 -0 0</pose>
-      </frame>
-      <frame name='velodyne_post_link' attached_to='wamv/base_to_velodyne_post_joint'/>
-      <frame name='wamv/dummy_joint' attached_to='wamv/base_link'>
-        <pose>0 0 0 0 -0 0</pose>
-      </frame>
-      <frame name='wamv/dummy_link' attached_to='wamv/dummy_joint'/>
-      <frame name='wamv/hydrophones_pinger_joint' attached_to='wamv/base_link'>
-        <pose>1 0 -1 0 -0 0</pose>
-      </frame>
-      <frame name='wamv/hydrophones' attached_to='wamv/hydrophones_pinger_joint'/>
-      <pose>-527 167 0.1 0 -0 1</pose>
-    </model>
-    <model name='polyform_a2_red'>
+    <model name='red_circular_buoy'>
       <static>0</static>
       <pose>-522.515 184.985 0 0 -0 0</pose>
       <link name='base_link'>
@@ -3634,7 +1508,7 @@
         </buoyancy>
       </plugin>
     </model>
-    <model name='polyform_a2_green'>
+    <model name='green_circular_buoy'>
       <static>0</static>
       <pose>-534.625 187.168 0 0 -0 0</pose>
       <link name='base_link'>
@@ -3703,7 +1577,7 @@
         </buoyancy>
       </plugin>
     </model>
-    <model name='polyform_a2_blue'>
+    <model name='blue_circular_buoy'>
       <static>0</static>
       <pose>-531.036 180.328 0 0 -0 0</pose>
       <link name='base_link'>
@@ -3947,7 +1821,7 @@
           <wrench>-0.091156 -0.062377 -0.000132 0 -0 0</wrench>
         </link>
       </model>
-      <model name='polyform_a2_blue'>
+      <model name='blue_circular_buoy'>
         <pose>-531.036 180.328 0.10507 0 -0 0</pose>
         <scale>1 1 1</scale>
         <link name='base_link'>
@@ -3957,7 +1831,7 @@
           <wrench>0 0 0 0 -0 0</wrench>
         </link>
       </model>
-      <model name='polyform_a2_green'>
+      <model name='green_circular_buoy'>
         <pose>-534.625 187.168 0.10507 0 -0 0</pose>
         <scale>1 1 1</scale>
         <link name='base_link'>
@@ -3967,7 +1841,7 @@
           <wrench>0 0 0 0 -0 0</wrench>
         </link>
       </model>
-      <model name='polyform_a2_red'>
+      <model name='red_circular_buoy'>
         <pose>-522.515 184.985 0.10507 0 -0 0</pose>
         <scale>1 1 1</scale>
         <link name='base_link'>
diff --git a/NaviGator/simulation/navigator_gazebo/worlds/wildlife/wildlife_encounter_test_case_2.world b/NaviGator/simulation/navigator_gazebo/worlds/wildlife/wildlife_encounter_test_case_2.world
index 4c862bf0b..99722fb02 100644
--- a/NaviGator/simulation/navigator_gazebo/worlds/wildlife/wildlife_encounter_test_case_2.world
+++ b/NaviGator/simulation/navigator_gazebo/worlds/wildlife/wildlife_encounter_test_case_2.world
@@ -1439,2133 +1439,7 @@
       <real_time_update_rate>1000</real_time_update_rate>
     </physics>
     <wind/>
-    <model name='wamv'>
-      <link name='wamv/base_link'>
-        <inertial>
-          <pose>0.012722 0 0.026171 0 -0 0</pose>
-          <mass>182.806</mass>
-          <inertia>
-            <ixx>128.232</ixx>
-            <ixy>-2.4178e-21</ixy>
-            <ixz>-3.85993</ixz>
-            <iyy>403.15</iyy>
-            <iyz>-9.28107e-22</iyz>
-            <izz>447.942</izz>
-          </inertia>
-        </inertial>
-        <collision name='wamv/base_link_fixed_joint_lump__front_left_cam_post_collision_collision'>
-          <pose>0.92 0.1 1.38475 0 -0 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.1765</length>
-              <radius>0.0076</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__front_left_cam_post_arm_collision_collision_1'>
-          <pose>0.95 0.1 1.473 -1.20428 -0 -1.5708</pose>
-          <geometry>
-            <cylinder>
-              <length>0.065</length>
-              <radius>0.008</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__front_left_cam_lens_collision_collision_2'>
-          <pose>0.987607 0.1 1.49626 3.14159 1.36136 3.14159</pose>
-          <geometry>
-            <cylinder>
-              <length>0.042</length>
-              <radius>0.015</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__front_left_cam_box_collision_collision_3'>
-          <pose>0.949948 0.1 1.50426 0 0.20944 0</pose>
-          <geometry>
-            <box>
-              <size>0.036 0.03 0.03</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__front_right_cam_post_collision_collision_4'>
-          <pose>0.92 -0.1 1.38475 0 -0 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.1765</length>
-              <radius>0.0076</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__front_right_cam_post_arm_collision_collision_5'>
-          <pose>0.95 -0.1 1.473 -1.20428 -0 -1.5708</pose>
-          <geometry>
-            <cylinder>
-              <length>0.065</length>
-              <radius>0.008</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__front_right_cam_lens_collision_collision_6'>
-          <pose>0.987607 -0.1 1.49626 3.14159 1.36136 3.14159</pose>
-          <geometry>
-            <cylinder>
-              <length>0.042</length>
-              <radius>0.015</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__front_right_cam_box_collision_collision_7'>
-          <pose>0.949948 -0.1 1.50426 0 0.20944 0</pose>
-          <geometry>
-            <box>
-              <size>0.036 0.03 0.03</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__velodyne_post_collision_collision_8'>
-          <pose>0.704 0 1.62325 0 -0 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.6535</length>
-              <radius>0.0076</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__velodyne_post_arm_collision_collision_9'>
-          <pose>0.734 0 1.95 -1.20428 -0 -1.5708</pose>
-          <geometry>
-            <cylinder>
-              <length>0.065</length>
-              <radius>0.008</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__velodyne_collision_collision_10'>
-          <pose>0.774 0 2 0 0.261799 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.075</length>
-              <radius>0.055</radius>
-            </cylinder>
-          </geometry>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <friction>
-              <ode/>
-              <torsional>
-                <ode/>
-              </torsional>
-            </friction>
-            <bounce/>
-          </surface>
-          <max_contacts>10</max_contacts>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__left_float_collision_11'>
-          <pose>-0.4 1.03 0.2 3.14159 1.57079 3.14159</pose>
-          <geometry>
-            <cylinder>
-              <length>4</length>
-              <radius>0.2</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__left_mid_float_collision_12'>
-          <pose>1.85 1.03 0.3 0 1.38 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.5</length>
-              <radius>0.17</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__left_front_float_collision_13'>
-          <pose>2.3 1.03 0.4 0 1.3 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.45</length>
-              <radius>0.12</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__front_left_beam_lower_collision_14'>
-          <pose>0.9 0.85 1 0.78 -0 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.5</length>
-              <radius>0.04</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__front_left_beam_upper_collision_15'>
-          <pose>0.9 0.6 1.18 1.35 -0 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.2</length>
-              <radius>0.04</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__mid_left_beam_lower_collision_16'>
-          <pose>-0.65 0.99 0.7 0.1 0.25 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.45</length>
-              <radius>0.03</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__mid_left_beam_medium_collision_17'>
-          <pose>-0.57 0.87 1.05 0.75 0.25 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.32</length>
-              <radius>0.03</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__mid_left_beam_upper_collision_18'>
-          <pose>-0.55 0.65 1.17 1.35 0.25 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.3</length>
-              <radius>0.03</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__rear_left_beam_lower_collision_19'>
-          <pose>-0.74 1.03 0.7 0 -0.15 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.45</length>
-              <radius>0.03</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__rear_left_beam_medium_collision_20'>
-          <pose>-0.79 0.91 1.05 0.75 -0.15 -0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.32</length>
-              <radius>0.03</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__rear_left_beam_upper_collision_21'>
-          <pose>-0.81 0.67 1.18 1.45 -0.15 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.3</length>
-              <radius>0.03</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__left_joint_collision_22'>
-          <pose>0.58 1.03 0.6 0 -0.6 0</pose>
-          <geometry>
-            <box>
-              <size>0.65 0.2 0.1</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__right_float_collision_23'>
-          <pose>-0.4 -1.03 0.2 3.14159 1.57079 3.14159</pose>
-          <geometry>
-            <cylinder>
-              <length>4</length>
-              <radius>0.2</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__right_mid_float_collision_24'>
-          <pose>1.85 -1.03 0.3 0 1.38 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.5</length>
-              <radius>0.17</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__right_front_float_collision_25'>
-          <pose>2.3 -1.03 0.4 0 1.3 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.45</length>
-              <radius>0.12</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__front_right_beam_lower_collision_26'>
-          <pose>0.9 -0.85 1 -0.78 0 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.5</length>
-              <radius>0.04</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__front_right_beam_upper_collision_27'>
-          <pose>0.9 -0.6 1.18 -1.35 0 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.2</length>
-              <radius>0.04</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__mid_right_beam_lower_collision_28'>
-          <pose>-0.65 -0.99 0.7 -0.1 0.25 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.45</length>
-              <radius>0.03</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__mid_right_beam_medium_collision_29'>
-          <pose>-0.57 -0.87 1.05 -0.75 0.25 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.32</length>
-              <radius>0.03</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__mid_right_beam_upper_collision_30'>
-          <pose>-0.55 -0.65 1.17 -1.35 0.25 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.3</length>
-              <radius>0.03</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__rear_right_beam_lower_collision_31'>
-          <pose>-0.74 -1.03 0.7 0 -0.15 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.45</length>
-              <radius>0.03</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__rear_right_beam_medium_collision_32'>
-          <pose>-0.79 -0.91 1.05 -0.75 -0.15 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.32</length>
-              <radius>0.03</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__rear_right_beam_upper_collision_33'>
-          <pose>-0.81 -0.67 1.18 -1.45 -0.15 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.3</length>
-              <radius>0.03</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__right_joint_collision_34'>
-          <pose>0.58 -1.03 0.6 0 -0.6 0</pose>
-          <geometry>
-            <box>
-              <size>0.65 0.2 0.1</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__top_base_collision_35'>
-          <pose>0 0 1.25 0 -0 0</pose>
-          <geometry>
-            <box>
-              <size>1.85 1 0.1</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__hydrophones_collision_collision_36'>
-          <pose>1 0 -1 0 -0 0</pose>
-          <geometry>
-            <box>
-              <size>0.1 0.1 0.1</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <visual name='wamv/base_link_fixed_joint_lump__wamv/front_left_cam_post_link_visual'>
-          <pose>0.92 0.1 1.38475 0 -0 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.1765</length>
-              <radius>0.0076</radius>
-            </cylinder>
-          </geometry>
-          <material>
-            <script>
-              <name>Gazebo/Black</name>
-              <uri>file://media/materials/scripts/gazebo.material</uri>
-            </script>
-          </material>
-        </visual>
-        <visual name='wamv/base_link_fixed_joint_lump__front_left_cam_post_arm_visual_visual_1'>
-          <pose>0.912 0.1 1.47 -1.0472 -0 -1.5708</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/vrx_gazebo/models/sensor_post/mesh/sensor_post_arm.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <visual name='wamv/base_link_fixed_joint_lump__front_left_cam_visual_visual_2'>
-          <pose>0.937721 0.1 1.50686 0.20944 -0 1.5708</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/vrx_gazebo/models/mono_camera/mesh/mono_camera.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <visual name='wamv/base_link_fixed_joint_lump__wamv/front_right_cam_post_link_visual_3'>
-          <pose>0.92 -0.1 1.38475 0 -0 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.1765</length>
-              <radius>0.0076</radius>
-            </cylinder>
-          </geometry>
-          <material>
-            <script>
-              <name>Gazebo/Black</name>
-              <uri>file://media/materials/scripts/gazebo.material</uri>
-            </script>
-          </material>
-        </visual>
-        <visual name='wamv/base_link_fixed_joint_lump__front_right_cam_post_arm_visual_visual_4'>
-          <pose>0.912 -0.1 1.47 -1.0472 -0 -1.5708</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/vrx_gazebo/models/sensor_post/mesh/sensor_post_arm.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <visual name='wamv/base_link_fixed_joint_lump__front_right_cam_visual_visual_5'>
-          <pose>0.937721 -0.1 1.50686 0.20944 -0 1.5708</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/vrx_gazebo/models/mono_camera/mesh/mono_camera.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <visual name='wamv/base_link_fixed_joint_lump__velodyne_post_link_visual_6'>
-          <pose>0.704 0 1.62325 0 -0 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.6535</length>
-              <radius>0.0076</radius>
-            </cylinder>
-          </geometry>
-          <material>
-            <script>
-              <name>Gazebo/Black</name>
-              <uri>file://media/materials/scripts/gazebo.material</uri>
-            </script>
-          </material>
-        </visual>
-        <visual name='wamv/base_link_fixed_joint_lump__velodyne_post_arm_visual_visual_7'>
-          <pose>0.696 0 1.947 -1.0472 -0 -1.5708</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/vrx_gazebo/models/sensor_post/mesh/sensor_post_arm.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <visual name='wamv/base_link_fixed_joint_lump__velodyne_visual_visual_8'>
-          <pose>0.764941 0 1.96619 0 0.261799 0</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/vrx_gazebo/models/3d_lidar/mesh/3d_lidar.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <visual name='wamv/base_link_fixed_joint_lump__wamv/dummy_link_visual_9'>
-          <pose>0 0 0 0 -0 0</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/wamv_description/models/WAM-V-Base/mesh/WAM-V-Base.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <visual name='wamv/base_link_fixed_joint_lump__wamv/hydrophones_visual_10'>
-          <pose>1 0 -1 0 -0 0</pose>
-          <geometry>
-            <box>
-              <size>0.1 0.1 0.1</size>
-            </box>
-          </geometry>
-        </visual>
-        <sensor name='front_left_cam_sensor' type='camera'>
-          <update_rate>30</update_rate>
-          <camera name='front_left_cam_camera'>
-            <horizontal_fov>1.39626</horizontal_fov>
-            <image>
-              <width>1280</width>
-              <height>720</height>
-              <format>R8G8B8</format>
-            </image>
-            <clip>
-              <near>0.05</near>
-              <far>300</far>
-            </clip>
-            <noise>
-              <type>gaussian</type>
-              <mean>0</mean>
-              <stddev>0.007</stddev>
-            </noise>
-          </camera>
-          <plugin name='camera_plugin_front_left_cam' filename='libgazebo_ros_camera.so'>
-            <alwaysOn>1</alwaysOn>
-            <updateRate>0.0</updateRate>
-            <cameraName>wamv/sensors/camera/front_left_cam</cameraName>
-            <imageTopicName>image_raw</imageTopicName>
-            <cameraInfoTopicName>camera_info</cameraInfoTopicName>
-            <frameName>wamv/front_left_cam_link_optical</frameName>
-            <hackBaseline>0.07</hackBaseline>
-            <distortionK1>0.0</distortionK1>
-            <distortionK2>0.0</distortionK2>
-            <distortionK3>0.0</distortionK3>
-            <distortionT1>0.0</distortionT1>
-            <distortionT2>0.0</distortionT2>
-            <robotNamespace>/</robotNamespace>
-          </plugin>
-          <pose>0.97 0.1 1.5 0 0.20944 0</pose>
-        </sensor>
-        <sensor name='front_right_cam_sensor' type='camera'>
-          <update_rate>30</update_rate>
-          <camera name='front_right_cam_camera'>
-            <horizontal_fov>1.39626</horizontal_fov>
-            <image>
-              <width>1280</width>
-              <height>720</height>
-              <format>R8G8B8</format>
-            </image>
-            <clip>
-              <near>0.05</near>
-              <far>300</far>
-            </clip>
-            <noise>
-              <type>gaussian</type>
-              <mean>0</mean>
-              <stddev>0.007</stddev>
-            </noise>
-          </camera>
-          <plugin name='camera_plugin_front_right_cam' filename='libgazebo_ros_camera.so'>
-            <alwaysOn>1</alwaysOn>
-            <updateRate>0.0</updateRate>
-            <cameraName>wamv/sensors/camera/front_right_cam</cameraName>
-            <imageTopicName>image_raw</imageTopicName>
-            <cameraInfoTopicName>camera_info</cameraInfoTopicName>
-            <frameName>wamv/front_right_cam_link_optical</frameName>
-            <hackBaseline>0.07</hackBaseline>
-            <distortionK1>0.0</distortionK1>
-            <distortionK2>0.0</distortionK2>
-            <distortionK3>0.0</distortionK3>
-            <distortionT1>0.0</distortionT1>
-            <distortionT2>0.0</distortionT2>
-            <robotNamespace>/</robotNamespace>
-          </plugin>
-          <pose>0.97 -0.1 1.5 0 0.20944 0</pose>
-        </sensor>
-        <sensor name='velodyne_sensor' type='gpu_ray'>
-          <update_rate>10</update_rate>
-          <ray>
-            <scan>
-              <horizontal>
-                <samples>18750</samples>
-                <resolution>0.1</resolution>
-                <min_angle>-3.14159</min_angle>
-                <max_angle>3.14159</max_angle>
-              </horizontal>
-              <vertical>
-                <samples>16</samples>
-                <resolution>1</resolution>
-                <min_angle>-0.261799</min_angle>
-                <max_angle>0.261799</max_angle>
-              </vertical>
-            </scan>
-            <range>
-              <min>0.1</min>
-              <max>130</max>
-            </range>
-            <noise>
-              <type>gaussian</type>
-              <mean>0</mean>
-              <stddev>0.01</stddev>
-            </noise>
-          </ray>
-          <plugin name='velodyne_plugin' filename='libgazebo_ros_velodyne_gpu_laser.so'>
-            <topicName>velodyne_points</topicName>
-            <frameName>velodyne</frameName>
-            <min_intensity>0</min_intensity>
-            <robotNamespace>/</robotNamespace>
-          </plugin>
-          <pose>0.774 0 2 0 0.261799 0</pose>
-        </sensor>
-        <self_collide>0</self_collide>
-        <enable_wind>0</enable_wind>
-        <kinematic>0</kinematic>
-      </link>
-      <joint name='BL_chasis_engine_joint' type='revolute'>
-        <pose relative_to='wamv/base_link'>-1.9304 1.1 0.318237 0 -0 0.785398</pose>
-        <parent>wamv/base_link</parent>
-        <child>BL_engine_link</child>
-        <axis>
-          <xyz>0 0 1</xyz>
-          <limit>
-            <lower>-3.14159</lower>
-            <upper>3.14159</upper>
-            <effort>10</effort>
-            <velocity>10</velocity>
-          </limit>
-          <dynamics>
-            <spring_reference>0</spring_reference>
-            <spring_stiffness>0</spring_stiffness>
-          </dynamics>
-        </axis>
-      </joint>
-      <link name='BL_engine_link'>
-        <pose relative_to='BL_chasis_engine_joint'>0 0 0 0 -0 0</pose>
-        <inertial>
-          <pose>0 0 0 0 -0 0</pose>
-          <mass>15</mass>
-          <inertia>
-            <ixx>0.889245</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>0.911125</iyy>
-            <iyz>0</iyz>
-            <izz>0.078125</izz>
-          </inertia>
-        </inertial>
-        <collision name='BL_engine_link_fixed_joint_lump__BL_engine_vertical_axis_collision_collision'>
-          <pose>-0.16 0 -0.24 0 -0 0</pose>
-          <geometry>
-            <box>
-              <size>0.2 0.15 0.83</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='BL_engine_link_fixed_joint_lump__BL_engine_rear_end_collision_collision_1'>
-          <pose>-0.34 0 0.12 0 -0 0</pose>
-          <geometry>
-            <box>
-              <size>0.12 0.15 0.12</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <visual name='BL_engine_link_visual'>
-          <pose>0 0 0 0 -0 0</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/wamv_description/models/engine/mesh/engine.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <self_collide>0</self_collide>
-        <enable_wind>0</enable_wind>
-        <kinematic>0</kinematic>
-      </link>
-      <joint name='BL_engine_propeller_joint' type='revolute'>
-        <pose relative_to='BL_engine_link'>-0.278156 0 -0.509371 0 -0 0</pose>
-        <parent>BL_engine_link</parent>
-        <child>BL_propeller_link</child>
-        <axis>
-          <xyz>1 0 0</xyz>
-          <limit>
-            <effort>400</effort>
-            <velocity>100</velocity>
-            <lower>-1e+16</lower>
-            <upper>1e+16</upper>
-          </limit>
-          <dynamics>
-            <damping>0.05</damping>
-            <friction>0.05</friction>
-            <spring_reference>0</spring_reference>
-            <spring_stiffness>0</spring_stiffness>
-          </dynamics>
-        </axis>
-      </joint>
-      <link name='BL_propeller_link'>
-        <pose relative_to='BL_engine_propeller_joint'>0 0 0 0 -0 0</pose>
-        <inertial>
-          <pose>0 0 0 0 -0 0</pose>
-          <mass>0.5</mass>
-          <inertia>
-            <ixx>0.008545</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>0.008545</iyy>
-            <iyz>0</iyz>
-            <izz>0.0144</izz>
-          </inertia>
-        </inertial>
-        <collision name='BL_propeller_link_fixed_joint_lump__BL_propeller_collision_collision'>
-          <pose>-0.08 0 0 3.14159 1.57079 3.14159</pose>
-          <geometry>
-            <cylinder>
-              <length>0.18</length>
-              <radius>0.24</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <visual name='BL_propeller_link_visual'>
-          <pose>0 0 0 0 -0 0</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/wamv_description/models/propeller/mesh/propeller.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <self_collide>0</self_collide>
-        <enable_wind>0</enable_wind>
-        <kinematic>0</kinematic>
-      </link>
-      <joint name='BR_chasis_engine_joint' type='revolute'>
-        <pose relative_to='wamv/base_link'>-1.9304 -1.1 0.318237 0 0 -0.785398</pose>
-        <parent>wamv/base_link</parent>
-        <child>BR_engine_link</child>
-        <axis>
-          <xyz>0 0 1</xyz>
-          <limit>
-            <lower>-3.14159</lower>
-            <upper>3.14159</upper>
-            <effort>10</effort>
-            <velocity>10</velocity>
-          </limit>
-          <dynamics>
-            <spring_reference>0</spring_reference>
-            <spring_stiffness>0</spring_stiffness>
-          </dynamics>
-        </axis>
-      </joint>
-      <link name='BR_engine_link'>
-        <pose relative_to='BR_chasis_engine_joint'>0 0 0 0 -0 0</pose>
-        <inertial>
-          <pose>0 0 0 0 -0 0</pose>
-          <mass>15</mass>
-          <inertia>
-            <ixx>0.889245</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>0.911125</iyy>
-            <iyz>0</iyz>
-            <izz>0.078125</izz>
-          </inertia>
-        </inertial>
-        <collision name='BR_engine_link_fixed_joint_lump__BR_engine_vertical_axis_collision_collision'>
-          <pose>-0.16 0 -0.24 0 -0 0</pose>
-          <geometry>
-            <box>
-              <size>0.2 0.15 0.83</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='BR_engine_link_fixed_joint_lump__BR_engine_rear_end_collision_collision_1'>
-          <pose>-0.34 0 0.12 0 -0 0</pose>
-          <geometry>
-            <box>
-              <size>0.12 0.15 0.12</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <visual name='BR_engine_link_visual'>
-          <pose>0 0 0 0 -0 0</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/wamv_description/models/engine/mesh/engine.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <self_collide>0</self_collide>
-        <enable_wind>0</enable_wind>
-        <kinematic>0</kinematic>
-      </link>
-      <joint name='BR_engine_propeller_joint' type='revolute'>
-        <pose relative_to='BR_engine_link'>-0.278156 0 -0.509371 0 -0 0</pose>
-        <parent>BR_engine_link</parent>
-        <child>BR_propeller_link</child>
-        <axis>
-          <xyz>1 0 0</xyz>
-          <limit>
-            <effort>400</effort>
-            <velocity>100</velocity>
-            <lower>-1e+16</lower>
-            <upper>1e+16</upper>
-          </limit>
-          <dynamics>
-            <damping>0.05</damping>
-            <friction>0.05</friction>
-            <spring_reference>0</spring_reference>
-            <spring_stiffness>0</spring_stiffness>
-          </dynamics>
-        </axis>
-      </joint>
-      <link name='BR_propeller_link'>
-        <pose relative_to='BR_engine_propeller_joint'>0 0 0 0 -0 0</pose>
-        <inertial>
-          <pose>0 0 0 0 -0 0</pose>
-          <mass>0.5</mass>
-          <inertia>
-            <ixx>0.008545</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>0.008545</iyy>
-            <iyz>0</iyz>
-            <izz>0.0144</izz>
-          </inertia>
-        </inertial>
-        <collision name='BR_propeller_link_fixed_joint_lump__BR_propeller_collision_collision'>
-          <pose>-0.08 0 0 3.14159 1.57079 3.14159</pose>
-          <geometry>
-            <cylinder>
-              <length>0.18</length>
-              <radius>0.24</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <visual name='BR_propeller_link_visual'>
-          <pose>0 0 0 0 -0 0</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/wamv_description/models/propeller/mesh/propeller.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <self_collide>0</self_collide>
-        <enable_wind>0</enable_wind>
-        <kinematic>0</kinematic>
-      </link>
-      <joint name='FL_chasis_engine_joint' type='revolute'>
-        <pose relative_to='wamv/base_link'>1.46 0.54 0.49675 0 0 -0.785398</pose>
-        <parent>wamv/base_link</parent>
-        <child>FL_engine_link</child>
-        <axis>
-          <xyz>0 0 1</xyz>
-          <limit>
-            <lower>-3.14159</lower>
-            <upper>3.14159</upper>
-            <effort>10</effort>
-            <velocity>10</velocity>
-          </limit>
-          <dynamics>
-            <spring_reference>0</spring_reference>
-            <spring_stiffness>0</spring_stiffness>
-          </dynamics>
-        </axis>
-      </joint>
-      <link name='FL_engine_link'>
-        <pose relative_to='FL_chasis_engine_joint'>0 0 0 0 -0 0</pose>
-        <inertial>
-          <pose>0 0 0 0 -0 0</pose>
-          <mass>15</mass>
-          <inertia>
-            <ixx>0.889245</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>0.911125</iyy>
-            <iyz>0</iyz>
-            <izz>0.078125</izz>
-          </inertia>
-        </inertial>
-        <collision name='FL_engine_link_fixed_joint_lump__FL_engine_vertical_axis_collision_collision'>
-          <pose>-0.16 0 -0.24 0 -0 0</pose>
-          <geometry>
-            <box>
-              <size>0.2 0.15 0.83</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='FL_engine_link_fixed_joint_lump__FL_engine_rear_end_collision_collision_1'>
-          <pose>-0.34 0 0.12 0 -0 0</pose>
-          <geometry>
-            <box>
-              <size>0.12 0.15 0.12</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <visual name='FL_engine_link_visual'>
-          <pose>0 0 0 0 -0 0</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/wamv_description/models/engine/mesh/engine.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <self_collide>0</self_collide>
-        <enable_wind>0</enable_wind>
-        <kinematic>0</kinematic>
-      </link>
-      <joint name='FL_engine_propeller_joint' type='revolute'>
-        <pose relative_to='FL_engine_link'>-0.278156 0 -0.509371 0 -0 0</pose>
-        <parent>FL_engine_link</parent>
-        <child>FL_propeller_link</child>
-        <axis>
-          <xyz>1 0 0</xyz>
-          <limit>
-            <effort>400</effort>
-            <velocity>100</velocity>
-            <lower>-1e+16</lower>
-            <upper>1e+16</upper>
-          </limit>
-          <dynamics>
-            <damping>0.05</damping>
-            <friction>0.05</friction>
-            <spring_reference>0</spring_reference>
-            <spring_stiffness>0</spring_stiffness>
-          </dynamics>
-        </axis>
-      </joint>
-      <link name='FL_propeller_link'>
-        <pose relative_to='FL_engine_propeller_joint'>0 0 0 0 -0 0</pose>
-        <inertial>
-          <pose>0 0 0 0 -0 0</pose>
-          <mass>0.5</mass>
-          <inertia>
-            <ixx>0.008545</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>0.008545</iyy>
-            <iyz>0</iyz>
-            <izz>0.0144</izz>
-          </inertia>
-        </inertial>
-        <collision name='FL_propeller_link_fixed_joint_lump__FL_propeller_collision_collision'>
-          <pose>-0.08 0 0 3.14159 1.57079 3.14159</pose>
-          <geometry>
-            <cylinder>
-              <length>0.18</length>
-              <radius>0.24</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <visual name='FL_propeller_link_visual'>
-          <pose>0 0 0 0 -0 0</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/wamv_description/models/propeller/mesh/propeller.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <self_collide>0</self_collide>
-        <enable_wind>0</enable_wind>
-        <kinematic>0</kinematic>
-      </link>
-      <joint name='FR_chasis_engine_joint' type='revolute'>
-        <pose relative_to='wamv/base_link'>1.46 -0.54 0.49657 0 -0 0.785398</pose>
-        <parent>wamv/base_link</parent>
-        <child>FR_engine_link</child>
-        <axis>
-          <xyz>0 0 1</xyz>
-          <limit>
-            <lower>-3.14159</lower>
-            <upper>3.14159</upper>
-            <effort>10</effort>
-            <velocity>10</velocity>
-          </limit>
-          <dynamics>
-            <spring_reference>0</spring_reference>
-            <spring_stiffness>0</spring_stiffness>
-          </dynamics>
-        </axis>
-      </joint>
-      <link name='FR_engine_link'>
-        <pose relative_to='FR_chasis_engine_joint'>0 0 0 0 -0 0</pose>
-        <inertial>
-          <pose>0 0 0 0 -0 0</pose>
-          <mass>15</mass>
-          <inertia>
-            <ixx>0.889245</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>0.911125</iyy>
-            <iyz>0</iyz>
-            <izz>0.078125</izz>
-          </inertia>
-        </inertial>
-        <collision name='FR_engine_link_fixed_joint_lump__FR_engine_vertical_axis_collision_collision'>
-          <pose>-0.16 0 -0.24 0 -0 0</pose>
-          <geometry>
-            <box>
-              <size>0.2 0.15 0.83</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='FR_engine_link_fixed_joint_lump__FR_engine_rear_end_collision_collision_1'>
-          <pose>-0.34 0 0.12 0 -0 0</pose>
-          <geometry>
-            <box>
-              <size>0.12 0.15 0.12</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <visual name='FR_engine_link_visual'>
-          <pose>0 0 0 0 -0 0</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/wamv_description/models/engine/mesh/engine.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <self_collide>0</self_collide>
-        <enable_wind>0</enable_wind>
-        <kinematic>0</kinematic>
-      </link>
-      <joint name='FR_engine_propeller_joint' type='revolute'>
-        <pose relative_to='FR_engine_link'>-0.278156 0 -0.509371 0 -0 0</pose>
-        <parent>FR_engine_link</parent>
-        <child>FR_propeller_link</child>
-        <axis>
-          <xyz>1 0 0</xyz>
-          <limit>
-            <effort>400</effort>
-            <velocity>100</velocity>
-            <lower>-1e+16</lower>
-            <upper>1e+16</upper>
-          </limit>
-          <dynamics>
-            <damping>0.05</damping>
-            <friction>0.05</friction>
-            <spring_reference>0</spring_reference>
-            <spring_stiffness>0</spring_stiffness>
-          </dynamics>
-        </axis>
-      </joint>
-      <link name='FR_propeller_link'>
-        <pose relative_to='FR_engine_propeller_joint'>0 0 0 0 -0 0</pose>
-        <inertial>
-          <pose>0 0 0 0 -0 0</pose>
-          <mass>0.5</mass>
-          <inertia>
-            <ixx>0.008545</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>0.008545</iyy>
-            <iyz>0</iyz>
-            <izz>0.0144</izz>
-          </inertia>
-        </inertial>
-        <collision name='FR_propeller_link_fixed_joint_lump__FR_propeller_collision_collision'>
-          <pose>-0.08 0 0 3.14159 1.57079 3.14159</pose>
-          <geometry>
-            <cylinder>
-              <length>0.18</length>
-              <radius>0.24</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <visual name='FR_propeller_link_visual'>
-          <pose>0 0 0 0 -0 0</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/wamv_description/models/propeller/mesh/propeller.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <self_collide>0</self_collide>
-        <enable_wind>0</enable_wind>
-        <kinematic>0</kinematic>
-      </link>
-      <joint name='wamv/ball_shooter_base_joint' type='revolute'>
-        <pose relative_to='wamv/base_link'>0.54 0.3 1.3 0 -0 0.1</pose>
-        <parent>wamv/base_link</parent>
-        <child>wamv/ball_shooter_base_link</child>
-        <axis>
-          <xyz>0 0 1</xyz>
-          <limit>
-            <lower>0</lower>
-            <upper>0</upper>
-            <effort>1000</effort>
-            <velocity>0</velocity>
-          </limit>
-          <dynamics>
-            <spring_reference>0</spring_reference>
-            <spring_stiffness>0</spring_stiffness>
-          </dynamics>
-        </axis>
-      </joint>
-      <link name='wamv/ball_shooter_base_link'>
-        <pose relative_to='wamv/ball_shooter_base_joint'>0 0 0 0 -0 0</pose>
-        <inertial>
-          <pose>-0.02 0 0.05 0 -0 0</pose>
-          <mass>0.5</mass>
-          <inertia>
-            <ixx>0.00083</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>0.00052083</iyy>
-            <iyz>0</iyz>
-            <izz>0.00052083</izz>
-          </inertia>
-        </inertial>
-        <visual name='wamv/ball_shooter_base_link_fixed_joint_lump__ball_shooter_base_visual_visual'>
-          <pose>0 0 0 0 -0 0</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/vrx_gazebo/models/ball_shooter/meshes/ball_shooter_base.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <self_collide>0</self_collide>
-        <enable_wind>0</enable_wind>
-        <kinematic>0</kinematic>
-      </link>
-      <joint name='wamv/ball_shooter_launcher_joint' type='revolute'>
-        <pose relative_to='wamv/ball_shooter_base_link'>-0.0204 0 0.107877 0 -0.8 0</pose>
-        <parent>wamv/ball_shooter_base_link</parent>
-        <child>wamv/ball_shooter_launcher_link</child>
-        <axis>
-          <xyz>0 0 1</xyz>
-          <limit>
-            <lower>0</lower>
-            <upper>0</upper>
-            <effort>1000</effort>
-            <velocity>0</velocity>
-          </limit>
-          <dynamics>
-            <spring_reference>0</spring_reference>
-            <spring_stiffness>0</spring_stiffness>
-          </dynamics>
-        </axis>
-      </joint>
-      <link name='wamv/ball_shooter_launcher_link'>
-        <pose relative_to='wamv/ball_shooter_launcher_joint'>0 0 0 0 -0 0</pose>
-        <inertial>
-          <pose>0 0 0 3.14159 -1.57079 3.14159</pose>
-          <mass>0.5</mass>
-          <inertia>
-            <ixx>0.00177917</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>0.00177917</iyy>
-            <iyz>0</iyz>
-            <izz>0.000225</izz>
-          </inertia>
-        </inertial>
-        <collision name='wamv/ball_shooter_launcher_link_fixed_joint_lump__ball_shooter_collision_collision'>
-          <pose>0 0 0 0 0.261799 0</pose>
-          <geometry>
-            <box>
-              <size>0.3 0.3 0.2</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <visual name='wamv/ball_shooter_launcher_link_fixed_joint_lump__ball_shooter_launcher_visual_visual'>
-          <pose>0 0 0 0 0.261799 0</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/vrx_gazebo/models/ball_shooter/meshes/ball_shooter_launcher.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <self_collide>0</self_collide>
-        <enable_wind>0</enable_wind>
-        <kinematic>0</kinematic>
-      </link>
-      <joint name='wamv/imu_wamv_joint' type='revolute'>
-        <pose relative_to='wamv/base_link'>0.24 0.17 1.5 0 -0 0</pose>
-        <parent>wamv/base_link</parent>
-        <child>wamv/imu_wamv_link</child>
-        <axis>
-          <xyz>0 0 1</xyz>
-          <limit>
-            <lower>0</lower>
-            <upper>0</upper>
-            <effort>1000</effort>
-            <velocity>0</velocity>
-          </limit>
-          <dynamics>
-            <spring_reference>0</spring_reference>
-            <spring_stiffness>0</spring_stiffness>
-          </dynamics>
-        </axis>
-      </joint>
-      <link name='wamv/imu_wamv_link'>
-        <pose relative_to='wamv/imu_wamv_joint'>0 0 0 0 -0 0</pose>
-        <inertial>
-          <pose>0 0 0 0 -0 0</pose>
-          <mass>0.1</mass>
-          <inertia>
-            <ixx>8.3e-05</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>8.3e-05</iyy>
-            <iyz>0</iyz>
-            <izz>0.0125</izz>
-          </inertia>
-        </inertial>
-        <visual name='wamv/imu_wamv_link_fixed_joint_lump__imu_wamv_visual_visual'>
-          <pose>0 0 0 0 -0 0</pose>
-          <geometry>
-            <box>
-              <size>0.05 0.025 0.005</size>
-            </box>
-          </geometry>
-        </visual>
-        <self_collide>0</self_collide>
-        <enable_wind>0</enable_wind>
-        <kinematic>0</kinematic>
-      </link>
-      <joint name='wamv/ins_joint' type='revolute'>
-        <pose relative_to='wamv/base_link'>0.24 0.17 1.5 0 -0 0</pose>
-        <parent>wamv/base_link</parent>
-        <child>wamv/ins_link</child>
-        <axis>
-          <xyz>0 0 1</xyz>
-          <limit>
-            <lower>0</lower>
-            <upper>0</upper>
-            <effort>1000</effort>
-            <velocity>0</velocity>
-          </limit>
-          <dynamics>
-            <spring_reference>0</spring_reference>
-            <spring_stiffness>0</spring_stiffness>
-          </dynamics>
-        </axis>
-      </joint>
-      <link name='wamv/ins_link'>
-        <pose relative_to='wamv/ins_joint'>0 0 0 0 -0 0</pose>
-        <inertial>
-          <pose>0 0 0 0 -0 0</pose>
-          <mass>1</mass>
-          <inertia>
-            <ixx>0.006458</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>0.006458</iyy>
-            <iyz>0</iyz>
-            <izz>0.01125</izz>
-          </inertia>
-        </inertial>
-        <collision name='wamv/ins_link_fixed_joint_lump__ins_collision_base_collision'>
-          <pose>0 0 0.025 0 -0 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.05</length>
-              <radius>0.015</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/ins_link_fixed_joint_lump__ins_collision_antenna_collision_1'>
-          <pose>0 0 0.11 0 -0 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.1</length>
-              <radius>0.15</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <visual name='wamv/ins_link_fixed_joint_lump__ins_visual_visual'>
-          <pose>0 0 0 0 -0 0</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/wamv_gazebo/models/gps/mesh/gps.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <self_collide>0</self_collide>
-        <enable_wind>0</enable_wind>
-        <kinematic>0</kinematic>
-      </link>
-      <plugin name='gps_plugin_ins' filename='libhector_gazebo_ros_gps.so'>
-        <updateRate>15</updateRate>
-        <alwaysOn>1</alwaysOn>
-        <bodyName>wamv/ins_link</bodyName>
-        <frameId>wamv/ins_link</frameId>
-        <topicName>wamv/sensors/gps/gps/fix</topicName>
-        <velocityTopicName>wamv/sensors/gps/gps/fix_velocity</velocityTopicName>
-        <useWorldSphericalCoordinates>1</useWorldSphericalCoordinates>
-        <offset>0.0 0.0 0.0</offset>
-        <drift>0.0 0.0 0.0</drift>
-        <gaussianNoise>0 0 0</gaussianNoise>
-        <velocityOffset>0.0 0.0 0.0</velocityOffset>
-        <velocityDrift>0.0 0.0 0.0</velocityDrift>
-        <velocityGaussianNoise>0.0 0.0 0.0</velocityGaussianNoise>
-        <robotNamespace>/</robotNamespace>
-      </plugin>
-      <plugin name='imu_plugin_imu_wamv' filename='libhector_gazebo_ros_imu.so'>
-        <updateRate>15</updateRate>
-        <bodyName>wamv/imu_wamv_link</bodyName>
-        <topicName>wamv/sensors/imu/imu/data</topicName>
-        <serviceName>wamv/sensors/imu_service</serviceName>
-        <frameId>wamv/imu_wamv_link</frameId>
-        <alwaysOn>1</alwaysOn>
-        <accelOffset>0.0 0.0 0.0</accelOffset>
-        <accelDrift>0.0 0.0 0.0</accelDrift>
-        <accelDriftFrequency>0.00027 0.00027 0.000027</accelDriftFrequency>
-        <accelGaussianNoise>0.0 0.0 0.0</accelGaussianNoise>
-        <rateOffset>0.0 0.0 0.0</rateOffset>
-        <rateDrift>0.0 0.0 0.0</rateDrift>
-        <rateDriftFrequency>0.00027 0.00027 0.000027</rateDriftFrequency>
-        <rateGaussianNoise>0.0 0.0 0.0</rateGaussianNoise>
-        <yawOffset>0.0</yawOffset>
-        <yawDrift>0.0</yawDrift>
-        <yawDriftFrequency>0.00027</yawDriftFrequency>
-        <yawGaussianNoise>0.0</yawGaussianNoise>
-        <robotNamespace>/</robotNamespace>
-      </plugin>
-      <plugin name='ball_shooter_plugin' filename='libball_shooter_plugin.so'>
-        <projectile>
-          <model_name>blue_projectile</model_name>
-          <link_name>link</link_name>
-          <frame>wamv/ball_shooter_launcher_link</frame>
-          <pose>0.14 0 0 0 0 0</pose>
-        </projectile>
-        <num_shots>4</num_shots>
-        <shot_force>300</shot_force>
-        <topic>wamv/shooters/ball_shooter/fire</topic>
-        <robotNamespace>/</robotNamespace>
-      </plugin>
-      <plugin name='pinger_plugin' filename='libusv_gazebo_acoustic_pinger_plugin.so'>
-        <frameId>wamv/pinger</frameId>
-        <topicName>wamv/sensors/pingers/hydrophones/range_bearing</topicName>
-        <setPositionTopicName>wamv/sensors/pingers/hydrophones/set_pinger_position</setPositionTopicName>
-        <rangeNoise>
-          <noise>
-            <type>gaussian</type>
-            <mean>0.0</mean>
-            <stddev>3.0</stddev>
-          </noise>
-        </rangeNoise>
-        <bearingNoise>
-          <noise>
-            <type>gaussian</type>
-            <mean>0.0</mean>
-            <stddev>0.01</stddev>
-          </noise>
-        </bearingNoise>
-        <elevationNoise>
-          <noise>
-            <type>gaussian</type>
-            <mean>0.0</mean>
-            <stddev>0.01</stddev>
-          </noise>
-        </elevationNoise>
-        <robotNamespace>/</robotNamespace>
-      </plugin>
-      <plugin name='wamv_gazebo_thrust' filename='libusv_gazebo_thrust_plugin.so'>
-        <cmdTimeout>1.0</cmdTimeout>
-        <robotNamespace>wamv</robotNamespace>
-        <thruster>
-          <linkName>BL_propeller_link</linkName>
-          <propJointName>BL_engine_propeller_joint</propJointName>
-          <engineJointName>BL_chasis_engine_joint</engineJointName>
-          <cmdTopic>thrusters/BL_thrust_cmd</cmdTopic>
-          <angleTopic>thrusters/BL_thrust_angle</angleTopic>
-          <enableAngle>1</enableAngle>
-          <mappingType>1</mappingType>
-          <maxCmd>1.0</maxCmd>
-          <maxForceFwd>250.0</maxForceFwd>
-          <maxForceRev>-100.0</maxForceRev>
-          <maxAngle>1.5707963267948966</maxAngle>
-        </thruster>
-        <thruster>
-          <linkName>BR_propeller_link</linkName>
-          <propJointName>BR_engine_propeller_joint</propJointName>
-          <engineJointName>BR_chasis_engine_joint</engineJointName>
-          <cmdTopic>thrusters/BR_thrust_cmd</cmdTopic>
-          <angleTopic>thrusters/BR_thrust_angle</angleTopic>
-          <enableAngle>1</enableAngle>
-          <mappingType>1</mappingType>
-          <maxCmd>1.0</maxCmd>
-          <maxForceFwd>250.0</maxForceFwd>
-          <maxForceRev>-100.0</maxForceRev>
-          <maxAngle>1.5707963267948966</maxAngle>
-        </thruster>
-        <thruster>
-          <linkName>FL_propeller_link</linkName>
-          <propJointName>FL_engine_propeller_joint</propJointName>
-          <engineJointName>FL_chasis_engine_joint</engineJointName>
-          <cmdTopic>thrusters/FL_thrust_cmd</cmdTopic>
-          <angleTopic>thrusters/FL_thrust_angle</angleTopic>
-          <enableAngle>1</enableAngle>
-          <mappingType>1</mappingType>
-          <maxCmd>1.0</maxCmd>
-          <maxForceFwd>250.0</maxForceFwd>
-          <maxForceRev>-100.0</maxForceRev>
-          <maxAngle>1.5707963267948966</maxAngle>
-        </thruster>
-        <thruster>
-          <linkName>FR_propeller_link</linkName>
-          <propJointName>FR_engine_propeller_joint</propJointName>
-          <engineJointName>FR_chasis_engine_joint</engineJointName>
-          <cmdTopic>thrusters/FR_thrust_cmd</cmdTopic>
-          <angleTopic>thrusters/FR_thrust_angle</angleTopic>
-          <enableAngle>1</enableAngle>
-          <mappingType>1</mappingType>
-          <maxCmd>1.0</maxCmd>
-          <maxForceFwd>250.0</maxForceFwd>
-          <maxForceRev>-100.0</maxForceRev>
-          <maxAngle>1.5707963267948966</maxAngle>
-        </thruster>
-      </plugin>
-      <plugin name='usv_dynamics_wamv_dynamics_plugin' filename='libusv_gazebo_dynamics_plugin.so'>
-        <bodyName>wamv/base_link</bodyName>
-        <waterLevel>0</waterLevel>
-        <waterDensity>997.8</waterDensity>
-        <xDotU>0.0</xDotU>
-        <yDotV>0.0</yDotV>
-        <nDotR>0.0</nDotR>
-        <xU>51.3</xU>
-        <xUU>72.4</xUU>
-        <yV>40.0</yV>
-        <yVV>0.0</yVV>
-        <zW>500.0</zW>
-        <kP>50.0</kP>
-        <mQ>50.0</mQ>
-        <nR>400.0</nR>
-        <nRR>0.0</nRR>
-        <hullRadius>0.213</hullRadius>
-        <boatWidth>2.4</boatWidth>
-        <boatLength>4.9</boatLength>
-        <length_n>2</length_n>
-        <wave_model>ocean_waves</wave_model>
-        <robotNamespace>/</robotNamespace>
-      </plugin>
-      <frame name='wamv/front_left_cam_to_front_left_cam_link_optical_joint' attached_to='wamv/front_left_cam_link'>
-        <pose>0 0 0 -1.5708 -0 -1.5708</pose>
-      </frame>
-      <frame name='wamv/front_left_cam_link_optical' attached_to='wamv/front_left_cam_to_front_left_cam_link_optical_joint'/>
-      <frame name='wamv/front_left_cam_post_arm_to_front_left_cam_joint' attached_to='wamv/front_left_cam_post_arm_link'>
-        <pose>0.02 0 0.027 0 0.20944 0</pose>
-      </frame>
-      <frame name='wamv/front_left_cam_link' attached_to='wamv/front_left_cam_post_arm_to_front_left_cam_joint'/>
-      <frame name='wamv/front_left_cam_post_to_front_left_cam_post_arm_joint' attached_to='wamv/front_left_cam_post_link'>
-        <pose>0.03 0 0.08825 0 -0 0</pose>
-      </frame>
-      <frame name='wamv/front_left_cam_post_arm_link' attached_to='wamv/front_left_cam_post_to_front_left_cam_post_arm_joint'/>
-      <frame name='wamv/base_to_front_left_cam_post_joint' attached_to='wamv/base_link'>
-        <pose>0.92 0.1 1.38475 0 -0 0</pose>
-      </frame>
-      <frame name='wamv/front_left_cam_post_link' attached_to='wamv/base_to_front_left_cam_post_joint'/>
-      <frame name='wamv/front_right_cam_to_front_right_cam_link_optical_joint' attached_to='wamv/front_right_cam_link'>
-        <pose>0 0 0 -1.5708 -0 -1.5708</pose>
-      </frame>
-      <frame name='wamv/front_right_cam_link_optical' attached_to='wamv/front_right_cam_to_front_right_cam_link_optical_joint'/>
-      <frame name='wamv/front_right_cam_post_arm_to_front_right_cam_joint' attached_to='wamv/front_right_cam_post_arm_link'>
-        <pose>0.02 0 0.027 0 0.20944 0</pose>
-      </frame>
-      <frame name='wamv/front_right_cam_link' attached_to='wamv/front_right_cam_post_arm_to_front_right_cam_joint'/>
-      <frame name='wamv/front_right_cam_post_to_front_right_cam_post_arm_joint' attached_to='wamv/front_right_cam_post_link'>
-        <pose>0.03 0 0.08825 0 -0 0</pose>
-      </frame>
-      <frame name='wamv/front_right_cam_post_arm_link' attached_to='wamv/front_right_cam_post_to_front_right_cam_post_arm_joint'/>
-      <frame name='wamv/base_to_front_right_cam_post_joint' attached_to='wamv/base_link'>
-        <pose>0.92 -0.1 1.38475 0 -0 0</pose>
-      </frame>
-      <frame name='wamv/front_right_cam_post_link' attached_to='wamv/base_to_front_right_cam_post_joint'/>
-      <frame name='wamv/velodyne_post_arm_to_velodyne_joint' attached_to='velodyne_post_arm_link'>
-        <pose>0.04 0 0.05 0 0.261799 0</pose>
-      </frame>
-      <frame name='velodyne' attached_to='wamv/velodyne_post_arm_to_velodyne_joint'/>
-      <frame name='wamv/velodyne_post_to_velodyne_post_arm_joint' attached_to='velodyne_post_link'>
-        <pose>0.03 0 0.32675 0 -0 0</pose>
-      </frame>
-      <frame name='velodyne_post_arm_link' attached_to='wamv/velodyne_post_to_velodyne_post_arm_joint'/>
-      <frame name='wamv/base_to_velodyne_post_joint' attached_to='wamv/base_link'>
-        <pose>0.704 0 1.62325 0 -0 0</pose>
-      </frame>
-      <frame name='velodyne_post_link' attached_to='wamv/base_to_velodyne_post_joint'/>
-      <frame name='wamv/dummy_joint' attached_to='wamv/base_link'>
-        <pose>0 0 0 0 -0 0</pose>
-      </frame>
-      <frame name='wamv/dummy_link' attached_to='wamv/dummy_joint'/>
-      <frame name='wamv/hydrophones_pinger_joint' attached_to='wamv/base_link'>
-        <pose>1 0 -1 0 -0 0</pose>
-      </frame>
-      <frame name='wamv/hydrophones' attached_to='wamv/hydrophones_pinger_joint'/>
-      <pose>-527 167 0.1 0 -0 1</pose>
-    </model>
-    <model name='polyform_a2_red'>
+    <model name='red_circular_buoy'>
       <static>0</static>
       <pose>-522.515 184.985 0 0 -0 0</pose>
       <link name='base_link'>
@@ -3634,7 +1508,7 @@
         </buoyancy>
       </plugin>
     </model>
-    <model name='polyform_a2_green'>
+    <model name='green_circular_buoy'>
       <static>0</static>
       <pose>-534.625 187.168 0 0 -0 0</pose>
       <link name='base_link'>
@@ -3703,7 +1577,7 @@
         </buoyancy>
       </plugin>
     </model>
-    <model name='polyform_a2_blue'>
+    <model name='blue_circular_buoy'>
       <static>0</static>
       <pose>-531.036 180.328 0 0 -0 0</pose>
       <link name='base_link'>
@@ -3947,7 +1821,7 @@
           <wrench>1.03798 0.730992 3.3e-05 0 -0 0</wrench>
         </link>
       </model>
-      <model name='polyform_a2_blue'>
+      <model name='blue_circular_buoy'>
         <pose>-522.036 184.328 0.10507 0 -0 0</pose>
         <scale>1 1 1</scale>
         <link name='base_link'>
@@ -3957,7 +1831,7 @@
           <wrench>0 0 0 0 -0 0</wrench>
         </link>
       </model>
-      <model name='polyform_a2_green'>
+      <model name='green_circular_buoy'>
         <pose>-534.625 187.168 0.10507 0 -0 0</pose>
         <scale>1 1 1</scale>
         <link name='base_link'>
@@ -3967,7 +1841,7 @@
           <wrench>0 0 0 0 -0 0</wrench>
         </link>
       </model>
-      <model name='polyform_a2_red'>
+      <model name='red_circular_buoy'>
         <pose>-531.515 180.985 0.10507 0 -0 0</pose>
         <scale>1 1 1</scale>
         <link name='base_link'>
diff --git a/NaviGator/simulation/navigator_gazebo/worlds/wildlife/wildlife_encounter_test_case_3.world b/NaviGator/simulation/navigator_gazebo/worlds/wildlife/wildlife_encounter_test_case_3.world
index 957a7f166..d1df04d6c 100644
--- a/NaviGator/simulation/navigator_gazebo/worlds/wildlife/wildlife_encounter_test_case_3.world
+++ b/NaviGator/simulation/navigator_gazebo/worlds/wildlife/wildlife_encounter_test_case_3.world
@@ -1439,2133 +1439,7 @@
       <real_time_update_rate>1000</real_time_update_rate>
     </physics>
     <wind/>
-    <model name='wamv'>
-      <link name='wamv/base_link'>
-        <inertial>
-          <pose>0.012722 0 0.026171 0 -0 0</pose>
-          <mass>182.806</mass>
-          <inertia>
-            <ixx>128.232</ixx>
-            <ixy>-2.4178e-21</ixy>
-            <ixz>-3.85993</ixz>
-            <iyy>403.15</iyy>
-            <iyz>-9.28107e-22</iyz>
-            <izz>447.942</izz>
-          </inertia>
-        </inertial>
-        <collision name='wamv/base_link_fixed_joint_lump__front_left_cam_post_collision_collision'>
-          <pose>0.92 0.1 1.38475 0 -0 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.1765</length>
-              <radius>0.0076</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__front_left_cam_post_arm_collision_collision_1'>
-          <pose>0.95 0.1 1.473 -1.20428 -0 -1.5708</pose>
-          <geometry>
-            <cylinder>
-              <length>0.065</length>
-              <radius>0.008</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__front_left_cam_lens_collision_collision_2'>
-          <pose>0.987607 0.1 1.49626 3.14159 1.36136 3.14159</pose>
-          <geometry>
-            <cylinder>
-              <length>0.042</length>
-              <radius>0.015</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__front_left_cam_box_collision_collision_3'>
-          <pose>0.949948 0.1 1.50426 0 0.20944 0</pose>
-          <geometry>
-            <box>
-              <size>0.036 0.03 0.03</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__front_right_cam_post_collision_collision_4'>
-          <pose>0.92 -0.1 1.38475 0 -0 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.1765</length>
-              <radius>0.0076</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__front_right_cam_post_arm_collision_collision_5'>
-          <pose>0.95 -0.1 1.473 -1.20428 -0 -1.5708</pose>
-          <geometry>
-            <cylinder>
-              <length>0.065</length>
-              <radius>0.008</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__front_right_cam_lens_collision_collision_6'>
-          <pose>0.987607 -0.1 1.49626 3.14159 1.36136 3.14159</pose>
-          <geometry>
-            <cylinder>
-              <length>0.042</length>
-              <radius>0.015</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__front_right_cam_box_collision_collision_7'>
-          <pose>0.949948 -0.1 1.50426 0 0.20944 0</pose>
-          <geometry>
-            <box>
-              <size>0.036 0.03 0.03</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__velodyne_post_collision_collision_8'>
-          <pose>0.704 0 1.62325 0 -0 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.6535</length>
-              <radius>0.0076</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__velodyne_post_arm_collision_collision_9'>
-          <pose>0.734 0 1.95 -1.20428 -0 -1.5708</pose>
-          <geometry>
-            <cylinder>
-              <length>0.065</length>
-              <radius>0.008</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__velodyne_collision_collision_10'>
-          <pose>0.774 0 2 0 0.261799 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.075</length>
-              <radius>0.055</radius>
-            </cylinder>
-          </geometry>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <friction>
-              <ode/>
-              <torsional>
-                <ode/>
-              </torsional>
-            </friction>
-            <bounce/>
-          </surface>
-          <max_contacts>10</max_contacts>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__left_float_collision_11'>
-          <pose>-0.4 1.03 0.2 3.14159 1.57079 3.14159</pose>
-          <geometry>
-            <cylinder>
-              <length>4</length>
-              <radius>0.2</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__left_mid_float_collision_12'>
-          <pose>1.85 1.03 0.3 0 1.38 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.5</length>
-              <radius>0.17</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__left_front_float_collision_13'>
-          <pose>2.3 1.03 0.4 0 1.3 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.45</length>
-              <radius>0.12</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__front_left_beam_lower_collision_14'>
-          <pose>0.9 0.85 1 0.78 -0 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.5</length>
-              <radius>0.04</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__front_left_beam_upper_collision_15'>
-          <pose>0.9 0.6 1.18 1.35 -0 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.2</length>
-              <radius>0.04</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__mid_left_beam_lower_collision_16'>
-          <pose>-0.65 0.99 0.7 0.1 0.25 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.45</length>
-              <radius>0.03</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__mid_left_beam_medium_collision_17'>
-          <pose>-0.57 0.87 1.05 0.75 0.25 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.32</length>
-              <radius>0.03</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__mid_left_beam_upper_collision_18'>
-          <pose>-0.55 0.65 1.17 1.35 0.25 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.3</length>
-              <radius>0.03</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__rear_left_beam_lower_collision_19'>
-          <pose>-0.74 1.03 0.7 0 -0.15 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.45</length>
-              <radius>0.03</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__rear_left_beam_medium_collision_20'>
-          <pose>-0.79 0.91 1.05 0.75 -0.15 -0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.32</length>
-              <radius>0.03</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__rear_left_beam_upper_collision_21'>
-          <pose>-0.81 0.67 1.18 1.45 -0.15 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.3</length>
-              <radius>0.03</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__left_joint_collision_22'>
-          <pose>0.58 1.03 0.6 0 -0.6 0</pose>
-          <geometry>
-            <box>
-              <size>0.65 0.2 0.1</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__right_float_collision_23'>
-          <pose>-0.4 -1.03 0.2 3.14159 1.57079 3.14159</pose>
-          <geometry>
-            <cylinder>
-              <length>4</length>
-              <radius>0.2</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__right_mid_float_collision_24'>
-          <pose>1.85 -1.03 0.3 0 1.38 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.5</length>
-              <radius>0.17</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__right_front_float_collision_25'>
-          <pose>2.3 -1.03 0.4 0 1.3 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.45</length>
-              <radius>0.12</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__front_right_beam_lower_collision_26'>
-          <pose>0.9 -0.85 1 -0.78 0 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.5</length>
-              <radius>0.04</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__front_right_beam_upper_collision_27'>
-          <pose>0.9 -0.6 1.18 -1.35 0 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.2</length>
-              <radius>0.04</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__mid_right_beam_lower_collision_28'>
-          <pose>-0.65 -0.99 0.7 -0.1 0.25 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.45</length>
-              <radius>0.03</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__mid_right_beam_medium_collision_29'>
-          <pose>-0.57 -0.87 1.05 -0.75 0.25 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.32</length>
-              <radius>0.03</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__mid_right_beam_upper_collision_30'>
-          <pose>-0.55 -0.65 1.17 -1.35 0.25 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.3</length>
-              <radius>0.03</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__rear_right_beam_lower_collision_31'>
-          <pose>-0.74 -1.03 0.7 0 -0.15 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.45</length>
-              <radius>0.03</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__rear_right_beam_medium_collision_32'>
-          <pose>-0.79 -0.91 1.05 -0.75 -0.15 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.32</length>
-              <radius>0.03</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__rear_right_beam_upper_collision_33'>
-          <pose>-0.81 -0.67 1.18 -1.45 -0.15 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.3</length>
-              <radius>0.03</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__right_joint_collision_34'>
-          <pose>0.58 -1.03 0.6 0 -0.6 0</pose>
-          <geometry>
-            <box>
-              <size>0.65 0.2 0.1</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__top_base_collision_35'>
-          <pose>0 0 1.25 0 -0 0</pose>
-          <geometry>
-            <box>
-              <size>1.85 1 0.1</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__hydrophones_collision_collision_36'>
-          <pose>1 0 -1 0 -0 0</pose>
-          <geometry>
-            <box>
-              <size>0.1 0.1 0.1</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <visual name='wamv/base_link_fixed_joint_lump__wamv/front_left_cam_post_link_visual'>
-          <pose>0.92 0.1 1.38475 0 -0 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.1765</length>
-              <radius>0.0076</radius>
-            </cylinder>
-          </geometry>
-          <material>
-            <script>
-              <name>Gazebo/Black</name>
-              <uri>file://media/materials/scripts/gazebo.material</uri>
-            </script>
-          </material>
-        </visual>
-        <visual name='wamv/base_link_fixed_joint_lump__front_left_cam_post_arm_visual_visual_1'>
-          <pose>0.912 0.1 1.47 -1.0472 -0 -1.5708</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/vrx_gazebo/models/sensor_post/mesh/sensor_post_arm.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <visual name='wamv/base_link_fixed_joint_lump__front_left_cam_visual_visual_2'>
-          <pose>0.937721 0.1 1.50686 0.20944 -0 1.5708</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/vrx_gazebo/models/mono_camera/mesh/mono_camera.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <visual name='wamv/base_link_fixed_joint_lump__wamv/front_right_cam_post_link_visual_3'>
-          <pose>0.92 -0.1 1.38475 0 -0 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.1765</length>
-              <radius>0.0076</radius>
-            </cylinder>
-          </geometry>
-          <material>
-            <script>
-              <name>Gazebo/Black</name>
-              <uri>file://media/materials/scripts/gazebo.material</uri>
-            </script>
-          </material>
-        </visual>
-        <visual name='wamv/base_link_fixed_joint_lump__front_right_cam_post_arm_visual_visual_4'>
-          <pose>0.912 -0.1 1.47 -1.0472 -0 -1.5708</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/vrx_gazebo/models/sensor_post/mesh/sensor_post_arm.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <visual name='wamv/base_link_fixed_joint_lump__front_right_cam_visual_visual_5'>
-          <pose>0.937721 -0.1 1.50686 0.20944 -0 1.5708</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/vrx_gazebo/models/mono_camera/mesh/mono_camera.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <visual name='wamv/base_link_fixed_joint_lump__velodyne_post_link_visual_6'>
-          <pose>0.704 0 1.62325 0 -0 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.6535</length>
-              <radius>0.0076</radius>
-            </cylinder>
-          </geometry>
-          <material>
-            <script>
-              <name>Gazebo/Black</name>
-              <uri>file://media/materials/scripts/gazebo.material</uri>
-            </script>
-          </material>
-        </visual>
-        <visual name='wamv/base_link_fixed_joint_lump__velodyne_post_arm_visual_visual_7'>
-          <pose>0.696 0 1.947 -1.0472 -0 -1.5708</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/vrx_gazebo/models/sensor_post/mesh/sensor_post_arm.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <visual name='wamv/base_link_fixed_joint_lump__velodyne_visual_visual_8'>
-          <pose>0.764941 0 1.96619 0 0.261799 0</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/vrx_gazebo/models/3d_lidar/mesh/3d_lidar.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <visual name='wamv/base_link_fixed_joint_lump__wamv/dummy_link_visual_9'>
-          <pose>0 0 0 0 -0 0</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/wamv_description/models/WAM-V-Base/mesh/WAM-V-Base.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <visual name='wamv/base_link_fixed_joint_lump__wamv/hydrophones_visual_10'>
-          <pose>1 0 -1 0 -0 0</pose>
-          <geometry>
-            <box>
-              <size>0.1 0.1 0.1</size>
-            </box>
-          </geometry>
-        </visual>
-        <sensor name='front_left_cam_sensor' type='camera'>
-          <update_rate>30</update_rate>
-          <camera name='front_left_cam_camera'>
-            <horizontal_fov>1.39626</horizontal_fov>
-            <image>
-              <width>1280</width>
-              <height>720</height>
-              <format>R8G8B8</format>
-            </image>
-            <clip>
-              <near>0.05</near>
-              <far>300</far>
-            </clip>
-            <noise>
-              <type>gaussian</type>
-              <mean>0</mean>
-              <stddev>0.007</stddev>
-            </noise>
-          </camera>
-          <plugin name='camera_plugin_front_left_cam' filename='libgazebo_ros_camera.so'>
-            <alwaysOn>1</alwaysOn>
-            <updateRate>0.0</updateRate>
-            <cameraName>wamv/sensors/camera/front_left_cam</cameraName>
-            <imageTopicName>image_raw</imageTopicName>
-            <cameraInfoTopicName>camera_info</cameraInfoTopicName>
-            <frameName>wamv/front_left_cam_link_optical</frameName>
-            <hackBaseline>0.07</hackBaseline>
-            <distortionK1>0.0</distortionK1>
-            <distortionK2>0.0</distortionK2>
-            <distortionK3>0.0</distortionK3>
-            <distortionT1>0.0</distortionT1>
-            <distortionT2>0.0</distortionT2>
-            <robotNamespace>/</robotNamespace>
-          </plugin>
-          <pose>0.97 0.1 1.5 0 0.20944 0</pose>
-        </sensor>
-        <sensor name='front_right_cam_sensor' type='camera'>
-          <update_rate>30</update_rate>
-          <camera name='front_right_cam_camera'>
-            <horizontal_fov>1.39626</horizontal_fov>
-            <image>
-              <width>1280</width>
-              <height>720</height>
-              <format>R8G8B8</format>
-            </image>
-            <clip>
-              <near>0.05</near>
-              <far>300</far>
-            </clip>
-            <noise>
-              <type>gaussian</type>
-              <mean>0</mean>
-              <stddev>0.007</stddev>
-            </noise>
-          </camera>
-          <plugin name='camera_plugin_front_right_cam' filename='libgazebo_ros_camera.so'>
-            <alwaysOn>1</alwaysOn>
-            <updateRate>0.0</updateRate>
-            <cameraName>wamv/sensors/camera/front_right_cam</cameraName>
-            <imageTopicName>image_raw</imageTopicName>
-            <cameraInfoTopicName>camera_info</cameraInfoTopicName>
-            <frameName>wamv/front_right_cam_link_optical</frameName>
-            <hackBaseline>0.07</hackBaseline>
-            <distortionK1>0.0</distortionK1>
-            <distortionK2>0.0</distortionK2>
-            <distortionK3>0.0</distortionK3>
-            <distortionT1>0.0</distortionT1>
-            <distortionT2>0.0</distortionT2>
-            <robotNamespace>/</robotNamespace>
-          </plugin>
-          <pose>0.97 -0.1 1.5 0 0.20944 0</pose>
-        </sensor>
-        <sensor name='velodyne_sensor' type='gpu_ray'>
-          <update_rate>10</update_rate>
-          <ray>
-            <scan>
-              <horizontal>
-                <samples>18750</samples>
-                <resolution>0.1</resolution>
-                <min_angle>-3.14159</min_angle>
-                <max_angle>3.14159</max_angle>
-              </horizontal>
-              <vertical>
-                <samples>16</samples>
-                <resolution>1</resolution>
-                <min_angle>-0.261799</min_angle>
-                <max_angle>0.261799</max_angle>
-              </vertical>
-            </scan>
-            <range>
-              <min>0.1</min>
-              <max>130</max>
-            </range>
-            <noise>
-              <type>gaussian</type>
-              <mean>0</mean>
-              <stddev>0.01</stddev>
-            </noise>
-          </ray>
-          <plugin name='velodyne_plugin' filename='libgazebo_ros_velodyne_gpu_laser.so'>
-            <topicName>velodyne_points</topicName>
-            <frameName>velodyne</frameName>
-            <min_intensity>0</min_intensity>
-            <robotNamespace>/</robotNamespace>
-          </plugin>
-          <pose>0.774 0 2 0 0.261799 0</pose>
-        </sensor>
-        <self_collide>0</self_collide>
-        <enable_wind>0</enable_wind>
-        <kinematic>0</kinematic>
-      </link>
-      <joint name='BL_chasis_engine_joint' type='revolute'>
-        <pose relative_to='wamv/base_link'>-1.9304 1.1 0.318237 0 -0 0.785398</pose>
-        <parent>wamv/base_link</parent>
-        <child>BL_engine_link</child>
-        <axis>
-          <xyz>0 0 1</xyz>
-          <limit>
-            <lower>-3.14159</lower>
-            <upper>3.14159</upper>
-            <effort>10</effort>
-            <velocity>10</velocity>
-          </limit>
-          <dynamics>
-            <spring_reference>0</spring_reference>
-            <spring_stiffness>0</spring_stiffness>
-          </dynamics>
-        </axis>
-      </joint>
-      <link name='BL_engine_link'>
-        <pose relative_to='BL_chasis_engine_joint'>0 0 0 0 -0 0</pose>
-        <inertial>
-          <pose>0 0 0 0 -0 0</pose>
-          <mass>15</mass>
-          <inertia>
-            <ixx>0.889245</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>0.911125</iyy>
-            <iyz>0</iyz>
-            <izz>0.078125</izz>
-          </inertia>
-        </inertial>
-        <collision name='BL_engine_link_fixed_joint_lump__BL_engine_vertical_axis_collision_collision'>
-          <pose>-0.16 0 -0.24 0 -0 0</pose>
-          <geometry>
-            <box>
-              <size>0.2 0.15 0.83</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='BL_engine_link_fixed_joint_lump__BL_engine_rear_end_collision_collision_1'>
-          <pose>-0.34 0 0.12 0 -0 0</pose>
-          <geometry>
-            <box>
-              <size>0.12 0.15 0.12</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <visual name='BL_engine_link_visual'>
-          <pose>0 0 0 0 -0 0</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/wamv_description/models/engine/mesh/engine.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <self_collide>0</self_collide>
-        <enable_wind>0</enable_wind>
-        <kinematic>0</kinematic>
-      </link>
-      <joint name='BL_engine_propeller_joint' type='revolute'>
-        <pose relative_to='BL_engine_link'>-0.278156 0 -0.509371 0 -0 0</pose>
-        <parent>BL_engine_link</parent>
-        <child>BL_propeller_link</child>
-        <axis>
-          <xyz>1 0 0</xyz>
-          <limit>
-            <effort>400</effort>
-            <velocity>100</velocity>
-            <lower>-1e+16</lower>
-            <upper>1e+16</upper>
-          </limit>
-          <dynamics>
-            <damping>0.05</damping>
-            <friction>0.05</friction>
-            <spring_reference>0</spring_reference>
-            <spring_stiffness>0</spring_stiffness>
-          </dynamics>
-        </axis>
-      </joint>
-      <link name='BL_propeller_link'>
-        <pose relative_to='BL_engine_propeller_joint'>0 0 0 0 -0 0</pose>
-        <inertial>
-          <pose>0 0 0 0 -0 0</pose>
-          <mass>0.5</mass>
-          <inertia>
-            <ixx>0.008545</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>0.008545</iyy>
-            <iyz>0</iyz>
-            <izz>0.0144</izz>
-          </inertia>
-        </inertial>
-        <collision name='BL_propeller_link_fixed_joint_lump__BL_propeller_collision_collision'>
-          <pose>-0.08 0 0 3.14159 1.57079 3.14159</pose>
-          <geometry>
-            <cylinder>
-              <length>0.18</length>
-              <radius>0.24</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <visual name='BL_propeller_link_visual'>
-          <pose>0 0 0 0 -0 0</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/wamv_description/models/propeller/mesh/propeller.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <self_collide>0</self_collide>
-        <enable_wind>0</enable_wind>
-        <kinematic>0</kinematic>
-      </link>
-      <joint name='BR_chasis_engine_joint' type='revolute'>
-        <pose relative_to='wamv/base_link'>-1.9304 -1.1 0.318237 0 0 -0.785398</pose>
-        <parent>wamv/base_link</parent>
-        <child>BR_engine_link</child>
-        <axis>
-          <xyz>0 0 1</xyz>
-          <limit>
-            <lower>-3.14159</lower>
-            <upper>3.14159</upper>
-            <effort>10</effort>
-            <velocity>10</velocity>
-          </limit>
-          <dynamics>
-            <spring_reference>0</spring_reference>
-            <spring_stiffness>0</spring_stiffness>
-          </dynamics>
-        </axis>
-      </joint>
-      <link name='BR_engine_link'>
-        <pose relative_to='BR_chasis_engine_joint'>0 0 0 0 -0 0</pose>
-        <inertial>
-          <pose>0 0 0 0 -0 0</pose>
-          <mass>15</mass>
-          <inertia>
-            <ixx>0.889245</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>0.911125</iyy>
-            <iyz>0</iyz>
-            <izz>0.078125</izz>
-          </inertia>
-        </inertial>
-        <collision name='BR_engine_link_fixed_joint_lump__BR_engine_vertical_axis_collision_collision'>
-          <pose>-0.16 0 -0.24 0 -0 0</pose>
-          <geometry>
-            <box>
-              <size>0.2 0.15 0.83</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='BR_engine_link_fixed_joint_lump__BR_engine_rear_end_collision_collision_1'>
-          <pose>-0.34 0 0.12 0 -0 0</pose>
-          <geometry>
-            <box>
-              <size>0.12 0.15 0.12</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <visual name='BR_engine_link_visual'>
-          <pose>0 0 0 0 -0 0</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/wamv_description/models/engine/mesh/engine.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <self_collide>0</self_collide>
-        <enable_wind>0</enable_wind>
-        <kinematic>0</kinematic>
-      </link>
-      <joint name='BR_engine_propeller_joint' type='revolute'>
-        <pose relative_to='BR_engine_link'>-0.278156 0 -0.509371 0 -0 0</pose>
-        <parent>BR_engine_link</parent>
-        <child>BR_propeller_link</child>
-        <axis>
-          <xyz>1 0 0</xyz>
-          <limit>
-            <effort>400</effort>
-            <velocity>100</velocity>
-            <lower>-1e+16</lower>
-            <upper>1e+16</upper>
-          </limit>
-          <dynamics>
-            <damping>0.05</damping>
-            <friction>0.05</friction>
-            <spring_reference>0</spring_reference>
-            <spring_stiffness>0</spring_stiffness>
-          </dynamics>
-        </axis>
-      </joint>
-      <link name='BR_propeller_link'>
-        <pose relative_to='BR_engine_propeller_joint'>0 0 0 0 -0 0</pose>
-        <inertial>
-          <pose>0 0 0 0 -0 0</pose>
-          <mass>0.5</mass>
-          <inertia>
-            <ixx>0.008545</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>0.008545</iyy>
-            <iyz>0</iyz>
-            <izz>0.0144</izz>
-          </inertia>
-        </inertial>
-        <collision name='BR_propeller_link_fixed_joint_lump__BR_propeller_collision_collision'>
-          <pose>-0.08 0 0 3.14159 1.57079 3.14159</pose>
-          <geometry>
-            <cylinder>
-              <length>0.18</length>
-              <radius>0.24</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <visual name='BR_propeller_link_visual'>
-          <pose>0 0 0 0 -0 0</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/wamv_description/models/propeller/mesh/propeller.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <self_collide>0</self_collide>
-        <enable_wind>0</enable_wind>
-        <kinematic>0</kinematic>
-      </link>
-      <joint name='FL_chasis_engine_joint' type='revolute'>
-        <pose relative_to='wamv/base_link'>1.46 0.54 0.49675 0 0 -0.785398</pose>
-        <parent>wamv/base_link</parent>
-        <child>FL_engine_link</child>
-        <axis>
-          <xyz>0 0 1</xyz>
-          <limit>
-            <lower>-3.14159</lower>
-            <upper>3.14159</upper>
-            <effort>10</effort>
-            <velocity>10</velocity>
-          </limit>
-          <dynamics>
-            <spring_reference>0</spring_reference>
-            <spring_stiffness>0</spring_stiffness>
-          </dynamics>
-        </axis>
-      </joint>
-      <link name='FL_engine_link'>
-        <pose relative_to='FL_chasis_engine_joint'>0 0 0 0 -0 0</pose>
-        <inertial>
-          <pose>0 0 0 0 -0 0</pose>
-          <mass>15</mass>
-          <inertia>
-            <ixx>0.889245</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>0.911125</iyy>
-            <iyz>0</iyz>
-            <izz>0.078125</izz>
-          </inertia>
-        </inertial>
-        <collision name='FL_engine_link_fixed_joint_lump__FL_engine_vertical_axis_collision_collision'>
-          <pose>-0.16 0 -0.24 0 -0 0</pose>
-          <geometry>
-            <box>
-              <size>0.2 0.15 0.83</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='FL_engine_link_fixed_joint_lump__FL_engine_rear_end_collision_collision_1'>
-          <pose>-0.34 0 0.12 0 -0 0</pose>
-          <geometry>
-            <box>
-              <size>0.12 0.15 0.12</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <visual name='FL_engine_link_visual'>
-          <pose>0 0 0 0 -0 0</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/wamv_description/models/engine/mesh/engine.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <self_collide>0</self_collide>
-        <enable_wind>0</enable_wind>
-        <kinematic>0</kinematic>
-      </link>
-      <joint name='FL_engine_propeller_joint' type='revolute'>
-        <pose relative_to='FL_engine_link'>-0.278156 0 -0.509371 0 -0 0</pose>
-        <parent>FL_engine_link</parent>
-        <child>FL_propeller_link</child>
-        <axis>
-          <xyz>1 0 0</xyz>
-          <limit>
-            <effort>400</effort>
-            <velocity>100</velocity>
-            <lower>-1e+16</lower>
-            <upper>1e+16</upper>
-          </limit>
-          <dynamics>
-            <damping>0.05</damping>
-            <friction>0.05</friction>
-            <spring_reference>0</spring_reference>
-            <spring_stiffness>0</spring_stiffness>
-          </dynamics>
-        </axis>
-      </joint>
-      <link name='FL_propeller_link'>
-        <pose relative_to='FL_engine_propeller_joint'>0 0 0 0 -0 0</pose>
-        <inertial>
-          <pose>0 0 0 0 -0 0</pose>
-          <mass>0.5</mass>
-          <inertia>
-            <ixx>0.008545</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>0.008545</iyy>
-            <iyz>0</iyz>
-            <izz>0.0144</izz>
-          </inertia>
-        </inertial>
-        <collision name='FL_propeller_link_fixed_joint_lump__FL_propeller_collision_collision'>
-          <pose>-0.08 0 0 3.14159 1.57079 3.14159</pose>
-          <geometry>
-            <cylinder>
-              <length>0.18</length>
-              <radius>0.24</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <visual name='FL_propeller_link_visual'>
-          <pose>0 0 0 0 -0 0</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/wamv_description/models/propeller/mesh/propeller.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <self_collide>0</self_collide>
-        <enable_wind>0</enable_wind>
-        <kinematic>0</kinematic>
-      </link>
-      <joint name='FR_chasis_engine_joint' type='revolute'>
-        <pose relative_to='wamv/base_link'>1.46 -0.54 0.49657 0 -0 0.785398</pose>
-        <parent>wamv/base_link</parent>
-        <child>FR_engine_link</child>
-        <axis>
-          <xyz>0 0 1</xyz>
-          <limit>
-            <lower>-3.14159</lower>
-            <upper>3.14159</upper>
-            <effort>10</effort>
-            <velocity>10</velocity>
-          </limit>
-          <dynamics>
-            <spring_reference>0</spring_reference>
-            <spring_stiffness>0</spring_stiffness>
-          </dynamics>
-        </axis>
-      </joint>
-      <link name='FR_engine_link'>
-        <pose relative_to='FR_chasis_engine_joint'>0 0 0 0 -0 0</pose>
-        <inertial>
-          <pose>0 0 0 0 -0 0</pose>
-          <mass>15</mass>
-          <inertia>
-            <ixx>0.889245</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>0.911125</iyy>
-            <iyz>0</iyz>
-            <izz>0.078125</izz>
-          </inertia>
-        </inertial>
-        <collision name='FR_engine_link_fixed_joint_lump__FR_engine_vertical_axis_collision_collision'>
-          <pose>-0.16 0 -0.24 0 -0 0</pose>
-          <geometry>
-            <box>
-              <size>0.2 0.15 0.83</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='FR_engine_link_fixed_joint_lump__FR_engine_rear_end_collision_collision_1'>
-          <pose>-0.34 0 0.12 0 -0 0</pose>
-          <geometry>
-            <box>
-              <size>0.12 0.15 0.12</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <visual name='FR_engine_link_visual'>
-          <pose>0 0 0 0 -0 0</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/wamv_description/models/engine/mesh/engine.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <self_collide>0</self_collide>
-        <enable_wind>0</enable_wind>
-        <kinematic>0</kinematic>
-      </link>
-      <joint name='FR_engine_propeller_joint' type='revolute'>
-        <pose relative_to='FR_engine_link'>-0.278156 0 -0.509371 0 -0 0</pose>
-        <parent>FR_engine_link</parent>
-        <child>FR_propeller_link</child>
-        <axis>
-          <xyz>1 0 0</xyz>
-          <limit>
-            <effort>400</effort>
-            <velocity>100</velocity>
-            <lower>-1e+16</lower>
-            <upper>1e+16</upper>
-          </limit>
-          <dynamics>
-            <damping>0.05</damping>
-            <friction>0.05</friction>
-            <spring_reference>0</spring_reference>
-            <spring_stiffness>0</spring_stiffness>
-          </dynamics>
-        </axis>
-      </joint>
-      <link name='FR_propeller_link'>
-        <pose relative_to='FR_engine_propeller_joint'>0 0 0 0 -0 0</pose>
-        <inertial>
-          <pose>0 0 0 0 -0 0</pose>
-          <mass>0.5</mass>
-          <inertia>
-            <ixx>0.008545</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>0.008545</iyy>
-            <iyz>0</iyz>
-            <izz>0.0144</izz>
-          </inertia>
-        </inertial>
-        <collision name='FR_propeller_link_fixed_joint_lump__FR_propeller_collision_collision'>
-          <pose>-0.08 0 0 3.14159 1.57079 3.14159</pose>
-          <geometry>
-            <cylinder>
-              <length>0.18</length>
-              <radius>0.24</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <visual name='FR_propeller_link_visual'>
-          <pose>0 0 0 0 -0 0</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/wamv_description/models/propeller/mesh/propeller.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <self_collide>0</self_collide>
-        <enable_wind>0</enable_wind>
-        <kinematic>0</kinematic>
-      </link>
-      <joint name='wamv/ball_shooter_base_joint' type='revolute'>
-        <pose relative_to='wamv/base_link'>0.54 0.3 1.3 0 -0 0.1</pose>
-        <parent>wamv/base_link</parent>
-        <child>wamv/ball_shooter_base_link</child>
-        <axis>
-          <xyz>0 0 1</xyz>
-          <limit>
-            <lower>0</lower>
-            <upper>0</upper>
-            <effort>1000</effort>
-            <velocity>0</velocity>
-          </limit>
-          <dynamics>
-            <spring_reference>0</spring_reference>
-            <spring_stiffness>0</spring_stiffness>
-          </dynamics>
-        </axis>
-      </joint>
-      <link name='wamv/ball_shooter_base_link'>
-        <pose relative_to='wamv/ball_shooter_base_joint'>0 0 0 0 -0 0</pose>
-        <inertial>
-          <pose>-0.02 0 0.05 0 -0 0</pose>
-          <mass>0.5</mass>
-          <inertia>
-            <ixx>0.00083</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>0.00052083</iyy>
-            <iyz>0</iyz>
-            <izz>0.00052083</izz>
-          </inertia>
-        </inertial>
-        <visual name='wamv/ball_shooter_base_link_fixed_joint_lump__ball_shooter_base_visual_visual'>
-          <pose>0 0 0 0 -0 0</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/vrx_gazebo/models/ball_shooter/meshes/ball_shooter_base.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <self_collide>0</self_collide>
-        <enable_wind>0</enable_wind>
-        <kinematic>0</kinematic>
-      </link>
-      <joint name='wamv/ball_shooter_launcher_joint' type='revolute'>
-        <pose relative_to='wamv/ball_shooter_base_link'>-0.0204 0 0.107877 0 -0.8 0</pose>
-        <parent>wamv/ball_shooter_base_link</parent>
-        <child>wamv/ball_shooter_launcher_link</child>
-        <axis>
-          <xyz>0 0 1</xyz>
-          <limit>
-            <lower>0</lower>
-            <upper>0</upper>
-            <effort>1000</effort>
-            <velocity>0</velocity>
-          </limit>
-          <dynamics>
-            <spring_reference>0</spring_reference>
-            <spring_stiffness>0</spring_stiffness>
-          </dynamics>
-        </axis>
-      </joint>
-      <link name='wamv/ball_shooter_launcher_link'>
-        <pose relative_to='wamv/ball_shooter_launcher_joint'>0 0 0 0 -0 0</pose>
-        <inertial>
-          <pose>0 0 0 3.14159 -1.57079 3.14159</pose>
-          <mass>0.5</mass>
-          <inertia>
-            <ixx>0.00177917</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>0.00177917</iyy>
-            <iyz>0</iyz>
-            <izz>0.000225</izz>
-          </inertia>
-        </inertial>
-        <collision name='wamv/ball_shooter_launcher_link_fixed_joint_lump__ball_shooter_collision_collision'>
-          <pose>0 0 0 0 0.261799 0</pose>
-          <geometry>
-            <box>
-              <size>0.3 0.3 0.2</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <visual name='wamv/ball_shooter_launcher_link_fixed_joint_lump__ball_shooter_launcher_visual_visual'>
-          <pose>0 0 0 0 0.261799 0</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/vrx_gazebo/models/ball_shooter/meshes/ball_shooter_launcher.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <self_collide>0</self_collide>
-        <enable_wind>0</enable_wind>
-        <kinematic>0</kinematic>
-      </link>
-      <joint name='wamv/imu_wamv_joint' type='revolute'>
-        <pose relative_to='wamv/base_link'>0.24 0.17 1.5 0 -0 0</pose>
-        <parent>wamv/base_link</parent>
-        <child>wamv/imu_wamv_link</child>
-        <axis>
-          <xyz>0 0 1</xyz>
-          <limit>
-            <lower>0</lower>
-            <upper>0</upper>
-            <effort>1000</effort>
-            <velocity>0</velocity>
-          </limit>
-          <dynamics>
-            <spring_reference>0</spring_reference>
-            <spring_stiffness>0</spring_stiffness>
-          </dynamics>
-        </axis>
-      </joint>
-      <link name='wamv/imu_wamv_link'>
-        <pose relative_to='wamv/imu_wamv_joint'>0 0 0 0 -0 0</pose>
-        <inertial>
-          <pose>0 0 0 0 -0 0</pose>
-          <mass>0.1</mass>
-          <inertia>
-            <ixx>8.3e-05</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>8.3e-05</iyy>
-            <iyz>0</iyz>
-            <izz>0.0125</izz>
-          </inertia>
-        </inertial>
-        <visual name='wamv/imu_wamv_link_fixed_joint_lump__imu_wamv_visual_visual'>
-          <pose>0 0 0 0 -0 0</pose>
-          <geometry>
-            <box>
-              <size>0.05 0.025 0.005</size>
-            </box>
-          </geometry>
-        </visual>
-        <self_collide>0</self_collide>
-        <enable_wind>0</enable_wind>
-        <kinematic>0</kinematic>
-      </link>
-      <joint name='wamv/ins_joint' type='revolute'>
-        <pose relative_to='wamv/base_link'>0.24 0.17 1.5 0 -0 0</pose>
-        <parent>wamv/base_link</parent>
-        <child>wamv/ins_link</child>
-        <axis>
-          <xyz>0 0 1</xyz>
-          <limit>
-            <lower>0</lower>
-            <upper>0</upper>
-            <effort>1000</effort>
-            <velocity>0</velocity>
-          </limit>
-          <dynamics>
-            <spring_reference>0</spring_reference>
-            <spring_stiffness>0</spring_stiffness>
-          </dynamics>
-        </axis>
-      </joint>
-      <link name='wamv/ins_link'>
-        <pose relative_to='wamv/ins_joint'>0 0 0 0 -0 0</pose>
-        <inertial>
-          <pose>0 0 0 0 -0 0</pose>
-          <mass>1</mass>
-          <inertia>
-            <ixx>0.006458</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>0.006458</iyy>
-            <iyz>0</iyz>
-            <izz>0.01125</izz>
-          </inertia>
-        </inertial>
-        <collision name='wamv/ins_link_fixed_joint_lump__ins_collision_base_collision'>
-          <pose>0 0 0.025 0 -0 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.05</length>
-              <radius>0.015</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/ins_link_fixed_joint_lump__ins_collision_antenna_collision_1'>
-          <pose>0 0 0.11 0 -0 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.1</length>
-              <radius>0.15</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <visual name='wamv/ins_link_fixed_joint_lump__ins_visual_visual'>
-          <pose>0 0 0 0 -0 0</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/wamv_gazebo/models/gps/mesh/gps.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <self_collide>0</self_collide>
-        <enable_wind>0</enable_wind>
-        <kinematic>0</kinematic>
-      </link>
-      <plugin name='gps_plugin_ins' filename='libhector_gazebo_ros_gps.so'>
-        <updateRate>15</updateRate>
-        <alwaysOn>1</alwaysOn>
-        <bodyName>wamv/ins_link</bodyName>
-        <frameId>wamv/ins_link</frameId>
-        <topicName>wamv/sensors/gps/gps/fix</topicName>
-        <velocityTopicName>wamv/sensors/gps/gps/fix_velocity</velocityTopicName>
-        <useWorldSphericalCoordinates>1</useWorldSphericalCoordinates>
-        <offset>0.0 0.0 0.0</offset>
-        <drift>0.0 0.0 0.0</drift>
-        <gaussianNoise>0 0 0</gaussianNoise>
-        <velocityOffset>0.0 0.0 0.0</velocityOffset>
-        <velocityDrift>0.0 0.0 0.0</velocityDrift>
-        <velocityGaussianNoise>0.0 0.0 0.0</velocityGaussianNoise>
-        <robotNamespace>/</robotNamespace>
-      </plugin>
-      <plugin name='imu_plugin_imu_wamv' filename='libhector_gazebo_ros_imu.so'>
-        <updateRate>15</updateRate>
-        <bodyName>wamv/imu_wamv_link</bodyName>
-        <topicName>wamv/sensors/imu/imu/data</topicName>
-        <serviceName>wamv/sensors/imu_service</serviceName>
-        <frameId>wamv/imu_wamv_link</frameId>
-        <alwaysOn>1</alwaysOn>
-        <accelOffset>0.0 0.0 0.0</accelOffset>
-        <accelDrift>0.0 0.0 0.0</accelDrift>
-        <accelDriftFrequency>0.00027 0.00027 0.000027</accelDriftFrequency>
-        <accelGaussianNoise>0.0 0.0 0.0</accelGaussianNoise>
-        <rateOffset>0.0 0.0 0.0</rateOffset>
-        <rateDrift>0.0 0.0 0.0</rateDrift>
-        <rateDriftFrequency>0.00027 0.00027 0.000027</rateDriftFrequency>
-        <rateGaussianNoise>0.0 0.0 0.0</rateGaussianNoise>
-        <yawOffset>0.0</yawOffset>
-        <yawDrift>0.0</yawDrift>
-        <yawDriftFrequency>0.00027</yawDriftFrequency>
-        <yawGaussianNoise>0.0</yawGaussianNoise>
-        <robotNamespace>/</robotNamespace>
-      </plugin>
-      <plugin name='ball_shooter_plugin' filename='libball_shooter_plugin.so'>
-        <projectile>
-          <model_name>blue_projectile</model_name>
-          <link_name>link</link_name>
-          <frame>wamv/ball_shooter_launcher_link</frame>
-          <pose>0.14 0 0 0 0 0</pose>
-        </projectile>
-        <num_shots>4</num_shots>
-        <shot_force>300</shot_force>
-        <topic>wamv/shooters/ball_shooter/fire</topic>
-        <robotNamespace>/</robotNamespace>
-      </plugin>
-      <plugin name='pinger_plugin' filename='libusv_gazebo_acoustic_pinger_plugin.so'>
-        <frameId>wamv/pinger</frameId>
-        <topicName>wamv/sensors/pingers/hydrophones/range_bearing</topicName>
-        <setPositionTopicName>wamv/sensors/pingers/hydrophones/set_pinger_position</setPositionTopicName>
-        <rangeNoise>
-          <noise>
-            <type>gaussian</type>
-            <mean>0.0</mean>
-            <stddev>3.0</stddev>
-          </noise>
-        </rangeNoise>
-        <bearingNoise>
-          <noise>
-            <type>gaussian</type>
-            <mean>0.0</mean>
-            <stddev>0.01</stddev>
-          </noise>
-        </bearingNoise>
-        <elevationNoise>
-          <noise>
-            <type>gaussian</type>
-            <mean>0.0</mean>
-            <stddev>0.01</stddev>
-          </noise>
-        </elevationNoise>
-        <robotNamespace>/</robotNamespace>
-      </plugin>
-      <plugin name='wamv_gazebo_thrust' filename='libusv_gazebo_thrust_plugin.so'>
-        <cmdTimeout>1.0</cmdTimeout>
-        <robotNamespace>wamv</robotNamespace>
-        <thruster>
-          <linkName>BL_propeller_link</linkName>
-          <propJointName>BL_engine_propeller_joint</propJointName>
-          <engineJointName>BL_chasis_engine_joint</engineJointName>
-          <cmdTopic>thrusters/BL_thrust_cmd</cmdTopic>
-          <angleTopic>thrusters/BL_thrust_angle</angleTopic>
-          <enableAngle>1</enableAngle>
-          <mappingType>1</mappingType>
-          <maxCmd>1.0</maxCmd>
-          <maxForceFwd>250.0</maxForceFwd>
-          <maxForceRev>-100.0</maxForceRev>
-          <maxAngle>1.5707963267948966</maxAngle>
-        </thruster>
-        <thruster>
-          <linkName>BR_propeller_link</linkName>
-          <propJointName>BR_engine_propeller_joint</propJointName>
-          <engineJointName>BR_chasis_engine_joint</engineJointName>
-          <cmdTopic>thrusters/BR_thrust_cmd</cmdTopic>
-          <angleTopic>thrusters/BR_thrust_angle</angleTopic>
-          <enableAngle>1</enableAngle>
-          <mappingType>1</mappingType>
-          <maxCmd>1.0</maxCmd>
-          <maxForceFwd>250.0</maxForceFwd>
-          <maxForceRev>-100.0</maxForceRev>
-          <maxAngle>1.5707963267948966</maxAngle>
-        </thruster>
-        <thruster>
-          <linkName>FL_propeller_link</linkName>
-          <propJointName>FL_engine_propeller_joint</propJointName>
-          <engineJointName>FL_chasis_engine_joint</engineJointName>
-          <cmdTopic>thrusters/FL_thrust_cmd</cmdTopic>
-          <angleTopic>thrusters/FL_thrust_angle</angleTopic>
-          <enableAngle>1</enableAngle>
-          <mappingType>1</mappingType>
-          <maxCmd>1.0</maxCmd>
-          <maxForceFwd>250.0</maxForceFwd>
-          <maxForceRev>-100.0</maxForceRev>
-          <maxAngle>1.5707963267948966</maxAngle>
-        </thruster>
-        <thruster>
-          <linkName>FR_propeller_link</linkName>
-          <propJointName>FR_engine_propeller_joint</propJointName>
-          <engineJointName>FR_chasis_engine_joint</engineJointName>
-          <cmdTopic>thrusters/FR_thrust_cmd</cmdTopic>
-          <angleTopic>thrusters/FR_thrust_angle</angleTopic>
-          <enableAngle>1</enableAngle>
-          <mappingType>1</mappingType>
-          <maxCmd>1.0</maxCmd>
-          <maxForceFwd>250.0</maxForceFwd>
-          <maxForceRev>-100.0</maxForceRev>
-          <maxAngle>1.5707963267948966</maxAngle>
-        </thruster>
-      </plugin>
-      <plugin name='usv_dynamics_wamv_dynamics_plugin' filename='libusv_gazebo_dynamics_plugin.so'>
-        <bodyName>wamv/base_link</bodyName>
-        <waterLevel>0</waterLevel>
-        <waterDensity>997.8</waterDensity>
-        <xDotU>0.0</xDotU>
-        <yDotV>0.0</yDotV>
-        <nDotR>0.0</nDotR>
-        <xU>51.3</xU>
-        <xUU>72.4</xUU>
-        <yV>40.0</yV>
-        <yVV>0.0</yVV>
-        <zW>500.0</zW>
-        <kP>50.0</kP>
-        <mQ>50.0</mQ>
-        <nR>400.0</nR>
-        <nRR>0.0</nRR>
-        <hullRadius>0.213</hullRadius>
-        <boatWidth>2.4</boatWidth>
-        <boatLength>4.9</boatLength>
-        <length_n>2</length_n>
-        <wave_model>ocean_waves</wave_model>
-        <robotNamespace>/</robotNamespace>
-      </plugin>
-      <frame name='wamv/front_left_cam_to_front_left_cam_link_optical_joint' attached_to='wamv/front_left_cam_link'>
-        <pose>0 0 0 -1.5708 -0 -1.5708</pose>
-      </frame>
-      <frame name='wamv/front_left_cam_link_optical' attached_to='wamv/front_left_cam_to_front_left_cam_link_optical_joint'/>
-      <frame name='wamv/front_left_cam_post_arm_to_front_left_cam_joint' attached_to='wamv/front_left_cam_post_arm_link'>
-        <pose>0.02 0 0.027 0 0.20944 0</pose>
-      </frame>
-      <frame name='wamv/front_left_cam_link' attached_to='wamv/front_left_cam_post_arm_to_front_left_cam_joint'/>
-      <frame name='wamv/front_left_cam_post_to_front_left_cam_post_arm_joint' attached_to='wamv/front_left_cam_post_link'>
-        <pose>0.03 0 0.08825 0 -0 0</pose>
-      </frame>
-      <frame name='wamv/front_left_cam_post_arm_link' attached_to='wamv/front_left_cam_post_to_front_left_cam_post_arm_joint'/>
-      <frame name='wamv/base_to_front_left_cam_post_joint' attached_to='wamv/base_link'>
-        <pose>0.92 0.1 1.38475 0 -0 0</pose>
-      </frame>
-      <frame name='wamv/front_left_cam_post_link' attached_to='wamv/base_to_front_left_cam_post_joint'/>
-      <frame name='wamv/front_right_cam_to_front_right_cam_link_optical_joint' attached_to='wamv/front_right_cam_link'>
-        <pose>0 0 0 -1.5708 -0 -1.5708</pose>
-      </frame>
-      <frame name='wamv/front_right_cam_link_optical' attached_to='wamv/front_right_cam_to_front_right_cam_link_optical_joint'/>
-      <frame name='wamv/front_right_cam_post_arm_to_front_right_cam_joint' attached_to='wamv/front_right_cam_post_arm_link'>
-        <pose>0.02 0 0.027 0 0.20944 0</pose>
-      </frame>
-      <frame name='wamv/front_right_cam_link' attached_to='wamv/front_right_cam_post_arm_to_front_right_cam_joint'/>
-      <frame name='wamv/front_right_cam_post_to_front_right_cam_post_arm_joint' attached_to='wamv/front_right_cam_post_link'>
-        <pose>0.03 0 0.08825 0 -0 0</pose>
-      </frame>
-      <frame name='wamv/front_right_cam_post_arm_link' attached_to='wamv/front_right_cam_post_to_front_right_cam_post_arm_joint'/>
-      <frame name='wamv/base_to_front_right_cam_post_joint' attached_to='wamv/base_link'>
-        <pose>0.92 -0.1 1.38475 0 -0 0</pose>
-      </frame>
-      <frame name='wamv/front_right_cam_post_link' attached_to='wamv/base_to_front_right_cam_post_joint'/>
-      <frame name='wamv/velodyne_post_arm_to_velodyne_joint' attached_to='velodyne_post_arm_link'>
-        <pose>0.04 0 0.05 0 0.261799 0</pose>
-      </frame>
-      <frame name='velodyne' attached_to='wamv/velodyne_post_arm_to_velodyne_joint'/>
-      <frame name='wamv/velodyne_post_to_velodyne_post_arm_joint' attached_to='velodyne_post_link'>
-        <pose>0.03 0 0.32675 0 -0 0</pose>
-      </frame>
-      <frame name='velodyne_post_arm_link' attached_to='wamv/velodyne_post_to_velodyne_post_arm_joint'/>
-      <frame name='wamv/base_to_velodyne_post_joint' attached_to='wamv/base_link'>
-        <pose>0.704 0 1.62325 0 -0 0</pose>
-      </frame>
-      <frame name='velodyne_post_link' attached_to='wamv/base_to_velodyne_post_joint'/>
-      <frame name='wamv/dummy_joint' attached_to='wamv/base_link'>
-        <pose>0 0 0 0 -0 0</pose>
-      </frame>
-      <frame name='wamv/dummy_link' attached_to='wamv/dummy_joint'/>
-      <frame name='wamv/hydrophones_pinger_joint' attached_to='wamv/base_link'>
-        <pose>1 0 -1 0 -0 0</pose>
-      </frame>
-      <frame name='wamv/hydrophones' attached_to='wamv/hydrophones_pinger_joint'/>
-      <pose>-527 167 0.1 0 -0 1</pose>
-    </model>
-    <model name='polyform_a2_red'>
+    <model name='red_circular_buoy'>
       <static>0</static>
       <pose>-522.515 184.985 0 0 -0 0</pose>
       <link name='base_link'>
@@ -3634,7 +1508,7 @@
         </buoyancy>
       </plugin>
     </model>
-    <model name='polyform_a2_green'>
+    <model name='green_circular_buoy'>
       <static>0</static>
       <pose>-534.625 187.168 0 0 -0 0</pose>
       <link name='base_link'>
@@ -3703,7 +1577,7 @@
         </buoyancy>
       </plugin>
     </model>
-    <model name='polyform_a2_blue'>
+    <model name='blue_circular_buoy'>
       <static>0</static>
       <pose>-531.036 180.328 0 0 -0 0</pose>
       <link name='base_link'>
@@ -3947,7 +1821,7 @@
           <wrench>0.629708 -1.07887 -6.3e-05 0 -0 0</wrench>
         </link>
       </model>
-      <model name='polyform_a2_blue'>
+      <model name='blue_circular_buoy'>
         <pose>-522.036 184.328 0.10507 0 -0 0</pose>
         <scale>1 1 1</scale>
         <link name='base_link'>
@@ -3957,7 +1831,7 @@
           <wrench>0 0 -0 0 -0 0</wrench>
         </link>
       </model>
-      <model name='polyform_a2_green'>
+      <model name='green_circular_buoy'>
         <pose>-531.625 180.168 0.10507 0 -0 0</pose>
         <scale>1 1 1</scale>
         <link name='base_link'>
@@ -3967,7 +1841,7 @@
           <wrench>0 0 -0 0 -0 0</wrench>
         </link>
       </model>
-      <model name='polyform_a2_red'>
+      <model name='red_circular_buoy'>
         <pose>-534.515 187.985 0.10507 0 -0 0</pose>
         <scale>1 1 1</scale>
         <link name='base_link'>
diff --git a/NaviGator/simulation/navigator_gazebo/worlds/wildlife/wildlife_encounter_test_case_4.world b/NaviGator/simulation/navigator_gazebo/worlds/wildlife/wildlife_encounter_test_case_4.world
index a40540233..897faf2df 100644
--- a/NaviGator/simulation/navigator_gazebo/worlds/wildlife/wildlife_encounter_test_case_4.world
+++ b/NaviGator/simulation/navigator_gazebo/worlds/wildlife/wildlife_encounter_test_case_4.world
@@ -1439,2133 +1439,7 @@
       <real_time_update_rate>1000</real_time_update_rate>
     </physics>
     <wind/>
-    <model name='wamv'>
-      <link name='wamv/base_link'>
-        <inertial>
-          <pose>0.012722 0 0.026171 0 -0 0</pose>
-          <mass>182.806</mass>
-          <inertia>
-            <ixx>128.232</ixx>
-            <ixy>-2.4178e-21</ixy>
-            <ixz>-3.85993</ixz>
-            <iyy>403.15</iyy>
-            <iyz>-9.28107e-22</iyz>
-            <izz>447.942</izz>
-          </inertia>
-        </inertial>
-        <collision name='wamv/base_link_fixed_joint_lump__front_left_cam_post_collision_collision'>
-          <pose>0.92 0.1 1.38475 0 -0 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.1765</length>
-              <radius>0.0076</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__front_left_cam_post_arm_collision_collision_1'>
-          <pose>0.95 0.1 1.473 -1.20428 -0 -1.5708</pose>
-          <geometry>
-            <cylinder>
-              <length>0.065</length>
-              <radius>0.008</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__front_left_cam_lens_collision_collision_2'>
-          <pose>0.987607 0.1 1.49626 3.14159 1.36136 3.14159</pose>
-          <geometry>
-            <cylinder>
-              <length>0.042</length>
-              <radius>0.015</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__front_left_cam_box_collision_collision_3'>
-          <pose>0.949948 0.1 1.50426 0 0.20944 0</pose>
-          <geometry>
-            <box>
-              <size>0.036 0.03 0.03</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__front_right_cam_post_collision_collision_4'>
-          <pose>0.92 -0.1 1.38475 0 -0 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.1765</length>
-              <radius>0.0076</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__front_right_cam_post_arm_collision_collision_5'>
-          <pose>0.95 -0.1 1.473 -1.20428 -0 -1.5708</pose>
-          <geometry>
-            <cylinder>
-              <length>0.065</length>
-              <radius>0.008</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__front_right_cam_lens_collision_collision_6'>
-          <pose>0.987607 -0.1 1.49626 3.14159 1.36136 3.14159</pose>
-          <geometry>
-            <cylinder>
-              <length>0.042</length>
-              <radius>0.015</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__front_right_cam_box_collision_collision_7'>
-          <pose>0.949948 -0.1 1.50426 0 0.20944 0</pose>
-          <geometry>
-            <box>
-              <size>0.036 0.03 0.03</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__velodyne_post_collision_collision_8'>
-          <pose>0.704 0 1.62325 0 -0 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.6535</length>
-              <radius>0.0076</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__velodyne_post_arm_collision_collision_9'>
-          <pose>0.734 0 1.95 -1.20428 -0 -1.5708</pose>
-          <geometry>
-            <cylinder>
-              <length>0.065</length>
-              <radius>0.008</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__velodyne_collision_collision_10'>
-          <pose>0.774 0 2 0 0.261799 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.075</length>
-              <radius>0.055</radius>
-            </cylinder>
-          </geometry>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <friction>
-              <ode/>
-              <torsional>
-                <ode/>
-              </torsional>
-            </friction>
-            <bounce/>
-          </surface>
-          <max_contacts>10</max_contacts>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__left_float_collision_11'>
-          <pose>-0.4 1.03 0.2 3.14159 1.57079 3.14159</pose>
-          <geometry>
-            <cylinder>
-              <length>4</length>
-              <radius>0.2</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__left_mid_float_collision_12'>
-          <pose>1.85 1.03 0.3 0 1.38 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.5</length>
-              <radius>0.17</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__left_front_float_collision_13'>
-          <pose>2.3 1.03 0.4 0 1.3 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.45</length>
-              <radius>0.12</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__front_left_beam_lower_collision_14'>
-          <pose>0.9 0.85 1 0.78 -0 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.5</length>
-              <radius>0.04</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__front_left_beam_upper_collision_15'>
-          <pose>0.9 0.6 1.18 1.35 -0 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.2</length>
-              <radius>0.04</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__mid_left_beam_lower_collision_16'>
-          <pose>-0.65 0.99 0.7 0.1 0.25 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.45</length>
-              <radius>0.03</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__mid_left_beam_medium_collision_17'>
-          <pose>-0.57 0.87 1.05 0.75 0.25 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.32</length>
-              <radius>0.03</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__mid_left_beam_upper_collision_18'>
-          <pose>-0.55 0.65 1.17 1.35 0.25 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.3</length>
-              <radius>0.03</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__rear_left_beam_lower_collision_19'>
-          <pose>-0.74 1.03 0.7 0 -0.15 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.45</length>
-              <radius>0.03</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__rear_left_beam_medium_collision_20'>
-          <pose>-0.79 0.91 1.05 0.75 -0.15 -0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.32</length>
-              <radius>0.03</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__rear_left_beam_upper_collision_21'>
-          <pose>-0.81 0.67 1.18 1.45 -0.15 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.3</length>
-              <radius>0.03</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__left_joint_collision_22'>
-          <pose>0.58 1.03 0.6 0 -0.6 0</pose>
-          <geometry>
-            <box>
-              <size>0.65 0.2 0.1</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__right_float_collision_23'>
-          <pose>-0.4 -1.03 0.2 3.14159 1.57079 3.14159</pose>
-          <geometry>
-            <cylinder>
-              <length>4</length>
-              <radius>0.2</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__right_mid_float_collision_24'>
-          <pose>1.85 -1.03 0.3 0 1.38 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.5</length>
-              <radius>0.17</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__right_front_float_collision_25'>
-          <pose>2.3 -1.03 0.4 0 1.3 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.45</length>
-              <radius>0.12</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__front_right_beam_lower_collision_26'>
-          <pose>0.9 -0.85 1 -0.78 0 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.5</length>
-              <radius>0.04</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__front_right_beam_upper_collision_27'>
-          <pose>0.9 -0.6 1.18 -1.35 0 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.2</length>
-              <radius>0.04</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__mid_right_beam_lower_collision_28'>
-          <pose>-0.65 -0.99 0.7 -0.1 0.25 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.45</length>
-              <radius>0.03</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__mid_right_beam_medium_collision_29'>
-          <pose>-0.57 -0.87 1.05 -0.75 0.25 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.32</length>
-              <radius>0.03</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__mid_right_beam_upper_collision_30'>
-          <pose>-0.55 -0.65 1.17 -1.35 0.25 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.3</length>
-              <radius>0.03</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__rear_right_beam_lower_collision_31'>
-          <pose>-0.74 -1.03 0.7 0 -0.15 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.45</length>
-              <radius>0.03</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__rear_right_beam_medium_collision_32'>
-          <pose>-0.79 -0.91 1.05 -0.75 -0.15 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.32</length>
-              <radius>0.03</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__rear_right_beam_upper_collision_33'>
-          <pose>-0.81 -0.67 1.18 -1.45 -0.15 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.3</length>
-              <radius>0.03</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__right_joint_collision_34'>
-          <pose>0.58 -1.03 0.6 0 -0.6 0</pose>
-          <geometry>
-            <box>
-              <size>0.65 0.2 0.1</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__top_base_collision_35'>
-          <pose>0 0 1.25 0 -0 0</pose>
-          <geometry>
-            <box>
-              <size>1.85 1 0.1</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__hydrophones_collision_collision_36'>
-          <pose>1 0 -1 0 -0 0</pose>
-          <geometry>
-            <box>
-              <size>0.1 0.1 0.1</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <visual name='wamv/base_link_fixed_joint_lump__wamv/front_left_cam_post_link_visual'>
-          <pose>0.92 0.1 1.38475 0 -0 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.1765</length>
-              <radius>0.0076</radius>
-            </cylinder>
-          </geometry>
-          <material>
-            <script>
-              <name>Gazebo/Black</name>
-              <uri>file://media/materials/scripts/gazebo.material</uri>
-            </script>
-          </material>
-        </visual>
-        <visual name='wamv/base_link_fixed_joint_lump__front_left_cam_post_arm_visual_visual_1'>
-          <pose>0.912 0.1 1.47 -1.0472 -0 -1.5708</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/vrx_gazebo/models/sensor_post/mesh/sensor_post_arm.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <visual name='wamv/base_link_fixed_joint_lump__front_left_cam_visual_visual_2'>
-          <pose>0.937721 0.1 1.50686 0.20944 -0 1.5708</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/vrx_gazebo/models/mono_camera/mesh/mono_camera.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <visual name='wamv/base_link_fixed_joint_lump__wamv/front_right_cam_post_link_visual_3'>
-          <pose>0.92 -0.1 1.38475 0 -0 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.1765</length>
-              <radius>0.0076</radius>
-            </cylinder>
-          </geometry>
-          <material>
-            <script>
-              <name>Gazebo/Black</name>
-              <uri>file://media/materials/scripts/gazebo.material</uri>
-            </script>
-          </material>
-        </visual>
-        <visual name='wamv/base_link_fixed_joint_lump__front_right_cam_post_arm_visual_visual_4'>
-          <pose>0.912 -0.1 1.47 -1.0472 -0 -1.5708</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/vrx_gazebo/models/sensor_post/mesh/sensor_post_arm.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <visual name='wamv/base_link_fixed_joint_lump__front_right_cam_visual_visual_5'>
-          <pose>0.937721 -0.1 1.50686 0.20944 -0 1.5708</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/vrx_gazebo/models/mono_camera/mesh/mono_camera.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <visual name='wamv/base_link_fixed_joint_lump__velodyne_post_link_visual_6'>
-          <pose>0.704 0 1.62325 0 -0 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.6535</length>
-              <radius>0.0076</radius>
-            </cylinder>
-          </geometry>
-          <material>
-            <script>
-              <name>Gazebo/Black</name>
-              <uri>file://media/materials/scripts/gazebo.material</uri>
-            </script>
-          </material>
-        </visual>
-        <visual name='wamv/base_link_fixed_joint_lump__velodyne_post_arm_visual_visual_7'>
-          <pose>0.696 0 1.947 -1.0472 -0 -1.5708</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/vrx_gazebo/models/sensor_post/mesh/sensor_post_arm.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <visual name='wamv/base_link_fixed_joint_lump__velodyne_visual_visual_8'>
-          <pose>0.764941 0 1.96619 0 0.261799 0</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/vrx_gazebo/models/3d_lidar/mesh/3d_lidar.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <visual name='wamv/base_link_fixed_joint_lump__wamv/dummy_link_visual_9'>
-          <pose>0 0 0 0 -0 0</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/wamv_description/models/WAM-V-Base/mesh/WAM-V-Base.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <visual name='wamv/base_link_fixed_joint_lump__wamv/hydrophones_visual_10'>
-          <pose>1 0 -1 0 -0 0</pose>
-          <geometry>
-            <box>
-              <size>0.1 0.1 0.1</size>
-            </box>
-          </geometry>
-        </visual>
-        <sensor name='front_left_cam_sensor' type='camera'>
-          <update_rate>30</update_rate>
-          <camera name='front_left_cam_camera'>
-            <horizontal_fov>1.39626</horizontal_fov>
-            <image>
-              <width>1280</width>
-              <height>720</height>
-              <format>R8G8B8</format>
-            </image>
-            <clip>
-              <near>0.05</near>
-              <far>300</far>
-            </clip>
-            <noise>
-              <type>gaussian</type>
-              <mean>0</mean>
-              <stddev>0.007</stddev>
-            </noise>
-          </camera>
-          <plugin name='camera_plugin_front_left_cam' filename='libgazebo_ros_camera.so'>
-            <alwaysOn>1</alwaysOn>
-            <updateRate>0.0</updateRate>
-            <cameraName>wamv/sensors/camera/front_left_cam</cameraName>
-            <imageTopicName>image_raw</imageTopicName>
-            <cameraInfoTopicName>camera_info</cameraInfoTopicName>
-            <frameName>wamv/front_left_cam_link_optical</frameName>
-            <hackBaseline>0.07</hackBaseline>
-            <distortionK1>0.0</distortionK1>
-            <distortionK2>0.0</distortionK2>
-            <distortionK3>0.0</distortionK3>
-            <distortionT1>0.0</distortionT1>
-            <distortionT2>0.0</distortionT2>
-            <robotNamespace>/</robotNamespace>
-          </plugin>
-          <pose>0.97 0.1 1.5 0 0.20944 0</pose>
-        </sensor>
-        <sensor name='front_right_cam_sensor' type='camera'>
-          <update_rate>30</update_rate>
-          <camera name='front_right_cam_camera'>
-            <horizontal_fov>1.39626</horizontal_fov>
-            <image>
-              <width>1280</width>
-              <height>720</height>
-              <format>R8G8B8</format>
-            </image>
-            <clip>
-              <near>0.05</near>
-              <far>300</far>
-            </clip>
-            <noise>
-              <type>gaussian</type>
-              <mean>0</mean>
-              <stddev>0.007</stddev>
-            </noise>
-          </camera>
-          <plugin name='camera_plugin_front_right_cam' filename='libgazebo_ros_camera.so'>
-            <alwaysOn>1</alwaysOn>
-            <updateRate>0.0</updateRate>
-            <cameraName>wamv/sensors/camera/front_right_cam</cameraName>
-            <imageTopicName>image_raw</imageTopicName>
-            <cameraInfoTopicName>camera_info</cameraInfoTopicName>
-            <frameName>wamv/front_right_cam_link_optical</frameName>
-            <hackBaseline>0.07</hackBaseline>
-            <distortionK1>0.0</distortionK1>
-            <distortionK2>0.0</distortionK2>
-            <distortionK3>0.0</distortionK3>
-            <distortionT1>0.0</distortionT1>
-            <distortionT2>0.0</distortionT2>
-            <robotNamespace>/</robotNamespace>
-          </plugin>
-          <pose>0.97 -0.1 1.5 0 0.20944 0</pose>
-        </sensor>
-        <sensor name='velodyne_sensor' type='gpu_ray'>
-          <update_rate>10</update_rate>
-          <ray>
-            <scan>
-              <horizontal>
-                <samples>18750</samples>
-                <resolution>0.1</resolution>
-                <min_angle>-3.14159</min_angle>
-                <max_angle>3.14159</max_angle>
-              </horizontal>
-              <vertical>
-                <samples>16</samples>
-                <resolution>1</resolution>
-                <min_angle>-0.261799</min_angle>
-                <max_angle>0.261799</max_angle>
-              </vertical>
-            </scan>
-            <range>
-              <min>0.1</min>
-              <max>130</max>
-            </range>
-            <noise>
-              <type>gaussian</type>
-              <mean>0</mean>
-              <stddev>0.01</stddev>
-            </noise>
-          </ray>
-          <plugin name='velodyne_plugin' filename='libgazebo_ros_velodyne_gpu_laser.so'>
-            <topicName>velodyne_points</topicName>
-            <frameName>velodyne</frameName>
-            <min_intensity>0</min_intensity>
-            <robotNamespace>/</robotNamespace>
-          </plugin>
-          <pose>0.774 0 2 0 0.261799 0</pose>
-        </sensor>
-        <self_collide>0</self_collide>
-        <enable_wind>0</enable_wind>
-        <kinematic>0</kinematic>
-      </link>
-      <joint name='BL_chasis_engine_joint' type='revolute'>
-        <pose relative_to='wamv/base_link'>-1.9304 1.1 0.318237 0 -0 0.785398</pose>
-        <parent>wamv/base_link</parent>
-        <child>BL_engine_link</child>
-        <axis>
-          <xyz>0 0 1</xyz>
-          <limit>
-            <lower>-3.14159</lower>
-            <upper>3.14159</upper>
-            <effort>10</effort>
-            <velocity>10</velocity>
-          </limit>
-          <dynamics>
-            <spring_reference>0</spring_reference>
-            <spring_stiffness>0</spring_stiffness>
-          </dynamics>
-        </axis>
-      </joint>
-      <link name='BL_engine_link'>
-        <pose relative_to='BL_chasis_engine_joint'>0 0 0 0 -0 0</pose>
-        <inertial>
-          <pose>0 0 0 0 -0 0</pose>
-          <mass>15</mass>
-          <inertia>
-            <ixx>0.889245</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>0.911125</iyy>
-            <iyz>0</iyz>
-            <izz>0.078125</izz>
-          </inertia>
-        </inertial>
-        <collision name='BL_engine_link_fixed_joint_lump__BL_engine_vertical_axis_collision_collision'>
-          <pose>-0.16 0 -0.24 0 -0 0</pose>
-          <geometry>
-            <box>
-              <size>0.2 0.15 0.83</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='BL_engine_link_fixed_joint_lump__BL_engine_rear_end_collision_collision_1'>
-          <pose>-0.34 0 0.12 0 -0 0</pose>
-          <geometry>
-            <box>
-              <size>0.12 0.15 0.12</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <visual name='BL_engine_link_visual'>
-          <pose>0 0 0 0 -0 0</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/wamv_description/models/engine/mesh/engine.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <self_collide>0</self_collide>
-        <enable_wind>0</enable_wind>
-        <kinematic>0</kinematic>
-      </link>
-      <joint name='BL_engine_propeller_joint' type='revolute'>
-        <pose relative_to='BL_engine_link'>-0.278156 0 -0.509371 0 -0 0</pose>
-        <parent>BL_engine_link</parent>
-        <child>BL_propeller_link</child>
-        <axis>
-          <xyz>1 0 0</xyz>
-          <limit>
-            <effort>400</effort>
-            <velocity>100</velocity>
-            <lower>-1e+16</lower>
-            <upper>1e+16</upper>
-          </limit>
-          <dynamics>
-            <damping>0.05</damping>
-            <friction>0.05</friction>
-            <spring_reference>0</spring_reference>
-            <spring_stiffness>0</spring_stiffness>
-          </dynamics>
-        </axis>
-      </joint>
-      <link name='BL_propeller_link'>
-        <pose relative_to='BL_engine_propeller_joint'>0 0 0 0 -0 0</pose>
-        <inertial>
-          <pose>0 0 0 0 -0 0</pose>
-          <mass>0.5</mass>
-          <inertia>
-            <ixx>0.008545</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>0.008545</iyy>
-            <iyz>0</iyz>
-            <izz>0.0144</izz>
-          </inertia>
-        </inertial>
-        <collision name='BL_propeller_link_fixed_joint_lump__BL_propeller_collision_collision'>
-          <pose>-0.08 0 0 3.14159 1.57079 3.14159</pose>
-          <geometry>
-            <cylinder>
-              <length>0.18</length>
-              <radius>0.24</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <visual name='BL_propeller_link_visual'>
-          <pose>0 0 0 0 -0 0</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/wamv_description/models/propeller/mesh/propeller.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <self_collide>0</self_collide>
-        <enable_wind>0</enable_wind>
-        <kinematic>0</kinematic>
-      </link>
-      <joint name='BR_chasis_engine_joint' type='revolute'>
-        <pose relative_to='wamv/base_link'>-1.9304 -1.1 0.318237 0 0 -0.785398</pose>
-        <parent>wamv/base_link</parent>
-        <child>BR_engine_link</child>
-        <axis>
-          <xyz>0 0 1</xyz>
-          <limit>
-            <lower>-3.14159</lower>
-            <upper>3.14159</upper>
-            <effort>10</effort>
-            <velocity>10</velocity>
-          </limit>
-          <dynamics>
-            <spring_reference>0</spring_reference>
-            <spring_stiffness>0</spring_stiffness>
-          </dynamics>
-        </axis>
-      </joint>
-      <link name='BR_engine_link'>
-        <pose relative_to='BR_chasis_engine_joint'>0 0 0 0 -0 0</pose>
-        <inertial>
-          <pose>0 0 0 0 -0 0</pose>
-          <mass>15</mass>
-          <inertia>
-            <ixx>0.889245</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>0.911125</iyy>
-            <iyz>0</iyz>
-            <izz>0.078125</izz>
-          </inertia>
-        </inertial>
-        <collision name='BR_engine_link_fixed_joint_lump__BR_engine_vertical_axis_collision_collision'>
-          <pose>-0.16 0 -0.24 0 -0 0</pose>
-          <geometry>
-            <box>
-              <size>0.2 0.15 0.83</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='BR_engine_link_fixed_joint_lump__BR_engine_rear_end_collision_collision_1'>
-          <pose>-0.34 0 0.12 0 -0 0</pose>
-          <geometry>
-            <box>
-              <size>0.12 0.15 0.12</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <visual name='BR_engine_link_visual'>
-          <pose>0 0 0 0 -0 0</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/wamv_description/models/engine/mesh/engine.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <self_collide>0</self_collide>
-        <enable_wind>0</enable_wind>
-        <kinematic>0</kinematic>
-      </link>
-      <joint name='BR_engine_propeller_joint' type='revolute'>
-        <pose relative_to='BR_engine_link'>-0.278156 0 -0.509371 0 -0 0</pose>
-        <parent>BR_engine_link</parent>
-        <child>BR_propeller_link</child>
-        <axis>
-          <xyz>1 0 0</xyz>
-          <limit>
-            <effort>400</effort>
-            <velocity>100</velocity>
-            <lower>-1e+16</lower>
-            <upper>1e+16</upper>
-          </limit>
-          <dynamics>
-            <damping>0.05</damping>
-            <friction>0.05</friction>
-            <spring_reference>0</spring_reference>
-            <spring_stiffness>0</spring_stiffness>
-          </dynamics>
-        </axis>
-      </joint>
-      <link name='BR_propeller_link'>
-        <pose relative_to='BR_engine_propeller_joint'>0 0 0 0 -0 0</pose>
-        <inertial>
-          <pose>0 0 0 0 -0 0</pose>
-          <mass>0.5</mass>
-          <inertia>
-            <ixx>0.008545</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>0.008545</iyy>
-            <iyz>0</iyz>
-            <izz>0.0144</izz>
-          </inertia>
-        </inertial>
-        <collision name='BR_propeller_link_fixed_joint_lump__BR_propeller_collision_collision'>
-          <pose>-0.08 0 0 3.14159 1.57079 3.14159</pose>
-          <geometry>
-            <cylinder>
-              <length>0.18</length>
-              <radius>0.24</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <visual name='BR_propeller_link_visual'>
-          <pose>0 0 0 0 -0 0</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/wamv_description/models/propeller/mesh/propeller.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <self_collide>0</self_collide>
-        <enable_wind>0</enable_wind>
-        <kinematic>0</kinematic>
-      </link>
-      <joint name='FL_chasis_engine_joint' type='revolute'>
-        <pose relative_to='wamv/base_link'>1.46 0.54 0.49675 0 0 -0.785398</pose>
-        <parent>wamv/base_link</parent>
-        <child>FL_engine_link</child>
-        <axis>
-          <xyz>0 0 1</xyz>
-          <limit>
-            <lower>-3.14159</lower>
-            <upper>3.14159</upper>
-            <effort>10</effort>
-            <velocity>10</velocity>
-          </limit>
-          <dynamics>
-            <spring_reference>0</spring_reference>
-            <spring_stiffness>0</spring_stiffness>
-          </dynamics>
-        </axis>
-      </joint>
-      <link name='FL_engine_link'>
-        <pose relative_to='FL_chasis_engine_joint'>0 0 0 0 -0 0</pose>
-        <inertial>
-          <pose>0 0 0 0 -0 0</pose>
-          <mass>15</mass>
-          <inertia>
-            <ixx>0.889245</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>0.911125</iyy>
-            <iyz>0</iyz>
-            <izz>0.078125</izz>
-          </inertia>
-        </inertial>
-        <collision name='FL_engine_link_fixed_joint_lump__FL_engine_vertical_axis_collision_collision'>
-          <pose>-0.16 0 -0.24 0 -0 0</pose>
-          <geometry>
-            <box>
-              <size>0.2 0.15 0.83</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='FL_engine_link_fixed_joint_lump__FL_engine_rear_end_collision_collision_1'>
-          <pose>-0.34 0 0.12 0 -0 0</pose>
-          <geometry>
-            <box>
-              <size>0.12 0.15 0.12</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <visual name='FL_engine_link_visual'>
-          <pose>0 0 0 0 -0 0</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/wamv_description/models/engine/mesh/engine.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <self_collide>0</self_collide>
-        <enable_wind>0</enable_wind>
-        <kinematic>0</kinematic>
-      </link>
-      <joint name='FL_engine_propeller_joint' type='revolute'>
-        <pose relative_to='FL_engine_link'>-0.278156 0 -0.509371 0 -0 0</pose>
-        <parent>FL_engine_link</parent>
-        <child>FL_propeller_link</child>
-        <axis>
-          <xyz>1 0 0</xyz>
-          <limit>
-            <effort>400</effort>
-            <velocity>100</velocity>
-            <lower>-1e+16</lower>
-            <upper>1e+16</upper>
-          </limit>
-          <dynamics>
-            <damping>0.05</damping>
-            <friction>0.05</friction>
-            <spring_reference>0</spring_reference>
-            <spring_stiffness>0</spring_stiffness>
-          </dynamics>
-        </axis>
-      </joint>
-      <link name='FL_propeller_link'>
-        <pose relative_to='FL_engine_propeller_joint'>0 0 0 0 -0 0</pose>
-        <inertial>
-          <pose>0 0 0 0 -0 0</pose>
-          <mass>0.5</mass>
-          <inertia>
-            <ixx>0.008545</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>0.008545</iyy>
-            <iyz>0</iyz>
-            <izz>0.0144</izz>
-          </inertia>
-        </inertial>
-        <collision name='FL_propeller_link_fixed_joint_lump__FL_propeller_collision_collision'>
-          <pose>-0.08 0 0 3.14159 1.57079 3.14159</pose>
-          <geometry>
-            <cylinder>
-              <length>0.18</length>
-              <radius>0.24</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <visual name='FL_propeller_link_visual'>
-          <pose>0 0 0 0 -0 0</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/wamv_description/models/propeller/mesh/propeller.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <self_collide>0</self_collide>
-        <enable_wind>0</enable_wind>
-        <kinematic>0</kinematic>
-      </link>
-      <joint name='FR_chasis_engine_joint' type='revolute'>
-        <pose relative_to='wamv/base_link'>1.46 -0.54 0.49657 0 -0 0.785398</pose>
-        <parent>wamv/base_link</parent>
-        <child>FR_engine_link</child>
-        <axis>
-          <xyz>0 0 1</xyz>
-          <limit>
-            <lower>-3.14159</lower>
-            <upper>3.14159</upper>
-            <effort>10</effort>
-            <velocity>10</velocity>
-          </limit>
-          <dynamics>
-            <spring_reference>0</spring_reference>
-            <spring_stiffness>0</spring_stiffness>
-          </dynamics>
-        </axis>
-      </joint>
-      <link name='FR_engine_link'>
-        <pose relative_to='FR_chasis_engine_joint'>0 0 0 0 -0 0</pose>
-        <inertial>
-          <pose>0 0 0 0 -0 0</pose>
-          <mass>15</mass>
-          <inertia>
-            <ixx>0.889245</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>0.911125</iyy>
-            <iyz>0</iyz>
-            <izz>0.078125</izz>
-          </inertia>
-        </inertial>
-        <collision name='FR_engine_link_fixed_joint_lump__FR_engine_vertical_axis_collision_collision'>
-          <pose>-0.16 0 -0.24 0 -0 0</pose>
-          <geometry>
-            <box>
-              <size>0.2 0.15 0.83</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='FR_engine_link_fixed_joint_lump__FR_engine_rear_end_collision_collision_1'>
-          <pose>-0.34 0 0.12 0 -0 0</pose>
-          <geometry>
-            <box>
-              <size>0.12 0.15 0.12</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <visual name='FR_engine_link_visual'>
-          <pose>0 0 0 0 -0 0</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/wamv_description/models/engine/mesh/engine.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <self_collide>0</self_collide>
-        <enable_wind>0</enable_wind>
-        <kinematic>0</kinematic>
-      </link>
-      <joint name='FR_engine_propeller_joint' type='revolute'>
-        <pose relative_to='FR_engine_link'>-0.278156 0 -0.509371 0 -0 0</pose>
-        <parent>FR_engine_link</parent>
-        <child>FR_propeller_link</child>
-        <axis>
-          <xyz>1 0 0</xyz>
-          <limit>
-            <effort>400</effort>
-            <velocity>100</velocity>
-            <lower>-1e+16</lower>
-            <upper>1e+16</upper>
-          </limit>
-          <dynamics>
-            <damping>0.05</damping>
-            <friction>0.05</friction>
-            <spring_reference>0</spring_reference>
-            <spring_stiffness>0</spring_stiffness>
-          </dynamics>
-        </axis>
-      </joint>
-      <link name='FR_propeller_link'>
-        <pose relative_to='FR_engine_propeller_joint'>0 0 0 0 -0 0</pose>
-        <inertial>
-          <pose>0 0 0 0 -0 0</pose>
-          <mass>0.5</mass>
-          <inertia>
-            <ixx>0.008545</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>0.008545</iyy>
-            <iyz>0</iyz>
-            <izz>0.0144</izz>
-          </inertia>
-        </inertial>
-        <collision name='FR_propeller_link_fixed_joint_lump__FR_propeller_collision_collision'>
-          <pose>-0.08 0 0 3.14159 1.57079 3.14159</pose>
-          <geometry>
-            <cylinder>
-              <length>0.18</length>
-              <radius>0.24</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <visual name='FR_propeller_link_visual'>
-          <pose>0 0 0 0 -0 0</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/wamv_description/models/propeller/mesh/propeller.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <self_collide>0</self_collide>
-        <enable_wind>0</enable_wind>
-        <kinematic>0</kinematic>
-      </link>
-      <joint name='wamv/ball_shooter_base_joint' type='revolute'>
-        <pose relative_to='wamv/base_link'>0.54 0.3 1.3 0 -0 0.1</pose>
-        <parent>wamv/base_link</parent>
-        <child>wamv/ball_shooter_base_link</child>
-        <axis>
-          <xyz>0 0 1</xyz>
-          <limit>
-            <lower>0</lower>
-            <upper>0</upper>
-            <effort>1000</effort>
-            <velocity>0</velocity>
-          </limit>
-          <dynamics>
-            <spring_reference>0</spring_reference>
-            <spring_stiffness>0</spring_stiffness>
-          </dynamics>
-        </axis>
-      </joint>
-      <link name='wamv/ball_shooter_base_link'>
-        <pose relative_to='wamv/ball_shooter_base_joint'>0 0 0 0 -0 0</pose>
-        <inertial>
-          <pose>-0.02 0 0.05 0 -0 0</pose>
-          <mass>0.5</mass>
-          <inertia>
-            <ixx>0.00083</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>0.00052083</iyy>
-            <iyz>0</iyz>
-            <izz>0.00052083</izz>
-          </inertia>
-        </inertial>
-        <visual name='wamv/ball_shooter_base_link_fixed_joint_lump__ball_shooter_base_visual_visual'>
-          <pose>0 0 0 0 -0 0</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/vrx_gazebo/models/ball_shooter/meshes/ball_shooter_base.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <self_collide>0</self_collide>
-        <enable_wind>0</enable_wind>
-        <kinematic>0</kinematic>
-      </link>
-      <joint name='wamv/ball_shooter_launcher_joint' type='revolute'>
-        <pose relative_to='wamv/ball_shooter_base_link'>-0.0204 0 0.107877 0 -0.8 0</pose>
-        <parent>wamv/ball_shooter_base_link</parent>
-        <child>wamv/ball_shooter_launcher_link</child>
-        <axis>
-          <xyz>0 0 1</xyz>
-          <limit>
-            <lower>0</lower>
-            <upper>0</upper>
-            <effort>1000</effort>
-            <velocity>0</velocity>
-          </limit>
-          <dynamics>
-            <spring_reference>0</spring_reference>
-            <spring_stiffness>0</spring_stiffness>
-          </dynamics>
-        </axis>
-      </joint>
-      <link name='wamv/ball_shooter_launcher_link'>
-        <pose relative_to='wamv/ball_shooter_launcher_joint'>0 0 0 0 -0 0</pose>
-        <inertial>
-          <pose>0 0 0 3.14159 -1.57079 3.14159</pose>
-          <mass>0.5</mass>
-          <inertia>
-            <ixx>0.00177917</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>0.00177917</iyy>
-            <iyz>0</iyz>
-            <izz>0.000225</izz>
-          </inertia>
-        </inertial>
-        <collision name='wamv/ball_shooter_launcher_link_fixed_joint_lump__ball_shooter_collision_collision'>
-          <pose>0 0 0 0 0.261799 0</pose>
-          <geometry>
-            <box>
-              <size>0.3 0.3 0.2</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <visual name='wamv/ball_shooter_launcher_link_fixed_joint_lump__ball_shooter_launcher_visual_visual'>
-          <pose>0 0 0 0 0.261799 0</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/vrx_gazebo/models/ball_shooter/meshes/ball_shooter_launcher.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <self_collide>0</self_collide>
-        <enable_wind>0</enable_wind>
-        <kinematic>0</kinematic>
-      </link>
-      <joint name='wamv/imu_wamv_joint' type='revolute'>
-        <pose relative_to='wamv/base_link'>0.24 0.17 1.5 0 -0 0</pose>
-        <parent>wamv/base_link</parent>
-        <child>wamv/imu_wamv_link</child>
-        <axis>
-          <xyz>0 0 1</xyz>
-          <limit>
-            <lower>0</lower>
-            <upper>0</upper>
-            <effort>1000</effort>
-            <velocity>0</velocity>
-          </limit>
-          <dynamics>
-            <spring_reference>0</spring_reference>
-            <spring_stiffness>0</spring_stiffness>
-          </dynamics>
-        </axis>
-      </joint>
-      <link name='wamv/imu_wamv_link'>
-        <pose relative_to='wamv/imu_wamv_joint'>0 0 0 0 -0 0</pose>
-        <inertial>
-          <pose>0 0 0 0 -0 0</pose>
-          <mass>0.1</mass>
-          <inertia>
-            <ixx>8.3e-05</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>8.3e-05</iyy>
-            <iyz>0</iyz>
-            <izz>0.0125</izz>
-          </inertia>
-        </inertial>
-        <visual name='wamv/imu_wamv_link_fixed_joint_lump__imu_wamv_visual_visual'>
-          <pose>0 0 0 0 -0 0</pose>
-          <geometry>
-            <box>
-              <size>0.05 0.025 0.005</size>
-            </box>
-          </geometry>
-        </visual>
-        <self_collide>0</self_collide>
-        <enable_wind>0</enable_wind>
-        <kinematic>0</kinematic>
-      </link>
-      <joint name='wamv/ins_joint' type='revolute'>
-        <pose relative_to='wamv/base_link'>0.24 0.17 1.5 0 -0 0</pose>
-        <parent>wamv/base_link</parent>
-        <child>wamv/ins_link</child>
-        <axis>
-          <xyz>0 0 1</xyz>
-          <limit>
-            <lower>0</lower>
-            <upper>0</upper>
-            <effort>1000</effort>
-            <velocity>0</velocity>
-          </limit>
-          <dynamics>
-            <spring_reference>0</spring_reference>
-            <spring_stiffness>0</spring_stiffness>
-          </dynamics>
-        </axis>
-      </joint>
-      <link name='wamv/ins_link'>
-        <pose relative_to='wamv/ins_joint'>0 0 0 0 -0 0</pose>
-        <inertial>
-          <pose>0 0 0 0 -0 0</pose>
-          <mass>1</mass>
-          <inertia>
-            <ixx>0.006458</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>0.006458</iyy>
-            <iyz>0</iyz>
-            <izz>0.01125</izz>
-          </inertia>
-        </inertial>
-        <collision name='wamv/ins_link_fixed_joint_lump__ins_collision_base_collision'>
-          <pose>0 0 0.025 0 -0 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.05</length>
-              <radius>0.015</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/ins_link_fixed_joint_lump__ins_collision_antenna_collision_1'>
-          <pose>0 0 0.11 0 -0 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.1</length>
-              <radius>0.15</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <visual name='wamv/ins_link_fixed_joint_lump__ins_visual_visual'>
-          <pose>0 0 0 0 -0 0</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/wamv_gazebo/models/gps/mesh/gps.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <self_collide>0</self_collide>
-        <enable_wind>0</enable_wind>
-        <kinematic>0</kinematic>
-      </link>
-      <plugin name='gps_plugin_ins' filename='libhector_gazebo_ros_gps.so'>
-        <updateRate>15</updateRate>
-        <alwaysOn>1</alwaysOn>
-        <bodyName>wamv/ins_link</bodyName>
-        <frameId>wamv/ins_link</frameId>
-        <topicName>wamv/sensors/gps/gps/fix</topicName>
-        <velocityTopicName>wamv/sensors/gps/gps/fix_velocity</velocityTopicName>
-        <useWorldSphericalCoordinates>1</useWorldSphericalCoordinates>
-        <offset>0.0 0.0 0.0</offset>
-        <drift>0.0 0.0 0.0</drift>
-        <gaussianNoise>0 0 0</gaussianNoise>
-        <velocityOffset>0.0 0.0 0.0</velocityOffset>
-        <velocityDrift>0.0 0.0 0.0</velocityDrift>
-        <velocityGaussianNoise>0.0 0.0 0.0</velocityGaussianNoise>
-        <robotNamespace>/</robotNamespace>
-      </plugin>
-      <plugin name='imu_plugin_imu_wamv' filename='libhector_gazebo_ros_imu.so'>
-        <updateRate>15</updateRate>
-        <bodyName>wamv/imu_wamv_link</bodyName>
-        <topicName>wamv/sensors/imu/imu/data</topicName>
-        <serviceName>wamv/sensors/imu_service</serviceName>
-        <frameId>wamv/imu_wamv_link</frameId>
-        <alwaysOn>1</alwaysOn>
-        <accelOffset>0.0 0.0 0.0</accelOffset>
-        <accelDrift>0.0 0.0 0.0</accelDrift>
-        <accelDriftFrequency>0.00027 0.00027 0.000027</accelDriftFrequency>
-        <accelGaussianNoise>0.0 0.0 0.0</accelGaussianNoise>
-        <rateOffset>0.0 0.0 0.0</rateOffset>
-        <rateDrift>0.0 0.0 0.0</rateDrift>
-        <rateDriftFrequency>0.00027 0.00027 0.000027</rateDriftFrequency>
-        <rateGaussianNoise>0.0 0.0 0.0</rateGaussianNoise>
-        <yawOffset>0.0</yawOffset>
-        <yawDrift>0.0</yawDrift>
-        <yawDriftFrequency>0.00027</yawDriftFrequency>
-        <yawGaussianNoise>0.0</yawGaussianNoise>
-        <robotNamespace>/</robotNamespace>
-      </plugin>
-      <plugin name='ball_shooter_plugin' filename='libball_shooter_plugin.so'>
-        <projectile>
-          <model_name>blue_projectile</model_name>
-          <link_name>link</link_name>
-          <frame>wamv/ball_shooter_launcher_link</frame>
-          <pose>0.14 0 0 0 0 0</pose>
-        </projectile>
-        <num_shots>4</num_shots>
-        <shot_force>300</shot_force>
-        <topic>wamv/shooters/ball_shooter/fire</topic>
-        <robotNamespace>/</robotNamespace>
-      </plugin>
-      <plugin name='pinger_plugin' filename='libusv_gazebo_acoustic_pinger_plugin.so'>
-        <frameId>wamv/pinger</frameId>
-        <topicName>wamv/sensors/pingers/hydrophones/range_bearing</topicName>
-        <setPositionTopicName>wamv/sensors/pingers/hydrophones/set_pinger_position</setPositionTopicName>
-        <rangeNoise>
-          <noise>
-            <type>gaussian</type>
-            <mean>0.0</mean>
-            <stddev>3.0</stddev>
-          </noise>
-        </rangeNoise>
-        <bearingNoise>
-          <noise>
-            <type>gaussian</type>
-            <mean>0.0</mean>
-            <stddev>0.01</stddev>
-          </noise>
-        </bearingNoise>
-        <elevationNoise>
-          <noise>
-            <type>gaussian</type>
-            <mean>0.0</mean>
-            <stddev>0.01</stddev>
-          </noise>
-        </elevationNoise>
-        <robotNamespace>/</robotNamespace>
-      </plugin>
-      <plugin name='wamv_gazebo_thrust' filename='libusv_gazebo_thrust_plugin.so'>
-        <cmdTimeout>1.0</cmdTimeout>
-        <robotNamespace>wamv</robotNamespace>
-        <thruster>
-          <linkName>BL_propeller_link</linkName>
-          <propJointName>BL_engine_propeller_joint</propJointName>
-          <engineJointName>BL_chasis_engine_joint</engineJointName>
-          <cmdTopic>thrusters/BL_thrust_cmd</cmdTopic>
-          <angleTopic>thrusters/BL_thrust_angle</angleTopic>
-          <enableAngle>1</enableAngle>
-          <mappingType>1</mappingType>
-          <maxCmd>1.0</maxCmd>
-          <maxForceFwd>250.0</maxForceFwd>
-          <maxForceRev>-100.0</maxForceRev>
-          <maxAngle>1.5707963267948966</maxAngle>
-        </thruster>
-        <thruster>
-          <linkName>BR_propeller_link</linkName>
-          <propJointName>BR_engine_propeller_joint</propJointName>
-          <engineJointName>BR_chasis_engine_joint</engineJointName>
-          <cmdTopic>thrusters/BR_thrust_cmd</cmdTopic>
-          <angleTopic>thrusters/BR_thrust_angle</angleTopic>
-          <enableAngle>1</enableAngle>
-          <mappingType>1</mappingType>
-          <maxCmd>1.0</maxCmd>
-          <maxForceFwd>250.0</maxForceFwd>
-          <maxForceRev>-100.0</maxForceRev>
-          <maxAngle>1.5707963267948966</maxAngle>
-        </thruster>
-        <thruster>
-          <linkName>FL_propeller_link</linkName>
-          <propJointName>FL_engine_propeller_joint</propJointName>
-          <engineJointName>FL_chasis_engine_joint</engineJointName>
-          <cmdTopic>thrusters/FL_thrust_cmd</cmdTopic>
-          <angleTopic>thrusters/FL_thrust_angle</angleTopic>
-          <enableAngle>1</enableAngle>
-          <mappingType>1</mappingType>
-          <maxCmd>1.0</maxCmd>
-          <maxForceFwd>250.0</maxForceFwd>
-          <maxForceRev>-100.0</maxForceRev>
-          <maxAngle>1.5707963267948966</maxAngle>
-        </thruster>
-        <thruster>
-          <linkName>FR_propeller_link</linkName>
-          <propJointName>FR_engine_propeller_joint</propJointName>
-          <engineJointName>FR_chasis_engine_joint</engineJointName>
-          <cmdTopic>thrusters/FR_thrust_cmd</cmdTopic>
-          <angleTopic>thrusters/FR_thrust_angle</angleTopic>
-          <enableAngle>1</enableAngle>
-          <mappingType>1</mappingType>
-          <maxCmd>1.0</maxCmd>
-          <maxForceFwd>250.0</maxForceFwd>
-          <maxForceRev>-100.0</maxForceRev>
-          <maxAngle>1.5707963267948966</maxAngle>
-        </thruster>
-      </plugin>
-      <plugin name='usv_dynamics_wamv_dynamics_plugin' filename='libusv_gazebo_dynamics_plugin.so'>
-        <bodyName>wamv/base_link</bodyName>
-        <waterLevel>0</waterLevel>
-        <waterDensity>997.8</waterDensity>
-        <xDotU>0.0</xDotU>
-        <yDotV>0.0</yDotV>
-        <nDotR>0.0</nDotR>
-        <xU>51.3</xU>
-        <xUU>72.4</xUU>
-        <yV>40.0</yV>
-        <yVV>0.0</yVV>
-        <zW>500.0</zW>
-        <kP>50.0</kP>
-        <mQ>50.0</mQ>
-        <nR>400.0</nR>
-        <nRR>0.0</nRR>
-        <hullRadius>0.213</hullRadius>
-        <boatWidth>2.4</boatWidth>
-        <boatLength>4.9</boatLength>
-        <length_n>2</length_n>
-        <wave_model>ocean_waves</wave_model>
-        <robotNamespace>/</robotNamespace>
-      </plugin>
-      <frame name='wamv/front_left_cam_to_front_left_cam_link_optical_joint' attached_to='wamv/front_left_cam_link'>
-        <pose>0 0 0 -1.5708 -0 -1.5708</pose>
-      </frame>
-      <frame name='wamv/front_left_cam_link_optical' attached_to='wamv/front_left_cam_to_front_left_cam_link_optical_joint'/>
-      <frame name='wamv/front_left_cam_post_arm_to_front_left_cam_joint' attached_to='wamv/front_left_cam_post_arm_link'>
-        <pose>0.02 0 0.027 0 0.20944 0</pose>
-      </frame>
-      <frame name='wamv/front_left_cam_link' attached_to='wamv/front_left_cam_post_arm_to_front_left_cam_joint'/>
-      <frame name='wamv/front_left_cam_post_to_front_left_cam_post_arm_joint' attached_to='wamv/front_left_cam_post_link'>
-        <pose>0.03 0 0.08825 0 -0 0</pose>
-      </frame>
-      <frame name='wamv/front_left_cam_post_arm_link' attached_to='wamv/front_left_cam_post_to_front_left_cam_post_arm_joint'/>
-      <frame name='wamv/base_to_front_left_cam_post_joint' attached_to='wamv/base_link'>
-        <pose>0.92 0.1 1.38475 0 -0 0</pose>
-      </frame>
-      <frame name='wamv/front_left_cam_post_link' attached_to='wamv/base_to_front_left_cam_post_joint'/>
-      <frame name='wamv/front_right_cam_to_front_right_cam_link_optical_joint' attached_to='wamv/front_right_cam_link'>
-        <pose>0 0 0 -1.5708 -0 -1.5708</pose>
-      </frame>
-      <frame name='wamv/front_right_cam_link_optical' attached_to='wamv/front_right_cam_to_front_right_cam_link_optical_joint'/>
-      <frame name='wamv/front_right_cam_post_arm_to_front_right_cam_joint' attached_to='wamv/front_right_cam_post_arm_link'>
-        <pose>0.02 0 0.027 0 0.20944 0</pose>
-      </frame>
-      <frame name='wamv/front_right_cam_link' attached_to='wamv/front_right_cam_post_arm_to_front_right_cam_joint'/>
-      <frame name='wamv/front_right_cam_post_to_front_right_cam_post_arm_joint' attached_to='wamv/front_right_cam_post_link'>
-        <pose>0.03 0 0.08825 0 -0 0</pose>
-      </frame>
-      <frame name='wamv/front_right_cam_post_arm_link' attached_to='wamv/front_right_cam_post_to_front_right_cam_post_arm_joint'/>
-      <frame name='wamv/base_to_front_right_cam_post_joint' attached_to='wamv/base_link'>
-        <pose>0.92 -0.1 1.38475 0 -0 0</pose>
-      </frame>
-      <frame name='wamv/front_right_cam_post_link' attached_to='wamv/base_to_front_right_cam_post_joint'/>
-      <frame name='wamv/velodyne_post_arm_to_velodyne_joint' attached_to='velodyne_post_arm_link'>
-        <pose>0.04 0 0.05 0 0.261799 0</pose>
-      </frame>
-      <frame name='velodyne' attached_to='wamv/velodyne_post_arm_to_velodyne_joint'/>
-      <frame name='wamv/velodyne_post_to_velodyne_post_arm_joint' attached_to='velodyne_post_link'>
-        <pose>0.03 0 0.32675 0 -0 0</pose>
-      </frame>
-      <frame name='velodyne_post_arm_link' attached_to='wamv/velodyne_post_to_velodyne_post_arm_joint'/>
-      <frame name='wamv/base_to_velodyne_post_joint' attached_to='wamv/base_link'>
-        <pose>0.704 0 1.62325 0 -0 0</pose>
-      </frame>
-      <frame name='velodyne_post_link' attached_to='wamv/base_to_velodyne_post_joint'/>
-      <frame name='wamv/dummy_joint' attached_to='wamv/base_link'>
-        <pose>0 0 0 0 -0 0</pose>
-      </frame>
-      <frame name='wamv/dummy_link' attached_to='wamv/dummy_joint'/>
-      <frame name='wamv/hydrophones_pinger_joint' attached_to='wamv/base_link'>
-        <pose>1 0 -1 0 -0 0</pose>
-      </frame>
-      <frame name='wamv/hydrophones' attached_to='wamv/hydrophones_pinger_joint'/>
-      <pose>-527 167 0.1 0 -0 1</pose>
-    </model>
-    <model name='polyform_a2_red'>
+    <model name='red_circular_buoy'>
       <static>0</static>
       <pose>-522.515 184.985 0 0 -0 0</pose>
       <link name='base_link'>
@@ -3634,7 +1508,7 @@
         </buoyancy>
       </plugin>
     </model>
-    <model name='polyform_a2_green'>
+    <model name='green_circular_buoy'>
       <static>0</static>
       <pose>-534.625 187.168 0 0 -0 0</pose>
       <link name='base_link'>
@@ -3703,7 +1577,7 @@
         </buoyancy>
       </plugin>
     </model>
-    <model name='polyform_a2_blue'>
+    <model name='blue_circular_buoy'>
       <static>0</static>
       <pose>-531.036 180.328 0 0 -0 0</pose>
       <link name='base_link'>
@@ -3947,7 +1821,7 @@
           <wrench>0.629708 -1.07887 -6.3e-05 0 -0 0</wrench>
         </link>
       </model>
-      <model name='polyform_a2_blue'>
+      <model name='blue_circular_buoy'>
         <pose>-531.036 180.328 0.10507 0 -0 0</pose>
         <scale>1 1 1</scale>
         <link name='base_link'>
@@ -3957,7 +1831,7 @@
           <wrench>0 0 -0 0 -0 0</wrench>
         </link>
       </model>
-      <model name='polyform_a2_green'>
+      <model name='green_circular_buoy'>
         <pose>-522.625 184.168 0.10507 0 -0 0</pose>
         <scale>1 1 1</scale>
         <link name='base_link'>
@@ -3967,7 +1841,7 @@
           <wrench>0 0 -0 0 -0 0</wrench>
         </link>
       </model>
-      <model name='polyform_a2_red'>
+      <model name='red_circular_buoy'>
         <pose>-534.515 187.985 0.10507 0 -0 0</pose>
         <scale>1 1 1</scale>
         <link name='base_link'>
diff --git a/NaviGator/simulation/navigator_gazebo/worlds/wildlife/wildlife_encounter_test_case_5.world b/NaviGator/simulation/navigator_gazebo/worlds/wildlife/wildlife_encounter_test_case_5.world
index 145e46afe..10108cd05 100644
--- a/NaviGator/simulation/navigator_gazebo/worlds/wildlife/wildlife_encounter_test_case_5.world
+++ b/NaviGator/simulation/navigator_gazebo/worlds/wildlife/wildlife_encounter_test_case_5.world
@@ -1439,2133 +1439,7 @@
       <real_time_update_rate>1000</real_time_update_rate>
     </physics>
     <wind/>
-    <model name='wamv'>
-      <link name='wamv/base_link'>
-        <inertial>
-          <pose>0.012722 0 0.026171 0 -0 0</pose>
-          <mass>182.806</mass>
-          <inertia>
-            <ixx>128.232</ixx>
-            <ixy>-2.4178e-21</ixy>
-            <ixz>-3.85993</ixz>
-            <iyy>403.15</iyy>
-            <iyz>-9.28107e-22</iyz>
-            <izz>447.942</izz>
-          </inertia>
-        </inertial>
-        <collision name='wamv/base_link_fixed_joint_lump__front_left_cam_post_collision_collision'>
-          <pose>0.92 0.1 1.38475 0 -0 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.1765</length>
-              <radius>0.0076</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__front_left_cam_post_arm_collision_collision_1'>
-          <pose>0.95 0.1 1.473 -1.20428 -0 -1.5708</pose>
-          <geometry>
-            <cylinder>
-              <length>0.065</length>
-              <radius>0.008</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__front_left_cam_lens_collision_collision_2'>
-          <pose>0.987607 0.1 1.49626 3.14159 1.36136 3.14159</pose>
-          <geometry>
-            <cylinder>
-              <length>0.042</length>
-              <radius>0.015</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__front_left_cam_box_collision_collision_3'>
-          <pose>0.949948 0.1 1.50426 0 0.20944 0</pose>
-          <geometry>
-            <box>
-              <size>0.036 0.03 0.03</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__front_right_cam_post_collision_collision_4'>
-          <pose>0.92 -0.1 1.38475 0 -0 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.1765</length>
-              <radius>0.0076</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__front_right_cam_post_arm_collision_collision_5'>
-          <pose>0.95 -0.1 1.473 -1.20428 -0 -1.5708</pose>
-          <geometry>
-            <cylinder>
-              <length>0.065</length>
-              <radius>0.008</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__front_right_cam_lens_collision_collision_6'>
-          <pose>0.987607 -0.1 1.49626 3.14159 1.36136 3.14159</pose>
-          <geometry>
-            <cylinder>
-              <length>0.042</length>
-              <radius>0.015</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__front_right_cam_box_collision_collision_7'>
-          <pose>0.949948 -0.1 1.50426 0 0.20944 0</pose>
-          <geometry>
-            <box>
-              <size>0.036 0.03 0.03</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__velodyne_post_collision_collision_8'>
-          <pose>0.704 0 1.62325 0 -0 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.6535</length>
-              <radius>0.0076</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__velodyne_post_arm_collision_collision_9'>
-          <pose>0.734 0 1.95 -1.20428 -0 -1.5708</pose>
-          <geometry>
-            <cylinder>
-              <length>0.065</length>
-              <radius>0.008</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__velodyne_collision_collision_10'>
-          <pose>0.774 0 2 0 0.261799 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.075</length>
-              <radius>0.055</radius>
-            </cylinder>
-          </geometry>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <friction>
-              <ode/>
-              <torsional>
-                <ode/>
-              </torsional>
-            </friction>
-            <bounce/>
-          </surface>
-          <max_contacts>10</max_contacts>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__left_float_collision_11'>
-          <pose>-0.4 1.03 0.2 3.14159 1.57079 3.14159</pose>
-          <geometry>
-            <cylinder>
-              <length>4</length>
-              <radius>0.2</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__left_mid_float_collision_12'>
-          <pose>1.85 1.03 0.3 0 1.38 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.5</length>
-              <radius>0.17</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__left_front_float_collision_13'>
-          <pose>2.3 1.03 0.4 0 1.3 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.45</length>
-              <radius>0.12</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__front_left_beam_lower_collision_14'>
-          <pose>0.9 0.85 1 0.78 -0 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.5</length>
-              <radius>0.04</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__front_left_beam_upper_collision_15'>
-          <pose>0.9 0.6 1.18 1.35 -0 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.2</length>
-              <radius>0.04</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__mid_left_beam_lower_collision_16'>
-          <pose>-0.65 0.99 0.7 0.1 0.25 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.45</length>
-              <radius>0.03</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__mid_left_beam_medium_collision_17'>
-          <pose>-0.57 0.87 1.05 0.75 0.25 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.32</length>
-              <radius>0.03</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__mid_left_beam_upper_collision_18'>
-          <pose>-0.55 0.65 1.17 1.35 0.25 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.3</length>
-              <radius>0.03</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__rear_left_beam_lower_collision_19'>
-          <pose>-0.74 1.03 0.7 0 -0.15 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.45</length>
-              <radius>0.03</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__rear_left_beam_medium_collision_20'>
-          <pose>-0.79 0.91 1.05 0.75 -0.15 -0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.32</length>
-              <radius>0.03</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__rear_left_beam_upper_collision_21'>
-          <pose>-0.81 0.67 1.18 1.45 -0.15 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.3</length>
-              <radius>0.03</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__left_joint_collision_22'>
-          <pose>0.58 1.03 0.6 0 -0.6 0</pose>
-          <geometry>
-            <box>
-              <size>0.65 0.2 0.1</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__right_float_collision_23'>
-          <pose>-0.4 -1.03 0.2 3.14159 1.57079 3.14159</pose>
-          <geometry>
-            <cylinder>
-              <length>4</length>
-              <radius>0.2</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__right_mid_float_collision_24'>
-          <pose>1.85 -1.03 0.3 0 1.38 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.5</length>
-              <radius>0.17</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__right_front_float_collision_25'>
-          <pose>2.3 -1.03 0.4 0 1.3 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.45</length>
-              <radius>0.12</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__front_right_beam_lower_collision_26'>
-          <pose>0.9 -0.85 1 -0.78 0 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.5</length>
-              <radius>0.04</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__front_right_beam_upper_collision_27'>
-          <pose>0.9 -0.6 1.18 -1.35 0 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.2</length>
-              <radius>0.04</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__mid_right_beam_lower_collision_28'>
-          <pose>-0.65 -0.99 0.7 -0.1 0.25 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.45</length>
-              <radius>0.03</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__mid_right_beam_medium_collision_29'>
-          <pose>-0.57 -0.87 1.05 -0.75 0.25 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.32</length>
-              <radius>0.03</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__mid_right_beam_upper_collision_30'>
-          <pose>-0.55 -0.65 1.17 -1.35 0.25 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.3</length>
-              <radius>0.03</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__rear_right_beam_lower_collision_31'>
-          <pose>-0.74 -1.03 0.7 0 -0.15 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.45</length>
-              <radius>0.03</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__rear_right_beam_medium_collision_32'>
-          <pose>-0.79 -0.91 1.05 -0.75 -0.15 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.32</length>
-              <radius>0.03</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__rear_right_beam_upper_collision_33'>
-          <pose>-0.81 -0.67 1.18 -1.45 -0.15 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.3</length>
-              <radius>0.03</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__right_joint_collision_34'>
-          <pose>0.58 -1.03 0.6 0 -0.6 0</pose>
-          <geometry>
-            <box>
-              <size>0.65 0.2 0.1</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__top_base_collision_35'>
-          <pose>0 0 1.25 0 -0 0</pose>
-          <geometry>
-            <box>
-              <size>1.85 1 0.1</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__hydrophones_collision_collision_36'>
-          <pose>1 0 -1 0 -0 0</pose>
-          <geometry>
-            <box>
-              <size>0.1 0.1 0.1</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <visual name='wamv/base_link_fixed_joint_lump__wamv/front_left_cam_post_link_visual'>
-          <pose>0.92 0.1 1.38475 0 -0 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.1765</length>
-              <radius>0.0076</radius>
-            </cylinder>
-          </geometry>
-          <material>
-            <script>
-              <name>Gazebo/Black</name>
-              <uri>file://media/materials/scripts/gazebo.material</uri>
-            </script>
-          </material>
-        </visual>
-        <visual name='wamv/base_link_fixed_joint_lump__front_left_cam_post_arm_visual_visual_1'>
-          <pose>0.912 0.1 1.47 -1.0472 -0 -1.5708</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/vrx_gazebo/models/sensor_post/mesh/sensor_post_arm.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <visual name='wamv/base_link_fixed_joint_lump__front_left_cam_visual_visual_2'>
-          <pose>0.937721 0.1 1.50686 0.20944 -0 1.5708</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/vrx_gazebo/models/mono_camera/mesh/mono_camera.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <visual name='wamv/base_link_fixed_joint_lump__wamv/front_right_cam_post_link_visual_3'>
-          <pose>0.92 -0.1 1.38475 0 -0 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.1765</length>
-              <radius>0.0076</radius>
-            </cylinder>
-          </geometry>
-          <material>
-            <script>
-              <name>Gazebo/Black</name>
-              <uri>file://media/materials/scripts/gazebo.material</uri>
-            </script>
-          </material>
-        </visual>
-        <visual name='wamv/base_link_fixed_joint_lump__front_right_cam_post_arm_visual_visual_4'>
-          <pose>0.912 -0.1 1.47 -1.0472 -0 -1.5708</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/vrx_gazebo/models/sensor_post/mesh/sensor_post_arm.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <visual name='wamv/base_link_fixed_joint_lump__front_right_cam_visual_visual_5'>
-          <pose>0.937721 -0.1 1.50686 0.20944 -0 1.5708</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/vrx_gazebo/models/mono_camera/mesh/mono_camera.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <visual name='wamv/base_link_fixed_joint_lump__velodyne_post_link_visual_6'>
-          <pose>0.704 0 1.62325 0 -0 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.6535</length>
-              <radius>0.0076</radius>
-            </cylinder>
-          </geometry>
-          <material>
-            <script>
-              <name>Gazebo/Black</name>
-              <uri>file://media/materials/scripts/gazebo.material</uri>
-            </script>
-          </material>
-        </visual>
-        <visual name='wamv/base_link_fixed_joint_lump__velodyne_post_arm_visual_visual_7'>
-          <pose>0.696 0 1.947 -1.0472 -0 -1.5708</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/vrx_gazebo/models/sensor_post/mesh/sensor_post_arm.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <visual name='wamv/base_link_fixed_joint_lump__velodyne_visual_visual_8'>
-          <pose>0.764941 0 1.96619 0 0.261799 0</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/vrx_gazebo/models/3d_lidar/mesh/3d_lidar.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <visual name='wamv/base_link_fixed_joint_lump__wamv/dummy_link_visual_9'>
-          <pose>0 0 0 0 -0 0</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/wamv_description/models/WAM-V-Base/mesh/WAM-V-Base.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <visual name='wamv/base_link_fixed_joint_lump__wamv/hydrophones_visual_10'>
-          <pose>1 0 -1 0 -0 0</pose>
-          <geometry>
-            <box>
-              <size>0.1 0.1 0.1</size>
-            </box>
-          </geometry>
-        </visual>
-        <sensor name='front_left_cam_sensor' type='camera'>
-          <update_rate>30</update_rate>
-          <camera name='front_left_cam_camera'>
-            <horizontal_fov>1.39626</horizontal_fov>
-            <image>
-              <width>1280</width>
-              <height>720</height>
-              <format>R8G8B8</format>
-            </image>
-            <clip>
-              <near>0.05</near>
-              <far>300</far>
-            </clip>
-            <noise>
-              <type>gaussian</type>
-              <mean>0</mean>
-              <stddev>0.007</stddev>
-            </noise>
-          </camera>
-          <plugin name='camera_plugin_front_left_cam' filename='libgazebo_ros_camera.so'>
-            <alwaysOn>1</alwaysOn>
-            <updateRate>0.0</updateRate>
-            <cameraName>wamv/sensors/camera/front_left_cam</cameraName>
-            <imageTopicName>image_raw</imageTopicName>
-            <cameraInfoTopicName>camera_info</cameraInfoTopicName>
-            <frameName>wamv/front_left_cam_link_optical</frameName>
-            <hackBaseline>0.07</hackBaseline>
-            <distortionK1>0.0</distortionK1>
-            <distortionK2>0.0</distortionK2>
-            <distortionK3>0.0</distortionK3>
-            <distortionT1>0.0</distortionT1>
-            <distortionT2>0.0</distortionT2>
-            <robotNamespace>/</robotNamespace>
-          </plugin>
-          <pose>0.97 0.1 1.5 0 0.20944 0</pose>
-        </sensor>
-        <sensor name='front_right_cam_sensor' type='camera'>
-          <update_rate>30</update_rate>
-          <camera name='front_right_cam_camera'>
-            <horizontal_fov>1.39626</horizontal_fov>
-            <image>
-              <width>1280</width>
-              <height>720</height>
-              <format>R8G8B8</format>
-            </image>
-            <clip>
-              <near>0.05</near>
-              <far>300</far>
-            </clip>
-            <noise>
-              <type>gaussian</type>
-              <mean>0</mean>
-              <stddev>0.007</stddev>
-            </noise>
-          </camera>
-          <plugin name='camera_plugin_front_right_cam' filename='libgazebo_ros_camera.so'>
-            <alwaysOn>1</alwaysOn>
-            <updateRate>0.0</updateRate>
-            <cameraName>wamv/sensors/camera/front_right_cam</cameraName>
-            <imageTopicName>image_raw</imageTopicName>
-            <cameraInfoTopicName>camera_info</cameraInfoTopicName>
-            <frameName>wamv/front_right_cam_link_optical</frameName>
-            <hackBaseline>0.07</hackBaseline>
-            <distortionK1>0.0</distortionK1>
-            <distortionK2>0.0</distortionK2>
-            <distortionK3>0.0</distortionK3>
-            <distortionT1>0.0</distortionT1>
-            <distortionT2>0.0</distortionT2>
-            <robotNamespace>/</robotNamespace>
-          </plugin>
-          <pose>0.97 -0.1 1.5 0 0.20944 0</pose>
-        </sensor>
-        <sensor name='velodyne_sensor' type='gpu_ray'>
-          <update_rate>10</update_rate>
-          <ray>
-            <scan>
-              <horizontal>
-                <samples>18750</samples>
-                <resolution>0.1</resolution>
-                <min_angle>-3.14159</min_angle>
-                <max_angle>3.14159</max_angle>
-              </horizontal>
-              <vertical>
-                <samples>16</samples>
-                <resolution>1</resolution>
-                <min_angle>-0.261799</min_angle>
-                <max_angle>0.261799</max_angle>
-              </vertical>
-            </scan>
-            <range>
-              <min>0.1</min>
-              <max>130</max>
-            </range>
-            <noise>
-              <type>gaussian</type>
-              <mean>0</mean>
-              <stddev>0.01</stddev>
-            </noise>
-          </ray>
-          <plugin name='velodyne_plugin' filename='libgazebo_ros_velodyne_gpu_laser.so'>
-            <topicName>velodyne_points</topicName>
-            <frameName>velodyne</frameName>
-            <min_intensity>0</min_intensity>
-            <robotNamespace>/</robotNamespace>
-          </plugin>
-          <pose>0.774 0 2 0 0.261799 0</pose>
-        </sensor>
-        <self_collide>0</self_collide>
-        <enable_wind>0</enable_wind>
-        <kinematic>0</kinematic>
-      </link>
-      <joint name='BL_chasis_engine_joint' type='revolute'>
-        <pose relative_to='wamv/base_link'>-1.9304 1.1 0.318237 0 -0 0.785398</pose>
-        <parent>wamv/base_link</parent>
-        <child>BL_engine_link</child>
-        <axis>
-          <xyz>0 0 1</xyz>
-          <limit>
-            <lower>-3.14159</lower>
-            <upper>3.14159</upper>
-            <effort>10</effort>
-            <velocity>10</velocity>
-          </limit>
-          <dynamics>
-            <spring_reference>0</spring_reference>
-            <spring_stiffness>0</spring_stiffness>
-          </dynamics>
-        </axis>
-      </joint>
-      <link name='BL_engine_link'>
-        <pose relative_to='BL_chasis_engine_joint'>0 0 0 0 -0 0</pose>
-        <inertial>
-          <pose>0 0 0 0 -0 0</pose>
-          <mass>15</mass>
-          <inertia>
-            <ixx>0.889245</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>0.911125</iyy>
-            <iyz>0</iyz>
-            <izz>0.078125</izz>
-          </inertia>
-        </inertial>
-        <collision name='BL_engine_link_fixed_joint_lump__BL_engine_vertical_axis_collision_collision'>
-          <pose>-0.16 0 -0.24 0 -0 0</pose>
-          <geometry>
-            <box>
-              <size>0.2 0.15 0.83</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='BL_engine_link_fixed_joint_lump__BL_engine_rear_end_collision_collision_1'>
-          <pose>-0.34 0 0.12 0 -0 0</pose>
-          <geometry>
-            <box>
-              <size>0.12 0.15 0.12</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <visual name='BL_engine_link_visual'>
-          <pose>0 0 0 0 -0 0</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/wamv_description/models/engine/mesh/engine.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <self_collide>0</self_collide>
-        <enable_wind>0</enable_wind>
-        <kinematic>0</kinematic>
-      </link>
-      <joint name='BL_engine_propeller_joint' type='revolute'>
-        <pose relative_to='BL_engine_link'>-0.278156 0 -0.509371 0 -0 0</pose>
-        <parent>BL_engine_link</parent>
-        <child>BL_propeller_link</child>
-        <axis>
-          <xyz>1 0 0</xyz>
-          <limit>
-            <effort>400</effort>
-            <velocity>100</velocity>
-            <lower>-1e+16</lower>
-            <upper>1e+16</upper>
-          </limit>
-          <dynamics>
-            <damping>0.05</damping>
-            <friction>0.05</friction>
-            <spring_reference>0</spring_reference>
-            <spring_stiffness>0</spring_stiffness>
-          </dynamics>
-        </axis>
-      </joint>
-      <link name='BL_propeller_link'>
-        <pose relative_to='BL_engine_propeller_joint'>0 0 0 0 -0 0</pose>
-        <inertial>
-          <pose>0 0 0 0 -0 0</pose>
-          <mass>0.5</mass>
-          <inertia>
-            <ixx>0.008545</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>0.008545</iyy>
-            <iyz>0</iyz>
-            <izz>0.0144</izz>
-          </inertia>
-        </inertial>
-        <collision name='BL_propeller_link_fixed_joint_lump__BL_propeller_collision_collision'>
-          <pose>-0.08 0 0 3.14159 1.57079 3.14159</pose>
-          <geometry>
-            <cylinder>
-              <length>0.18</length>
-              <radius>0.24</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <visual name='BL_propeller_link_visual'>
-          <pose>0 0 0 0 -0 0</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/wamv_description/models/propeller/mesh/propeller.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <self_collide>0</self_collide>
-        <enable_wind>0</enable_wind>
-        <kinematic>0</kinematic>
-      </link>
-      <joint name='BR_chasis_engine_joint' type='revolute'>
-        <pose relative_to='wamv/base_link'>-1.9304 -1.1 0.318237 0 0 -0.785398</pose>
-        <parent>wamv/base_link</parent>
-        <child>BR_engine_link</child>
-        <axis>
-          <xyz>0 0 1</xyz>
-          <limit>
-            <lower>-3.14159</lower>
-            <upper>3.14159</upper>
-            <effort>10</effort>
-            <velocity>10</velocity>
-          </limit>
-          <dynamics>
-            <spring_reference>0</spring_reference>
-            <spring_stiffness>0</spring_stiffness>
-          </dynamics>
-        </axis>
-      </joint>
-      <link name='BR_engine_link'>
-        <pose relative_to='BR_chasis_engine_joint'>0 0 0 0 -0 0</pose>
-        <inertial>
-          <pose>0 0 0 0 -0 0</pose>
-          <mass>15</mass>
-          <inertia>
-            <ixx>0.889245</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>0.911125</iyy>
-            <iyz>0</iyz>
-            <izz>0.078125</izz>
-          </inertia>
-        </inertial>
-        <collision name='BR_engine_link_fixed_joint_lump__BR_engine_vertical_axis_collision_collision'>
-          <pose>-0.16 0 -0.24 0 -0 0</pose>
-          <geometry>
-            <box>
-              <size>0.2 0.15 0.83</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='BR_engine_link_fixed_joint_lump__BR_engine_rear_end_collision_collision_1'>
-          <pose>-0.34 0 0.12 0 -0 0</pose>
-          <geometry>
-            <box>
-              <size>0.12 0.15 0.12</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <visual name='BR_engine_link_visual'>
-          <pose>0 0 0 0 -0 0</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/wamv_description/models/engine/mesh/engine.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <self_collide>0</self_collide>
-        <enable_wind>0</enable_wind>
-        <kinematic>0</kinematic>
-      </link>
-      <joint name='BR_engine_propeller_joint' type='revolute'>
-        <pose relative_to='BR_engine_link'>-0.278156 0 -0.509371 0 -0 0</pose>
-        <parent>BR_engine_link</parent>
-        <child>BR_propeller_link</child>
-        <axis>
-          <xyz>1 0 0</xyz>
-          <limit>
-            <effort>400</effort>
-            <velocity>100</velocity>
-            <lower>-1e+16</lower>
-            <upper>1e+16</upper>
-          </limit>
-          <dynamics>
-            <damping>0.05</damping>
-            <friction>0.05</friction>
-            <spring_reference>0</spring_reference>
-            <spring_stiffness>0</spring_stiffness>
-          </dynamics>
-        </axis>
-      </joint>
-      <link name='BR_propeller_link'>
-        <pose relative_to='BR_engine_propeller_joint'>0 0 0 0 -0 0</pose>
-        <inertial>
-          <pose>0 0 0 0 -0 0</pose>
-          <mass>0.5</mass>
-          <inertia>
-            <ixx>0.008545</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>0.008545</iyy>
-            <iyz>0</iyz>
-            <izz>0.0144</izz>
-          </inertia>
-        </inertial>
-        <collision name='BR_propeller_link_fixed_joint_lump__BR_propeller_collision_collision'>
-          <pose>-0.08 0 0 3.14159 1.57079 3.14159</pose>
-          <geometry>
-            <cylinder>
-              <length>0.18</length>
-              <radius>0.24</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <visual name='BR_propeller_link_visual'>
-          <pose>0 0 0 0 -0 0</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/wamv_description/models/propeller/mesh/propeller.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <self_collide>0</self_collide>
-        <enable_wind>0</enable_wind>
-        <kinematic>0</kinematic>
-      </link>
-      <joint name='FL_chasis_engine_joint' type='revolute'>
-        <pose relative_to='wamv/base_link'>1.46 0.54 0.49675 0 0 -0.785398</pose>
-        <parent>wamv/base_link</parent>
-        <child>FL_engine_link</child>
-        <axis>
-          <xyz>0 0 1</xyz>
-          <limit>
-            <lower>-3.14159</lower>
-            <upper>3.14159</upper>
-            <effort>10</effort>
-            <velocity>10</velocity>
-          </limit>
-          <dynamics>
-            <spring_reference>0</spring_reference>
-            <spring_stiffness>0</spring_stiffness>
-          </dynamics>
-        </axis>
-      </joint>
-      <link name='FL_engine_link'>
-        <pose relative_to='FL_chasis_engine_joint'>0 0 0 0 -0 0</pose>
-        <inertial>
-          <pose>0 0 0 0 -0 0</pose>
-          <mass>15</mass>
-          <inertia>
-            <ixx>0.889245</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>0.911125</iyy>
-            <iyz>0</iyz>
-            <izz>0.078125</izz>
-          </inertia>
-        </inertial>
-        <collision name='FL_engine_link_fixed_joint_lump__FL_engine_vertical_axis_collision_collision'>
-          <pose>-0.16 0 -0.24 0 -0 0</pose>
-          <geometry>
-            <box>
-              <size>0.2 0.15 0.83</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='FL_engine_link_fixed_joint_lump__FL_engine_rear_end_collision_collision_1'>
-          <pose>-0.34 0 0.12 0 -0 0</pose>
-          <geometry>
-            <box>
-              <size>0.12 0.15 0.12</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <visual name='FL_engine_link_visual'>
-          <pose>0 0 0 0 -0 0</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/wamv_description/models/engine/mesh/engine.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <self_collide>0</self_collide>
-        <enable_wind>0</enable_wind>
-        <kinematic>0</kinematic>
-      </link>
-      <joint name='FL_engine_propeller_joint' type='revolute'>
-        <pose relative_to='FL_engine_link'>-0.278156 0 -0.509371 0 -0 0</pose>
-        <parent>FL_engine_link</parent>
-        <child>FL_propeller_link</child>
-        <axis>
-          <xyz>1 0 0</xyz>
-          <limit>
-            <effort>400</effort>
-            <velocity>100</velocity>
-            <lower>-1e+16</lower>
-            <upper>1e+16</upper>
-          </limit>
-          <dynamics>
-            <damping>0.05</damping>
-            <friction>0.05</friction>
-            <spring_reference>0</spring_reference>
-            <spring_stiffness>0</spring_stiffness>
-          </dynamics>
-        </axis>
-      </joint>
-      <link name='FL_propeller_link'>
-        <pose relative_to='FL_engine_propeller_joint'>0 0 0 0 -0 0</pose>
-        <inertial>
-          <pose>0 0 0 0 -0 0</pose>
-          <mass>0.5</mass>
-          <inertia>
-            <ixx>0.008545</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>0.008545</iyy>
-            <iyz>0</iyz>
-            <izz>0.0144</izz>
-          </inertia>
-        </inertial>
-        <collision name='FL_propeller_link_fixed_joint_lump__FL_propeller_collision_collision'>
-          <pose>-0.08 0 0 3.14159 1.57079 3.14159</pose>
-          <geometry>
-            <cylinder>
-              <length>0.18</length>
-              <radius>0.24</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <visual name='FL_propeller_link_visual'>
-          <pose>0 0 0 0 -0 0</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/wamv_description/models/propeller/mesh/propeller.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <self_collide>0</self_collide>
-        <enable_wind>0</enable_wind>
-        <kinematic>0</kinematic>
-      </link>
-      <joint name='FR_chasis_engine_joint' type='revolute'>
-        <pose relative_to='wamv/base_link'>1.46 -0.54 0.49657 0 -0 0.785398</pose>
-        <parent>wamv/base_link</parent>
-        <child>FR_engine_link</child>
-        <axis>
-          <xyz>0 0 1</xyz>
-          <limit>
-            <lower>-3.14159</lower>
-            <upper>3.14159</upper>
-            <effort>10</effort>
-            <velocity>10</velocity>
-          </limit>
-          <dynamics>
-            <spring_reference>0</spring_reference>
-            <spring_stiffness>0</spring_stiffness>
-          </dynamics>
-        </axis>
-      </joint>
-      <link name='FR_engine_link'>
-        <pose relative_to='FR_chasis_engine_joint'>0 0 0 0 -0 0</pose>
-        <inertial>
-          <pose>0 0 0 0 -0 0</pose>
-          <mass>15</mass>
-          <inertia>
-            <ixx>0.889245</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>0.911125</iyy>
-            <iyz>0</iyz>
-            <izz>0.078125</izz>
-          </inertia>
-        </inertial>
-        <collision name='FR_engine_link_fixed_joint_lump__FR_engine_vertical_axis_collision_collision'>
-          <pose>-0.16 0 -0.24 0 -0 0</pose>
-          <geometry>
-            <box>
-              <size>0.2 0.15 0.83</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='FR_engine_link_fixed_joint_lump__FR_engine_rear_end_collision_collision_1'>
-          <pose>-0.34 0 0.12 0 -0 0</pose>
-          <geometry>
-            <box>
-              <size>0.12 0.15 0.12</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <visual name='FR_engine_link_visual'>
-          <pose>0 0 0 0 -0 0</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/wamv_description/models/engine/mesh/engine.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <self_collide>0</self_collide>
-        <enable_wind>0</enable_wind>
-        <kinematic>0</kinematic>
-      </link>
-      <joint name='FR_engine_propeller_joint' type='revolute'>
-        <pose relative_to='FR_engine_link'>-0.278156 0 -0.509371 0 -0 0</pose>
-        <parent>FR_engine_link</parent>
-        <child>FR_propeller_link</child>
-        <axis>
-          <xyz>1 0 0</xyz>
-          <limit>
-            <effort>400</effort>
-            <velocity>100</velocity>
-            <lower>-1e+16</lower>
-            <upper>1e+16</upper>
-          </limit>
-          <dynamics>
-            <damping>0.05</damping>
-            <friction>0.05</friction>
-            <spring_reference>0</spring_reference>
-            <spring_stiffness>0</spring_stiffness>
-          </dynamics>
-        </axis>
-      </joint>
-      <link name='FR_propeller_link'>
-        <pose relative_to='FR_engine_propeller_joint'>0 0 0 0 -0 0</pose>
-        <inertial>
-          <pose>0 0 0 0 -0 0</pose>
-          <mass>0.5</mass>
-          <inertia>
-            <ixx>0.008545</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>0.008545</iyy>
-            <iyz>0</iyz>
-            <izz>0.0144</izz>
-          </inertia>
-        </inertial>
-        <collision name='FR_propeller_link_fixed_joint_lump__FR_propeller_collision_collision'>
-          <pose>-0.08 0 0 3.14159 1.57079 3.14159</pose>
-          <geometry>
-            <cylinder>
-              <length>0.18</length>
-              <radius>0.24</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <visual name='FR_propeller_link_visual'>
-          <pose>0 0 0 0 -0 0</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/wamv_description/models/propeller/mesh/propeller.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <self_collide>0</self_collide>
-        <enable_wind>0</enable_wind>
-        <kinematic>0</kinematic>
-      </link>
-      <joint name='wamv/ball_shooter_base_joint' type='revolute'>
-        <pose relative_to='wamv/base_link'>0.54 0.3 1.3 0 -0 0.1</pose>
-        <parent>wamv/base_link</parent>
-        <child>wamv/ball_shooter_base_link</child>
-        <axis>
-          <xyz>0 0 1</xyz>
-          <limit>
-            <lower>0</lower>
-            <upper>0</upper>
-            <effort>1000</effort>
-            <velocity>0</velocity>
-          </limit>
-          <dynamics>
-            <spring_reference>0</spring_reference>
-            <spring_stiffness>0</spring_stiffness>
-          </dynamics>
-        </axis>
-      </joint>
-      <link name='wamv/ball_shooter_base_link'>
-        <pose relative_to='wamv/ball_shooter_base_joint'>0 0 0 0 -0 0</pose>
-        <inertial>
-          <pose>-0.02 0 0.05 0 -0 0</pose>
-          <mass>0.5</mass>
-          <inertia>
-            <ixx>0.00083</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>0.00052083</iyy>
-            <iyz>0</iyz>
-            <izz>0.00052083</izz>
-          </inertia>
-        </inertial>
-        <visual name='wamv/ball_shooter_base_link_fixed_joint_lump__ball_shooter_base_visual_visual'>
-          <pose>0 0 0 0 -0 0</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/vrx_gazebo/models/ball_shooter/meshes/ball_shooter_base.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <self_collide>0</self_collide>
-        <enable_wind>0</enable_wind>
-        <kinematic>0</kinematic>
-      </link>
-      <joint name='wamv/ball_shooter_launcher_joint' type='revolute'>
-        <pose relative_to='wamv/ball_shooter_base_link'>-0.0204 0 0.107877 0 -0.8 0</pose>
-        <parent>wamv/ball_shooter_base_link</parent>
-        <child>wamv/ball_shooter_launcher_link</child>
-        <axis>
-          <xyz>0 0 1</xyz>
-          <limit>
-            <lower>0</lower>
-            <upper>0</upper>
-            <effort>1000</effort>
-            <velocity>0</velocity>
-          </limit>
-          <dynamics>
-            <spring_reference>0</spring_reference>
-            <spring_stiffness>0</spring_stiffness>
-          </dynamics>
-        </axis>
-      </joint>
-      <link name='wamv/ball_shooter_launcher_link'>
-        <pose relative_to='wamv/ball_shooter_launcher_joint'>0 0 0 0 -0 0</pose>
-        <inertial>
-          <pose>0 0 0 3.14159 -1.57079 3.14159</pose>
-          <mass>0.5</mass>
-          <inertia>
-            <ixx>0.00177917</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>0.00177917</iyy>
-            <iyz>0</iyz>
-            <izz>0.000225</izz>
-          </inertia>
-        </inertial>
-        <collision name='wamv/ball_shooter_launcher_link_fixed_joint_lump__ball_shooter_collision_collision'>
-          <pose>0 0 0 0 0.261799 0</pose>
-          <geometry>
-            <box>
-              <size>0.3 0.3 0.2</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <visual name='wamv/ball_shooter_launcher_link_fixed_joint_lump__ball_shooter_launcher_visual_visual'>
-          <pose>0 0 0 0 0.261799 0</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/vrx_gazebo/models/ball_shooter/meshes/ball_shooter_launcher.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <self_collide>0</self_collide>
-        <enable_wind>0</enable_wind>
-        <kinematic>0</kinematic>
-      </link>
-      <joint name='wamv/imu_wamv_joint' type='revolute'>
-        <pose relative_to='wamv/base_link'>0.24 0.17 1.5 0 -0 0</pose>
-        <parent>wamv/base_link</parent>
-        <child>wamv/imu_wamv_link</child>
-        <axis>
-          <xyz>0 0 1</xyz>
-          <limit>
-            <lower>0</lower>
-            <upper>0</upper>
-            <effort>1000</effort>
-            <velocity>0</velocity>
-          </limit>
-          <dynamics>
-            <spring_reference>0</spring_reference>
-            <spring_stiffness>0</spring_stiffness>
-          </dynamics>
-        </axis>
-      </joint>
-      <link name='wamv/imu_wamv_link'>
-        <pose relative_to='wamv/imu_wamv_joint'>0 0 0 0 -0 0</pose>
-        <inertial>
-          <pose>0 0 0 0 -0 0</pose>
-          <mass>0.1</mass>
-          <inertia>
-            <ixx>8.3e-05</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>8.3e-05</iyy>
-            <iyz>0</iyz>
-            <izz>0.0125</izz>
-          </inertia>
-        </inertial>
-        <visual name='wamv/imu_wamv_link_fixed_joint_lump__imu_wamv_visual_visual'>
-          <pose>0 0 0 0 -0 0</pose>
-          <geometry>
-            <box>
-              <size>0.05 0.025 0.005</size>
-            </box>
-          </geometry>
-        </visual>
-        <self_collide>0</self_collide>
-        <enable_wind>0</enable_wind>
-        <kinematic>0</kinematic>
-      </link>
-      <joint name='wamv/ins_joint' type='revolute'>
-        <pose relative_to='wamv/base_link'>0.24 0.17 1.5 0 -0 0</pose>
-        <parent>wamv/base_link</parent>
-        <child>wamv/ins_link</child>
-        <axis>
-          <xyz>0 0 1</xyz>
-          <limit>
-            <lower>0</lower>
-            <upper>0</upper>
-            <effort>1000</effort>
-            <velocity>0</velocity>
-          </limit>
-          <dynamics>
-            <spring_reference>0</spring_reference>
-            <spring_stiffness>0</spring_stiffness>
-          </dynamics>
-        </axis>
-      </joint>
-      <link name='wamv/ins_link'>
-        <pose relative_to='wamv/ins_joint'>0 0 0 0 -0 0</pose>
-        <inertial>
-          <pose>0 0 0 0 -0 0</pose>
-          <mass>1</mass>
-          <inertia>
-            <ixx>0.006458</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>0.006458</iyy>
-            <iyz>0</iyz>
-            <izz>0.01125</izz>
-          </inertia>
-        </inertial>
-        <collision name='wamv/ins_link_fixed_joint_lump__ins_collision_base_collision'>
-          <pose>0 0 0.025 0 -0 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.05</length>
-              <radius>0.015</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/ins_link_fixed_joint_lump__ins_collision_antenna_collision_1'>
-          <pose>0 0 0.11 0 -0 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.1</length>
-              <radius>0.15</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <visual name='wamv/ins_link_fixed_joint_lump__ins_visual_visual'>
-          <pose>0 0 0 0 -0 0</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/wamv_gazebo/models/gps/mesh/gps.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <self_collide>0</self_collide>
-        <enable_wind>0</enable_wind>
-        <kinematic>0</kinematic>
-      </link>
-      <plugin name='gps_plugin_ins' filename='libhector_gazebo_ros_gps.so'>
-        <updateRate>15</updateRate>
-        <alwaysOn>1</alwaysOn>
-        <bodyName>wamv/ins_link</bodyName>
-        <frameId>wamv/ins_link</frameId>
-        <topicName>wamv/sensors/gps/gps/fix</topicName>
-        <velocityTopicName>wamv/sensors/gps/gps/fix_velocity</velocityTopicName>
-        <useWorldSphericalCoordinates>1</useWorldSphericalCoordinates>
-        <offset>0.0 0.0 0.0</offset>
-        <drift>0.0 0.0 0.0</drift>
-        <gaussianNoise>0 0 0</gaussianNoise>
-        <velocityOffset>0.0 0.0 0.0</velocityOffset>
-        <velocityDrift>0.0 0.0 0.0</velocityDrift>
-        <velocityGaussianNoise>0.0 0.0 0.0</velocityGaussianNoise>
-        <robotNamespace>/</robotNamespace>
-      </plugin>
-      <plugin name='imu_plugin_imu_wamv' filename='libhector_gazebo_ros_imu.so'>
-        <updateRate>15</updateRate>
-        <bodyName>wamv/imu_wamv_link</bodyName>
-        <topicName>wamv/sensors/imu/imu/data</topicName>
-        <serviceName>wamv/sensors/imu_service</serviceName>
-        <frameId>wamv/imu_wamv_link</frameId>
-        <alwaysOn>1</alwaysOn>
-        <accelOffset>0.0 0.0 0.0</accelOffset>
-        <accelDrift>0.0 0.0 0.0</accelDrift>
-        <accelDriftFrequency>0.00027 0.00027 0.000027</accelDriftFrequency>
-        <accelGaussianNoise>0.0 0.0 0.0</accelGaussianNoise>
-        <rateOffset>0.0 0.0 0.0</rateOffset>
-        <rateDrift>0.0 0.0 0.0</rateDrift>
-        <rateDriftFrequency>0.00027 0.00027 0.000027</rateDriftFrequency>
-        <rateGaussianNoise>0.0 0.0 0.0</rateGaussianNoise>
-        <yawOffset>0.0</yawOffset>
-        <yawDrift>0.0</yawDrift>
-        <yawDriftFrequency>0.00027</yawDriftFrequency>
-        <yawGaussianNoise>0.0</yawGaussianNoise>
-        <robotNamespace>/</robotNamespace>
-      </plugin>
-      <plugin name='ball_shooter_plugin' filename='libball_shooter_plugin.so'>
-        <projectile>
-          <model_name>blue_projectile</model_name>
-          <link_name>link</link_name>
-          <frame>wamv/ball_shooter_launcher_link</frame>
-          <pose>0.14 0 0 0 0 0</pose>
-        </projectile>
-        <num_shots>4</num_shots>
-        <shot_force>300</shot_force>
-        <topic>wamv/shooters/ball_shooter/fire</topic>
-        <robotNamespace>/</robotNamespace>
-      </plugin>
-      <plugin name='pinger_plugin' filename='libusv_gazebo_acoustic_pinger_plugin.so'>
-        <frameId>wamv/pinger</frameId>
-        <topicName>wamv/sensors/pingers/hydrophones/range_bearing</topicName>
-        <setPositionTopicName>wamv/sensors/pingers/hydrophones/set_pinger_position</setPositionTopicName>
-        <rangeNoise>
-          <noise>
-            <type>gaussian</type>
-            <mean>0.0</mean>
-            <stddev>3.0</stddev>
-          </noise>
-        </rangeNoise>
-        <bearingNoise>
-          <noise>
-            <type>gaussian</type>
-            <mean>0.0</mean>
-            <stddev>0.01</stddev>
-          </noise>
-        </bearingNoise>
-        <elevationNoise>
-          <noise>
-            <type>gaussian</type>
-            <mean>0.0</mean>
-            <stddev>0.01</stddev>
-          </noise>
-        </elevationNoise>
-        <robotNamespace>/</robotNamespace>
-      </plugin>
-      <plugin name='wamv_gazebo_thrust' filename='libusv_gazebo_thrust_plugin.so'>
-        <cmdTimeout>1.0</cmdTimeout>
-        <robotNamespace>wamv</robotNamespace>
-        <thruster>
-          <linkName>BL_propeller_link</linkName>
-          <propJointName>BL_engine_propeller_joint</propJointName>
-          <engineJointName>BL_chasis_engine_joint</engineJointName>
-          <cmdTopic>thrusters/BL_thrust_cmd</cmdTopic>
-          <angleTopic>thrusters/BL_thrust_angle</angleTopic>
-          <enableAngle>1</enableAngle>
-          <mappingType>1</mappingType>
-          <maxCmd>1.0</maxCmd>
-          <maxForceFwd>250.0</maxForceFwd>
-          <maxForceRev>-100.0</maxForceRev>
-          <maxAngle>1.5707963267948966</maxAngle>
-        </thruster>
-        <thruster>
-          <linkName>BR_propeller_link</linkName>
-          <propJointName>BR_engine_propeller_joint</propJointName>
-          <engineJointName>BR_chasis_engine_joint</engineJointName>
-          <cmdTopic>thrusters/BR_thrust_cmd</cmdTopic>
-          <angleTopic>thrusters/BR_thrust_angle</angleTopic>
-          <enableAngle>1</enableAngle>
-          <mappingType>1</mappingType>
-          <maxCmd>1.0</maxCmd>
-          <maxForceFwd>250.0</maxForceFwd>
-          <maxForceRev>-100.0</maxForceRev>
-          <maxAngle>1.5707963267948966</maxAngle>
-        </thruster>
-        <thruster>
-          <linkName>FL_propeller_link</linkName>
-          <propJointName>FL_engine_propeller_joint</propJointName>
-          <engineJointName>FL_chasis_engine_joint</engineJointName>
-          <cmdTopic>thrusters/FL_thrust_cmd</cmdTopic>
-          <angleTopic>thrusters/FL_thrust_angle</angleTopic>
-          <enableAngle>1</enableAngle>
-          <mappingType>1</mappingType>
-          <maxCmd>1.0</maxCmd>
-          <maxForceFwd>250.0</maxForceFwd>
-          <maxForceRev>-100.0</maxForceRev>
-          <maxAngle>1.5707963267948966</maxAngle>
-        </thruster>
-        <thruster>
-          <linkName>FR_propeller_link</linkName>
-          <propJointName>FR_engine_propeller_joint</propJointName>
-          <engineJointName>FR_chasis_engine_joint</engineJointName>
-          <cmdTopic>thrusters/FR_thrust_cmd</cmdTopic>
-          <angleTopic>thrusters/FR_thrust_angle</angleTopic>
-          <enableAngle>1</enableAngle>
-          <mappingType>1</mappingType>
-          <maxCmd>1.0</maxCmd>
-          <maxForceFwd>250.0</maxForceFwd>
-          <maxForceRev>-100.0</maxForceRev>
-          <maxAngle>1.5707963267948966</maxAngle>
-        </thruster>
-      </plugin>
-      <plugin name='usv_dynamics_wamv_dynamics_plugin' filename='libusv_gazebo_dynamics_plugin.so'>
-        <bodyName>wamv/base_link</bodyName>
-        <waterLevel>0</waterLevel>
-        <waterDensity>997.8</waterDensity>
-        <xDotU>0.0</xDotU>
-        <yDotV>0.0</yDotV>
-        <nDotR>0.0</nDotR>
-        <xU>51.3</xU>
-        <xUU>72.4</xUU>
-        <yV>40.0</yV>
-        <yVV>0.0</yVV>
-        <zW>500.0</zW>
-        <kP>50.0</kP>
-        <mQ>50.0</mQ>
-        <nR>400.0</nR>
-        <nRR>0.0</nRR>
-        <hullRadius>0.213</hullRadius>
-        <boatWidth>2.4</boatWidth>
-        <boatLength>4.9</boatLength>
-        <length_n>2</length_n>
-        <wave_model>ocean_waves</wave_model>
-        <robotNamespace>/</robotNamespace>
-      </plugin>
-      <frame name='wamv/front_left_cam_to_front_left_cam_link_optical_joint' attached_to='wamv/front_left_cam_link'>
-        <pose>0 0 0 -1.5708 -0 -1.5708</pose>
-      </frame>
-      <frame name='wamv/front_left_cam_link_optical' attached_to='wamv/front_left_cam_to_front_left_cam_link_optical_joint'/>
-      <frame name='wamv/front_left_cam_post_arm_to_front_left_cam_joint' attached_to='wamv/front_left_cam_post_arm_link'>
-        <pose>0.02 0 0.027 0 0.20944 0</pose>
-      </frame>
-      <frame name='wamv/front_left_cam_link' attached_to='wamv/front_left_cam_post_arm_to_front_left_cam_joint'/>
-      <frame name='wamv/front_left_cam_post_to_front_left_cam_post_arm_joint' attached_to='wamv/front_left_cam_post_link'>
-        <pose>0.03 0 0.08825 0 -0 0</pose>
-      </frame>
-      <frame name='wamv/front_left_cam_post_arm_link' attached_to='wamv/front_left_cam_post_to_front_left_cam_post_arm_joint'/>
-      <frame name='wamv/base_to_front_left_cam_post_joint' attached_to='wamv/base_link'>
-        <pose>0.92 0.1 1.38475 0 -0 0</pose>
-      </frame>
-      <frame name='wamv/front_left_cam_post_link' attached_to='wamv/base_to_front_left_cam_post_joint'/>
-      <frame name='wamv/front_right_cam_to_front_right_cam_link_optical_joint' attached_to='wamv/front_right_cam_link'>
-        <pose>0 0 0 -1.5708 -0 -1.5708</pose>
-      </frame>
-      <frame name='wamv/front_right_cam_link_optical' attached_to='wamv/front_right_cam_to_front_right_cam_link_optical_joint'/>
-      <frame name='wamv/front_right_cam_post_arm_to_front_right_cam_joint' attached_to='wamv/front_right_cam_post_arm_link'>
-        <pose>0.02 0 0.027 0 0.20944 0</pose>
-      </frame>
-      <frame name='wamv/front_right_cam_link' attached_to='wamv/front_right_cam_post_arm_to_front_right_cam_joint'/>
-      <frame name='wamv/front_right_cam_post_to_front_right_cam_post_arm_joint' attached_to='wamv/front_right_cam_post_link'>
-        <pose>0.03 0 0.08825 0 -0 0</pose>
-      </frame>
-      <frame name='wamv/front_right_cam_post_arm_link' attached_to='wamv/front_right_cam_post_to_front_right_cam_post_arm_joint'/>
-      <frame name='wamv/base_to_front_right_cam_post_joint' attached_to='wamv/base_link'>
-        <pose>0.92 -0.1 1.38475 0 -0 0</pose>
-      </frame>
-      <frame name='wamv/front_right_cam_post_link' attached_to='wamv/base_to_front_right_cam_post_joint'/>
-      <frame name='wamv/velodyne_post_arm_to_velodyne_joint' attached_to='velodyne_post_arm_link'>
-        <pose>0.04 0 0.05 0 0.261799 0</pose>
-      </frame>
-      <frame name='velodyne' attached_to='wamv/velodyne_post_arm_to_velodyne_joint'/>
-      <frame name='wamv/velodyne_post_to_velodyne_post_arm_joint' attached_to='velodyne_post_link'>
-        <pose>0.03 0 0.32675 0 -0 0</pose>
-      </frame>
-      <frame name='velodyne_post_arm_link' attached_to='wamv/velodyne_post_to_velodyne_post_arm_joint'/>
-      <frame name='wamv/base_to_velodyne_post_joint' attached_to='wamv/base_link'>
-        <pose>0.704 0 1.62325 0 -0 0</pose>
-      </frame>
-      <frame name='velodyne_post_link' attached_to='wamv/base_to_velodyne_post_joint'/>
-      <frame name='wamv/dummy_joint' attached_to='wamv/base_link'>
-        <pose>0 0 0 0 -0 0</pose>
-      </frame>
-      <frame name='wamv/dummy_link' attached_to='wamv/dummy_joint'/>
-      <frame name='wamv/hydrophones_pinger_joint' attached_to='wamv/base_link'>
-        <pose>1 0 -1 0 -0 0</pose>
-      </frame>
-      <frame name='wamv/hydrophones' attached_to='wamv/hydrophones_pinger_joint'/>
-      <pose>-527 167 0.1 0 -0 1</pose>
-    </model>
-    <model name='polyform_a2_red'>
+    <model name='red_circular_buoy'>
       <static>0</static>
       <pose>-522.515 184.985 0 0 -0 0</pose>
       <link name='base_link'>
@@ -3634,7 +1508,7 @@
         </buoyancy>
       </plugin>
     </model>
-    <model name='polyform_a2_green'>
+    <model name='green_circular_buoy'>
       <static>0</static>
       <pose>-534.625 187.168 0 0 -0 0</pose>
       <link name='base_link'>
@@ -3703,7 +1577,7 @@
         </buoyancy>
       </plugin>
     </model>
-    <model name='polyform_a2_blue'>
+    <model name='blue_circular_buoy'>
       <static>0</static>
       <pose>-531.036 180.328 0 0 -0 0</pose>
       <link name='base_link'>
@@ -3947,7 +1821,7 @@
           <wrench>0.311502 0.112816 0.00034 0 -0 0</wrench>
         </link>
       </model>
-      <model name='polyform_a2_blue'>
+      <model name='blue_circular_buoy'>
         <pose>-534.036 187.328 0.10507 0 -0 0</pose>
         <scale>1 1 1</scale>
         <link name='base_link'>
@@ -3957,7 +1831,7 @@
           <wrench>0 0 -0 0 -0 0</wrench>
         </link>
       </model>
-      <model name='polyform_a2_green'>
+      <model name='green_circular_buoy'>
         <pose>-522.625 184.168 0.10507 0 -0 0</pose>
         <scale>1 1 1</scale>
         <link name='base_link'>
@@ -3967,7 +1841,7 @@
           <wrench>0 0 -0 0 -0 0</wrench>
         </link>
       </model>
-      <model name='polyform_a2_red'>
+      <model name='red_circular_buoy'>
         <pose>-531.515 180.985 0.10507 0 -0 0</pose>
         <scale>1 1 1</scale>
         <link name='base_link'>
diff --git a/NaviGator/simulation/navigator_gazebo/worlds/wildlife/wildlife_encounter_test_case_6.world b/NaviGator/simulation/navigator_gazebo/worlds/wildlife/wildlife_encounter_test_case_6.world
index 8fcc37477..7715d6656 100644
--- a/NaviGator/simulation/navigator_gazebo/worlds/wildlife/wildlife_encounter_test_case_6.world
+++ b/NaviGator/simulation/navigator_gazebo/worlds/wildlife/wildlife_encounter_test_case_6.world
@@ -1439,2133 +1439,7 @@
       <real_time_update_rate>1000</real_time_update_rate>
     </physics>
     <wind/>
-    <model name='wamv'>
-      <link name='wamv/base_link'>
-        <inertial>
-          <pose>0.012722 0 0.026171 0 -0 0</pose>
-          <mass>182.806</mass>
-          <inertia>
-            <ixx>128.232</ixx>
-            <ixy>-2.4178e-21</ixy>
-            <ixz>-3.85993</ixz>
-            <iyy>403.15</iyy>
-            <iyz>-9.28107e-22</iyz>
-            <izz>447.942</izz>
-          </inertia>
-        </inertial>
-        <collision name='wamv/base_link_fixed_joint_lump__front_left_cam_post_collision_collision'>
-          <pose>0.92 0.1 1.38475 0 -0 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.1765</length>
-              <radius>0.0076</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__front_left_cam_post_arm_collision_collision_1'>
-          <pose>0.95 0.1 1.473 -1.20428 -0 -1.5708</pose>
-          <geometry>
-            <cylinder>
-              <length>0.065</length>
-              <radius>0.008</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__front_left_cam_lens_collision_collision_2'>
-          <pose>0.987607 0.1 1.49626 3.14159 1.36136 3.14159</pose>
-          <geometry>
-            <cylinder>
-              <length>0.042</length>
-              <radius>0.015</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__front_left_cam_box_collision_collision_3'>
-          <pose>0.949948 0.1 1.50426 0 0.20944 0</pose>
-          <geometry>
-            <box>
-              <size>0.036 0.03 0.03</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__front_right_cam_post_collision_collision_4'>
-          <pose>0.92 -0.1 1.38475 0 -0 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.1765</length>
-              <radius>0.0076</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__front_right_cam_post_arm_collision_collision_5'>
-          <pose>0.95 -0.1 1.473 -1.20428 -0 -1.5708</pose>
-          <geometry>
-            <cylinder>
-              <length>0.065</length>
-              <radius>0.008</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__front_right_cam_lens_collision_collision_6'>
-          <pose>0.987607 -0.1 1.49626 3.14159 1.36136 3.14159</pose>
-          <geometry>
-            <cylinder>
-              <length>0.042</length>
-              <radius>0.015</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__front_right_cam_box_collision_collision_7'>
-          <pose>0.949948 -0.1 1.50426 0 0.20944 0</pose>
-          <geometry>
-            <box>
-              <size>0.036 0.03 0.03</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__velodyne_post_collision_collision_8'>
-          <pose>0.704 0 1.62325 0 -0 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.6535</length>
-              <radius>0.0076</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__velodyne_post_arm_collision_collision_9'>
-          <pose>0.734 0 1.95 -1.20428 -0 -1.5708</pose>
-          <geometry>
-            <cylinder>
-              <length>0.065</length>
-              <radius>0.008</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__velodyne_collision_collision_10'>
-          <pose>0.774 0 2 0 0.261799 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.075</length>
-              <radius>0.055</radius>
-            </cylinder>
-          </geometry>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <friction>
-              <ode/>
-              <torsional>
-                <ode/>
-              </torsional>
-            </friction>
-            <bounce/>
-          </surface>
-          <max_contacts>10</max_contacts>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__left_float_collision_11'>
-          <pose>-0.4 1.03 0.2 3.14159 1.57079 3.14159</pose>
-          <geometry>
-            <cylinder>
-              <length>4</length>
-              <radius>0.2</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__left_mid_float_collision_12'>
-          <pose>1.85 1.03 0.3 0 1.38 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.5</length>
-              <radius>0.17</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__left_front_float_collision_13'>
-          <pose>2.3 1.03 0.4 0 1.3 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.45</length>
-              <radius>0.12</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__front_left_beam_lower_collision_14'>
-          <pose>0.9 0.85 1 0.78 -0 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.5</length>
-              <radius>0.04</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__front_left_beam_upper_collision_15'>
-          <pose>0.9 0.6 1.18 1.35 -0 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.2</length>
-              <radius>0.04</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__mid_left_beam_lower_collision_16'>
-          <pose>-0.65 0.99 0.7 0.1 0.25 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.45</length>
-              <radius>0.03</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__mid_left_beam_medium_collision_17'>
-          <pose>-0.57 0.87 1.05 0.75 0.25 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.32</length>
-              <radius>0.03</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__mid_left_beam_upper_collision_18'>
-          <pose>-0.55 0.65 1.17 1.35 0.25 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.3</length>
-              <radius>0.03</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__rear_left_beam_lower_collision_19'>
-          <pose>-0.74 1.03 0.7 0 -0.15 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.45</length>
-              <radius>0.03</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__rear_left_beam_medium_collision_20'>
-          <pose>-0.79 0.91 1.05 0.75 -0.15 -0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.32</length>
-              <radius>0.03</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__rear_left_beam_upper_collision_21'>
-          <pose>-0.81 0.67 1.18 1.45 -0.15 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.3</length>
-              <radius>0.03</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__left_joint_collision_22'>
-          <pose>0.58 1.03 0.6 0 -0.6 0</pose>
-          <geometry>
-            <box>
-              <size>0.65 0.2 0.1</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__right_float_collision_23'>
-          <pose>-0.4 -1.03 0.2 3.14159 1.57079 3.14159</pose>
-          <geometry>
-            <cylinder>
-              <length>4</length>
-              <radius>0.2</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__right_mid_float_collision_24'>
-          <pose>1.85 -1.03 0.3 0 1.38 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.5</length>
-              <radius>0.17</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__right_front_float_collision_25'>
-          <pose>2.3 -1.03 0.4 0 1.3 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.45</length>
-              <radius>0.12</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__front_right_beam_lower_collision_26'>
-          <pose>0.9 -0.85 1 -0.78 0 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.5</length>
-              <radius>0.04</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__front_right_beam_upper_collision_27'>
-          <pose>0.9 -0.6 1.18 -1.35 0 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.2</length>
-              <radius>0.04</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__mid_right_beam_lower_collision_28'>
-          <pose>-0.65 -0.99 0.7 -0.1 0.25 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.45</length>
-              <radius>0.03</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__mid_right_beam_medium_collision_29'>
-          <pose>-0.57 -0.87 1.05 -0.75 0.25 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.32</length>
-              <radius>0.03</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__mid_right_beam_upper_collision_30'>
-          <pose>-0.55 -0.65 1.17 -1.35 0.25 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.3</length>
-              <radius>0.03</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__rear_right_beam_lower_collision_31'>
-          <pose>-0.74 -1.03 0.7 0 -0.15 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.45</length>
-              <radius>0.03</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__rear_right_beam_medium_collision_32'>
-          <pose>-0.79 -0.91 1.05 -0.75 -0.15 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.32</length>
-              <radius>0.03</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__rear_right_beam_upper_collision_33'>
-          <pose>-0.81 -0.67 1.18 -1.45 -0.15 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.3</length>
-              <radius>0.03</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__right_joint_collision_34'>
-          <pose>0.58 -1.03 0.6 0 -0.6 0</pose>
-          <geometry>
-            <box>
-              <size>0.65 0.2 0.1</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__top_base_collision_35'>
-          <pose>0 0 1.25 0 -0 0</pose>
-          <geometry>
-            <box>
-              <size>1.85 1 0.1</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/base_link_fixed_joint_lump__hydrophones_collision_collision_36'>
-          <pose>1 0 -1 0 -0 0</pose>
-          <geometry>
-            <box>
-              <size>0.1 0.1 0.1</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <visual name='wamv/base_link_fixed_joint_lump__wamv/front_left_cam_post_link_visual'>
-          <pose>0.92 0.1 1.38475 0 -0 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.1765</length>
-              <radius>0.0076</radius>
-            </cylinder>
-          </geometry>
-          <material>
-            <script>
-              <name>Gazebo/Black</name>
-              <uri>file://media/materials/scripts/gazebo.material</uri>
-            </script>
-          </material>
-        </visual>
-        <visual name='wamv/base_link_fixed_joint_lump__front_left_cam_post_arm_visual_visual_1'>
-          <pose>0.912 0.1 1.47 -1.0472 -0 -1.5708</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/vrx_gazebo/models/sensor_post/mesh/sensor_post_arm.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <visual name='wamv/base_link_fixed_joint_lump__front_left_cam_visual_visual_2'>
-          <pose>0.937721 0.1 1.50686 0.20944 -0 1.5708</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/vrx_gazebo/models/mono_camera/mesh/mono_camera.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <visual name='wamv/base_link_fixed_joint_lump__wamv/front_right_cam_post_link_visual_3'>
-          <pose>0.92 -0.1 1.38475 0 -0 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.1765</length>
-              <radius>0.0076</radius>
-            </cylinder>
-          </geometry>
-          <material>
-            <script>
-              <name>Gazebo/Black</name>
-              <uri>file://media/materials/scripts/gazebo.material</uri>
-            </script>
-          </material>
-        </visual>
-        <visual name='wamv/base_link_fixed_joint_lump__front_right_cam_post_arm_visual_visual_4'>
-          <pose>0.912 -0.1 1.47 -1.0472 -0 -1.5708</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/vrx_gazebo/models/sensor_post/mesh/sensor_post_arm.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <visual name='wamv/base_link_fixed_joint_lump__front_right_cam_visual_visual_5'>
-          <pose>0.937721 -0.1 1.50686 0.20944 -0 1.5708</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/vrx_gazebo/models/mono_camera/mesh/mono_camera.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <visual name='wamv/base_link_fixed_joint_lump__velodyne_post_link_visual_6'>
-          <pose>0.704 0 1.62325 0 -0 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.6535</length>
-              <radius>0.0076</radius>
-            </cylinder>
-          </geometry>
-          <material>
-            <script>
-              <name>Gazebo/Black</name>
-              <uri>file://media/materials/scripts/gazebo.material</uri>
-            </script>
-          </material>
-        </visual>
-        <visual name='wamv/base_link_fixed_joint_lump__velodyne_post_arm_visual_visual_7'>
-          <pose>0.696 0 1.947 -1.0472 -0 -1.5708</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/vrx_gazebo/models/sensor_post/mesh/sensor_post_arm.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <visual name='wamv/base_link_fixed_joint_lump__velodyne_visual_visual_8'>
-          <pose>0.764941 0 1.96619 0 0.261799 0</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/vrx_gazebo/models/3d_lidar/mesh/3d_lidar.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <visual name='wamv/base_link_fixed_joint_lump__wamv/dummy_link_visual_9'>
-          <pose>0 0 0 0 -0 0</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/wamv_description/models/WAM-V-Base/mesh/WAM-V-Base.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <visual name='wamv/base_link_fixed_joint_lump__wamv/hydrophones_visual_10'>
-          <pose>1 0 -1 0 -0 0</pose>
-          <geometry>
-            <box>
-              <size>0.1 0.1 0.1</size>
-            </box>
-          </geometry>
-        </visual>
-        <sensor name='front_left_cam_sensor' type='camera'>
-          <update_rate>30</update_rate>
-          <camera name='front_left_cam_camera'>
-            <horizontal_fov>1.39626</horizontal_fov>
-            <image>
-              <width>1280</width>
-              <height>720</height>
-              <format>R8G8B8</format>
-            </image>
-            <clip>
-              <near>0.05</near>
-              <far>300</far>
-            </clip>
-            <noise>
-              <type>gaussian</type>
-              <mean>0</mean>
-              <stddev>0.007</stddev>
-            </noise>
-          </camera>
-          <plugin name='camera_plugin_front_left_cam' filename='libgazebo_ros_camera.so'>
-            <alwaysOn>1</alwaysOn>
-            <updateRate>0.0</updateRate>
-            <cameraName>wamv/sensors/camera/front_left_cam</cameraName>
-            <imageTopicName>image_raw</imageTopicName>
-            <cameraInfoTopicName>camera_info</cameraInfoTopicName>
-            <frameName>wamv/front_left_cam_link_optical</frameName>
-            <hackBaseline>0.07</hackBaseline>
-            <distortionK1>0.0</distortionK1>
-            <distortionK2>0.0</distortionK2>
-            <distortionK3>0.0</distortionK3>
-            <distortionT1>0.0</distortionT1>
-            <distortionT2>0.0</distortionT2>
-            <robotNamespace>/</robotNamespace>
-          </plugin>
-          <pose>0.97 0.1 1.5 0 0.20944 0</pose>
-        </sensor>
-        <sensor name='front_right_cam_sensor' type='camera'>
-          <update_rate>30</update_rate>
-          <camera name='front_right_cam_camera'>
-            <horizontal_fov>1.39626</horizontal_fov>
-            <image>
-              <width>1280</width>
-              <height>720</height>
-              <format>R8G8B8</format>
-            </image>
-            <clip>
-              <near>0.05</near>
-              <far>300</far>
-            </clip>
-            <noise>
-              <type>gaussian</type>
-              <mean>0</mean>
-              <stddev>0.007</stddev>
-            </noise>
-          </camera>
-          <plugin name='camera_plugin_front_right_cam' filename='libgazebo_ros_camera.so'>
-            <alwaysOn>1</alwaysOn>
-            <updateRate>0.0</updateRate>
-            <cameraName>wamv/sensors/camera/front_right_cam</cameraName>
-            <imageTopicName>image_raw</imageTopicName>
-            <cameraInfoTopicName>camera_info</cameraInfoTopicName>
-            <frameName>wamv/front_right_cam_link_optical</frameName>
-            <hackBaseline>0.07</hackBaseline>
-            <distortionK1>0.0</distortionK1>
-            <distortionK2>0.0</distortionK2>
-            <distortionK3>0.0</distortionK3>
-            <distortionT1>0.0</distortionT1>
-            <distortionT2>0.0</distortionT2>
-            <robotNamespace>/</robotNamespace>
-          </plugin>
-          <pose>0.97 -0.1 1.5 0 0.20944 0</pose>
-        </sensor>
-        <sensor name='velodyne_sensor' type='gpu_ray'>
-          <update_rate>10</update_rate>
-          <ray>
-            <scan>
-              <horizontal>
-                <samples>18750</samples>
-                <resolution>0.1</resolution>
-                <min_angle>-3.14159</min_angle>
-                <max_angle>3.14159</max_angle>
-              </horizontal>
-              <vertical>
-                <samples>16</samples>
-                <resolution>1</resolution>
-                <min_angle>-0.261799</min_angle>
-                <max_angle>0.261799</max_angle>
-              </vertical>
-            </scan>
-            <range>
-              <min>0.1</min>
-              <max>130</max>
-            </range>
-            <noise>
-              <type>gaussian</type>
-              <mean>0</mean>
-              <stddev>0.01</stddev>
-            </noise>
-          </ray>
-          <plugin name='velodyne_plugin' filename='libgazebo_ros_velodyne_gpu_laser.so'>
-            <topicName>velodyne_points</topicName>
-            <frameName>velodyne</frameName>
-            <min_intensity>0</min_intensity>
-            <robotNamespace>/</robotNamespace>
-          </plugin>
-          <pose>0.774 0 2 0 0.261799 0</pose>
-        </sensor>
-        <self_collide>0</self_collide>
-        <enable_wind>0</enable_wind>
-        <kinematic>0</kinematic>
-      </link>
-      <joint name='BL_chasis_engine_joint' type='revolute'>
-        <pose relative_to='wamv/base_link'>-1.9304 1.1 0.318237 0 -0 0.785398</pose>
-        <parent>wamv/base_link</parent>
-        <child>BL_engine_link</child>
-        <axis>
-          <xyz>0 0 1</xyz>
-          <limit>
-            <lower>-3.14159</lower>
-            <upper>3.14159</upper>
-            <effort>10</effort>
-            <velocity>10</velocity>
-          </limit>
-          <dynamics>
-            <spring_reference>0</spring_reference>
-            <spring_stiffness>0</spring_stiffness>
-          </dynamics>
-        </axis>
-      </joint>
-      <link name='BL_engine_link'>
-        <pose relative_to='BL_chasis_engine_joint'>0 0 0 0 -0 0</pose>
-        <inertial>
-          <pose>0 0 0 0 -0 0</pose>
-          <mass>15</mass>
-          <inertia>
-            <ixx>0.889245</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>0.911125</iyy>
-            <iyz>0</iyz>
-            <izz>0.078125</izz>
-          </inertia>
-        </inertial>
-        <collision name='BL_engine_link_fixed_joint_lump__BL_engine_vertical_axis_collision_collision'>
-          <pose>-0.16 0 -0.24 0 -0 0</pose>
-          <geometry>
-            <box>
-              <size>0.2 0.15 0.83</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='BL_engine_link_fixed_joint_lump__BL_engine_rear_end_collision_collision_1'>
-          <pose>-0.34 0 0.12 0 -0 0</pose>
-          <geometry>
-            <box>
-              <size>0.12 0.15 0.12</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <visual name='BL_engine_link_visual'>
-          <pose>0 0 0 0 -0 0</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/wamv_description/models/engine/mesh/engine.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <self_collide>0</self_collide>
-        <enable_wind>0</enable_wind>
-        <kinematic>0</kinematic>
-      </link>
-      <joint name='BL_engine_propeller_joint' type='revolute'>
-        <pose relative_to='BL_engine_link'>-0.278156 0 -0.509371 0 -0 0</pose>
-        <parent>BL_engine_link</parent>
-        <child>BL_propeller_link</child>
-        <axis>
-          <xyz>1 0 0</xyz>
-          <limit>
-            <effort>400</effort>
-            <velocity>100</velocity>
-            <lower>-1e+16</lower>
-            <upper>1e+16</upper>
-          </limit>
-          <dynamics>
-            <damping>0.05</damping>
-            <friction>0.05</friction>
-            <spring_reference>0</spring_reference>
-            <spring_stiffness>0</spring_stiffness>
-          </dynamics>
-        </axis>
-      </joint>
-      <link name='BL_propeller_link'>
-        <pose relative_to='BL_engine_propeller_joint'>0 0 0 0 -0 0</pose>
-        <inertial>
-          <pose>0 0 0 0 -0 0</pose>
-          <mass>0.5</mass>
-          <inertia>
-            <ixx>0.008545</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>0.008545</iyy>
-            <iyz>0</iyz>
-            <izz>0.0144</izz>
-          </inertia>
-        </inertial>
-        <collision name='BL_propeller_link_fixed_joint_lump__BL_propeller_collision_collision'>
-          <pose>-0.08 0 0 3.14159 1.57079 3.14159</pose>
-          <geometry>
-            <cylinder>
-              <length>0.18</length>
-              <radius>0.24</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <visual name='BL_propeller_link_visual'>
-          <pose>0 0 0 0 -0 0</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/wamv_description/models/propeller/mesh/propeller.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <self_collide>0</self_collide>
-        <enable_wind>0</enable_wind>
-        <kinematic>0</kinematic>
-      </link>
-      <joint name='BR_chasis_engine_joint' type='revolute'>
-        <pose relative_to='wamv/base_link'>-1.9304 -1.1 0.318237 0 0 -0.785398</pose>
-        <parent>wamv/base_link</parent>
-        <child>BR_engine_link</child>
-        <axis>
-          <xyz>0 0 1</xyz>
-          <limit>
-            <lower>-3.14159</lower>
-            <upper>3.14159</upper>
-            <effort>10</effort>
-            <velocity>10</velocity>
-          </limit>
-          <dynamics>
-            <spring_reference>0</spring_reference>
-            <spring_stiffness>0</spring_stiffness>
-          </dynamics>
-        </axis>
-      </joint>
-      <link name='BR_engine_link'>
-        <pose relative_to='BR_chasis_engine_joint'>0 0 0 0 -0 0</pose>
-        <inertial>
-          <pose>0 0 0 0 -0 0</pose>
-          <mass>15</mass>
-          <inertia>
-            <ixx>0.889245</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>0.911125</iyy>
-            <iyz>0</iyz>
-            <izz>0.078125</izz>
-          </inertia>
-        </inertial>
-        <collision name='BR_engine_link_fixed_joint_lump__BR_engine_vertical_axis_collision_collision'>
-          <pose>-0.16 0 -0.24 0 -0 0</pose>
-          <geometry>
-            <box>
-              <size>0.2 0.15 0.83</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='BR_engine_link_fixed_joint_lump__BR_engine_rear_end_collision_collision_1'>
-          <pose>-0.34 0 0.12 0 -0 0</pose>
-          <geometry>
-            <box>
-              <size>0.12 0.15 0.12</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <visual name='BR_engine_link_visual'>
-          <pose>0 0 0 0 -0 0</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/wamv_description/models/engine/mesh/engine.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <self_collide>0</self_collide>
-        <enable_wind>0</enable_wind>
-        <kinematic>0</kinematic>
-      </link>
-      <joint name='BR_engine_propeller_joint' type='revolute'>
-        <pose relative_to='BR_engine_link'>-0.278156 0 -0.509371 0 -0 0</pose>
-        <parent>BR_engine_link</parent>
-        <child>BR_propeller_link</child>
-        <axis>
-          <xyz>1 0 0</xyz>
-          <limit>
-            <effort>400</effort>
-            <velocity>100</velocity>
-            <lower>-1e+16</lower>
-            <upper>1e+16</upper>
-          </limit>
-          <dynamics>
-            <damping>0.05</damping>
-            <friction>0.05</friction>
-            <spring_reference>0</spring_reference>
-            <spring_stiffness>0</spring_stiffness>
-          </dynamics>
-        </axis>
-      </joint>
-      <link name='BR_propeller_link'>
-        <pose relative_to='BR_engine_propeller_joint'>0 0 0 0 -0 0</pose>
-        <inertial>
-          <pose>0 0 0 0 -0 0</pose>
-          <mass>0.5</mass>
-          <inertia>
-            <ixx>0.008545</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>0.008545</iyy>
-            <iyz>0</iyz>
-            <izz>0.0144</izz>
-          </inertia>
-        </inertial>
-        <collision name='BR_propeller_link_fixed_joint_lump__BR_propeller_collision_collision'>
-          <pose>-0.08 0 0 3.14159 1.57079 3.14159</pose>
-          <geometry>
-            <cylinder>
-              <length>0.18</length>
-              <radius>0.24</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <visual name='BR_propeller_link_visual'>
-          <pose>0 0 0 0 -0 0</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/wamv_description/models/propeller/mesh/propeller.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <self_collide>0</self_collide>
-        <enable_wind>0</enable_wind>
-        <kinematic>0</kinematic>
-      </link>
-      <joint name='FL_chasis_engine_joint' type='revolute'>
-        <pose relative_to='wamv/base_link'>1.46 0.54 0.49675 0 0 -0.785398</pose>
-        <parent>wamv/base_link</parent>
-        <child>FL_engine_link</child>
-        <axis>
-          <xyz>0 0 1</xyz>
-          <limit>
-            <lower>-3.14159</lower>
-            <upper>3.14159</upper>
-            <effort>10</effort>
-            <velocity>10</velocity>
-          </limit>
-          <dynamics>
-            <spring_reference>0</spring_reference>
-            <spring_stiffness>0</spring_stiffness>
-          </dynamics>
-        </axis>
-      </joint>
-      <link name='FL_engine_link'>
-        <pose relative_to='FL_chasis_engine_joint'>0 0 0 0 -0 0</pose>
-        <inertial>
-          <pose>0 0 0 0 -0 0</pose>
-          <mass>15</mass>
-          <inertia>
-            <ixx>0.889245</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>0.911125</iyy>
-            <iyz>0</iyz>
-            <izz>0.078125</izz>
-          </inertia>
-        </inertial>
-        <collision name='FL_engine_link_fixed_joint_lump__FL_engine_vertical_axis_collision_collision'>
-          <pose>-0.16 0 -0.24 0 -0 0</pose>
-          <geometry>
-            <box>
-              <size>0.2 0.15 0.83</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='FL_engine_link_fixed_joint_lump__FL_engine_rear_end_collision_collision_1'>
-          <pose>-0.34 0 0.12 0 -0 0</pose>
-          <geometry>
-            <box>
-              <size>0.12 0.15 0.12</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <visual name='FL_engine_link_visual'>
-          <pose>0 0 0 0 -0 0</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/wamv_description/models/engine/mesh/engine.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <self_collide>0</self_collide>
-        <enable_wind>0</enable_wind>
-        <kinematic>0</kinematic>
-      </link>
-      <joint name='FL_engine_propeller_joint' type='revolute'>
-        <pose relative_to='FL_engine_link'>-0.278156 0 -0.509371 0 -0 0</pose>
-        <parent>FL_engine_link</parent>
-        <child>FL_propeller_link</child>
-        <axis>
-          <xyz>1 0 0</xyz>
-          <limit>
-            <effort>400</effort>
-            <velocity>100</velocity>
-            <lower>-1e+16</lower>
-            <upper>1e+16</upper>
-          </limit>
-          <dynamics>
-            <damping>0.05</damping>
-            <friction>0.05</friction>
-            <spring_reference>0</spring_reference>
-            <spring_stiffness>0</spring_stiffness>
-          </dynamics>
-        </axis>
-      </joint>
-      <link name='FL_propeller_link'>
-        <pose relative_to='FL_engine_propeller_joint'>0 0 0 0 -0 0</pose>
-        <inertial>
-          <pose>0 0 0 0 -0 0</pose>
-          <mass>0.5</mass>
-          <inertia>
-            <ixx>0.008545</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>0.008545</iyy>
-            <iyz>0</iyz>
-            <izz>0.0144</izz>
-          </inertia>
-        </inertial>
-        <collision name='FL_propeller_link_fixed_joint_lump__FL_propeller_collision_collision'>
-          <pose>-0.08 0 0 3.14159 1.57079 3.14159</pose>
-          <geometry>
-            <cylinder>
-              <length>0.18</length>
-              <radius>0.24</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <visual name='FL_propeller_link_visual'>
-          <pose>0 0 0 0 -0 0</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/wamv_description/models/propeller/mesh/propeller.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <self_collide>0</self_collide>
-        <enable_wind>0</enable_wind>
-        <kinematic>0</kinematic>
-      </link>
-      <joint name='FR_chasis_engine_joint' type='revolute'>
-        <pose relative_to='wamv/base_link'>1.46 -0.54 0.49657 0 -0 0.785398</pose>
-        <parent>wamv/base_link</parent>
-        <child>FR_engine_link</child>
-        <axis>
-          <xyz>0 0 1</xyz>
-          <limit>
-            <lower>-3.14159</lower>
-            <upper>3.14159</upper>
-            <effort>10</effort>
-            <velocity>10</velocity>
-          </limit>
-          <dynamics>
-            <spring_reference>0</spring_reference>
-            <spring_stiffness>0</spring_stiffness>
-          </dynamics>
-        </axis>
-      </joint>
-      <link name='FR_engine_link'>
-        <pose relative_to='FR_chasis_engine_joint'>0 0 0 0 -0 0</pose>
-        <inertial>
-          <pose>0 0 0 0 -0 0</pose>
-          <mass>15</mass>
-          <inertia>
-            <ixx>0.889245</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>0.911125</iyy>
-            <iyz>0</iyz>
-            <izz>0.078125</izz>
-          </inertia>
-        </inertial>
-        <collision name='FR_engine_link_fixed_joint_lump__FR_engine_vertical_axis_collision_collision'>
-          <pose>-0.16 0 -0.24 0 -0 0</pose>
-          <geometry>
-            <box>
-              <size>0.2 0.15 0.83</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='FR_engine_link_fixed_joint_lump__FR_engine_rear_end_collision_collision_1'>
-          <pose>-0.34 0 0.12 0 -0 0</pose>
-          <geometry>
-            <box>
-              <size>0.12 0.15 0.12</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <visual name='FR_engine_link_visual'>
-          <pose>0 0 0 0 -0 0</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/wamv_description/models/engine/mesh/engine.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <self_collide>0</self_collide>
-        <enable_wind>0</enable_wind>
-        <kinematic>0</kinematic>
-      </link>
-      <joint name='FR_engine_propeller_joint' type='revolute'>
-        <pose relative_to='FR_engine_link'>-0.278156 0 -0.509371 0 -0 0</pose>
-        <parent>FR_engine_link</parent>
-        <child>FR_propeller_link</child>
-        <axis>
-          <xyz>1 0 0</xyz>
-          <limit>
-            <effort>400</effort>
-            <velocity>100</velocity>
-            <lower>-1e+16</lower>
-            <upper>1e+16</upper>
-          </limit>
-          <dynamics>
-            <damping>0.05</damping>
-            <friction>0.05</friction>
-            <spring_reference>0</spring_reference>
-            <spring_stiffness>0</spring_stiffness>
-          </dynamics>
-        </axis>
-      </joint>
-      <link name='FR_propeller_link'>
-        <pose relative_to='FR_engine_propeller_joint'>0 0 0 0 -0 0</pose>
-        <inertial>
-          <pose>0 0 0 0 -0 0</pose>
-          <mass>0.5</mass>
-          <inertia>
-            <ixx>0.008545</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>0.008545</iyy>
-            <iyz>0</iyz>
-            <izz>0.0144</izz>
-          </inertia>
-        </inertial>
-        <collision name='FR_propeller_link_fixed_joint_lump__FR_propeller_collision_collision'>
-          <pose>-0.08 0 0 3.14159 1.57079 3.14159</pose>
-          <geometry>
-            <cylinder>
-              <length>0.18</length>
-              <radius>0.24</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <visual name='FR_propeller_link_visual'>
-          <pose>0 0 0 0 -0 0</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/wamv_description/models/propeller/mesh/propeller.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <self_collide>0</self_collide>
-        <enable_wind>0</enable_wind>
-        <kinematic>0</kinematic>
-      </link>
-      <joint name='wamv/ball_shooter_base_joint' type='revolute'>
-        <pose relative_to='wamv/base_link'>0.54 0.3 1.3 0 -0 0.1</pose>
-        <parent>wamv/base_link</parent>
-        <child>wamv/ball_shooter_base_link</child>
-        <axis>
-          <xyz>0 0 1</xyz>
-          <limit>
-            <lower>0</lower>
-            <upper>0</upper>
-            <effort>1000</effort>
-            <velocity>0</velocity>
-          </limit>
-          <dynamics>
-            <spring_reference>0</spring_reference>
-            <spring_stiffness>0</spring_stiffness>
-          </dynamics>
-        </axis>
-      </joint>
-      <link name='wamv/ball_shooter_base_link'>
-        <pose relative_to='wamv/ball_shooter_base_joint'>0 0 0 0 -0 0</pose>
-        <inertial>
-          <pose>-0.02 0 0.05 0 -0 0</pose>
-          <mass>0.5</mass>
-          <inertia>
-            <ixx>0.00083</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>0.00052083</iyy>
-            <iyz>0</iyz>
-            <izz>0.00052083</izz>
-          </inertia>
-        </inertial>
-        <visual name='wamv/ball_shooter_base_link_fixed_joint_lump__ball_shooter_base_visual_visual'>
-          <pose>0 0 0 0 -0 0</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/vrx_gazebo/models/ball_shooter/meshes/ball_shooter_base.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <self_collide>0</self_collide>
-        <enable_wind>0</enable_wind>
-        <kinematic>0</kinematic>
-      </link>
-      <joint name='wamv/ball_shooter_launcher_joint' type='revolute'>
-        <pose relative_to='wamv/ball_shooter_base_link'>-0.0204 0 0.107877 0 -0.8 0</pose>
-        <parent>wamv/ball_shooter_base_link</parent>
-        <child>wamv/ball_shooter_launcher_link</child>
-        <axis>
-          <xyz>0 0 1</xyz>
-          <limit>
-            <lower>0</lower>
-            <upper>0</upper>
-            <effort>1000</effort>
-            <velocity>0</velocity>
-          </limit>
-          <dynamics>
-            <spring_reference>0</spring_reference>
-            <spring_stiffness>0</spring_stiffness>
-          </dynamics>
-        </axis>
-      </joint>
-      <link name='wamv/ball_shooter_launcher_link'>
-        <pose relative_to='wamv/ball_shooter_launcher_joint'>0 0 0 0 -0 0</pose>
-        <inertial>
-          <pose>0 0 0 3.14159 -1.57079 3.14159</pose>
-          <mass>0.5</mass>
-          <inertia>
-            <ixx>0.00177917</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>0.00177917</iyy>
-            <iyz>0</iyz>
-            <izz>0.000225</izz>
-          </inertia>
-        </inertial>
-        <collision name='wamv/ball_shooter_launcher_link_fixed_joint_lump__ball_shooter_collision_collision'>
-          <pose>0 0 0 0 0.261799 0</pose>
-          <geometry>
-            <box>
-              <size>0.3 0.3 0.2</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <visual name='wamv/ball_shooter_launcher_link_fixed_joint_lump__ball_shooter_launcher_visual_visual'>
-          <pose>0 0 0 0 0.261799 0</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/vrx_gazebo/models/ball_shooter/meshes/ball_shooter_launcher.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <self_collide>0</self_collide>
-        <enable_wind>0</enable_wind>
-        <kinematic>0</kinematic>
-      </link>
-      <joint name='wamv/imu_wamv_joint' type='revolute'>
-        <pose relative_to='wamv/base_link'>0.24 0.17 1.5 0 -0 0</pose>
-        <parent>wamv/base_link</parent>
-        <child>wamv/imu_wamv_link</child>
-        <axis>
-          <xyz>0 0 1</xyz>
-          <limit>
-            <lower>0</lower>
-            <upper>0</upper>
-            <effort>1000</effort>
-            <velocity>0</velocity>
-          </limit>
-          <dynamics>
-            <spring_reference>0</spring_reference>
-            <spring_stiffness>0</spring_stiffness>
-          </dynamics>
-        </axis>
-      </joint>
-      <link name='wamv/imu_wamv_link'>
-        <pose relative_to='wamv/imu_wamv_joint'>0 0 0 0 -0 0</pose>
-        <inertial>
-          <pose>0 0 0 0 -0 0</pose>
-          <mass>0.1</mass>
-          <inertia>
-            <ixx>8.3e-05</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>8.3e-05</iyy>
-            <iyz>0</iyz>
-            <izz>0.0125</izz>
-          </inertia>
-        </inertial>
-        <visual name='wamv/imu_wamv_link_fixed_joint_lump__imu_wamv_visual_visual'>
-          <pose>0 0 0 0 -0 0</pose>
-          <geometry>
-            <box>
-              <size>0.05 0.025 0.005</size>
-            </box>
-          </geometry>
-        </visual>
-        <self_collide>0</self_collide>
-        <enable_wind>0</enable_wind>
-        <kinematic>0</kinematic>
-      </link>
-      <joint name='wamv/ins_joint' type='revolute'>
-        <pose relative_to='wamv/base_link'>0.24 0.17 1.5 0 -0 0</pose>
-        <parent>wamv/base_link</parent>
-        <child>wamv/ins_link</child>
-        <axis>
-          <xyz>0 0 1</xyz>
-          <limit>
-            <lower>0</lower>
-            <upper>0</upper>
-            <effort>1000</effort>
-            <velocity>0</velocity>
-          </limit>
-          <dynamics>
-            <spring_reference>0</spring_reference>
-            <spring_stiffness>0</spring_stiffness>
-          </dynamics>
-        </axis>
-      </joint>
-      <link name='wamv/ins_link'>
-        <pose relative_to='wamv/ins_joint'>0 0 0 0 -0 0</pose>
-        <inertial>
-          <pose>0 0 0 0 -0 0</pose>
-          <mass>1</mass>
-          <inertia>
-            <ixx>0.006458</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>0.006458</iyy>
-            <iyz>0</iyz>
-            <izz>0.01125</izz>
-          </inertia>
-        </inertial>
-        <collision name='wamv/ins_link_fixed_joint_lump__ins_collision_base_collision'>
-          <pose>0 0 0.025 0 -0 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.05</length>
-              <radius>0.015</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <collision name='wamv/ins_link_fixed_joint_lump__ins_collision_antenna_collision_1'>
-          <pose>0 0 0.11 0 -0 0</pose>
-          <geometry>
-            <cylinder>
-              <length>0.1</length>
-              <radius>0.15</radius>
-            </cylinder>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
-        <visual name='wamv/ins_link_fixed_joint_lump__ins_visual_visual'>
-          <pose>0 0 0 0 -0 0</pose>
-          <geometry>
-            <mesh>
-              <scale>1 1 1</scale>
-              <uri>/home/marcin/catkin_ws/src/mil/NaviGator/simulation/VRX/vrx/wamv_gazebo/models/gps/mesh/gps.dae</uri>
-            </mesh>
-          </geometry>
-        </visual>
-        <self_collide>0</self_collide>
-        <enable_wind>0</enable_wind>
-        <kinematic>0</kinematic>
-      </link>
-      <plugin name='gps_plugin_ins' filename='libhector_gazebo_ros_gps.so'>
-        <updateRate>15</updateRate>
-        <alwaysOn>1</alwaysOn>
-        <bodyName>wamv/ins_link</bodyName>
-        <frameId>wamv/ins_link</frameId>
-        <topicName>wamv/sensors/gps/gps/fix</topicName>
-        <velocityTopicName>wamv/sensors/gps/gps/fix_velocity</velocityTopicName>
-        <useWorldSphericalCoordinates>1</useWorldSphericalCoordinates>
-        <offset>0.0 0.0 0.0</offset>
-        <drift>0.0 0.0 0.0</drift>
-        <gaussianNoise>0 0 0</gaussianNoise>
-        <velocityOffset>0.0 0.0 0.0</velocityOffset>
-        <velocityDrift>0.0 0.0 0.0</velocityDrift>
-        <velocityGaussianNoise>0.0 0.0 0.0</velocityGaussianNoise>
-        <robotNamespace>/</robotNamespace>
-      </plugin>
-      <plugin name='imu_plugin_imu_wamv' filename='libhector_gazebo_ros_imu.so'>
-        <updateRate>15</updateRate>
-        <bodyName>wamv/imu_wamv_link</bodyName>
-        <topicName>wamv/sensors/imu/imu/data</topicName>
-        <serviceName>wamv/sensors/imu_service</serviceName>
-        <frameId>wamv/imu_wamv_link</frameId>
-        <alwaysOn>1</alwaysOn>
-        <accelOffset>0.0 0.0 0.0</accelOffset>
-        <accelDrift>0.0 0.0 0.0</accelDrift>
-        <accelDriftFrequency>0.00027 0.00027 0.000027</accelDriftFrequency>
-        <accelGaussianNoise>0.0 0.0 0.0</accelGaussianNoise>
-        <rateOffset>0.0 0.0 0.0</rateOffset>
-        <rateDrift>0.0 0.0 0.0</rateDrift>
-        <rateDriftFrequency>0.00027 0.00027 0.000027</rateDriftFrequency>
-        <rateGaussianNoise>0.0 0.0 0.0</rateGaussianNoise>
-        <yawOffset>0.0</yawOffset>
-        <yawDrift>0.0</yawDrift>
-        <yawDriftFrequency>0.00027</yawDriftFrequency>
-        <yawGaussianNoise>0.0</yawGaussianNoise>
-        <robotNamespace>/</robotNamespace>
-      </plugin>
-      <plugin name='ball_shooter_plugin' filename='libball_shooter_plugin.so'>
-        <projectile>
-          <model_name>blue_projectile</model_name>
-          <link_name>link</link_name>
-          <frame>wamv/ball_shooter_launcher_link</frame>
-          <pose>0.14 0 0 0 0 0</pose>
-        </projectile>
-        <num_shots>4</num_shots>
-        <shot_force>300</shot_force>
-        <topic>wamv/shooters/ball_shooter/fire</topic>
-        <robotNamespace>/</robotNamespace>
-      </plugin>
-      <plugin name='pinger_plugin' filename='libusv_gazebo_acoustic_pinger_plugin.so'>
-        <frameId>wamv/pinger</frameId>
-        <topicName>wamv/sensors/pingers/hydrophones/range_bearing</topicName>
-        <setPositionTopicName>wamv/sensors/pingers/hydrophones/set_pinger_position</setPositionTopicName>
-        <rangeNoise>
-          <noise>
-            <type>gaussian</type>
-            <mean>0.0</mean>
-            <stddev>3.0</stddev>
-          </noise>
-        </rangeNoise>
-        <bearingNoise>
-          <noise>
-            <type>gaussian</type>
-            <mean>0.0</mean>
-            <stddev>0.01</stddev>
-          </noise>
-        </bearingNoise>
-        <elevationNoise>
-          <noise>
-            <type>gaussian</type>
-            <mean>0.0</mean>
-            <stddev>0.01</stddev>
-          </noise>
-        </elevationNoise>
-        <robotNamespace>/</robotNamespace>
-      </plugin>
-      <plugin name='wamv_gazebo_thrust' filename='libusv_gazebo_thrust_plugin.so'>
-        <cmdTimeout>1.0</cmdTimeout>
-        <robotNamespace>wamv</robotNamespace>
-        <thruster>
-          <linkName>BL_propeller_link</linkName>
-          <propJointName>BL_engine_propeller_joint</propJointName>
-          <engineJointName>BL_chasis_engine_joint</engineJointName>
-          <cmdTopic>thrusters/BL_thrust_cmd</cmdTopic>
-          <angleTopic>thrusters/BL_thrust_angle</angleTopic>
-          <enableAngle>1</enableAngle>
-          <mappingType>1</mappingType>
-          <maxCmd>1.0</maxCmd>
-          <maxForceFwd>250.0</maxForceFwd>
-          <maxForceRev>-100.0</maxForceRev>
-          <maxAngle>1.5707963267948966</maxAngle>
-        </thruster>
-        <thruster>
-          <linkName>BR_propeller_link</linkName>
-          <propJointName>BR_engine_propeller_joint</propJointName>
-          <engineJointName>BR_chasis_engine_joint</engineJointName>
-          <cmdTopic>thrusters/BR_thrust_cmd</cmdTopic>
-          <angleTopic>thrusters/BR_thrust_angle</angleTopic>
-          <enableAngle>1</enableAngle>
-          <mappingType>1</mappingType>
-          <maxCmd>1.0</maxCmd>
-          <maxForceFwd>250.0</maxForceFwd>
-          <maxForceRev>-100.0</maxForceRev>
-          <maxAngle>1.5707963267948966</maxAngle>
-        </thruster>
-        <thruster>
-          <linkName>FL_propeller_link</linkName>
-          <propJointName>FL_engine_propeller_joint</propJointName>
-          <engineJointName>FL_chasis_engine_joint</engineJointName>
-          <cmdTopic>thrusters/FL_thrust_cmd</cmdTopic>
-          <angleTopic>thrusters/FL_thrust_angle</angleTopic>
-          <enableAngle>1</enableAngle>
-          <mappingType>1</mappingType>
-          <maxCmd>1.0</maxCmd>
-          <maxForceFwd>250.0</maxForceFwd>
-          <maxForceRev>-100.0</maxForceRev>
-          <maxAngle>1.5707963267948966</maxAngle>
-        </thruster>
-        <thruster>
-          <linkName>FR_propeller_link</linkName>
-          <propJointName>FR_engine_propeller_joint</propJointName>
-          <engineJointName>FR_chasis_engine_joint</engineJointName>
-          <cmdTopic>thrusters/FR_thrust_cmd</cmdTopic>
-          <angleTopic>thrusters/FR_thrust_angle</angleTopic>
-          <enableAngle>1</enableAngle>
-          <mappingType>1</mappingType>
-          <maxCmd>1.0</maxCmd>
-          <maxForceFwd>250.0</maxForceFwd>
-          <maxForceRev>-100.0</maxForceRev>
-          <maxAngle>1.5707963267948966</maxAngle>
-        </thruster>
-      </plugin>
-      <plugin name='usv_dynamics_wamv_dynamics_plugin' filename='libusv_gazebo_dynamics_plugin.so'>
-        <bodyName>wamv/base_link</bodyName>
-        <waterLevel>0</waterLevel>
-        <waterDensity>997.8</waterDensity>
-        <xDotU>0.0</xDotU>
-        <yDotV>0.0</yDotV>
-        <nDotR>0.0</nDotR>
-        <xU>51.3</xU>
-        <xUU>72.4</xUU>
-        <yV>40.0</yV>
-        <yVV>0.0</yVV>
-        <zW>500.0</zW>
-        <kP>50.0</kP>
-        <mQ>50.0</mQ>
-        <nR>400.0</nR>
-        <nRR>0.0</nRR>
-        <hullRadius>0.213</hullRadius>
-        <boatWidth>2.4</boatWidth>
-        <boatLength>4.9</boatLength>
-        <length_n>2</length_n>
-        <wave_model>ocean_waves</wave_model>
-        <robotNamespace>/</robotNamespace>
-      </plugin>
-      <frame name='wamv/front_left_cam_to_front_left_cam_link_optical_joint' attached_to='wamv/front_left_cam_link'>
-        <pose>0 0 0 -1.5708 -0 -1.5708</pose>
-      </frame>
-      <frame name='wamv/front_left_cam_link_optical' attached_to='wamv/front_left_cam_to_front_left_cam_link_optical_joint'/>
-      <frame name='wamv/front_left_cam_post_arm_to_front_left_cam_joint' attached_to='wamv/front_left_cam_post_arm_link'>
-        <pose>0.02 0 0.027 0 0.20944 0</pose>
-      </frame>
-      <frame name='wamv/front_left_cam_link' attached_to='wamv/front_left_cam_post_arm_to_front_left_cam_joint'/>
-      <frame name='wamv/front_left_cam_post_to_front_left_cam_post_arm_joint' attached_to='wamv/front_left_cam_post_link'>
-        <pose>0.03 0 0.08825 0 -0 0</pose>
-      </frame>
-      <frame name='wamv/front_left_cam_post_arm_link' attached_to='wamv/front_left_cam_post_to_front_left_cam_post_arm_joint'/>
-      <frame name='wamv/base_to_front_left_cam_post_joint' attached_to='wamv/base_link'>
-        <pose>0.92 0.1 1.38475 0 -0 0</pose>
-      </frame>
-      <frame name='wamv/front_left_cam_post_link' attached_to='wamv/base_to_front_left_cam_post_joint'/>
-      <frame name='wamv/front_right_cam_to_front_right_cam_link_optical_joint' attached_to='wamv/front_right_cam_link'>
-        <pose>0 0 0 -1.5708 -0 -1.5708</pose>
-      </frame>
-      <frame name='wamv/front_right_cam_link_optical' attached_to='wamv/front_right_cam_to_front_right_cam_link_optical_joint'/>
-      <frame name='wamv/front_right_cam_post_arm_to_front_right_cam_joint' attached_to='wamv/front_right_cam_post_arm_link'>
-        <pose>0.02 0 0.027 0 0.20944 0</pose>
-      </frame>
-      <frame name='wamv/front_right_cam_link' attached_to='wamv/front_right_cam_post_arm_to_front_right_cam_joint'/>
-      <frame name='wamv/front_right_cam_post_to_front_right_cam_post_arm_joint' attached_to='wamv/front_right_cam_post_link'>
-        <pose>0.03 0 0.08825 0 -0 0</pose>
-      </frame>
-      <frame name='wamv/front_right_cam_post_arm_link' attached_to='wamv/front_right_cam_post_to_front_right_cam_post_arm_joint'/>
-      <frame name='wamv/base_to_front_right_cam_post_joint' attached_to='wamv/base_link'>
-        <pose>0.92 -0.1 1.38475 0 -0 0</pose>
-      </frame>
-      <frame name='wamv/front_right_cam_post_link' attached_to='wamv/base_to_front_right_cam_post_joint'/>
-      <frame name='wamv/velodyne_post_arm_to_velodyne_joint' attached_to='velodyne_post_arm_link'>
-        <pose>0.04 0 0.05 0 0.261799 0</pose>
-      </frame>
-      <frame name='velodyne' attached_to='wamv/velodyne_post_arm_to_velodyne_joint'/>
-      <frame name='wamv/velodyne_post_to_velodyne_post_arm_joint' attached_to='velodyne_post_link'>
-        <pose>0.03 0 0.32675 0 -0 0</pose>
-      </frame>
-      <frame name='velodyne_post_arm_link' attached_to='wamv/velodyne_post_to_velodyne_post_arm_joint'/>
-      <frame name='wamv/base_to_velodyne_post_joint' attached_to='wamv/base_link'>
-        <pose>0.704 0 1.62325 0 -0 0</pose>
-      </frame>
-      <frame name='velodyne_post_link' attached_to='wamv/base_to_velodyne_post_joint'/>
-      <frame name='wamv/dummy_joint' attached_to='wamv/base_link'>
-        <pose>0 0 0 0 -0 0</pose>
-      </frame>
-      <frame name='wamv/dummy_link' attached_to='wamv/dummy_joint'/>
-      <frame name='wamv/hydrophones_pinger_joint' attached_to='wamv/base_link'>
-        <pose>1 0 -1 0 -0 0</pose>
-      </frame>
-      <frame name='wamv/hydrophones' attached_to='wamv/hydrophones_pinger_joint'/>
-      <pose>-527 167 0.1 0 -0 1</pose>
-    </model>
-    <model name='polyform_a2_red'>
+    <model name='red_circular_buoy'>
       <static>0</static>
       <pose>-522.515 184.985 0 0 -0 0</pose>
       <link name='base_link'>
@@ -3634,7 +1508,7 @@
         </buoyancy>
       </plugin>
     </model>
-    <model name='polyform_a2_green'>
+    <model name='green_circular_buoy'>
       <static>0</static>
       <pose>-534.625 187.168 0 0 -0 0</pose>
       <link name='base_link'>
@@ -3703,7 +1577,7 @@
         </buoyancy>
       </plugin>
     </model>
-    <model name='polyform_a2_blue'>
+    <model name='blue_circular_buoy'>
       <static>0</static>
       <pose>-531.036 180.328 0 0 -0 0</pose>
       <link name='base_link'>
@@ -3947,7 +1821,7 @@
           <wrench>0.629708 -1.07887 -6.3e-05 0 -0 0</wrench>
         </link>
       </model>
-      <model name='polyform_a2_blue'>
+      <model name='blue_circular_buoy'>
         <pose>-534.036 187.328 0.10507 0 -0 0</pose>
         <scale>1 1 1</scale>
         <link name='base_link'>
@@ -3957,7 +1831,7 @@
           <wrench>0 0 -0 0 -0 0</wrench>
         </link>
       </model>
-      <model name='polyform_a2_green'>
+      <model name='green_circular_buoy'>
         <pose>-531.625 180.168 0.10507 0 -0 0</pose>
         <scale>1 1 1</scale>
         <link name='base_link'>
@@ -3967,7 +1841,7 @@
           <wrench>0 0 -0 0 -0 0</wrench>
         </link>
       </model>
-      <model name='polyform_a2_red'>
+      <model name='red_circular_buoy'>
         <pose>-523.515 183.985 0.10507 0 -0 0</pose>
         <scale>1 1 1</scale>
         <link name='base_link'>