diff --git a/examples/worlds/buoyancy_engine.sdf b/examples/worlds/buoyancy_engine.sdf
new file mode 100644
index 0000000000..145e955cab
--- /dev/null
+++ b/examples/worlds/buoyancy_engine.sdf
@@ -0,0 +1,142 @@
+
+
+
+
+
+
+ 0.0 1.0 1.0
+ 0.0 0.7 0.8
+
+
+
+ 0.001
+ 1.0
+
+
+
+
+
+
+
+
+
+
+
+ 1000
+
+
+
+ true
+ 0 0 10 0 0 0
+ 1 1 1 1
+ 0.5 0.5 0.5 1
+
+ 1000
+ 0.9
+ 0.01
+ 0.001
+
+ -0.5 0.1 -0.9
+
+
+
+ 0 0 0 0 0 0
+
+
+ 1000
+
+ 133.3333
+ 133.3333
+ 133.3333
+
+
+
+
+
+ 1 1 1
+
+
+
+
+
+
+ 1 1 1
+
+
+
+
+
+ body
+ buoyant_box
+ 0.0
+ 0.002
+ 0.002
+ 0.003
+ 0.0003
+
+
+
+
+
diff --git a/src/systems/CMakeLists.txt b/src/systems/CMakeLists.txt
index 2e2f830236..e63c1ccd86 100644
--- a/src/systems/CMakeLists.txt
+++ b/src/systems/CMakeLists.txt
@@ -92,6 +92,7 @@ add_subdirectory(apply_joint_force)
add_subdirectory(battery_plugin)
add_subdirectory(breadcrumbs)
add_subdirectory(buoyancy)
+add_subdirectory(buoyancy_engine)
add_subdirectory(collada_world_exporter)
add_subdirectory(contact)
add_subdirectory(camera_video_recorder)
diff --git a/src/systems/buoyancy_engine/BuoyancyEngine.cc b/src/systems/buoyancy_engine/BuoyancyEngine.cc
new file mode 100644
index 0000000000..14131f6c2d
--- /dev/null
+++ b/src/systems/buoyancy_engine/BuoyancyEngine.cc
@@ -0,0 +1,249 @@
+/*
+ * 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.
+ *
+ */
+
+#include
+#include
+#include
+#include
+#include
+
+#include
+#include
+#include
+#include
+#include
+#include
+
+#include "BuoyancyEngine.hh"
+
+using namespace ignition;
+using namespace gazebo;
+using namespace systems;
+
+class ignition::gazebo::systems::BuoyancyEnginePrivateData
+{
+ /// \brief Callback for incoming commands
+ /// \param[in] _volumeSetPoint - ignition message containing the desired
+ /// volume (in m^3) to fill/drain bladder to.
+ public: void OnCmdBuoyancyEngine(
+ const ignition::msgs::Double &_volumeSetPoint);
+
+ /// \brief Current volume of bladder in m^3
+ public: double bladderVolume = 3e-5;
+
+ /// \brief Maximum inflation rate in m^3*s^-1
+ public: double maxInflationRate = 3e-6;
+
+ /// \brief Set-point for volume, in m^3
+ public: double volumeSetPoint = 0.000030;
+
+ /// \brief Minimum volume of bladder in m^3
+ public: double minVolume = 0.000030;
+
+ /// \brief Maximum volume of bladder in m^3
+ public: double maxVolume = 0.000990;
+
+ /// \brief The link which the bladder is attached to
+ public: ignition::gazebo::Entity linkEntity{kNullEntity};
+
+ /// \brief The world entity
+ public: Entity world{kNullEntity};
+
+ /// \brief The fluid density in kg*m^-3
+ public: double fluidDensity = 1000;
+
+ /// \brief The neutral volume in m^3
+ public: double neutralVolume = 0.0003;
+
+ /// \brief Trasport node for control
+ public: ignition::transport::Node node;
+
+ /// \brief Publishes bladder status
+ public: ignition::transport::Node::Publisher statusPub;
+
+ /// \brief mutex for protecting bladder volume and set point.
+ public: std::mutex mtx;
+};
+
+//////////////////////////////////////////////////
+void BuoyancyEnginePrivateData::OnCmdBuoyancyEngine(
+ const ignition::msgs::Double &_volumeSetpoint)
+{
+ auto volume = std::max(this->minVolume, _volumeSetpoint.data());
+ volume = std::min(volume, this->maxVolume);
+
+ std::lock_guard lock(this->mtx);
+ this->volumeSetPoint = volume;
+}
+
+//////////////////////////////////////////////////
+BuoyancyEnginePlugin::BuoyancyEnginePlugin()
+ : dataPtr(std::make_unique())
+{
+}
+
+//////////////////////////////////////////////////
+void BuoyancyEnginePlugin::Configure(
+ const ignition::gazebo::Entity &_entity,
+ const std::shared_ptr &_sdf,
+ ignition::gazebo::EntityComponentManager &_ecm,
+ ignition::gazebo::EventManager &/*_eventMgr*/)
+{
+ auto model = ignition::gazebo::Model(_entity);
+ if (!_sdf->HasElement("link_name"))
+ {
+ ignerr << "Buoyancy Engine must be attached to some link." << std::endl;
+ return;
+ }
+
+ this->dataPtr->linkEntity =
+ model.LinkByName(_ecm, _sdf->Get("link_name"));
+ if (this->dataPtr->linkEntity == kNullEntity)
+ {
+ ignerr << "Link [" << _sdf->Get("link_name")
+ << "] was not found in model [" << model.Name(_ecm) << "]" << std::endl;
+ return;
+ }
+
+ if (_sdf->HasElement("min_volume"))
+ {
+ this->dataPtr->minVolume = _sdf->Get("min_volume");
+ }
+
+ if (_sdf->HasElement("max_volume"))
+ {
+ this->dataPtr->maxVolume = _sdf->Get("max_volume");
+ }
+
+ if (_sdf->HasElement("fluid_density"))
+ {
+ this->dataPtr->fluidDensity = _sdf->Get("fluid_density");
+ }
+
+ this->dataPtr->bladderVolume = this->dataPtr->minVolume;
+ if (_sdf->HasElement("default_volume"))
+ {
+ this->dataPtr->bladderVolume = _sdf->Get("default_volume");
+ this->dataPtr->volumeSetPoint = this->dataPtr->bladderVolume;
+ }
+
+ if (_sdf->HasElement("neutral_volume"))
+ {
+ this->dataPtr->neutralVolume = _sdf->Get("neutral_volume");
+ }
+
+ if(_sdf->HasElement("max_inflation_rate"))
+ {
+ this->dataPtr->maxInflationRate = _sdf->Get("max_inflation_rate");
+ }
+
+ this->dataPtr->world = _ecm.EntityByComponents(components::World());
+ if (this->dataPtr->world == kNullEntity)
+ {
+ ignerr << "World entity not found" <HasElement("namespace"))
+ {
+ cmdTopic = ignition::transport::TopicUtils::AsValidTopic(
+ "/model/" + _sdf->Get("namespace") + "/buoyancy_engine/");
+ statusTopic = ignition::transport::TopicUtils::AsValidTopic(
+ "/model/" + _sdf->Get("namespace")
+ + "/buoyancy_engine/current_volume");
+ }
+
+ if(!this->dataPtr->node.Subscribe(cmdTopic,
+ &BuoyancyEnginePrivateData::OnCmdBuoyancyEngine, this->dataPtr.get()))
+ {
+ ignerr << "Failed to subscribe to [" << cmdTopic << "]" << std::endl;
+ }
+
+ this->dataPtr->statusPub =
+ this->dataPtr->node.Advertise(statusTopic);
+
+ igndbg << "Listening to commands on [" << cmdTopic
+ << "], publishing status on [" << statusTopic << "]" <> DurationInSecs;
+ auto dt = std::chrono::duration_cast(_info.dt).count();
+
+ ignition::msgs::Double msg;
+
+ const components::Gravity *gravity = _ecm.Component(
+ this->dataPtr->world);
+ if (!gravity)
+ {
+ ignerr << "World has no gravity component" << std::endl;
+ return;
+ }
+
+ math::Vector3d zForce;
+ {
+ std::lock_guard lock(this->dataPtr->mtx);
+ // Adjust the bladder volume using the pump. Assume ability to pump at
+ // max flow rate
+ if (this->dataPtr->bladderVolume < this->dataPtr->volumeSetPoint)
+ {
+ this->dataPtr->bladderVolume +=
+ std::min(
+ dt * this->dataPtr->maxInflationRate,
+ this->dataPtr->volumeSetPoint - this->dataPtr->bladderVolume
+ );
+ }
+ else if (this->dataPtr->bladderVolume > this->dataPtr->volumeSetPoint)
+ {
+ this->dataPtr->bladderVolume -=
+ std::min(
+ dt * this->dataPtr->maxInflationRate,
+ this->dataPtr->bladderVolume - this->dataPtr->volumeSetPoint
+ );
+ }
+
+ /// Populate status message
+ msg.set_data(this->dataPtr->bladderVolume);
+ this->dataPtr->statusPub.Publish(msg);
+
+ // Simply use Archimede's principle to apply a force at the desired link
+ // position. We take off the neutral buoyancy element in order to simulate
+ // the mass of the oil in the bladder.
+ zForce = - gravity->Data() * this->dataPtr->fluidDensity
+ * (this->dataPtr->bladderVolume - this->dataPtr->neutralVolume);
+ }
+ ignition::gazebo::Link link(this->dataPtr->linkEntity);
+ link.AddWorldWrench(_ecm, zForce, {0, 0, 0});
+}
+
+IGNITION_ADD_PLUGIN(
+ BuoyancyEnginePlugin,
+ ignition::gazebo::System,
+ BuoyancyEnginePlugin::ISystemConfigure,
+ BuoyancyEnginePlugin::ISystemPreUpdate)
+
+IGNITION_ADD_PLUGIN_ALIAS(BuoyancyEnginePlugin,
+ "ignition::gazebo::systems::BuoyancyEngine")
diff --git a/src/systems/buoyancy_engine/BuoyancyEngine.hh b/src/systems/buoyancy_engine/BuoyancyEngine.hh
new file mode 100644
index 0000000000..c4dd70cf80
--- /dev/null
+++ b/src/systems/buoyancy_engine/BuoyancyEngine.hh
@@ -0,0 +1,114 @@
+/*
+ * 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.
+ *
+ */
+#ifndef IGNITION_GAZEBO_SYSTEMS_BUOYANCYENGINE_HH_
+#define IGNITION_GAZEBO_SYSTEMS_BUOYANCYENGINE_HH_
+
+#include
+
+#include
+
+namespace ignition
+{
+namespace gazebo
+{
+// Inline bracket to help doxygen filtering.
+inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
+namespace systems
+{
+ class BuoyancyEnginePrivateData;
+
+ /// \brief This class provides a simple mechanical bladder which is used to
+ /// control the buoyancy of an underwater glider. It uses Archimedes'
+ /// principle to apply an upward force based on the volume of the bladder. It
+ /// listens to the topic `buoyancy_engine` or
+ /// `/model/{namespace}/buoyancy_engine` topic for the volume of the bladder
+ /// in *cubicmeters*.
+ ///
+ /// ## Parameters
+ /// - The link which the plugin is attached to [required, string]
+ /// - The namespace for the topic. If empty the plugin will listen
+ /// on `buoyancy_engine` otherwise it listens on
+ /// `/model/{namespace}/buoyancy_engine` [optional, string]
+ /// - Minimum volume of the engine [optional, float,
+ /// default=0.00003m^3]
+ /// - At this volume the engine has neutral buoyancy. Used to
+ /// estimate the weight of the engine [optional, float, default=0.0003m^3]
+ /// - The volume which the engine starts at [optional, float,
+ /// default=0.0003m^3]
+ /// - Maximum volume of the engine [optional, float,
+ /// default=0.00099m^3]
+ /// - Maximum inflation rate for bladder [optional,
+ /// float, default=0.000003m^3/s]
+ /// - The fluid density of the liquid its suspended in kgm^-3.
+ /// [optional, float, default=1000kgm^-3]
+ ///
+ /// ## Topics
+ /// * Subscribes to a ignition::msgs::Double on `buoyancy_engine` or
+ /// `/model/{namespace}/buoyancy_engine`. This is the set point for the
+ /// engine.
+ /// * Publishes a ignition::msgs::Double on `buoyancy_engine` or
+ /// `/model/{namespace}/buoyancy_engine/current_volume` on the current volume
+ ///
+ /// ## Examples
+ /// To get started run:
+ /// ```
+ /// ign gazebo buoyancy_engine.sdf
+ /// ```
+ /// Enter the following in a separate terminal:
+ /// ```
+ /// ign topic -t /model/buoyant_box/buoyancy_engine/ -m ignition.msgs.Double
+ /// -p "data: 0.003"
+ /// ```
+ /// To see the box float up.
+ /// ```
+ /// ign topic -t /model/buoyant_box/buoyancy_engine/ -m ignition.msgs.Double
+ /// -p "data: 0.001"
+ /// ```
+ /// To see the box go down.
+ /// To see the current volume enter:
+ /// ```
+ /// ign topic -t /model/buoyant_box/buoyancy_engine/current_volume -e
+ /// ```
+ class BuoyancyEnginePlugin:
+ public ignition::gazebo::System,
+ public ignition::gazebo::ISystemConfigure,
+ public ignition::gazebo::ISystemPreUpdate
+ {
+ /// \brief Constructor
+ public: BuoyancyEnginePlugin();
+
+ // Documentation inherited
+ public: void Configure(
+ const ignition::gazebo::Entity &_entity,
+ const std::shared_ptr &_sdf,
+ ignition::gazebo::EntityComponentManager &_ecm,
+ ignition::gazebo::EventManager &/*_eventMgr*/
+ );
+
+ // Documentation inherited
+ public: void PreUpdate(
+ const ignition::gazebo::UpdateInfo &_info,
+ ignition::gazebo::EntityComponentManager &_ecm);
+
+ /// \brief Private data pointer
+ private: std::unique_ptr dataPtr;
+ };
+}
+}
+}
+}
+#endif
diff --git a/src/systems/buoyancy_engine/CMakeLists.txt b/src/systems/buoyancy_engine/CMakeLists.txt
new file mode 100644
index 0000000000..b849e89309
--- /dev/null
+++ b/src/systems/buoyancy_engine/CMakeLists.txt
@@ -0,0 +1,7 @@
+gz_add_system(buoyancy-engine
+ SOURCES
+ BuoyancyEngine.cc
+ PUBLIC_LINK_LIBS
+ ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER}
+ ignition-transport${IGN_TRANSPORT_VER}::ignition-transport${IGN_TRANSPORT_VER}
+)
diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt
index 9753c3ef2c..d4fb16a10a 100644
--- a/test/integration/CMakeLists.txt
+++ b/test/integration/CMakeLists.txt
@@ -8,6 +8,7 @@ set(tests
battery_plugin.cc
breadcrumbs.cc
buoyancy.cc
+ buoyancy_engine.cc
collada_world_exporter.cc
components.cc
contact_system.cc
diff --git a/test/integration/buoyancy_engine.cc b/test/integration/buoyancy_engine.cc
new file mode 100644
index 0000000000..822666d22c
--- /dev/null
+++ b/test/integration/buoyancy_engine.cc
@@ -0,0 +1,152 @@
+/*
+ * 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.
+ *
+ */
+
+#include
+
+#include
+#include
+#include
+#include
+
+#include "ignition/gazebo/Server.hh"
+#include "ignition/gazebo/SystemLoader.hh"
+#include "ignition/gazebo/components/Name.hh"
+#include "ignition/gazebo/components/Pose.hh"
+#include "ignition/gazebo/components/Model.hh"
+
+#include "ignition/gazebo/test_config.hh"
+#include "../helpers/Relay.hh"
+#include "../helpers/EnvTestFixture.hh"
+
+using namespace ignition;
+using namespace gazebo;
+
+class BuoyancyEngineTest : public InternalFixture<::testing::Test>
+{
+ // Documentation inherited
+ protected: void SetUp() override
+ {
+ InternalFixture::SetUp();
+ this->pub = this->node.Advertise(
+ "/model/buoyant_box/buoyancy_engine/");
+ }
+
+ /// \brief Node for communication
+ public: ignition::transport::Node node;
+
+ /// \brief Publishes commands
+ public: ignition::transport::Node::Publisher pub;
+};
+
+/////////////////////////////////////////////////
+TEST_F(BuoyancyEngineTest, TestDownward)
+{
+ ServerConfig serverConfig;
+ const auto sdfFile = common::joinPaths(std::string(PROJECT_SOURCE_PATH),
+ "test", "worlds", "buoyancy_engine.sdf");
+ serverConfig.SetSdfFile(sdfFile);
+
+ Server server(serverConfig);
+ EXPECT_FALSE(server.Running());
+ EXPECT_FALSE(*server.Running(0));
+
+ using namespace std::chrono_literals;
+ server.SetUpdatePeriod(1ns);
+
+ std::size_t iterations = 10000;
+
+ test::Relay testSystem;
+ std::vector poses;
+
+ testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &/*_info*/,
+ const gazebo::EntityComponentManager &_ecm)
+ {
+ // Check pose
+ Entity buoyantBox = _ecm.EntityByComponents(
+ components::Model(), components::Name("buoyant_box"));
+ EXPECT_NE(kNullEntity, buoyantBox);
+
+ auto submarineBuoyantPose = _ecm.Component(buoyantBox);
+ EXPECT_NE(submarineBuoyantPose, nullptr);
+ if (submarineBuoyantPose == nullptr)
+ {
+ ignerr << "Unable to get pose" <Data());
+ });
+
+ server.AddSystem(testSystem.systemPtr);
+ ignition::msgs::Double volume;
+ volume.set_data(0);
+ this->pub.Publish(volume);
+ server.Run(true, iterations, false);
+
+ EXPECT_LT(poses.rbegin()->Pos().Z(), poses.begin()->Pos().Z());
+ EXPECT_NEAR(poses.rbegin()->Pos().X(), poses.begin()->Pos().X(), 1e-3);
+ EXPECT_NEAR(poses.rbegin()->Pos().Y(), poses.begin()->Pos().Y(), 1e-3);
+}
+
+/////////////////////////////////////////////////
+TEST_F(BuoyancyEngineTest, TestUpward)
+{
+ ServerConfig serverConfig;
+ const auto sdfFile = std::string(PROJECT_SOURCE_PATH) +
+ "/test/worlds/buoyancy_engine.sdf";
+ serverConfig.SetSdfFile(sdfFile);
+
+ Server server(serverConfig);
+ EXPECT_FALSE(server.Running());
+ EXPECT_FALSE(*server.Running(0));
+
+ using namespace std::chrono_literals;
+ server.SetUpdatePeriod(1ns);
+
+ std::size_t iterations = 10000;
+
+ test::Relay testSystem;
+ std::vector poses;
+
+ testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &/*_info*/,
+ const gazebo::EntityComponentManager &_ecm)
+ {
+ // Check pose
+ Entity buoyantBox = _ecm.EntityByComponents(
+ components::Model(), components::Name("buoyant_box"));
+ EXPECT_NE(kNullEntity, buoyantBox);
+
+ auto submarineBuoyantPose = _ecm.Component(buoyantBox);
+ EXPECT_NE(submarineBuoyantPose, nullptr);
+ if (submarineBuoyantPose == nullptr)
+ {
+ ignerr << "Unable to get pose" <Data());
+ });
+
+ server.AddSystem(testSystem.systemPtr);
+ ignition::msgs::Double volume;
+ volume.set_data(10);
+ this->pub.Publish(volume);
+ server.Run(true, iterations, false);
+
+ EXPECT_GT(poses.rbegin()->Pos().Z(), poses.begin()->Pos().Z());
+ EXPECT_NEAR(poses.rbegin()->Pos().X(), poses.begin()->Pos().X(), 1e-3);
+ EXPECT_NEAR(poses.rbegin()->Pos().Y(), poses.begin()->Pos().Y(), 1e-3);
+}
+
diff --git a/test/worlds/buoyancy_engine.sdf b/test/worlds/buoyancy_engine.sdf
new file mode 100644
index 0000000000..51ce5897ba
--- /dev/null
+++ b/test/worlds/buoyancy_engine.sdf
@@ -0,0 +1,72 @@
+
+
+
+
+ 0.001
+ 1.0
+
+
+
+
+ 1000
+
+
+
+ true
+ 0 0 10 0 0 0
+ 1 1 1 1
+ 0.5 0.5 0.5 1
+
+ 1000
+ 0.9
+ 0.01
+ 0.001
+
+ -0.5 0.1 -0.9
+
+
+
+ 0 0 0 0 0 0
+
+
+ 1000
+
+ 133.3333
+ 133.3333
+ 133.3333
+
+
+
+
+
+ 1 1 1
+
+
+
+
+
+
+ 1 1 1
+
+
+
+
+
+ body
+ buoyant_box
+ 0.001
+ 0.002
+ 0.002
+ 0.003
+ 0.0003
+
+
+
+
+