From 2a076e91c6df7699284ab93e244894bb0f55336f Mon Sep 17 00:00:00 2001
From: Louise Poubel <louise@openrobotics.org>
Date: Wed, 22 Dec 2021 10:46:14 -0800
Subject: [PATCH 01/17] =?UTF-8?q?=F0=9F=8E=88=204.14.0=20(#1261)?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Signed-off-by: Louise Poubel <louise@openrobotics.org>
---
 CMakeLists.txt |  2 +-
 Changelog.md   | 21 ++++++++++++++++++---
 2 files changed, 19 insertions(+), 4 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 9c10002405..5546ef954b 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -3,7 +3,7 @@ cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR)
 #============================================================================
 # Initialize the project
 #============================================================================
-project(ignition-gazebo4 VERSION 4.13.0)
+project(ignition-gazebo4 VERSION 4.14.0)
 
 #============================================================================
 # Find ignition-cmake
diff --git a/Changelog.md b/Changelog.md
index 881e09fb96..7804bff869 100644
--- a/Changelog.md
+++ b/Changelog.md
@@ -1,5 +1,20 @@
 ## Ignition Gazebo 4.x
 
+### Ignition Gazebo 4.14.0 (2021-12-20)
+
+1. Support battery draining start via topics
+    * [Pull request #1255](https://github.com/ignitionrobotics/ign-gazebo/pull/1255)
+
+1. Make tests run as fast as possible
+    * [Pull request #1194](https://github.com/ignitionrobotics/ign-gazebo/pull/1194)
+    * [Pull request #1250](https://github.com/ignitionrobotics/ign-gazebo/pull/1250)
+
+1. Fix visualize lidar
+    * [Pull request #1224](https://github.com/ignitionrobotics/ign-gazebo/pull/1224)
+
+1. Disable user commands light test on macOS
+    * [Pull request #1204](https://github.com/ignitionrobotics/ign-gazebo/pull/1204)
+
 ### Ignition Gazebo 4.13.0 (2021-11-15)
 
 1. Prevent creation of spurious `<plugin>` elements when saving worlds
@@ -202,7 +217,7 @@
 1. Support configuring particle scatter ratio in particle emitter system.
     * [Pull Request 674](https://github.com/ignitionrobotics/ign-gazebo/pull/674)
 
-1. Fix diffuse and ambient values for ackermann example. 
+1. Fix diffuse and ambient values for ackermann example.
     * [Pull Request 707](https://github.com/ignitionrobotics/ign-gazebo/pull/707)
 
 1. Scenebroadcaster sensors.
@@ -223,7 +238,7 @@
 1. Ackermann Steering Plugin.
     * [Pull Request 618](https://github.com/ignitionrobotics/ign-gazebo/pull/618)
 
-1. Remove bounding box when model is deleted 
+1. Remove bounding box when model is deleted
     * [Pull Request 675](https://github.com/ignitionrobotics/ign-gazebo/pull/675)
 
 1. Cache link poses to improve performance.
@@ -232,7 +247,7 @@
 1. Check empty world name in Scene3d.
     * [Pull Request 662](https://github.com/ignitionrobotics/ign-gazebo/pull/662)
 
-1. All changes up to 3.8.0. 
+1. All changes up to 3.8.0.
 
 ### Ignition Gazebo 4.6.0 (2021-03-01)
 

From 6f58c919a95e8ab03864d25af022599e64bc090f Mon Sep 17 00:00:00 2001
From: Louise Poubel <louise@openrobotics.org>
Date: Tue, 18 Jan 2022 08:57:19 -0800
Subject: [PATCH 02/17] =?UTF-8?q?Prevent=20GzScene3D=20=F0=9F=92=A5=20if?=
 =?UTF-8?q?=20another=20scene=20is=20already=20loaded=20(#1294)?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Signed-off-by: Louise Poubel <louise@openrobotics.org>
---
 src/gui/plugins/scene3d/GzScene3D.qml | 19 ++++--
 src/gui/plugins/scene3d/Scene3D.cc    | 87 +++++++++++++++++----------
 src/gui/plugins/scene3d/Scene3D.hh    | 44 +++++++++++++-
 3 files changed, 112 insertions(+), 38 deletions(-)

diff --git a/src/gui/plugins/scene3d/GzScene3D.qml b/src/gui/plugins/scene3d/GzScene3D.qml
index 4411fb5d7e..6dea55f64e 100644
--- a/src/gui/plugins/scene3d/GzScene3D.qml
+++ b/src/gui/plugins/scene3d/GzScene3D.qml
@@ -14,16 +14,18 @@
  * limitations under the License.
  *
 */
+import IgnGazebo 1.0 as IgnGazebo
+import QtGraphicalEffects 1.0
 import QtQuick 2.9
 import QtQuick.Controls 2.2
 import QtQuick.Dialogs 1.0
+import QtQuick.Layouts 1.3
 import RenderWindow 1.0
-import QtGraphicalEffects 1.0
-import IgnGazebo 1.0 as IgnGazebo
 
 Rectangle {
-  width: 1000
-  height: 800
+  Layout.minimumWidth: 200
+  Layout.minimumHeight: 200
+  anchors.fill: parent
 
   /**
    * True to enable gamma correction
@@ -38,6 +40,7 @@ Rectangle {
     anchors.fill: parent
     hoverEnabled: true
     acceptedButtons: Qt.NoButton
+    visible: GzScene3D.loadingError.length == 0
     onEntered: {
       GzScene3D.OnFocusWindow()
     }
@@ -50,6 +53,7 @@ Rectangle {
     id: renderWindow
     objectName: "renderWindow"
     anchors.fill: parent
+    visible: GzScene3D.loadingError.length == 0
 
     /**
      * Message to be displayed over the render window
@@ -120,4 +124,11 @@ Rectangle {
     standardButtons: Dialog.Ok
   }
 
+  Label {
+    anchors.fill: parent
+    anchors.margins: 10
+    text: GzScene3D.loadingError
+    visible: (GzScene3D.loadingError.length > 0);
+    wrapMode: Text.WordWrap
+  }
 }
diff --git a/src/gui/plugins/scene3d/Scene3D.cc b/src/gui/plugins/scene3d/Scene3D.cc
index 09ae8c5919..429294f7d4 100644
--- a/src/gui/plugins/scene3d/Scene3D.cc
+++ b/src/gui/plugins/scene3d/Scene3D.cc
@@ -1643,17 +1643,25 @@ void IgnRenderer::HandleMouseViewControl()
 }
 
 /////////////////////////////////////////////////
-void IgnRenderer::Initialize()
+std::string IgnRenderer::Initialize()
 {
   if (this->initialized)
-    return;
+    return std::string();
+
+  // Only one engine / scene / user camera is currently supported.
+  // Fail gracefully even before getting to renderUtil.
+  if (!rendering::loadedEngines().empty())
+  {
+    return "Currently only one plugin providing a 3D scene is supported at a "
+            "time.";
+  }
 
   this->dataPtr->renderUtil.SetUseCurrentGLContext(true);
   this->dataPtr->renderUtil.Init();
 
   rendering::ScenePtr scene = this->dataPtr->renderUtil.Scene();
   if (!scene)
-    return;
+    return "Failed to create a 3D scene.";
 
   auto root = scene->RootVisual();
 
@@ -1674,6 +1682,7 @@ void IgnRenderer::Initialize()
   this->dataPtr->rayQuery = this->dataPtr->camera->Scene()->CreateRayQuery();
 
   this->initialized = true;
+  return std::string();
 }
 
 /////////////////////////////////////////////////
@@ -2066,6 +2075,12 @@ RenderThread::RenderThread()
   qRegisterMetaType<std::string>();
 }
 
+/////////////////////////////////////////////////
+void RenderThread::SetErrorCb(std::function<void(const QString&)> _cb)
+{
+  this->errorCb = _cb;
+}
+
 /////////////////////////////////////////////////
 void RenderThread::RenderNext()
 {
@@ -2074,7 +2089,12 @@ void RenderThread::RenderNext()
   if (!this->ignRenderer.initialized)
   {
     // Initialize renderer
-    this->ignRenderer.Initialize();
+    auto loadingError = this->ignRenderer.Initialize();
+    if (!loadingError.empty())
+    {
+      this->errorCb(QString::fromStdString(loadingError));
+      return;
+    }
   }
 
   // check if engine has been successfully initialized
@@ -2092,18 +2112,24 @@ void RenderThread::RenderNext()
 /////////////////////////////////////////////////
 void RenderThread::ShutDown()
 {
-  this->context->makeCurrent(this->surface);
+  if (this->context && this->surface)
+    this->context->makeCurrent(this->surface);
 
   this->ignRenderer.Destroy();
 
-  this->context->doneCurrent();
-  delete this->context;
+  if (this->context)
+  {
+    this->context->doneCurrent();
+    delete this->context;
+  }
 
   // schedule this to be deleted only after we're done cleaning up
-  this->surface->deleteLater();
+  if (this->surface)
+    this->surface->deleteLater();
 
   // Stop event processing, move the thread to GUI and make sure it is deleted.
-  this->moveToThread(QGuiApplication::instance()->thread());
+  if (this->ignRenderer.initialized)
+    this->moveToThread(QGuiApplication::instance()->thread());
 }
 
 
@@ -2203,16 +2229,6 @@ void TextureNode::PrepareNode()
 RenderWindowItem::RenderWindowItem(QQuickItem *_parent)
   : QQuickItem(_parent), dataPtr(new RenderWindowItemPrivate)
 {
-  // FIXME(anyone) Ogre 1/2 singletons crash when there's an attempt to load
-  // this plugin twice, so shortcut here. Ideally this would be caught at
-  // Ignition Rendering.
-  static bool done{false};
-  if (done)
-  {
-    return;
-  }
-  done = true;
-
   this->setAcceptedMouseButtons(Qt::AllButtons);
   this->setFlag(ItemHasContents);
   this->dataPtr->renderThread = new RenderThread();
@@ -2378,18 +2394,6 @@ Scene3D::~Scene3D() = default;
 /////////////////////////////////////////////////
 void Scene3D::LoadConfig(const tinyxml2::XMLElement *_pluginElem)
 {
-  // FIXME(anyone) Ogre 1/2 singletons crash when there's an attempt to load
-  // this plugin twice, so shortcut here. Ideally this would be caught at
-  // Ignition Rendering.
-  static bool done{false};
-  if (done)
-  {
-    ignerr << "Only one Scene3D is supported per process at the moment."
-           << std::endl;
-    return;
-  }
-  done = true;
-
   auto renderWindow = this->PluginItem()->findChild<RenderWindowItem *>();
   if (!renderWindow)
   {
@@ -2397,6 +2401,8 @@ void Scene3D::LoadConfig(const tinyxml2::XMLElement *_pluginElem)
            << "Render window will not be created" << std::endl;
     return;
   }
+  renderWindow->SetErrorCb(std::bind(&Scene3D::SetLoadingError, this,
+      std::placeholders::_1));
 
   if (this->title.empty())
     this->title = "3D Scene";
@@ -2891,6 +2897,19 @@ void Scene3D::OnFocusWindow()
   renderWindow->forceActiveFocus();
 }
 
+/////////////////////////////////////////////////
+QString Scene3D::LoadingError() const
+{
+  return this->loadingError;
+}
+
+/////////////////////////////////////////////////
+void Scene3D::SetLoadingError(const QString &_loadingError)
+{
+  this->loadingError = _loadingError;
+  this->LoadingErrorChanged();
+}
+
 /////////////////////////////////////////////////
 void RenderWindowItem::SetXYZSnap(const math::Vector3d &_xyz)
 {
@@ -3180,6 +3199,12 @@ void RenderWindowItem::OnHovered(const ignition::math::Vector2i &_hoverPos)
   this->dataPtr->renderThread->ignRenderer.NewHoverEvent(_hoverPos);
 }
 
+/////////////////////////////////////////////////
+void RenderWindowItem::SetErrorCb(std::function<void(const QString&)> _cb)
+{
+  this->dataPtr->renderThread->SetErrorCb(_cb);
+}
+
 /////////////////////////////////////////////////
 void RenderWindowItem::mousePressEvent(QMouseEvent *_e)
 {
diff --git a/src/gui/plugins/scene3d/Scene3D.hh b/src/gui/plugins/scene3d/Scene3D.hh
index 1f743a4323..37b95f5db7 100644
--- a/src/gui/plugins/scene3d/Scene3D.hh
+++ b/src/gui/plugins/scene3d/Scene3D.hh
@@ -56,10 +56,13 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
   class Scene3DPrivate;
   class RenderUtil;
 
-  /// \brief Creates a new ignition rendering scene or adds a user-camera to an
-  /// existing scene. It is possible to orbit the camera around the scene with
+  /// \brief Creates an ignition rendering scene and user camera.
+  /// It is possible to orbit the camera around the scene with
   /// the mouse. Use other plugins to manage objects in the scene.
   ///
+  /// Only one plugin displaying an Ignition Rendering scene can be used at a
+  /// time.
+  ///
   /// ## Configuration
   ///
   /// * \<engine\> : Optional render engine name, defaults to 'ogre'.
@@ -88,6 +91,14 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
       NOTIFY ErrorPopupTextChanged
     )
 
+    /// \brief Loading error message
+    Q_PROPERTY(
+      QString loadingError
+      READ LoadingError
+      WRITE SetLoadingError
+      NOTIFY LoadingErrorChanged
+    )
+
     /// \brief Constructor
     public: Scene3D();
 
@@ -185,6 +196,20 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
     /// the connection to work on the QML side
     signals: void popupError();
 
+    /// \brief Get the loading error string.
+    /// \return String explaining the loading error. If empty, there's no error.
+    public: Q_INVOKABLE QString LoadingError() const;
+
+    /// \brief Set the loading error message.
+    /// \param[in] _loadingError Error message.
+    public: Q_INVOKABLE void SetLoadingError(const QString &_loadingError);
+
+    /// \brief Notify that loading error has changed
+    signals: void LoadingErrorChanged();
+
+    /// \brief Loading error message
+    public: QString loadingError;
+
     /// \internal
     /// \brief Pointer to private data.
     private: std::unique_ptr<Scene3DPrivate> dataPtr;
@@ -210,7 +235,9 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
     public: void Render();
 
     /// \brief Initialize the render engine
-    public: void Initialize();
+    /// \return Error message if initialization failed. If empty, no errors
+    /// occurred.
+    public: std::string Initialize();
 
     /// \brief Destroy camera associated with this renderer
     public: void Destroy();
@@ -519,6 +546,13 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
     /// \param[in] _size Size of the texture
     signals: void TextureReady(int _id, const QSize &_size);
 
+    /// \brief Set a callback to be called in case there are errors.
+    /// \param[in] _cb Error callback
+    public: void SetErrorCb(std::function<void(const QString &)> _cb);
+
+    /// \brief Function to be called if there are errors.
+    public: std::function<void(const QString &)> errorCb;
+
     /// \brief Offscreen surface to render to
     public: QOffscreenSurface *surface = nullptr;
 
@@ -726,6 +760,10 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
     /// \param[in] _entity Scoped name of entity.
     public slots: void OnContextMenuRequested(QString _entity);
 
+    /// \brief Set a callback to be called in case there are errors.
+    /// \param[in] _cb Error callback
+    public: void SetErrorCb(std::function<void(const QString &)> _cb);
+
     /// \internal
     /// \brief Pointer to private data.
     private: std::unique_ptr<RenderWindowItemPrivate> dataPtr;

From 650b74658d0b8713de169b25a2e24c6d9c3e7ec3 Mon Sep 17 00:00:00 2001
From: Louise Poubel <louise@openrobotics.org>
Date: Tue, 18 Jan 2022 08:59:02 -0800
Subject: [PATCH 03/17] Remove EachNew calls from sensor PreUpdates (#1281)

Signed-off-by: Louise Poubel <louise@openrobotics.org>
---
 Migration.md                                |  4 ++
 src/systems/air_pressure/AirPressure.cc     | 46 ++++++++++++------
 src/systems/altimeter/Altimeter.cc          | 46 ++++++++++++------
 src/systems/imu/Imu.cc                      | 52 ++++++++++++++-------
 src/systems/logical_camera/LogicalCamera.cc | 45 ++++++++++++------
 src/systems/magnetometer/Magnetometer.cc    | 47 +++++++++++++------
 test/integration/air_pressure_system.cc     |  6 ++-
 test/integration/altimeter_system.cc        |  6 ++-
 test/integration/imu_system.cc              |  6 ++-
 test/integration/logical_camera_system.cc   | 30 +++++++-----
 test/integration/magnetometer_system.cc     |  6 ++-
 11 files changed, 211 insertions(+), 83 deletions(-)

diff --git a/Migration.md b/Migration.md
index 012dbafdb8..52e498a41b 100644
--- a/Migration.md
+++ b/Migration.md
@@ -5,6 +5,10 @@ Deprecated code produces compile-time warnings. These warning serve as
 notification to users that their code should be upgraded. The next major
 release will remove the deprecated code.
 
+## Ignition Gazebo 3.12.0 to 3.X.X
+
+* Some sensors will only have the `SensorTopic` component after the 1st iteration.
+
 ## Ignition Gazebo 2.x to 3.x
 
 * Use ign-rendering3, ign-sensors3 and ign-gui3.
diff --git a/src/systems/air_pressure/AirPressure.cc b/src/systems/air_pressure/AirPressure.cc
index cff6ea8858..4346cf1f1b 100644
--- a/src/systems/air_pressure/AirPressure.cc
+++ b/src/systems/air_pressure/AirPressure.cc
@@ -21,6 +21,7 @@
 
 #include <string>
 #include <unordered_map>
+#include <unordered_set>
 #include <utility>
 
 #include <ignition/plugin/Register.hh>
@@ -49,30 +50,35 @@ using namespace systems;
 /// \brief Private AirPressure data class.
 class ignition::gazebo::systems::AirPressurePrivate
 {
-  /// \brief A map of air pressure entity to its vertical reference
+  /// \brief A map of air pressure entity to its sensor
   public: std::unordered_map<Entity,
       std::unique_ptr<sensors::AirPressureSensor>> entitySensorMap;
 
   /// \brief Ign-sensors sensor factory for creating sensors
   public: sensors::SensorFactory sensorFactory;
 
+  /// \brief Keep list of sensors that were created during the previous
+  /// `PostUpdate`, so that components can be created during the next
+  /// `PreUpdate`.
+  public: std::unordered_set<Entity> newSensors;
+
   /// True if the rendering component is initialized
   public: bool initialized = false;
 
   /// \brief Create sensor
-  /// \param[in] _ecm Mutable reference to ECM.
+  /// \param[in] _ecm Immutable reference to ECM.
   /// \param[in] _entity Entity of the IMU
   /// \param[in] _airPressure AirPressureSensor component.
   /// \param[in] _parent Parent entity component.
   public: void AddAirPressure(
-    EntityComponentManager &_ecm,
+    const EntityComponentManager &_ecm,
     const Entity _entity,
     const components::AirPressureSensor *_airPressure,
     const components::ParentEntity *_parent);
 
   /// \brief Create air pressure sensor
-  /// \param[in] _ecm Mutable reference to ECM.
-  public: void CreateAirPressureEntities(EntityComponentManager &_ecm);
+  /// \param[in] _ecm Immutable reference to ECM.
+  public: void CreateSensors(const EntityComponentManager &_ecm);
 
   /// \brief Update air pressure sensor data based on physics data
   /// \param[in] _ecm Immutable reference to ECM.
@@ -98,7 +104,21 @@ void AirPressure::PreUpdate(const UpdateInfo &/*_info*/,
     EntityComponentManager &_ecm)
 {
   IGN_PROFILE("AirPressure::PreUpdate");
-  this->dataPtr->CreateAirPressureEntities(_ecm);
+
+  // Create components
+  for (auto entity : this->dataPtr->newSensors)
+  {
+    auto it = this->dataPtr->entitySensorMap.find(entity);
+    if (it == this->dataPtr->entitySensorMap.end())
+    {
+      ignerr << "Entity [" << entity
+             << "] isn't in sensor map, this shouldn't happen." << std::endl;
+      continue;
+    }
+    // Set topic
+    _ecm.CreateComponent(entity, components::SensorTopic(it->second->Topic()));
+  }
+  this->dataPtr->newSensors.clear();
 }
 
 //////////////////////////////////////////////////
@@ -116,6 +136,8 @@ void AirPressure::PostUpdate(const UpdateInfo &_info,
         << "s]. System may not work properly." << std::endl;
   }
 
+  this->dataPtr->CreateSensors(_ecm);
+
   if (!_info.paused)
   {
     this->dataPtr->UpdateAirPressures(_ecm);
@@ -134,7 +156,7 @@ void AirPressure::PostUpdate(const UpdateInfo &_info,
 
 //////////////////////////////////////////////////
 void AirPressurePrivate::AddAirPressure(
-  EntityComponentManager &_ecm,
+  const EntityComponentManager &_ecm,
   const Entity _entity,
   const components::AirPressureSensor *_airPressure,
   const components::ParentEntity *_parent)
@@ -171,15 +193,13 @@ void AirPressurePrivate::AddAirPressure(
   math::Pose3d sensorWorldPose = worldPose(_entity, _ecm);
   sensor->SetPose(sensorWorldPose);
 
-  // Set topic
-  _ecm.CreateComponent(_entity, components::SensorTopic(sensor->Topic()));
-
   this->entitySensorMap.insert(
       std::make_pair(_entity, std::move(sensor)));
+  this->newSensors.insert(_entity);
 }
 
 //////////////////////////////////////////////////
-void AirPressurePrivate::CreateAirPressureEntities(EntityComponentManager &_ecm)
+void AirPressurePrivate::CreateSensors(const EntityComponentManager &_ecm)
 {
   IGN_PROFILE("AirPressurePrivate::CreateAirPressureEntities");
   if (!this->initialized)
@@ -190,7 +210,7 @@ void AirPressurePrivate::CreateAirPressureEntities(EntityComponentManager &_ecm)
           const components::AirPressureSensor *_airPressure,
           const components::ParentEntity *_parent)->bool
         {
-          AddAirPressure(_ecm, _entity, _airPressure, _parent);
+          this->AddAirPressure(_ecm, _entity, _airPressure, _parent);
           return true;
         });
     this->initialized = true;
@@ -203,7 +223,7 @@ void AirPressurePrivate::CreateAirPressureEntities(EntityComponentManager &_ecm)
           const components::AirPressureSensor *_airPressure,
           const components::ParentEntity *_parent)->bool
         {
-          AddAirPressure(_ecm, _entity, _airPressure, _parent);
+          this->AddAirPressure(_ecm, _entity, _airPressure, _parent);
           return true;
         });
   }
diff --git a/src/systems/altimeter/Altimeter.cc b/src/systems/altimeter/Altimeter.cc
index 4cc12f12bc..1b34fc63c1 100644
--- a/src/systems/altimeter/Altimeter.cc
+++ b/src/systems/altimeter/Altimeter.cc
@@ -21,6 +21,7 @@
 
 #include <string>
 #include <unordered_map>
+#include <unordered_set>
 #include <utility>
 
 #include <ignition/common/Profiler.hh>
@@ -51,30 +52,35 @@ using namespace systems;
 /// \brief Private Altimeter data class.
 class ignition::gazebo::systems::AltimeterPrivate
 {
-  /// \brief A map of altimeter entity to its vertical reference
+  /// \brief A map of altimeter entity to its sensor
   public: std::unordered_map<Entity,
       std::unique_ptr<sensors::AltimeterSensor>> entitySensorMap;
 
   /// \brief Ign-sensors sensor factory for creating sensors
   public: sensors::SensorFactory sensorFactory;
 
+  /// \brief Keep list of sensors that were created during the previous
+  /// `PostUpdate`, so that components can be created during the next
+  /// `PreUpdate`.
+  public: std::unordered_set<Entity> newSensors;
+
   /// True if the rendering component is initialized
   public: bool initialized = false;
 
   /// \brief Create sensor
-  /// \param[in] _ecm Mutable reference to ECM.
+  /// \param[in] _ecm Immutable reference to ECM.
   /// \param[in] _entity Entity of the IMU
   /// \param[in] _altimeter Altimeter component.
   /// \param[in] _parent Parent entity component.
   public: void AddAltimeter(
-    EntityComponentManager &_ecm,
+    const EntityComponentManager &_ecm,
     const Entity _entity,
     const components::Altimeter *_altimeter,
     const components::ParentEntity *_parent);
 
   /// \brief Create altimeter sensor
-  /// \param[in] _ecm Mutable reference to ECM.
-  public: void CreateAltimeterEntities(EntityComponentManager &_ecm);
+  /// \param[in] _ecm Immutable reference to ECM.
+  public: void CreateSensors(const EntityComponentManager &_ecm);
 
   /// \brief Update altimeter sensor data based on physics data
   /// \param[in] _ecm Immutable reference to ECM.
@@ -99,7 +105,21 @@ void Altimeter::PreUpdate(const UpdateInfo &/*_info*/,
     EntityComponentManager &_ecm)
 {
   IGN_PROFILE("Altimeter::PreUpdate");
-  this->dataPtr->CreateAltimeterEntities(_ecm);
+
+  // Create components
+  for (auto entity : this->dataPtr->newSensors)
+  {
+    auto it = this->dataPtr->entitySensorMap.find(entity);
+    if (it == this->dataPtr->entitySensorMap.end())
+    {
+      ignerr << "Entity [" << entity
+             << "] isn't in sensor map, this shouldn't happen." << std::endl;
+      continue;
+    }
+    // Set topic
+    _ecm.CreateComponent(entity, components::SensorTopic(it->second->Topic()));
+  }
+  this->dataPtr->newSensors.clear();
 }
 
 //////////////////////////////////////////////////
@@ -116,6 +136,8 @@ void Altimeter::PostUpdate(const UpdateInfo &_info,
         << "s]. System may not work properly." << std::endl;
   }
 
+  this->dataPtr->CreateSensors(_ecm);
+
   // Only update and publish if not paused.
   if (!_info.paused)
   {
@@ -135,7 +157,7 @@ void Altimeter::PostUpdate(const UpdateInfo &_info,
 
 //////////////////////////////////////////////////
 void AltimeterPrivate::AddAltimeter(
-  EntityComponentManager &_ecm,
+  const EntityComponentManager &_ecm,
   const Entity _entity,
   const components::Altimeter *_altimeter,
   const components::ParentEntity *_parent)
@@ -173,15 +195,13 @@ void AltimeterPrivate::AddAltimeter(
   sensor->SetVerticalReference(verticalReference);
   sensor->SetPosition(verticalReference);
 
-  // Set topic
-  _ecm.CreateComponent(_entity, components::SensorTopic(sensor->Topic()));
-
   this->entitySensorMap.insert(
       std::make_pair(_entity, std::move(sensor)));
+  this->newSensors.insert(_entity);
 }
 
 //////////////////////////////////////////////////
-void AltimeterPrivate::CreateAltimeterEntities(EntityComponentManager &_ecm)
+void AltimeterPrivate::CreateSensors(const EntityComponentManager &_ecm)
 {
   IGN_PROFILE("Altimeter::CreateAltimeterEntities");
   if (!this->initialized)
@@ -192,7 +212,7 @@ void AltimeterPrivate::CreateAltimeterEntities(EntityComponentManager &_ecm)
           const components::Altimeter *_altimeter,
           const components::ParentEntity *_parent)->bool
         {
-          AddAltimeter(_ecm, _entity, _altimeter, _parent);
+          this->AddAltimeter(_ecm, _entity, _altimeter, _parent);
           return true;
         });
     this->initialized = true;
@@ -205,7 +225,7 @@ void AltimeterPrivate::CreateAltimeterEntities(EntityComponentManager &_ecm)
           const components::Altimeter *_altimeter,
           const components::ParentEntity *_parent)->bool
         {
-          AddAltimeter(_ecm, _entity, _altimeter, _parent);
+          this->AddAltimeter(_ecm, _entity, _altimeter, _parent);
           return true;
         });
   }
diff --git a/src/systems/imu/Imu.cc b/src/systems/imu/Imu.cc
index d883f6cc91..a6a3ecf7d9 100644
--- a/src/systems/imu/Imu.cc
+++ b/src/systems/imu/Imu.cc
@@ -17,9 +17,10 @@
 
 #include "Imu.hh"
 
+#include <string>
 #include <unordered_map>
+#include <unordered_set>
 #include <utility>
-#include <string>
 
 #include <ignition/plugin/Register.hh>
 
@@ -56,6 +57,11 @@ class ignition::gazebo::systems::ImuPrivate
   /// \brief Ign-sensors sensor factory for creating sensors
   public: sensors::SensorFactory sensorFactory;
 
+  /// \brief Keep list of sensors that were created during the previous
+  /// `PostUpdate`, so that components can be created during the next
+  /// `PreUpdate`.
+  public: std::unordered_set<Entity> newSensors;
+
   /// \brief Keep track of world ID, which is equivalent to the scene's
   /// root visual.
   /// Defaults to zero, which is considered invalid by Ignition Gazebo.
@@ -64,21 +70,21 @@ class ignition::gazebo::systems::ImuPrivate
   /// True if the rendering component is initialized
   public: bool initialized = false;
 
-  /// \brief Create IMU sensor
-  /// \param[in] _ecm Mutable reference to ECM.
-  public: void CreateImuEntities(EntityComponentManager &_ecm);
+  /// \brief Create IMU sensors in ign-sensors
+  /// \param[in] _ecm Immutable reference to ECM.
+  public: void CreateSensors(const EntityComponentManager &_ecm);
 
   /// \brief Update IMU sensor data based on physics data
   /// \param[in] _ecm Immutable reference to ECM.
   public: void Update(const EntityComponentManager &_ecm);
 
   /// \brief Create sensor
-  /// \param[in] _ecm Mutable reference to ECM.
+  /// \param[in] _ecm Immutable reference to ECM.
   /// \param[in] _entity Entity of the IMU
   /// \param[in] _imu IMU component.
   /// \param[in] _parent Parent entity component.
-  public: void addIMU(
-    EntityComponentManager &_ecm,
+  public: void AddSensor(
+    const EntityComponentManager &_ecm,
     const Entity _entity,
     const components::Imu *_imu,
     const components::ParentEntity *_parent);
@@ -102,7 +108,21 @@ void Imu::PreUpdate(const UpdateInfo &/*_info*/,
     EntityComponentManager &_ecm)
 {
   IGN_PROFILE("Imu::PreUpdate");
-  this->dataPtr->CreateImuEntities(_ecm);
+
+  // Create components
+  for (auto entity : this->dataPtr->newSensors)
+  {
+    auto it = this->dataPtr->entitySensorMap.find(entity);
+    if (it == this->dataPtr->entitySensorMap.end())
+    {
+      ignerr << "Entity [" << entity
+             << "] isn't in sensor map, this shouldn't happen." << std::endl;
+      continue;
+    }
+    // Set topic
+    _ecm.CreateComponent(entity, components::SensorTopic(it->second->Topic()));
+  }
+  this->dataPtr->newSensors.clear();
 }
 
 //////////////////////////////////////////////////
@@ -119,6 +139,8 @@ void Imu::PostUpdate(const UpdateInfo &_info,
         << "s]. System may not work properly." << std::endl;
   }
 
+  this->dataPtr->CreateSensors(_ecm);
+
   // Only update and publish if not paused.
   if (!_info.paused)
   {
@@ -137,8 +159,8 @@ void Imu::PostUpdate(const UpdateInfo &_info,
 }
 
 //////////////////////////////////////////////////
-void ImuPrivate::addIMU(
-  EntityComponentManager &_ecm,
+void ImuPrivate::AddSensor(
+  const EntityComponentManager &_ecm,
   const Entity _entity,
   const components::Imu *_imu,
   const components::ParentEntity *_parent)
@@ -186,15 +208,13 @@ void ImuPrivate::addIMU(
   math::Pose3d p = worldPose(_entity, _ecm);
   sensor->SetOrientationReference(p.Rot());
 
-  // Set topic
-  _ecm.CreateComponent(_entity, components::SensorTopic(sensor->Topic()));
-
   this->entitySensorMap.insert(
       std::make_pair(_entity, std::move(sensor)));
+  this->newSensors.insert(_entity);
 }
 
 //////////////////////////////////////////////////
-void ImuPrivate::CreateImuEntities(EntityComponentManager &_ecm)
+void ImuPrivate::CreateSensors(const EntityComponentManager &_ecm)
 {
   IGN_PROFILE("ImuPrivate::CreateImuEntities");
   // Get World Entity
@@ -214,7 +234,7 @@ void ImuPrivate::CreateImuEntities(EntityComponentManager &_ecm)
           const components::Imu *_imu,
           const components::ParentEntity *_parent)->bool
         {
-          addIMU(_ecm, _entity, _imu, _parent);
+          this->AddSensor(_ecm, _entity, _imu, _parent);
           return true;
         });
       this->initialized = true;
@@ -227,7 +247,7 @@ void ImuPrivate::CreateImuEntities(EntityComponentManager &_ecm)
           const components::Imu *_imu,
           const components::ParentEntity *_parent)->bool
         {
-          addIMU(_ecm, _entity, _imu, _parent);
+          this->AddSensor(_ecm, _entity, _imu, _parent);
           return true;
         });
     }
diff --git a/src/systems/logical_camera/LogicalCamera.cc b/src/systems/logical_camera/LogicalCamera.cc
index 3b53fb4c0c..fbed671f66 100644
--- a/src/systems/logical_camera/LogicalCamera.cc
+++ b/src/systems/logical_camera/LogicalCamera.cc
@@ -22,6 +22,7 @@
 #include <map>
 #include <string>
 #include <unordered_map>
+#include <unordered_set>
 #include <utility>
 
 #include <ignition/common/Profiler.hh>
@@ -59,23 +60,28 @@ class ignition::gazebo::systems::LogicalCameraPrivate
   /// \brief Ign-sensors sensor factory for creating sensors
   public: sensors::SensorFactory sensorFactory;
 
+  /// \brief Keep list of sensors that were created during the previous
+  /// `PostUpdate`, so that components can be created during the next
+  /// `PreUpdate`.
+  public: std::unordered_set<Entity> newSensors;
+
   /// True if the rendering component is initialized
   public: bool initialized = false;
 
   /// \brief Create sensor
-  /// \param[in] _ecm Mutable reference to ECM.
+  /// \param[in] _ecm Immutable reference to ECM.
   /// \param[in] _entity Entity of the IMU
   /// \param[in] _logicalCamera LogicalCamera component.
   /// \param[in] _parent Parent entity component.
   public: void AddLogicalCamera(
-    EntityComponentManager &_ecm,
+    const EntityComponentManager &_ecm,
     const Entity _entity,
     const components::LogicalCamera *_logicalCamera,
     const components::ParentEntity *_parent);
 
   /// \brief Create logicalCamera sensor
-  /// \param[in] _ecm Mutable reference to ECM.
-  public: void CreateLogicalCameraEntities(EntityComponentManager &_ecm);
+  /// \param[in] _ecm Immutable reference to ECM.
+  public: void CreateSensors(const EntityComponentManager &_ecm);
 
   /// \brief Update logicalCamera sensor data based on physics data
   /// \param[in] _ecm Immutable reference to ECM.
@@ -101,7 +107,21 @@ void LogicalCamera::PreUpdate(const UpdateInfo &/*_info*/,
     EntityComponentManager &_ecm)
 {
   IGN_PROFILE("LogicalCamera::PreUpdate");
-  this->dataPtr->CreateLogicalCameraEntities(_ecm);
+
+  // Create components
+  for (auto entity : this->dataPtr->newSensors)
+  {
+    auto it = this->dataPtr->entitySensorMap.find(entity);
+    if (it == this->dataPtr->entitySensorMap.end())
+    {
+      ignerr << "Entity [" << entity
+             << "] isn't in sensor map, this shouldn't happen." << std::endl;
+      continue;
+    }
+    // Set topic
+    _ecm.CreateComponent(entity, components::SensorTopic(it->second->Topic()));
+  }
+  this->dataPtr->newSensors.clear();
 }
 
 //////////////////////////////////////////////////
@@ -118,6 +138,8 @@ void LogicalCamera::PostUpdate(const UpdateInfo &_info,
         << "s]. System may not work properly." << std::endl;
   }
 
+  this->dataPtr->CreateSensors(_ecm);
+
   // Only update and publish if not paused.
   if (!_info.paused)
   {
@@ -137,7 +159,7 @@ void LogicalCamera::PostUpdate(const UpdateInfo &_info,
 
 //////////////////////////////////////////////////
 void LogicalCameraPrivate::AddLogicalCamera(
-  EntityComponentManager &_ecm,
+  const EntityComponentManager &_ecm,
   const Entity _entity,
   const components::LogicalCamera *_logicalCamera,
   const components::ParentEntity *_parent)
@@ -172,16 +194,13 @@ void LogicalCameraPrivate::AddLogicalCamera(
   math::Pose3d sensorWorldPose = worldPose(_entity, _ecm);
   sensor->SetPose(sensorWorldPose);
 
-  // Set topic
-  _ecm.CreateComponent(_entity, components::SensorTopic(sensor->Topic()));
-
   this->entitySensorMap.insert(
       std::make_pair(_entity, std::move(sensor)));
+  this->newSensors.insert(_entity);
 }
 
 //////////////////////////////////////////////////
-void LogicalCameraPrivate::CreateLogicalCameraEntities(
-    EntityComponentManager &_ecm)
+void LogicalCameraPrivate::CreateSensors(const EntityComponentManager &_ecm)
 {
   IGN_PROFILE("LogicalCameraPrivate::CreateLogicalCameraEntities");
   if (!this->initialized)
@@ -192,7 +211,7 @@ void LogicalCameraPrivate::CreateLogicalCameraEntities(
           const components::LogicalCamera *_logicalCamera,
           const components::ParentEntity *_parent)->bool
         {
-          AddLogicalCamera(_ecm, _entity, _logicalCamera, _parent);
+          this->AddLogicalCamera(_ecm, _entity, _logicalCamera, _parent);
           return true;
         });
     this->initialized = true;
@@ -206,7 +225,7 @@ void LogicalCameraPrivate::CreateLogicalCameraEntities(
           const components::LogicalCamera *_logicalCamera,
           const components::ParentEntity *_parent)->bool
         {
-          AddLogicalCamera(_ecm, _entity, _logicalCamera, _parent);
+          this->AddLogicalCamera(_ecm, _entity, _logicalCamera, _parent);
           return true;
         });
   }
diff --git a/src/systems/magnetometer/Magnetometer.cc b/src/systems/magnetometer/Magnetometer.cc
index ade0e3dd08..bf974caa82 100644
--- a/src/systems/magnetometer/Magnetometer.cc
+++ b/src/systems/magnetometer/Magnetometer.cc
@@ -19,6 +19,7 @@
 
 #include <string>
 #include <unordered_map>
+#include <unordered_set>
 #include <utility>
 
 #include <ignition/plugin/Register.hh>
@@ -56,25 +57,30 @@ class ignition::gazebo::systems::MagnetometerPrivate
   /// \brief Ign-sensors sensor factory for creating sensors
   public: sensors::SensorFactory sensorFactory;
 
+  /// \brief Keep list of sensors that were created during the previous
+  /// `PostUpdate`, so that components can be created during the next
+  /// `PreUpdate`.
+  public: std::unordered_set<Entity> newSensors;
+
   /// True if the rendering component is initialized
   public: bool initialized = false;
 
   /// \brief Create sensor
-  /// \param[in] _ecm Mutable reference to ECM.
+  /// \param[in] _ecm Immutable reference to ECM.
   /// \param[in] _entity Entity of the IMU
   /// \param[in] _magnetometer Magnetometer component.
   /// \param[in] _worldField MagneticField component.
   /// \param[in] _parent Parent entity component.
   public: void AddMagnetometer(
-    EntityComponentManager &_ecm,
+    const EntityComponentManager &_ecm,
     const Entity _entity,
     const components::Magnetometer *_magnetometer,
     const components::MagneticField *_worldField,
     const components::ParentEntity *_parent);
 
   /// \brief Create magnetometer sensor
-  /// \param[in] _ecm Mutable reference to ECM.
-  public: void CreateMagnetometerEntities(EntityComponentManager &_ecm);
+  /// \param[in] _ecm Immutable reference to ECM.
+  public: void CreateSensors(const EntityComponentManager &_ecm);
 
   /// \brief Update magnetometer sensor data based on physics data
   /// \param[in] _ecm Immutable reference to ECM.
@@ -100,7 +106,21 @@ void Magnetometer::PreUpdate(const UpdateInfo &/*_info*/,
     EntityComponentManager &_ecm)
 {
   IGN_PROFILE("Magnetometer::PreUpdate");
-  this->dataPtr->CreateMagnetometerEntities(_ecm);
+
+  // Create components
+  for (auto entity : this->dataPtr->newSensors)
+  {
+    auto it = this->dataPtr->entitySensorMap.find(entity);
+    if (it == this->dataPtr->entitySensorMap.end())
+    {
+      ignerr << "Entity [" << entity
+             << "] isn't in sensor map, this shouldn't happen." << std::endl;
+      continue;
+    }
+    // Set topic
+    _ecm.CreateComponent(entity, components::SensorTopic(it->second->Topic()));
+  }
+  this->dataPtr->newSensors.clear();
 }
 
 //////////////////////////////////////////////////
@@ -117,6 +137,8 @@ void Magnetometer::PostUpdate(const UpdateInfo &_info,
         << "s]. System may not work properly." << std::endl;
   }
 
+  this->dataPtr->CreateSensors(_ecm);
+
   // Only update and publish if not paused.
   if (!_info.paused)
   {
@@ -136,7 +158,7 @@ void Magnetometer::PostUpdate(const UpdateInfo &_info,
 
 //////////////////////////////////////////////////
 void MagnetometerPrivate::AddMagnetometer(
-  EntityComponentManager &_ecm,
+  const EntityComponentManager &_ecm,
   const Entity _entity,
   const components::Magnetometer *_magnetometer,
   const components::MagneticField *_worldField,
@@ -178,16 +200,13 @@ void MagnetometerPrivate::AddMagnetometer(
   math::Pose3d p = worldPose(_entity, _ecm);
   sensor->SetWorldPose(p);
 
-  // Set topic
-  _ecm.CreateComponent(_entity, components::SensorTopic(sensor->Topic()));
-
   this->entitySensorMap.insert(
       std::make_pair(_entity, std::move(sensor)));
+  this->newSensors.insert(_entity);
 }
 
 //////////////////////////////////////////////////
-void MagnetometerPrivate::CreateMagnetometerEntities(
-    EntityComponentManager &_ecm)
+void MagnetometerPrivate::CreateSensors(const EntityComponentManager &_ecm)
 {
   IGN_PROFILE("MagnetometerPrivate::CreateMagnetometerEntities");
   auto worldEntity = _ecm.EntityByComponents(components::World());
@@ -213,7 +232,8 @@ void MagnetometerPrivate::CreateMagnetometerEntities(
           const components::Magnetometer *_magnetometer,
           const components::ParentEntity *_parent)->bool
         {
-          AddMagnetometer(_ecm, _entity, _magnetometer, worldField, _parent);
+          this->AddMagnetometer(_ecm, _entity, _magnetometer, worldField,
+              _parent);
           return true;
         });
     this->initialized = true;
@@ -226,7 +246,8 @@ void MagnetometerPrivate::CreateMagnetometerEntities(
           const components::Magnetometer *_magnetometer,
           const components::ParentEntity *_parent)->bool
         {
-          AddMagnetometer(_ecm, _entity, _magnetometer, worldField, _parent);
+          this->AddMagnetometer(_ecm, _entity, _magnetometer, worldField,
+              _parent);
           return true;
         });
   }
diff --git a/test/integration/air_pressure_system.cc b/test/integration/air_pressure_system.cc
index 51e9000d90..52ad96ab50 100644
--- a/test/integration/air_pressure_system.cc
+++ b/test/integration/air_pressure_system.cc
@@ -62,7 +62,7 @@ TEST_F(AirPressureTest, AirPressure)
 
   // Create a system that checks sensor topic
   test::Relay testSystem;
-  testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &,
+  testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &_info,
                               const gazebo::EntityComponentManager &_ecm)
       {
         _ecm.Each<components::AirPressureSensor, components::Name>(
@@ -75,6 +75,10 @@ TEST_F(AirPressureTest, AirPressure)
               auto sensorComp = _ecm.Component<components::Sensor>(_entity);
               EXPECT_NE(nullptr, sensorComp);
 
+              if (_info.iterations == 1)
+                return true;
+
+              // This component is created on the 2nd PreUpdate
               auto topicComp = _ecm.Component<components::SensorTopic>(_entity);
               EXPECT_NE(nullptr, topicComp);
               if (topicComp)
diff --git a/test/integration/altimeter_system.cc b/test/integration/altimeter_system.cc
index 4b391963ac..bac80b7c25 100644
--- a/test/integration/altimeter_system.cc
+++ b/test/integration/altimeter_system.cc
@@ -80,7 +80,7 @@ TEST_F(AltimeterTest, ModelFalling)
   test::Relay testSystem;
   std::vector<math::Pose3d> poses;
   std::vector<math::Vector3d> velocities;
-  testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &,
+  testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &_info,
                               const gazebo::EntityComponentManager &_ecm)
       {
         _ecm.Each<components::Altimeter, components::Name,
@@ -100,6 +100,10 @@ TEST_F(AltimeterTest, ModelFalling)
               auto sensorComp = _ecm.Component<components::Sensor>(_entity);
               EXPECT_NE(nullptr, sensorComp);
 
+              if (_info.iterations == 1)
+                return true;
+
+              // This component is created on the 2nd PreUpdate
               auto topicComp = _ecm.Component<components::SensorTopic>(_entity);
               EXPECT_NE(nullptr, topicComp);
               if (topicComp)
diff --git a/test/integration/imu_system.cc b/test/integration/imu_system.cc
index 68147f4e30..c38c3cf6c7 100644
--- a/test/integration/imu_system.cc
+++ b/test/integration/imu_system.cc
@@ -89,7 +89,7 @@ TEST_F(ImuTest, ModelFalling)
   std::vector<math::Pose3d> poses;
   std::vector<math::Vector3d> accelerations;
   std::vector<math::Vector3d> angularVelocities;
-  testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &,
+  testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &_info,
                               const gazebo::EntityComponentManager &_ecm)
       {
         _ecm.Each<components::Imu,
@@ -113,6 +113,10 @@ TEST_F(ImuTest, ModelFalling)
               auto sensorComp = _ecm.Component<components::Sensor>(_entity);
               EXPECT_NE(nullptr, sensorComp);
 
+              if (_info.iterations == 1)
+                return true;
+
+              // This component is created on the 2nd PreUpdate
               auto topicComp = _ecm.Component<components::SensorTopic>(_entity);
               EXPECT_NE(nullptr, topicComp);
               if (topicComp)
diff --git a/test/integration/logical_camera_system.cc b/test/integration/logical_camera_system.cc
index 32bae02ef5..4abcf00700 100644
--- a/test/integration/logical_camera_system.cc
+++ b/test/integration/logical_camera_system.cc
@@ -91,7 +91,7 @@ TEST_F(LogicalCameraTest, LogicalCameraBox)
 
   // Create a system that checks sensor topics
   test::Relay testSystem;
-  testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &,
+  testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &_info,
                               const gazebo::EntityComponentManager &_ecm)
       {
         _ecm.Each<components::LogicalCamera, components::Name>(
@@ -105,11 +105,15 @@ TEST_F(LogicalCameraTest, LogicalCameraBox)
                 auto sensorComp = _ecm.Component<components::Sensor>(_entity);
                 EXPECT_NE(nullptr, sensorComp);
 
-                auto topicComp =
-                    _ecm.Component<components::SensorTopic>(_entity);
-                EXPECT_NE(nullptr, topicComp);
-                if (topicComp) {
-                  EXPECT_EQ(topic1, topicComp->Data());
+                if (_info.iterations != 1)
+                {
+                  auto topicComp =
+                      _ecm.Component<components::SensorTopic>(_entity);
+                  EXPECT_NE(nullptr, topicComp);
+                  if (topicComp)
+                  {
+                    EXPECT_EQ(topic1, topicComp->Data());
+                  }
                 }
                 update1Checked = true;
               }
@@ -119,11 +123,15 @@ TEST_F(LogicalCameraTest, LogicalCameraBox)
                 auto sensorComp = _ecm.Component<components::Sensor>(_entity);
                 EXPECT_NE(nullptr, sensorComp);
 
-                auto topicComp =
-                    _ecm.Component<components::SensorTopic>(_entity);
-                EXPECT_NE(nullptr, topicComp);
-                if (topicComp) {
-                EXPECT_EQ(topic2, topicComp->Data());
+                if (_info.iterations != 1)
+                {
+                  auto topicComp =
+                      _ecm.Component<components::SensorTopic>(_entity);
+                  EXPECT_NE(nullptr, topicComp);
+                  if (topicComp)
+                  {
+                    EXPECT_EQ(topic2, topicComp->Data());
+                  }
                 }
                 update2Checked = true;
               }
diff --git a/test/integration/magnetometer_system.cc b/test/integration/magnetometer_system.cc
index cf29f976f8..e17be9b9f7 100644
--- a/test/integration/magnetometer_system.cc
+++ b/test/integration/magnetometer_system.cc
@@ -81,7 +81,7 @@ TEST_F(MagnetometerTest, RotatedMagnetometer)
   test::Relay testSystem;
 
   std::vector<math::Pose3d> poses;
-  testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &,
+  testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &_info,
                               const gazebo::EntityComponentManager &_ecm)
       {
         _ecm.Each<components::Magnetometer,
@@ -98,6 +98,10 @@ TEST_F(MagnetometerTest, RotatedMagnetometer)
               auto sensorComp = _ecm.Component<components::Sensor>(_entity);
               EXPECT_NE(nullptr, sensorComp);
 
+              if (_info.iterations == 1)
+                return true;
+
+              // This component is created on the 2nd PreUpdate
               auto topicComp = _ecm.Component<components::SensorTopic>(_entity);
               EXPECT_NE(nullptr, topicComp);
               if (topicComp)

From 749884db3f031f6159101c1d98880df6ac2deb46 Mon Sep 17 00:00:00 2001
From: Louise Poubel <louise@openrobotics.org>
Date: Fri, 21 Jan 2022 09:10:55 -0800
Subject: [PATCH 04/17] Buoyancy: fix center of volume's reference frame
 (#1302)

Signed-off-by: Louise Poubel <louise@openrobotics.org>
---
 .../gazebo/components/CenterOfVolume.hh       |   2 +-
 src/systems/buoyancy/Buoyancy.cc              |  11 +-
 test/integration/buoyancy.cc                  |  76 +++++++++
 test/worlds/center_of_volume.sdf              | 160 ++++++++++++++++++
 4 files changed, 242 insertions(+), 7 deletions(-)
 create mode 100644 test/worlds/center_of_volume.sdf

diff --git a/include/ignition/gazebo/components/CenterOfVolume.hh b/include/ignition/gazebo/components/CenterOfVolume.hh
index 4bd2389101..2096d2d5d0 100644
--- a/include/ignition/gazebo/components/CenterOfVolume.hh
+++ b/include/ignition/gazebo/components/CenterOfVolume.hh
@@ -33,7 +33,7 @@ namespace components
   /// \brief A component for an entity's center of volume. Units are in meters.
   /// The Vector3 value indicates center of volume of an entity. The
   /// position of the center of volume is relative to the pose of the parent
-  /// entity.
+  /// entity, which is usually a link.
   using CenterOfVolume = Component<math::Vector3d, class CenterOfVolumeTag>;
   IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.CenterOfVolume",
       CenterOfVolume)
diff --git a/src/systems/buoyancy/Buoyancy.cc b/src/systems/buoyancy/Buoyancy.cc
index 81a880a21a..0e7a286583 100644
--- a/src/systems/buoyancy/Buoyancy.cc
+++ b/src/systems/buoyancy/Buoyancy.cc
@@ -151,7 +151,7 @@ void Buoyancy::PreUpdate(const ignition::gazebo::UpdateInfo &_info,
         _entity, components::Collision());
 
     double volumeSum = 0;
-    ignition::math::Vector3d weightedPosSum =
+    ignition::math::Vector3d weightedPosInLinkSum =
       ignition::math::Vector3d::Zero;
 
     // Compute the volume of the link by iterating over all the collision
@@ -210,16 +210,15 @@ void Buoyancy::PreUpdate(const ignition::gazebo::UpdateInfo &_info,
       }
 
       volumeSum += volume;
-      math::Pose3d pose = worldPose(collision, _ecm);
-      weightedPosSum += volume * pose.Pos();
+      auto poseInLink = _ecm.Component<components::Pose>(collision)->Data();
+      weightedPosInLinkSum += volume * poseInLink.Pos();
     }
 
     if (volumeSum > 0)
     {
-      // Store the center of volume
-      math::Pose3d linkWorldPose = worldPose(_entity, _ecm);
+      // Store the center of volume expressed in the link frame
       _ecm.CreateComponent(_entity, components::CenterOfVolume(
-            weightedPosSum / volumeSum - linkWorldPose.Pos()));
+            weightedPosInLinkSum / volumeSum));
 
       // Store the volume
       _ecm.CreateComponent(_entity, components::Volume(volumeSum));
diff --git a/test/integration/buoyancy.cc b/test/integration/buoyancy.cc
index 77445fdd61..7acfee7a5e 100644
--- a/test/integration/buoyancy.cc
+++ b/test/integration/buoyancy.cc
@@ -20,8 +20,10 @@
 #include <ignition/common/Console.hh>
 #include <ignition/common/Util.hh>
 
+#include "ignition/gazebo/Util.hh"
 #include "ignition/gazebo/Server.hh"
 #include "ignition/gazebo/SystemLoader.hh"
+#include "ignition/gazebo/TestFixture.hh"
 #include "ignition/gazebo/components/CenterOfVolume.hh"
 #include "ignition/gazebo/components/Link.hh"
 #include "ignition/gazebo/components/Name.hh"
@@ -194,3 +196,77 @@ TEST_F(BuoyancyTest, Movement)
   server.Run(true, iterations, false);
   EXPECT_TRUE(finished);
 }
+
+/////////////////////////////////////////////////
+TEST_F(BuoyancyTest, OffsetAndRotation)
+{
+  TestFixture fixture(common::joinPaths(std::string(PROJECT_SOURCE_PATH),
+    "test", "worlds", "center_of_volume.sdf"));
+
+  std::size_t iterations{0};
+  fixture.OnPostUpdate([&](
+      const gazebo::UpdateInfo &,
+      const gazebo::EntityComponentManager &_ecm)
+  {
+    // Get links
+    auto noOffsets = entitiesFromScopedName("no_offset::link", _ecm);
+    ASSERT_EQ(1u, noOffsets.size());
+    auto noOffset = *noOffsets.begin();
+    EXPECT_NE(kNullEntity, noOffset);
+
+    auto noOffsetRotateds = entitiesFromScopedName("no_offset_rotated::link",
+        _ecm);
+    ASSERT_EQ(1u, noOffsetRotateds.size());
+    auto noOffsetRotated = *noOffsetRotateds.begin();
+    EXPECT_NE(kNullEntity, noOffsetRotated);
+
+    auto withOffsets = entitiesFromScopedName("com_cov_offset::link", _ecm);
+    ASSERT_EQ(1u, withOffsets.size());
+    auto withOffset = *withOffsets.begin();
+    EXPECT_NE(kNullEntity, withOffset);
+
+    auto withOffsetRotateds = entitiesFromScopedName(
+        "com_cov_offset_rotated::link", _ecm);
+    ASSERT_EQ(1u, withOffsetRotateds.size());
+    auto withOffsetRotated = *withOffsetRotateds.begin();
+    EXPECT_NE(kNullEntity, withOffsetRotated);
+
+    // Check CoVs have correct offsets
+    auto noOffsetCoV = _ecm.Component<components::CenterOfVolume>(noOffset);
+    ASSERT_NE(noOffsetCoV, nullptr);
+    EXPECT_EQ(math::Vector3d::Zero, noOffsetCoV->Data());
+
+    auto noOffsetRotatedCoV = _ecm.Component<components::CenterOfVolume>(
+        noOffsetRotated);
+    ASSERT_NE(noOffsetRotatedCoV, nullptr);
+    EXPECT_EQ(math::Vector3d::Zero, noOffsetRotatedCoV->Data());
+
+    auto withOffsetCoV = _ecm.Component<components::CenterOfVolume>(withOffset);
+    ASSERT_NE(withOffsetCoV, nullptr);
+    EXPECT_EQ(math::Vector3d::One, withOffsetCoV->Data());
+
+    auto withOffsetRotatedCoV = _ecm.Component<components::CenterOfVolume>(
+        withOffsetRotated);
+    ASSERT_NE(withOffsetRotatedCoV, nullptr);
+    EXPECT_EQ(math::Vector3d::One, withOffsetRotatedCoV->Data());
+
+    // Check that all objects are neutrally buoyant and stay still
+    auto noOffsetPose = worldPose(noOffset, _ecm);
+    EXPECT_EQ(math::Pose3d(), noOffsetPose);
+
+    auto noOffsetRotatedPose = worldPose(noOffsetRotated, _ecm);
+    EXPECT_EQ(math::Pose3d(-3, 0, 0, 0.1, 0.2, 0.3), noOffsetRotatedPose);
+
+    auto withOffsetPose = worldPose(withOffset, _ecm);
+    EXPECT_EQ(math::Pose3d(0, 3, 0, 0, 0, 0), withOffsetPose);
+
+    auto withOffsetRotatedPose = worldPose(withOffsetRotated, _ecm);
+    EXPECT_EQ(math::Pose3d(-3, 3, 0, 0.1, 0.2, 0.3), withOffsetRotatedPose);
+
+    iterations++;
+  }).Finalize();
+
+  std::size_t targetIterations{1000};
+  fixture.Server()->Run(true, targetIterations, false);
+  EXPECT_EQ(targetIterations, iterations);
+}
diff --git a/test/worlds/center_of_volume.sdf b/test/worlds/center_of_volume.sdf
new file mode 100644
index 0000000000..0972ca554d
--- /dev/null
+++ b/test/worlds/center_of_volume.sdf
@@ -0,0 +1,160 @@
+<?xml version="1.0" ?>
+<sdf version="1.6">
+  <world name="center_of_volume">
+
+    <physics name="fast" type="ignored">
+      <real_time_factor>0</real_time_factor>
+    </physics>
+    <plugin
+      filename="libignition-gazebo-physics-system.so"
+      name="ignition::gazebo::systems::Physics">
+    </plugin>
+    <plugin
+      filename="libignition-gazebo-buoyancy-system.so"
+      name="ignition::gazebo::systems::Buoyancy">
+      <uniform_fluid_density>1000</uniform_fluid_density>
+    </plugin>
+
+    <model name='no_offset'>
+      <pose>0 0 0 0 0 0</pose>
+      <link name='link'>
+        <pose>0 0 0 0 0 0</pose>
+        <inertial>
+          <pose>0 0 0 0 0 0</pose>
+          <inertia>
+            <ixx>166.66</ixx>
+            <ixy>0</ixy>
+            <ixz>0</ixz>
+            <iyy>166.66</iyy>
+            <iyz>0</iyz>
+            <izz>166.66</izz>
+          </inertia>
+          <mass>1000.0</mass>
+        </inertial>
+        <visual name='visual'>
+          <pose>0 0 0 0 0 0</pose>
+          <geometry>
+            <box>
+              <size>1.0 1.0 1.0</size>
+            </box>
+          </geometry>
+        </visual>
+        <collision name='collision'>
+          <pose>0 0 0 0 0 0</pose>
+          <geometry>
+            <box>
+              <size>1.0 1.0 1.0</size>
+            </box>
+          </geometry>
+        </collision>
+      </link>
+    </model>
+
+    <model name='no_offset_rotated'>
+      <pose>-3 0 0 0.1 0.2 0.3</pose>
+      <link name='link'>
+        <pose>0 0 0 0 0 0</pose>
+        <inertial>
+          <pose>0 0 0 0 0 0</pose>
+          <inertia>
+            <ixx>166.66</ixx>
+            <ixy>0</ixy>
+            <ixz>0</ixz>
+            <iyy>166.66</iyy>
+            <iyz>0</iyz>
+            <izz>166.66</izz>
+          </inertia>
+          <mass>1000.0</mass>
+        </inertial>
+        <visual name='visual'>
+          <pose>0 0 0 0 0 0</pose>
+          <geometry>
+            <box>
+              <size>1.0 1.0 1.0</size>
+            </box>
+          </geometry>
+        </visual>
+        <collision name='collision'>
+          <pose>0 0 0 0 0 0</pose>
+          <geometry>
+            <box>
+              <size>1.0 1.0 1.0</size>
+            </box>
+          </geometry>
+        </collision>
+      </link>
+    </model>
+
+    <model name='com_cov_offset'>
+      <pose>0 3 0 0 0 0</pose>
+      <link name='link'>
+        <pose>0 0 0 0 0 0</pose>
+        <inertial>
+          <pose>1 1 1 0 0 0</pose>
+          <inertia>
+            <ixx>166.66</ixx>
+            <ixy>0</ixy>
+            <ixz>0</ixz>
+            <iyy>166.66</iyy>
+            <iyz>0</iyz>
+            <izz>166.66</izz>
+          </inertia>
+          <mass>1000.0</mass>
+        </inertial>
+        <visual name='visual'>
+          <pose>1 1 1 0 0 0</pose>
+          <geometry>
+            <box>
+              <size>1.0 1.0 1.0</size>
+            </box>
+          </geometry>
+        </visual>
+        <collision name='collision'>
+          <pose>1 1 1 0 0 0</pose>
+          <geometry>
+            <box>
+              <size>1.0 1.0 1.0</size>
+            </box>
+          </geometry>
+        </collision>
+      </link>
+    </model>
+
+    <model name='com_cov_offset_rotated'>
+      <pose>-3 3 0 0.1 0.2 0.3</pose>
+      <link name='link'>
+        <pose>0 0 0 0 0 0</pose>
+        <inertial>
+          <pose>1 1 1 0 0 0</pose>
+          <inertia>
+            <ixx>166.66</ixx>
+            <ixy>0</ixy>
+            <ixz>0</ixz>
+            <iyy>166.66</iyy>
+            <iyz>0</iyz>
+            <izz>166.66</izz>
+          </inertia>
+          <mass>1000.0</mass>
+        </inertial>
+        <visual name='visual'>
+          <pose>1 1 1 0 0 0</pose>
+          <geometry>
+            <box>
+              <size>1.0 1.0 1.0</size>
+            </box>
+          </geometry>
+        </visual>
+        <collision name='collision'>
+          <pose>1 1 1 0 0 0</pose>
+          <geometry>
+            <box>
+              <size>1.0 1.0 1.0</size>
+            </box>
+          </geometry>
+        </collision>
+      </link>
+    </model>
+
+  </world>
+</sdf>
+

From 912e2cebec89a96fd8632ed5358d6036f7873ef6 Mon Sep 17 00:00:00 2001
From: Louise Poubel <louise@openrobotics.org>
Date: Tue, 25 Jan 2022 11:36:36 -0800
Subject: [PATCH 05/17] Update source install instructions (#1311)

Signed-off-by: Louise Poubel <louise@openrobotics.org>
---
 tutorials/install.md | 25 ++++++++++++++-----------
 1 file changed, 14 insertions(+), 11 deletions(-)

diff --git a/tutorials/install.md b/tutorials/install.md
index 97d4e598ac..3260f10154 100644
--- a/tutorials/install.md
+++ b/tutorials/install.md
@@ -59,33 +59,36 @@ feature which hasn't been released yet.
 
 ### Ubuntu 18.04 or above
 
-1. Enable the Ignition software repositories:
+1. Install tools
+  ```
+  sudo apt install -y build-essential cmake g++-8 git gnupg lsb-release wget
+  ```
+
+2. Enable the Ignition software repositories:
   ```
   sudo sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list'
   wget http://packages.osrfoundation.org/gazebo.key -O - | sudo apt-key add -
   sudo apt-get update
   ```
 
-2. Install package dependencies:
+3. Clone repository
   ```
   git clone https://github.com/ignitionrobotics/ign-gazebo -b ign-gazebo<#>
-  export SYSTEM_VERSION=bionic
-  sudo apt -y install \
-    $(sort -u $(find . -iname 'packages-'`lsb_release -cs`'.apt' -o -iname 'packages.apt' | grep -v '/\.git/') | sed '/ignition\|sdf/d' | tr '\n' ' ')
   ```
 
-3. (Ubuntu 18.04 only) Configure gcc8
+4. Install package dependencies (including other Ignition libraries):
   ```
-  sudo apt-get install g++-8
-  sudo update-alternatives --install /usr/bin/gcc gcc /usr/bin/gcc-8 800 --slave /usr/bin/g++ g++ /usr/bin/g++-8 --slave /usr/bin/gcov gcov /usr/bin/gcov-8
+  sudo apt -y install \
+    $(sort -u $(find . -iname 'packages-'`lsb_release -cs`'.apt' -o -iname 'packages.apt' | tr '\n' ' '))
   ```
 
-4. Clone the repository if you haven't already.
+5. (Ubuntu 18.04 only) Configure gcc8
   ```
-  git clone https://github.com/ignitionrobotics/ign-gazebo -b ign-gazebo<#>
+  sudo apt-get install g++-8
+  sudo update-alternatives --install /usr/bin/gcc gcc /usr/bin/gcc-8 800 --slave /usr/bin/g++ g++ /usr/bin/g++-8 --slave /usr/bin/gcov gcov /usr/bin/gcov-8
   ```
 
-5. Configure and build.
+6. Configure and build.
   ```
   cd ign-gazebo
   mkdir build

From ebb3a9ad8a7484eeb97bf2585f7731e7d4225f9b Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Fri, 28 Jan 2022 16:48:17 +0800
Subject: [PATCH 06/17] Log an error if JointPositionController cannot find the
 joint. (citadel retarget) (#1314)

In the event a user enters the wrong name for a certain joint, the JointPositionController system will silently fail. This PR adds a simple error message that tells the user that the joint was not found.

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>
---
 .../joint_position_controller/JointPositionController.cc | 9 +++++++++
 1 file changed, 9 insertions(+)

diff --git a/src/systems/joint_position_controller/JointPositionController.cc b/src/systems/joint_position_controller/JointPositionController.cc
index 067c622161..9530256eff 100644
--- a/src/systems/joint_position_controller/JointPositionController.cc
+++ b/src/systems/joint_position_controller/JointPositionController.cc
@@ -192,8 +192,17 @@ void JointPositionController::PreUpdate(
         this->dataPtr->model.JointByName(_ecm, this->dataPtr->jointName);
   }
 
+  // If the joint is still not found then warn the user, they may have entered
+  // the wrong joint name.
   if (this->dataPtr->jointEntity == kNullEntity)
+  {
+    static bool warned = false;
+    if(!warned)
+      ignerr << "Could not find joint with name ["
+        << this->dataPtr->jointName <<"]\n";
+    warned = true;
     return;
+  }
 
   // Nothing left to do if paused.
   if (_info.paused)

From 717a7e91b73e968d836689d23dcdbf2579e7fbce Mon Sep 17 00:00:00 2001
From: Ian Chen <ichen@osrfoundation.org>
Date: Thu, 3 Feb 2022 17:40:22 -0800
Subject: [PATCH 07/17] Load and run visual plugin (system) on GUI side (#1275)

* load and run visual plugins on gui end

Signed-off-by: Ian Chen <ichen@osrfoundation.org>

* scene update event emitted on both server and gui side

Signed-off-by: Ian Chen <ichen@osrfoundation.org>

* shader param update working

Signed-off-by: Ian Chen <ichen@osrfoundation.org>

* sim time, constants, full example working

Signed-off-by: Ian Chen <ichen@osrfoundation.org>

* add integration test

Signed-off-by: Ian Chen <ichen@osrfoundation.org>

* code cleanup

Signed-off-by: Ian Chen <ichen@osrfoundation.org>

* more code cleanup

Signed-off-by: Ian Chen <ichen@osrfoundation.org>

* style fixes and add some comments

Signed-off-by: Ian Chen <ichen@osrfoundation.org>

* review changes

Signed-off-by: Ian Chen <ichen@osrfoundation.org>

* require display for shader param test

Signed-off-by: Ian Chen <ichen@osrfoundation.org>

* style and comment

Signed-off-by: Ian Chen <ichen@osrfoundation.org>
---
 examples/worlds/shader_param.sdf              | 268 ++++++++++++
 include/ignition/gazebo/components/Visual.hh  |  54 +++
 include/ignition/gazebo/gui/GuiEvents.hh      |  24 ++
 include/ignition/gazebo/rendering/Events.hh   |  12 +
 .../ignition/gazebo/rendering/RenderUtil.hh   |   4 +
 src/SdfEntityCreator.cc                       |  11 +
 src/SimulationRunner.hh                       |   3 -
 src/SimulationRunner_TEST.cc                  |  21 +
 src/gui/GuiEvents.cc                          |  30 ++
 src/gui/GuiRunner.cc                          | 144 ++++++-
 src/gui/GuiRunner.hh                          |  10 +
 .../plugins/scene_manager/GzSceneManager.cc   |  55 +++
 src/rendering/RenderUtil.cc                   |  13 +
 src/systems/CMakeLists.txt                    |   1 +
 src/systems/sensors/Sensors.cc                |   4 +-
 src/systems/shader_param/CMakeLists.txt       |   8 +
 src/systems/shader_param/ShaderParam.cc       | 396 ++++++++++++++++++
 src/systems/shader_param/ShaderParam.hh       | 100 +++++
 test/integration/CMakeLists.txt               |   1 +
 test/integration/shader_param_system.cc       | 110 +++++
 test/plugins/CMakeLists.txt                   |   1 +
 test/plugins/TestVisualSystem.cc              |  23 +
 test/plugins/TestVisualSystem.hh              |  81 ++++
 test/worlds/plugins.sdf                       |   7 +
 24 files changed, 1371 insertions(+), 10 deletions(-)
 create mode 100644 examples/worlds/shader_param.sdf
 create mode 100644 src/systems/shader_param/CMakeLists.txt
 create mode 100644 src/systems/shader_param/ShaderParam.cc
 create mode 100644 src/systems/shader_param/ShaderParam.hh
 create mode 100644 test/integration/shader_param_system.cc
 create mode 100644 test/plugins/TestVisualSystem.cc
 create mode 100644 test/plugins/TestVisualSystem.hh

diff --git a/examples/worlds/shader_param.sdf b/examples/worlds/shader_param.sdf
new file mode 100644
index 0000000000..db3d48ca78
--- /dev/null
+++ b/examples/worlds/shader_param.sdf
@@ -0,0 +1,268 @@
+<?xml version="1.0" ?>
+<!--
+
+This world contains a sphere model whose visual appearance is altered by the
+ShaderParam visual plugin over time.
+
+-->
+<sdf version="1.6">
+  <world name="deformable_sphere">
+
+    <plugin
+      filename="ignition-gazebo-physics-system"
+      name="ignition::gazebo::systems::Physics">
+    </plugin>
+    <plugin
+      filename="ignition-gazebo-sensors-system"
+      name="ignition::gazebo::systems::Sensors">
+      <render_engine>ogre2</render_engine>
+      <background_color>0.8 0.8 0.8</background_color>
+    </plugin>
+    <plugin
+      filename="ignition-gazebo-scene-broadcaster-system"
+      name="ignition::gazebo::systems::SceneBroadcaster">
+    </plugin>
+    <plugin
+      filename="ignition-gazebo-user-commands-system"
+      name="ignition::gazebo::systems::UserCommands">
+    </plugin>
+
+    <gui fullscreen="0">
+
+      <!-- 3D scene -->
+      <plugin filename="MinimalScene" name="3D View">
+        <ignition-gui>
+          <title>3D View</title>
+          <property type="bool" key="showTitleBar">false</property>
+          <property type="string" key="state">docked</property>
+        </ignition-gui>
+
+        <engine>ogre2</engine>
+        <scene>scene</scene>
+        <ambient_light>0.4 0.4 0.4</ambient_light>
+        <background_color>0.8 0.8 0.8</background_color>
+        <camera_pose>-6 0 6 0 0.5 0</camera_pose>
+      </plugin>
+
+      <!-- Plugins that add functionality to the scene -->
+      <plugin filename="EntityContextMenuPlugin" name="Entity context menu">
+        <ignition-gui>
+          <property key="state" type="string">floating</property>
+          <property key="width" type="double">5</property>
+          <property key="height" type="double">5</property>
+          <property key="showTitleBar" type="bool">false</property>
+        </ignition-gui>
+      </plugin>
+      <plugin filename="GzSceneManager" name="Scene Manager">
+        <ignition-gui>
+          <property key="resizable" type="bool">false</property>
+          <property key="width" type="double">5</property>
+          <property key="height" type="double">5</property>
+          <property key="state" type="string">floating</property>
+          <property key="showTitleBar" type="bool">false</property>
+        </ignition-gui>
+      </plugin>
+      <plugin filename="InteractiveViewControl" name="Interactive view control">
+        <ignition-gui>
+          <property key="resizable" type="bool">false</property>
+          <property key="width" type="double">5</property>
+          <property key="height" type="double">5</property>
+          <property key="state" type="string">floating</property>
+          <property key="showTitleBar" type="bool">false</property>
+        </ignition-gui>
+      </plugin>
+      <plugin filename="CameraTracking" name="Camera Tracking">
+        <ignition-gui>
+          <property key="resizable" type="bool">false</property>
+          <property key="width" type="double">5</property>
+          <property key="height" type="double">5</property>
+          <property key="state" type="string">floating</property>
+          <property key="showTitleBar" type="bool">false</property>
+        </ignition-gui>
+      </plugin>
+      <!-- World control -->
+      <plugin filename="WorldControl" name="World control">
+        <ignition-gui>
+          <title>World control</title>
+          <property type="bool" key="showTitleBar">false</property>
+          <property type="bool" key="resizable">false</property>
+          <property type="double" key="height">72</property>
+          <property type="double" key="width">121</property>
+          <property type="double" key="z">1</property>
+
+          <property type="string" key="state">floating</property>
+          <anchors target="3D View">
+            <line own="left" target="left"/>
+            <line own="bottom" target="bottom"/>
+          </anchors>
+        </ignition-gui>
+
+        <play_pause>true</play_pause>
+        <step>true</step>
+        <start_paused>true</start_paused>
+        <use_event>true</use_event>
+
+      </plugin>
+
+      <!-- World statistics -->
+      <plugin filename="WorldStats" name="World stats">
+        <ignition-gui>
+          <title>World stats</title>
+          <property type="bool" key="showTitleBar">false</property>
+          <property type="bool" key="resizable">false</property>
+          <property type="double" key="height">110</property>
+          <property type="double" key="width">290</property>
+          <property type="double" key="z">1</property>
+
+          <property type="string" key="state">floating</property>
+          <anchors target="3D View">
+            <line own="right" target="right"/>
+            <line own="bottom" target="bottom"/>
+          </anchors>
+        </ignition-gui>
+
+        <sim_time>true</sim_time>
+        <real_time>true</real_time>
+        <real_time_factor>true</real_time_factor>
+        <iterations>true</iterations>
+      </plugin>
+
+      <!-- Inspector -->
+      <plugin filename="ComponentInspector" name="Component inspector">
+        <ignition-gui>
+          <property type="string" key="state">docked</property>
+        </ignition-gui>
+      </plugin>
+
+      <!-- Entity tree -->
+      <plugin filename="EntityTree" name="Entity tree">
+        <ignition-gui>
+          <property type="string" key="state">docked</property>
+        </ignition-gui>
+      </plugin>
+
+      <plugin filename="ImageDisplay" name="Image Display">
+        <ignition-gui>
+          <title>RGB camera</title>
+          <property key="state" type="string">floating</property>
+          <property type="double" key="width">350</property>
+          <property type="double" key="height">315</property>
+        </ignition-gui>
+        <topic>camera</topic>
+        <topic_picker>false</topic_picker>
+      </plugin>
+      <plugin filename="ImageDisplay" name="Image Display 2">
+        <ignition-gui>
+          <title>Depth camera</title>
+          <property key="state" type="string">floating</property>
+          <property type="double" key="width">350</property>
+          <property type="double" key="height">315</property>
+          <property type="double" key="x">500</property>
+        </ignition-gui>
+        <topic>depth_camera</topic>
+        <topic_picker>false</topic_picker>
+      </plugin>
+    </gui>
+
+    <scene>
+      <ambient>1.0 1.0 1.0</ambient>
+      <background>0.8 0.8 0.8</background>
+    </scene>
+
+    <light type="directional" name="sun">
+      <cast_shadows>true</cast_shadows>
+      <pose>0 0 10 0 0 0</pose>
+      <diffuse>0.8 0.8 0.8 1</diffuse>
+      <specular>0.6 0.6 0.6 1</specular>
+      <direction>-0.5 0.1 -0.9</direction>
+    </light>
+
+    <model name="ground_plane">
+      <static>true</static>
+      <link name="link">
+        <collision name="collision">
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+        </collision>
+        <visual name="visual">
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+          <material>
+            <ambient>0.8 0.8 0.8 1</ambient>
+            <diffuse>0.8 0.8 0.8 1</diffuse>
+            <specular>0.8 0.8 0.8 1</specular>
+          </material>
+        </visual>
+      </link>
+    </model>
+
+    <include>
+      <pose>0 0.0 0.5 0 0 0</pose>
+      <name>deformable_sphere</name>
+      <pose>3 0 0 0 0 0</pose>
+      <uri>https://fuel.ignitionrobotics.org/1.0/openrobotics/models/deformable_sphere</uri>
+    </include>
+
+    <model name="camera">
+      <static>true</static>
+      <pose>2.5 0 0.5 0 0.0 3.14</pose>
+      <link name="link">
+        <pose>0.05 0.05 0.05 0 0 0</pose>
+        <collision name="collision">
+          <geometry>
+            <box>
+              <size>0.1 0.1 0.1</size>
+            </box>
+          </geometry>
+        </collision>
+        <visual name="visual">
+          <geometry>
+            <box>
+              <size>0.1 0.1 0.1</size>
+            </box>
+          </geometry>
+        </visual>
+        <sensor name="camera" type="camera">
+          <camera>
+            <horizontal_fov>1.047</horizontal_fov>
+            <image>
+              <width>320</width>
+              <height>240</height>
+            </image>
+            <clip>
+              <near>0.1</near>
+              <far>100</far>
+            </clip>
+          </camera>
+          <always_on>1</always_on>
+          <update_rate>30</update_rate>
+          <topic>camera</topic>
+        </sensor>
+        <sensor name="depth_camera" type="depth_camera">
+          <update_rate>10</update_rate>
+          <topic>depth_camera</topic>
+          <camera>
+            <horizontal_fov>1.05</horizontal_fov>
+            <image>
+              <width>320</width>
+              <height>240</height>
+            </image>
+            <clip>
+              <near>0.1</near>
+              <far>10.0</far>
+            </clip>
+          </camera>
+          </sensor>
+      </link>
+    </model>
+
+  </world>
+</sdf>
diff --git a/include/ignition/gazebo/components/Visual.hh b/include/ignition/gazebo/components/Visual.hh
index 9d90871a7b..de94818059 100644
--- a/include/ignition/gazebo/components/Visual.hh
+++ b/include/ignition/gazebo/components/Visual.hh
@@ -17,6 +17,11 @@
 #ifndef IGNITION_GAZEBO_COMPONENTS_VISUAL_HH_
 #define IGNITION_GAZEBO_COMPONENTS_VISUAL_HH_
 
+#include <string>
+
+#include <sdf/parser.hh>
+#include <sdf/Element.hh>
+
 #include <ignition/gazebo/components/Factory.hh>
 #include <ignition/gazebo/components/Component.hh>
 #include <ignition/gazebo/config.hh>
@@ -27,11 +32,60 @@ namespace gazebo
 {
 // Inline bracket to help doxygen filtering.
 inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
+namespace serializers
+{
+  class SdfElementSerializer
+  {
+    /// \brief Serialization for `sdf::Model`.
+    /// \param[in] _out Output stream.
+    /// \param[in] _elem Visual plugin elem to stream
+    /// \return The stream.
+    public: static std::ostream &Serialize(std::ostream &_out,
+                const sdf::ElementPtr &_elem)
+    {
+      _out << "<?xml version=\"1.0\" ?>"
+           << "<sdf version='" << SDF_PROTOCOL_VERSION << "'>"
+           << _elem->ToString("")
+           << "</sdf>";
+      return _out;
+    }
+
+    /// \brief Deserialization for `sdf::Element`.
+    /// \param[in] _in Input stream.
+    /// \param[out] _elem Visual plugin elem to populate
+    /// \return The stream.
+    public: static std::istream &Deserialize(std::istream &_in,
+                sdf::ElementPtr &_elem)
+    {
+      std::string sdfStr(std::istreambuf_iterator<char>(_in), {});
+
+      sdf::SDFPtr sdfParsed(new sdf::SDF());
+      sdf::init(sdfParsed);
+      bool result = sdf::readString(sdfStr, sdfParsed);
+      if (!result)
+      {
+        ignerr << "Unable to deserialize sdf::ElementPtr" << std::endl;
+        return _in;
+      }
+
+      _elem = sdfParsed->Root()->GetFirstElement();
+      return _in;
+    }
+  };
+}
+
 namespace components
 {
   /// \brief A component that identifies an entity as being a visual.
   using Visual = Component<NoData, class VisualTag>;
   IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.Visual", Visual)
+
+  /// \brief A component that contains a visual plugin's SDF element.
+  using VisualPlugin = Component<sdf::ElementPtr,
+                                 class VisualPluginTag,
+                                 serializers::SdfElementSerializer>;
+  IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.VisualPlugin",
+      VisualPlugin)
 }
 }
 }
diff --git a/include/ignition/gazebo/gui/GuiEvents.hh b/include/ignition/gazebo/gui/GuiEvents.hh
index 4407d68598..afbb751879 100644
--- a/include/ignition/gazebo/gui/GuiEvents.hh
+++ b/include/ignition/gazebo/gui/GuiEvents.hh
@@ -25,8 +25,11 @@
 #include <string>
 #include <utility>
 #include <vector>
+
 #include <ignition/math/Vector3.hh>
 #include <ignition/utils/ImplPtr.hh>
+#include <sdf/Element.hh>
+
 #include "ignition/gazebo/gui/Export.hh"
 #include "ignition/gazebo/Entity.hh"
 #include "ignition/gazebo/config.hh"
@@ -211,6 +214,27 @@ namespace events
     IGN_UTILS_IMPL_PTR(dataPtr)
   };
 
+  /// \brief Event that notifies a visual plugin is to be loaded
+  class IGNITION_GAZEBO_GUI_VISIBLE VisualPlugin: public QEvent
+  {
+    /// \brief Constructor
+    /// \param[in] _entity Visual entity id
+    /// \param[in] _elem Visual plugin SDF element
+    public: explicit VisualPlugin(ignition::gazebo::Entity _entity,
+                const sdf::ElementPtr &_elem);
+
+    /// \brief Get the entity to load the visual plugin for
+    public: ignition::gazebo::Entity Entity() const;
+
+    /// \brief Get the sdf element of the visual plugin
+    public: sdf::ElementPtr Element() const;
+
+    static const QEvent::Type kType = QEvent::Type(QEvent::User + 8);
+
+    /// \internal
+    /// \brief Private data pointer
+    IGN_UTILS_IMPL_PTR(dataPtr)
+  };
 }  // namespace events
 }
 }  // namespace gui
diff --git a/include/ignition/gazebo/rendering/Events.hh b/include/ignition/gazebo/rendering/Events.hh
index d17b3922c5..683c66f529 100644
--- a/include/ignition/gazebo/rendering/Events.hh
+++ b/include/ignition/gazebo/rendering/Events.hh
@@ -32,6 +32,18 @@ namespace ignition
     /// more information about events.
     namespace events
     {
+      /// \brief The render event is emitted when the the scene manager is
+      /// updated with contents from the ECM. This event is emitted
+      /// before the PreRender event on the server side in the rendering
+      /// thread. It is also accessible on the GUI side.
+      ///
+      /// For example:
+      /// \code
+      /// eventManager.Emit<ignition::gazebo::events::SceneUpdate>();
+      /// \endcode
+      using SceneUpdate = ignition::common::EventT<void(void),
+          struct SceneUpdateTag>;
+
       /// \brief The render event is emitted before rendering updates.
       /// The event is emitted in the rendering thread so rendering
       /// calls can ben make in this event callback
diff --git a/include/ignition/gazebo/rendering/RenderUtil.hh b/include/ignition/gazebo/rendering/RenderUtil.hh
index 6110b2083c..a757ec5378 100644
--- a/include/ignition/gazebo/rendering/RenderUtil.hh
+++ b/include/ignition/gazebo/rendering/RenderUtil.hh
@@ -214,6 +214,10 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
     /// \param[in] _active True if active.
     public: void SetTransformActive(bool _active);
 
+    /// \brief Set the event manager to use
+    /// \param[in] _mgr Event manager to set to.
+    public: void SetEventManager(EventManager *_mgr);
+
     /// \brief Private data pointer.
     private: std::unique_ptr<RenderUtilPrivate> dataPtr;
   };
diff --git a/src/SdfEntityCreator.cc b/src/SdfEntityCreator.cc
index ae98f41ec7..d54e1f377f 100644
--- a/src/SdfEntityCreator.cc
+++ b/src/SdfEntityCreator.cc
@@ -749,6 +749,17 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Visual *_visual)
         components::Material(*_visual->Material()));
   }
 
+  // store the plugin in a component
+  if (_visual->Element())
+  {
+    sdf::ElementPtr pluginElem =  _visual->Element()->FindElement("plugin");
+    if (pluginElem)
+    {
+      this->dataPtr->ecm->CreateComponent(visualEntity,
+          components::VisualPlugin(pluginElem));
+    }
+  }
+
   // Keep track of visuals so we can load their plugins after loading the
   // entire model and having its full scoped name.
   this->dataPtr->newVisuals[visualEntity] = _visual->Element();
diff --git a/src/SimulationRunner.hh b/src/SimulationRunner.hh
index a52210425e..5da194e209 100644
--- a/src/SimulationRunner.hh
+++ b/src/SimulationRunner.hh
@@ -495,9 +495,6 @@ namespace ignition
       /// \brief Mutex to protect pendingSystems
       private: mutable std::mutex pendingSystemsMutex;
 
-      /// \brief Systems implementing Configure
-      private: std::vector<ISystemConfigure *> systemsConfigure;
-
       /// \brief Systems implementing PreUpdate
       private: std::vector<ISystemPreUpdate *> systemsPreupdate;
 
diff --git a/src/SimulationRunner_TEST.cc b/src/SimulationRunner_TEST.cc
index cc9b33161a..17641a873c 100644
--- a/src/SimulationRunner_TEST.cc
+++ b/src/SimulationRunner_TEST.cc
@@ -1234,6 +1234,18 @@ TEST_P(SimulationRunnerTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(LoadPlugins) )
       });
   EXPECT_NE(kNullEntity, sensorId);
 
+  // Get visual entity
+  Entity visualId{kNullEntity};
+  runner.EntityCompMgr().Each<ignition::gazebo::components::Visual>([&](
+      const ignition::gazebo::Entity &_entity,
+      const ignition::gazebo::components::Visual *_visual)->bool
+      {
+        EXPECT_NE(nullptr, _visual);
+        visualId = _entity;
+        return true;
+      });
+  EXPECT_NE(kNullEntity, visualId);
+
   // Check component registered by world plugin
   std::string worldComponentName{"WorldPluginComponent"};
   auto worldComponentId = ignition::common::hash64(worldComponentName);
@@ -1258,6 +1270,14 @@ TEST_P(SimulationRunnerTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(LoadPlugins) )
   EXPECT_TRUE(runner.EntityCompMgr().EntityHasComponentType(sensorId,
       sensorComponentId));
 
+  // Check component registered by visual plugin
+  std::string visualComponentName{"VisualPluginComponent"};
+  auto visualComponentId = ignition::common::hash64(visualComponentName);
+
+  EXPECT_TRUE(runner.EntityCompMgr().HasComponentType(visualComponentId));
+  EXPECT_TRUE(runner.EntityCompMgr().EntityHasComponentType(visualId,
+      visualComponentId));
+
   // Clang re-registers components between tests. If we don't unregister them
   // beforehand, the new plugin tries to create a storage type from a previous
   // plugin, causing a crash.
@@ -1268,6 +1288,7 @@ TEST_P(SimulationRunnerTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(LoadPlugins) )
     components::Factory::Instance()->Unregister(worldComponentId);
     components::Factory::Instance()->Unregister(modelComponentId);
     components::Factory::Instance()->Unregister(sensorComponentId);
+    components::Factory::Instance()->Unregister(visualComponentId);
   #endif
 }
 
diff --git a/src/gui/GuiEvents.cc b/src/gui/GuiEvents.cc
index ed195212ed..a89a5d8254 100644
--- a/src/gui/GuiEvents.cc
+++ b/src/gui/GuiEvents.cc
@@ -50,6 +50,15 @@ class ignition::gazebo::gui::events::ModelEditorAddEntity::Implementation
   public: ignition::gazebo::Entity parent;
 };
 
+class ignition::gazebo::gui::events::VisualPlugin::Implementation
+{
+  /// \brief Entity to load the visual plugin for
+  public: ignition::gazebo::Entity entity;
+
+  /// \brief Sdf element of the visual plugin
+  public: sdf::ElementPtr element;
+};
+
 using namespace ignition;
 using namespace gazebo;
 using namespace gui;
@@ -132,3 +141,24 @@ QMap<QString, QString> &ModelEditorAddEntity::Data()
 {
   return this->dataPtr->data;
 }
+
+/////////////////////////////////////////////////
+VisualPlugin::VisualPlugin(ignition::gazebo::Entity _entity,
+    const sdf::ElementPtr &_elem) :
+    QEvent(kType), dataPtr(utils::MakeImpl<Implementation>())
+{
+  this->dataPtr->entity = _entity;
+  this->dataPtr->element = _elem;
+}
+
+/////////////////////////////////////////////////
+ignition::gazebo::Entity VisualPlugin::Entity() const
+{
+  return this->dataPtr->entity;
+}
+
+/////////////////////////////////////////////////
+sdf::ElementPtr VisualPlugin::Element() const
+{
+  return this->dataPtr->element;
+}
diff --git a/src/gui/GuiRunner.cc b/src/gui/GuiRunner.cc
index 50a2c3f075..2a70c033ac 100644
--- a/src/gui/GuiRunner.cc
+++ b/src/gui/GuiRunner.cc
@@ -15,6 +15,10 @@
  *
 */
 
+#include <memory>
+#include <utility>
+#include <vector>
+
 #include <ignition/common/Console.hh>
 #include <ignition/common/Profiler.hh>
 #include <ignition/fuel_tools/Interface.hh>
@@ -28,7 +32,9 @@
 #include "ignition/gazebo/components/components.hh"
 #include "ignition/gazebo/Conversions.hh"
 #include "ignition/gazebo/EntityComponentManager.hh"
+#include <ignition/gazebo/gui/GuiEvents.hh>
 #include "ignition/gazebo/gui/GuiSystem.hh"
+#include "ignition/gazebo/SystemLoader.hh"
 
 #include "GuiRunner.hh"
 
@@ -68,6 +74,31 @@ class ignition::gazebo::GuiRunner::Implementation
 
   /// \brief Name of WorldControl service
   public: std::string controlService;
+
+  /// \brief System loader for loading ign-gazebo systems
+  public: std::unique_ptr<SystemLoader> systemLoader;
+
+  /// \brief Mutex to protect systemLoader
+  public: std::mutex systemLoadMutex;
+
+  /// \brief Events containing visual plugins to load
+  public: std::vector<std::pair<ignition::gazebo::Entity, sdf::ElementPtr>>
+      visualPlugins;
+
+  /// \brief Systems implementing PreUpdate
+  public: std::vector<SystemPluginPtr> systems;
+
+  /// \brief Systems implementing PreUpdate
+  public: std::vector<ISystemPreUpdate *> systemsPreupdate;
+
+  /// \brief Systems implementing Update
+  public: std::vector<ISystemUpdate *> systemsUpdate;
+
+  /// \brief Systems implementing PostUpdate
+  public: std::vector<ISystemPostUpdate *> systemsPostupdate;
+
+  /// \brief Manager of all events.
+  public: EventManager eventMgr;
 };
 
 /////////////////////////////////////////////////
@@ -92,7 +123,7 @@ GuiRunner::GuiRunner(const std::string &_worldName)
   // so that an offset is not required
   this->dataPtr->ecm.SetEntityCreateOffset(math::MAX_I32 / 2);
 
-  auto win = gui::App()->findChild<ignition::gui::MainWindow *>();
+  auto win = ignition::gui::App()->findChild<ignition::gui::MainWindow *>();
   auto winWorldNames = win->property("worldNames").toStringList();
   winWorldNames.append(QString::fromStdString(_worldName));
   win->setProperty("worldNames", winWorldNames);
@@ -136,7 +167,7 @@ bool GuiRunner::eventFilter(QObject *_obj, QEvent *_event)
   if (_event->type() == ignition::gui::events::WorldControl::kType)
   {
     auto worldControlEvent =
-      reinterpret_cast<gui::events::WorldControl *>(_event);
+      reinterpret_cast<ignition::gui::events::WorldControl *>(_event);
     if (worldControlEvent)
     {
       msgs::WorldControlState req;
@@ -162,6 +193,20 @@ bool GuiRunner::eventFilter(QObject *_obj, QEvent *_event)
       this->dataPtr->node.Request(this->dataPtr->controlService, req, cb);
     }
   }
+  else if (_event->type() == ignition::gazebo::gui::events::VisualPlugin::kType)
+  {
+    auto visualPluginEvent =
+      reinterpret_cast<gui::events::VisualPlugin *>(_event);
+    if (visualPluginEvent)
+    {
+      std::lock_guard<std::mutex> lock(this->dataPtr->systemLoadMutex);
+
+      Entity entity = visualPluginEvent->Entity();
+      sdf::ElementPtr pluginElem = visualPluginEvent->Element();
+      this->dataPtr->visualPlugins.push_back(
+         std::make_pair(entity, pluginElem));
+    }
+  }
 
   // Standard event processing
   return QObject::eventFilter(_obj, _event);
@@ -171,7 +216,7 @@ bool GuiRunner::eventFilter(QObject *_obj, QEvent *_event)
 void GuiRunner::RequestState()
 {
   // set up service for async state response callback
-  std::string id = std::to_string(gui::App()->applicationPid());
+  std::string id = std::to_string(ignition::gui::App()->applicationPid());
   std::string reqSrv =
       this->dataPtr->node.Options().NameSpace() + "/" + id + "/state_async";
   auto reqSrvValid = transport::TopicUtils::AsValidTopic(reqSrv);
@@ -225,7 +270,7 @@ void GuiRunner::OnStateAsyncService(const msgs::SerializedStepMap &_res)
 
   // todo(anyone) store reqSrv string in a member variable and use it here
   // and in RequestState()
-  std::string id = std::to_string(gui::App()->applicationPid());
+  std::string id = std::to_string(ignition::gui::App()->applicationPid());
   std::string reqSrv =
       this->dataPtr->node.Options().NameSpace() + "/" + id + "/state_async";
   this->dataPtr->node.UnadvertiseSrv(reqSrv);
@@ -264,7 +309,8 @@ void GuiRunner::OnStateQt(const msgs::SerializedStepMap &_msg)
 /////////////////////////////////////////////////
 void GuiRunner::UpdatePlugins()
 {
-  auto plugins = gui::App()->findChildren<GuiSystem *>();
+  // gui plugins
+  auto plugins = ignition::gui::App()->findChildren<GuiSystem *>();
   for (auto plugin : plugins)
   {
     plugin->Update(this->dataPtr->updateInfo, this->dataPtr->ecm);
@@ -272,4 +318,92 @@ void GuiRunner::UpdatePlugins()
   this->dataPtr->ecm.ClearRemovedComponents();
   this->dataPtr->ecm.ClearNewlyCreatedEntities();
   this->dataPtr->ecm.ProcessRemoveEntityRequests();
+
+  // ign-gazebo systems
+  this->LoadSystems();
+  this->UpdateSystems();
+}
+
+/////////////////////////////////////////////////
+void GuiRunner::LoadSystems()
+{
+  std::lock_guard<std::mutex> lock(this->dataPtr->systemLoadMutex);
+  // currently only support systems that are visual plugins
+  for (auto &visualPlugin : this->dataPtr->visualPlugins)
+  {
+    Entity entity = visualPlugin.first;
+    sdf::ElementPtr pluginElem = visualPlugin.second;
+    auto filename = pluginElem->Get<std::string>("filename");
+    auto name = pluginElem->Get<std::string>("name");
+    if (filename != "__default__" && name != "__default__")
+    {
+      std::optional<SystemPluginPtr> system;
+      if (!this->dataPtr->systemLoader)
+        this->dataPtr->systemLoader = std::make_unique<SystemLoader>();
+      system = this->dataPtr->systemLoader->LoadPlugin(
+          filename, name, pluginElem);
+      if (system)
+      {
+        SystemPluginPtr sys = system.value();
+        this->dataPtr->systems.push_back(sys);
+        this->dataPtr->systemsPreupdate.push_back(
+            sys->QueryInterface<ISystemPreUpdate>());
+        this->dataPtr->systemsUpdate.push_back(
+            sys->QueryInterface<ISystemUpdate>());
+        this->dataPtr->systemsPostupdate.push_back(
+            sys->QueryInterface<ISystemPostUpdate>());
+
+        auto sysConfigure = sys->QueryInterface<ISystemConfigure>();
+        if (sysConfigure)
+        {
+          sysConfigure->Configure(entity, pluginElem, this->dataPtr->ecm,
+              this->dataPtr->eventMgr);
+        }
+        igndbg << "Loaded system [" << name
+               << "] for entity [" << entity << "] in GUI"
+               << std::endl;
+      }
+    }
+  }
+  this->dataPtr->visualPlugins.clear();
+}
+
+/////////////////////////////////////////////////
+void GuiRunner::UpdateSystems()
+{
+  IGN_PROFILE("GuiRunner::UpdateSystems");
+
+  {
+    IGN_PROFILE("PreUpdate");
+    for (auto& system : this->dataPtr->systemsPreupdate)
+    {
+      if (system)
+        system->PreUpdate(this->dataPtr->updateInfo, this->dataPtr->ecm);
+    }
+  }
+
+  {
+    IGN_PROFILE("Update");
+    for (auto& system : this->dataPtr->systemsUpdate)
+    {
+      if (system)
+        system->Update(this->dataPtr->updateInfo, this->dataPtr->ecm);
+    }
+  }
+
+  {
+    IGN_PROFILE("PostUpdate");
+    // \todo(anyone) Do PostUpdates in parallel
+    for (auto& system : this->dataPtr->systemsPostupdate)
+    {
+      if (system)
+        system->PostUpdate(this->dataPtr->updateInfo, this->dataPtr->ecm);
+    }
+  }
+}
+
+/////////////////////////////////////////////////
+EventManager &GuiRunner::GuiEventManager() const
+{
+  return this->dataPtr->eventMgr;
 }
diff --git a/src/gui/GuiRunner.hh b/src/gui/GuiRunner.hh
index 1a06098161..bc5a730553 100644
--- a/src/gui/GuiRunner.hh
+++ b/src/gui/GuiRunner.hh
@@ -25,6 +25,7 @@
 #include <ignition/utils/ImplPtr.hh>
 
 #include "ignition/gazebo/config.hh"
+#include "ignition/gazebo/EventManager.hh"
 #include "ignition/gazebo/gui/Export.hh"
 
 namespace ignition
@@ -46,6 +47,9 @@ class IGNITION_GAZEBO_GUI_VISIBLE GuiRunner : public QObject
   /// \brief Destructor
   public: ~GuiRunner() override;
 
+  /// \brief Get the event manager for the gui
+  public: EventManager &GuiEventManager() const;
+
   // Documentation inherited
   protected: bool eventFilter(QObject *_obj, QEvent *_event) override;
 
@@ -74,6 +78,12 @@ class IGNITION_GAZEBO_GUI_VISIBLE GuiRunner : public QObject
   /// \todo(anyone) Move to GuiRunner::Implementation when porting to v5
   private: Q_INVOKABLE void UpdatePlugins();
 
+  /// \brief Load systems
+  private: void LoadSystems();
+
+  /// \brief Update systems
+  private: void UpdateSystems();
+
   /// \brief Pointer to private data.
   IGN_UTILS_UNIQUE_IMPL_PTR(dataPtr)
 };
diff --git a/src/gui/plugins/scene_manager/GzSceneManager.cc b/src/gui/plugins/scene_manager/GzSceneManager.cc
index cccb6caaa4..48c0735f10 100644
--- a/src/gui/plugins/scene_manager/GzSceneManager.cc
+++ b/src/gui/plugins/scene_manager/GzSceneManager.cc
@@ -14,10 +14,15 @@
  * limitations under the License.
  *
 */
+
+#include <map>
 #include <set>
 
+#include "../../GuiRunner.hh"
 #include "GzSceneManager.hh"
 
+#include <sdf/Element.hh>
+
 #include <ignition/common/Profiler.hh>
 #include <ignition/gui/Application.hh>
 #include <ignition/gui/GuiEvents.hh>
@@ -28,6 +33,7 @@
 
 #include "ignition/gazebo/EntityComponentManager.hh"
 #include "ignition/gazebo/components/Name.hh"
+#include "ignition/gazebo/components/Visual.hh"
 #include "ignition/gazebo/components/World.hh"
 #include "ignition/gazebo/gui/GuiEvents.hh"
 #include "ignition/gazebo/rendering/RenderUtil.hh"
@@ -58,6 +64,9 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
     /// \brief Mutex to protect gui event and system upate call race conditions
     /// for newEntities and removedEntities
     public: std::mutex newRemovedEntityMutex;
+
+    /// \brief Indicates whether initial visual plugins have been loaded or not.
+    public: bool initializedVisualPlugins = false;
   };
 }
 }
@@ -102,6 +111,42 @@ void GzSceneManager::Update(const UpdateInfo &_info,
 
   this->dataPtr->renderUtil.UpdateFromECM(_info, _ecm);
 
+  // load visual plugin on gui side
+  std::map<Entity, sdf::ElementPtr> pluginElems;
+  if (!this->dataPtr->initializedVisualPlugins)
+  {
+    _ecm.Each<components::Visual, components::VisualPlugin>(
+        [&](const Entity &_entity,
+            const components::Visual *,
+            const components::VisualPlugin *_plugin)->bool
+    {
+      sdf::ElementPtr pluginElem = _plugin->Data();
+      pluginElems[_entity] = _plugin->Data();
+      return true;
+    });
+    this->dataPtr->initializedVisualPlugins = true;
+  }
+  else
+  {
+    _ecm.EachNew<components::Visual, components::VisualPlugin>(
+        [&](const Entity &_entity,
+            const components::Visual *,
+            const components::VisualPlugin *_plugin)->bool
+    {
+      sdf::ElementPtr pluginElem = _plugin->Data();
+      pluginElems[_entity] = _plugin->Data();
+      return true;
+    });
+  }
+  for (const auto &it : pluginElems)
+  {
+    ignition::gazebo::gui::events::VisualPlugin visualPluginEvent(
+        it.first, it.second);
+    ignition::gui::App()->sendEvent(
+        ignition::gui::App()->findChild<ignition::gui::MainWindow *>(),
+        &visualPluginEvent);
+  }
+
   // Emit entities created / removed event for gui::Plugins which don't have
   // direct access to the ECM.
   std::set<Entity> created;
@@ -163,6 +208,16 @@ void GzSceneManagerPrivate::OnRender()
       return;
 
     this->renderUtil.SetScene(this->scene);
+
+    auto runners = ignition::gui::App()->findChildren<GuiRunner *>();
+    if (runners.empty() || runners[0] == nullptr)
+    {
+      ignerr << "Internal error: no GuiRunner found." << std::endl;
+    }
+    else
+    {
+      this->renderUtil.SetEventManager(&runners[0]->GuiEventManager());
+    }
   }
 
   this->renderUtil.Update();
diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc
index 3df544c588..0679db3769 100644
--- a/src/rendering/RenderUtil.cc
+++ b/src/rendering/RenderUtil.cc
@@ -89,6 +89,7 @@
 #include "ignition/gazebo/components/World.hh"
 #include "ignition/gazebo/EntityComponentManager.hh"
 
+#include "ignition/gazebo/rendering/Events.hh"
 #include "ignition/gazebo/rendering/RenderUtil.hh"
 #include "ignition/gazebo/rendering/SceneManager.hh"
 #include "ignition/gazebo/rendering/MarkerManager.hh"
@@ -193,6 +194,9 @@ class ignition::gazebo::RenderUtilPrivate
       const components::Name *_name,
       const components::ParentEntity *_parent);
 
+  /// \brief Event manager used for emitting render / scene events
+  public: EventManager *eventManager{nullptr};
+
   /// \brief Total time elapsed in simulation. This will not increase while
   /// paused.
   public: std::chrono::steady_clock::duration simTime{0};
@@ -1583,6 +1587,9 @@ void RenderUtil::Update()
       }
     }
   }
+
+  if (this->dataPtr->eventManager)
+    this->dataPtr->eventManager->Emit<events::SceneUpdate>();
 }
 
 //////////////////////////////////////////////////
@@ -3637,3 +3644,9 @@ void RenderUtilPrivate::CreateLight(
   this->newLights.push_back(std::make_tuple(_entity, _light->Data(),
       _name->Data(), _parent->Data()));
 }
+
+//////////////////////////////////////////////////
+void RenderUtil::SetEventManager(EventManager *_mgr)
+{
+  this->dataPtr->eventManager = _mgr;
+}
diff --git a/src/systems/CMakeLists.txt b/src/systems/CMakeLists.txt
index 17514029f8..5054dad438 100644
--- a/src/systems/CMakeLists.txt
+++ b/src/systems/CMakeLists.txt
@@ -128,6 +128,7 @@ add_subdirectory(physics)
 add_subdirectory(pose_publisher)
 add_subdirectory(scene_broadcaster)
 add_subdirectory(sensors)
+add_subdirectory(shader_param)
 add_subdirectory(thermal)
 add_subdirectory(thruster)
 add_subdirectory(touch_plugin)
diff --git a/src/systems/sensors/Sensors.cc b/src/systems/sensors/Sensors.cc
index 5d802189b1..d5309a82c7 100644
--- a/src/systems/sensors/Sensors.cc
+++ b/src/systems/sensors/Sensors.cc
@@ -242,7 +242,6 @@ void SensorsPrivate::RunOnce()
     this->renderUtil.Update();
   }
 
-
   if (!this->activeSensors.empty())
   {
     this->sensorMaskMutex.lock();
@@ -422,8 +421,9 @@ void Sensors::Configure(const Entity &/*_id*/,
   this->dataPtr->renderUtil.SetEnableSensors(true,
       std::bind(&Sensors::CreateSensor, this,
       std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
-    this->dataPtr->renderUtil.SetRemoveSensorCb(
+  this->dataPtr->renderUtil.SetRemoveSensorCb(
       std::bind(&Sensors::RemoveSensor, this, std::placeholders::_1));
+  this->dataPtr->renderUtil.SetEventManager(&_eventMgr);
 
   // parse sensor-specific data
   auto worldEntity = _ecm.EntityByComponents(components::World());
diff --git a/src/systems/shader_param/CMakeLists.txt b/src/systems/shader_param/CMakeLists.txt
new file mode 100644
index 0000000000..0d14af33f3
--- /dev/null
+++ b/src/systems/shader_param/CMakeLists.txt
@@ -0,0 +1,8 @@
+gz_add_system(shader-param
+  SOURCES
+    ShaderParam.cc
+  PUBLIC_LINK_LIBS
+    ${rendering_target}
+    ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER}
+    ignition-rendering${IGN_RENDERING_VER}::ignition-rendering${IGN_RENDERING_VER}
+)
diff --git a/src/systems/shader_param/ShaderParam.cc b/src/systems/shader_param/ShaderParam.cc
new file mode 100644
index 0000000000..feab7709b2
--- /dev/null
+++ b/src/systems/shader_param/ShaderParam.cc
@@ -0,0 +1,396 @@
+/*
+ * Copyright (C) 2022 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 "ShaderParam.hh"
+
+#include <chrono>
+#include <list>
+#include <mutex>
+#include <vector>
+#include <string>
+
+#include <ignition/common/Profiler.hh>
+#include <ignition/plugin/Register.hh>
+#include <ignition/rendering/Material.hh>
+#include <ignition/rendering/RenderingIface.hh>
+#include <ignition/rendering/Scene.hh>
+#include <ignition/rendering/ShaderParams.hh>
+#include <ignition/rendering/Visual.hh>
+
+#include <sdf/Element.hh>
+
+#include "ignition/gazebo/components/Name.hh"
+#include "ignition/gazebo/components/SourceFilePath.hh"
+#include "ignition/gazebo/rendering/Events.hh"
+#include "ignition/gazebo/rendering/RenderUtil.hh"
+#include "ignition/gazebo/Util.hh"
+
+using namespace ignition;
+using namespace gazebo;
+using namespace systems;
+
+class ignition::gazebo::systems::ShaderParamPrivate
+{
+  /// \brief Data structure for storing shader param info
+  public: class ShaderParamValue
+  {
+    /// \brief shader type: vertex or fragment
+    public: std::string shader;
+
+    /// \brief variable type: int, float, float_array, int_array
+    /// \todo(anyone) support samplers
+    public: std::string type;
+
+    /// \brief variable name of param
+    public: std::string name;
+
+    /// \brief param value
+    public: std::string value;
+  };
+
+  /// \brief Path to vertex shader
+  public: std::string vertexShaderUri;
+
+  /// \brief Path to fragment shader
+  public: std::string fragmentShaderUri;
+
+  /// \brief Mutex to protect sim time updates.
+  public: std::mutex mutex;
+
+  /// \brief Connection to pre-render event callback
+  public: ignition::common::ConnectionPtr connection{nullptr};
+
+  /// \brief Name of visual this plugin is attached to
+  public: std::string visualName;
+
+  /// \brief Pointer to visual
+  public: rendering::VisualPtr visual;
+
+  /// \brief Material used by this visual
+  public: rendering::MaterialPtr material;
+
+  /// \brief Pointer to scene
+  public: rendering::ScenePtr scene;
+
+  /// \brief Entity id of the visual
+  public: Entity entity = kNullEntity;
+
+  /// \brief A list of shader params
+  public: std::vector<ShaderParamValue> shaderParams;
+
+  /// \brief Time params that will be updated every iteration
+  public: std::vector<ShaderParamValue> timeParams;
+
+  /// \brief Current sim time
+  public: std::chrono::steady_clock::duration currentSimTime;
+
+  /// \brief All rendering operations must happen within this call
+  public: void OnUpdate();
+};
+
+/////////////////////////////////////////////////
+ShaderParam::ShaderParam()
+    : System(), dataPtr(std::make_unique<ShaderParamPrivate>())
+{
+}
+
+/////////////////////////////////////////////////
+ShaderParam::~ShaderParam()
+{
+}
+
+/////////////////////////////////////////////////
+void ShaderParam::Configure(const Entity &_entity,
+               const std::shared_ptr<const sdf::Element> &_sdf,
+               EntityComponentManager &_ecm,
+               EventManager &_eventMgr)
+{
+  IGN_PROFILE("ShaderParam::Configure");
+  // Ugly, but needed because the sdf::Element::GetElement is not a const
+  // function and _sdf is a const shared pointer to a const sdf::Element.
+  auto sdf = const_cast<sdf::Element *>(_sdf.get());
+
+  if (sdf->HasElement("param"))
+  {
+    // loop and parse all shader params
+    sdf::ElementPtr paramElem = sdf->GetElement("param");
+    while (paramElem)
+    {
+      if (!paramElem->HasElement("shader") ||
+          !paramElem->HasElement("name"))
+      {
+        ignerr << "<param> must have <shader> and <name> sdf elements"
+               << std::endl;
+        paramElem = paramElem->GetNextElement("param");
+        continue;
+      }
+      std::string shaderType = paramElem->Get<std::string>("shader");
+      std::string paramName = paramElem->Get<std::string>("name");
+
+      std::string type = paramElem->Get<std::string>("type", "float").first;
+      std::string value = paramElem->Get<std::string>("value", "").first;
+
+      ShaderParamPrivate::ShaderParamValue spv;
+      spv.shader = shaderType;
+      spv.name = paramName;
+      spv.value = value;
+      spv.type = type;
+      this->dataPtr->shaderParams.push_back(spv);
+      paramElem = paramElem->GetNextElement("param");
+    }
+  }
+
+  // parse path to shaders
+  if (sdf->HasElement("shader"))
+  {
+    sdf::ElementPtr shaderElem = sdf->GetElement("shader");
+    if (!shaderElem->HasElement("vertex") ||
+        !shaderElem->HasElement("fragment"))
+    {
+      ignerr << "<shader> must have <vertex> and <fragment> sdf elements"
+             << std::endl;
+    }
+    else
+    {
+      auto modelEntity = topLevelModel(_entity, _ecm);
+      auto modelPath =
+          _ecm.ComponentData<components::SourceFilePath>(modelEntity);
+      sdf::ElementPtr vertexElem = shaderElem->GetElement("vertex");
+      this->dataPtr->vertexShaderUri = common::findFile(
+          asFullPath(vertexElem->Get<std::string>(), modelPath.value()));
+      sdf::ElementPtr fragmentElem = shaderElem->GetElement("fragment");
+      this->dataPtr->fragmentShaderUri = common::findFile(
+          asFullPath(fragmentElem->Get<std::string>(), modelPath.value()));
+    }
+  }
+
+  this->dataPtr->entity = _entity;
+  auto nameComp = _ecm.Component<components::Name>(_entity);
+  this->dataPtr->visualName = nameComp->Data();
+
+  // connect to the SceneUpdate event
+  // the callback is executed in the rendering thread so do all
+  // rendering operations in that thread
+  this->dataPtr->connection =
+      _eventMgr.Connect<ignition::gazebo::events::SceneUpdate>(
+      std::bind(&ShaderParamPrivate::OnUpdate, this->dataPtr.get()));
+}
+
+//////////////////////////////////////////////////
+void ShaderParam::PreUpdate(
+  const ignition::gazebo::UpdateInfo &_info,
+  ignition::gazebo::EntityComponentManager &)
+{
+  IGN_PROFILE("ShaderParam::PreUpdate");
+  std::lock_guard<std::mutex> lock(this->dataPtr->mutex);
+  this->dataPtr->currentSimTime = _info.simTime;
+}
+
+//////////////////////////////////////////////////
+void ShaderParamPrivate::OnUpdate()
+{
+  std::lock_guard<std::mutex> lock(this->mutex);
+  if (this->visualName.empty())
+    return;
+
+  if (!this->scene)
+    this->scene = rendering::sceneFromFirstRenderEngine();
+
+  if (!this->scene)
+    return;
+
+  if (!this->visual)
+  {
+    // this does a breadth first search for visual with the entity id
+    // \todo(anyone) provide a helper function in RenderUtil to search for
+    // visual by entity id?
+    auto rootVis = scene->RootVisual();
+    std::list<rendering::NodePtr> nodes;
+    nodes.push_back(rootVis);
+    while (!nodes.empty())
+    {
+      auto n = nodes.front();
+      nodes.pop_front();
+      if (n && n->HasUserData("gazebo-entity"))
+      {
+        // RenderUti stores gazebo-entity user data as int
+        // \todo(anyone) Change this to uint64_t in Ignition H?
+        auto variant = n->UserData("gazebo-entity");
+        const int *value = std::get_if<int>(&variant);
+        if (value && *value == static_cast<int>(this->entity))
+        {
+          this->visual = std::dynamic_pointer_cast<rendering::Visual>(n);
+          break;
+        }
+      }
+      for (unsigned int i = 0; i < n->ChildCount(); ++i)
+        nodes.push_back(n->ChildByIndex(i));
+    }
+  }
+
+  if (!this->visual)
+    return;
+
+  // get the material and set shaders
+  if (!this->material)
+  {
+    auto mat = scene->CreateMaterial();
+    mat->SetVertexShader(this->vertexShaderUri);
+    mat->SetFragmentShader(this->fragmentShaderUri);
+    this->visual->SetMaterial(mat);
+    scene->DestroyMaterial(mat);
+    this->material = this->visual->Material();
+  }
+
+  if (!this->material)
+    return;
+
+  // set the shader params read from SDF
+  // this is only done once
+  for (const auto & spv : this->shaderParams)
+  {
+    std::vector<std::string> values = common::split(spv.value, " ");
+
+    int intValue = 0;
+    float floatValue = 0;
+    std::vector<float> floatArrayValue;
+
+    rendering::ShaderParam::ParamType paramType =
+        rendering::ShaderParam::PARAM_NONE;
+
+    rendering::ShaderParamsPtr params;
+    if (spv.shader == "fragment")
+    {
+      params = this->material->FragmentShaderParams();
+    }
+    else if (spv.shader == "vertex")
+    {
+      params = this->material->VertexShaderParams();
+    }
+
+    // if no <value> is specified, this could be a constant
+    if (values.empty())
+    {
+      // \todo handle args for constants
+      (*params)[spv.name] = intValue;
+    }
+    // float / int
+    else if (values.size() == 1u)
+    {
+      std::string str = values[0];
+
+      // TIME is reserved keyword for sim time
+      if (str == "TIME")
+      {
+        this->timeParams.push_back(spv);
+        continue;
+      }
+
+      // if <type> is not empty, respect the specified type
+      if (!spv.type.empty())
+      {
+        if (spv.type == "int")
+        {
+          intValue = std::stoi(str);
+          paramType = rendering::ShaderParam::PARAM_INT;
+        }
+        else if (spv.type == "float")
+        {
+          floatValue = std::stof(str);
+          paramType = rendering::ShaderParam::PARAM_FLOAT;
+        }
+        else
+        {
+          // \todo(anyone) support texture samplers
+        }
+      }
+      // else do our best guess at what the type is
+      else
+      {
+        std::string::size_type sz;
+        int n = std::stoi(str, &sz);
+        if ( sz == str.size())
+        {
+          intValue = n;
+          paramType = rendering::ShaderParam::PARAM_INT;
+        }
+        else
+        {
+          floatValue = std::stof(str);
+          paramType = rendering::ShaderParam::PARAM_FLOAT;
+        }
+      }
+    }
+    // arrays
+    else
+    {
+      // int array
+      if (!spv.type.empty() && spv.type == "int_array")
+      {
+        for (const auto &v : values)
+          floatArrayValue.push_back(std::stoi(v));
+        paramType = rendering::ShaderParam::PARAM_INT_BUFFER;
+      }
+      // treat everything else as float_array
+      else
+      {
+        for (const auto &v : values)
+          floatArrayValue.push_back(std::stof(v));
+        paramType = rendering::ShaderParam::PARAM_FLOAT_BUFFER;
+      }
+    }
+
+    // set the params
+    if (paramType == rendering::ShaderParam::PARAM_INT)
+    {
+      (*params)[spv.name] = intValue;
+    }
+    else if (paramType == rendering::ShaderParam::PARAM_FLOAT)
+    {
+      (*params)[spv.name] = floatValue;
+    }
+    else if (paramType == rendering::ShaderParam::PARAM_INT_BUFFER ||
+        paramType == rendering::ShaderParam::PARAM_FLOAT_BUFFER)
+    {
+      (*params)[spv.name].InitializeBuffer(floatArrayValue.size());
+      float *fv = &floatArrayValue[0];
+      (*params)[spv.name].UpdateBuffer(fv);
+    }
+  }
+  this->shaderParams.clear();
+
+  // time variables need to be updated every iteration
+  for (const auto & spv : this->timeParams)
+  {
+    float floatValue = (std::chrono::duration_cast<std::chrono::nanoseconds>(
+        this->currentSimTime).count()) * 1e-9;
+    rendering::ShaderParamsPtr params;
+    if (spv.shader == "fragment")
+      params = this->material->FragmentShaderParams();
+    else if (spv.shader == "vertex")
+      params = this->material->VertexShaderParams();
+    (*params)[spv.name] = floatValue;
+  }
+}
+
+IGNITION_ADD_PLUGIN(ShaderParam,
+                    ignition::gazebo::System,
+                    ShaderParam::ISystemConfigure,
+                    ShaderParam::ISystemPreUpdate)
+
+IGNITION_ADD_PLUGIN_ALIAS(ShaderParam,
+  "ignition::gazebo::systems::ShaderParam")
diff --git a/src/systems/shader_param/ShaderParam.hh b/src/systems/shader_param/ShaderParam.hh
new file mode 100644
index 0000000000..d77e8bfd78
--- /dev/null
+++ b/src/systems/shader_param/ShaderParam.hh
@@ -0,0 +1,100 @@
+/*
+ * Copyright (C) 2022 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_SHADER_PARAM_HH_
+#define IGNITION_GAZEBO_SYSTEMS_SHADER_PARAM_HH_
+
+#include <memory>
+
+#include "ignition/gazebo/System.hh"
+
+namespace ignition
+{
+namespace gazebo
+{
+// Inline bracket to help doxygen filtering.
+inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
+namespace systems
+{
+  // Forward declaration
+  class ShaderParamPrivate;
+
+  /// \brief A plugin for setting shaders to a visual and its params
+  ///
+  /// Plugin parameters:
+  ///
+  /// <shader>
+  ///   <vertex>   Path to vertex program
+  ///   <fragment> Path to fragment program
+  /// <param>      Shader parameter - can be repeated within plugin SDF element
+  ///   <name>     Name of uniform variable bound to the shader
+  ///   <shader>   Type of shader, i.e. vertex, fragment
+  ///   <type>     Variable type: float, int, float_array, int_array
+  ///   <value>    Value to set the shader parameter to. The vallue string can
+  ///              be an int, float, or a space delimited array of ints or
+  ///              floats. It can also be 'TIME', in which case the value will
+  ///              be bound to sim time.
+  ///
+  /// Example usage:
+  ///
+  /// \verbatim
+  ///     <plugin filename="ignition-gazebo-shader-param-system"
+  ///             name="ignition::gazebo::systems::ShaderParam">
+  ///        <shader>
+  ///          <vertex>materials/my_vs.glsl</vertex>
+  ///          <fragment>materials/my_fs.glsl</fragment>
+  ///        </shader>
+  ///        <!-- Sets a fragment shader variable named "ambient" to red -->
+  ///        <param>
+  ///          <name>ambient</name>
+  ///          <shader>fragment</shader>
+  ///          <type>float_array</type>
+  ///          <value>1.0 0.0 0.0 1.0</value>
+  ///        </param>
+  ///    </plugin>
+  /// \endverbatim
+  class ShaderParam
+      : public System,
+        public ISystemConfigure,
+        public ISystemPreUpdate
+  {
+    /// \brief Constructor
+    public: ShaderParam();
+
+    /// \brief Destructor
+    public: ~ShaderParam() override;
+
+    // Documentation inherited
+    public: void Configure(const Entity &_entity,
+                           const std::shared_ptr<const sdf::Element> &_sdf,
+                           EntityComponentManager &_ecm,
+                           EventManager &_eventMgr) final;
+
+    /// Documentation inherited
+    public: void PreUpdate(
+                const ignition::gazebo::UpdateInfo &_info,
+                ignition::gazebo::EntityComponentManager &_ecm) override;
+
+    /// \brief Private data pointer
+    private: std::unique_ptr<ShaderParamPrivate> dataPtr;
+  };
+  }
+}
+}
+}
+
+#endif
diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt
index 6b712785a9..77197e73c3 100644
--- a/test/integration/CMakeLists.txt
+++ b/test/integration/CMakeLists.txt
@@ -76,6 +76,7 @@ set(tests_needing_display
   optical_tactile_plugin.cc
   rgbd_camera.cc
   sensors_system.cc
+  shader_param_system.cc
   thermal_system.cc
   thermal_sensor_system.cc
 )
diff --git a/test/integration/shader_param_system.cc b/test/integration/shader_param_system.cc
new file mode 100644
index 0000000000..906133e937
--- /dev/null
+++ b/test/integration/shader_param_system.cc
@@ -0,0 +1,110 @@
+/*
+ * Copyright (C) 2022 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 <gtest/gtest.h>
+
+#include <ignition/msgs/image.pb.h>
+
+#include <ignition/common/Console.hh>
+#include <ignition/common/Util.hh>
+#include <ignition/transport/Node.hh>
+#include <ignition/utilities/ExtraTestMacros.hh>
+
+#include "ignition/gazebo/Server.hh"
+#include "ignition/gazebo/test_config.hh"
+
+#include "../helpers/EnvTestFixture.hh"
+
+using namespace ignition;
+using namespace gazebo;
+using namespace std::chrono_literals;
+
+/// \brief Test ShaderParamTest system
+class ShaderParamTest : public InternalFixture<::testing::Test>
+{
+};
+
+std::mutex mutex;
+msgs::Image imageMsg;
+unsigned char *imageBuffer = nullptr;
+
+/////////////////////////////////////////////////
+void imageCb(const msgs::Image &_msg)
+{
+  mutex.lock();
+  unsigned int channels = 3u;
+  unsigned int imageSamples = _msg.width() * _msg.height();
+  unsigned int imageBufferSize = imageSamples * sizeof(unsigned char)
+      * channels;
+
+  if (!imageBuffer)
+    imageBuffer = new unsigned char[imageSamples * channels];
+  memcpy(imageBuffer, _msg.data().c_str(), imageBufferSize);
+  mutex.unlock();
+}
+
+/////////////////////////////////////////////////
+// The test checks camera image data to verify that the sphere is using
+// custom material shaders
+TEST_F(ShaderParamTest, IGN_UTILS_TEST_DISABLED_ON_MAC(ShaderParam))
+{
+  // Start server
+  ServerConfig serverConfig;
+  const auto sdfFile = common::joinPaths(std::string(PROJECT_SOURCE_PATH),
+    "examples", "worlds", "shader_param.sdf");
+  serverConfig.SetSdfFile(sdfFile);
+
+  Server server(serverConfig);
+  EXPECT_FALSE(server.Running());
+  EXPECT_FALSE(*server.Running(0));
+
+  // subscribe to the image camera topic
+  transport::Node node;
+  node.Subscribe("/camera", &imageCb);
+
+  // Run server and verify that we are receiving a message
+  // from the image camera
+  size_t iters100 = 100u;
+  server.Run(true, iters100, false);
+
+  int sleep{0};
+  int maxSleep{30};
+  while (imageBuffer == nullptr && sleep < maxSleep)
+  {
+    std::this_thread::sleep_for(100ms);
+    sleep++;
+  }
+  EXPECT_LT(sleep, maxSleep);
+  ASSERT_NE(imageBuffer, nullptr);
+
+  // shaders set the sphere color to red
+  unsigned int height = 320;
+  unsigned int width = 240;
+
+  int mid = (height / 2 * width * 3u) + (width / 2 - 1) * 3u;
+
+  // Lock access to buffer and don't release it
+  mutex.lock();
+  int r = static_cast<int>(imageBuffer[mid]);
+  int g = static_cast<int>(imageBuffer[mid+1]);
+  int b = static_cast<int>(imageBuffer[mid+2]);
+  EXPECT_GT(r, g);
+  EXPECT_GT(r, b);
+  EXPECT_EQ(g, b);
+
+  delete[] imageBuffer;
+}
diff --git a/test/plugins/CMakeLists.txt b/test/plugins/CMakeLists.txt
index 19306775b4..554589094a 100644
--- a/test/plugins/CMakeLists.txt
+++ b/test/plugins/CMakeLists.txt
@@ -13,6 +13,7 @@ set (test_plugins
   EventTriggerSystem
   TestModelSystem
   TestSensorSystem
+  TestVisualSystem
   TestWorldSystem
   MockSystem
   Null
diff --git a/test/plugins/TestVisualSystem.cc b/test/plugins/TestVisualSystem.cc
new file mode 100644
index 0000000000..9cc3741117
--- /dev/null
+++ b/test/plugins/TestVisualSystem.cc
@@ -0,0 +1,23 @@
+/*
+ * Copyright (C) 2022 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 "TestVisualSystem.hh"
+
+#include <ignition/plugin/Register.hh>
+
+IGNITION_ADD_PLUGIN(ignition::gazebo::TestVisualSystem,
+    ignition::gazebo::System,
+    ignition::gazebo::TestVisualSystem::ISystemConfigure)
diff --git a/test/plugins/TestVisualSystem.hh b/test/plugins/TestVisualSystem.hh
new file mode 100644
index 0000000000..e8cdf4a45f
--- /dev/null
+++ b/test/plugins/TestVisualSystem.hh
@@ -0,0 +1,81 @@
+/*
+ * Copyright (C) 2022 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_TEST_TESTVISUALSYSTEM_HH_
+#define IGNITION_GAZEBO_TEST_TESTVISUALSYSTEM_HH_
+
+#include <ignition/gazebo/components/Component.hh>
+#include <ignition/gazebo/components/Factory.hh>
+#include <ignition/gazebo/System.hh>
+#include <ignition/gazebo/config.hh>
+#include <ignition/transport/Node.hh>
+
+namespace ignition
+{
+namespace gazebo
+{
+inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
+namespace components
+{
+using VisualPluginComponent = components::Component<int,
+    class VisualPluginComponentTag>;
+IGN_GAZEBO_REGISTER_COMPONENT("VisualPluginComponent",
+    VisualPluginComponent)
+}
+}
+
+class TestVisualSystem :
+  public System,
+  public ISystemConfigure
+{
+  public: TestVisualSystem()
+        {
+          igndbg << "Constructing TestVisualSystem" << std::endl;
+        }
+
+  public: ~TestVisualSystem()
+        {
+          igndbg << "Destroying TestVisualSystem" << std::endl;
+        }
+
+  private: bool Service(msgs::StringMsg &_msg)
+        {
+          igndbg << "TestVisualSystem service called" << std::endl;
+          _msg.set_data("TestVisualSystem");
+          return true;
+        }
+
+  public: void Configure(const Entity &_entity,
+                         const std::shared_ptr<const sdf::Element> &_sdf,
+                         EntityComponentManager &_ecm,
+                         EventManager &/*_eventManager*/) override
+        {
+          igndbg << "Configuring TestVisualSystem" << std::endl;
+          auto value = _sdf->Get<int>("visual_key");
+          _ecm.CreateComponent(_entity,
+              components::VisualPluginComponent(value));
+
+          // Create a test service
+          this->node.Advertise("/test/service/visual",
+              &TestVisualSystem::Service, this);
+        }
+
+  private: transport::Node node;
+};
+}
+}
+
+#endif
diff --git a/test/worlds/plugins.sdf b/test/worlds/plugins.sdf
index 466215d186..d4668e294d 100644
--- a/test/worlds/plugins.sdf
+++ b/test/worlds/plugins.sdf
@@ -25,6 +25,13 @@
             <sensor_key>456</sensor_key>
           </plugin>
         </sensor>
+        <visual name="visual">
+          <plugin
+            filename="TestVisualSystem"
+            name="ignition::gazebo::TestVisualSystem">
+            <sensor_key>890</sensor_key>
+          </plugin>
+        </visual>
       </link>
     </model>
   </world>

From 986e30f411fba49f60111d708a7cf4cbf5c302a7 Mon Sep 17 00:00:00 2001
From: Ian Chen <ichen@osrfoundation.org>
Date: Thu, 3 Feb 2022 17:41:53 -0800
Subject: [PATCH 08/17] Limit thruster system's input thrust cmd (#1318)

* limit thrust cmd

Signed-off-by: Ian Chen <ichen@osrfoundation.org>

* check and error when max < min

Signed-off-by: Ian Chen <ichen@osrfoundation.org>
---
 src/systems/thruster/Thruster.cc | 31 +++++++++++++++++++++++++++----
 src/systems/thruster/Thruster.hh |  4 ++++
 test/integration/thruster.cc     | 22 ++++++++++++++++++++--
 test/worlds/thruster_pid.sdf     |  2 ++
 test/worlds/thruster_vel_cmd.sdf |  2 ++
 5 files changed, 55 insertions(+), 6 deletions(-)

diff --git a/src/systems/thruster/Thruster.cc b/src/systems/thruster/Thruster.cc
index 6211434bc9..885f5ca209 100644
--- a/src/systems/thruster/Thruster.cc
+++ b/src/systems/thruster/Thruster.cc
@@ -79,12 +79,12 @@ class ignition::gazebo::systems::ThrusterPrivateData
   /// and writes the angular velocity directly to the joint. default: false
   public: bool velocityControl = false;
 
-  /// \brief Maximum input force [N] for the propellerController, default: 1000N
-  /// TODO(chapulina) Make it configurable from SDF.
+  /// \brief Maximum input force [N] for the propellerController,
+  /// default: 1000N
   public: double cmdMax = 1000;
 
-  /// \brief Minimum input force [N] for the propellerController, default: 1000N
-  /// TODO(chapulina) Make it configurable from SDF.
+  /// \brief Minimum input force [N] for the propellerController,
+  /// default: -1000N
   public: double cmdMin = -1000;
 
   /// \brief Thrust coefficient relating the propeller angular velocity to the
@@ -206,6 +206,29 @@ void Thruster::Configure(
   enableComponent<components::WorldAngularVelocity>(_ecm,
       this->dataPtr->linkEntity);
 
+  double minThrustCmd = this->dataPtr->cmdMin;
+  double maxThrustCmd = this->dataPtr->cmdMax;
+  if (_sdf->HasElement("max_thrust_cmd"))
+  {
+    maxThrustCmd = _sdf->Get<double>("max_thrust_cmd");
+  }
+  if (_sdf->HasElement("min_thrust_cmd"))
+  {
+    minThrustCmd = _sdf->Get<double>("min_thrust_cmd");
+  }
+  if (maxThrustCmd < minThrustCmd)
+  {
+    ignerr << "<max_thrust_cmd> must be greater than or equal to "
+           << "<min_thrust_cmd>. Revert to using default values: "
+           << "min: " << this->dataPtr->cmdMin << ", "
+           << "max: " << this->dataPtr->cmdMax << std::endl;
+  }
+  else
+  {
+    this->dataPtr->cmdMax = maxThrustCmd;
+    this->dataPtr->cmdMin = minThrustCmd;
+  }
+
   if (_sdf->HasElement("velocity_control"))
   {
     this->dataPtr->velocityControl = _sdf->Get<bool>("velocity_control");
diff --git a/src/systems/thruster/Thruster.hh b/src/systems/thruster/Thruster.hh
index ec1a607d21..78eafb849b 100644
--- a/src/systems/thruster/Thruster.hh
+++ b/src/systems/thruster/Thruster.hh
@@ -64,6 +64,10 @@ namespace systems
   ///              no units, defaults to 0.0]
   /// - <d_gain> - Derivative gain for joint PID controller. [Optional,
   ///              no units, defaults to 0.0]
+  /// - <max_thrust_cmd> - Maximum thrust command. [Optional,
+  ///                      defaults to 1000N]
+  /// - <min_thrust_cmd> - Minimum thrust command. [Optional,
+  ///                      defaults to -1000N]
   ///
   /// ## Example
   /// An example configuration is installed with Gazebo. The example
diff --git a/test/integration/thruster.cc b/test/integration/thruster.cc
index af1ec159b8..d2333bc3f1 100644
--- a/test/integration/thruster.cc
+++ b/test/integration/thruster.cc
@@ -134,9 +134,24 @@ void ThrusterTest::TestWorld(const std::string &_world,
   EXPECT_LT(sleep, maxSleep);
   EXPECT_TRUE(pub.HasConnections());
 
-  double force{300.0};
+  // input force cmd - this should be capped to 0
+  double forceCmd{-1000.0};
   msgs::Double msg;
-  msg.set_data(force);
+  msg.set_data(forceCmd);
+  pub.Publish(msg);
+
+  // Check no movement
+  fixture.Server()->Run(true, 100, false);
+  std::this_thread::sleep_for(std::chrono::milliseconds(100));
+  EXPECT_DOUBLE_EQ(0.0, modelPoses.back().Pos().X());
+  EXPECT_EQ(100u, modelPoses.size());
+  EXPECT_EQ(100u, propellerAngVels.size());
+  modelPoses.clear();
+  propellerAngVels.clear();
+
+  // input force cmd this should be capped to 300
+  forceCmd = 1000.0;
+  msg.set_data(forceCmd);
   pub.Publish(msg);
 
   // Check movement
@@ -152,6 +167,9 @@ void ThrusterTest::TestWorld(const std::string &_world,
   EXPECT_EQ(100u * sleep, modelPoses.size());
   EXPECT_EQ(100u * sleep, propellerAngVels.size());
 
+  // max allowed force
+  double force{300.0};
+
   // F = m * a
   // s = a * t^2 / 2
   // F = m * 2 * s / t^2
diff --git a/test/worlds/thruster_pid.sdf b/test/worlds/thruster_pid.sdf
index c18becc7d0..540c73170b 100644
--- a/test/worlds/thruster_pid.sdf
+++ b/test/worlds/thruster_pid.sdf
@@ -104,6 +104,8 @@
         <thrust_coefficient>0.004</thrust_coefficient>
         <fluid_density>1000</fluid_density>
         <propeller_diameter>0.2</propeller_diameter>
+        <max_thrust_cmd>300</max_thrust_cmd>
+        <min_thrust_cmd>0</min_thrust_cmd>
       </plugin>
     </model>
 
diff --git a/test/worlds/thruster_vel_cmd.sdf b/test/worlds/thruster_vel_cmd.sdf
index f78236a8ca..1850eac8f4 100644
--- a/test/worlds/thruster_vel_cmd.sdf
+++ b/test/worlds/thruster_vel_cmd.sdf
@@ -103,6 +103,8 @@
         <fluid_density>950</fluid_density>
         <propeller_diameter>0.25</propeller_diameter>
         <velocity_control>true</velocity_control>
+        <max_thrust_cmd>300</max_thrust_cmd>
+        <min_thrust_cmd>0</min_thrust_cmd>
       </plugin>
     </model>
 

From 756fa13630c1dc8cf11ec0e6f3f1ae82a77b6b50 Mon Sep 17 00:00:00 2001
From: Ian Chen <ichen@osrfoundation.org>
Date: Fri, 4 Feb 2022 10:21:44 -0800
Subject: [PATCH 09/17] fix buoyancy test by increasing tol

Signed-off-by: Ian Chen <ichen@osrfoundation.org>
---
 test/integration/buoyancy.cc | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/test/integration/buoyancy.cc b/test/integration/buoyancy.cc
index 9b2ca5d40d..b41f9e000b 100644
--- a/test/integration/buoyancy.cc
+++ b/test/integration/buoyancy.cc
@@ -156,7 +156,8 @@ TEST_F(BuoyancyTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(RestoringMoments))
   EXPECT_NEAR(maxUniformRoll, 0.11, 1e-1);
   EXPECT_NEAR(minUniformRoll, minGradedRoll, 1e-1);
   // Emperically derived
-  EXPECT_NEAR(minUniformRoll, -0.15, 1e-1);
+  // added extra tol (3e-5) after fixing center of volume's reference frame
+  EXPECT_NEAR(minUniformRoll, -0.15, 1e-1 + 3e-5);
 }
 
 /////////////////////////////////////////////////

From 1ef2c62889460ea464be970f3bb4006d28374aac Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Mon, 7 Feb 2022 20:20:07 +0800
Subject: [PATCH 10/17] Fix weird indentation in `Link.hh` (#1324)

There was some weird indentation going on in the `Link.hh` header file. This PR fixes it.

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>
---
 include/ignition/gazebo/Link.hh | 14 +++++++-------
 1 file changed, 7 insertions(+), 7 deletions(-)

diff --git a/include/ignition/gazebo/Link.hh b/include/ignition/gazebo/Link.hh
index 094567951b..944a013dbb 100644
--- a/include/ignition/gazebo/Link.hh
+++ b/include/ignition/gazebo/Link.hh
@@ -220,13 +220,13 @@ namespace ignition
       public: void EnableVelocityChecks(EntityComponentManager &_ecm,
           bool _enable = true) const;
 
-       /// \brief Get the angular acceleration of the body in the world frame.
-       /// \param[in] _ecm Entity-component manager.
-       /// \return Angular acceleration of the body in the world frame or
-       /// nullopt if acceleration checks aren't enabled.
-       /// \sa EnableAccelerationChecks
-       public: std::optional<math::Vector3d> WorldAngularAcceleration(
-           const EntityComponentManager &_ecm) const;
+      /// \brief Get the angular acceleration of the body in the world frame.
+      /// \param[in] _ecm Entity-component manager.
+      /// \return Angular acceleration of the body in the world frame or
+      /// nullopt if acceleration checks aren't enabled.
+      /// \sa EnableAccelerationChecks
+      public: std::optional<math::Vector3d> WorldAngularAcceleration(
+          const EntityComponentManager &_ecm) const;
 
       /// \brief Get the linear acceleration of the body in the world frame.
       /// \param[in] _ecm Entity-component manager.

From 83f2f74c5e3da3b70fa0422192216b05d8d4814e Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Tue, 8 Feb 2022 10:27:29 +0800
Subject: [PATCH 11/17] Adds a `Link::SetLinearVelocity()` method (#1323)

This PR adds a Link::SetLinearVelocity() method. I foresee this method being useful for testing behaviour of systems like the hydrodynamics or liftDrag plugin which are dependent on velocity for their output forces.

Signed-off-by: Arjo Chakravarty arjo@openrobotics.org
---
 include/ignition/gazebo/Link.hh |  7 +++++++
 src/Link.cc                     | 20 +++++++++++++++++++
 test/integration/link.cc        | 34 +++++++++++++++++++++++++++++++++
 3 files changed, 61 insertions(+)

diff --git a/include/ignition/gazebo/Link.hh b/include/ignition/gazebo/Link.hh
index 944a013dbb..9dd3d5ccc7 100644
--- a/include/ignition/gazebo/Link.hh
+++ b/include/ignition/gazebo/Link.hh
@@ -220,6 +220,13 @@ namespace ignition
       public: void EnableVelocityChecks(EntityComponentManager &_ecm,
           bool _enable = true) const;
 
+      /// \brief Set the linear velocity on this link. If this is set, wrenches
+      /// on this link will be ignored for the current time step.
+      /// \param[in] _ecm Entity Component manager.
+      /// \param[in] _vel Linear velocity to set in Link's Frame.
+      public: void SetLinearVelocity(EntityComponentManager &_ecm,
+          const math::Vector3d &_vel) const;
+
       /// \brief Get the angular acceleration of the body in the world frame.
       /// \param[in] _ecm Entity-component manager.
       /// \return Angular acceleration of the body in the world frame or
diff --git a/src/Link.cc b/src/Link.cc
index 855adf4352..0d07f17140 100644
--- a/src/Link.cc
+++ b/src/Link.cc
@@ -26,6 +26,7 @@
 #include "ignition/gazebo/components/Joint.hh"
 #include "ignition/gazebo/components/LinearAcceleration.hh"
 #include "ignition/gazebo/components/LinearVelocity.hh"
+#include "ignition/gazebo/components/LinearVelocityCmd.hh"
 #include "ignition/gazebo/components/Link.hh"
 #include "ignition/gazebo/components/Model.hh"
 #include "ignition/gazebo/components/Name.hh"
@@ -246,6 +247,25 @@ void Link::EnableVelocityChecks(EntityComponentManager &_ecm, bool _enable)
       _enable);
 }
 
+//////////////////////////////////////////////////
+void Link::SetLinearVelocity(EntityComponentManager &_ecm,
+  const math::Vector3d &_vel) const
+{
+    auto vel =
+      _ecm.Component<components::LinearVelocityCmd>(this->dataPtr->id);
+
+    if (vel == nullptr)
+    {
+      _ecm.CreateComponent(
+          this->dataPtr->id,
+          components::LinearVelocityCmd(_vel));
+    }
+    else
+    {
+      vel->Data() = _vel;
+    }
+}
+
 //////////////////////////////////////////////////
 std::optional<math::Vector3d> Link::WorldAngularAcceleration(
     const EntityComponentManager &_ecm) const
diff --git a/test/integration/link.cc b/test/integration/link.cc
index da64000838..53a1dc9f7c 100644
--- a/test/integration/link.cc
+++ b/test/integration/link.cc
@@ -29,6 +29,7 @@
 #include <ignition/gazebo/components/Joint.hh>
 #include <ignition/gazebo/components/LinearAcceleration.hh>
 #include <ignition/gazebo/components/LinearVelocity.hh>
+#include <ignition/gazebo/components/LinearVelocityCmd.hh>
 #include <ignition/gazebo/components/Link.hh>
 #include <ignition/gazebo/components/Model.hh>
 #include <ignition/gazebo/components/Name.hh>
@@ -468,6 +469,39 @@ TEST_F(LinkIntegrationTest, LinkKineticEnergy)
   EXPECT_DOUBLE_EQ(100.0, *link.WorldKineticEnergy(ecm));
 }
 
+//////////////////////////////////////////////////
+TEST_F(LinkIntegrationTest, LinkSetVelocity)
+{
+  EntityComponentManager ecm;
+  EventManager eventMgr;
+  SdfEntityCreator creator(ecm, eventMgr);
+
+  auto eLink = ecm.CreateEntity();
+  ecm.CreateComponent(eLink, components::Link());
+
+  Link link(eLink);
+  EXPECT_EQ(eLink, link.Entity());
+
+  ASSERT_TRUE(link.Valid(ecm));
+
+  // No LinearVelocityCmd should exist by default
+  EXPECT_EQ(nullptr, ecm.Component<components::LinearVelocityCmd>(eLink));
+
+  math::Vector3d linVel(1, 0, 0);
+  link.SetLinearVelocity(ecm, linVel);
+
+  // LinearVelocity cmd should exist
+  EXPECT_NE(nullptr, ecm.Component<components::LinearVelocityCmd>(eLink));
+  EXPECT_EQ(linVel,
+    ecm.Component<components::LinearVelocityCmd>(eLink)->Data());
+
+  // Make sure the linear velocity is updated
+  math::Vector3d linVel2(0, 0, 0);
+  link.SetLinearVelocity(ecm, linVel2);
+  EXPECT_EQ(linVel2,
+    ecm.Component<components::LinearVelocityCmd>(eLink)->Data());
+}
+
 //////////////////////////////////////////////////
 TEST_F(LinkIntegrationTest, LinkAddWorldForce)
 {

From 77dd8551e942c6f2a9dfbd6360a51cab9185ebab Mon Sep 17 00:00:00 2001
From: Ian Chen <ichen@osrfoundation.org>
Date: Wed, 9 Feb 2022 13:42:39 -0800
Subject: [PATCH 12/17] Extend ShaderParam system to support textures (#1310)

updates the ShaderParam system with the ability to parse texture params and pass them to custom shaders

Signed-off-by: Ian Chen <ichen@osrfoundation.org>
---
 examples/worlds/shader_param.sdf        |  40 ++---
 src/systems/shader_param/ShaderParam.cc | 198 +++++++++++++++---------
 2 files changed, 131 insertions(+), 107 deletions(-)

diff --git a/examples/worlds/shader_param.sdf b/examples/worlds/shader_param.sdf
index db3d48ca78..1aab09d7d4 100644
--- a/examples/worlds/shader_param.sdf
+++ b/examples/worlds/shader_param.sdf
@@ -167,6 +167,8 @@ ShaderParam visual plugin over time.
     <scene>
       <ambient>1.0 1.0 1.0</ambient>
       <background>0.8 0.8 0.8</background>
+      <sky></sky>
+      <grid>false</grid>
     </scene>
 
     <light type="directional" name="sun">
@@ -177,43 +179,21 @@ ShaderParam visual plugin over time.
       <direction>-0.5 0.1 -0.9</direction>
     </light>
 
-    <model name="ground_plane">
-      <static>true</static>
-      <link name="link">
-        <collision name="collision">
-          <geometry>
-            <plane>
-              <normal>0 0 1</normal>
-              <size>100 100</size>
-            </plane>
-          </geometry>
-        </collision>
-        <visual name="visual">
-          <geometry>
-            <plane>
-              <normal>0 0 1</normal>
-              <size>100 100</size>
-            </plane>
-          </geometry>
-          <material>
-            <ambient>0.8 0.8 0.8 1</ambient>
-            <diffuse>0.8 0.8 0.8 1</diffuse>
-            <specular>0.8 0.8 0.8 1</specular>
-          </material>
-        </visual>
-      </link>
-    </model>
-
     <include>
-      <pose>0 0.0 0.5 0 0 0</pose>
       <name>deformable_sphere</name>
-      <pose>3 0 0 0 0 0</pose>
+      <pose>0 0 1.5 0 0 0</pose>
       <uri>https://fuel.ignitionrobotics.org/1.0/openrobotics/models/deformable_sphere</uri>
     </include>
 
+    <include>
+      <name>waves</name>
+      <pose>0 0 0 0 0 0</pose>
+      <uri>https://fuel.ignitionrobotics.org/1.0/openrobotics/models/waves</uri>
+    </include>
+
     <model name="camera">
       <static>true</static>
-      <pose>2.5 0 0.5 0 0.0 3.14</pose>
+      <pose>2.5 0 1.5 0 0.0 3.14</pose>
       <link name="link">
         <pose>0.05 0.05 0.05 0 0 0</pose>
         <collision name="collision">
diff --git a/src/systems/shader_param/ShaderParam.cc b/src/systems/shader_param/ShaderParam.cc
index feab7709b2..973a7cac08 100644
--- a/src/systems/shader_param/ShaderParam.cc
+++ b/src/systems/shader_param/ShaderParam.cc
@@ -51,8 +51,8 @@ class ignition::gazebo::systems::ShaderParamPrivate
     /// \brief shader type: vertex or fragment
     public: std::string shader;
 
-    /// \brief variable type: int, float, float_array, int_array
-    /// \todo(anyone) support samplers
+    /// \brief variable type: int, float, float_array, int_array,
+    /// texture, texture_cube
     public: std::string type;
 
     /// \brief variable name of param
@@ -60,6 +60,9 @@ class ignition::gazebo::systems::ShaderParamPrivate
 
     /// \brief param value
     public: std::string value;
+
+    /// \brief Any additional arguments
+    public: std::vector<std::string> args;
   };
 
   /// \brief Path to vertex shader
@@ -98,6 +101,9 @@ class ignition::gazebo::systems::ShaderParamPrivate
   /// \brief Current sim time
   public: std::chrono::steady_clock::duration currentSimTime;
 
+  /// \brief Path to model
+  public: std::string modelPath;
+
   /// \brief All rendering operations must happen within this call
   public: void OnUpdate();
 };
@@ -149,11 +155,29 @@ void ShaderParam::Configure(const Entity &_entity,
       spv.name = paramName;
       spv.value = value;
       spv.type = type;
+
+      if (paramElem->HasElement("arg"))
+      {
+        sdf::ElementPtr argElem = paramElem->GetElement("arg");
+        while (argElem)
+        {
+          spv.args.push_back(argElem->Get<std::string>());
+          argElem = argElem->GetNextElement("arg");
+        }
+      }
+
       this->dataPtr->shaderParams.push_back(spv);
       paramElem = paramElem->GetNextElement("param");
     }
   }
 
+  if (this->dataPtr->modelPath.empty())
+  {
+    auto modelEntity = topLevelModel(_entity, _ecm);
+    this->dataPtr->modelPath =
+        _ecm.ComponentData<components::SourceFilePath>(modelEntity).value();
+  }
+
   // parse path to shaders
   if (sdf->HasElement("shader"))
   {
@@ -166,15 +190,14 @@ void ShaderParam::Configure(const Entity &_entity,
     }
     else
     {
-      auto modelEntity = topLevelModel(_entity, _ecm);
-      auto modelPath =
-          _ecm.ComponentData<components::SourceFilePath>(modelEntity);
       sdf::ElementPtr vertexElem = shaderElem->GetElement("vertex");
       this->dataPtr->vertexShaderUri = common::findFile(
-          asFullPath(vertexElem->Get<std::string>(), modelPath.value()));
+          asFullPath(vertexElem->Get<std::string>(),
+          this->dataPtr->modelPath));
       sdf::ElementPtr fragmentElem = shaderElem->GetElement("fragment");
       this->dataPtr->fragmentShaderUri = common::findFile(
-          asFullPath(fragmentElem->Get<std::string>(), modelPath.value()));
+          asFullPath(fragmentElem->Get<std::string>(),
+          this->dataPtr->modelPath));
     }
   }
 
@@ -263,14 +286,12 @@ void ShaderParamPrivate::OnUpdate()
   // this is only done once
   for (const auto & spv : this->shaderParams)
   {
-    std::vector<std::string> values = common::split(spv.value, " ");
-
-    int intValue = 0;
-    float floatValue = 0;
-    std::vector<float> floatArrayValue;
-
-    rendering::ShaderParam::ParamType paramType =
-        rendering::ShaderParam::PARAM_NONE;
+    // TIME is reserved keyword for sim time
+    if (spv.value == "TIME")
+    {
+      this->timeParams.push_back(spv);
+      continue;
+    }
 
     rendering::ShaderParamsPtr params;
     if (spv.shader == "fragment")
@@ -283,92 +304,115 @@ void ShaderParamPrivate::OnUpdate()
     }
 
     // if no <value> is specified, this could be a constant
-    if (values.empty())
+    if (spv.value.empty())
+    {
+      // \todo handle args for constants in ign-rendering
+      (*params)[spv.name] = 1;
+      continue;
+    }
+
+    // handle texture params
+    if (spv.type == "texture")
     {
-      // \todo handle args for constants
-      (*params)[spv.name] = intValue;
+      unsigned int uvSetIndex = spv.args.empty() ? 0u :
+          static_cast<unsigned int>(std::stoul(spv.args[0]));
+      std::string texPath = common::findFile(
+          asFullPath(spv.value, this->modelPath));
+      (*params)[spv.name].SetTexture(texPath,
+          rendering::ShaderParam::ParamType::PARAM_TEXTURE, uvSetIndex);
     }
-    // float / int
-    else if (values.size() == 1u)
+    else if (spv.type == "texture_cube")
+    {
+      unsigned int uvSetIndex = spv.args.empty() ? 0u :
+          static_cast<unsigned int>(std::stoul(spv.args[0]));
+      std::string texPath = common::findFile(
+          asFullPath(spv.value, this->modelPath));
+      (*params)[spv.name].SetTexture(texPath,
+          rendering::ShaderParam::ParamType::PARAM_TEXTURE_CUBE, uvSetIndex);
+    }
+    // handle int, float, int_array, and float_array params
+    else
     {
-      std::string str = values[0];
+      std::vector<std::string> values = common::split(spv.value, " ");
 
-      // TIME is reserved keyword for sim time
-      if (str == "TIME")
-      {
-        this->timeParams.push_back(spv);
-        continue;
-      }
+      int intValue = 0;
+      float floatValue = 0;
+      std::vector<float> floatArrayValue;
 
-      // if <type> is not empty, respect the specified type
-      if (!spv.type.empty())
+      rendering::ShaderParam::ParamType paramType =
+          rendering::ShaderParam::PARAM_NONE;
+
+      // float / int
+      if (values.size() == 1u)
       {
-        if (spv.type == "int")
-        {
-          intValue = std::stoi(str);
-          paramType = rendering::ShaderParam::PARAM_INT;
-        }
-        else if (spv.type == "float")
+        std::string str = values[0];
+
+        // if <type> is not empty, respect the specified type
+        if (!spv.type.empty())
         {
-          floatValue = std::stof(str);
-          paramType = rendering::ShaderParam::PARAM_FLOAT;
+          if (spv.type == "int")
+          {
+            intValue = std::stoi(str);
+            paramType = rendering::ShaderParam::PARAM_INT;
+          }
+          else if (spv.type == "float")
+          {
+            floatValue = std::stof(str);
+            paramType = rendering::ShaderParam::PARAM_FLOAT;
+          }
         }
+        // else do our best guess at what the type is
         else
         {
-          // \todo(anyone) support texture samplers
+          std::string::size_type sz;
+          int n = std::stoi(str, &sz);
+          if ( sz == str.size())
+          {
+            intValue = n;
+            paramType = rendering::ShaderParam::PARAM_INT;
+          }
+          else
+          {
+            floatValue = std::stof(str);
+            paramType = rendering::ShaderParam::PARAM_FLOAT;
+          }
         }
       }
-      // else do our best guess at what the type is
+      // arrays
       else
       {
-        std::string::size_type sz;
-        int n = std::stoi(str, &sz);
-        if ( sz == str.size())
+        // int array
+        if (!spv.type.empty() && spv.type == "int_array")
         {
-          intValue = n;
-          paramType = rendering::ShaderParam::PARAM_INT;
+          for (const auto &v : values)
+            floatArrayValue.push_back(std::stoi(v));
+          paramType = rendering::ShaderParam::PARAM_INT_BUFFER;
         }
+        // treat everything else as float_array
         else
         {
-          floatValue = std::stof(str);
-          paramType = rendering::ShaderParam::PARAM_FLOAT;
+          for (const auto &v : values)
+            floatArrayValue.push_back(std::stof(v));
+          paramType = rendering::ShaderParam::PARAM_FLOAT_BUFFER;
         }
       }
-    }
-    // arrays
-    else
-    {
-      // int array
-      if (!spv.type.empty() && spv.type == "int_array")
+
+      // set the params
+      if (paramType == rendering::ShaderParam::PARAM_INT)
       {
-        for (const auto &v : values)
-          floatArrayValue.push_back(std::stoi(v));
-        paramType = rendering::ShaderParam::PARAM_INT_BUFFER;
+        (*params)[spv.name] = intValue;
       }
-      // treat everything else as float_array
-      else
+      else if (paramType == rendering::ShaderParam::PARAM_FLOAT)
       {
-        for (const auto &v : values)
-          floatArrayValue.push_back(std::stof(v));
-        paramType = rendering::ShaderParam::PARAM_FLOAT_BUFFER;
+        (*params)[spv.name] = floatValue;
+      }
+      else if (paramType == rendering::ShaderParam::PARAM_INT_BUFFER ||
+          paramType == rendering::ShaderParam::PARAM_FLOAT_BUFFER)
+      {
+        (*params)[spv.name].InitializeBuffer(floatArrayValue.size());
+        float *fv = &floatArrayValue[0];
+        (*params)[spv.name].UpdateBuffer(fv);
       }
-    }
-
-    // set the params
-    if (paramType == rendering::ShaderParam::PARAM_INT)
-    {
-      (*params)[spv.name] = intValue;
-    }
-    else if (paramType == rendering::ShaderParam::PARAM_FLOAT)
-    {
-      (*params)[spv.name] = floatValue;
-    }
-    else if (paramType == rendering::ShaderParam::PARAM_INT_BUFFER ||
-        paramType == rendering::ShaderParam::PARAM_FLOAT_BUFFER)
-    {
-      (*params)[spv.name].InitializeBuffer(floatArrayValue.size());
-      float *fv = &floatArrayValue[0];
-      (*params)[spv.name].UpdateBuffer(fv);
     }
   }
   this->shaderParams.clear();

From 49d0673f318fd1d29dc1a75e9be5de2f829217ab Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= <caguero@openrobotics.org>
Date: Sun, 13 Feb 2022 19:03:49 +0100
Subject: [PATCH 13/17] New trajectory follower system (#1332)
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

* New trajectory follower system

Signed-off-by: Carlos Agüero <caguero@openrobotics.org>
Signed-off-by: Ian Chen <ichen@osrfoundation.org>
---
 examples/worlds/shader_param.sdf              |   1 -
 examples/worlds/trajectory_follower.sdf       | 220 +++++++++++
 src/systems/CMakeLists.txt                    |   1 +
 .../trajectory_follower/CMakeLists.txt        |   6 +
 .../trajectory_follower/TrajectoryFollower.cc | 352 ++++++++++++++++++
 .../trajectory_follower/TrajectoryFollower.hh | 152 ++++++++
 6 files changed, 731 insertions(+), 1 deletion(-)
 create mode 100644 examples/worlds/trajectory_follower.sdf
 create mode 100644 src/systems/trajectory_follower/CMakeLists.txt
 create mode 100644 src/systems/trajectory_follower/TrajectoryFollower.cc
 create mode 100644 src/systems/trajectory_follower/TrajectoryFollower.hh

diff --git a/examples/worlds/shader_param.sdf b/examples/worlds/shader_param.sdf
index 1aab09d7d4..25e8b12f14 100644
--- a/examples/worlds/shader_param.sdf
+++ b/examples/worlds/shader_param.sdf
@@ -184,7 +184,6 @@ ShaderParam visual plugin over time.
       <pose>0 0 1.5 0 0 0</pose>
       <uri>https://fuel.ignitionrobotics.org/1.0/openrobotics/models/deformable_sphere</uri>
     </include>
-
     <include>
       <name>waves</name>
       <pose>0 0 0 0 0 0</pose>
diff --git a/examples/worlds/trajectory_follower.sdf b/examples/worlds/trajectory_follower.sdf
new file mode 100644
index 0000000000..128d0d1442
--- /dev/null
+++ b/examples/worlds/trajectory_follower.sdf
@@ -0,0 +1,220 @@
+<?xml version="1.0" ?>
+<!--
+  Ignition Gazebo trajectory follower plugin demo.
+-->
+<sdf version="1.6">
+  <world name="trajectory_follower">
+
+    <physics name="1ms" type="ignored">
+      <max_step_size>0.001</max_step_size>
+      <real_time_factor>1.0</real_time_factor>
+    </physics>
+    <plugin
+      filename="ignition-gazebo-physics-system"
+      name="ignition::gazebo::systems::Physics">
+    </plugin>
+    <plugin
+      filename="ignition-gazebo-user-commands-system"
+      name="ignition::gazebo::systems::UserCommands">
+    </plugin>
+    <plugin
+      filename="ignition-gazebo-scene-broadcaster-system"
+      name="ignition::gazebo::systems::SceneBroadcaster">
+    </plugin>
+
+    <light type="directional" name="sun">
+      <cast_shadows>true</cast_shadows>
+      <pose>0 0 10 0 0 0</pose>
+      <diffuse>1 1 1 1</diffuse>
+      <specular>0.5 0.5 0.5 1</specular>
+      <attenuation>
+        <range>1000</range>
+        <constant>0.9</constant>
+        <linear>0.01</linear>
+        <quadratic>0.001</quadratic>
+      </attenuation>
+      <direction>-0.5 0.1 -0.9</direction>
+    </light>
+
+    <model name="ground_plane">
+      <static>true</static>
+      <link name="link">
+        <collision name="collision">
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+        </collision>
+        <visual name="visual">
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+          <material>
+            <ambient>0.8 0.8 0.8 1</ambient>
+            <diffuse>0.8 0.8 0.8 1</diffuse>
+            <specular>0.8 0.8 0.8 1</specular>
+          </material>
+        </visual>
+      </link>
+    </model>
+
+    <model name="box">
+      <pose>2 0 0.5 0 0 0</pose>
+      <link name="box_link">
+        <inertial>
+          <inertia>
+            <ixx>0.16666</ixx>
+            <ixy>0</ixy>
+            <ixz>0</ixz>
+            <iyy>0.16666</iyy>
+            <iyz>0</iyz>
+            <izz>0.16666</izz>
+          </inertia>
+          <mass>1.0</mass>
+        </inertial>
+        <collision name="box_collision">
+          <geometry>
+            <box>
+              <size>1 1 1</size>
+            </box>
+          </geometry>
+        </collision>
+
+        <visual name="box_visual">
+          <geometry>
+            <box>
+              <size>1 1 1</size>
+            </box>
+          </geometry>
+          <material>
+            <ambient>1 0 0 1</ambient>
+            <diffuse>1 0 0 1</diffuse>
+            <specular>1 0 0 1</specular>
+          </material>
+        </visual>
+      </link>
+
+      <plugin
+        filename="ignition-gazebo-trajectory-follower-system"
+        name="ignition::gazebo::systems::TrajectoryFollower">
+        <link_name>box_link</link_name>
+        <loop>true</loop>
+        <force>10</force>
+        <torque>10</torque>
+        <waypoints>
+          <waypoint>2 0</waypoint>
+          <waypoint>7 0</waypoint>
+        </waypoints>
+      </plugin>
+
+    </model>
+
+    <model name="box_2">
+      <pose>0 2 0.5 0 0 1.5708</pose>
+      <link name="box_link">
+        <inertial>
+          <inertia>
+            <ixx>0.16666</ixx>
+            <ixy>0</ixy>
+            <ixz>0</ixz>
+            <iyy>0.16666</iyy>
+            <iyz>0</iyz>
+            <izz>0.16666</izz>
+          </inertia>
+          <mass>1.0</mass>
+        </inertial>
+        <collision name="box_collision">
+          <geometry>
+            <box>
+              <size>1 1 1</size>
+            </box>
+          </geometry>
+        </collision>
+
+        <visual name="box_visual">
+          <geometry>
+            <box>
+              <size>1 1 1</size>
+            </box>
+          </geometry>
+          <material>
+            <ambient>1 0 0 1</ambient>
+            <diffuse>1 0 0 1</diffuse>
+            <specular>1 0 0 1</specular>
+          </material>
+        </visual>
+      </link>
+
+      <plugin
+        filename="ignition-gazebo-trajectory-follower-system"
+        name="ignition::gazebo::systems::TrajectoryFollower">
+        <link_name>box_link</link_name>
+        <loop>true</loop>
+        <force>10</force>
+        <torque>10</torque>
+        <line>
+          <direction>0</direction>
+          <length>5</length>
+        </line>
+      </plugin>
+
+    </model>
+
+    <model name="box_3">
+      <pose>0 -2 0.5 0 0 -1.5708</pose>
+      <link name="box_link">
+        <inertial>
+          <inertia>
+            <ixx>0.16666</ixx>
+            <ixy>0</ixy>
+            <ixz>0</ixz>
+            <iyy>0.16666</iyy>
+            <iyz>0</iyz>
+            <izz>0.16666</izz>
+          </inertia>
+          <mass>1.0</mass>
+        </inertial>
+        <collision name="box_collision">
+          <geometry>
+            <box>
+              <size>1 1 1</size>
+            </box>
+          </geometry>
+        </collision>
+
+        <visual name="box_visual">
+          <geometry>
+            <box>
+              <size>1 1 1</size>
+            </box>
+          </geometry>
+          <material>
+            <ambient>1 0 0 1</ambient>
+            <diffuse>1 0 0 1</diffuse>
+            <specular>1 0 0 1</specular>
+          </material>
+        </visual>
+      </link>
+
+      <plugin
+        filename="ignition-gazebo-trajectory-follower-system"
+        name="ignition::gazebo::systems::TrajectoryFollower">
+        <link_name>box_link</link_name>
+        <loop>true</loop>
+        <force>10</force>
+        <torque>10</torque>
+        <line>
+          <direction>0</direction>
+          <length>5</length>
+        </line>
+      </plugin>
+
+    </model>
+
+  </world>
+</sdf>
diff --git a/src/systems/CMakeLists.txt b/src/systems/CMakeLists.txt
index 5054dad438..78ef2dc886 100644
--- a/src/systems/CMakeLists.txt
+++ b/src/systems/CMakeLists.txt
@@ -134,6 +134,7 @@ add_subdirectory(thruster)
 add_subdirectory(touch_plugin)
 add_subdirectory(track_controller)
 add_subdirectory(tracked_vehicle)
+add_subdirectory(trajectory_follower)
 add_subdirectory(triggered_publisher)
 add_subdirectory(user_commands)
 add_subdirectory(velocity_control)
diff --git a/src/systems/trajectory_follower/CMakeLists.txt b/src/systems/trajectory_follower/CMakeLists.txt
new file mode 100644
index 0000000000..419aa55429
--- /dev/null
+++ b/src/systems/trajectory_follower/CMakeLists.txt
@@ -0,0 +1,6 @@
+gz_add_system(trajectory-follower
+  SOURCES
+  TrajectoryFollower.cc
+  PUBLIC_LINK_LIBS
+    ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER}
+)
diff --git a/src/systems/trajectory_follower/TrajectoryFollower.cc b/src/systems/trajectory_follower/TrajectoryFollower.cc
new file mode 100644
index 0000000000..7ea2ccd6bb
--- /dev/null
+++ b/src/systems/trajectory_follower/TrajectoryFollower.cc
@@ -0,0 +1,352 @@
+/*
+ * Copyright (C) 2022 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 <string>
+#include <vector>
+
+#include <ignition/common/Profiler.hh>
+#include <ignition/math/Angle.hh>
+#include <ignition/math/Helpers.hh>
+#include <ignition/math/Pose3.hh>
+#include <ignition/math/Vector2.hh>
+#include <ignition/math/Vector3.hh>
+#include <ignition/plugin/Register.hh>
+#include <sdf/sdf.hh>
+
+#include "ignition/gazebo/components/Pose.hh"
+#include "ignition/gazebo/Link.hh"
+#include "ignition/gazebo/Model.hh"
+#include "ignition/gazebo/Util.hh"
+
+#include "TrajectoryFollower.hh"
+
+using namespace ignition;
+using namespace gazebo;
+using namespace systems;
+
+class ignition::gazebo::systems::TrajectoryFollowerPrivate
+{
+  /// \brief Initialize the plugin.
+  /// \param[in] _ecm Immutable reference to the EntityComponentManager.
+  /// \param[in] _sdf The SDF Element associated with this system plugin.
+  public: void Load(const EntityComponentManager &_ecm,
+                    const sdf::ElementPtr &_sdf);
+
+  /// \brief The link entity
+  public: ignition::gazebo::Link link;
+
+  /// \brief Model interface
+  public: Model model{kNullEntity};
+
+  /// \brief The initial pose of the model relative to the world frame.
+  public: ignition::math::Pose3<double> modelPose;
+
+  /// \brief True if the model should continue looping though the waypoints.
+  public: bool loopForever = false;
+
+  /// \brief Linear force to apply to the model in its X direction.
+  public: double forceToApply = 60;
+
+  /// \brief Torque to apply to the model to align it with the next goal.
+  public: double torqueToApply = 50;
+
+  /// \brief When the model is at this distance or closer we won't try to move.
+  /// Units are in meters.
+  public: double rangeTolerance = 0.5;
+
+  /// \brief When the model is at this angle or closer we won't try to rotate.
+  /// Units are in degrees.
+  public: double bearingTolerance = 2.0;
+
+  /// \brief Use to sample waypoints around a circle.
+  public: unsigned int numSamples = 8u;
+
+  /// \brief The next position to reach.
+  public: ignition::math::Vector3d nextGoal;
+
+  /// \brief Vector containing waypoints as 3D vectors of doubles representing
+  /// X Y, where X and Y are local (Gazebo) coordinates.
+  public: std::vector<ignition::math::Vector2d> localWaypoints;
+
+  /// \brief Initialization flag.
+  public: bool initialized{false};
+
+  /// \brief Copy of the sdf configuration used for this plugin
+  public: sdf::ElementPtr sdfConfig;
+};
+
+//////////////////////////////////////////////////
+void TrajectoryFollowerPrivate::Load(const EntityComponentManager &_ecm,
+    const sdf::ElementPtr &_sdf)
+{
+  // Parse required elements.
+  if (!_sdf->HasElement("link_name"))
+  {
+    ignerr << "No <link_name> specified" << std::endl;
+    return;
+  }
+
+  std::string linkName = _sdf->Get<std::string>("link_name");
+  this->link = Link(this->model.LinkByName(_ecm, linkName));
+  if (!this->link.Valid(_ecm))
+  {
+    ignerr << "Could not find link named [" << linkName
+           << "] in model" << std::endl;
+    return;
+  }
+
+  this->modelPose = ignition::gazebo::worldPose(this->link.Entity(), _ecm);
+  this->modelPose.Pos().Z() = 0;
+
+  // Parse the optional <waypoints> element.
+  if (_sdf->HasElement("waypoints"))
+  {
+    auto waypointsElem = _sdf->GetElement("waypoints");
+
+    // We need at least one waypoint
+    if (!waypointsElem->HasElement("waypoint"))
+    {
+      ignerr << "TrajectoryFollower: Unable to find <waypoints><waypoint> "
+            << "element in SDF." << std::endl;
+      return;
+    }
+    auto waypointElem = waypointsElem->GetElement("waypoint");
+    while (waypointElem)
+    {
+      ignition::math::Vector2d position =
+        waypointElem->Get<ignition::math::Vector2d>();
+
+      // Save the position.
+      this->localWaypoints.push_back(position);
+
+      // Print some debugging messages
+      igndbg << "Waypoint, Local: X = " << position.X()
+             << " Y = " << position.Y() << std::endl;
+
+      waypointElem = waypointElem->GetNextElement("waypoint");
+    }
+  }
+  // If no waypoints present, check for the <circle> element and parse.
+  else if (_sdf->HasElement("circle"))
+  {
+    igndbg << "Circle element activated" << std::endl;
+    auto circleElem = _sdf->GetElement("circle");
+
+    if (!circleElem->HasElement("radius"))
+    {
+      ignerr << "No <circle><radius> specified" << std::endl;
+      return;
+    }
+
+    // Parse the required <radius> field.
+    double radius = circleElem->Get<double>("radius");
+
+    // Get the current model position in global coordinates. Create
+    // local vectors that represent a path along a rough circle.
+    ignition::math::Vector2d position(this->modelPose.Pos().X(),
+                                      this->modelPose.Pos().Y());
+    double angle = 0;
+    ignition::math::Vector2d vec(radius, 0);
+    for (unsigned int i = 0u; i < this->numSamples; ++i)
+    {
+      // Add the local vector to the current position.
+      // Store global position as a waypoint.
+      this->localWaypoints.push_back(position + vec);
+      angle += 2 * IGN_PI / this->numSamples;
+      vec.Set(radius * cos(angle), radius * sin(angle));
+      igndbg << "Entered circle waypoint " << position + vec << std::endl;
+    }
+  }
+  // If no waypoints or circle, check for the <line> element and parse.
+  else if (_sdf->HasElement("line"))
+  {
+    auto lineElem = _sdf->GetElement("line");
+    // Parse the required <direction> field.
+    if (!lineElem->HasElement("direction"))
+    {
+      ignerr << "No <line><direction> specified" << std::endl;
+      return;
+    }
+    ignition::math::Angle direction =
+      lineElem->Get<ignition::math::Angle>("direction");
+
+    // Parse the required <length> field.
+    if (!lineElem->HasElement("length"))
+    {
+      ignerr << "No <line><length> specified" << std::endl;
+      return;
+    }
+    auto length = lineElem->Get<double>("length");
+
+    // Create a relative vector in the direction of "direction" and of
+    // length "length".
+    ignition::math::Vector3d lineVec(
+      length * cos(direction.Radian()),
+      length * sin(direction.Radian()), 0);
+    ignition::math::Vector2d position(this->modelPose.Pos().X(),
+                                      this->modelPose.Pos().Y());
+    // Add the initial model position and calculated endpoint as waypoints.
+    this->localWaypoints.push_back(position);
+    ignition::math::Vector3d p = this->modelPose.CoordPositionAdd(lineVec);
+    ignition::math::Vector2d p2D = {p.X(), p.Y()};
+    this->localWaypoints.push_back(p2D);
+    igndbg << "Entered line waypoints " << position << ", " << p2D << std::endl;
+  }
+
+  // Parse the optional <loop> element.
+  if (_sdf->HasElement("loop"))
+    this->loopForever = _sdf->Get<bool>("loop");
+
+  // Parse the optional <force> element.
+  if (_sdf->HasElement("force"))
+    this->forceToApply = _sdf->Get<double>("force");
+
+  // Parse the optional <torque> element.
+  if (_sdf->HasElement("torque"))
+    this->torqueToApply = _sdf->Get<double>("torque");
+
+  // Parse the optional <range_tolerance> element.
+  if (_sdf->HasElement("range_tolerance"))
+    this->rangeTolerance = _sdf->Get<double>("range_tolerance");
+
+  // Parse the optional <bearing_tolerance> element.
+  if (_sdf->HasElement("bearing_tolerance"))
+    this->bearingTolerance = _sdf->Get<double>("bearing_tolerance");
+
+  // If we have waypoints to visit, read the first one.
+  if (!this->localWaypoints.empty())
+  {
+    this->nextGoal =
+      {this->localWaypoints.front().X(), this->localWaypoints.front().Y(), 0};
+  }
+}
+
+//////////////////////////////////////////////////
+TrajectoryFollower::TrajectoryFollower()
+  : dataPtr(std::make_unique<TrajectoryFollowerPrivate>())
+{
+}
+
+//////////////////////////////////////////////////
+void TrajectoryFollower::Configure(const Entity &_entity,
+    const std::shared_ptr<const sdf::Element> &_sdf,
+    EntityComponentManager &/*_ecm*/,
+    EventManager &/*_eventMgr*/)
+{
+  this->dataPtr->model = Model(_entity);
+  this->dataPtr->sdfConfig = _sdf->Clone();
+}
+
+//////////////////////////////////////////////////
+void TrajectoryFollower::PreUpdate(
+    const ignition::gazebo::UpdateInfo &_info,
+    ignition::gazebo::EntityComponentManager &_ecm)
+{
+  IGN_PROFILE("TrajectoryFollower::PreUpdate");
+
+  if (_info.paused)
+    return;
+
+  if (!this->dataPtr->initialized)
+  {
+    // We call Load here instead of Configure because we can't be guaranteed
+    // that all entities have been created when Configure is called
+    this->dataPtr->Load(_ecm, this->dataPtr->sdfConfig);
+    this->dataPtr->initialized = true;
+  }
+
+  // Nothing to do.
+  if (this->dataPtr->localWaypoints.empty())
+    return;
+
+  this->dataPtr->modelPose = ignition::gazebo::worldPose(
+    this->dataPtr->link.Entity(), _ecm);
+  this->dataPtr->modelPose.Pos().Z() = 0;
+
+  // Direction vector to the goal from the model.
+  ignition::math::Vector3d direction =
+    this->dataPtr->nextGoal - this->dataPtr->modelPose.Pos();
+
+  // Direction vector in the local frame of the model.
+  ignition::math::Vector3d directionLocalFrame =
+    this->dataPtr->modelPose.Rot().RotateVectorReverse(direction);
+
+  double range = directionLocalFrame.Length();
+  ignition::math::Angle bearing(
+    atan2(directionLocalFrame.Y(), directionLocalFrame.X()));
+  bearing.Normalize();
+
+  // Waypoint reached!
+  if (range <= this->dataPtr->rangeTolerance)
+  {
+    // We always keep the last waypoint in the vector to keep the model
+    // "alive" in case it moves away from its goal.
+    if (this->dataPtr->localWaypoints.size() == 1)
+      return;
+
+    if (this->dataPtr->loopForever)
+    {
+      // Rotate to the left.
+      std::rotate(this->dataPtr->localWaypoints.begin(),
+                  this->dataPtr->localWaypoints.begin() + 1,
+                  this->dataPtr->localWaypoints.end());
+    }
+    else
+    {
+      // Remove the first waypoint.
+      this->dataPtr->localWaypoints.erase(
+        this->dataPtr->localWaypoints.begin());
+    }
+
+    this->dataPtr->nextGoal = {
+      this->dataPtr->localWaypoints.front().X(),
+      this->dataPtr->localWaypoints.front().Y(), 0};
+
+    return;
+  }
+
+  // Transform from world to local frame.
+  auto comPose = this->dataPtr->link.WorldInertialPose(_ecm);
+
+  // Transform the force and torque to the world frame.
+  // Move commands. The vehicle always move forward (X direction).
+  ignition::math::Vector3d forceWorld;
+  if (std::abs(bearing.Degree()) <= this->dataPtr->bearingTolerance)
+  {
+    forceWorld = (*comPose).Rot().RotateVector(
+      ignition::math::Vector3d(this->dataPtr->forceToApply, 0, 0));
+  }
+
+  ignition::math::Vector3d torqueWorld;
+  if (std::abs(bearing.Degree()) > this->dataPtr->bearingTolerance)
+  {
+    int sign = std::abs(bearing.Degree()) / bearing.Degree();
+    torqueWorld = (*comPose).Rot().RotateVector(
+       ignition::math::Vector3d(0, 0, sign * this->dataPtr->torqueToApply));
+  }
+
+  // Apply the force and torque at COM.
+  this->dataPtr->link.AddWorldWrench(_ecm, forceWorld, torqueWorld);
+}
+
+IGNITION_ADD_PLUGIN(TrajectoryFollower,
+                    ignition::gazebo::System,
+                    TrajectoryFollower::ISystemConfigure,
+                    TrajectoryFollower::ISystemPreUpdate)
+
+IGNITION_ADD_PLUGIN_ALIAS(TrajectoryFollower,
+                          "ignition::gazebo::systems::TrajectoryFollower")
diff --git a/src/systems/trajectory_follower/TrajectoryFollower.hh b/src/systems/trajectory_follower/TrajectoryFollower.hh
new file mode 100644
index 0000000000..c1d52aca06
--- /dev/null
+++ b/src/systems/trajectory_follower/TrajectoryFollower.hh
@@ -0,0 +1,152 @@
+/*
+ * Copyright (C) 2022 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_TRAJECTORYFOLLOWER_HH_
+#define IGNITION_GAZEBO_SYSTEMS_TRAJECTORYFOLLOWER_HH_
+
+#include <ignition/gazebo/System.hh>
+#include <memory>
+
+namespace ignition
+{
+namespace gazebo
+{
+// Inline bracket to help doxygen filtering.
+inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
+namespace systems
+{
+  // Forward declaration
+  class TrajectoryFollowerPrivate;
+
+  /// \brief A plugin that scripts the movement of a model based on waypoints.
+  /// Note that at this point, the trajectory is always in 2D (x, y).
+  /// The plugin applies a torque until the model is aligned with the next
+  /// waypoint. Then, it applies a force until the model is close enough to
+  /// the waypoint.
+  ///
+  /// This plugin loads a set of waypoints via SDF and traverse the waypoints
+  /// in sequence. The movement is generated applying force and torque to one of
+  /// of the model links. It's also possible to loop through the waypoints for
+  /// generating a never ending trajectory.
+  /// Waypoints may be inserted manually via the <waypoints> element, or
+  /// generated relative to the model's initial position via the <line> or
+  /// <circle> elements.  Only one of these options should be used.
+  ///
+  /// This plugin requires the following SDF parameters:
+  /// * Required parameters:
+  /// <link_name>: The name of the link within the model where the force/torque
+  ///              will be applied when moving the vehicle.
+  ///
+  /// * Optional parameters:
+  /// <loop>: When true, all waypoints will be visited continously in a
+  ///         circular pattern. If false, the model will stop when the
+  ///         last waypoint is reached. Note, that if the vehicle moves,
+  ///         it will still try to reach the very last waypoint.
+  /// <waypoints>: Element specifying the set of waypoints that the
+  ///              the model should navigate through. This block should contain
+  ///              at least one of these blocks:
+  ///                <waypoint>: This block should contain the X, Y of a
+  ///                            waypoint.
+  /// <range_tolerance>: The current waypoint is considered reached if the
+  ///                    distance to it is within +- this tolerance (m).
+  ///                    The default value is 0.5m.
+  /// <bearing_tolerance>: If the bearing to the current waypoint is within
+  ///                      +- this tolerance, a torque won't be applied (degree)
+  ///                      The default value is 2 deg.
+  /// <force>: The force to apply at every plugin iteration in the X direction
+  ///          of the link (N). The default value is 60.
+  /// <torque>: The torque to apply at every plugin iteration in the Yaw
+  ///           direction of the link (Nm). The default value is 50.
+  /// <line>: Element that indicates the model should travel in "line" mode.
+  ///         The block should contain the relative direction and distance from
+  ///         the initial position in which the vehicle should move, specified
+  ///         in the world frame.
+  ///           <direction>: Relative direction (radians) in the world frame for
+  ///                        the vehicle to travel.
+  ///           <length>: Distance (meters) for the vehicle to travel.
+  /// <circle>: Element that indicates the model should travel in "circle" mode.
+  ///           The block should contain the desired radius of the circle about
+  ///           the vehicle's initial position
+  ///             <radius>: Radius (meters) of circular path to travel.
+  ///
+  /// Here are three examples:
+  // <plugin
+  //   filename="ignition-gazebo-trajectory-follower-system"
+  //   name="ignition::gazebo::systems::TrajectoryFollower">
+  //   <link_name>base_link</link_name>
+  //   <loop>true</loop>
+  //   <waypoints>
+  //     <waypoint>25 0</waypoint>
+  //     <waypoint>15 0</waypoint>
+  //   </waypoints>
+  // </plugin>
+  // <plugin
+  //   filename="ignition-gazebo-trajectory-follower-system"
+  //   name="ignition::gazebo::systems::TrajectoryFollower">
+  //   <link_name>base_link</link_name>
+  //   <loop>true</loop>
+  //   <line>
+  //     <direction>0</direction>
+  //     <length>5</length>
+  //   </line>
+  // </plugin>
+  // <plugin
+  //   filename="ignition-gazebo-trajectory-follower-system"
+  //   name="ignition::gazebo::systems::TrajectoryFollower">
+  //   <link_name>base_link</link_name>
+  //   <loop>true</loop>
+  //   <circle>
+  //     <radius>2</radius>
+  //   </circle>
+  // </plugin>
+  class TrajectoryFollower
+      : public System,
+        public ISystemConfigure,
+        public ISystemPreUpdate
+  {
+    /// \brief Constructor
+    public: TrajectoryFollower();
+
+    /// \brief Destructor
+    public: ~TrajectoryFollower() override = default;
+
+    // Documentation inherited
+    public: void Configure(const Entity &_entity,
+                           const std::shared_ptr<const sdf::Element> &_sdf,
+                           EntityComponentManager &_ecm,
+                           EventManager &_eventMgr) override;
+
+    // Documentation inherited
+    public: void PreUpdate(
+                const ignition::gazebo::UpdateInfo &_info,
+                ignition::gazebo::EntityComponentManager &_ecm) override;
+
+    /// \brief Check if an entity is enabled or not.
+    /// \param[in] _entity Target entity
+    /// \param[in] _ecm Entity component manager
+    /// \return True if buoyancy should be applied.
+    public: bool IsEnabled(Entity _entity,
+        const EntityComponentManager &_ecm) const;
+
+    /// \brief Private data pointer
+    private: std::unique_ptr<TrajectoryFollowerPrivate> dataPtr;
+  };
+  }
+}
+}
+}
+
+#endif

From 7901903d06a24b9a62d2e584241cfd4d95798f39 Mon Sep 17 00:00:00 2001
From: Jose Luis Rivero <jrivero@osrfoundation.org>
Date: Mon, 14 Feb 2022 23:56:57 +0100
Subject: [PATCH 14/17] Prepare for releasing 6.5.0 (#1338)

* Update changelog
* Bump to 6.5.0

Signed-off-by: Jose Luis Rivero <jrivero@osrfoundation.org>
---
 CMakeLists.txt |  2 +-
 Changelog.md   | 67 ++++++++++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 68 insertions(+), 1 deletion(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index ab770aa945..eaf570f512 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -3,7 +3,7 @@ cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR)
 #============================================================================
 # Initialize the project
 #============================================================================
-project(ignition-gazebo6 VERSION 6.4.0)
+project(ignition-gazebo6 VERSION 6.5.0)
 
 #============================================================================
 # Find ignition-cmake
diff --git a/Changelog.md b/Changelog.md
index b2931c172b..36bfd47980 100644
--- a/Changelog.md
+++ b/Changelog.md
@@ -2,6 +2,73 @@
 
 ### Ignition Gazebo 6.X.X (202X-XX-XX)
 
+### Ignition Gazebo 6.5.0 (202X-XX-XX)
+1. New trajectory follower system
+    * [Pull request #1332](https://github.com/ignitionrobotics/ign-gazebo/pull/1332)
+
+1. Extend ShaderParam system to support textures
+    * [Pull request #1310](https://github.com/ignitionrobotics/ign-gazebo/pull/1310)
+
+1. Adds a `Link::SetLinearVelocity()` method
+    * [Pull request #1323](https://github.com/ignitionrobotics/ign-gazebo/pull/1323)
+
+1. Fix weird indentation in `Link.hh`
+    * [Pull request #1324](https://github.com/ignitionrobotics/ign-gazebo/pull/1324)
+
+1. Limit thruster system's input thrust cmd
+    * [Pull request #1318](https://github.com/ignitionrobotics/ign-gazebo/pull/1318)
+
+1. Load and run visual plugin (system) on GUI side
+    * [Pull request #1275](https://github.com/ignitionrobotics/ign-gazebo/pull/1275)
+
+1. Log an error if JointPositionController cannot find the joint. (citadel retarget)
+    * [Pull request #1314](https://github.com/ignitionrobotics/ign-gazebo/pull/1314)
+
+1. Update source install instructions
+    * [Pull request #1311](https://github.com/ignitionrobotics/ign-gazebo/pull/1311)
+
+1. Document the `<topic>` option for JointPositionController.
+    * [Pull request #1309](https://github.com/ignitionrobotics/ign-gazebo/pull/1309)
+
+1. Fix typo in EntityComponentManager
+    * [Pull request #1304](https://github.com/ignitionrobotics/ign-gazebo/pull/1304)
+
+1. Buoyancy: fix center of volume's reference frame
+    * [Pull request #1302](https://github.com/ignitionrobotics/ign-gazebo/pull/1302)
+
+1. Fix graded buoyancy problems
+    * [Pull request #1297](https://github.com/ignitionrobotics/ign-gazebo/pull/1297)
+
+1. Add surface to buoyancy engine. (retarget fortress)
+    * [Pull request #1298](https://github.com/ignitionrobotics/ign-gazebo/pull/1298)
+
+1. Remove EachNew calls from sensor PreUpdates
+    * [Pull request #1281](https://github.com/ignitionrobotics/ign-gazebo/pull/1281)
+
+1. Prevent GzScene3D 💥 if another scene is already loaded
+    * [Pull request #1294](https://github.com/ignitionrobotics/ign-gazebo/pull/1294)
+
+1. Fix various typos on API documentation
+    * [Pull request #1291](https://github.com/ignitionrobotics/ign-gazebo/pull/1291)
+
+1. Optional orientation when spawning entity using spherical coordinates
+    * [Pull request #1263](https://github.com/ignitionrobotics/ign-gazebo/pull/1263)
+
+1. Cleanup update call for non-rendering sensors
+    * [Pull request #1282](https://github.com/ignitionrobotics/ign-gazebo/pull/1282)
+
+1. Documentation Error
+    * [Pull request #1285](https://github.com/ignitionrobotics/ign-gazebo/pull/1285)
+
+1. Min and max parameters for velocity, acceleration, and jerk apply to linear and angular separately.
+    * [Pull request #1229](https://github.com/ignitionrobotics/ign-gazebo/pull/1229)
+
+1. Add project() call to examples
+    * [Pull request #1274](https://github.com/ignitionrobotics/ign-gazebo/pull/1274)
+
+1. Implement /server_control::stop
+    * [Pull request #1240](https://github.com/ignitionrobotics/ign-gazebo/pull/1240)
+
 ### Ignition Gazebo 6.4.0 (2021-01-13)
 
 1. Disable more tests on Windows

From 7313edea5a1ab75933cf23e40af8eef7ae94ea45 Mon Sep 17 00:00:00 2001
From: Louise Poubel <louise@openrobotics.org>
Date: Wed, 9 Mar 2022 13:42:55 -0800
Subject: [PATCH 15/17] Enable WorldPose component on TrajectoryFollower
 (#1380)

Signed-off-by: Louise Poubel <louise@openrobotics.org>
---
 src/systems/trajectory_follower/TrajectoryFollower.cc | 7 +++++++
 1 file changed, 7 insertions(+)

diff --git a/src/systems/trajectory_follower/TrajectoryFollower.cc b/src/systems/trajectory_follower/TrajectoryFollower.cc
index 7ea2ccd6bb..85c804e0d7 100644
--- a/src/systems/trajectory_follower/TrajectoryFollower.cc
+++ b/src/systems/trajectory_follower/TrajectoryFollower.cc
@@ -266,6 +266,7 @@ void TrajectoryFollower::PreUpdate(
     // We call Load here instead of Configure because we can't be guaranteed
     // that all entities have been created when Configure is called
     this->dataPtr->Load(_ecm, this->dataPtr->sdfConfig);
+    enableComponent<components::WorldPose>(_ecm, this->dataPtr->link.Entity());
     this->dataPtr->initialized = true;
   }
 
@@ -321,6 +322,12 @@ void TrajectoryFollower::PreUpdate(
 
   // Transform from world to local frame.
   auto comPose = this->dataPtr->link.WorldInertialPose(_ecm);
+  if (!comPose.has_value())
+  {
+    ignerr << "Failed to get CoM pose for link ["
+           << this->dataPtr->link.Entity() << "]" << std::endl;
+    return;
+  }
 
   // Transform the force and torque to the world frame.
   // Move commands. The vehicle always move forward (X direction).

From 9d8afdfaedafbf3e330cdd1e8426a9fccc2e2f06 Mon Sep 17 00:00:00 2001
From: Louise Poubel <louise@openrobotics.org>
Date: Wed, 9 Mar 2022 13:49:33 -0800
Subject: [PATCH 16/17] Extend thruster limits to angular velocity test (#1380)

Signed-off-by: Louise Poubel <louise@openrobotics.org>
---
 test/worlds/thruster_ang_vel_cmd.sdf | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/test/worlds/thruster_ang_vel_cmd.sdf b/test/worlds/thruster_ang_vel_cmd.sdf
index c345259735..28185e1ec5 100644
--- a/test/worlds/thruster_ang_vel_cmd.sdf
+++ b/test/worlds/thruster_ang_vel_cmd.sdf
@@ -104,6 +104,8 @@
         <propeller_diameter>0.2</propeller_diameter>
         <velocity_control>true</velocity_control>
         <use_angvel_cmd>true</use_angvel_cmd>
+        <max_thrust_cmd>300</max_thrust_cmd>
+        <min_thrust_cmd>0</min_thrust_cmd>
       </plugin>
     </model>
 

From bf857e2973dca4290dfabc3435ff646663affb58 Mon Sep 17 00:00:00 2001
From: Louise Poubel <louise@openrobotics.org>
Date: Thu, 10 Mar 2022 08:58:02 -0800
Subject: [PATCH 17/17] Disable buoyancy offset test on Windows (#1380)

Signed-off-by: Louise Poubel <louise@openrobotics.org>
---
 test/integration/buoyancy.cc | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/test/integration/buoyancy.cc b/test/integration/buoyancy.cc
index b41f9e000b..807b2c475f 100644
--- a/test/integration/buoyancy.cc
+++ b/test/integration/buoyancy.cc
@@ -401,7 +401,7 @@ TEST_F(BuoyancyTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(GradedBuoyancy))
 }
 
 /////////////////////////////////////////////////
-TEST_F(BuoyancyTest, OffsetAndRotation)
+TEST_F(BuoyancyTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(OffsetAndRotation))
 {
   TestFixture fixture(common::joinPaths(std::string(PROJECT_SOURCE_PATH),
     "test", "worlds", "center_of_volume.sdf"));