Add service to change camera follow pgain.

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,24 +42,36 @@ 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
 ```
 
 Follow box:
 
-```
+```bash
 gz service -s /gui/follow --reqtype gz.msgs.StringMsg --reptype gz.msgs.Boolean --timeout 2000 --req 'data: "box_model"'
 ```
 
 Update follow offset:
 
-```
+```bash
 gz service -s /gui/follow/offset --reqtype gz.msgs.Vector3d --reptype gz.msgs.Boolean --timeout 2000 --req 'x: 5, y: 5, z: 5'
 ```
+
+Set next follow p_gain:
+
+```bash
+gz service -s /gui/follow/p_gain --reqtype gz.msgs.Double --reptype gz.msgs.Boolean --timeout 2000 --req 'data: 1.0'
+```
+
+Update follow offset with new p_gain:
+
+```bash
+gz service -s /gui/follow/offset --reqtype gz.msgs.Vector3d --reptype gz.msgs.Boolean --timeout 2000 --req 'x: 1, y: 1, z: 5'
+```

src/plugins/CMakeLists.txt

@@ -114,6 +114,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)

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.
@@ -77,6 +78,13 @@ class CameraTrackingPrivate
   public: bool OnFollowOffset(const msgs::Vector3d &_msg,
                msgs::Boolean &_res);
 
+  /// \brief Callback for a follow pgain request
+  /// \param[in] _msg Request message to set the camera's follow pgain.
+  /// \param[in] _res Response data
+  /// \return True if the request is received
+  public: bool OnFollowPGain(const msgs::Double &_msg,
+               msgs::Boolean &_res);
+
   /// \brief Callback when a move to animation is complete
   private: void OnMoveToComplete();
 
@@ -142,6 +150,9 @@ class CameraTrackingPrivate
   /// \brief Follow offset service
   public: std::string followOffsetService;
 
+  /// \brief Follow offset pgain service
+  public: std::string followPGainService;
+
   /// \brief Camera pose topic
   public: std::string cameraPoseTopic;
 
@@ -206,12 +217,19 @@ void CameraTrackingPrivate::Initialize()
   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;
+  // follow offset
+  this->followOffsetService = "/gui/follow/offset";
+  this->node.Advertise(this->followOffsetService,
+      &CameraTrackingPrivate::OnFollowOffset, this);
+  gzmsg << "Follow offset service on ["
+         << this->followOffsetService << "]" << std::endl;
+
+  // follow pgain
+  this->followPGainService = "/gui/follow/p_gain";
+  this->node.Advertise(this->followPGainService,
+      &CameraTrackingPrivate::OnFollowPGain, this);
+  gzmsg << "Follow P gain service on ["
+         << this->followPGainService << "]" << std::endl;
 }
 
 /////////////////////////////////////////////////
@@ -263,6 +281,20 @@ bool CameraTrackingPrivate::OnFollowOffset(const msgs::Vector3d &_msg,
   return true;
 }
 
+/////////////////////////////////////////////////
+bool CameraTrackingPrivate::OnFollowPGain(const msgs::Double &_msg,
+  msgs::Boolean &_res)
+{
+  std::lock_guard<std::mutex> lock(this->mutex);
+  if (!this->followTarget.empty())
+  {
+    this->newFollowOffset = true;
+    this->followPGain = msgs::Convert(_msg);
+  }
+  _res.set_data(true);
+  return true;
+}
+
 /////////////////////////////////////////////////
 bool CameraTrackingPrivate::OnMoveToPose(const msgs::GUICamera &_msg,
   msgs::Boolean &_res)
@@ -449,11 +481,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("follow_target"))
+    {
+      this->dataPtr->followTarget = nameElem->GetText();
+      gzmsg << "CameraTracking: Loaded follow_target from sdf ["
+            << this->dataPtr->followTarget << "]" << std::endl;
+      this->dataPtr->followTargetWait = 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->newFollowOffset = 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->newFollowOffset = true;
+    }
+  }
+
   App()->findChild<MainWindow *>()->installEventFilter(this);
 }
 

src/plugins/camera_tracking/CameraTracking.hh

@@ -35,6 +35,7 @@ namespace gz::gui::plugins
   /// * `/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/p_gain`: Set the pgain for following.
   /// * `/gui/follow/offset`: Set the offset for following.
   ///
   /// Topics:

src/plugins/camera_tracking/CameraTracking.qml

@@ -29,6 +29,7 @@ ColumnLayout {
       '<li>/gui/move_to</li>' +
       '<li>/gui/move_to/pose</li>' +
       '<li>/gui/follow</li>' +
+      '<li>/gui/follow/p_gain</li>' +
       '<li>/gui/follow/offset</li></ul><br>Topics provided:<br><ul>' +
       '<li>/gui/camera/pose</li></ul>'
 

src/plugins/follow_config/CMakeLists.txt

@@ -0,0 +1,6 @@
+gz_gui_add_plugin(FollowConfig
+  SOURCES
+    FollowConfig.cc
+  QT_HEADERS
+    FollowConfig.hh
+)