diff --git a/lrauv_description/models/tethys/model.sdf b/lrauv_description/models/tethys/model.sdf
index 4a231101..fd854274 100644
--- a/lrauv_description/models/tethys/model.sdf
+++ b/lrauv_description/models/tethys/model.sdf
@@ -273,11 +273,20 @@
0.4 0 0 0 0 0
+
0 0 0 0 0 0
-
0.3
0.000143971303
diff --git a/lrauv_description/models/tethys_equipped/model.sdf b/lrauv_description/models/tethys_equipped/model.sdf
index 5edb83b9..d5debb0f 100644
--- a/lrauv_description/models/tethys_equipped/model.sdf
+++ b/lrauv_description/models/tethys_equipped/model.sdf
@@ -177,10 +177,15 @@
name="ignition::gazebo::v6::systems::BuoyancyEnginePlugin">
buoyancy_engine
tethys
+ 1000
+
0.000080
+
0.0005
0.0005
+
0.000955
+
0.000003
+ * $ LRAUV_example_buoyancy
*/
+
#include
#include
@@ -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(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;
}
diff --git a/lrauv_ignition_plugins/proto/lrauv_command.proto b/lrauv_ignition_plugins/proto/lrauv_command.proto
index 1b36a7c6..ac6195c8 100644
--- a/lrauv_ignition_plugins/proto/lrauv_command.proto
+++ b/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;
diff --git a/lrauv_ignition_plugins/proto/lrauv_state.proto b/lrauv_ignition_plugins/proto/lrauv_state.proto
index b4d5ec2a..164b21b4 100644
--- a/lrauv_ignition_plugins/proto/lrauv_state.proto
+++ b/lrauv_ignition_plugins/proto/lrauv_state.proto
@@ -56,7 +56,10 @@ 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)
@@ -64,7 +67,9 @@ message LRAUVState
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
diff --git a/lrauv_ignition_plugins/src/TethysCommPlugin.cc b/lrauv_ignition_plugins/src/TethysCommPlugin.cc
index 64866bf8..fa19099c 100644
--- a/lrauv_ignition_plugins/src/TethysCommPlugin.cc
+++ b/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): "