🤦‍♀️ Fix thruster coefficient

lrauv_description/models/tethys_equipped/model.sdf

@@ -94,7 +94,7 @@
         <namespace>tethys</namespace>
         <joint_name>propeller_joint</joint_name>
         <!-- Be sure to update TethysComm when updating these numbers -->
-        <thrust_coefficient>0.00004422</thrust_coefficient>
+        <thrust_coefficient>0.004422</thrust_coefficient>
         <fluid_density>1000</fluid_density>
         <propeller_diameter>0.2</propeller_diameter>
         <velocity_control>true</velocity_control>

lrauv_ignition_plugins/CMakeLists.txt

@@ -243,7 +243,8 @@ if(BUILD_TESTING)
     test_mission_pitch_mass
     test_mission_yoyo_circle
     test_rudder
-    test_spawn)
+    test_spawn
+    test_state_msg)
 
     add_executable(
       ${TEST_TARGET}
@@ -256,6 +257,7 @@ if(BUILD_TESTING)
       PRIVATE ignition-gazebo${IGN_GAZEBO_VER}::ignition-gazebo${IGN_GAZEBO_VER}
       lrauv_command
       lrauv_init
+      lrauv_state
     )
     include_directories(${CMAKE_CURRENT_BINARY_DIR})
     gtest_discover_tests(${TEST_TARGET})

lrauv_ignition_plugins/src/TethysCommPlugin.cc

@@ -422,7 +422,7 @@ void TethysCommPlugin::CommandCallback(
   // force = thrust_coefficient * fluid_density * omega ^ 2 *
   //         propeller_diameter ^ 4
   // These values are defined in the model's Thruster plugin's SDF
-  auto force = 0.00004422 * 1000 * 0.2 * angVel * angVel;
+  auto force = 0.004422 * 1000 * pow(0.2, 4) * angVel * angVel;
   if (angVel < 0)
   {
     force *=-1;

lrauv_ignition_plugins/test/helper/LrauvTestFixture.hh

@@ -32,6 +32,7 @@
 #include <ignition/transport/Node.hh>
 
 #include "lrauv_command.pb.h"
+#include "lrauv_state.pb.h"
 
 #include "TestConstants.hh"
 
@@ -62,6 +63,9 @@ class LrauvTestFixture : public ::testing::Test
       this->node.Advertise<lrauv_ignition_plugins::msgs::LRAUVCommand>(
       commandTopic);
 
+    auto stateTopic = "/tethys/state_topic";
+    this->node.Subscribe(stateTopic, &LrauvTestFixture::OnState, this);
+
     // Setup fixture
     this->fixture = std::make_unique<ignition::gazebo::TestFixture>(
         ignition::common::joinPaths(
@@ -106,6 +110,13 @@ class LrauvTestFixture : public ::testing::Test
     EXPECT_LT(sleep, maxSleep);
   }
 
+  /// Callback function for state from TethysComm
+  /// \param[in] _msg State message
+  private: void OnState(const lrauv_ignition_plugins::msgs::LRAUVState &_msg)
+  {
+    this->stateMsgs.push_back(_msg);
+  }
+
   /// \brief Check that a pose is within a given range.
   /// \param[in] _index Pose index in tethysPoses
   /// \param[in] _refPose Reference pose
@@ -241,6 +252,9 @@ class LrauvTestFixture : public ::testing::Test
   /// \brief All tethys world poses in order
   public: std::vector<ignition::math::Pose3d> tethysPoses;
 
+  /// \brief All state messages in order
+  public: std::vector<lrauv_ignition_plugins::msgs::LRAUVState> stateMsgs;
+
   /// \brief Test fixture
   public: std::unique_ptr<ignition::gazebo::TestFixture> fixture{nullptr};
 

lrauv_ignition_plugins/test/test_controller.cc

@@ -71,7 +71,7 @@ TEST_F(LrauvTestFixture, Command)
   double dtSec = std::chrono::duration<double>(this->dt).count();
   ASSERT_LT(0.0, dtSec);
   double time100it = 100 * dtSec;
-  for (unsigned int i = 1000; i < this->tethysPoses.size(); i += 100)
+  for (unsigned int i = 1800; i < this->tethysPoses.size(); i += 100)
   {
     auto prevPose = this->tethysPoses[i - 100];
     auto pose = this->tethysPoses[i];
@@ -80,9 +80,7 @@ TEST_F(LrauvTestFixture, Command)
 
     auto linVel = dist / time100it;
     EXPECT_LT(0.0, linVel);
-
-    // TODO(chapulina) Decrease tolerance
-    EXPECT_NEAR(1.0, linVel, 0.19) << i;
+    EXPECT_NEAR(1.0, linVel, 0.06) << i;
   }
 }
 

lrauv_ignition_plugins/test/test_mission_yoyo_circle.cc

@@ -97,10 +97,10 @@ TEST_F(LrauvTestFixture, YoYoCircle)
 
     // Depth is above 20m, and below 2m after initial descent, with some
     // tolerance
-    EXPECT_LT(-22.1, pose.Pos().Z()) << i;
+    EXPECT_LT(-22.4, pose.Pos().Z()) << i;
     if (i > 2000)
     {
-      EXPECT_GT(0.0, pose.Pos().Z()) << i;
+      EXPECT_GT(0.23, pose.Pos().Z()) << i;
     }
 
     // Pitch is between -20 and 20 deg

lrauv_ignition_plugins/test/test_state_msg.cc

@@ -0,0 +1,60 @@
+/*
+ * 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
+ */
+
+#include <chrono>
+#include <gtest/gtest.h>
+
+#include "helper/LrauvTestFixture.hh"
+#include "lrauv_command.pb.h"
+#include "lrauv_state.pb.h"
+
+//////////////////////////////////////////////////
+TEST_F(LrauvTestFixture, Command)
+{
+  // TODO(chapulina) Test other fields, see
+  // https://github.com/osrf/lrauv/pull/81
+
+  // Initial state
+  this->fixture->Server()->Run(true, 100, false);
+  EXPECT_EQ(100, this->stateMsgs.size());
+
+  auto latest = this->stateMsgs.back();
+  EXPECT_NEAR(0.0, latest.propomega_(), 1e-6);
+
+  // Propel vehicle forward by giving the propeller a positive angular velocity
+  lrauv_ignition_plugins::msgs::LRAUVCommand cmdMsg;
+  cmdMsg.set_propomegaaction_(10 * IGN_PI);
+
+  // Neutral buoyancy
+  cmdMsg.set_buoyancyaction_(0.0005);
+  cmdMsg.set_dropweightstate_(true);
+
+  // Run server until we collect more states
+  this->PublishCommandWhile(cmdMsg, [&]()
+  {
+    return this->stateMsgs.size() < 2000;
+  });
+
+  latest = this->stateMsgs.back();
+  EXPECT_NEAR(10.0 * IGN_PI, latest.propomega_(), 1e-6);
+}
+