From 65a533699b28a378e1b04ec254734e34ea9a200e Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Wed, 10 Nov 2021 22:16:18 +0800
Subject: [PATCH 01/10] Add starworld

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>
---
 .../test/test_hydrodynamics_equilbrium_vel.cc |  95 +++++++++
 lrauv_ignition_plugins/worlds/star_world.sdf  | 200 ++++++++++++++++++
 2 files changed, 295 insertions(+)
 create mode 100644 lrauv_ignition_plugins/test/test_hydrodynamics_equilbrium_vel.cc
 create mode 100644 lrauv_ignition_plugins/worlds/star_world.sdf

diff --git a/lrauv_ignition_plugins/test/test_hydrodynamics_equilbrium_vel.cc b/lrauv_ignition_plugins/test/test_hydrodynamics_equilbrium_vel.cc
new file mode 100644
index 00000000..7c5b2557
--- /dev/null
+++ b/lrauv_ignition_plugins/test/test_hydrodynamics_equilbrium_vel.cc
@@ -0,0 +1,95 @@
+/*
+ * Copyright (C) 2021 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+/*
+ * Development of this module has been funded by the Monterey Bay Aquarium
+ * Research Institute (MBARI) and the David and Lucile Packard Foundation
+ */
+
+/*
+* This test evaluates whether the 
+*
+*/
+
+#include <chrono>
+#include <thread>
+#include <gtest/gtest.h>
+
+#include <ignition/gazebo/TestFixture.hh>
+#include <ignition/gazebo/Util.hh>
+#include <ignition/gazebo/World.hh>
+#include <ignition/math/SphericalCoordinates.hh>
+#include <ignition/transport/Node.hh>
+
+#include "lrauv_init.pb.h"
+
+#include "TestConstants.hh"
+
+//////////////////////////////////////////////////
+TEST(SpawnTest, Spawn)
+{
+  ignition::common::Console::SetVerbosity(4);
+
+  // Setup fixture
+  auto fixture = std::make_unique<ignition::gazebo::TestFixture>(
+      ignition::common::joinPaths(
+      std::string(PROJECT_SOURCE_PATH), "worlds", "empty_environment.sdf"));
+
+  ignition::gazebo::Entity vehicle1{ignition::gazebo::kNullEntity};
+  ignition::gazebo::Entity vehicle2{ignition::gazebo::kNullEntity};
+  ignition::gazebo::Entity vehicle3{ignition::gazebo::kNullEntity};
+  ignition::gazebo::Entity vehicle4{ignition::gazebo::kNullEntity};
+
+  fixture->OnPostUpdate(
+    [&](const ignition::gazebo::UpdateInfo &_info,
+    const ignition::gazebo::EntityComponentManager &_ecm)
+    {
+      auto worldEntity = ignition::gazebo::worldEntity(_ecm);
+      ignition::gazebo::World world(worldEntity);
+
+      vehicle1 = world.ModelByName(_ecm, "tethys");
+      auto baselink1 = vehicle1.LinkByName(_ecm, "base_link");
+      if (ignition::gazebo::kNullEntity != vehicle1)
+      {
+        baselink1.
+      }
+
+      vehicle2 = world.ModelByName(_ecm, "thetys2");
+      auto baselink2 = vehicle1.LinkByName(_ecm, "base_link");
+      if (ignition::gazebo::kNullEntity != vehicle2)
+      {
+      }
+
+      vehicle3 = world.ModelByName(_ecm, "tethys3");
+      auto baselink3 = vehicle1.LinkByName(_ecm, "base_link");
+      if (ignition::gazebo::kNullEntity != vehicle3)
+      {
+      }
+
+      vehicle4 = world.ModelByName(_ecm, "tethys4");
+      auto baselink4 = vehicle1.LinkByName(_ecm, "base_link");
+      if (ignition::gazebo::kNullEntity != vehicle4)
+      {
+      }
+
+      iterations++;
+    });
+  fixture->Finalize();
+
+  // Check that vehicles don't exist
+  fixture->Server()->Run(true, 100, false);
+}
\ No newline at end of file
diff --git a/lrauv_ignition_plugins/worlds/star_world.sdf b/lrauv_ignition_plugins/worlds/star_world.sdf
new file mode 100644
index 00000000..5a7cb607
--- /dev/null
+++ b/lrauv_ignition_plugins/worlds/star_world.sdf
@@ -0,0 +1,200 @@
+<?xml version="1.0" ?>
+<!--
+  Development of this module has been funded by the Monterey Bay Aquarium
+  Research Institute (MBARI) and the David and Lucile Packard Foundation
+-->
+<sdf version="1.9">
+  <world name="multi_lrauv">
+    <scene>
+      <!-- For turquoise ambient to match particle effect -->
+      <ambient>0.0 1.0 1.0</ambient>
+      <!-- For default gray ambient -->
+      <!--background>0.8 0.8 0.8</background-->
+      <background>0.0 0.7 0.8</background>
+    </scene>
+
+    <physics name="1ms" type="ode">
+      <max_step_size>0.001</max_step_size>
+      <real_time_factor>1.0</real_time_factor>
+    </physics>
+    <plugin
+      filename="ignition-gazebo-physics-system"
+      name="ignition::gazebo::systems::Physics">
+    </plugin>
+    <plugin
+      filename="ignition-gazebo-user-commands-system"
+      name="ignition::gazebo::systems::UserCommands">
+    </plugin>
+    <plugin
+      filename="ignition-gazebo-scene-broadcaster-system"
+      name="ignition::gazebo::systems::SceneBroadcaster">
+    </plugin>
+
+    <plugin
+      filename="ignition-gazebo-buoyancy-system"
+      name="ignition::gazebo::systems::Buoyancy">
+      <uniform_fluid_density>1000</uniform_fluid_density>
+    </plugin>
+
+    <!-- Requires ParticleEmitter2 in ign-gazebo 4.8.0, which will be copied
+      to ParticleEmitter in Ignition G.
+      See https://github.com/ignitionrobotics/ign-gazebo/pull/730 -->
+    <plugin
+      filename="ignition-gazebo-particle-emitter2-system"
+      name="ignition::gazebo::systems::ParticleEmitter2">
+    </plugin>
+
+    <!-- Uncomment for time analysis -->
+    <!--plugin
+      filename="TimeAnalysisPlugin"
+      name="tethys::TimeAnalysisPlugin">
+    </plugin-->
+
+    <light type="directional" name="sun">
+      <cast_shadows>true</cast_shadows>
+      <pose>0 0 10 0 0 0</pose>
+      <diffuse>1 1 1 1</diffuse>
+      <specular>0.5 0.5 0.5 1</specular>
+      <attenuation>
+        <range>1000</range>
+        <constant>0.9</constant>
+        <linear>0.01</linear>
+        <quadratic>0.001</quadratic>
+      </attenuation>
+      <direction>-0.5 0.1 -0.9</direction>
+    </light>
+
+
+    <include>
+      <pose>-5 0 1 0 0 0</pose>
+      <uri>tethys_equipped</uri>
+    </include>
+
+    <include>
+      <pose>-4.3301 2.5 1 0 0 0.524</pose>
+      <uri>tethys_equipped</uri>
+      <name>tethys2</name>
+      <experimental:params>
+        <sensor element_id="base_link::salinity_sensor" action="modify">
+          <topic>/model/tethys2/salinity</topic>
+        </sensor>
+        <sensor element_id="base_link::temperature_sensor" action="modify">
+          <topic>/model/tethys2/temperature</topic>
+        </sensor>
+        <sensor element_id="base_link::chlorophyll_sensor" action="modify">
+          <topic>/model/tethys2/chlorophyll</topic>
+        </sensor>
+        <sensor element_id="base_link::current_sensor" action="modify">
+          <topic>/model/tethys2/current</topic>
+        </sensor>
+        <plugin element_id="tethys::Thruster" action="modify">
+          <namespace>tethys2</namespace>
+        </plugin>
+        <plugin element_id="tethys::TethysCommPlugin" action="modify">
+          <namespace>tethys2</namespace>
+          <command_topic>tethys2/command_topic</command_topic>
+          <state_topic>tethys2/state_topic</state_topic>
+        </plugin>
+        <plugin element_id="ignition::gazebo::v6::systems::BuoyancyEnginePlugin" action="modify">
+          <namespace>tethys2</namespace>
+        </plugin>
+        <plugin element_id="ignition::gazebo::systems::DetachableJoint" action="modify">
+          <topic>/model/tethys2/drop_weight</topic>
+        </plugin>
+        <plugin element_id="tethys::AcousticCommsPlugin" action="modify">
+          <address>3</address>
+        </plugin>
+        <plugin element_id="tethys::RangeBearingPlugin" action="modify">
+          <address>3</address>
+          <namespace>tethys2</namespace>
+        </plugin>
+      </experimental:params>
+    </include>
+
+    <include>
+      <pose>-2.5 -4.3301 1 0 0 1.0472</pose>
+      <uri>tethys_equipped</uri>
+      <name>tethys3</name>
+      <experimental:params>
+        <sensor element_id="base_link::salinity_sensor" action="modify">
+          <topic>/model/tethys3/salinity</topic>
+        </sensor>
+        <sensor element_id="base_link::temperature_sensor" action="modify">
+          <topic>/model/tethys3/temperature</topic>
+        </sensor>
+        <sensor element_id="base_link::chlorophyll_sensor" action="modify">
+          <topic>/model/tethys3/chlorophyll</topic>
+        </sensor>
+        <sensor element_id="base_link::current_sensor" action="modify">
+          <topic>/model/tethys3/current</topic>
+        </sensor>
+        <plugin element_id="tethys::Thruster" action="modify">
+          <namespace>tethys3</namespace>
+        </plugin>
+        <plugin element_id="tethys::TethysCommPlugin" action="modify">
+          <namespace>tethys3</namespace>
+          <command_topic>tethys3/command_topic</command_topic>
+          <state_topic>tethys3/state_topic</state_topic>
+        </plugin>
+        <plugin element_id="ignition::gazebo::v6::systems::BuoyancyEnginePlugin" action="modify">
+          <namespace>tethys3</namespace>
+        </plugin>
+        <plugin element_id="ignition::gazebo::systems::DetachableJoint" action="modify">
+          <topic>/model/tethys3/drop_weight</topic>
+        </plugin>
+        <plugin element_id="tethys::AcousticCommsPlugin" action="modify">
+          <address>4</address>
+        </plugin>
+        <plugin element_id="tethys::RangeBearingPlugin" action="modify">
+          <address>4</address>
+          <namespace>tethys3</namespace>
+        </plugin>
+      </experimental:params>
+    </include>
+
+    <include>
+      <pose>0 5 1 0 0 1.57</pose>
+      <uri>tethys_equipped</uri>
+      <name>tethys4</name>
+      <experimental:params>
+        <sensor element_id="base_link::salinity_sensor" action="modify">
+          <topic>/model/tethys4/salinity</topic>
+        </sensor>
+        <sensor element_id="base_link::temperature_sensor" action="modify">
+          <topic>/model/tethys4/temperature</topic>
+        </sensor>
+        <sensor element_id="base_link::chlorophyll_sensor" action="modify">
+          <topic>/model/tethys4/chlorophyll</topic>
+        </sensor>
+        <sensor element_id="base_link::current_sensor" action="modify">
+          <topic>/model/tethys4/current</topic>
+        </sensor>
+        <plugin element_id="tethys::Thruster" action="modify">
+          <namespace>tethys4</namespace>
+        </plugin>
+        <plugin element_id="tethys::TethysCommPlugin" action="modify">
+          <namespace>tethys4</namespace>
+          <command_topic>tethys4/command_topic</command_topic>
+          <state_topic>tethys4/state_topic</state_topic>
+        </plugin>
+        <plugin element_id="ignition::gazebo::v6::systems::BuoyancyEnginePlugin" action="modify">
+          <namespace>tethys4</namespace>
+        </plugin>
+        <plugin element_id="ignition::gazebo::systems::DetachableJoint" action="modify">
+          <topic>/model/tethys4/drop_weight</topic>
+        </plugin>
+        <plugin element_id="tethys::AcousticCommsPlugin" action="modify">
+          <address>5</address>
+        </plugin>
+        <plugin element_id="tethys::RangeBearingPlugin" action="modify">
+          <address>5</address>
+          <namespace>tethys4</namespace>
+        </plugin>
+      </experimental:params>
+
+    </include>
+
+
+ 
+  </world>
+</sdf>

From 534032d20db493b8b4a97edc0a0d956befd591f5 Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Wed, 10 Nov 2021 23:27:39 +0800
Subject: [PATCH 02/10] rename test add to build

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>
---
 lrauv_ignition_plugins/CMakeLists.txt         |  1 +
 ...est_hydrodynamics_equilibrium_velocity.cc} | 44 ++++++++++++-------
 2 files changed, 28 insertions(+), 17 deletions(-)
 rename lrauv_ignition_plugins/test/{test_hydrodynamics_equilbrium_vel.cc => test_hydrodynamics_equilibrium_velocity.cc} (64%)

diff --git a/lrauv_ignition_plugins/CMakeLists.txt b/lrauv_ignition_plugins/CMakeLists.txt
index 7fa15115..ea4ee979 100644
--- a/lrauv_ignition_plugins/CMakeLists.txt
+++ b/lrauv_ignition_plugins/CMakeLists.txt
@@ -237,6 +237,7 @@ if(BUILD_TESTING)
     test_buoyancy_action
     test_controller
     test_drop_weight
+    test_hydrodynamics_equilibrium_velocity
     test_mass_shifter
     test_mission_depth_vbs
     test_mission_pitch_mass
diff --git a/lrauv_ignition_plugins/test/test_hydrodynamics_equilbrium_vel.cc b/lrauv_ignition_plugins/test/test_hydrodynamics_equilibrium_velocity.cc
similarity index 64%
rename from lrauv_ignition_plugins/test/test_hydrodynamics_equilbrium_vel.cc
rename to lrauv_ignition_plugins/test/test_hydrodynamics_equilibrium_velocity.cc
index 7c5b2557..838d7d67 100644
--- a/lrauv_ignition_plugins/test/test_hydrodynamics_equilbrium_vel.cc
+++ b/lrauv_ignition_plugins/test/test_hydrodynamics_equilibrium_velocity.cc
@@ -42,41 +42,52 @@
 //////////////////////////////////////////////////
 TEST(SpawnTest, Spawn)
 {
-  ignition::common::Console::SetVerbosity(4);
+  using namespace ignition;
+  common::Console::SetVerbosity(4);
 
   // Setup fixture
   auto fixture = std::make_unique<ignition::gazebo::TestFixture>(
       ignition::common::joinPaths(
       std::string(PROJECT_SOURCE_PATH), "worlds", "empty_environment.sdf"));
 
-  ignition::gazebo::Entity vehicle1{ignition::gazebo::kNullEntity};
-  ignition::gazebo::Entity vehicle2{ignition::gazebo::kNullEntity};
-  ignition::gazebo::Entity vehicle3{ignition::gazebo::kNullEntity};
-  ignition::gazebo::Entity vehicle4{ignition::gazebo::kNullEntity};
+  gazebo::Entity vehicle1{gazebo::kNullEntity};
+  gazebo::Entity vehicle2{gazebo::kNullEntity};
+  gazebo::Entity vehicle3{gazebo::kNullEntity};
+  gazebo::Entity vehicle4{gazebo::kNullEntity};
+
+  std::vector<math::Vector3d> velocitiesV1;
+  std::vector<math::Vector3d> velocitiesV2;
+  std::vector<math::Vector3d> velocitiesV3;
+  std::vector<math::Vector3d> velocitiesV4;
+  
+  std::vector<math::Pose3d> posesV1;
+  std::vector<math::Pose3d> posesV2;
+  std::vector<math::Pose3d> posesV3;
+  std::vector<math::Pose3d> posesV4;
 
   fixture->OnPostUpdate(
-    [&](const ignition::gazebo::UpdateInfo &_info,
-    const ignition::gazebo::EntityComponentManager &_ecm)
+    [&](const gazebo::UpdateInfo &_info,
+    const gazebo::EntityComponentManager &_ecm)
     {
-      auto worldEntity = ignition::gazebo::worldEntity(_ecm);
-      ignition::gazebo::World world(worldEntity);
+      auto worldEntity = gazebo::worldEntity(_ecm);
+      gazebo::World world(worldEntity);
 
       vehicle1 = world.ModelByName(_ecm, "tethys");
-      auto baselink1 = vehicle1.LinkByName(_ecm, "base_link");
-      if (ignition::gazebo::kNullEntity != vehicle1)
+      if (gazebo::kNullEntity != vehicle1)
       {
-        baselink1.
+        auto baselink1 = Model(vehicle1).LinkByName(_ecm, "base_link");
+        gazebo::Link link(baselink1);
+        auto velocity = link.WorldLinearVelocity(_ecm);
+        auto pose = link.WorldPose(_ecm);
       }
 
       vehicle2 = world.ModelByName(_ecm, "thetys2");
-      auto baselink2 = vehicle1.LinkByName(_ecm, "base_link");
-      if (ignition::gazebo::kNullEntity != vehicle2)
+      if (gazebo::kNullEntity != vehicle2)
       {
       }
 
       vehicle3 = world.ModelByName(_ecm, "tethys3");
-      auto baselink3 = vehicle1.LinkByName(_ecm, "base_link");
-      if (ignition::gazebo::kNullEntity != vehicle3)
+      if (gazebo::kNullEntity != vehicle3)
       {
       }
 
@@ -86,7 +97,6 @@ TEST(SpawnTest, Spawn)
       {
       }
 
-      iterations++;
     });
   fixture->Finalize();
 

From 02e4f76d491188b1669f65c7c64d7b06479880fa Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Thu, 11 Nov 2021 12:36:04 +0800
Subject: [PATCH 03/10] check final velocity with hydrodynamics.

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>
---
 ...test_hydrodynamics_equilibrium_velocity.cc | 183 ++++++++++++++----
 lrauv_ignition_plugins/worlds/star_world.sdf  |   5 +-
 2 files changed, 149 insertions(+), 39 deletions(-)

diff --git a/lrauv_ignition_plugins/test/test_hydrodynamics_equilibrium_velocity.cc b/lrauv_ignition_plugins/test/test_hydrodynamics_equilibrium_velocity.cc
index 838d7d67..aca91df1 100644
--- a/lrauv_ignition_plugins/test/test_hydrodynamics_equilibrium_velocity.cc
+++ b/lrauv_ignition_plugins/test/test_hydrodynamics_equilibrium_velocity.cc
@@ -21,8 +21,8 @@
  */
 
 /*
-* This test evaluates whether the 
-*
+* This test evaluates whether the hydrodynamic plugin successfully performs
+* damping when a thrust is applied.
 */
 
 #include <chrono>
@@ -32,13 +32,33 @@
 #include <ignition/gazebo/TestFixture.hh>
 #include <ignition/gazebo/Util.hh>
 #include <ignition/gazebo/World.hh>
+#include <ignition/gazebo/Model.hh>
+#include <ignition/gazebo/Link.hh>
 #include <ignition/math/SphericalCoordinates.hh>
 #include <ignition/transport/Node.hh>
 
 #include "lrauv_init.pb.h"
-
 #include "TestConstants.hh"
 
+//////////////////////////////////////////////////
+void commandVehicleForward(const std::string &name)
+{
+  using namespace ignition;
+
+  transport::Node node;
+  auto pub =
+      node.Advertise<msgs::Double>(
+          "/model/" + name + "/joint/propeller_joint/cmd_pos");
+
+  msgs::Double thrustCmd;
+  thrustCmd.set_data(-6.7);
+
+  for (int i = 0; i < 3; i++)
+  {
+    pub.Publish(thrustCmd);
+  }
+}
+
 //////////////////////////////////////////////////
 TEST(SpawnTest, Spawn)
 {
@@ -48,7 +68,7 @@ TEST(SpawnTest, Spawn)
   // Setup fixture
   auto fixture = std::make_unique<ignition::gazebo::TestFixture>(
       ignition::common::joinPaths(
-      std::string(PROJECT_SOURCE_PATH), "worlds", "empty_environment.sdf"));
+        std::string(PROJECT_SOURCE_PATH), "worlds", "star_world.sdf"));
 
   gazebo::Entity vehicle1{gazebo::kNullEntity};
   gazebo::Entity vehicle2{gazebo::kNullEntity};
@@ -59,47 +79,136 @@ TEST(SpawnTest, Spawn)
   std::vector<math::Vector3d> velocitiesV2;
   std::vector<math::Vector3d> velocitiesV3;
   std::vector<math::Vector3d> velocitiesV4;
-  
+
   std::vector<math::Pose3d> posesV1;
   std::vector<math::Pose3d> posesV2;
   std::vector<math::Pose3d> posesV3;
   std::vector<math::Pose3d> posesV4;
 
   fixture->OnPostUpdate(
-    [&](const gazebo::UpdateInfo &_info,
-    const gazebo::EntityComponentManager &_ecm)
-    {
-      auto worldEntity = gazebo::worldEntity(_ecm);
-      gazebo::World world(worldEntity);
-
-      vehicle1 = world.ModelByName(_ecm, "tethys");
-      if (gazebo::kNullEntity != vehicle1)
-      {
-        auto baselink1 = Model(vehicle1).LinkByName(_ecm, "base_link");
-        gazebo::Link link(baselink1);
-        auto velocity = link.WorldLinearVelocity(_ecm);
-        auto pose = link.WorldPose(_ecm);
-      }
-
-      vehicle2 = world.ModelByName(_ecm, "thetys2");
-      if (gazebo::kNullEntity != vehicle2)
+      [&](const gazebo::UpdateInfo &_info,
+          const gazebo::EntityComponentManager &_ecm)
       {
-      }
+        auto worldEntity = gazebo::worldEntity(_ecm);
+        gazebo::World world(worldEntity);
+
+        vehicle1 = world.ModelByName(_ecm, "tethys");
+        if (gazebo::kNullEntity != vehicle1)
+        {
+          auto baselink = gazebo::Model(vehicle1).LinkByName(_ecm, "base_link");
+          gazebo::Link link(baselink);
+          auto velocity = link.WorldLinearVelocity(_ecm);
+          auto pose = link.WorldPose(_ecm);
+          if(!pose.has_value() || !velocity.has_value())
+          {
+            ignerr << "No pose/velocity\n";
+            return;
+          }
+          velocitiesV1.push_back(velocity.value());
+          posesV1.push_back(pose.value());
+        }
+        else
+        {
+          ignerr << "Model tethys not found\n";
+          return;
+        }
+
+        vehicle2 = world.ModelByName(_ecm, "tethys2");
+        if (gazebo::kNullEntity != vehicle2)
+        {
+          auto baselink = gazebo::Model(vehicle2).LinkByName(_ecm, "base_link");
+          gazebo::Link link(baselink);
+          auto velocity = link.WorldLinearVelocity(_ecm);
+          auto pose = link.WorldPose(_ecm);
+          if(!pose.has_value() || !velocity.has_value())
+          {
+            ignerr << "No pose/velocity\n";
+          }
+          velocitiesV2.push_back(velocity.value());
+          posesV2.push_back(pose.value());
+        }
+        else
+        {
+          ignerr << "Model tethys2 not found\n";
+          return;
+        }
+
+        vehicle3 = world.ModelByName(_ecm, "tethys3");
+        if (gazebo::kNullEntity != vehicle3)
+        {
+          auto baselink = gazebo::Model(vehicle3).LinkByName(_ecm, "base_link");
+          gazebo::Link link(baselink);
+          auto velocity = link.WorldLinearVelocity(_ecm);
+          auto pose = link.WorldPose(_ecm);
+          if(!pose.has_value() || !velocity.has_value())
+          {
+            ignerr << "No pose/velocity\n";
+            return;
+          }
+          velocitiesV3.push_back(velocity.value());
+          posesV3.push_back(pose.value());
+        }
+        else
+        {
+          ignerr << "Model tethys3 not found\n";
+          return;
+        }
+
+        vehicle4 = world.ModelByName(_ecm, "tethys4");
+        if (ignition::gazebo::kNullEntity != vehicle4)
+        {
+          auto baselink = gazebo::Model(vehicle4).LinkByName(_ecm, "base_link");
+          gazebo::Link link(baselink);
+          auto velocity = link.WorldLinearVelocity(_ecm);
+          auto pose = link.WorldPose(_ecm);
+          if(!pose.has_value() || !velocity.has_value())
+          {
+            ignerr << "No pose/velocity\n";
+            return;
+          }
+          velocitiesV4.push_back(velocity.value());
+          posesV4.push_back(pose.value());
+        }
+        else
+        {
+          ignerr << "Model tethys4 not found\n";
+          return;
+        }
+      });
+  fixture->Finalize();
 
-      vehicle3 = world.ModelByName(_ecm, "tethys3");
-      if (gazebo::kNullEntity != vehicle3)
-      {
-      }
+  commandVehicleForward("tethys");
+  commandVehicleForward("tethys2");
+  commandVehicleForward("tethys3");
+  commandVehicleForward("tethys4");
 
-      vehicle4 = world.ModelByName(_ecm, "tethys4");
-      auto baselink4 = vehicle1.LinkByName(_ecm, "base_link");
-      if (ignition::gazebo::kNullEntity != vehicle4)
-      {
-      }
+  // Check that vehicles don't exist
+  fixture->Server()->Run(true, 50000, false);
 
-    });
-  fixture->Finalize();
+  // Expect all their final velocity lengths to be similar - around 1m/s as
+  // specified early on.
 
-  // Check that vehicles don't exist
-  fixture->Server()->Run(true, 100, false);
-}
\ No newline at end of file
+  EXPECT_NEAR(
+    velocitiesV1.rbegin()->Length(), velocitiesV2.rbegin()->Length(), 1e-3);
+  EXPECT_NEAR(
+    velocitiesV1.rbegin()->Length(), velocitiesV3.rbegin()->Length(), 1e-3);
+  EXPECT_NEAR(
+    velocitiesV1.rbegin()->Length(), velocitiesV4.rbegin()->Length(), 1e-3);
+  
+  // This value seems a little off. Possibly due to the sinking motion
+  EXPECT_NEAR(
+    velocitiesV1.rbegin()->Length(), 1.01, 1e-2);
+
+  // Should not have a Z velocity.
+  // TODO(arjo): We seem to have a very slight 0.3mm/s sinking motion
+  EXPECT_NEAR(
+    velocitiesV1.rbegin()->Z(), 0, 1e-3);
+
+
+  // Rotations should not have changed much throuh the course of the test
+
+  std::cout << "tethys 1 pose " << *posesV1.rbegin() << "\n";
+  std::cout << "tethys 2 pose " << *posesV2.rbegin() << "\n";
+  std::cout << "tethys 3 pose " << *posesV3.rbegin() << "\n";
+  std::cout << "tethys 4 pose " << *posesV4.rbegin() << "\n";
+}
diff --git a/lrauv_ignition_plugins/worlds/star_world.sdf b/lrauv_ignition_plugins/worlds/star_world.sdf
index 5a7cb607..b5c722bb 100644
--- a/lrauv_ignition_plugins/worlds/star_world.sdf
+++ b/lrauv_ignition_plugins/worlds/star_world.sdf
@@ -68,10 +68,11 @@
     <include>
       <pose>-5 0 1 0 0 0</pose>
       <uri>tethys_equipped</uri>
+      <name>tethys</name>
     </include>
 
     <include>
-      <pose>-4.3301 2.5 1 0 0 0.524</pose>
+      <pose>-4.3301 -2.5 1 0 0 0.524</pose>
       <uri>tethys_equipped</uri>
       <name>tethys2</name>
       <experimental:params>
@@ -153,7 +154,7 @@
     </include>
 
     <include>
-      <pose>0 5 1 0 0 1.57</pose>
+      <pose>0 -5 1 0 0 1.57</pose>
       <uri>tethys_equipped</uri>
       <name>tethys4</name>
       <experimental:params>

From 191298b9be67830d8b920a0548dfce92d9dee43b Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Thu, 11 Nov 2021 13:13:41 +0800
Subject: [PATCH 04/10] Add rotation constraints

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>
---
 ...test_hydrodynamics_equilibrium_velocity.cc | 28 +++++++++++++------
 1 file changed, 20 insertions(+), 8 deletions(-)

diff --git a/lrauv_ignition_plugins/test/test_hydrodynamics_equilibrium_velocity.cc b/lrauv_ignition_plugins/test/test_hydrodynamics_equilibrium_velocity.cc
index aca91df1..067d3f0f 100644
--- a/lrauv_ignition_plugins/test/test_hydrodynamics_equilibrium_velocity.cc
+++ b/lrauv_ignition_plugins/test/test_hydrodynamics_equilibrium_velocity.cc
@@ -22,7 +22,11 @@
 
 /*
 * This test evaluates whether the hydrodynamic plugin successfully performs
-* damping when a thrust is applied.
+* damping when a thrust is applied. Based on previous discussion with MBARI
+* at 300rpm, we should be moving at 1ms^-1. This test checks this behaviour.
+* Furthermore, by starting 4 vehicles out in different positions and
+* orientations, this test makes sure that the transforms of the hydrodynamics
+* plugin are correct.
 */
 
 #include <chrono>
@@ -185,9 +189,18 @@ TEST(SpawnTest, Spawn)
   // Check that vehicles don't exist
   fixture->Server()->Run(true, 50000, false);
 
+  ASSERT_EQ(velocitiesV1.size(), 50000);
+  ASSERT_EQ(velocitiesV2.size(), 50000);
+  ASSERT_EQ(velocitiesV3.size(), 50000);
+  ASSERT_EQ(velocitiesV4.size(), 50000);
+
+  ASSERT_EQ(posesV1.size(), 50000);
+  ASSERT_EQ(posesV2.size(), 50000);
+  ASSERT_EQ(posesV3.size(), 50000);
+  ASSERT_EQ(posesV4.size(), 50000);
+
   // Expect all their final velocity lengths to be similar - around 1m/s as
   // specified early on.
-
   EXPECT_NEAR(
     velocitiesV1.rbegin()->Length(), velocitiesV2.rbegin()->Length(), 1e-3);
   EXPECT_NEAR(
@@ -204,11 +217,10 @@ TEST(SpawnTest, Spawn)
   EXPECT_NEAR(
     velocitiesV1.rbegin()->Z(), 0, 1e-3);
 
-
   // Rotations should not have changed much throuh the course of the test
-
-  std::cout << "tethys 1 pose " << *posesV1.rbegin() << "\n";
-  std::cout << "tethys 2 pose " << *posesV2.rbegin() << "\n";
-  std::cout << "tethys 3 pose " << *posesV3.rbegin() << "\n";
-  std::cout << "tethys 4 pose " << *posesV4.rbegin() << "\n";
+  EXPECT_EQ(posesV1.rbegin()->Rot(), posesV1.begin()->Rot());
+  EXPECT_EQ(posesV2.rbegin()->Rot(), posesV2.begin()->Rot());
+  EXPECT_EQ(posesV3.rbegin()->Rot(), posesV3.begin()->Rot());
+  EXPECT_EQ(posesV4.rbegin()->Rot(), posesV4.begin()->Rot());
+  
 }

From 46d40f3333b6c54708ef912e74b4720fc793ca95 Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Thu, 11 Nov 2021 14:10:56 +0800
Subject: [PATCH 05/10] Add damping checks

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>
---
 ...test_hydrodynamics_equilibrium_velocity.cc | 50 +++++++++++++++++--
 1 file changed, 46 insertions(+), 4 deletions(-)

diff --git a/lrauv_ignition_plugins/test/test_hydrodynamics_equilibrium_velocity.cc b/lrauv_ignition_plugins/test/test_hydrodynamics_equilibrium_velocity.cc
index 067d3f0f..2b288ae7 100644
--- a/lrauv_ignition_plugins/test/test_hydrodynamics_equilibrium_velocity.cc
+++ b/lrauv_ignition_plugins/test/test_hydrodynamics_equilibrium_velocity.cc
@@ -45,14 +45,14 @@
 #include "TestConstants.hh"
 
 //////////////////////////////////////////////////
-void commandVehicleForward(const std::string &name)
+void commandVehicleForward(const std::string &_name)
 {
   using namespace ignition;
 
   transport::Node node;
   auto pub =
       node.Advertise<msgs::Double>(
-          "/model/" + name + "/joint/propeller_joint/cmd_pos");
+          "/model/" + _name + "/joint/propeller_joint/cmd_pos");
 
   msgs::Double thrustCmd;
   thrustCmd.set_data(-6.7);
@@ -64,7 +64,44 @@ void commandVehicleForward(const std::string &name)
 }
 
 //////////////////////////////////////////////////
-TEST(SpawnTest, Spawn)
+void checkDamping(const std::vector<ignition::math::Vector3d> &_velocities)
+{
+  using namespace ignition;
+  math::Vector3d prev{0, 0, 0}, prevAcc{0,0,0};
+  bool firstTime{true}, firstAcc{true};
+
+  int idx = 0;
+  for(auto vel: _velocities)
+  {
+    if (firstTime)
+    {
+      prev = vel;
+      firstTime = false;
+      continue;
+    }
+
+    auto acc = vel - prev;
+    prev = vel;
+    if(firstAcc)
+    {
+      prevAcc = acc;
+      firstAcc = false;
+    }
+
+    idx++;
+    if(idx > 30) // Wait few iterations for message to be received :(
+    {
+      EXPECT_LE(acc.Length(), prevAcc.Length());
+    }
+    prevAcc = acc;
+  }
+
+  EXPECT_EQ(firstTime, false);
+  EXPECT_EQ(firstAcc, false);
+}
+
+//////////////////////////////////////////////////
+TEST(HydrodynamicsTest, DampForwardThrust)
 {
   using namespace ignition;
   common::Console::SetVerbosity(4);
@@ -217,10 +254,15 @@ TEST(SpawnTest, Spawn)
   EXPECT_NEAR(
     velocitiesV1.rbegin()->Z(), 0, 1e-3);
 
+  // Acceleration should always be decreasing.
+  checkDamping(velocitiesV1);
+  checkDamping(velocitiesV2);
+  checkDamping(velocitiesV3);
+  checkDamping(velocitiesV4);
+
   // Rotations should not have changed much throuh the course of the test
   EXPECT_EQ(posesV1.rbegin()->Rot(), posesV1.begin()->Rot());
   EXPECT_EQ(posesV2.rbegin()->Rot(), posesV2.begin()->Rot());
   EXPECT_EQ(posesV3.rbegin()->Rot(), posesV3.begin()->Rot());
   EXPECT_EQ(posesV4.rbegin()->Rot(), posesV4.begin()->Rot());
-  
 }

From 8413beb889a3596717a08725a4c69768f661441b Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Fri, 12 Nov 2021 10:07:27 +0800
Subject: [PATCH 06/10] Hydrodynamic equilibrium

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>
---
 .../test_hydrodynamics_equilibrium_velocity.cc | 18 ++++++++++++++----
 1 file changed, 14 insertions(+), 4 deletions(-)

diff --git a/lrauv_ignition_plugins/test/test_hydrodynamics_equilibrium_velocity.cc b/lrauv_ignition_plugins/test/test_hydrodynamics_equilibrium_velocity.cc
index 2b288ae7..3e0ed134 100644
--- a/lrauv_ignition_plugins/test/test_hydrodynamics_equilibrium_velocity.cc
+++ b/lrauv_ignition_plugins/test/test_hydrodynamics_equilibrium_velocity.cc
@@ -55,12 +55,21 @@ void commandVehicleForward(const std::string &_name)
           "/model/" + _name + "/joint/propeller_joint/cmd_pos");
 
   msgs::Double thrustCmd;
-  thrustCmd.set_data(-6.7);
 
-  for (int i = 0; i < 3; i++)
+  // 300 rpm -> 31.42rads^-1
+  //   -> (31.42rads^-1)^2 * 0.004422 (thrust coeff) * 1000 (fluid density) 
+  //      * 0.2m ^ 4 (prop diameter) = 6.9857
+  thrustCmd.set_data(-6.9857);
+
+  int sleep{0};
+  int maxSleep{30};
+  for (; !pub.HasConnections() && sleep < maxSleep; ++sleep)
   {
-    pub.Publish(thrustCmd);
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
   }
+  ASSERT_LT(sleep, maxSleep);
+  pub.Publish(thrustCmd);
+
 }
 
 //////////////////////////////////////////////////
@@ -89,6 +98,7 @@ void checkDamping(const std::vector<ignition::math::Vector3d> &_velocities)
     }
 
     idx++;
+
     if(idx > 30) // Wait few iterations for message to be received :(
     {
       EXPECT_LE(acc.Length(), prevAcc.Length());
@@ -247,7 +257,7 @@ TEST(HydrodynamicsTest, DampForwardThrust)
   
   // This value seems a little off. Possibly due to the sinking motion
   EXPECT_NEAR(
-    velocitiesV1.rbegin()->Length(), 1.01, 1e-2);
+    velocitiesV1.rbegin()->Length(), 1.01, 1e-1);
 
   // Should not have a Z velocity.
   // TODO(arjo): We seem to have a very slight 0.3mm/s sinking motion

From 4fa946484bfb084388848dbf8763c5a09d6a39a0 Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Wed, 24 Nov 2021 11:05:15 +0800
Subject: [PATCH 07/10] Update to use new upstreamed thruster plugin

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>
---
 .../test/test_hydrodynamics_equilibrium_velocity.cc        | 7 +++----
 lrauv_ignition_plugins/worlds/star_world.sdf               | 6 +++---
 2 files changed, 6 insertions(+), 7 deletions(-)

diff --git a/lrauv_ignition_plugins/test/test_hydrodynamics_equilibrium_velocity.cc b/lrauv_ignition_plugins/test/test_hydrodynamics_equilibrium_velocity.cc
index 3e0ed134..002986b3 100644
--- a/lrauv_ignition_plugins/test/test_hydrodynamics_equilibrium_velocity.cc
+++ b/lrauv_ignition_plugins/test/test_hydrodynamics_equilibrium_velocity.cc
@@ -52,14 +52,14 @@ void commandVehicleForward(const std::string &_name)
   transport::Node node;
   auto pub =
       node.Advertise<msgs::Double>(
-          "/model/" + _name + "/joint/propeller_joint/cmd_pos");
+          "/model/" + _name + "/joint/propeller_joint/cmd_thrust");
 
   msgs::Double thrustCmd;
 
   // 300 rpm -> 31.42rads^-1
   //   -> (31.42rads^-1)^2 * 0.004422 (thrust coeff) * 1000 (fluid density) 
   //      * 0.2m ^ 4 (prop diameter) = 6.9857
-  thrustCmd.set_data(-6.9857);
+  thrustCmd.set_data(6.9857);
 
   int sleep{0};
   int maxSleep{30};
@@ -67,7 +67,7 @@ void commandVehicleForward(const std::string &_name)
   {
     std::this_thread::sleep_for(std::chrono::milliseconds(100));
   }
-  ASSERT_LT(sleep, maxSleep);
+  ASSERT_LE(sleep, maxSleep);
   pub.Publish(thrustCmd);
 
 }
@@ -233,7 +233,6 @@ TEST(HydrodynamicsTest, DampForwardThrust)
   commandVehicleForward("tethys3");
   commandVehicleForward("tethys4");
 
-  // Check that vehicles don't exist
   fixture->Server()->Run(true, 50000, false);
 
   ASSERT_EQ(velocitiesV1.size(), 50000);
diff --git a/lrauv_ignition_plugins/worlds/star_world.sdf b/lrauv_ignition_plugins/worlds/star_world.sdf
index b5c722bb..8652a38a 100644
--- a/lrauv_ignition_plugins/worlds/star_world.sdf
+++ b/lrauv_ignition_plugins/worlds/star_world.sdf
@@ -88,7 +88,7 @@
         <sensor element_id="base_link::current_sensor" action="modify">
           <topic>/model/tethys2/current</topic>
         </sensor>
-        <plugin element_id="tethys::Thruster" action="modify">
+        <plugin element_id="ignition::gazebo::systems::Thruster" action="modify">
           <namespace>tethys2</namespace>
         </plugin>
         <plugin element_id="tethys::TethysCommPlugin" action="modify">
@@ -129,7 +129,7 @@
         <sensor element_id="base_link::current_sensor" action="modify">
           <topic>/model/tethys3/current</topic>
         </sensor>
-        <plugin element_id="tethys::Thruster" action="modify">
+        <plugin element_id="ignition::gazebo::systems::Thruster" action="modify">
           <namespace>tethys3</namespace>
         </plugin>
         <plugin element_id="tethys::TethysCommPlugin" action="modify">
@@ -170,7 +170,7 @@
         <sensor element_id="base_link::current_sensor" action="modify">
           <topic>/model/tethys4/current</topic>
         </sensor>
-        <plugin element_id="tethys::Thruster" action="modify">
+        <plugin element_id="ignition::gazebo::systems::Thruster" action="modify">
           <namespace>tethys4</namespace>
         </plugin>
         <plugin element_id="tethys::TethysCommPlugin" action="modify">

From 3202298ba82fe9a64ca70e67bc81eb4ccfe4af78 Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Wed, 24 Nov 2021 13:56:43 +0800
Subject: [PATCH 08/10] Address remaining PR feedback. Add assertions.

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>
---
 ...test_hydrodynamics_equilibrium_velocity.cc | 70 ++++++-------------
 1 file changed, 23 insertions(+), 47 deletions(-)

diff --git a/lrauv_ignition_plugins/test/test_hydrodynamics_equilibrium_velocity.cc b/lrauv_ignition_plugins/test/test_hydrodynamics_equilibrium_velocity.cc
index 002986b3..c15d5c83 100644
--- a/lrauv_ignition_plugins/test/test_hydrodynamics_equilibrium_velocity.cc
+++ b/lrauv_ignition_plugins/test/test_hydrodynamics_equilibrium_velocity.cc
@@ -143,88 +143,64 @@ TEST(HydrodynamicsTest, DampForwardThrust)
         auto worldEntity = gazebo::worldEntity(_ecm);
         gazebo::World world(worldEntity);
 
-        vehicle1 = world.ModelByName(_ecm, "tethys");
-        if (gazebo::kNullEntity != vehicle1)
         {
+          vehicle1 = world.ModelByName(_ecm, "tethys");
+          ASSERT_NE(gazebo::kNullEntity, vehicle1);
+
           auto baselink = gazebo::Model(vehicle1).LinkByName(_ecm, "base_link");
           gazebo::Link link(baselink);
           auto velocity = link.WorldLinearVelocity(_ecm);
           auto pose = link.WorldPose(_ecm);
-          if(!pose.has_value() || !velocity.has_value())
-          {
-            ignerr << "No pose/velocity\n";
-            return;
-          }
+
+          ASSERT_TRUE(pose.has_value());
+          ASSERT_TRUE(velocity.has_value());
           velocitiesV1.push_back(velocity.value());
           posesV1.push_back(pose.value());
         }
-        else
+        
         {
-          ignerr << "Model tethys not found\n";
-          return;
-        }
+          vehicle2 = world.ModelByName(_ecm, "tethys2");
+          ASSERT_NE(gazebo::kNullEntity, vehicle2);
 
-        vehicle2 = world.ModelByName(_ecm, "tethys2");
-        if (gazebo::kNullEntity != vehicle2)
-        {
           auto baselink = gazebo::Model(vehicle2).LinkByName(_ecm, "base_link");
           gazebo::Link link(baselink);
           auto velocity = link.WorldLinearVelocity(_ecm);
           auto pose = link.WorldPose(_ecm);
-          if(!pose.has_value() || !velocity.has_value())
-          {
-            ignerr << "No pose/velocity\n";
-          }
+          ASSERT_TRUE(pose.has_value());
+          ASSERT_TRUE(velocity.has_value());
           velocitiesV2.push_back(velocity.value());
           posesV2.push_back(pose.value());
         }
-        else
-        {
-          ignerr << "Model tethys2 not found\n";
-          return;
-        }
+     
 
-        vehicle3 = world.ModelByName(_ecm, "tethys3");
-        if (gazebo::kNullEntity != vehicle3)
         {
+          vehicle3 = world.ModelByName(_ecm, "tethys3");
+          ASSERT_NE(gazebo::kNullEntity, vehicle3);
+
           auto baselink = gazebo::Model(vehicle3).LinkByName(_ecm, "base_link");
           gazebo::Link link(baselink);
           auto velocity = link.WorldLinearVelocity(_ecm);
           auto pose = link.WorldPose(_ecm);
-          if(!pose.has_value() || !velocity.has_value())
-          {
-            ignerr << "No pose/velocity\n";
-            return;
-          }
+          ASSERT_TRUE(pose.has_value());
+          ASSERT_TRUE(velocity.has_value());
           velocitiesV3.push_back(velocity.value());
           posesV3.push_back(pose.value());
         }
-        else
-        {
-          ignerr << "Model tethys3 not found\n";
-          return;
-        }
+  
 
-        vehicle4 = world.ModelByName(_ecm, "tethys4");
-        if (ignition::gazebo::kNullEntity != vehicle4)
         {
+          vehicle4 = world.ModelByName(_ecm, "tethys4");
+          ASSERT_NE(gazebo::kNullEntity, vehicle4);
+
           auto baselink = gazebo::Model(vehicle4).LinkByName(_ecm, "base_link");
           gazebo::Link link(baselink);
           auto velocity = link.WorldLinearVelocity(_ecm);
           auto pose = link.WorldPose(_ecm);
-          if(!pose.has_value() || !velocity.has_value())
-          {
-            ignerr << "No pose/velocity\n";
-            return;
-          }
+          ASSERT_TRUE(pose.has_value());
+          ASSERT_TRUE(velocity.has_value());
           velocitiesV4.push_back(velocity.value());
           posesV4.push_back(pose.value());
         }
-        else
-        {
-          ignerr << "Model tethys4 not found\n";
-          return;
-        }
       });
   fixture->Finalize();
 

From 59da8dfd6b565272039b68a1ca6cc295afc66592 Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Wed, 1 Dec 2021 09:15:30 +0800
Subject: [PATCH 09/10] spelling fix

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>
---
 .../test/test_hydrodynamics_equilibrium_velocity.cc       | 8 +++-----
 1 file changed, 3 insertions(+), 5 deletions(-)

diff --git a/lrauv_ignition_plugins/test/test_hydrodynamics_equilibrium_velocity.cc b/lrauv_ignition_plugins/test/test_hydrodynamics_equilibrium_velocity.cc
index c15d5c83..f6dbe58f 100644
--- a/lrauv_ignition_plugins/test/test_hydrodynamics_equilibrium_velocity.cc
+++ b/lrauv_ignition_plugins/test/test_hydrodynamics_equilibrium_velocity.cc
@@ -157,7 +157,7 @@ TEST(HydrodynamicsTest, DampForwardThrust)
           velocitiesV1.push_back(velocity.value());
           posesV1.push_back(pose.value());
         }
-        
+
         {
           vehicle2 = world.ModelByName(_ecm, "tethys2");
           ASSERT_NE(gazebo::kNullEntity, vehicle2);
@@ -171,7 +171,6 @@ TEST(HydrodynamicsTest, DampForwardThrust)
           velocitiesV2.push_back(velocity.value());
           posesV2.push_back(pose.value());
         }
-     
 
         {
           vehicle3 = world.ModelByName(_ecm, "tethys3");
@@ -186,7 +185,6 @@ TEST(HydrodynamicsTest, DampForwardThrust)
           velocitiesV3.push_back(velocity.value());
           posesV3.push_back(pose.value());
         }
-  
 
         {
           vehicle4 = world.ModelByName(_ecm, "tethys4");
@@ -229,7 +227,7 @@ TEST(HydrodynamicsTest, DampForwardThrust)
     velocitiesV1.rbegin()->Length(), velocitiesV3.rbegin()->Length(), 1e-3);
   EXPECT_NEAR(
     velocitiesV1.rbegin()->Length(), velocitiesV4.rbegin()->Length(), 1e-3);
-  
+
   // This value seems a little off. Possibly due to the sinking motion
   EXPECT_NEAR(
     velocitiesV1.rbegin()->Length(), 1.01, 1e-1);
@@ -245,7 +243,7 @@ TEST(HydrodynamicsTest, DampForwardThrust)
   checkDamping(velocitiesV3);
   checkDamping(velocitiesV4);
 
-  // Rotations should not have changed much throuh the course of the test
+  // Rotations should not have changed much through the course of the test
   EXPECT_EQ(posesV1.rbegin()->Rot(), posesV1.begin()->Rot());
   EXPECT_EQ(posesV2.rbegin()->Rot(), posesV2.begin()->Rot());
   EXPECT_EQ(posesV3.rbegin()->Rot(), posesV3.begin()->Rot());

From f0929eb6962946b7e44a726095858d1a94762d7a Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Wed, 1 Dec 2021 10:07:26 +0800
Subject: [PATCH 10/10] losen yoyo expectations

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>
---
 lrauv_ignition_plugins/test/test_mission_yoyo_circle.cc | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/lrauv_ignition_plugins/test/test_mission_yoyo_circle.cc b/lrauv_ignition_plugins/test/test_mission_yoyo_circle.cc
index 5b3d77a2..34c8d88b 100644
--- a/lrauv_ignition_plugins/test/test_mission_yoyo_circle.cc
+++ b/lrauv_ignition_plugins/test/test_mission_yoyo_circle.cc
@@ -100,7 +100,7 @@ TEST_F(LrauvTestFixture, YoYoCircle)
     EXPECT_LT(-22.5, pose.Pos().Z()) << i;
     if (i > 2000)
     {
-      EXPECT_GT(0.23, pose.Pos().Z()) << i;
+      EXPECT_GT(0.3, pose.Pos().Z()) << i;
     }
 
     // Pitch is between -20 and 20 deg