Document buoyancy action / VBS (#65)

Signed-off-by: Louise Poubel <louise@openrobotics.org>

lrauv_description/models/tethys/model.sdf

@@ -273,11 +273,20 @@
       <!-- TODO: Determine location of buoyancy engine -->
       <pose>0.4 0 0 0 0 0</pose>
 
+      <!-- TODO: Remove inertial and collision
+
+         This link was originally given inertia because Gazebo would segfault
+         with links without inertia. To counter this mass, a volume (collision)
+         was added, which adds buoyancy.
+
+         Gazebo doesn't have this limitation anymore, so both the inertia and
+         collision could be removed. However, it looks like the link's weight
+         and buoyancy aren't cancelling each other out precisely at the moment
+         (why?), so removing them makes the vehicle unstable.
+
+      -->
       <inertial>
         <pose>0 0 0 0 0 0</pose>
-        <!-- Ignition requires a mass assigned to the link otherwise weird
-        and wonderful segfaults may happen. So we create a neutral buoyancy link
-        and then use the plugin to emulate the weight-->
         <mass>0.3</mass>
         <inertia>
           <ixx>0.000143971303</ixx>

lrauv_description/models/tethys_equipped/model.sdf

@@ -177,10 +177,15 @@
         name="ignition::gazebo::v6::systems::BuoyancyEnginePlugin">
         <link_name>buoyancy_engine</link_name>
         <namespace>tethys</namespace>
+        <fluid_density>1000</fluid_density>
+        <!-- 80 cc == 0.00008 m^3 -->
         <min_volume>0.000080</min_volume>
+        <!-- 500 cc == 0.0005 m^3 -->
         <neutral_volume>0.0005</neutral_volume>
         <default_volume>0.0005</default_volume>
+        <!-- 955 cc == 0.000955 m^3 -->
         <max_volume>0.000955</max_volume>
+        <!-- m^3/s -->
         <max_inflation_rate>0.000003</max_inflation_rate>
       </plugin>
       <plugin

lrauv_ignition_plugins/example/example_buoyancy.cc

@@ -21,9 +21,15 @@
  */
 
 /**
+ * Control the VBS (Variable Buoyancy System).
+ *
+ * The neutral volume keeps the vehicle neutrally buoyant, higher volumes
+ * apply an upwards force, lower ones don't apply enough force to counter gravity.
+ *
  * Usage:
- *   $ LRAUV_example_buoyancy <vehicle_name>
+ *   $ LRAUV_example_buoyancy <vehicle_name> <volume_cubic_meter>
  */
+
 #include <chrono>
 #include <thread>
 
@@ -39,17 +45,30 @@ int main(int _argc, char **_argv)
     vehicleName = _argv[1];
   }
 
+  double volume{0.0004};
+  if (_argc > 2)
+  {
+    volume = atof(_argv[2]);
+  }
+
   ignition::transport::Node node;
   auto commandTopic = "/" + vehicleName + "/command_topic";
   auto commandPub =
     node.Advertise<lrauv_ignition_plugins::msgs::LRAUVCommand>(commandTopic);
 
-  lrauv_ignition_plugins::msgs::LRAUVCommand buoyancyMessage;
-  std::cout << "Expect vehicle to move up \n";
-  for (int i = 0 ; i < 10; i++)
+  while (!commandPub.HasConnections())
   {
-    buoyancyMessage.set_buoyancyaction_(0.0004);
-    commandPub.Publish(buoyancyMessage);
+    std::cout << "Command publisher waiting for connections..." << std::endl;
     std::this_thread::sleep_for(std::chrono::milliseconds(1000));
   }
+
+  lrauv_ignition_plugins::msgs::LRAUVCommand cmdMsg;
+  cmdMsg.set_buoyancyaction_(volume);
+
+  // Don't release drop-weight
+  cmdMsg.set_dropweightstate_(1);
+
+  commandPub.Publish(cmdMsg);
+
+  std::cout << "Changing VBS volume to [" << volume << "] rad" << std::endl;
 }

lrauv_ignition_plugins/proto/lrauv_command.proto

@@ -48,7 +48,10 @@ message LRAUVCommand
                                      // the mass shifter's joint is in. Unit:
                                      // meters. Positive values have the battery
                                      // forward, tilting the nose downward.
-  float buoyancyPosition_      = 6;
+  float buoyancyPosition_      = 6;  // Volume that the controller believes the
+                                     // VBS currently has. Unit: cubic meters.
+                                     // Volumes higher than the neutral volume
+                                     // push the vehicle upward.
   bool dropWeightState_        = 7;  // Indicator "dropweight OK"
                                      // 1 = in place, 0 = dropped
 
@@ -62,7 +65,10 @@ message LRAUVCommand
                                       // Unit: meters. Positive values move the
                                       // battery forward, tilting the nose
                                       // downward.
-  float buoyancyAction_        = 12;  // Volume in cubic meters
+  float buoyancyAction_        = 12;  // Target volume for the VBS (Variable
+                                      // Buoyancy System). Unit: cubic meters.
+                                      // Volumes higher than the neutral volume
+                                      // make the vehicle float.
 
   float density_               = 13;
   float dt_                    = 14;

lrauv_ignition_plugins/proto/lrauv_state.proto

@@ -56,15 +56,20 @@ message LRAUVState
                                          // Unit: meters. Positive values have
                                          // the battery forward, tilting the
                                          // nose downward.
-  float buoyancyPosition_         = 11;  // Volume in cubic meters
+  float buoyancyPosition_         = 11;  // Volume of the VBS. Unit: cubic
+                                         // meters. Volumes higher than the
+                                         // neutral volume push the vehicle
+                                         // upwards.
   float depth_                    = 12;
 
   ignition.msgs.Vector3d rph_     = 13;  // roll_, pitch_, heading_ in SimResultStruct (rad)
 
   float speed_                    = 14;
   double latitudeDeg_             = 15;
   double longitudeDeg_            = 16;
-  float netBuoy_                  = 17;
+  float netBuoy_                  = 17;  // Net buoyancy forces applied to the
+                                         // vehicle. Unit: Newtons. Currently
+                                         // not populated.
 
   ignition.msgs.Vector3d force_   = 18;  // forceX_, forceY_, forceZ_ in SimResultStruct
   ignition.msgs.Vector3d pos_     = 19;  // posX_, posY_, posZ_ in SimResultStruct

lrauv_ignition_plugins/src/TethysCommPlugin.cc

@@ -443,7 +443,6 @@ void TethysCommPlugin::CommandCallback(
 void TethysCommPlugin::BuoyancyStateCallback(
   const ignition::msgs::Double &_msg)
 {
-  // Units: cubic centimeters
   this->buoyancyBladderVolume = _msg.data();
 }
 
@@ -531,8 +530,7 @@ void TethysCommPlugin::PostUpdate(
   stateMsg.set_massposition_(massShifterPosComp->Data()[0]);
 
   // Buoyancy position
-  // Convert from cubic centimeters to cubic meters
-  stateMsg.set_buoyancyposition_(buoyancyBladderVolume);
+  stateMsg.set_buoyancyposition_(this->buoyancyBladderVolume);
 
   // Depth
   stateMsg.set_depth_(-modelPose.Pos().Z());
@@ -613,6 +611,7 @@ void TethysCommPlugin::PostUpdate(
       << "\tElevator angle: " << stateMsg.elevatorangle_() << std::endl
       << "\tRudder angle: " << stateMsg.rudderangle_() << std::endl
       << "\tMass shifter (m): " << stateMsg.massposition_() << std::endl
+      << "\tVBS volume (m^3): " << stateMsg.buoyancyposition_() << std::endl
       << "\tPitch angle (deg): "
         << stateMsg.rph_().y() * 180 / M_PI << std::endl
       << "\tCurrent (ENU, m/s): "