From 5e2c4ae6584ee70848b8c9d6aeedb1fd726b6be3 Mon Sep 17 00:00:00 2001
From: Benjamin Perseghetti <bperseghetti@rudislabs.com>
Date: Mon, 6 May 2024 22:03:21 -0400
Subject: [PATCH 1/8] Camera track and camera follow config.

Signed-off-by: Benjamin Perseghetti <bperseghetti@rudislabs.com>
---
 src/plugins/CMakeLists.txt                    |   1 +
 src/plugins/camera_tracking/CameraTracking.cc | 219 ++++++++++--------
 src/plugins/camera_tracking/CameraTracking.hh |   5 +-
 .../camera_tracking/CameraTracking.qml        |   3 +-
 src/plugins/follow_config/CMakeLists.txt      |   6 +
 src/plugins/follow_config/FollowConfig.cc     | 139 +++++++++++
 src/plugins/follow_config/FollowConfig.hh     |  64 +++++
 src/plugins/follow_config/FollowConfig.qml    | 123 ++++++++++
 src/plugins/follow_config/FollowConfig.qrc    |   5 +
 9 files changed, 467 insertions(+), 98 deletions(-)
 create mode 100644 src/plugins/follow_config/CMakeLists.txt
 create mode 100644 src/plugins/follow_config/FollowConfig.cc
 create mode 100644 src/plugins/follow_config/FollowConfig.hh
 create mode 100644 src/plugins/follow_config/FollowConfig.qml
 create mode 100644 src/plugins/follow_config/FollowConfig.qrc

diff --git a/src/plugins/CMakeLists.txt b/src/plugins/CMakeLists.txt
index 01f7fc687..b8f2b3d59 100644
--- a/src/plugins/CMakeLists.txt
+++ b/src/plugins/CMakeLists.txt
@@ -116,6 +116,7 @@ endfunction()
 # Plugins
 add_subdirectory(camera_fps)
 add_subdirectory(camera_tracking)
+add_subdirectory(follow_config)
 add_subdirectory(grid_config)
 add_subdirectory(image_display)
 add_subdirectory(interactive_view_control)
diff --git a/src/plugins/camera_tracking/CameraTracking.cc b/src/plugins/camera_tracking/CameraTracking.cc
index 172bb6e05..cf16d539f 100644
--- a/src/plugins/camera_tracking/CameraTracking.cc
+++ b/src/plugins/camera_tracking/CameraTracking.cc
@@ -22,6 +22,7 @@
 #include <gz/msgs/gui_camera.pb.h>
 #include <gz/msgs/stringmsg.pb.h>
 #include <gz/msgs/vector3d.pb.h>
+#include <gz/msgs/cameratrack.pb.h>
 
 #include <gz/common/Console.hh>
 #include <gz/common/Profiler.hh>
@@ -59,12 +60,9 @@ class CameraTrackingPrivate
   public: bool OnMoveTo(const msgs::StringMsg &_msg,
       msgs::Boolean &_res);
 
-  /// \brief Callback for a follow request
-  /// \param[in] _msg Request message to set the target to follow.
-  /// \param[in] _res Response data
-  /// \return True if the request is received
-  public: bool OnFollow(const msgs::StringMsg &_msg,
-      msgs::Boolean &_res);
+  /// \brief Callback for a track message
+  /// \param[in] _msg Message consistes of the target to track, type of tracking, offset and pgain.
+  public: void OnTrackSub(const msgs::CameraTrack &_msg);
 
   /// \brief Callback for a move to pose request.
   /// \param[in] _msg GUICamera request message.
@@ -73,13 +71,6 @@ class CameraTrackingPrivate
   public: bool OnMoveToPose(const msgs::GUICamera &_msg,
                msgs::Boolean &_res);
 
-  /// \brief Callback for a follow offset request
-  /// \param[in] _msg Request message to set the camera's follow offset.
-  /// \param[in] _res Response data
-  /// \return True if the request is received
-  public: bool OnFollowOffset(const msgs::Vector3d &_msg,
-               msgs::Boolean &_res);
-
   /// \brief Callback when a move to animation is complete
   private: void OnMoveToComplete();
 
@@ -96,27 +87,29 @@ class CameraTrackingPrivate
   //// \brief Pointer to the rendering scene
   public: rendering::ScenePtr scene = nullptr;
 
-  /// \brief Target to follow
-  public: std::string followTarget;
+  /// \brief Target to track
+  public: std::string selectedTarget;
+
+  /// \brief Wait for target to track
+  public: bool selectedTargetWait = false;
 
-  /// \brief Wait for follow target
-  public: bool followTargetWait = false;
+  /// \brief Offset of camera from target being tracked
+  public: math::Vector3d trackOffset = math::Vector3d(-5, 0, 3);
 
-  /// \brief Offset of camera from target being followed
-  public: math::Vector3d followOffset = math::Vector3d(-5, 0, 3);
+  public: gz::msgs::CameraTrack trackMsg;
 
-  /// \brief Flag to indicate the follow offset needs to be updated
-  public: bool followOffsetDirty = false;
+  /// \brief Flag to indicate the tracking offset needs to be updated
+  public: bool trackOffsetDirty = false;
 
-  /// \brief Flag to indicate the follow offset has been updated
-  public: bool newFollowOffset = true;
+  /// \brief Flag to indicate new tracking
+  public: bool newTrack = true;
 
-  /// \brief Follow P gain
-  public: double followPGain = 0.01;
+  /// \brief Track P gain
+  public: double trackPGain = 0.01;
 
-  /// \brief True follow the target at an offset that is in world frame,
-  /// false to follow in target's local frame
-  public: bool followWorldFrame = false;
+  /// \brief True track the target at an offset that is in world frame,
+  /// false to track in target's local frame
+  public: bool trackWorldFrame = false;
 
   /// \brief Last move to animation time
   public: std::chrono::time_point<std::chrono::system_clock> prevMoveToTime;
@@ -139,11 +132,11 @@ class CameraTrackingPrivate
   /// \brief The pose set from the move to pose service.
   public: std::optional<math::Pose3d> moveToPoseValue;
 
-  /// \brief Follow service
-  public: std::string followService;
+  /// \brief Track topic
+  public: std::string trackTopic;
 
-  /// \brief Follow offset service
-  public: std::string followOffsetService;
+  /// \brief Track status topic
+  public: std::string trackStatusTopic;
 
   /// \brief Camera pose topic
   public: std::string cameraPoseTopic;
@@ -151,6 +144,9 @@ class CameraTrackingPrivate
   /// \brief Move to pose service
   public: std::string moveToPoseService;
 
+  /// \brief Camera pose publisher
+  public: transport::Node::Publisher trackStatusPub;
+
   /// \brief Camera pose publisher
   public: transport::Node::Publisher cameraPosePub;
 
@@ -187,12 +183,19 @@ void CameraTrackingPrivate::Initialize()
   gzmsg << "Move to service on ["
          << this->moveToService << "]" << std::endl;
 
-  // follow
-  this->followService = "/gui/follow";
-  this->node.Advertise(this->followService,
-      &CameraTrackingPrivate::OnFollow, this);
-  gzmsg << "Follow service on ["
-         << this->followService << "]" << std::endl;
+  // track
+  this->trackTopic = "/gui/track";
+  this->node.Subscribe(this->trackTopic, 
+      &CameraTrackingPrivate::OnTrackSub, this);
+  gzmsg << "Tracking topic on ["
+         << this->trackTopic << "]" << std::endl;
+
+  // tracking status
+  this->trackStatusTopic = "/gui/currently_tracked";
+  this->trackStatusPub =
+    this->node.Advertise<msgs::CameraTrack>(this->trackStatusTopic);
+  gzmsg << "Tracking status topic on ["
+         << this->trackStatusTopic << "]" << std::endl;
 
   // move to pose service
   this->moveToPoseService =
@@ -208,13 +211,6 @@ void CameraTrackingPrivate::Initialize()
     this->node.Advertise<msgs::Pose>(this->cameraPoseTopic);
   gzmsg << "Camera pose topic advertised on ["
          << this->cameraPoseTopic << "]" << std::endl;
-
-   // follow offset
-   this->followOffsetService = "/gui/follow/offset";
-   this->node.Advertise(this->followOffsetService,
-       &CameraTrackingPrivate::OnFollowOffset, this);
-   gzmsg << "Follow offset service on ["
-          << this->followOffsetService << "]" << std::endl;
 }
 
 /////////////////////////////////////////////////
@@ -229,14 +225,30 @@ bool CameraTrackingPrivate::OnMoveTo(const msgs::StringMsg &_msg,
 }
 
 /////////////////////////////////////////////////
-bool CameraTrackingPrivate::OnFollow(const msgs::StringMsg &_msg,
-  msgs::Boolean &_res)
+void CameraTrackingPrivate::OnTrackSub(const msgs::CameraTrack &_msg)
 {
   std::lock_guard<std::mutex> lock(this->mutex);
-  this->followTarget = _msg.data();
+  gzmsg << "Got new track message."<< std::endl;
+  if (!_msg.target().empty())
+  {
+    this->selectedTarget = _msg.target(); 
+  }
+  else
+  {
+    gzmsg << "Target name empty."<< std::endl;
+  }
+  if (_msg.has_offset())
+  {
+    this->trackOffset = msgs::Convert(_msg.offset());
+  }
 
-  _res.set_data(true);
-  return true;
+  if (_msg.pgain() > 0.00001)
+  {
+    this->trackPGain = _msg.pgain();
+  }
+
+  this->newTrack = true;
+  return;
 }
 
 /////////////////////////////////////////////////
@@ -251,21 +263,6 @@ void CameraTrackingPrivate::OnMoveToPoseComplete()
   this->moveToPoseValue.reset();
 }
 
-/////////////////////////////////////////////////
-bool CameraTrackingPrivate::OnFollowOffset(const msgs::Vector3d &_msg,
-  msgs::Boolean &_res)
-{
-  std::lock_guard<std::mutex> lock(this->mutex);
-  if (!this->followTarget.empty())
-  {
-    this->newFollowOffset = true;
-    this->followOffset = msgs::Convert(_msg);
-  }
-
-  _res.set_data(true);
-  return true;
-}
-
 /////////////////////////////////////////////////
 bool CameraTrackingPrivate::OnMoveToPose(const msgs::GUICamera &_msg,
   msgs::Boolean &_res)
@@ -364,63 +361,63 @@ void CameraTrackingPrivate::OnRender()
     }
   }
 
-  // Follow
+  // Track
   {
-    GZ_PROFILE("CameraTrackingPrivate::OnRender Follow");
-    // reset follow mode if target node got removed
-    if (!this->followTarget.empty())
+    GZ_PROFILE("CameraTrackingPrivate::OnRender Track");
+    // reset track mode if target node got removed
+    if (!this->selectedTarget.empty())
     {
-      rendering::NodePtr target = this->scene->NodeByName(this->followTarget);
-      if (!target && !this->followTargetWait)
+      rendering::NodePtr target = this->scene->NodeByName(this->selectedTarget);
+      if (!target && !this->selectedTargetWait)
       {
         this->camera->SetFollowTarget(nullptr);
         this->camera->SetTrackTarget(nullptr);
-        this->followTarget.clear();
+        this->selectedTarget.clear();
       }
     }
 
     if (!this->moveToTarget.empty())
       return;
-    rendering::NodePtr followTargetTmp = this->camera->FollowTarget();
-    if (!this->followTarget.empty())
+    rendering::NodePtr selectedTargetTmp = this->camera->FollowTarget();
+    if (!this->selectedTarget.empty())
     {
       rendering::NodePtr target = scene->NodeByName(
-          this->followTarget);
+          this->selectedTarget);
       if (target)
       {
-        if (!followTargetTmp || target != followTargetTmp
-              || this->newFollowOffset)
+        if (!selectedTargetTmp || target != selectedTargetTmp
+              || this->newTrack)
         {
           this->camera->SetFollowTarget(target,
-              this->followOffset,
-              this->followWorldFrame);
-          this->camera->SetFollowPGain(this->followPGain);
+              this->trackOffset,
+              this->trackWorldFrame);
+          this->camera->SetFollowPGain(this->trackPGain);
 
           this->camera->SetTrackTarget(target);
           // found target, no need to wait anymore
-          this->newFollowOffset = false;
-          this->followTargetWait = false;
+          this->newTrack = false;
+          this->selectedTargetWait = false;
         }
-        else if (this->followOffsetDirty)
+        else if (this->trackOffsetDirty)
         {
           math::Vector3d offset =
               this->camera->WorldPosition() - target->WorldPosition();
-          if (!this->followWorldFrame)
+          if (!this->trackWorldFrame)
           {
             offset = target->WorldRotation().RotateVectorReverse(offset);
           }
           this->camera->SetFollowOffset(offset);
-          this->followOffsetDirty = false;
+          this->trackOffsetDirty = false;
         }
       }
-      else if (!this->followTargetWait)
+      else if (!this->selectedTargetWait)
       {
-        gzerr << "Unable to follow target. Target: '"
-               << this->followTarget << "' not found" << std::endl;
-        this->followTarget.clear();
+        gzerr << "Unable to track target. Target: '"
+               << this->selectedTarget << "' not found" << std::endl;
+        this->selectedTarget.clear();
       }
     }
-    else if (followTargetTmp)
+    else if (selectedTargetTmp)
     {
       this->camera->SetFollowTarget(nullptr);
       this->camera->SetTrackTarget(nullptr);
@@ -443,6 +440,15 @@ CameraTracking::CameraTracking()
       auto poseMsg = msgs::Convert(this->dataPtr->camera->WorldPose());
       this->dataPtr->cameraPosePub.Publish(poseMsg);
     }
+    if (this->dataPtr->trackStatusPub.HasConnections())
+    {
+      this->dataPtr->trackMsg.set_target(this->dataPtr->selectedTarget);
+      this->dataPtr->trackMsg.mutable_offset()->set_x(this->dataPtr->trackOffset.X());
+      this->dataPtr->trackMsg.mutable_offset()->set_y(this->dataPtr->trackOffset.Y());
+      this->dataPtr->trackMsg.mutable_offset()->set_z(this->dataPtr->trackOffset.Z());
+      this->dataPtr->trackMsg.set_pgain(this->dataPtr->trackPGain);
+      this->dataPtr->trackStatusPub.Publish(this->dataPtr->trackMsg);
+    }
   });
   this->dataPtr->timer->setInterval(1000.0 / 50.0);
   this->dataPtr->timer->start();
@@ -452,11 +458,38 @@ CameraTracking::CameraTracking()
 CameraTracking::~CameraTracking() = default;
 
 /////////////////////////////////////////////////
-void CameraTracking::LoadConfig(const tinyxml2::XMLElement *)
+void CameraTracking::LoadConfig(const tinyxml2::XMLElement *_pluginElem)
 {
   if (this->title.empty())
     this->title = "Camera tracking";
 
+  if (_pluginElem)
+  {
+    if (auto nameElem = _pluginElem->FirstChildElement("target"))
+    {
+      this->dataPtr->selectedTarget = nameElem->GetText();
+      gzmsg << "CameraTracking: Loaded target from sdf ["
+            << this->dataPtr->selectedTarget << "]" << std::endl;
+      this->dataPtr->selectedTargetWait = true;
+    }
+    if (auto offsetElem = _pluginElem->FirstChildElement("offset"))
+    {
+      std::stringstream offsetStr;
+      offsetStr << std::string(offsetElem->GetText());
+      offsetStr >> this->dataPtr->trackOffset;
+      gzmsg << "CameraTracking: Loaded offset from sdf ["
+            << this->dataPtr->trackOffset << "]" << std::endl;
+      this->dataPtr->newTrack = true;
+    }
+    if (auto pGainElem = _pluginElem->FirstChildElement("pgain"))
+    {
+      this->dataPtr->trackPGain = std::stod(std::string(pGainElem->GetText()));
+      gzmsg << "CameraTracking: Loaded pgain from sdf ["
+            << this->dataPtr->trackPGain << "]" << std::endl;
+      this->dataPtr->newTrack = true;
+    }
+  }
+
   App()->findChild<MainWindow *>()->installEventFilter(this);
 }
 
@@ -465,9 +498,9 @@ void CameraTrackingPrivate::HandleKeyRelease(events::KeyReleaseOnScene *_e)
 {
   if (_e->Key().Key() == Qt::Key_Escape)
   {
-    if (!this->followTarget.empty())
+    if (!this->selectedTarget.empty())
     {
-      this->followTarget = std::string();
+      this->selectedTarget = std::string();
 
       _e->accept();
     }
diff --git a/src/plugins/camera_tracking/CameraTracking.hh b/src/plugins/camera_tracking/CameraTracking.hh
index 68176e2c5..d94c9ab2c 100644
--- a/src/plugins/camera_tracking/CameraTracking.hh
+++ b/src/plugins/camera_tracking/CameraTracking.hh
@@ -33,9 +33,8 @@ namespace gz::gui::plugins
   /// * `/gui/move_to`: Move the user camera to look at a given target,
   ///                   identified by name.
   /// * `/gui/move_to/pose`: Move the user camera to a given pose.
-  /// * `/gui/follow`: Set the user camera to follow a given target,
-  ///                   identified by name.
-  /// * `/gui/follow/offset`: Set the offset for following.
+  /// * `/gui/track`: Set the user camera to follow a given target,
+  ///                   identified by name, offset, pgain, track type.
   ///
   /// Topics:
   /// * `/gui/camera/pose`: Publishes the current user camera pose.
diff --git a/src/plugins/camera_tracking/CameraTracking.qml b/src/plugins/camera_tracking/CameraTracking.qml
index 09c339f0b..5980cd1c8 100644
--- a/src/plugins/camera_tracking/CameraTracking.qml
+++ b/src/plugins/camera_tracking/CameraTracking.qml
@@ -28,8 +28,7 @@ ColumnLayout {
   property string message: 'Services provided:<br><ul>' +
       '<li>/gui/move_to</li>' +
       '<li>/gui/move_to/pose</li>' +
-      '<li>/gui/follow</li>' +
-      '<li>/gui/follow/offset</li></ul><br>Topics provided:<br><ul>' +
+      '<li>/gui/track</li></ul><br>Topics provided:<br><ul>' +
       '<li>/gui/camera/pose</li></ul>'
 
   Label {
diff --git a/src/plugins/follow_config/CMakeLists.txt b/src/plugins/follow_config/CMakeLists.txt
new file mode 100644
index 000000000..6710b1174
--- /dev/null
+++ b/src/plugins/follow_config/CMakeLists.txt
@@ -0,0 +1,6 @@
+gz_gui_add_plugin(FollowConfig
+  SOURCES
+    FollowConfig.cc
+  QT_HEADERS
+    FollowConfig.hh
+)
diff --git a/src/plugins/follow_config/FollowConfig.cc b/src/plugins/follow_config/FollowConfig.cc
new file mode 100644
index 000000000..7d2789269
--- /dev/null
+++ b/src/plugins/follow_config/FollowConfig.cc
@@ -0,0 +1,139 @@
+/*
+ * Copyright (C) 2023 Rudis Laboratories LLC
+ *
+ * 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 <gz/msgs/double.pb.h>
+#include <gz/msgs/cameratrack.pb.h>
+#include <gz/msgs/vector3d.pb.h>
+
+#include <gz/common/Console.hh>
+#include <gz/msgs/Utility.hh>
+#include <gz/plugin/Register.hh>
+
+#include "gz/gui/Application.hh"
+#include "gz/gui/Conversions.hh"
+#include "gz/gui/GuiEvents.hh"
+#include "gz/gui/MainWindow.hh"
+
+#include <gz/transport/Node.hh>
+
+#include "FollowConfig.hh"
+
+/// \brief Private data class for FollowConfig
+class gz::gui::plugins::FollowConfigPrivate
+{
+
+  /// \brief Topic for follow message
+  public: std::string followTopic;
+
+  /// \brief Offset of camera from target being followed
+  public: math::Vector3d followOffset{math::Vector3d(-5.0, 0.0, 3.0)};
+
+  /// \brief Follow P gain
+  public: double followPGain{0.01};
+
+  public: transport::Node node;
+
+  /// \brief Process updated follow
+  public: void UpdateFollow();
+
+  /// \brief flag for updating
+  public: bool newFollowUpdate = false;
+
+  /// \brief follow publisher
+  public: transport::Node::Publisher followPub;
+};
+
+using namespace gz;
+using namespace gui;
+using namespace plugins;
+
+/////////////////////////////////////////////////
+FollowConfig::FollowConfig()
+  : gz::gui::Plugin(), dataPtr(std::make_unique<FollowConfigPrivate>())
+{
+}
+
+/////////////////////////////////////////////////
+FollowConfig::~FollowConfig() = default;
+
+/////////////////////////////////////////////////
+void FollowConfig::LoadConfig(const tinyxml2::XMLElement *)
+{
+  if (this->title.empty())
+    this->title = "Follow Config";
+
+  // Follow target offset service
+  this->dataPtr->followTopic = "/gui/track";
+  this->dataPtr->followPub =
+    this->dataPtr->node.Advertise<msgs::CameraTrack>(this->dataPtr->followTopic);
+  gzmsg << "FollowConfig: Follow topic publisher advertised on ["
+         << this->dataPtr->followTopic << "]" << std::endl;
+
+  gui::App()->findChild<
+      MainWindow *>()->installEventFilter(this);
+}
+
+/////////////////////////////////////////////////
+bool FollowConfig::eventFilter(QObject *_obj, QEvent *_event)
+{
+  if (_event->type() == events::Render::kType)
+  {
+    if (this->dataPtr->newFollowUpdate)
+    {
+      this->dataPtr->UpdateFollow();
+    }
+  }
+
+  // Standard event processing
+  return QObject::eventFilter(_obj, _event);
+}
+
+/////////////////////////////////////////////////
+void FollowConfig::SetFollow(double _x,
+          double _y, double _z, double _p)
+{
+  if (!this->dataPtr->newFollowUpdate)
+  {
+    this->dataPtr->followOffset = math::Vector3d(
+      _x, _y, _z);
+    this->dataPtr->followPGain = _p;
+    gzmsg << "FollowConfig: Offset("
+          << this->dataPtr->followOffset << "), PGain("
+          << this->dataPtr->followPGain << ")" << std::endl;
+    this->dataPtr->newFollowUpdate = true;
+  }
+}
+
+
+/////////////////////////////////////////////////
+void FollowConfigPrivate::UpdateFollow()
+{
+  // Follow
+  msgs::CameraTrack followMsg;
+  followMsg.mutable_offset()->set_x(this->followOffset.X());
+  followMsg.mutable_offset()->set_y(this->followOffset.Y());
+  followMsg.mutable_offset()->set_z(this->followOffset.Z());
+  followMsg.set_pgain(this->followPGain);
+  this->followPub.Publish(followMsg);
+  gzmsg << "FollowConfig: Publishing message." << std::endl;
+  this->newFollowUpdate = false;
+}
+
+// Register this plugin
+GZ_ADD_PLUGIN(FollowConfig,
+              gui::Plugin)
diff --git a/src/plugins/follow_config/FollowConfig.hh b/src/plugins/follow_config/FollowConfig.hh
new file mode 100644
index 000000000..9adfda41f
--- /dev/null
+++ b/src/plugins/follow_config/FollowConfig.hh
@@ -0,0 +1,64 @@
+/*
+ * Copyright (C) 2023 Rudis Laboratories LLC
+ *
+ * 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 GZ_GUI_PLUGINS_FOLLOWCONFIG_HH_
+#define GZ_GUI_PLUGINS_FOLLOWCONFIG_HH_
+
+#include <memory>
+
+#include "gz/gui/Plugin.hh"
+
+namespace gz
+{
+namespace gui
+{
+namespace plugins
+{
+  class FollowConfigPrivate;
+
+  class FollowConfig : public Plugin
+  {
+    Q_OBJECT
+
+    /// \brief Constructor
+    public: FollowConfig();
+
+    /// \brief Destructor
+    public: virtual ~FollowConfig();
+
+    // Documentation inherited
+    public: virtual void LoadConfig(const tinyxml2::XMLElement *)
+        override;
+
+    /// \brief Set the follow offset, requested from the GUI.
+    /// \param[in] _x The follow offset distance in x
+    /// \param[in] _y The follow offset distance in y
+    /// \param[in] _z The follow offset distance in z
+    /// \param[in] _p The follow pgain
+    public slots: void SetFollow(double _x,
+          double _y, double _z, double _p);
+
+    // Documentation inherited
+    private: bool eventFilter(QObject *_obj, QEvent *_event) override;
+
+    /// \internal
+    /// \brief Pointer to private data.
+    private: std::unique_ptr<FollowConfigPrivate> dataPtr;
+  };
+}
+}
+}
+#endif
diff --git a/src/plugins/follow_config/FollowConfig.qml b/src/plugins/follow_config/FollowConfig.qml
new file mode 100644
index 000000000..6aaf66e0e
--- /dev/null
+++ b/src/plugins/follow_config/FollowConfig.qml
@@ -0,0 +1,123 @@
+/*
+ * Copyright (C) 2023 Rudis Laboratories LLC
+ *
+ * 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.
+ *
+*/
+
+import QtQuick 2.9
+import QtQuick.Controls 2.2
+import QtQuick.Controls.Material 2.1
+import QtQuick.Controls.Styles 1.4
+import QtQuick.Layouts 1.3
+import gz.gui 1.0
+
+ColumnLayout {
+  Layout.minimumWidth: 200
+  Layout.minimumHeight: 200
+  Layout.margins: 2
+  anchors.fill: parent
+  focus: true
+
+  // X camera follow distance
+  property double xFollowOffset: -5.0
+  // Y camera follow distance
+  property double yFollowOffset: 0.0
+  // Z camera follow distance
+  property double zFollowOffset: 3.0
+  // P Gain camera follow distance
+  property double pGainFollow: 0.01
+
+  GridLayout {
+    Layout.fillWidth: true
+    Layout.margins: 2
+    columns: 2
+  
+    // X follow offset distance
+    Label {
+      id: xFollowOffsetLabel
+      text: "Camera follow X (m)"
+      color: "dimgrey"
+    }
+    GzSpinBox {
+      id: xFollowOffsetField
+      Layout.fillWidth: true
+      value: xFollowOffset
+      maximumValue: 100.0
+      minimumValue: -100.0
+      decimals: 2
+      stepSize: 0.5
+      onEditingFinished:{
+        xFollowOffset = value
+        FollowConfig.SetFollow(xFollowOffset, yFollowOffset, zFollowOffset,pGainFollow)
+      }
+    }
+    // Y follow offset distance
+    Label {
+      id: yFollowOffsetLabel
+      text: "Camera follow Y (m)"
+      color: "dimgrey"
+    }
+    GzSpinBox {
+      id: yFollowOffsetField
+      Layout.fillWidth: true
+      value: yFollowOffset
+      maximumValue: 100.0
+      minimumValue: -100.0
+      decimals: 2
+      stepSize: 0.5
+      onEditingFinished:{
+        yFollowOffset = value
+        FollowConfig.SetFollow(xFollowOffset, yFollowOffset, zFollowOffset,pGainFollow)
+      }
+    }
+    // Z follow offset distance
+    Label {
+      id: zFollowOffsetLabel
+      text: "Camera follow Z (m)"
+      color: "dimgrey"
+    }
+    GzSpinBox {
+      id: zFollowOffsetField
+      Layout.fillWidth: true
+      value: zFollowOffset
+      maximumValue: 100.0
+      minimumValue: -100.0
+      decimals: 2
+      stepSize: 0.5
+      onEditingFinished:{
+        zFollowOffset = value
+        FollowConfig.SetFollow(xFollowOffset, yFollowOffset, zFollowOffset,pGainFollow)
+      }
+    }
+    // P Gain follow
+    Label {
+      id: pGainFollowLabel
+      text: "Camera follow P Gain"
+      color: "dimgrey"
+    }
+    GzSpinBox {
+      id: pGainFollowField
+      Layout.fillWidth: true
+      value: pGainFollow
+      maximumValue: 1.0
+      minimumValue: 0.001
+      decimals: 3
+      stepSize: 0.01
+      onEditingFinished:{
+        pGainFollow = value
+        FollowConfig.SetFollow(xFollowOffset, yFollowOffset, zFollowOffset,pGainFollow)
+      }
+    }
+  }
+}
diff --git a/src/plugins/follow_config/FollowConfig.qrc b/src/plugins/follow_config/FollowConfig.qrc
new file mode 100644
index 000000000..61099f8f1
--- /dev/null
+++ b/src/plugins/follow_config/FollowConfig.qrc
@@ -0,0 +1,5 @@
+<!DOCTYPE RCC><RCC version="1.0">
+<qresource prefix="FollowConfig/">
+  <file>FollowConfig.qml</file>
+</qresource>
+</RCC>

From 8f9ed4103478751f1603ec33407aed206d5f3491 Mon Sep 17 00:00:00 2001
From: Benjamin Perseghetti <bperseghetti@rudislabs.com>
Date: Wed, 8 May 2024 04:19:21 -0400
Subject: [PATCH 2/8] Add track camera config and multiple track modes.

Signed-off-by: Benjamin Perseghetti <bperseghetti@rudislabs.com>
---
 src/plugins/CMakeLists.txt                    |   1 +
 src/plugins/camera_tracking/CameraTracking.cc |  86 ++++++++---
 src/plugins/camera_tracking/CameraTracking.hh |   1 +
 .../camera_tracking/CameraTracking.qml        |   1 +
 src/plugins/follow_config/FollowConfig.cc     |  25 ++++
 src/plugins/follow_config/FollowConfig.hh     |  19 +++
 src/plugins/follow_config/FollowConfig.qml    |  19 ++-
 src/plugins/track_config/CMakeLists.txt       |   6 +
 src/plugins/track_config/TrackConfig.cc       | 140 ++++++++++++++++++
 src/plugins/track_config/TrackConfig.hh       |  64 ++++++++
 src/plugins/track_config/TrackConfig.qml      | 123 +++++++++++++++
 src/plugins/track_config/TrackConfig.qrc      |   5 +
 12 files changed, 462 insertions(+), 28 deletions(-)
 create mode 100644 src/plugins/track_config/CMakeLists.txt
 create mode 100644 src/plugins/track_config/TrackConfig.cc
 create mode 100644 src/plugins/track_config/TrackConfig.hh
 create mode 100644 src/plugins/track_config/TrackConfig.qml
 create mode 100644 src/plugins/track_config/TrackConfig.qrc

diff --git a/src/plugins/CMakeLists.txt b/src/plugins/CMakeLists.txt
index b8f2b3d59..27047b139 100644
--- a/src/plugins/CMakeLists.txt
+++ b/src/plugins/CMakeLists.txt
@@ -117,6 +117,7 @@ endfunction()
 add_subdirectory(camera_fps)
 add_subdirectory(camera_tracking)
 add_subdirectory(follow_config)
+add_subdirectory(track_config)
 add_subdirectory(grid_config)
 add_subdirectory(image_display)
 add_subdirectory(interactive_view_control)
diff --git a/src/plugins/camera_tracking/CameraTracking.cc b/src/plugins/camera_tracking/CameraTracking.cc
index cf16d539f..1e306b877 100644
--- a/src/plugins/camera_tracking/CameraTracking.cc
+++ b/src/plugins/camera_tracking/CameraTracking.cc
@@ -1,5 +1,6 @@
 /*
  * Copyright (C) 2021 Open Source Robotics Foundation
+ * Copyright (C) 2024 Rudis Laboratories LLC
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
@@ -96,6 +97,9 @@ class CameraTrackingPrivate
   /// \brief Offset of camera from target being tracked
   public: math::Vector3d trackOffset = math::Vector3d(-5, 0, 3);
 
+  /// \brief Pose of camera in world target is being tracked from
+  public: math::Vector3d trackPose = math::Vector3d(0, 0, 0);
+
   public: gz::msgs::CameraTrack trackMsg;
 
   /// \brief Flag to indicate the tracking offset needs to be updated
@@ -107,6 +111,12 @@ class CameraTrackingPrivate
   /// \brief Track P gain
   public: double trackPGain = 0.01;
 
+  /// \brief Free Look P gain
+  public: double freeLookPGain = 1.0;
+
+  /// \brief Default track mode to Follow
+  public: int trackMode = gz::msgs::CameraTrack::FOLLOW;
+
   /// \brief True track the target at an offset that is in world frame,
   /// false to track in target's local frame
   public: bool trackWorldFrame = false;
@@ -229,6 +239,9 @@ void CameraTrackingPrivate::OnTrackSub(const msgs::CameraTrack &_msg)
 {
   std::lock_guard<std::mutex> lock(this->mutex);
   gzmsg << "Got new track message."<< std::endl;
+
+  this->trackMode = _msg.track_mode();
+
   if (!_msg.target().empty())
   {
     this->selectedTarget = _msg.target(); 
@@ -241,7 +254,10 @@ void CameraTrackingPrivate::OnTrackSub(const msgs::CameraTrack &_msg)
   {
     this->trackOffset = msgs::Convert(_msg.offset());
   }
-
+  if (_msg.has_track_pose())
+  {
+    this->trackPose = msgs::Convert(_msg.track_pose());
+  }
   if (_msg.pgain() > 0.00001)
   {
     this->trackPGain = _msg.pgain();
@@ -378,36 +394,53 @@ void CameraTrackingPrivate::OnRender()
 
     if (!this->moveToTarget.empty())
       return;
-    rendering::NodePtr selectedTargetTmp = this->camera->FollowTarget();
+    rendering::NodePtr selectedFollowTargetTmp = this->camera->FollowTarget();
+    rendering::NodePtr selectedTrackTargetTmp = this->camera->TrackTarget();
     if (!this->selectedTarget.empty())
     {
       rendering::NodePtr target = scene->NodeByName(
           this->selectedTarget);
       if (target)
       {
-        if (!selectedTargetTmp || target != selectedTargetTmp
-              || this->newTrack)
+        if (this->trackMode == gz::msgs::CameraTrack::FOLLOW_FREE_LOOK ||
+              this->trackMode == gz::msgs::CameraTrack::FOLLOW)
         {
-          this->camera->SetFollowTarget(target,
-              this->trackOffset,
-              this->trackWorldFrame);
-          this->camera->SetFollowPGain(this->trackPGain);
-
-          this->camera->SetTrackTarget(target);
-          // found target, no need to wait anymore
-          this->newTrack = false;
-          this->selectedTargetWait = false;
+          if (!selectedFollowTargetTmp || target != selectedFollowTargetTmp
+                || this->newTrack)
+          {
+            this->trackWorldFrame = false;
+            this->camera->SetFollowTarget(target,
+                this->trackOffset,
+                this->trackWorldFrame);
+            if (this->trackMode == gz::msgs::CameraTrack::FOLLOW)
+            {
+              this->camera->SetTrackTarget(target);
+              this->camera->SetTrackPGain(this->trackPGain);
+              this->camera->SetFollowPGain(this->trackPGain);
+            }
+            else
+            {
+              this->camera->SetTrackTarget(nullptr);
+              this->camera->SetFollowPGain(this->freeLookPGain);
+            }
+            this->newTrack = false;
+            this->selectedTargetWait = false;
+          }
         }
-        else if (this->trackOffsetDirty)
+        if (this->trackMode == gz::msgs::CameraTrack::TRACK)
         {
-          math::Vector3d offset =
-              this->camera->WorldPosition() - target->WorldPosition();
-          if (!this->trackWorldFrame)
+          if (!selectedTrackTargetTmp || target != selectedTrackTargetTmp
+                || this->newTrack)
           {
-            offset = target->WorldRotation().RotateVectorReverse(offset);
+            this->trackWorldFrame = true;
+            this->camera->SetFollowTarget(nullptr);
+            this->camera->SetTrackTarget(target,
+                this->trackPose,
+                this->trackWorldFrame);
+            this->camera->SetTrackPGain(this->trackPGain);
+            this->newTrack = false;
+            this->selectedTargetWait = false;
           }
-          this->camera->SetFollowOffset(offset);
-          this->trackOffsetDirty = false;
         }
       }
       else if (!this->selectedTargetWait)
@@ -417,10 +450,16 @@ void CameraTrackingPrivate::OnRender()
         this->selectedTarget.clear();
       }
     }
-    else if (selectedTargetTmp)
+    else
     {
-      this->camera->SetFollowTarget(nullptr);
-      this->camera->SetTrackTarget(nullptr);
+      if (selectedFollowTargetTmp)
+      {
+        this->camera->SetFollowTarget(nullptr);
+      }
+      if (selectedTrackTargetTmp)
+      {
+        this->camera->SetTrackTarget(nullptr);
+      }
     }
   }
 }
@@ -526,6 +565,7 @@ bool CameraTracking::eventFilter(QObject *_obj, QEvent *_event)
   // Standard event processing
   return QObject::eventFilter(_obj, _event);
 }
+
 }  // namespace gz::gui::plugins
 
 // Register this plugin
diff --git a/src/plugins/camera_tracking/CameraTracking.hh b/src/plugins/camera_tracking/CameraTracking.hh
index d94c9ab2c..395d99477 100644
--- a/src/plugins/camera_tracking/CameraTracking.hh
+++ b/src/plugins/camera_tracking/CameraTracking.hh
@@ -1,5 +1,6 @@
 /*
  * Copyright (C) 2021 Open Source Robotics Foundation
+ * Copyright (C) 2024 Rudis Laboratories LLC
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
diff --git a/src/plugins/camera_tracking/CameraTracking.qml b/src/plugins/camera_tracking/CameraTracking.qml
index 5980cd1c8..acdf09575 100644
--- a/src/plugins/camera_tracking/CameraTracking.qml
+++ b/src/plugins/camera_tracking/CameraTracking.qml
@@ -1,5 +1,6 @@
 /*
  * Copyright (C) 2021 Open Source Robotics Foundation
+ * Copyright (C) 2024 Rudis Laboratories LLC
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
diff --git a/src/plugins/follow_config/FollowConfig.cc b/src/plugins/follow_config/FollowConfig.cc
index 7d2789269..9b89c1b26 100644
--- a/src/plugins/follow_config/FollowConfig.cc
+++ b/src/plugins/follow_config/FollowConfig.cc
@@ -46,6 +46,9 @@ class gz::gui::plugins::FollowConfigPrivate
   /// \brief Follow P gain
   public: double followPGain{0.01};
 
+  /// \brief Flag used to set free look.
+  public: bool freelook{false};
+
   public: transport::Node node;
 
   /// \brief Process updated follow
@@ -119,6 +122,19 @@ void FollowConfig::SetFollow(double _x,
   }
 }
 
+/////////////////////////////////////////////////
+bool FollowConfig::FreeLook() const
+{
+  return this->dataPtr->freelook;
+}
+
+/////////////////////////////////////////////////
+void FollowConfig::SetFreeLook(const bool &_freelook)
+{
+  this->dataPtr->freelook = _freelook;
+  this->FreeLookChanged();
+  this->dataPtr->UpdateFollow();
+}
 
 /////////////////////////////////////////////////
 void FollowConfigPrivate::UpdateFollow()
@@ -129,6 +145,15 @@ void FollowConfigPrivate::UpdateFollow()
   followMsg.mutable_offset()->set_y(this->followOffset.Y());
   followMsg.mutable_offset()->set_z(this->followOffset.Z());
   followMsg.set_pgain(this->followPGain);
+  if (this->freelook)
+  {
+    followMsg.set_track_mode(msgs::CameraTrack::FOLLOW_FREE_LOOK);
+  }
+  else
+  {
+    followMsg.set_track_mode(msgs::CameraTrack::FOLLOW);
+  }
+
   this->followPub.Publish(followMsg);
   gzmsg << "FollowConfig: Publishing message." << std::endl;
   this->newFollowUpdate = false;
diff --git a/src/plugins/follow_config/FollowConfig.hh b/src/plugins/follow_config/FollowConfig.hh
index 9adfda41f..4cebdcf40 100644
--- a/src/plugins/follow_config/FollowConfig.hh
+++ b/src/plugins/follow_config/FollowConfig.hh
@@ -33,6 +33,14 @@ namespace plugins
   {
     Q_OBJECT
 
+    /// \brief Free Look
+    Q_PROPERTY(
+      bool freelook
+      READ FreeLook
+      WRITE SetFreeLook
+      NOTIFY FreeLookChanged
+    )
+
     /// \brief Constructor
     public: FollowConfig();
 
@@ -54,6 +62,17 @@ namespace plugins
     // Documentation inherited
     private: bool eventFilter(QObject *_obj, QEvent *_event) override;
 
+    /// \brief Get whether it is free look
+    /// \return True if freelook
+    public: Q_INVOKABLE bool FreeLook() const;
+
+    /// \brief Set to free look
+    /// \param[in] _freelook True if free look
+    public: Q_INVOKABLE void SetFreeLook(const bool &_freelook);
+
+    /// \brief Notify that free look has changed
+    signals: void FreeLookChanged();
+
     /// \internal
     /// \brief Pointer to private data.
     private: std::unique_ptr<FollowConfigPrivate> dataPtr;
diff --git a/src/plugins/follow_config/FollowConfig.qml b/src/plugins/follow_config/FollowConfig.qml
index 6aaf66e0e..9f139d438 100644
--- a/src/plugins/follow_config/FollowConfig.qml
+++ b/src/plugins/follow_config/FollowConfig.qml
@@ -38,15 +38,24 @@ ColumnLayout {
   // P Gain camera follow distance
   property double pGainFollow: 0.01
 
+  CheckBox {
+    objectName: "freelookCheck"
+    text: qsTr("Free Look")
+    checked: FollowConfig.freelook
+    onClicked: {
+      FollowConfig.SetFreeLook(checked)
+    }
+  }
+
   GridLayout {
     Layout.fillWidth: true
     Layout.margins: 2
     columns: 2
-  
+
     // X follow offset distance
     Label {
       id: xFollowOffsetLabel
-      text: "Camera follow X (m)"
+      text: "Camera offset X (m)"
       color: "dimgrey"
     }
     GzSpinBox {
@@ -65,7 +74,7 @@ ColumnLayout {
     // Y follow offset distance
     Label {
       id: yFollowOffsetLabel
-      text: "Camera follow Y (m)"
+      text: "Camera offset Y (m)"
       color: "dimgrey"
     }
     GzSpinBox {
@@ -84,7 +93,7 @@ ColumnLayout {
     // Z follow offset distance
     Label {
       id: zFollowOffsetLabel
-      text: "Camera follow Z (m)"
+      text: "Camera offset Z (m)"
       color: "dimgrey"
     }
     GzSpinBox {
@@ -103,7 +112,7 @@ ColumnLayout {
     // P Gain follow
     Label {
       id: pGainFollowLabel
-      text: "Camera follow P Gain"
+      text: "Camera offset P Gain"
       color: "dimgrey"
     }
     GzSpinBox {
diff --git a/src/plugins/track_config/CMakeLists.txt b/src/plugins/track_config/CMakeLists.txt
new file mode 100644
index 000000000..9ca83e5ab
--- /dev/null
+++ b/src/plugins/track_config/CMakeLists.txt
@@ -0,0 +1,6 @@
+gz_gui_add_plugin(TrackConfig
+  SOURCES
+    TrackConfig.cc
+  QT_HEADERS
+    TrackConfig.hh
+)
diff --git a/src/plugins/track_config/TrackConfig.cc b/src/plugins/track_config/TrackConfig.cc
new file mode 100644
index 000000000..5e2308d4a
--- /dev/null
+++ b/src/plugins/track_config/TrackConfig.cc
@@ -0,0 +1,140 @@
+/*
+ * Copyright (C) 2024 Rudis Laboratories LLC
+ *
+ * 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 <gz/msgs/double.pb.h>
+#include <gz/msgs/cameratrack.pb.h>
+#include <gz/msgs/vector3d.pb.h>
+
+#include <gz/common/Console.hh>
+#include <gz/msgs/Utility.hh>
+#include <gz/plugin/Register.hh>
+
+#include "gz/gui/Application.hh"
+#include "gz/gui/Conversions.hh"
+#include "gz/gui/GuiEvents.hh"
+#include "gz/gui/MainWindow.hh"
+
+#include <gz/transport/Node.hh>
+
+#include "TrackConfig.hh"
+
+/// \brief Private data class for TrackConfig
+class gz::gui::plugins::TrackConfigPrivate
+{
+
+  /// \brief Topic for track message
+  public: std::string trackTopic;
+
+  /// \brief Pose of camera for tracking target
+  public: math::Vector3d trackPose{math::Vector3d(0.0, 0.0, 0.0)};
+
+  /// \brief track P gain
+  public: double trackPGain{0.01};
+
+  public: transport::Node node;
+
+  /// \brief Process updated track
+  public: void UpdateTrack();
+
+  /// \brief flag for updating
+  public: bool newTrackUpdate = false;
+
+  /// \brief track publisher
+  public: transport::Node::Publisher trackPub;
+};
+
+using namespace gz;
+using namespace gui;
+using namespace plugins;
+
+/////////////////////////////////////////////////
+TrackConfig::TrackConfig()
+  : gz::gui::Plugin(), dataPtr(std::make_unique<TrackConfigPrivate>())
+{
+}
+
+/////////////////////////////////////////////////
+TrackConfig::~TrackConfig() = default;
+
+/////////////////////////////////////////////////
+void TrackConfig::LoadConfig(const tinyxml2::XMLElement *)
+{
+  if (this->title.empty())
+    this->title = "Track Config";
+
+  // Track target pose service
+  this->dataPtr->trackTopic = "/gui/track";
+  this->dataPtr->trackPub =
+    this->dataPtr->node.Advertise<msgs::CameraTrack>(this->dataPtr->trackTopic);
+  gzmsg << "TrackConfig: Track topic publisher advertised on ["
+         << this->dataPtr->trackTopic << "]" << std::endl;
+
+  gui::App()->findChild<
+      MainWindow *>()->installEventFilter(this);
+}
+
+/////////////////////////////////////////////////
+bool TrackConfig::eventFilter(QObject *_obj, QEvent *_event)
+{
+  if (_event->type() == events::Render::kType)
+  {
+    if (this->dataPtr->newTrackUpdate)
+    {
+      this->dataPtr->UpdateTrack();
+    }
+  }
+
+  // Standard event processing
+  return QObject::eventFilter(_obj, _event);
+}
+
+/////////////////////////////////////////////////
+void TrackConfig::SetTrack(double _x,
+          double _y, double _z, double _p)
+{
+  if (!this->dataPtr->newTrackUpdate)
+  {
+    this->dataPtr->trackPose = math::Vector3d(
+      _x, _y, _z);
+    this->dataPtr->trackPGain = _p;
+    gzmsg << "TrackConfig: Track Pose("
+          << this->dataPtr->trackPose << "), PGain("
+          << this->dataPtr->trackPGain << ")" << std::endl;
+    this->dataPtr->newTrackUpdate = true;
+  }
+}
+
+/////////////////////////////////////////////////
+void TrackConfigPrivate::UpdateTrack()
+{
+  // Track
+  msgs::CameraTrack trackMsg;
+  trackMsg.set_track_mode(msgs::CameraTrack::TRACK);
+  trackMsg.mutable_track_pose()->set_x(this->trackPose.X());
+  trackMsg.mutable_track_pose()->set_y(this->trackPose.Y());
+  trackMsg.mutable_track_pose()->set_z(this->trackPose.Z());
+  trackMsg.set_pgain(this->trackPGain);
+
+  this->trackPub.Publish(trackMsg);
+  gzmsg << "TrackConfig: Publishing message." << std::endl;
+  this->newTrackUpdate = false;
+}
+
+// Register this plugin
+GZ_ADD_PLUGIN(TrackConfig,
+              gui::Plugin)
diff --git a/src/plugins/track_config/TrackConfig.hh b/src/plugins/track_config/TrackConfig.hh
new file mode 100644
index 000000000..a308621c9
--- /dev/null
+++ b/src/plugins/track_config/TrackConfig.hh
@@ -0,0 +1,64 @@
+/*
+ * Copyright (C) 2024 Rudis Laboratories LLC
+ *
+ * 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 GZ_GUI_PLUGINS_TRACKCONFIG_HH_
+#define GZ_GUI_PLUGINS_TRACKCONFIG_HH_
+
+#include <memory>
+
+#include "gz/gui/Plugin.hh"
+
+namespace gz
+{
+namespace gui
+{
+namespace plugins
+{
+  class TrackConfigPrivate;
+
+  class TrackConfig : public Plugin
+  {
+    Q_OBJECT
+
+    /// \brief Constructor
+    public: TrackConfig();
+
+    /// \brief Destructor
+    public: virtual ~TrackConfig();
+
+    // Documentation inherited
+    public: virtual void LoadConfig(const tinyxml2::XMLElement *)
+        override;
+
+    /// \brief Set the track camera pose location, requested from the GUI.
+    /// \param[in] _x The track camera pose location in x
+    /// \param[in] _y The track camera pose location in y
+    /// \param[in] _z The track camera pose location in z
+    /// \param[in] _p The track camera P gain
+    public slots: void SetTrack(double _x,
+          double _y, double _z, double _p);
+
+    // Documentation inherited
+    private: bool eventFilter(QObject *_obj, QEvent *_event) override;
+
+    /// \internal
+    /// \brief Pointer to private data.
+    private: std::unique_ptr<TrackConfigPrivate> dataPtr;
+  };
+}
+}
+}
+#endif
diff --git a/src/plugins/track_config/TrackConfig.qml b/src/plugins/track_config/TrackConfig.qml
new file mode 100644
index 000000000..013ead612
--- /dev/null
+++ b/src/plugins/track_config/TrackConfig.qml
@@ -0,0 +1,123 @@
+/*
+ * Copyright (C) 2024 Rudis Laboratories LLC
+ *
+ * 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.
+ *
+*/
+
+import QtQuick 2.9
+import QtQuick.Controls 2.2
+import QtQuick.Controls.Material 2.1
+import QtQuick.Controls.Styles 1.4
+import QtQuick.Layouts 1.3
+import gz.gui 1.0
+
+ColumnLayout {
+  Layout.minimumWidth: 200
+  Layout.minimumHeight: 200
+  Layout.margins: 2
+  anchors.fill: parent
+  focus: true
+
+  // X track camera pose
+  property double xTrackPose: -5.0
+  // Y track camera pose
+  property double yTrackPose: 0.0
+  // Z track camera pose
+  property double zTrackPose: 3.0
+  // P Gain track camera pose
+  property double pGainTrack: 0.01
+
+  GridLayout {
+    Layout.fillWidth: true
+    Layout.margins: 2
+    columns: 2
+
+    // X camera track pose 
+    Label {
+      id: xTrackPoseLabel
+      text: "Camera track pose X (m)"
+      color: "dimgrey"
+    }
+    GzSpinBox {
+      id: xTrackPoseField
+      Layout.fillWidth: true
+      value: xTrackPose
+      maximumValue: 1000.0
+      minimumValue: -1000.0
+      decimals: 2
+      stepSize: 0.5
+      onEditingFinished:{
+        xTrackPose = value
+        TrackConfig.SetTrack(xTrackPose, yTrackPose, zTrackPose, pGainTrack)
+      }
+    }
+    // Y camera track pose 
+    Label {
+      id: yTrackPoseLabel
+      text: "Camera track pose Y (m)"
+      color: "dimgrey"
+    }
+    GzSpinBox {
+      id: yTrackPoseField
+      Layout.fillWidth: true
+      value: yTrackPose
+      maximumValue: 1000.0
+      minimumValue: -1000.0
+      decimals: 2
+      stepSize: 0.5
+      onEditingFinished:{
+        yTrackPose = value
+        TrackConfig.SetTrack(xTrackPose, yTrackPose, zTrackPose, pGainTrack)
+      }
+    }
+    // Z camera track pose 
+    Label {
+      id: zTrackPoseLabel
+      text: "Camera track pose Z (m)"
+      color: "dimgrey"
+    }
+    GzSpinBox {
+      id: zTrackPoseField
+      Layout.fillWidth: true
+      value: zTrackPose
+      maximumValue: 1000.0
+      minimumValue: -1000.0
+      decimals: 2
+      stepSize: 0.5
+      onEditingFinished:{
+        zTrackPose = value
+        TrackConfig.SetTrack(xTrackPose, yTrackPose, zTrackPose, pGainTrack)
+      }
+    }
+    // P Gain track
+    Label {
+      id: pGainTrackLabel
+      text: "Camera Track P Gain"
+      color: "dimgrey"
+    }
+    GzSpinBox {
+      id: pGainTrackField
+      Layout.fillWidth: true
+      value: pGainTrack
+      maximumValue: 1.0
+      minimumValue: 0.001
+      decimals: 3
+      stepSize: 0.01
+      onEditingFinished:{
+        pGainTrack = value
+        TrackConfig.SetTrack(xTrackPose, yTrackPose, zTrackPose, pGainTrack)
+      }
+    }
+  }
+}
diff --git a/src/plugins/track_config/TrackConfig.qrc b/src/plugins/track_config/TrackConfig.qrc
new file mode 100644
index 000000000..33e3b03a1
--- /dev/null
+++ b/src/plugins/track_config/TrackConfig.qrc
@@ -0,0 +1,5 @@
+<!DOCTYPE RCC><RCC version="1.0">
+<qresource prefix="TrackConfig/">
+  <file>TrackConfig.qml</file>
+</qresource>
+</RCC>

From ac1c2012b6d8dc35e153fe103c8d4b9092b96577 Mon Sep 17 00:00:00 2001
From: Benjamin Perseghetti <bperseghetti@rudislabs.com>
Date: Thu, 9 May 2024 02:50:46 -0400
Subject: [PATCH 3/8] Camera track look at and unique targets and pgains.

Signed-off-by: Benjamin Perseghetti <bperseghetti@rudislabs.com>
---
 src/plugins/CMakeLists.txt                    |   3 +-
 src/plugins/camera_tracking/CameraTracking.cc | 214 +++++++++++++-----
 .../camera_tracking_config/CMakeLists.txt     |   6 +
 .../CameraTrackingConfig.cc                   | 157 +++++++++++++
 .../CameraTrackingConfig.hh}                  |  33 +--
 .../CameraTrackingConfig.qml                  | 207 +++++++++++++++++
 .../CameraTrackingConfig.qrc                  |   5 +
 src/plugins/follow_config/CMakeLists.txt      |   6 -
 src/plugins/follow_config/FollowConfig.cc     | 164 --------------
 src/plugins/follow_config/FollowConfig.hh     |  83 -------
 src/plugins/follow_config/FollowConfig.qml    | 132 -----------
 src/plugins/follow_config/FollowConfig.qrc    |   5 -
 src/plugins/track_config/CMakeLists.txt       |   6 -
 src/plugins/track_config/TrackConfig.cc       | 140 ------------
 src/plugins/track_config/TrackConfig.qml      | 123 ----------
 src/plugins/track_config/TrackConfig.qrc      |   5 -
 16 files changed, 547 insertions(+), 742 deletions(-)
 create mode 100644 src/plugins/camera_tracking_config/CMakeLists.txt
 create mode 100644 src/plugins/camera_tracking_config/CameraTrackingConfig.cc
 rename src/plugins/{track_config/TrackConfig.hh => camera_tracking_config/CameraTrackingConfig.hh} (54%)
 create mode 100644 src/plugins/camera_tracking_config/CameraTrackingConfig.qml
 create mode 100644 src/plugins/camera_tracking_config/CameraTrackingConfig.qrc
 delete mode 100644 src/plugins/follow_config/CMakeLists.txt
 delete mode 100644 src/plugins/follow_config/FollowConfig.cc
 delete mode 100644 src/plugins/follow_config/FollowConfig.hh
 delete mode 100644 src/plugins/follow_config/FollowConfig.qml
 delete mode 100644 src/plugins/follow_config/FollowConfig.qrc
 delete mode 100644 src/plugins/track_config/CMakeLists.txt
 delete mode 100644 src/plugins/track_config/TrackConfig.cc
 delete mode 100644 src/plugins/track_config/TrackConfig.qml
 delete mode 100644 src/plugins/track_config/TrackConfig.qrc

diff --git a/src/plugins/CMakeLists.txt b/src/plugins/CMakeLists.txt
index 27047b139..81ca4d848 100644
--- a/src/plugins/CMakeLists.txt
+++ b/src/plugins/CMakeLists.txt
@@ -116,8 +116,7 @@ endfunction()
 # Plugins
 add_subdirectory(camera_fps)
 add_subdirectory(camera_tracking)
-add_subdirectory(follow_config)
-add_subdirectory(track_config)
+add_subdirectory(camera_tracking_config)
 add_subdirectory(grid_config)
 add_subdirectory(image_display)
 add_subdirectory(interactive_view_control)
diff --git a/src/plugins/camera_tracking/CameraTracking.cc b/src/plugins/camera_tracking/CameraTracking.cc
index 1e306b877..c5bdea7c7 100644
--- a/src/plugins/camera_tracking/CameraTracking.cc
+++ b/src/plugins/camera_tracking/CameraTracking.cc
@@ -89,33 +89,36 @@ class CameraTrackingPrivate
   public: rendering::ScenePtr scene = nullptr;
 
   /// \brief Target to track
-  public: std::string selectedTarget;
+  public: std::string selectedTrackTarget;
+
+  /// \brief Target to follow
+  public: std::string selectedFollowTarget;
 
   /// \brief Wait for target to track
   public: bool selectedTargetWait = false;
 
-  /// \brief Offset of camera from target being tracked
-  public: math::Vector3d trackOffset = math::Vector3d(-5, 0, 3);
+  /// \brief Offset of camera from target being followed
+  public: math::Vector3d followOffset = math::Vector3d(-3, 0, 2);
 
-  /// \brief Pose of camera in world target is being tracked from
-  public: math::Vector3d trackPose = math::Vector3d(0, 0, 0);
+  /// \brief Offset on target to be tracked
+  public: math::Vector3d trackOffset = math::Vector3d(0, 0, 0);
 
   public: gz::msgs::CameraTrack trackMsg;
 
-  /// \brief Flag to indicate the tracking offset needs to be updated
-  public: bool trackOffsetDirty = false;
-
   /// \brief Flag to indicate new tracking
   public: bool newTrack = true;
 
   /// \brief Track P gain
   public: double trackPGain = 0.01;
 
+  /// \brief Follow P gain
+  public: double followPGain = 0.01;
+
   /// \brief Free Look P gain
   public: double freeLookPGain = 1.0;
 
-  /// \brief Default track mode to Follow
-  public: int trackMode = gz::msgs::CameraTrack::FOLLOW;
+  /// \brief Default track mode to None
+  public: int trackMode = gz::msgs::CameraTrack::NONE;
 
   /// \brief True track the target at an offset that is in world frame,
   /// false to track in target's local frame
@@ -240,27 +243,38 @@ void CameraTrackingPrivate::OnTrackSub(const msgs::CameraTrack &_msg)
   std::lock_guard<std::mutex> lock(this->mutex);
   gzmsg << "Got new track message."<< std::endl;
 
-  this->trackMode = _msg.track_mode();
-
-  if (!_msg.target().empty())
+  if (_msg.track_mode() != gz::msgs::CameraTrack::USE_LAST)
+  {
+    this->trackMode = _msg.track_mode();
+  }
+  if (!_msg.follow_target().empty())
+  {
+    this->selectedFollowTarget = _msg.follow_target(); 
+  }
+  if (!_msg.track_target().empty())
   {
-    this->selectedTarget = _msg.target(); 
+    this->selectedTrackTarget = _msg.track_target(); 
   }
-  else
+  if (_msg.follow_target().empty() && _msg.track_target().empty()
+        && _msg.track_mode() != gz::msgs::CameraTrack::USE_LAST)
   {
-    gzmsg << "Target name empty."<< std::endl;
+    gzmsg << "Track and Follow target names empty."<< std::endl;
   }
-  if (_msg.has_offset())
+  if (_msg.has_follow_offset())
   {
-    this->trackOffset = msgs::Convert(_msg.offset());
+    this->followOffset = msgs::Convert(_msg.follow_offset());
   }
-  if (_msg.has_track_pose())
+  if (_msg.has_track_offset())
   {
-    this->trackPose = msgs::Convert(_msg.track_pose());
+    this->trackOffset = msgs::Convert(_msg.track_offset());
   }
-  if (_msg.pgain() > 0.00001)
+  if (_msg.track_pgain() > 0.00001)
   {
-    this->trackPGain = _msg.pgain();
+    this->trackPGain = _msg.track_pgain();
+  }
+  if (_msg.follow_pgain() > 0.00001)
+  {
+    this->followPGain = _msg.follow_pgain();
   }
 
   this->newTrack = true;
@@ -381,14 +395,22 @@ void CameraTrackingPrivate::OnRender()
   {
     GZ_PROFILE("CameraTrackingPrivate::OnRender Track");
     // reset track mode if target node got removed
-    if (!this->selectedTarget.empty())
+    if (!this->selectedFollowTarget.empty())
     {
-      rendering::NodePtr target = this->scene->NodeByName(this->selectedTarget);
-      if (!target && !this->selectedTargetWait)
+      rendering::NodePtr targetFollow = this->scene->NodeByName(this->selectedFollowTarget);
+      if (!targetFollow && !this->selectedTargetWait)
       {
         this->camera->SetFollowTarget(nullptr);
+        this->selectedFollowTarget.clear();
+      }
+    }
+    if (!this->selectedTrackTarget.empty())
+    {
+      rendering::NodePtr targetTrack = this->scene->NodeByName(this->selectedTrackTarget);
+      if (!targetTrack && !this->selectedTargetWait)
+      {
         this->camera->SetTrackTarget(nullptr);
-        this->selectedTarget.clear();
+        this->selectedTrackTarget.clear();
       }
     }
 
@@ -396,29 +418,36 @@ void CameraTrackingPrivate::OnRender()
       return;
     rendering::NodePtr selectedFollowTargetTmp = this->camera->FollowTarget();
     rendering::NodePtr selectedTrackTargetTmp = this->camera->TrackTarget();
-    if (!this->selectedTarget.empty())
+    if (!this->selectedTrackTarget.empty() || !this->selectedFollowTarget.empty())
     {
-      rendering::NodePtr target = scene->NodeByName(
-          this->selectedTarget);
-      if (target)
+      rendering::NodePtr targetFollow = this->scene->NodeByName(this->selectedFollowTarget);
+      rendering::NodePtr targetTrack = this->scene->NodeByName(this->selectedTrackTarget);
+      if (targetFollow || targetTrack)
       {
         if (this->trackMode == gz::msgs::CameraTrack::FOLLOW_FREE_LOOK ||
-              this->trackMode == gz::msgs::CameraTrack::FOLLOW)
+              this->trackMode == gz::msgs::CameraTrack::FOLLOW ||
+              this->trackMode == gz::msgs::CameraTrack::FOLLOW_LOOK_AT )
         {
-          if (!selectedFollowTargetTmp || target != selectedFollowTargetTmp
+          if (!selectedFollowTargetTmp || targetFollow != selectedFollowTargetTmp
                 || this->newTrack)
           {
             this->trackWorldFrame = false;
-            this->camera->SetFollowTarget(target,
-                this->trackOffset,
+            this->camera->SetFollowTarget(targetFollow,
+                this->followOffset,
                 this->trackWorldFrame);
             if (this->trackMode == gz::msgs::CameraTrack::FOLLOW)
             {
-              this->camera->SetTrackTarget(target);
-              this->camera->SetTrackPGain(this->trackPGain);
+              this->camera->SetTrackTarget(targetFollow);
+              this->camera->SetTrackPGain(this->followPGain);
               this->camera->SetFollowPGain(this->trackPGain);
             }
-            else
+            if (this->trackMode == gz::msgs::CameraTrack::FOLLOW_LOOK_AT)
+            {
+              this->camera->SetTrackTarget(targetTrack);
+              this->camera->SetTrackPGain(this->followPGain);
+              this->camera->SetFollowPGain(this->trackPGain);
+            }
+            if (this->trackMode == gz::msgs::CameraTrack::FOLLOW_FREE_LOOK)
             {
               this->camera->SetTrackTarget(nullptr);
               this->camera->SetFollowPGain(this->freeLookPGain);
@@ -429,13 +458,13 @@ void CameraTrackingPrivate::OnRender()
         }
         if (this->trackMode == gz::msgs::CameraTrack::TRACK)
         {
-          if (!selectedTrackTargetTmp || target != selectedTrackTargetTmp
+          if (!selectedTrackTargetTmp || targetTrack != selectedTrackTargetTmp
                 || this->newTrack)
           {
             this->trackWorldFrame = true;
             this->camera->SetFollowTarget(nullptr);
-            this->camera->SetTrackTarget(target,
-                this->trackPose,
+            this->camera->SetTrackTarget(targetTrack,
+                this->trackOffset,
                 this->trackWorldFrame);
             this->camera->SetTrackPGain(this->trackPGain);
             this->newTrack = false;
@@ -446,8 +475,11 @@ void CameraTrackingPrivate::OnRender()
       else if (!this->selectedTargetWait)
       {
         gzerr << "Unable to track target. Target: '"
-               << this->selectedTarget << "' not found" << std::endl;
-        this->selectedTarget.clear();
+               << this->selectedTrackTarget << "' not found" << std::endl;
+        gzerr << "Unable to follow target. Target: '"
+               << this->selectedFollowTarget << "' not found" << std::endl;
+        this->selectedFollowTarget.clear();
+        this->selectedTrackTarget.clear();
       }
     }
     else
@@ -481,11 +513,67 @@ CameraTracking::CameraTracking()
     }
     if (this->dataPtr->trackStatusPub.HasConnections())
     {
-      this->dataPtr->trackMsg.set_target(this->dataPtr->selectedTarget);
-      this->dataPtr->trackMsg.mutable_offset()->set_x(this->dataPtr->trackOffset.X());
-      this->dataPtr->trackMsg.mutable_offset()->set_y(this->dataPtr->trackOffset.Y());
-      this->dataPtr->trackMsg.mutable_offset()->set_z(this->dataPtr->trackOffset.Z());
-      this->dataPtr->trackMsg.set_pgain(this->dataPtr->trackPGain);
+      if (this->dataPtr->trackMode == gz::msgs::CameraTrack::TRACK)
+      {
+        this->dataPtr->trackMsg.set_track_mode(gz::msgs::CameraTrack::TRACK);
+        this->dataPtr->trackMsg.set_track_target(this->dataPtr->selectedTrackTarget);
+        this->dataPtr->trackMsg.mutable_track_offset()->set_x(this->dataPtr->trackOffset.X());
+        this->dataPtr->trackMsg.mutable_track_offset()->set_y(this->dataPtr->trackOffset.Y());
+        this->dataPtr->trackMsg.mutable_track_offset()->set_z(this->dataPtr->trackOffset.Z());
+        this->dataPtr->trackMsg.set_track_pgain(this->dataPtr->trackPGain);
+        this->dataPtr->trackMsg.clear_follow_target();
+        this->dataPtr->trackMsg.clear_follow_offset();
+        this->dataPtr->trackMsg.clear_follow_pgain();
+      }
+      else if (this->dataPtr->trackMode == gz::msgs::CameraTrack::FOLLOW)
+      {
+        this->dataPtr->trackMsg.set_track_mode(gz::msgs::CameraTrack::FOLLOW);
+        this->dataPtr->trackMsg.set_follow_target(this->dataPtr->selectedFollowTarget);
+        this->dataPtr->trackMsg.mutable_follow_offset()->set_x(this->dataPtr->followOffset.X());
+        this->dataPtr->trackMsg.mutable_follow_offset()->set_y(this->dataPtr->followOffset.Y());
+        this->dataPtr->trackMsg.mutable_follow_offset()->set_z(this->dataPtr->followOffset.Z());
+        this->dataPtr->trackMsg.set_follow_pgain(this->dataPtr->followPGain);
+        this->dataPtr->trackMsg.clear_track_target();
+        this->dataPtr->trackMsg.clear_track_offset();
+        this->dataPtr->trackMsg.clear_track_pgain();
+      }
+      else if (this->dataPtr->trackMode == gz::msgs::CameraTrack::FOLLOW_FREE_LOOK)
+      {
+        this->dataPtr->trackMsg.set_track_mode(gz::msgs::CameraTrack::FOLLOW_FREE_LOOK);
+        this->dataPtr->trackMsg.set_follow_target(this->dataPtr->selectedFollowTarget);
+        this->dataPtr->trackMsg.mutable_follow_offset()->set_x(this->dataPtr->followOffset.X());
+        this->dataPtr->trackMsg.mutable_follow_offset()->set_y(this->dataPtr->followOffset.Y());
+        this->dataPtr->trackMsg.mutable_follow_offset()->set_z(this->dataPtr->followOffset.Z());
+        this->dataPtr->trackMsg.set_follow_pgain(this->dataPtr->followPGain);
+        this->dataPtr->trackMsg.clear_track_target();
+        this->dataPtr->trackMsg.clear_track_offset();
+        this->dataPtr->trackMsg.clear_track_pgain();
+      }
+      else if (this->dataPtr->trackMode == gz::msgs::CameraTrack::FOLLOW_LOOK_AT)
+      {
+        this->dataPtr->trackMsg.set_track_mode(gz::msgs::CameraTrack::FOLLOW_LOOK_AT);
+        this->dataPtr->trackMsg.set_follow_target(this->dataPtr->selectedFollowTarget);
+        this->dataPtr->trackMsg.set_track_target(this->dataPtr->selectedTrackTarget);
+        this->dataPtr->trackMsg.mutable_follow_offset()->set_x(this->dataPtr->followOffset.X());
+        this->dataPtr->trackMsg.mutable_follow_offset()->set_y(this->dataPtr->followOffset.Y());
+        this->dataPtr->trackMsg.mutable_follow_offset()->set_z(this->dataPtr->followOffset.Z());
+        this->dataPtr->trackMsg.mutable_track_offset()->set_x(this->dataPtr->trackOffset.X());
+        this->dataPtr->trackMsg.mutable_track_offset()->set_y(this->dataPtr->trackOffset.Y());
+        this->dataPtr->trackMsg.mutable_track_offset()->set_z(this->dataPtr->trackOffset.Z());
+        this->dataPtr->trackMsg.set_follow_pgain(this->dataPtr->followPGain);
+        this->dataPtr->trackMsg.set_track_pgain(this->dataPtr->trackPGain);
+      }
+      else
+      {
+        this->dataPtr->trackMsg.set_track_mode(gz::msgs::CameraTrack::NONE);
+        this->dataPtr->trackMsg.clear_track_target();
+        this->dataPtr->trackMsg.clear_track_offset();
+        this->dataPtr->trackMsg.clear_track_pgain();
+        this->dataPtr->trackMsg.clear_follow_target();
+        this->dataPtr->trackMsg.clear_follow_offset();
+        this->dataPtr->trackMsg.clear_follow_pgain();
+      }
+
       this->dataPtr->trackStatusPub.Publish(this->dataPtr->trackMsg);
     }
   });
@@ -504,27 +592,27 @@ void CameraTracking::LoadConfig(const tinyxml2::XMLElement *_pluginElem)
 
   if (_pluginElem)
   {
-    if (auto nameElem = _pluginElem->FirstChildElement("target"))
+    if (auto followTargetElem = _pluginElem->FirstChildElement("follow_target"))
     {
-      this->dataPtr->selectedTarget = nameElem->GetText();
-      gzmsg << "CameraTracking: Loaded target from sdf ["
-            << this->dataPtr->selectedTarget << "]" << std::endl;
+      this->dataPtr->selectedFollowTarget = followTargetElem->GetText();
+      gzmsg << "CameraTracking: Loaded follow target from sdf ["
+            << this->dataPtr->selectedFollowTarget << "]" << std::endl;
       this->dataPtr->selectedTargetWait = true;
     }
-    if (auto offsetElem = _pluginElem->FirstChildElement("offset"))
+    if (auto followOffsetElem = _pluginElem->FirstChildElement("follow_offset"))
     {
-      std::stringstream offsetStr;
-      offsetStr << std::string(offsetElem->GetText());
-      offsetStr >> this->dataPtr->trackOffset;
+      std::stringstream followOffsetStr;
+      followOffsetStr << std::string(followOffsetElem->GetText());
+      followOffsetStr >> this->dataPtr->followOffset;
       gzmsg << "CameraTracking: Loaded offset from sdf ["
-            << this->dataPtr->trackOffset << "]" << std::endl;
+            << this->dataPtr->followOffset << "]" << std::endl;
       this->dataPtr->newTrack = true;
     }
-    if (auto pGainElem = _pluginElem->FirstChildElement("pgain"))
+    if (auto followPGainElem = _pluginElem->FirstChildElement("follow_pgain"))
     {
-      this->dataPtr->trackPGain = std::stod(std::string(pGainElem->GetText()));
-      gzmsg << "CameraTracking: Loaded pgain from sdf ["
-            << this->dataPtr->trackPGain << "]" << std::endl;
+      this->dataPtr->followPGain = std::stod(std::string(followPGainElem->GetText()));
+      gzmsg << "CameraTracking: Loaded follow pgain from sdf ["
+            << this->dataPtr->followPGain << "]" << std::endl;
       this->dataPtr->newTrack = true;
     }
   }
@@ -537,9 +625,11 @@ void CameraTrackingPrivate::HandleKeyRelease(events::KeyReleaseOnScene *_e)
 {
   if (_e->Key().Key() == Qt::Key_Escape)
   {
-    if (!this->selectedTarget.empty())
+    this->trackMode = gz::msgs::CameraTrack::NONE;
+    if (!this->selectedFollowTarget.empty() || !this->selectedTrackTarget.empty() )
     {
-      this->selectedTarget = std::string();
+      this->selectedFollowTarget = std::string();
+      this->selectedTrackTarget = std::string();
 
       _e->accept();
     }
diff --git a/src/plugins/camera_tracking_config/CMakeLists.txt b/src/plugins/camera_tracking_config/CMakeLists.txt
new file mode 100644
index 000000000..b6877357c
--- /dev/null
+++ b/src/plugins/camera_tracking_config/CMakeLists.txt
@@ -0,0 +1,6 @@
+gz_gui_add_plugin(CameraTrackingConfig
+  SOURCES
+    CameraTrackingConfig.cc
+  QT_HEADERS
+    CameraTrackingConfig.hh
+)
diff --git a/src/plugins/camera_tracking_config/CameraTrackingConfig.cc b/src/plugins/camera_tracking_config/CameraTrackingConfig.cc
new file mode 100644
index 000000000..f3117077e
--- /dev/null
+++ b/src/plugins/camera_tracking_config/CameraTrackingConfig.cc
@@ -0,0 +1,157 @@
+/*
+ * Copyright (C) 2024 Rudis Laboratories LLC
+ *
+ * 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 <gz/msgs/double.pb.h>
+#include <gz/msgs/cameratrack.pb.h>
+#include <gz/msgs/vector3d.pb.h>
+
+#include <gz/common/Console.hh>
+#include <gz/msgs/Utility.hh>
+#include <gz/plugin/Register.hh>
+
+#include "gz/gui/Application.hh"
+#include "gz/gui/Conversions.hh"
+#include "gz/gui/GuiEvents.hh"
+#include "gz/gui/MainWindow.hh"
+
+#include <gz/transport/Node.hh>
+
+#include "CameraTrackingConfig.hh"
+
+/// \brief Private data class for CameraTrackingConfig
+class gz::gui::plugins::CameraTrackingConfigPrivate
+{
+
+  /// \brief Topic for track message
+  public: std::string cameraTrackingTopic;
+
+  /// \brief tracking offset
+  public: math::Vector3d trackOffset{math::Vector3d(0.0, 0.0, 0.0)};
+
+  /// \brief track P gain
+  public: double trackPGain{0.01};
+
+  /// \brief Offset of camera from target being followed
+  public: math::Vector3d followOffset{math::Vector3d(-3.0, 0.0, -2.0)};
+
+  /// \brief Follow P gain
+  public: double followPGain{0.01};
+
+  public: transport::Node node;
+
+  /// \brief Process updated track
+  public: void UpdateTracking();
+
+  /// \brief flag for updating
+  public: bool newTrackingUpdate = false;
+
+  /// \brief track publisher
+  public: transport::Node::Publisher trackingPub;
+};
+
+using namespace gz;
+using namespace gui;
+using namespace plugins;
+
+/////////////////////////////////////////////////
+CameraTrackingConfig::CameraTrackingConfig()
+  : gz::gui::Plugin(), dataPtr(std::make_unique<CameraTrackingConfigPrivate>())
+{
+}
+
+/////////////////////////////////////////////////
+CameraTrackingConfig::~CameraTrackingConfig() = default;
+
+/////////////////////////////////////////////////
+void CameraTrackingConfig::LoadConfig(const tinyxml2::XMLElement *)
+{
+  if (this->title.empty())
+    this->title = "Camera Tracking Config";
+
+  // Track target pose service
+  this->dataPtr->cameraTrackingTopic = "/gui/track";
+  this->dataPtr->trackingPub =
+    this->dataPtr->node.Advertise<msgs::CameraTrack>(this->dataPtr->cameraTrackingTopic);
+  gzmsg << "CameraTrackingConfig: Tracking topic publisher advertised on ["
+         << this->dataPtr->cameraTrackingTopic << "]" << std::endl;
+
+  gui::App()->findChild<
+      MainWindow *>()->installEventFilter(this);
+}
+
+/////////////////////////////////////////////////
+bool CameraTrackingConfig::eventFilter(QObject *_obj, QEvent *_event)
+{
+  if (_event->type() == events::Render::kType)
+  {
+    if (this->dataPtr->newTrackingUpdate)
+    {
+      this->dataPtr->UpdateTracking();
+    }
+  }
+
+  // Standard event processing
+  return QObject::eventFilter(_obj, _event);
+}
+
+/////////////////////////////////////////////////
+void CameraTrackingConfig::SetTracking(
+          double _tx, double _ty, double _tz, double _tp,
+          double _fx,double _fy, double _fz, double _fp)
+{
+  if (!this->dataPtr->newTrackingUpdate)
+  {
+    this->dataPtr->trackOffset = math::Vector3d(
+      _tx, _ty, _tz);
+    this->dataPtr->followOffset = math::Vector3d(
+      _fx, _fy, _fz);
+    this->dataPtr->trackPGain = _tp;
+    this->dataPtr->followPGain = _fp;
+    gzmsg << "CameraTrackingConfig: Track: Offset("
+          << this->dataPtr->trackOffset << "), PGain("
+          << this->dataPtr->trackPGain << ")" << std::endl;
+    gzmsg << "CameraTrackingConfig: Follow: Offset("
+          << this->dataPtr->followOffset << "), PGain("
+          << this->dataPtr->followPGain << ")" << std::endl;
+    this->dataPtr->newTrackingUpdate = true;
+  }
+}
+
+/////////////////////////////////////////////////
+void CameraTrackingConfigPrivate::UpdateTracking()
+{
+  // Track
+  msgs::CameraTrack trackingMsg;
+  trackingMsg.set_track_mode(msgs::CameraTrack::USE_LAST);
+  trackingMsg.mutable_track_offset()->set_x(this->trackOffset.X());
+  trackingMsg.mutable_track_offset()->set_y(this->trackOffset.Y());
+  trackingMsg.mutable_track_offset()->set_z(this->trackOffset.Z());
+  trackingMsg.mutable_follow_offset()->set_x(this->followOffset.X());
+  trackingMsg.mutable_follow_offset()->set_y(this->followOffset.Y());
+  trackingMsg.mutable_follow_offset()->set_z(this->followOffset.Z());
+  trackingMsg.set_follow_pgain(this->followPGain);
+  trackingMsg.set_track_pgain(this->trackPGain);
+
+  this->trackingPub.Publish(trackingMsg);
+  gzmsg << "CameraTrackingConfig: Publishing message." << std::endl;
+  this->newTrackingUpdate = false;
+}
+
+// Register this plugin
+GZ_ADD_PLUGIN(CameraTrackingConfig,
+              gui::Plugin)
diff --git a/src/plugins/track_config/TrackConfig.hh b/src/plugins/camera_tracking_config/CameraTrackingConfig.hh
similarity index 54%
rename from src/plugins/track_config/TrackConfig.hh
rename to src/plugins/camera_tracking_config/CameraTrackingConfig.hh
index a308621c9..4ec79d6e2 100644
--- a/src/plugins/track_config/TrackConfig.hh
+++ b/src/plugins/camera_tracking_config/CameraTrackingConfig.hh
@@ -14,8 +14,8 @@
  * limitations under the License.
  *
 */
-#ifndef GZ_GUI_PLUGINS_TRACKCONFIG_HH_
-#define GZ_GUI_PLUGINS_TRACKCONFIG_HH_
+#ifndef GZ_GUI_PLUGINS_CAMERATRACKINGCONFIG_HH_
+#define GZ_GUI_PLUGINS_CAMERATRACKINGCONFIG_HH_
 
 #include <memory>
 
@@ -27,36 +27,41 @@ namespace gui
 {
 namespace plugins
 {
-  class TrackConfigPrivate;
+  class CameraTrackingConfigPrivate;
 
-  class TrackConfig : public Plugin
+  class CameraTrackingConfig : public Plugin
   {
     Q_OBJECT
 
     /// \brief Constructor
-    public: TrackConfig();
+    public: CameraTrackingConfig();
 
     /// \brief Destructor
-    public: virtual ~TrackConfig();
+    public: virtual ~CameraTrackingConfig();
 
     // Documentation inherited
     public: virtual void LoadConfig(const tinyxml2::XMLElement *)
         override;
 
-    /// \brief Set the track camera pose location, requested from the GUI.
-    /// \param[in] _x The track camera pose location in x
-    /// \param[in] _y The track camera pose location in y
-    /// \param[in] _z The track camera pose location in z
-    /// \param[in] _p The track camera P gain
-    public slots: void SetTrack(double _x,
-          double _y, double _z, double _p);
+    /// \brief Set the tracking camera, requested from the GUI.
+    /// \param[in] _tx The track offset in x
+    /// \param[in] _ty The track offset in y
+    /// \param[in] _tz The track offset in z
+    /// \param[in] _tp The track camera P gain
+    /// \param[in] _fx The follow offset in x
+    /// \param[in] _fy The follow offset in y
+    /// \param[in] _fz The follow offset in z
+    /// \param[in] _fp The follow camera P gain
+    public slots: void SetTracking(
+          double _tx, double _ty, double _tz, double _tp,
+          double _fx, double _fy, double _fz, double _fp);
 
     // Documentation inherited
     private: bool eventFilter(QObject *_obj, QEvent *_event) override;
 
     /// \internal
     /// \brief Pointer to private data.
-    private: std::unique_ptr<TrackConfigPrivate> dataPtr;
+    private: std::unique_ptr<CameraTrackingConfigPrivate> dataPtr;
   };
 }
 }
diff --git a/src/plugins/camera_tracking_config/CameraTrackingConfig.qml b/src/plugins/camera_tracking_config/CameraTrackingConfig.qml
new file mode 100644
index 000000000..2f9a0fda6
--- /dev/null
+++ b/src/plugins/camera_tracking_config/CameraTrackingConfig.qml
@@ -0,0 +1,207 @@
+/*
+ * Copyright (C) 2024 Rudis Laboratories LLC
+ *
+ * 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.
+ *
+*/
+
+import QtQuick 2.9
+import QtQuick.Controls 2.2
+import QtQuick.Controls.Material 2.1
+import QtQuick.Controls.Styles 1.4
+import QtQuick.Layouts 1.3
+import gz.gui 1.0
+
+ColumnLayout {
+  Layout.minimumWidth: 200
+  Layout.minimumHeight: 200
+  Layout.margins: 2
+  anchors.fill: parent
+  focus: true
+
+  // X track camera pose
+  property double xTrackPose: 0.0
+  // Y track camera pose
+  property double yTrackPose: 0.0
+  // Z track camera pose
+  property double zTrackPose: 0.0
+  // P Gain track camera pose
+  property double pGainTrack: 0.01
+  // X camera follow distance
+  property double xFollowOffset: -3.0
+  // Y camera follow distance
+  property double yFollowOffset: 0.0
+  // Z camera follow distance
+  property double zFollowOffset: 2.0
+  // P Gain camera follow distance
+  property double pGainFollow: 0.01
+
+  GridLayout {
+    Layout.fillWidth: true
+    Layout.margins: 2
+    columns: 2
+
+    // X Track Offset 
+    Label {
+      id: xTrackPoseLabel
+      text: "Track Offset X (m)"
+      color: "dimgrey"
+    }
+    GzSpinBox {
+      id: xTrackPoseField
+      Layout.fillWidth: true
+      value: xTrackPose
+      maximumValue: 1000.0
+      minimumValue: -1000.0
+      decimals: 2
+      stepSize: 0.5
+      onEditingFinished:{
+        xTrackPose = value
+        CameraTrackingConfig.SetTracking(xTrackPose, yTrackPose, zTrackPose, pGainTrack, xFollowOffset, yFollowOffset, zFollowOffset, pGainFollow)
+      }
+    }
+    // Y Track Offset 
+    Label {
+      id: yTrackPoseLabel
+      text: "Track Offset Y (m)"
+      color: "dimgrey"
+    }
+    GzSpinBox {
+      id: yTrackPoseField
+      Layout.fillWidth: true
+      value: yTrackPose
+      maximumValue: 1000.0
+      minimumValue: -1000.0
+      decimals: 2
+      stepSize: 0.5
+      onEditingFinished:{
+        yTrackPose = value
+        CameraTrackingConfig.SetTracking(xTrackPose, yTrackPose, zTrackPose, pGainTrack, xFollowOffset, yFollowOffset, zFollowOffset, pGainFollow)
+      }
+    }
+    // Z Track Offset 
+    Label {
+      id: zTrackPoseLabel
+      text: "Track Offset Z (m)"
+      color: "dimgrey"
+    }
+    GzSpinBox {
+      id: zTrackPoseField
+      Layout.fillWidth: true
+      value: zTrackPose
+      maximumValue: 1000.0
+      minimumValue: -1000.0
+      decimals: 2
+      stepSize: 0.5
+      onEditingFinished:{
+        zTrackPose = value
+        CameraTrackingConfig.SetTracking(xTrackPose, yTrackPose, zTrackPose, pGainTrack, xFollowOffset, yFollowOffset, zFollowOffset, pGainFollow)
+      }
+    }
+    // P Gain track
+    Label {
+      id: pGainTrackLabel
+      text: "Track P Gain"
+      color: "dimgrey"
+    }
+    GzSpinBox {
+      id: pGainTrackField
+      Layout.fillWidth: true
+      value: pGainTrack
+      maximumValue: 1.0
+      minimumValue: 0.001
+      decimals: 3
+      stepSize: 0.01
+      onEditingFinished:{
+        pGainTrack = value
+        CameraTrackingConfig.SetTracking(xTrackPose, yTrackPose, zTrackPose, pGainTrack, xFollowOffset, yFollowOffset, zFollowOffset, pGainFollow)
+      }
+    }
+    // X Follow Offset
+    Label {
+      id: xFollowOffsetLabel
+      text: "Follow Offset X (m)"
+      color: "dimgrey"
+    }
+    GzSpinBox {
+      id: xFollowOffsetField
+      Layout.fillWidth: true
+      value: xFollowOffset
+      maximumValue: 100.0
+      minimumValue: -100.0
+      decimals: 2
+      stepSize: 0.5
+      onEditingFinished:{
+        xFollowOffset = value
+        CameraTrackingConfig.SetTracking(xTrackPose, yTrackPose, zTrackPose, pGainTrack, xFollowOffset, yFollowOffset, zFollowOffset,pGainFollow)
+      }
+    }
+    // Y Follow Offset
+    Label {
+      id: yFollowOffsetLabel
+      text: "Follow Offset Y (m)"
+      color: "dimgrey"
+    }
+    GzSpinBox {
+      id: yFollowOffsetField
+      Layout.fillWidth: true
+      value: yFollowOffset
+      maximumValue: 100.0
+      minimumValue: -100.0
+      decimals: 2
+      stepSize: 0.5
+      onEditingFinished:{
+        yFollowOffset = value
+        CameraTrackingConfig.SetTracking(xTrackPose, yTrackPose, zTrackPose, pGainTrack, xFollowOffset, yFollowOffset, zFollowOffset,pGainFollow)
+      }
+    }
+    // Z Follow Offset
+    Label {
+      id: zFollowOffsetLabel
+      text: "Follow Offset Z (m)"
+      color: "dimgrey"
+    }
+    GzSpinBox {
+      id: zFollowOffsetField
+      Layout.fillWidth: true
+      value: zFollowOffset
+      maximumValue: 100.0
+      minimumValue: -100.0
+      decimals: 2
+      stepSize: 0.5
+      onEditingFinished:{
+        zFollowOffset = value
+        CameraTrackingConfig.SetTracking(xTrackPose, yTrackPose, zTrackPose, pGainTrack, xFollowOffset, yFollowOffset, zFollowOffset,pGainFollow)
+      }
+    }
+    // P Gain follow
+    Label {
+      id: pGainFollowLabel
+      text: "Follow Offset P Gain"
+      color: "dimgrey"
+    }
+    GzSpinBox {
+      id: pGainFollowField
+      Layout.fillWidth: true
+      value: pGainFollow
+      maximumValue: 1.0
+      minimumValue: 0.001
+      decimals: 3
+      stepSize: 0.01
+      onEditingFinished:{
+        pGainFollow = value
+        CameraTrackingConfig.SetTracking(xTrackPose, yTrackPose, zTrackPose, pGainTrack, xFollowOffset, yFollowOffset, zFollowOffset,pGainFollow)
+      }
+    }
+  }
+}
diff --git a/src/plugins/camera_tracking_config/CameraTrackingConfig.qrc b/src/plugins/camera_tracking_config/CameraTrackingConfig.qrc
new file mode 100644
index 000000000..5b9e0d08d
--- /dev/null
+++ b/src/plugins/camera_tracking_config/CameraTrackingConfig.qrc
@@ -0,0 +1,5 @@
+<!DOCTYPE RCC><RCC version="1.0">
+<qresource prefix="CameraTrackingConfig/">
+  <file>CameraTrackingConfig.qml</file>
+</qresource>
+</RCC>
diff --git a/src/plugins/follow_config/CMakeLists.txt b/src/plugins/follow_config/CMakeLists.txt
deleted file mode 100644
index 6710b1174..000000000
--- a/src/plugins/follow_config/CMakeLists.txt
+++ /dev/null
@@ -1,6 +0,0 @@
-gz_gui_add_plugin(FollowConfig
-  SOURCES
-    FollowConfig.cc
-  QT_HEADERS
-    FollowConfig.hh
-)
diff --git a/src/plugins/follow_config/FollowConfig.cc b/src/plugins/follow_config/FollowConfig.cc
deleted file mode 100644
index 9b89c1b26..000000000
--- a/src/plugins/follow_config/FollowConfig.cc
+++ /dev/null
@@ -1,164 +0,0 @@
-/*
- * Copyright (C) 2023 Rudis Laboratories LLC
- *
- * 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 <gz/msgs/double.pb.h>
-#include <gz/msgs/cameratrack.pb.h>
-#include <gz/msgs/vector3d.pb.h>
-
-#include <gz/common/Console.hh>
-#include <gz/msgs/Utility.hh>
-#include <gz/plugin/Register.hh>
-
-#include "gz/gui/Application.hh"
-#include "gz/gui/Conversions.hh"
-#include "gz/gui/GuiEvents.hh"
-#include "gz/gui/MainWindow.hh"
-
-#include <gz/transport/Node.hh>
-
-#include "FollowConfig.hh"
-
-/// \brief Private data class for FollowConfig
-class gz::gui::plugins::FollowConfigPrivate
-{
-
-  /// \brief Topic for follow message
-  public: std::string followTopic;
-
-  /// \brief Offset of camera from target being followed
-  public: math::Vector3d followOffset{math::Vector3d(-5.0, 0.0, 3.0)};
-
-  /// \brief Follow P gain
-  public: double followPGain{0.01};
-
-  /// \brief Flag used to set free look.
-  public: bool freelook{false};
-
-  public: transport::Node node;
-
-  /// \brief Process updated follow
-  public: void UpdateFollow();
-
-  /// \brief flag for updating
-  public: bool newFollowUpdate = false;
-
-  /// \brief follow publisher
-  public: transport::Node::Publisher followPub;
-};
-
-using namespace gz;
-using namespace gui;
-using namespace plugins;
-
-/////////////////////////////////////////////////
-FollowConfig::FollowConfig()
-  : gz::gui::Plugin(), dataPtr(std::make_unique<FollowConfigPrivate>())
-{
-}
-
-/////////////////////////////////////////////////
-FollowConfig::~FollowConfig() = default;
-
-/////////////////////////////////////////////////
-void FollowConfig::LoadConfig(const tinyxml2::XMLElement *)
-{
-  if (this->title.empty())
-    this->title = "Follow Config";
-
-  // Follow target offset service
-  this->dataPtr->followTopic = "/gui/track";
-  this->dataPtr->followPub =
-    this->dataPtr->node.Advertise<msgs::CameraTrack>(this->dataPtr->followTopic);
-  gzmsg << "FollowConfig: Follow topic publisher advertised on ["
-         << this->dataPtr->followTopic << "]" << std::endl;
-
-  gui::App()->findChild<
-      MainWindow *>()->installEventFilter(this);
-}
-
-/////////////////////////////////////////////////
-bool FollowConfig::eventFilter(QObject *_obj, QEvent *_event)
-{
-  if (_event->type() == events::Render::kType)
-  {
-    if (this->dataPtr->newFollowUpdate)
-    {
-      this->dataPtr->UpdateFollow();
-    }
-  }
-
-  // Standard event processing
-  return QObject::eventFilter(_obj, _event);
-}
-
-/////////////////////////////////////////////////
-void FollowConfig::SetFollow(double _x,
-          double _y, double _z, double _p)
-{
-  if (!this->dataPtr->newFollowUpdate)
-  {
-    this->dataPtr->followOffset = math::Vector3d(
-      _x, _y, _z);
-    this->dataPtr->followPGain = _p;
-    gzmsg << "FollowConfig: Offset("
-          << this->dataPtr->followOffset << "), PGain("
-          << this->dataPtr->followPGain << ")" << std::endl;
-    this->dataPtr->newFollowUpdate = true;
-  }
-}
-
-/////////////////////////////////////////////////
-bool FollowConfig::FreeLook() const
-{
-  return this->dataPtr->freelook;
-}
-
-/////////////////////////////////////////////////
-void FollowConfig::SetFreeLook(const bool &_freelook)
-{
-  this->dataPtr->freelook = _freelook;
-  this->FreeLookChanged();
-  this->dataPtr->UpdateFollow();
-}
-
-/////////////////////////////////////////////////
-void FollowConfigPrivate::UpdateFollow()
-{
-  // Follow
-  msgs::CameraTrack followMsg;
-  followMsg.mutable_offset()->set_x(this->followOffset.X());
-  followMsg.mutable_offset()->set_y(this->followOffset.Y());
-  followMsg.mutable_offset()->set_z(this->followOffset.Z());
-  followMsg.set_pgain(this->followPGain);
-  if (this->freelook)
-  {
-    followMsg.set_track_mode(msgs::CameraTrack::FOLLOW_FREE_LOOK);
-  }
-  else
-  {
-    followMsg.set_track_mode(msgs::CameraTrack::FOLLOW);
-  }
-
-  this->followPub.Publish(followMsg);
-  gzmsg << "FollowConfig: Publishing message." << std::endl;
-  this->newFollowUpdate = false;
-}
-
-// Register this plugin
-GZ_ADD_PLUGIN(FollowConfig,
-              gui::Plugin)
diff --git a/src/plugins/follow_config/FollowConfig.hh b/src/plugins/follow_config/FollowConfig.hh
deleted file mode 100644
index 4cebdcf40..000000000
--- a/src/plugins/follow_config/FollowConfig.hh
+++ /dev/null
@@ -1,83 +0,0 @@
-/*
- * Copyright (C) 2023 Rudis Laboratories LLC
- *
- * 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 GZ_GUI_PLUGINS_FOLLOWCONFIG_HH_
-#define GZ_GUI_PLUGINS_FOLLOWCONFIG_HH_
-
-#include <memory>
-
-#include "gz/gui/Plugin.hh"
-
-namespace gz
-{
-namespace gui
-{
-namespace plugins
-{
-  class FollowConfigPrivate;
-
-  class FollowConfig : public Plugin
-  {
-    Q_OBJECT
-
-    /// \brief Free Look
-    Q_PROPERTY(
-      bool freelook
-      READ FreeLook
-      WRITE SetFreeLook
-      NOTIFY FreeLookChanged
-    )
-
-    /// \brief Constructor
-    public: FollowConfig();
-
-    /// \brief Destructor
-    public: virtual ~FollowConfig();
-
-    // Documentation inherited
-    public: virtual void LoadConfig(const tinyxml2::XMLElement *)
-        override;
-
-    /// \brief Set the follow offset, requested from the GUI.
-    /// \param[in] _x The follow offset distance in x
-    /// \param[in] _y The follow offset distance in y
-    /// \param[in] _z The follow offset distance in z
-    /// \param[in] _p The follow pgain
-    public slots: void SetFollow(double _x,
-          double _y, double _z, double _p);
-
-    // Documentation inherited
-    private: bool eventFilter(QObject *_obj, QEvent *_event) override;
-
-    /// \brief Get whether it is free look
-    /// \return True if freelook
-    public: Q_INVOKABLE bool FreeLook() const;
-
-    /// \brief Set to free look
-    /// \param[in] _freelook True if free look
-    public: Q_INVOKABLE void SetFreeLook(const bool &_freelook);
-
-    /// \brief Notify that free look has changed
-    signals: void FreeLookChanged();
-
-    /// \internal
-    /// \brief Pointer to private data.
-    private: std::unique_ptr<FollowConfigPrivate> dataPtr;
-  };
-}
-}
-}
-#endif
diff --git a/src/plugins/follow_config/FollowConfig.qml b/src/plugins/follow_config/FollowConfig.qml
deleted file mode 100644
index 9f139d438..000000000
--- a/src/plugins/follow_config/FollowConfig.qml
+++ /dev/null
@@ -1,132 +0,0 @@
-/*
- * Copyright (C) 2023 Rudis Laboratories LLC
- *
- * 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.
- *
-*/
-
-import QtQuick 2.9
-import QtQuick.Controls 2.2
-import QtQuick.Controls.Material 2.1
-import QtQuick.Controls.Styles 1.4
-import QtQuick.Layouts 1.3
-import gz.gui 1.0
-
-ColumnLayout {
-  Layout.minimumWidth: 200
-  Layout.minimumHeight: 200
-  Layout.margins: 2
-  anchors.fill: parent
-  focus: true
-
-  // X camera follow distance
-  property double xFollowOffset: -5.0
-  // Y camera follow distance
-  property double yFollowOffset: 0.0
-  // Z camera follow distance
-  property double zFollowOffset: 3.0
-  // P Gain camera follow distance
-  property double pGainFollow: 0.01
-
-  CheckBox {
-    objectName: "freelookCheck"
-    text: qsTr("Free Look")
-    checked: FollowConfig.freelook
-    onClicked: {
-      FollowConfig.SetFreeLook(checked)
-    }
-  }
-
-  GridLayout {
-    Layout.fillWidth: true
-    Layout.margins: 2
-    columns: 2
-
-    // X follow offset distance
-    Label {
-      id: xFollowOffsetLabel
-      text: "Camera offset X (m)"
-      color: "dimgrey"
-    }
-    GzSpinBox {
-      id: xFollowOffsetField
-      Layout.fillWidth: true
-      value: xFollowOffset
-      maximumValue: 100.0
-      minimumValue: -100.0
-      decimals: 2
-      stepSize: 0.5
-      onEditingFinished:{
-        xFollowOffset = value
-        FollowConfig.SetFollow(xFollowOffset, yFollowOffset, zFollowOffset,pGainFollow)
-      }
-    }
-    // Y follow offset distance
-    Label {
-      id: yFollowOffsetLabel
-      text: "Camera offset Y (m)"
-      color: "dimgrey"
-    }
-    GzSpinBox {
-      id: yFollowOffsetField
-      Layout.fillWidth: true
-      value: yFollowOffset
-      maximumValue: 100.0
-      minimumValue: -100.0
-      decimals: 2
-      stepSize: 0.5
-      onEditingFinished:{
-        yFollowOffset = value
-        FollowConfig.SetFollow(xFollowOffset, yFollowOffset, zFollowOffset,pGainFollow)
-      }
-    }
-    // Z follow offset distance
-    Label {
-      id: zFollowOffsetLabel
-      text: "Camera offset Z (m)"
-      color: "dimgrey"
-    }
-    GzSpinBox {
-      id: zFollowOffsetField
-      Layout.fillWidth: true
-      value: zFollowOffset
-      maximumValue: 100.0
-      minimumValue: -100.0
-      decimals: 2
-      stepSize: 0.5
-      onEditingFinished:{
-        zFollowOffset = value
-        FollowConfig.SetFollow(xFollowOffset, yFollowOffset, zFollowOffset,pGainFollow)
-      }
-    }
-    // P Gain follow
-    Label {
-      id: pGainFollowLabel
-      text: "Camera offset P Gain"
-      color: "dimgrey"
-    }
-    GzSpinBox {
-      id: pGainFollowField
-      Layout.fillWidth: true
-      value: pGainFollow
-      maximumValue: 1.0
-      minimumValue: 0.001
-      decimals: 3
-      stepSize: 0.01
-      onEditingFinished:{
-        pGainFollow = value
-        FollowConfig.SetFollow(xFollowOffset, yFollowOffset, zFollowOffset,pGainFollow)
-      }
-    }
-  }
-}
diff --git a/src/plugins/follow_config/FollowConfig.qrc b/src/plugins/follow_config/FollowConfig.qrc
deleted file mode 100644
index 61099f8f1..000000000
--- a/src/plugins/follow_config/FollowConfig.qrc
+++ /dev/null
@@ -1,5 +0,0 @@
-<!DOCTYPE RCC><RCC version="1.0">
-<qresource prefix="FollowConfig/">
-  <file>FollowConfig.qml</file>
-</qresource>
-</RCC>
diff --git a/src/plugins/track_config/CMakeLists.txt b/src/plugins/track_config/CMakeLists.txt
deleted file mode 100644
index 9ca83e5ab..000000000
--- a/src/plugins/track_config/CMakeLists.txt
+++ /dev/null
@@ -1,6 +0,0 @@
-gz_gui_add_plugin(TrackConfig
-  SOURCES
-    TrackConfig.cc
-  QT_HEADERS
-    TrackConfig.hh
-)
diff --git a/src/plugins/track_config/TrackConfig.cc b/src/plugins/track_config/TrackConfig.cc
deleted file mode 100644
index 5e2308d4a..000000000
--- a/src/plugins/track_config/TrackConfig.cc
+++ /dev/null
@@ -1,140 +0,0 @@
-/*
- * Copyright (C) 2024 Rudis Laboratories LLC
- *
- * 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 <gz/msgs/double.pb.h>
-#include <gz/msgs/cameratrack.pb.h>
-#include <gz/msgs/vector3d.pb.h>
-
-#include <gz/common/Console.hh>
-#include <gz/msgs/Utility.hh>
-#include <gz/plugin/Register.hh>
-
-#include "gz/gui/Application.hh"
-#include "gz/gui/Conversions.hh"
-#include "gz/gui/GuiEvents.hh"
-#include "gz/gui/MainWindow.hh"
-
-#include <gz/transport/Node.hh>
-
-#include "TrackConfig.hh"
-
-/// \brief Private data class for TrackConfig
-class gz::gui::plugins::TrackConfigPrivate
-{
-
-  /// \brief Topic for track message
-  public: std::string trackTopic;
-
-  /// \brief Pose of camera for tracking target
-  public: math::Vector3d trackPose{math::Vector3d(0.0, 0.0, 0.0)};
-
-  /// \brief track P gain
-  public: double trackPGain{0.01};
-
-  public: transport::Node node;
-
-  /// \brief Process updated track
-  public: void UpdateTrack();
-
-  /// \brief flag for updating
-  public: bool newTrackUpdate = false;
-
-  /// \brief track publisher
-  public: transport::Node::Publisher trackPub;
-};
-
-using namespace gz;
-using namespace gui;
-using namespace plugins;
-
-/////////////////////////////////////////////////
-TrackConfig::TrackConfig()
-  : gz::gui::Plugin(), dataPtr(std::make_unique<TrackConfigPrivate>())
-{
-}
-
-/////////////////////////////////////////////////
-TrackConfig::~TrackConfig() = default;
-
-/////////////////////////////////////////////////
-void TrackConfig::LoadConfig(const tinyxml2::XMLElement *)
-{
-  if (this->title.empty())
-    this->title = "Track Config";
-
-  // Track target pose service
-  this->dataPtr->trackTopic = "/gui/track";
-  this->dataPtr->trackPub =
-    this->dataPtr->node.Advertise<msgs::CameraTrack>(this->dataPtr->trackTopic);
-  gzmsg << "TrackConfig: Track topic publisher advertised on ["
-         << this->dataPtr->trackTopic << "]" << std::endl;
-
-  gui::App()->findChild<
-      MainWindow *>()->installEventFilter(this);
-}
-
-/////////////////////////////////////////////////
-bool TrackConfig::eventFilter(QObject *_obj, QEvent *_event)
-{
-  if (_event->type() == events::Render::kType)
-  {
-    if (this->dataPtr->newTrackUpdate)
-    {
-      this->dataPtr->UpdateTrack();
-    }
-  }
-
-  // Standard event processing
-  return QObject::eventFilter(_obj, _event);
-}
-
-/////////////////////////////////////////////////
-void TrackConfig::SetTrack(double _x,
-          double _y, double _z, double _p)
-{
-  if (!this->dataPtr->newTrackUpdate)
-  {
-    this->dataPtr->trackPose = math::Vector3d(
-      _x, _y, _z);
-    this->dataPtr->trackPGain = _p;
-    gzmsg << "TrackConfig: Track Pose("
-          << this->dataPtr->trackPose << "), PGain("
-          << this->dataPtr->trackPGain << ")" << std::endl;
-    this->dataPtr->newTrackUpdate = true;
-  }
-}
-
-/////////////////////////////////////////////////
-void TrackConfigPrivate::UpdateTrack()
-{
-  // Track
-  msgs::CameraTrack trackMsg;
-  trackMsg.set_track_mode(msgs::CameraTrack::TRACK);
-  trackMsg.mutable_track_pose()->set_x(this->trackPose.X());
-  trackMsg.mutable_track_pose()->set_y(this->trackPose.Y());
-  trackMsg.mutable_track_pose()->set_z(this->trackPose.Z());
-  trackMsg.set_pgain(this->trackPGain);
-
-  this->trackPub.Publish(trackMsg);
-  gzmsg << "TrackConfig: Publishing message." << std::endl;
-  this->newTrackUpdate = false;
-}
-
-// Register this plugin
-GZ_ADD_PLUGIN(TrackConfig,
-              gui::Plugin)
diff --git a/src/plugins/track_config/TrackConfig.qml b/src/plugins/track_config/TrackConfig.qml
deleted file mode 100644
index 013ead612..000000000
--- a/src/plugins/track_config/TrackConfig.qml
+++ /dev/null
@@ -1,123 +0,0 @@
-/*
- * Copyright (C) 2024 Rudis Laboratories LLC
- *
- * 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.
- *
-*/
-
-import QtQuick 2.9
-import QtQuick.Controls 2.2
-import QtQuick.Controls.Material 2.1
-import QtQuick.Controls.Styles 1.4
-import QtQuick.Layouts 1.3
-import gz.gui 1.0
-
-ColumnLayout {
-  Layout.minimumWidth: 200
-  Layout.minimumHeight: 200
-  Layout.margins: 2
-  anchors.fill: parent
-  focus: true
-
-  // X track camera pose
-  property double xTrackPose: -5.0
-  // Y track camera pose
-  property double yTrackPose: 0.0
-  // Z track camera pose
-  property double zTrackPose: 3.0
-  // P Gain track camera pose
-  property double pGainTrack: 0.01
-
-  GridLayout {
-    Layout.fillWidth: true
-    Layout.margins: 2
-    columns: 2
-
-    // X camera track pose 
-    Label {
-      id: xTrackPoseLabel
-      text: "Camera track pose X (m)"
-      color: "dimgrey"
-    }
-    GzSpinBox {
-      id: xTrackPoseField
-      Layout.fillWidth: true
-      value: xTrackPose
-      maximumValue: 1000.0
-      minimumValue: -1000.0
-      decimals: 2
-      stepSize: 0.5
-      onEditingFinished:{
-        xTrackPose = value
-        TrackConfig.SetTrack(xTrackPose, yTrackPose, zTrackPose, pGainTrack)
-      }
-    }
-    // Y camera track pose 
-    Label {
-      id: yTrackPoseLabel
-      text: "Camera track pose Y (m)"
-      color: "dimgrey"
-    }
-    GzSpinBox {
-      id: yTrackPoseField
-      Layout.fillWidth: true
-      value: yTrackPose
-      maximumValue: 1000.0
-      minimumValue: -1000.0
-      decimals: 2
-      stepSize: 0.5
-      onEditingFinished:{
-        yTrackPose = value
-        TrackConfig.SetTrack(xTrackPose, yTrackPose, zTrackPose, pGainTrack)
-      }
-    }
-    // Z camera track pose 
-    Label {
-      id: zTrackPoseLabel
-      text: "Camera track pose Z (m)"
-      color: "dimgrey"
-    }
-    GzSpinBox {
-      id: zTrackPoseField
-      Layout.fillWidth: true
-      value: zTrackPose
-      maximumValue: 1000.0
-      minimumValue: -1000.0
-      decimals: 2
-      stepSize: 0.5
-      onEditingFinished:{
-        zTrackPose = value
-        TrackConfig.SetTrack(xTrackPose, yTrackPose, zTrackPose, pGainTrack)
-      }
-    }
-    // P Gain track
-    Label {
-      id: pGainTrackLabel
-      text: "Camera Track P Gain"
-      color: "dimgrey"
-    }
-    GzSpinBox {
-      id: pGainTrackField
-      Layout.fillWidth: true
-      value: pGainTrack
-      maximumValue: 1.0
-      minimumValue: 0.001
-      decimals: 3
-      stepSize: 0.01
-      onEditingFinished:{
-        pGainTrack = value
-        TrackConfig.SetTrack(xTrackPose, yTrackPose, zTrackPose, pGainTrack)
-      }
-    }
-  }
-}
diff --git a/src/plugins/track_config/TrackConfig.qrc b/src/plugins/track_config/TrackConfig.qrc
deleted file mode 100644
index 33e3b03a1..000000000
--- a/src/plugins/track_config/TrackConfig.qrc
+++ /dev/null
@@ -1,5 +0,0 @@
-<!DOCTYPE RCC><RCC version="1.0">
-<qresource prefix="TrackConfig/">
-  <file>TrackConfig.qml</file>
-</qresource>
-</RCC>

From 9f715e280416b418e5a4fbd6bf1d094d3269fa2c Mon Sep 17 00:00:00 2001
From: Benjamin Perseghetti <bperseghetti@rudislabs.com>
Date: Thu, 9 May 2024 23:28:16 -0400
Subject: [PATCH 4/8] Update camera tracking docs.

Signed-off-by: Benjamin Perseghetti <bperseghetti@rudislabs.com>
---
 examples/standalone/scene_provider/README.md  | 16 +++++++--
 src/plugins/camera_tracking/CameraTracking.cc | 33 ++++++++++++++++++-
 .../camera_tracking/CameraTracking.qml        |  4 ++-
 test/integration/camera_tracking.cc           | 11 -------
 4 files changed, 49 insertions(+), 15 deletions(-)

diff --git a/examples/standalone/scene_provider/README.md b/examples/standalone/scene_provider/README.md
index 3183a83e4..4d83fe9ca 100644
--- a/examples/standalone/scene_provider/README.md
+++ b/examples/standalone/scene_provider/README.md
@@ -52,14 +52,26 @@ Echo camera pose:
 gz topic -e -t /gui/camera/pose
 ```
 
-Follow box:
+Echo camera tracking information:
+
+```
+gz topic -e -t /gui/currently_tracked
+```
+
+Follow box from service (depricated):
 
 ```
 gz service -s /gui/follow --reqtype gz.msgs.StringMsg --reptype gz.msgs.Boolean --timeout 2000 --req 'data: "box_model"'
 ```
 
+Follow box from service (depricated):
+
+```
+gz topic -t /gui/track -m gz.msgs.CameraTrack -p 'track_mode: 2, follow_target: "box_model"'
+```
+
 Update follow offset:
 
 ```
-gz service -s /gui/follow/offset --reqtype gz.msgs.Vector3d --reptype gz.msgs.Boolean --timeout 2000 --req 'x: 5, y: 5, z: 5'
+gz topic -t /gui/track -m gz.msgs.CameraTrack -p 'track_mode: 2, follow_target: "box_model", follow_offset: {x: -1, y: 0, z: 1}'
 ```
diff --git a/src/plugins/camera_tracking/CameraTracking.cc b/src/plugins/camera_tracking/CameraTracking.cc
index c5bdea7c7..640b1daee 100644
--- a/src/plugins/camera_tracking/CameraTracking.cc
+++ b/src/plugins/camera_tracking/CameraTracking.cc
@@ -62,9 +62,16 @@ class CameraTrackingPrivate
       msgs::Boolean &_res);
 
   /// \brief Callback for a track message
-  /// \param[in] _msg Message consistes of the target to track, type of tracking, offset and pgain.
+  /// \param[in] _msg Message consists of the target to track, type of tracking, offset and pgain.
   public: void OnTrackSub(const msgs::CameraTrack &_msg);
 
+  /// \brief Callback for a follow request
+  /// \param[in] _msg Request message to set the target to follow.
+  /// \param[in] _res Response data
+  /// \return True if the request is received
+  public: bool OnFollow(const msgs::StringMsg &_msg,
+      msgs::Boolean &_res);
+
   /// \brief Callback for a move to pose request.
   /// \param[in] _msg GUICamera request message.
   /// \param[in] _res Response data
@@ -142,6 +149,9 @@ class CameraTrackingPrivate
   /// \brief Move to service
   public: std::string moveToService;
 
+  /// \brief Follow service
+  public: std::string followService;
+
   /// \brief The pose set from the move to pose service.
   public: std::optional<math::Pose3d> moveToPoseValue;
 
@@ -196,6 +206,12 @@ void CameraTrackingPrivate::Initialize()
   gzmsg << "Move to service on ["
          << this->moveToService << "]" << std::endl;
 
+  this->followService = "/gui/follow";
+  this->node.Advertise(this->followService,
+      &CameraTrackingPrivate::OnFollow, this);
+  gzmsg << "Follow service on ["
+         << this->followService << "]" << std::endl;
+
   // track
   this->trackTopic = "/gui/track";
   this->node.Subscribe(this->trackTopic, 
@@ -237,6 +253,21 @@ bool CameraTrackingPrivate::OnMoveTo(const msgs::StringMsg &_msg,
   return true;
 }
 
+/////////////////////////////////////////////////
+bool CameraTrackingPrivate::OnFollow(const msgs::StringMsg &_msg,
+  msgs::Boolean &_res)
+{
+  std::lock_guard<std::mutex> lock(this->mutex);
+  this->selectedFollowTarget = _msg.data();
+
+  _res.set_data(true);
+
+  this->trackMode = gz::msgs::CameraTrack::FOLLOW;
+
+  this->newTrack = true;
+  return true;
+}
+
 /////////////////////////////////////////////////
 void CameraTrackingPrivate::OnTrackSub(const msgs::CameraTrack &_msg)
 {
diff --git a/src/plugins/camera_tracking/CameraTracking.qml b/src/plugins/camera_tracking/CameraTracking.qml
index acdf09575..a8c82d45e 100644
--- a/src/plugins/camera_tracking/CameraTracking.qml
+++ b/src/plugins/camera_tracking/CameraTracking.qml
@@ -27,10 +27,12 @@ ColumnLayout {
   anchors.margins: 10
 
   property string message: 'Services provided:<br><ul>' +
+      '<li>/gui/follow/pose</li>' +
       '<li>/gui/move_to</li>' +
       '<li>/gui/move_to/pose</li>' +
       '<li>/gui/track</li></ul><br>Topics provided:<br><ul>' +
-      '<li>/gui/camera/pose</li></ul>'
+      '<li>/gui/camera/pose</li>'+
+      '<li>/gui/currently_tracked</li></ul>'
 
   Label {
     Layout.fillWidth: true
diff --git a/test/integration/camera_tracking.cc b/test/integration/camera_tracking.cc
index 010e12c62..0bb8a2a3d 100644
--- a/test/integration/camera_tracking.cc
+++ b/test/integration/camera_tracking.cc
@@ -186,17 +186,6 @@ TEST(MinimalSceneTest, GZ_UTILS_TEST_ENABLED_ONLY_ON_LINUX(Config))
   EXPECT_TRUE(result);
   EXPECT_TRUE(rep.data());
 
-  msgs::Vector3d reqOffset;
-  reqOffset.set_x(1.0);
-  reqOffset.set_y(1.0);
-  reqOffset.set_z(1.0);
-  result = false;
-  executed = node.Request("/gui/follow/offset", reqOffset, timeout, rep,
-      result);
-  EXPECT_TRUE(executed);
-  EXPECT_TRUE(result);
-  EXPECT_TRUE(rep.data());
-
   // Many update loops to process many events
   maxSleep = 600;
   for (auto it : {150.0, 200.0})

From 703705411b3191a6a780b2249bd524a88315e5c3 Mon Sep 17 00:00:00 2001
From: Benjamin Perseghetti <bperseghetti@rudislabs.com>
Date: Fri, 10 May 2024 02:42:15 -0400
Subject: [PATCH 5/8] Apply suggestions from code review
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Co-authored-by: Alejandro Hernández Cordero <ahcorde@gmail.com>
Signed-off-by: Benjamin Perseghetti <bperseghetti@rudislabs.com>
---
 examples/standalone/scene_provider/README.md  |   4 +-
 src/plugins/camera_tracking/CameraTracking.cc | 119 +++++++++++-------
 .../CameraTrackingConfig.cc                   |   7 +-
 .../CameraTrackingConfig.hh                   |   2 +
 4 files changed, 85 insertions(+), 47 deletions(-)

diff --git a/examples/standalone/scene_provider/README.md b/examples/standalone/scene_provider/README.md
index 4d83fe9ca..79d837f50 100644
--- a/examples/standalone/scene_provider/README.md
+++ b/examples/standalone/scene_provider/README.md
@@ -54,7 +54,7 @@ gz topic -e -t /gui/camera/pose
 
 Echo camera tracking information:
 
-```
+```bash
 gz topic -e -t /gui/currently_tracked
 ```
 
@@ -66,7 +66,7 @@ gz service -s /gui/follow --reqtype gz.msgs.StringMsg --reptype gz.msgs.Boolean
 
 Follow box from service (depricated):
 
-```
+```bash
 gz topic -t /gui/track -m gz.msgs.CameraTrack -p 'track_mode: 2, follow_target: "box_model"'
 ```
 
diff --git a/src/plugins/camera_tracking/CameraTracking.cc b/src/plugins/camera_tracking/CameraTracking.cc
index 640b1daee..7452450c0 100644
--- a/src/plugins/camera_tracking/CameraTracking.cc
+++ b/src/plugins/camera_tracking/CameraTracking.cc
@@ -20,10 +20,10 @@
 #include <string>
 
 #include <gz/msgs/boolean.pb.h>
+#include <gz/msgs/cameratrack.pb.h>
 #include <gz/msgs/gui_camera.pb.h>
 #include <gz/msgs/stringmsg.pb.h>
 #include <gz/msgs/vector3d.pb.h>
-#include <gz/msgs/cameratrack.pb.h>
 
 #include <gz/common/Console.hh>
 #include <gz/common/Profiler.hh>
@@ -62,7 +62,7 @@ class CameraTrackingPrivate
       msgs::Boolean &_res);
 
   /// \brief Callback for a track message
-  /// \param[in] _msg Message consists of the target to track, type of tracking, offset and pgain.
+  /// \param[in] _msg Message is of type CamerTrack.
   public: void OnTrackSub(const msgs::CameraTrack &_msg);
 
   /// \brief Callback for a follow request
@@ -214,7 +214,7 @@ void CameraTrackingPrivate::Initialize()
 
   // track
   this->trackTopic = "/gui/track";
-  this->node.Subscribe(this->trackTopic, 
+  this->node.Subscribe(this->trackTopic,
       &CameraTrackingPrivate::OnTrackSub, this);
   gzmsg << "Tracking topic on ["
          << this->trackTopic << "]" << std::endl;
@@ -272,7 +272,7 @@ bool CameraTrackingPrivate::OnFollow(const msgs::StringMsg &_msg,
 void CameraTrackingPrivate::OnTrackSub(const msgs::CameraTrack &_msg)
 {
   std::lock_guard<std::mutex> lock(this->mutex);
-  gzmsg << "Got new track message."<< std::endl;
+  gzmsg << "Got new track message." << std::endl;
 
   if (_msg.track_mode() != gz::msgs::CameraTrack::USE_LAST)
   {
@@ -280,11 +280,11 @@ void CameraTrackingPrivate::OnTrackSub(const msgs::CameraTrack &_msg)
   }
   if (!_msg.follow_target().empty())
   {
-    this->selectedFollowTarget = _msg.follow_target(); 
+    this->selectedFollowTarget = _msg.follow_target();
   }
   if (!_msg.track_target().empty())
   {
-    this->selectedTrackTarget = _msg.track_target(); 
+    this->selectedTrackTarget = _msg.track_target();
   }
   if (_msg.follow_target().empty() && _msg.track_target().empty()
         && _msg.track_mode() != gz::msgs::CameraTrack::USE_LAST)
@@ -428,7 +428,8 @@ void CameraTrackingPrivate::OnRender()
     // reset track mode if target node got removed
     if (!this->selectedFollowTarget.empty())
     {
-      rendering::NodePtr targetFollow = this->scene->NodeByName(this->selectedFollowTarget);
+      rendering::NodePtr targetFollow = this->scene->NodeByName(
+            this->selectedFollowTarget);
       if (!targetFollow && !this->selectedTargetWait)
       {
         this->camera->SetFollowTarget(nullptr);
@@ -437,7 +438,8 @@ void CameraTrackingPrivate::OnRender()
     }
     if (!this->selectedTrackTarget.empty())
     {
-      rendering::NodePtr targetTrack = this->scene->NodeByName(this->selectedTrackTarget);
+      rendering::NodePtr targetTrack = this->scene->NodeByName(
+            this->selectedTrackTarget);
       if (!targetTrack && !this->selectedTargetWait)
       {
         this->camera->SetTrackTarget(nullptr);
@@ -449,18 +451,22 @@ void CameraTrackingPrivate::OnRender()
       return;
     rendering::NodePtr selectedFollowTargetTmp = this->camera->FollowTarget();
     rendering::NodePtr selectedTrackTargetTmp = this->camera->TrackTarget();
-    if (!this->selectedTrackTarget.empty() || !this->selectedFollowTarget.empty())
+    if (!this->selectedTrackTarget.empty() ||
+          !this->selectedFollowTarget.empty())
     {
-      rendering::NodePtr targetFollow = this->scene->NodeByName(this->selectedFollowTarget);
-      rendering::NodePtr targetTrack = this->scene->NodeByName(this->selectedTrackTarget);
+      rendering::NodePtr targetFollow = this->scene->NodeByName(
+            this->selectedFollowTarget);
+      rendering::NodePtr targetTrack = this->scene->NodeByName(
+            this->selectedTrackTarget);
       if (targetFollow || targetTrack)
       {
         if (this->trackMode == gz::msgs::CameraTrack::FOLLOW_FREE_LOOK ||
               this->trackMode == gz::msgs::CameraTrack::FOLLOW ||
               this->trackMode == gz::msgs::CameraTrack::FOLLOW_LOOK_AT )
         {
-          if (!selectedFollowTargetTmp || targetFollow != selectedFollowTargetTmp
-                || this->newTrack)
+          if (!selectedFollowTargetTmp ||
+                targetFollow != selectedFollowTargetTmp ||
+                this->newTrack)
           {
             this->trackWorldFrame = false;
             this->camera->SetFollowTarget(targetFollow,
@@ -489,8 +495,9 @@ void CameraTrackingPrivate::OnRender()
         }
         if (this->trackMode == gz::msgs::CameraTrack::TRACK)
         {
-          if (!selectedTrackTargetTmp || targetTrack != selectedTrackTargetTmp
-                || this->newTrack)
+          if (!selectedTrackTargetTmp ||
+                targetTrack != selectedTrackTargetTmp ||
+                this->newTrack)
           {
             this->trackWorldFrame = true;
             this->camera->SetFollowTarget(nullptr);
@@ -547,11 +554,16 @@ CameraTracking::CameraTracking()
       if (this->dataPtr->trackMode == gz::msgs::CameraTrack::TRACK)
       {
         this->dataPtr->trackMsg.set_track_mode(gz::msgs::CameraTrack::TRACK);
-        this->dataPtr->trackMsg.set_track_target(this->dataPtr->selectedTrackTarget);
-        this->dataPtr->trackMsg.mutable_track_offset()->set_x(this->dataPtr->trackOffset.X());
-        this->dataPtr->trackMsg.mutable_track_offset()->set_y(this->dataPtr->trackOffset.Y());
-        this->dataPtr->trackMsg.mutable_track_offset()->set_z(this->dataPtr->trackOffset.Z());
-        this->dataPtr->trackMsg.set_track_pgain(this->dataPtr->trackPGain);
+        this->dataPtr->trackMsg.set_track_target(
+              this->dataPtr->selectedTrackTarget);
+        this->dataPtr->trackMsg.mutable_track_offset()->set_x(
+              this->dataPtr->trackOffset.X());
+        this->dataPtr->trackMsg.mutable_track_offset()->set_y(
+              this->dataPtr->trackOffset.Y());
+        this->dataPtr->trackMsg.mutable_track_offset()->set_z(
+              this->dataPtr->trackOffset.Z());
+        this->dataPtr->trackMsg.set_track_pgain(
+              this->dataPtr->trackPGain);
         this->dataPtr->trackMsg.clear_follow_target();
         this->dataPtr->trackMsg.clear_follow_offset();
         this->dataPtr->trackMsg.clear_follow_pgain();
@@ -559,38 +571,58 @@ CameraTracking::CameraTracking()
       else if (this->dataPtr->trackMode == gz::msgs::CameraTrack::FOLLOW)
       {
         this->dataPtr->trackMsg.set_track_mode(gz::msgs::CameraTrack::FOLLOW);
-        this->dataPtr->trackMsg.set_follow_target(this->dataPtr->selectedFollowTarget);
-        this->dataPtr->trackMsg.mutable_follow_offset()->set_x(this->dataPtr->followOffset.X());
-        this->dataPtr->trackMsg.mutable_follow_offset()->set_y(this->dataPtr->followOffset.Y());
-        this->dataPtr->trackMsg.mutable_follow_offset()->set_z(this->dataPtr->followOffset.Z());
+        this->dataPtr->trackMsg.set_follow_target(
+              this->dataPtr->selectedFollowTarget);
+        this->dataPtr->trackMsg.mutable_follow_offset()->set_x(
+              this->dataPtr->followOffset.X());
+        this->dataPtr->trackMsg.mutable_follow_offset()->set_y(
+              this->dataPtr->followOffset.Y());
+        this->dataPtr->trackMsg.mutable_follow_offset()->set_z(
+              this->dataPtr->followOffset.Z());
         this->dataPtr->trackMsg.set_follow_pgain(this->dataPtr->followPGain);
         this->dataPtr->trackMsg.clear_track_target();
         this->dataPtr->trackMsg.clear_track_offset();
         this->dataPtr->trackMsg.clear_track_pgain();
       }
-      else if (this->dataPtr->trackMode == gz::msgs::CameraTrack::FOLLOW_FREE_LOOK)
+      else if (this->dataPtr->trackMode ==
+            gz::msgs::CameraTrack::FOLLOW_FREE_LOOK)
       {
-        this->dataPtr->trackMsg.set_track_mode(gz::msgs::CameraTrack::FOLLOW_FREE_LOOK);
-        this->dataPtr->trackMsg.set_follow_target(this->dataPtr->selectedFollowTarget);
-        this->dataPtr->trackMsg.mutable_follow_offset()->set_x(this->dataPtr->followOffset.X());
-        this->dataPtr->trackMsg.mutable_follow_offset()->set_y(this->dataPtr->followOffset.Y());
-        this->dataPtr->trackMsg.mutable_follow_offset()->set_z(this->dataPtr->followOffset.Z());
+        this->dataPtr->trackMsg.set_track_mode(
+              gz::msgs::CameraTrack::FOLLOW_FREE_LOOK);
+        this->dataPtr->trackMsg.set_follow_target(
+              this->dataPtr->selectedFollowTarget);
+        this->dataPtr->trackMsg.mutable_follow_offset()->set_x(
+              this->dataPtr->followOffset.X());
+        this->dataPtr->trackMsg.mutable_follow_offset()->set_y(
+              this->dataPtr->followOffset.Y());
+        this->dataPtr->trackMsg.mutable_follow_offset()->set_z(
+              this->dataPtr->followOffset.Z());
         this->dataPtr->trackMsg.set_follow_pgain(this->dataPtr->followPGain);
         this->dataPtr->trackMsg.clear_track_target();
         this->dataPtr->trackMsg.clear_track_offset();
         this->dataPtr->trackMsg.clear_track_pgain();
       }
-      else if (this->dataPtr->trackMode == gz::msgs::CameraTrack::FOLLOW_LOOK_AT)
+      else if (this->dataPtr->trackMode ==
+            gz::msgs::CameraTrack::FOLLOW_LOOK_AT)
       {
-        this->dataPtr->trackMsg.set_track_mode(gz::msgs::CameraTrack::FOLLOW_LOOK_AT);
-        this->dataPtr->trackMsg.set_follow_target(this->dataPtr->selectedFollowTarget);
-        this->dataPtr->trackMsg.set_track_target(this->dataPtr->selectedTrackTarget);
-        this->dataPtr->trackMsg.mutable_follow_offset()->set_x(this->dataPtr->followOffset.X());
-        this->dataPtr->trackMsg.mutable_follow_offset()->set_y(this->dataPtr->followOffset.Y());
-        this->dataPtr->trackMsg.mutable_follow_offset()->set_z(this->dataPtr->followOffset.Z());
-        this->dataPtr->trackMsg.mutable_track_offset()->set_x(this->dataPtr->trackOffset.X());
-        this->dataPtr->trackMsg.mutable_track_offset()->set_y(this->dataPtr->trackOffset.Y());
-        this->dataPtr->trackMsg.mutable_track_offset()->set_z(this->dataPtr->trackOffset.Z());
+        this->dataPtr->trackMsg.set_track_mode(
+              gz::msgs::CameraTrack::FOLLOW_LOOK_AT);
+        this->dataPtr->trackMsg.set_follow_target(
+              this->dataPtr->selectedFollowTarget);
+        this->dataPtr->trackMsg.set_track_target(
+              this->dataPtr->selectedTrackTarget);
+        this->dataPtr->trackMsg.mutable_follow_offset()->set_x(
+              this->dataPtr->followOffset.X());
+        this->dataPtr->trackMsg.mutable_follow_offset()->set_y(
+              this->dataPtr->followOffset.Y());
+        this->dataPtr->trackMsg.mutable_follow_offset()->set_z(
+              this->dataPtr->followOffset.Z());
+        this->dataPtr->trackMsg.mutable_track_offset()->set_x(
+              this->dataPtr->trackOffset.X());
+        this->dataPtr->trackMsg.mutable_track_offset()->set_y(
+              this->dataPtr->trackOffset.Y());
+        this->dataPtr->trackMsg.mutable_track_offset()->set_z(
+              this->dataPtr->trackOffset.Z());
         this->dataPtr->trackMsg.set_follow_pgain(this->dataPtr->followPGain);
         this->dataPtr->trackMsg.set_track_pgain(this->dataPtr->trackPGain);
       }
@@ -641,7 +673,8 @@ void CameraTracking::LoadConfig(const tinyxml2::XMLElement *_pluginElem)
     }
     if (auto followPGainElem = _pluginElem->FirstChildElement("follow_pgain"))
     {
-      this->dataPtr->followPGain = std::stod(std::string(followPGainElem->GetText()));
+      this->dataPtr->followPGain = std::stod(
+            std::string(followPGainElem->GetText()));
       gzmsg << "CameraTracking: Loaded follow pgain from sdf ["
             << this->dataPtr->followPGain << "]" << std::endl;
       this->dataPtr->newTrack = true;
@@ -657,7 +690,8 @@ void CameraTrackingPrivate::HandleKeyRelease(events::KeyReleaseOnScene *_e)
   if (_e->Key().Key() == Qt::Key_Escape)
   {
     this->trackMode = gz::msgs::CameraTrack::NONE;
-    if (!this->selectedFollowTarget.empty() || !this->selectedTrackTarget.empty() )
+    if (!this->selectedFollowTarget.empty() ||
+          !this->selectedTrackTarget.empty())
     {
       this->selectedFollowTarget = std::string();
       this->selectedTrackTarget = std::string();
@@ -686,7 +720,6 @@ bool CameraTracking::eventFilter(QObject *_obj, QEvent *_event)
   // Standard event processing
   return QObject::eventFilter(_obj, _event);
 }
-
 }  // namespace gz::gui::plugins
 
 // Register this plugin
diff --git a/src/plugins/camera_tracking_config/CameraTrackingConfig.cc b/src/plugins/camera_tracking_config/CameraTrackingConfig.cc
index f3117077e..6eefd4731 100644
--- a/src/plugins/camera_tracking_config/CameraTrackingConfig.cc
+++ b/src/plugins/camera_tracking_config/CameraTrackingConfig.cc
@@ -14,6 +14,7 @@
  * limitations under the License.
  *
 */
+#include <memory>
 #include <string>
 
 #include <gz/msgs/double.pb.h>
@@ -21,6 +22,7 @@
 #include <gz/msgs/vector3d.pb.h>
 
 #include <gz/common/Console.hh>
+#include <gz/math/Vector3.h>
 #include <gz/msgs/Utility.hh>
 #include <gz/plugin/Register.hh>
 
@@ -86,7 +88,8 @@ void CameraTrackingConfig::LoadConfig(const tinyxml2::XMLElement *)
   // Track target pose service
   this->dataPtr->cameraTrackingTopic = "/gui/track";
   this->dataPtr->trackingPub =
-    this->dataPtr->node.Advertise<msgs::CameraTrack>(this->dataPtr->cameraTrackingTopic);
+    this->dataPtr->node.Advertise<msgs::CameraTrack>(
+          this->dataPtr->cameraTrackingTopic);
   gzmsg << "CameraTrackingConfig: Tracking topic publisher advertised on ["
          << this->dataPtr->cameraTrackingTopic << "]" << std::endl;
 
@@ -112,7 +115,7 @@ bool CameraTrackingConfig::eventFilter(QObject *_obj, QEvent *_event)
 /////////////////////////////////////////////////
 void CameraTrackingConfig::SetTracking(
           double _tx, double _ty, double _tz, double _tp,
-          double _fx,double _fy, double _fz, double _fp)
+          double _fx, double _fy, double _fz, double _fp)
 {
   if (!this->dataPtr->newTrackingUpdate)
   {
diff --git a/src/plugins/camera_tracking_config/CameraTrackingConfig.hh b/src/plugins/camera_tracking_config/CameraTrackingConfig.hh
index 4ec79d6e2..5080d8ea6 100644
--- a/src/plugins/camera_tracking_config/CameraTrackingConfig.hh
+++ b/src/plugins/camera_tracking_config/CameraTrackingConfig.hh
@@ -18,6 +18,8 @@
 #define GZ_GUI_PLUGINS_CAMERATRACKINGCONFIG_HH_
 
 #include <memory>
+#include <QEvent>
+#include <QObject>
 
 #include "gz/gui/Plugin.hh"
 

From db150f56623496c8d6991726cab5be50cd12ec70 Mon Sep 17 00:00:00 2001
From: Benjamin Perseghetti <bperseghetti@rudislabs.com>
Date: Fri, 10 May 2024 20:43:21 -0400
Subject: [PATCH 6/8] Add back depricated follow offset, use Entity instead of
 string.

Signed-off-by: Benjamin Perseghetti <bperseghetti@rudislabs.com>
---
 examples/standalone/scene_provider/README.md  | 34 ++++++++----
 src/plugins/camera_tracking/CameraTracking.cc | 55 +++++++++++++++----
 .../CameraTrackingConfig.cc                   |  5 +-
 test/integration/camera_tracking.cc           | 11 ++++
 4 files changed, 81 insertions(+), 24 deletions(-)

diff --git a/examples/standalone/scene_provider/README.md b/examples/standalone/scene_provider/README.md
index 79d837f50..342e94b95 100644
--- a/examples/standalone/scene_provider/README.md
+++ b/examples/standalone/scene_provider/README.md
@@ -9,7 +9,7 @@ plugin to update the scene using Gazebo Transport.
 
 ## Build
 
-```
+```bash
 cd examples/standalone/scene_provider
 mkdir build
 cd build
@@ -21,14 +21,14 @@ make
 
 In one terminal, start the scene provider:
 
-```
+```bash
 cd examples/standalone/scene_provider/build
 ./scene_provider
 ```
 
 On another terminal, start the example config:
 
-```
+```bash
 gz gui -c examples/config/scene3d.config
 ```
 
@@ -42,13 +42,13 @@ Some commands to test camera tracking with this demo:
 
 Move to box:
 
-```
+```bash
 gz service -s /gui/move_to --reqtype gz.msgs.StringMsg --reptype gz.msgs.Boolean --timeout 2000 --req 'data: "box_model"'
 ```
 
 Echo camera pose:
 
-```
+```bash
 gz topic -e -t /gui/camera/pose
 ```
 
@@ -58,20 +58,32 @@ Echo camera tracking information:
 gz topic -e -t /gui/currently_tracked
 ```
 
-Follow box from service (depricated):
+Follow box from track topic:
 
+```bash
+gz topic -t /gui/track -m gz.msgs.CameraTrack -p 'track_mode: 2, follow_target: { name: "box_model"}'
 ```
-gz service -s /gui/follow --reqtype gz.msgs.StringMsg --reptype gz.msgs.Boolean --timeout 2000 --req 'data: "box_model"'
+
+Follow box from track topic:
+
+```bash
+gz topic -t /gui/track -m gz.msgs.CameraTrack -p 'track_mode: 2, follow_target: "box_model", follow_offset: {x: -1, y: 0, z: 1}'
 ```
 
-Follow box from service (depricated):
+Update follow offset from track topic:
 
 ```bash
-gz topic -t /gui/track -m gz.msgs.CameraTrack -p 'track_mode: 2, follow_target: "box_model"'
+gz topic -t /gui/track -m gz.msgs.CameraTrack -p 'track_mode: 2, follow_target: {name: "box_model"}, follow_offset: {x: -1, y: 0, z: 1}'
 ```
 
-Update follow offset:
+Follow box from service (depricated):
 
+```bash
+gz service -s /gui/follow --reqtype gz.msgs.StringMsg --reptype gz.msgs.Boolean --timeout 2000 --req 'data: "box_model"'
 ```
-gz topic -t /gui/track -m gz.msgs.CameraTrack -p 'track_mode: 2, follow_target: "box_model", follow_offset: {x: -1, y: 0, z: 1}'
+
+Update follow offset from follow offset service (depricated):
+
+```bash
+gz service -s /gui/follow/offset --reqtype gz.msgs.Vector3d --reptype gz.msgs.Boolean --timeout 2000 --req 'x: 5, y: 5, z: 5'
 ```
diff --git a/src/plugins/camera_tracking/CameraTracking.cc b/src/plugins/camera_tracking/CameraTracking.cc
index 7452450c0..4392be5b5 100644
--- a/src/plugins/camera_tracking/CameraTracking.cc
+++ b/src/plugins/camera_tracking/CameraTracking.cc
@@ -21,6 +21,7 @@
 
 #include <gz/msgs/boolean.pb.h>
 #include <gz/msgs/cameratrack.pb.h>
+#include <gz/msgs/entity.pb.h>
 #include <gz/msgs/gui_camera.pb.h>
 #include <gz/msgs/stringmsg.pb.h>
 #include <gz/msgs/vector3d.pb.h>
@@ -62,7 +63,7 @@ class CameraTrackingPrivate
       msgs::Boolean &_res);
 
   /// \brief Callback for a track message
-  /// \param[in] _msg Message is of type CamerTrack.
+  /// \param[in] _msg Message is of type CameraTrack.
   public: void OnTrackSub(const msgs::CameraTrack &_msg);
 
   /// \brief Callback for a follow request
@@ -79,6 +80,13 @@ class CameraTrackingPrivate
   public: bool OnMoveToPose(const msgs::GUICamera &_msg,
                msgs::Boolean &_res);
 
+  /// \brief Callback for a follow offset request
+  /// \param[in] _msg Request message to set the camera's follow offset.
+  /// \param[in] _res Response data
+  /// \return True if the request is received
+  public: bool OnFollowOffset(const msgs::Vector3d &_msg,
+               msgs::Boolean &_res);
+
   /// \brief Callback when a move to animation is complete
   private: void OnMoveToComplete();
 
@@ -152,6 +160,9 @@ class CameraTrackingPrivate
   /// \brief Follow service
   public: std::string followService;
 
+  /// \brief Follow offset service
+  public: std::string followOffsetService;
+
   /// \brief The pose set from the move to pose service.
   public: std::optional<math::Pose3d> moveToPoseValue;
 
@@ -240,6 +251,13 @@ void CameraTrackingPrivate::Initialize()
     this->node.Advertise<msgs::Pose>(this->cameraPoseTopic);
   gzmsg << "Camera pose topic advertised on ["
          << this->cameraPoseTopic << "]" << std::endl;
+
+   // follow offset
+   this->followOffsetService = "/gui/follow/offset";
+   this->node.Advertise(this->followOffsetService,
+       &CameraTrackingPrivate::OnFollowOffset, this);
+   gzmsg << "Follow offset service on ["
+          << this->followOffsetService << "]" << std::endl;
 }
 
 /////////////////////////////////////////////////
@@ -278,15 +296,15 @@ void CameraTrackingPrivate::OnTrackSub(const msgs::CameraTrack &_msg)
   {
     this->trackMode = _msg.track_mode();
   }
-  if (!_msg.follow_target().empty())
+  if (!_msg.follow_target().name().empty())
   {
-    this->selectedFollowTarget = _msg.follow_target();
+    this->selectedFollowTarget = _msg.follow_target().name();
   }
-  if (!_msg.track_target().empty())
+  if (!_msg.track_target().name().empty())
   {
-    this->selectedTrackTarget = _msg.track_target();
+    this->selectedTrackTarget = _msg.track_target().name();
   }
-  if (_msg.follow_target().empty() && _msg.track_target().empty()
+  if (_msg.follow_target().name().empty() && _msg.track_target().name().empty()
         && _msg.track_mode() != gz::msgs::CameraTrack::USE_LAST)
   {
     gzmsg << "Track and Follow target names empty."<< std::endl;
@@ -324,6 +342,21 @@ void CameraTrackingPrivate::OnMoveToPoseComplete()
   this->moveToPoseValue.reset();
 }
 
+/////////////////////////////////////////////////
+bool CameraTrackingPrivate::OnFollowOffset(const msgs::Vector3d &_msg,
+  msgs::Boolean &_res)
+{
+  std::lock_guard<std::mutex> lock(this->mutex);
+  if (!this->selectedFollowTarget.empty())
+  {
+    this->newTrack = true;
+    this->followOffset = msgs::Convert(_msg);
+  }
+
+  _res.set_data(true);
+  return true;
+}
+
 /////////////////////////////////////////////////
 bool CameraTrackingPrivate::OnMoveToPose(const msgs::GUICamera &_msg,
   msgs::Boolean &_res)
@@ -554,7 +587,7 @@ CameraTracking::CameraTracking()
       if (this->dataPtr->trackMode == gz::msgs::CameraTrack::TRACK)
       {
         this->dataPtr->trackMsg.set_track_mode(gz::msgs::CameraTrack::TRACK);
-        this->dataPtr->trackMsg.set_track_target(
+        this->dataPtr->trackMsg.mutable_track_target()->set_name(
               this->dataPtr->selectedTrackTarget);
         this->dataPtr->trackMsg.mutable_track_offset()->set_x(
               this->dataPtr->trackOffset.X());
@@ -571,7 +604,7 @@ CameraTracking::CameraTracking()
       else if (this->dataPtr->trackMode == gz::msgs::CameraTrack::FOLLOW)
       {
         this->dataPtr->trackMsg.set_track_mode(gz::msgs::CameraTrack::FOLLOW);
-        this->dataPtr->trackMsg.set_follow_target(
+        this->dataPtr->trackMsg.mutable_follow_target()->set_name(
               this->dataPtr->selectedFollowTarget);
         this->dataPtr->trackMsg.mutable_follow_offset()->set_x(
               this->dataPtr->followOffset.X());
@@ -589,7 +622,7 @@ CameraTracking::CameraTracking()
       {
         this->dataPtr->trackMsg.set_track_mode(
               gz::msgs::CameraTrack::FOLLOW_FREE_LOOK);
-        this->dataPtr->trackMsg.set_follow_target(
+        this->dataPtr->trackMsg.mutable_follow_target()->set_name(
               this->dataPtr->selectedFollowTarget);
         this->dataPtr->trackMsg.mutable_follow_offset()->set_x(
               this->dataPtr->followOffset.X());
@@ -607,9 +640,9 @@ CameraTracking::CameraTracking()
       {
         this->dataPtr->trackMsg.set_track_mode(
               gz::msgs::CameraTrack::FOLLOW_LOOK_AT);
-        this->dataPtr->trackMsg.set_follow_target(
+        this->dataPtr->trackMsg.mutable_follow_target()->set_name(
               this->dataPtr->selectedFollowTarget);
-        this->dataPtr->trackMsg.set_track_target(
+        this->dataPtr->trackMsg.mutable_track_target()->set_name(
               this->dataPtr->selectedTrackTarget);
         this->dataPtr->trackMsg.mutable_follow_offset()->set_x(
               this->dataPtr->followOffset.X());
diff --git a/src/plugins/camera_tracking_config/CameraTrackingConfig.cc b/src/plugins/camera_tracking_config/CameraTrackingConfig.cc
index 6eefd4731..287fc8de4 100644
--- a/src/plugins/camera_tracking_config/CameraTrackingConfig.cc
+++ b/src/plugins/camera_tracking_config/CameraTrackingConfig.cc
@@ -17,12 +17,13 @@
 #include <memory>
 #include <string>
 
-#include <gz/msgs/double.pb.h>
 #include <gz/msgs/cameratrack.pb.h>
+#include <gz/msgs/double.pb.h>
+#include <gz/msgs/entity.pb.h>
 #include <gz/msgs/vector3d.pb.h>
 
 #include <gz/common/Console.hh>
-#include <gz/math/Vector3.h>
+#include <gz/math/Vector3.hh>
 #include <gz/msgs/Utility.hh>
 #include <gz/plugin/Register.hh>
 
diff --git a/test/integration/camera_tracking.cc b/test/integration/camera_tracking.cc
index 0bb8a2a3d..010e12c62 100644
--- a/test/integration/camera_tracking.cc
+++ b/test/integration/camera_tracking.cc
@@ -186,6 +186,17 @@ TEST(MinimalSceneTest, GZ_UTILS_TEST_ENABLED_ONLY_ON_LINUX(Config))
   EXPECT_TRUE(result);
   EXPECT_TRUE(rep.data());
 
+  msgs::Vector3d reqOffset;
+  reqOffset.set_x(1.0);
+  reqOffset.set_y(1.0);
+  reqOffset.set_z(1.0);
+  result = false;
+  executed = node.Request("/gui/follow/offset", reqOffset, timeout, rep,
+      result);
+  EXPECT_TRUE(executed);
+  EXPECT_TRUE(result);
+  EXPECT_TRUE(rep.data());
+
   // Many update loops to process many events
   maxSleep = 600;
   for (auto it : {150.0, 200.0})

From 0417d7609da4d8569698e511f68a2147c2295ab0 Mon Sep 17 00:00:00 2001
From: Benjamin Perseghetti <bperseghetti@rudislabs.com>
Date: Wed, 15 May 2024 09:20:40 -0400
Subject: [PATCH 7/8] Add includes.

Signed-off-by: Benjamin Perseghetti <bperseghetti@rudislabs.com>
---
 src/plugins/camera_tracking/CameraTracking.cc | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/src/plugins/camera_tracking/CameraTracking.cc b/src/plugins/camera_tracking/CameraTracking.cc
index 4392be5b5..c007e48f4 100644
--- a/src/plugins/camera_tracking/CameraTracking.cc
+++ b/src/plugins/camera_tracking/CameraTracking.cc
@@ -17,7 +17,9 @@
 */
 
 #include <mutex>
+#include <sstream>
 #include <string>
+#include <optional>
 
 #include <gz/msgs/boolean.pb.h>
 #include <gz/msgs/cameratrack.pb.h>

From 12516de5074262bd1888184b2575c0b3fc88eaeb Mon Sep 17 00:00:00 2001
From: Benjamin Perseghetti <bperseghetti@rudislabs.com>
Date: Sat, 18 May 2024 15:29:04 -0400
Subject: [PATCH 8/8] Add back /gui/follow doxygen.

Signed-off-by: Benjamin Perseghetti <bperseghetti@rudislabs.com>
---
 examples/standalone/scene_provider/README.md           |  4 ++--
 src/plugins/camera_tracking/CameraTracking.cc          | 10 ++++++----
 src/plugins/camera_tracking/CameraTracking.hh          |  3 +++
 .../camera_tracking_config/CameraTrackingConfig.cc     |  1 +
 .../camera_tracking_config/CameraTrackingConfig.hh     |  1 +
 .../camera_tracking_config/CameraTrackingConfig.qml    |  1 +
 6 files changed, 14 insertions(+), 6 deletions(-)

diff --git a/examples/standalone/scene_provider/README.md b/examples/standalone/scene_provider/README.md
index 342e94b95..ce2da00fc 100644
--- a/examples/standalone/scene_provider/README.md
+++ b/examples/standalone/scene_provider/README.md
@@ -76,13 +76,13 @@ Update follow offset from track topic:
 gz topic -t /gui/track -m gz.msgs.CameraTrack -p 'track_mode: 2, follow_target: {name: "box_model"}, follow_offset: {x: -1, y: 0, z: 1}'
 ```
 
-Follow box from service (depricated):
+Follow box from service (deprecated):
 
 ```bash
 gz service -s /gui/follow --reqtype gz.msgs.StringMsg --reptype gz.msgs.Boolean --timeout 2000 --req 'data: "box_model"'
 ```
 
-Update follow offset from follow offset service (depricated):
+Update follow offset from follow offset service (deprecated):
 
 ```bash
 gz service -s /gui/follow/offset --reqtype gz.msgs.Vector3d --reptype gz.msgs.Boolean --timeout 2000 --req 'x: 5, y: 5, z: 5'
diff --git a/src/plugins/camera_tracking/CameraTracking.cc b/src/plugins/camera_tracking/CameraTracking.cc
index c007e48f4..2042459d4 100644
--- a/src/plugins/camera_tracking/CameraTracking.cc
+++ b/src/plugins/camera_tracking/CameraTracking.cc
@@ -120,6 +120,7 @@ class CameraTrackingPrivate
   /// \brief Offset on target to be tracked
   public: math::Vector3d trackOffset = math::Vector3d(0, 0, 0);
 
+  /// \brief Camera tracking message
   public: gz::msgs::CameraTrack trackMsg;
 
   /// \brief Flag to indicate new tracking
@@ -159,10 +160,10 @@ class CameraTrackingPrivate
   /// \brief Move to service
   public: std::string moveToService;
 
-  /// \brief Follow service
+  /// \brief Follow service (deprecated)
   public: std::string followService;
 
-  /// \brief Follow offset service
+  /// \brief Follow offset service (deprecated)
   public: std::string followOffsetService;
 
   /// \brief The pose set from the move to pose service.
@@ -219,11 +220,12 @@ void CameraTrackingPrivate::Initialize()
   gzmsg << "Move to service on ["
          << this->moveToService << "]" << std::endl;
 
+  // follow
   this->followService = "/gui/follow";
   this->node.Advertise(this->followService,
       &CameraTrackingPrivate::OnFollow, this);
   gzmsg << "Follow service on ["
-         << this->followService << "]" << std::endl;
+         << this->followService << "] (deprecated)" << std::endl;
 
   // track
   this->trackTopic = "/gui/track";
@@ -259,7 +261,7 @@ void CameraTrackingPrivate::Initialize()
    this->node.Advertise(this->followOffsetService,
        &CameraTrackingPrivate::OnFollowOffset, this);
    gzmsg << "Follow offset service on ["
-          << this->followOffsetService << "]" << std::endl;
+          << this->followOffsetService << "] (deprecated)" << std::endl;
 }
 
 /////////////////////////////////////////////////
diff --git a/src/plugins/camera_tracking/CameraTracking.hh b/src/plugins/camera_tracking/CameraTracking.hh
index 395d99477..ae6833b9c 100644
--- a/src/plugins/camera_tracking/CameraTracking.hh
+++ b/src/plugins/camera_tracking/CameraTracking.hh
@@ -31,6 +31,9 @@ namespace gz::gui::plugins
   /// and "follow".
   ///
   /// Services:
+  /// * `/gui/follow`: Set the user camera to follow a given target,
+  ///                   identified by name (deprecated).
+  /// * `/gui/follow/offset`: Set the offset for following (deprecated).
   /// * `/gui/move_to`: Move the user camera to look at a given target,
   ///                   identified by name.
   /// * `/gui/move_to/pose`: Move the user camera to a given pose.
diff --git a/src/plugins/camera_tracking_config/CameraTrackingConfig.cc b/src/plugins/camera_tracking_config/CameraTrackingConfig.cc
index 287fc8de4..c61f8ac44 100644
--- a/src/plugins/camera_tracking_config/CameraTrackingConfig.cc
+++ b/src/plugins/camera_tracking_config/CameraTrackingConfig.cc
@@ -1,4 +1,5 @@
 /*
+ * Copyright (C) 2024 CogniPilot Foundation
  * Copyright (C) 2024 Rudis Laboratories LLC
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
diff --git a/src/plugins/camera_tracking_config/CameraTrackingConfig.hh b/src/plugins/camera_tracking_config/CameraTrackingConfig.hh
index 5080d8ea6..dca58a52e 100644
--- a/src/plugins/camera_tracking_config/CameraTrackingConfig.hh
+++ b/src/plugins/camera_tracking_config/CameraTrackingConfig.hh
@@ -1,4 +1,5 @@
 /*
+ * Copyright (C) 2024 CogniPilot Foundation
  * Copyright (C) 2024 Rudis Laboratories LLC
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
diff --git a/src/plugins/camera_tracking_config/CameraTrackingConfig.qml b/src/plugins/camera_tracking_config/CameraTrackingConfig.qml
index 2f9a0fda6..91c5e144a 100644
--- a/src/plugins/camera_tracking_config/CameraTrackingConfig.qml
+++ b/src/plugins/camera_tracking_config/CameraTrackingConfig.qml
@@ -1,4 +1,5 @@
 /*
+ * Copyright (C) 2024 CogniPilot Foundation
  * Copyright (C) 2024 Rudis Laboratories LLC
  *
  * Licensed under the Apache License, Version 2.0 (the "License");