Create Follow Config plugin for camera following.

Allows for follow camera control set from sdf as well as gui.

Signed-off-by: Benjamin Perseghetti <[email protected]>

Co-authored-by: Jenn Nguyen <[email protected]>
Signed-off-by: Benjamin Perseghetti <[email protected]>

src/plugins/CMakeLists.txt

@@ -113,6 +113,7 @@ endfunction()
 
 # Plugins
 add_subdirectory(camera_tracking)
+add_subdirectory(follow_config)
 add_subdirectory(grid_config)
 add_subdirectory(image_display)
 add_subdirectory(interactive_view_control)

src/plugins/camera_tracking/CameraTracking.cc

@@ -1,5 +1,6 @@
 /*
  * Copyright (C) 2021 Open Source Robotics Foundation
+ * 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.
@@ -289,8 +290,11 @@ bool CameraTrackingPrivate::OnFollowPGain(const msgs::Double &_msg,
   msgs::Boolean &_res)
 {
   std::lock_guard<std::mutex> lock(this->mutex);
-  this->followPGain = msgs::Convert(_msg);
-
+  if (!this->followTarget.empty())
+  {
+    this->newFollowOffset = true;
+    this->followPGain = msgs::Convert(_msg);
+  }
   _res.set_data(true);
   return true;
 }

src/plugins/follow_config/CMakeLists.txt

@@ -0,0 +1,8 @@
+gz_gui_add_plugin(FollowConfig
+  SOURCES
+    FollowConfig.cc
+  QT_HEADERS
+    FollowConfig.hh
+  TEST_SOURCES
+    # FollowConfig_TEST.cc
+)

src/plugins/follow_config/FollowConfig.cc

@@ -0,0 +1,253 @@
+/*
+ * 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/stringmsg.pb.h>
+#include <gz/msgs/vector3d.pb.h>
+#include <gz/msgs/double.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
+{
+  public: std::string followTargetNameService;
+
+  public: std::string followOffsetService;
+
+  public: std::string followPGainService;
+
+  /// \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: std::string followTargetName;
+
+  public: transport::Node node;
+
+   public: void UpdateFollowTargetName();
+
+  public: void UpdateFollowOffset();
+
+  public: void UpdateFollowPGain();
+
+  public: bool newFollowUpdateTargetName = false;
+
+  public: bool newFollowUpdatePGain = false;
+
+  public: bool newFollowUpdateOffset = false;
+
+};
+
+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 *_pluginElem)
+{
+  if (this->title.empty())
+    this->title = "Follow Config";
+
+  // Follow target name service
+  this->dataPtr->followTargetNameService = "/gui/follow";
+  gzmsg << "FollowConfig: Follow target name service on ["
+         << this->dataPtr->followTargetNameService << "]" << std::endl;
+
+  // Follow target offset service
+  this->dataPtr->followOffsetService = "/gui/follow/offset";
+  gzmsg << "FollowConfig: Follow offset service on ["
+         << this->dataPtr->followOffsetService << "]" << std::endl;
+
+  // Follow target pgain service
+  this->dataPtr->followPGainService = "/gui/follow/pgain";
+  gzmsg << "FollowConfig: Follow P gain service on ["
+         << this->dataPtr->followPGainService << "]" << std::endl;
+
+
+  // Read configuration
+  if (_pluginElem)
+  {
+    if (auto nameElem = _pluginElem->FirstChildElement("follow_target"))
+    {
+      this->dataPtr->followTargetName = nameElem->GetText();
+      gzmsg << "FollowConfig: Loaded follow_target from sdf ["
+         << this->dataPtr->followTargetName << "]" << std::endl;
+      this->dataPtr->newFollowUpdateTargetName = true;
+    }
+    if (auto offsetElem = _pluginElem->FirstChildElement("follow_offset"))
+    {
+      std::stringstream offsetStr;
+      offsetStr << std::string(offsetElem->GetText());
+      offsetStr >> this->dataPtr->followOffset;
+      gzmsg << "FollowConfig: Loaded follow_offset from sdf ["
+         << this->dataPtr->followOffset << "]" << std::endl;
+      this->dataPtr->newFollowUpdateOffset = true;
+    }
+    if (auto pGainElem = _pluginElem->FirstChildElement("follow_pgain"))
+    {
+      this->dataPtr->followPGain = std::stod(std::string(pGainElem->GetText()));
+      gzmsg << "FollowConfig: Loaded follow_pgain from sdf ["
+         << this->dataPtr->followPGain << "]" << std::endl;
+      this->dataPtr->newFollowUpdatePGain = true;
+    }
+  }
+
+  gui::App()->findChild<
+      MainWindow *>()->installEventFilter(this);
+}
+
+/////////////////////////////////////////////////
+bool FollowConfig::eventFilter(QObject *_obj, QEvent *_event)
+{
+  if (_event->type() == events::Render::kType)
+  {
+    if (this->dataPtr->newFollowUpdateTargetName)
+    {
+      this->dataPtr->UpdateFollowTargetName();
+    }
+    if (this->dataPtr->newFollowUpdatePGain)
+    {
+      this->dataPtr->UpdateFollowPGain();
+    }
+    if (this->dataPtr->newFollowUpdateOffset)
+    {
+      this->dataPtr->UpdateFollowOffset();
+    }
+  }
+
+  // Standard event processing
+  return QObject::eventFilter(_obj, _event);
+}
+
+/////////////////////////////////////////////////
+void FollowConfig::SetFollowOffset(double _x,
+          double _y, double _z)
+{
+  if (!this->dataPtr->newFollowUpdateOffset)
+  {
+    this->dataPtr->followOffset = math::Vector3d(
+      _x, _y, _z);
+      gzmsg << "FollowConfig: SetFollowOffset("
+        << this->dataPtr->followOffset << ")" << std::endl;
+    this->dataPtr->newFollowUpdateOffset = true;
+  }
+}
+
+/////////////////////////////////////////////////
+void FollowConfig::SetFollowPGain(double _p)
+{
+  if (!this->dataPtr->newFollowUpdatePGain)
+  {
+    this->dataPtr->followPGain = _p;
+    gzmsg << "FollowConfig: SetFollowPGain("
+      << this->dataPtr->followPGain << ")" << std::endl;
+    this->dataPtr->newFollowUpdatePGain = true;
+  }
+}
+
+/////////////////////////////////////////////////
+void FollowConfigPrivate::UpdateFollowTargetName()
+{
+  // Offset
+  std::function<void(const msgs::Boolean &, const bool)> cbName =
+    [&](const msgs::Boolean &/*_rep*/, const bool _resultName)
+  {
+    if (!_resultName) {
+      gzerr << "FollowConfig: Error sending follow target name." << std::endl;
+    } else {
+      gzmsg << "FollowConfig: Request Target Name: "
+        << this->followTargetName << " sent" << std::endl;
+    }
+  };
+
+  msgs::StringMsg reqName;
+  reqName.set_data(this->followTargetName);
+  node.Request(this->followTargetNameService, reqName, cbName);
+  this->newFollowUpdateTargetName = false;
+}
+
+/////////////////////////////////////////////////
+void FollowConfigPrivate::UpdateFollowOffset()
+{
+  // Offset
+  std::function<void(const msgs::Boolean &, const bool)> cbOffset =
+    [&](const msgs::Boolean &/*_rep*/, const bool _resultOffset)
+  {
+    if (!_resultOffset) {
+      gzerr << "FollowConfig: Error sending follow offset." << std::endl;
+    } else {
+      gzmsg << "FollowConfig: Request Offset: "
+        << this->followOffset << " sent" << std::endl;
+    }
+  };
+
+  msgs::Vector3d reqOffset;
+  reqOffset.set_x(this->followOffset.X());
+  reqOffset.set_y(this->followOffset.Y());
+  reqOffset.set_z(this->followOffset.Z());
+  node.Request(this->followOffsetService, reqOffset, cbOffset);
+  this->newFollowUpdateOffset = false;
+}
+
+/////////////////////////////////////////////////
+void FollowConfigPrivate::UpdateFollowPGain()
+{
+  // PGain
+  std::function<void(const msgs::Boolean &, const bool)> cbPGain =
+    [&](const msgs::Boolean &/*_rep*/, const bool _resultPGain)
+  {
+    if (!_resultPGain) {
+      gzerr << "FollowConfig: Error sending follow pgain." << std::endl;
+    } else {
+      gzmsg << "FollowConfig: Request PGain: "
+        << this->followPGain << " sent" << std::endl;
+    }
+  };
+
+  msgs::Double reqPGain;
+  reqPGain.set_data(this->followPGain);
+  node.Request(this->followPGainService, reqPGain, cbPGain);
+  this->newFollowUpdatePGain = false;
+}
+
+// Register this plugin
+GZ_ADD_PLUGIN(FollowConfig,
+              gui::Plugin)

src/plugins/follow_config/FollowConfig.hh

@@ -0,0 +1,68 @@
+/*
+ * 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 *_pluginElem)
+        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
+    public slots: void SetFollowOffset(double _x,
+          double _y, double _z);
+
+    /// \brief Set the follow pgain, requested from the GUI.
+    /// \param[in] _p The follow offset distance in x
+    public slots: void SetFollowPGain(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