Merge branch 'main' into mabelzhang/science_viz

docker/debug_integration/Dockerfile

@@ -41,7 +41,6 @@ RUN apt-get update \
 
 # Add Ignition's latest packages, which may be more up-to-date than the ones from the MBARI image
 RUN /bin/sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list' && \
-    /bin/sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-prerelease `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-prerelease.list' && \
     /bin/sh -c 'wget http://packages.osrfoundation.org/gazebo.key -O - | apt-key add -'
 
 # Install the latest Ignition binaries

docker/tests/Dockerfile

@@ -41,7 +41,6 @@ RUN apt-get update \
 
 # Add Ignition's latest packages, which may be more up-to-date than the ones from the MBARI image
 RUN /bin/sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list' && \
-    /bin/sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-prerelease `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-prerelease.list' && \
     /bin/sh -c 'wget http://packages.osrfoundation.org/gazebo.key -O - | apt-key add -'
 
 # Install the latest Ignition binaries

lrauv_description/models/tethys/model.sdf

@@ -344,7 +344,7 @@
     </joint>
 
     <joint name="propeller_joint" type="revolute">
-      <pose>0 0 0 0 0 0</pose>
+      <pose degrees="true">0 0 0   0 180 0</pose>
       <parent>base_link</parent>
       <child>propeller</child>
       <axis>

lrauv_description/models/tethys_equipped/model.sdf

@@ -93,6 +93,7 @@
         name="ignition::gazebo::systems::Thruster">
         <namespace>tethys</namespace>
         <joint_name>propeller_joint</joint_name>
+        <!-- Be sure to update TethysComm when updating these numbers -->
         <thrust_coefficient>0.004422</thrust_coefficient>
         <fluid_density>1000</fluid_density>
         <propeller_diameter>0.2</propeller_diameter>

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/example/example_thruster.cc

@@ -21,7 +21,10 @@
  */
 
 /**
- * Send velocity commands to the thruster.
+ * Send angular velocity commands to the propeller.
+ *
+ * Positive values rotate the propeller clockwise when looking
+ * from the back, and propel the vehicle forward.
  *
  * Usage:
  *   $ LRAUV_example_thruster <vehicle_name> <rad_per_sec>

lrauv_ignition_plugins/proto/lrauv_command.proto

@@ -37,7 +37,12 @@ message LRAUVCommand
   /// \brief Optional header data
   ignition.msgs.Header header  = 1;
 
-  float propOmega_             = 2;
+  float propOmega_             = 2;  // Angular velocity that the controller
+                                     // believes the propeller is currently at.
+                                     // Unit: rad / s. Positive values rotate
+                                     // the propeller clockwise when looking
+                                     // from the back, and propel the vehicle
+                                     // forward.
   float rudderAngle_           = 3;  // Angle that the controller believes the
                                      // rudder is currently at. Unit: radians.
                                      // Higher values have the vertical fins
@@ -60,7 +65,11 @@ message LRAUVCommand
   bool dropWeightState_        = 7;  // Indicator "dropweight OK"
                                      // 1 = in place, 0 = dropped
 
-  float propOmegaAction_       = 8;
+  float propOmegaAction_       = 8;  // Target angular velocity for the
+                                     // propeller. Unit: rad / s. Positive
+                                     // values rotate the propeller clockwise
+                                     // when looking from the back, and propel
+                                     // the vehicle forward.
   float rudderAngleAction_     = 9;  // Target angle for rudder joint. Unit:
                                      // radians. Higher values rotate the
                                      // vertical fins clockwise when looking

lrauv_ignition_plugins/proto/lrauv_state.proto

@@ -43,9 +43,13 @@ message LRAUVState
   int32 errorPad_                 = 2;
   int32 utmZone_                  = 3;
   int32 northernHemi_             = 4;
-  float propOmega_                = 5;
-  float propThrust_               = 6;
-  float propTorque_               = 7;
+  float propOmega_                = 5;  // Current angular velocity of the
+                                        // propeller. Unit: rad / s. Positive
+                                        // values rotate the propeller
+                                        // clockwise when looking from the back,
+                                        // and propel the vehicle forward.
+  float propThrust_               = 6;  // Not populated
+  float propTorque_               = 7;  // Not populated
   float rudderAngle_              = 8;  // Angle that the rudder joint is
                                         // currently at. Unit: radians.
                                         // Higher values have the vertical fins
@@ -64,7 +68,8 @@ message LRAUVState
 
   ignition.msgs.Vector3d rph_     = 13;  // roll_, pitch_, heading_ in SimResultStruct (rad)
 
-  float speed_                    = 14;
+  float speed_                    = 14;  // Magnitude of linear velocity in the
+                                         // world frame. Unit: m / s
   double latitudeDeg_             = 15;
   double longitudeDeg_            = 16;
   float netBuoy_                  = 17;  // Net buoyancy forces applied to the

lrauv_ignition_plugins/src/TethysCommPlugin.cc

@@ -414,10 +414,15 @@ void TethysCommPlugin::CommandCallback(
   // Thruster
   ignition::msgs::Double thrusterMsg;
   // TODO(arjo):
-  // Conversion from rpm-> force b/c thruster plugin takes force
+  // Conversion from angular velocity to force b/c thruster plugin takes force
   // Maybe we should change that?
+  // https://github.com/osrf/lrauv/issues/75
   auto angVel = _msg.propomegaaction_();
-  auto force = -0.004422 * 1000 * 0.0016 * angVel * angVel;
+
+  // 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.004422 * 1000 * pow(0.2, 4) * angVel * angVel;
   if (angVel < 0)
   {
     force *=-1;
@@ -479,8 +484,8 @@ void TethysCommPlugin::PostUpdate(
   const ignition::gazebo::UpdateInfo &_info,
   const ignition::gazebo::EntityComponentManager &_ecm)
 {
-  ignition::gazebo::Link baseLink(modelLink);
-  auto modelPose = ignition::gazebo::worldPose(modelLink, _ecm);
+  ignition::gazebo::Link baseLink(this->modelLink);
+  auto modelPose = ignition::gazebo::worldPose(this->modelLink, _ecm);
 
   // Publish state
   lrauv_ignition_plugins::msgs::LRAUVState stateMsg;
@@ -500,7 +505,7 @@ void TethysCommPlugin::PostUpdate(
     ignerr << "Propeller joint has wrong size\n";
     return;
   }
-  stateMsg.set_propomega_(-propAngVelComp->Data()[0]);
+  stateMsg.set_propomega_(propAngVelComp->Data()[0]);
 
   // Rudder joint position
   auto rudderPosComp =
@@ -553,7 +558,7 @@ void TethysCommPlugin::PostUpdate(
   // Speed
   auto linearVelocity =
     _ecm.Component<ignition::gazebo::components::WorldLinearVelocity>(
-    modelLink);
+    this->modelLink);
   stateMsg.set_speed_(linearVelocity->Data().Length());
 
   // Lat long

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(
@@ -71,6 +75,8 @@ class LrauvTestFixture : public ::testing::Test
       [&](const ignition::gazebo::UpdateInfo &_info,
       const ignition::gazebo::EntityComponentManager &_ecm)
       {
+        this->dt = _info.dt;
+
         auto worldEntity = ignition::gazebo::worldEntity(_ecm);
         ignition::gazebo::World world(worldEntity);
 
@@ -104,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
@@ -233,9 +246,15 @@ class LrauvTestFixture : public ::testing::Test
   /// \brief How many times has OnPostUpdate been run
   public: unsigned int iterations{0u};
 
+  /// \brief Latest simulation time step.
+  public: std::chrono::steady_clock::duration dt{0};
+
   /// \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

@@ -37,23 +37,50 @@ TEST_F(LrauvTestFixture, Command)
   EXPECT_EQ(100, this->tethysPoses.size());
   EXPECT_NEAR(0.0, this->tethysPoses.back().Pos().X(), 1e-6);
 
-  // Propel vehicle
+  // Propel vehicle forward by giving the propeller a positive angular velocity
+  // Vehicle is supposed to move at around 1 m/s with 300 RPM.
+  // 300 RPM = 300 * 2 pi / 60 = 10 pi rad/s
   lrauv_ignition_plugins::msgs::LRAUVCommand cmdMsg;
-  cmdMsg.set_propomegaaction_(30);
+  cmdMsg.set_propomegaaction_(10 * IGN_PI);
 
   // Neutral buoyancy
+  cmdMsg.set_dropweightstate_(true);
   cmdMsg.set_buoyancyaction_(0.0005);
 
   // Run server until the command is processed and the model reaches a certain
   // point ahead (the robot moves towards -X)
-  double targetX{-1.5};
+  double targetX{-100.0};
   this->PublishCommandWhile(cmdMsg, [&]()
   {
     return this->tethysPoses.back().Pos().X() > targetX;
   });
 
-  EXPECT_LT(100, this->iterations);
-  EXPECT_LT(100, this->tethysPoses.size());
+  int minIterations{5100};
+  EXPECT_LT(minIterations, this->iterations);
+  EXPECT_LT(minIterations, this->tethysPoses.size());
+
+  // Check final position
   EXPECT_GT(targetX, this->tethysPoses.back().Pos().X());
+  EXPECT_NEAR(0.0, this->tethysPoses.back().Pos().Y(), 1e-3);
+  EXPECT_NEAR(0.0, this->tethysPoses.back().Pos().Z(), 0.05);
+  EXPECT_NEAR(0.0, this->tethysPoses.back().Rot().Roll(), 1e-2);
+  EXPECT_NEAR(0.0, this->tethysPoses.back().Rot().Pitch(), 1e-3);
+  EXPECT_NEAR(0.0, this->tethysPoses.back().Rot().Yaw(), 1e-3);
+
+  // Check approximate instantaneous speed after initial acceleration
+  double dtSec = std::chrono::duration<double>(this->dt).count();
+  ASSERT_LT(0.0, dtSec);
+  double time100it = 100 * dtSec;
+  for (unsigned int i = 1800; i < this->tethysPoses.size(); i += 100)
+  {
+    auto prevPose = this->tethysPoses[i - 100];
+    auto pose = this->tethysPoses[i];
+
+    auto dist = (pose.Pos() - prevPose.Pos()).Length();
+
+    auto linVel = dist / time100it;
+    EXPECT_LT(0.0, linVel);
+    EXPECT_NEAR(1.0, linVel, 0.06) << i;
+  }
 }