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]> Co-authored-by: Alejandro Hernández Cordero <[email protected]> Co-authored-by: Ian Chen <[email protected]> Signed-off-by: Benjamin Perseghetti <[email protected]>
gazebosim · Feb 13, 2023 · 55e73fe · 55e73fe
1 parent 0b70f45
commit 55e73fe
Show file tree

Hide file tree

Showing 12 changed files with 658 additions and 18 deletions.
diff --git 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,36 +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 pgain:
+Set next follow p_gain:
 
-```
-gz service -s /gui/follow/pgain --reqtype gz.msgs.Double --reptype gz.msgs.Boolean --timeout 2000 --req 'data: 1.0'
+```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 pgain:
+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'
 ```
diff --git a/src/plugins/CMakeLists.txt b/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)

diff --git 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) 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.
@@ -226,7 +227,7 @@ void CameraTrackingPrivate::Initialize()
          << this->followOffsetService << "]" << std::endl;
 
   // follow pgain
-  this->followPGainService = "/gui/follow/pgain";
+  this->followPGainService = "/gui/follow/p_gain";
   this->node.Advertise(this->followPGainService,
       &CameraTrackingPrivate::OnFollowPGain, this);
   gzmsg << "Follow P gain service on ["
@@ -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;
 }

diff --git a/src/plugins/camera_tracking/CameraTracking.hh b/src/plugins/camera_tracking/CameraTracking.hh
@@ -39,7 +39,7 @@ namespace 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/pgain`: Set the pgain for following.
+  /// * `/gui/follow/p_gain`: Set the pgain for following.
   /// * `/gui/follow/offset`: Set the offset for following.
   ///
   /// Topics:

diff --git a/src/plugins/camera_tracking/CameraTracking.qml b/src/plugins/camera_tracking/CameraTracking.qml
@@ -29,7 +29,7 @@ ColumnLayout {
       '<li>/gui/move_to</li>' +
       '<li>/gui/move_to/pose</li>' +
       '<li>/gui/follow</li>' +
-      '<li>/gui/follow/pgain</li>' +
+      '<li>/gui/follow/p_gain</li>' +
       '<li>/gui/follow/offset</li></ul><br>Topics provided:<br><ul>' +
       '<li>/gui/camera/pose</li></ul>'
 

diff --git a/src/plugins/follow_config/CMakeLists.txt b/src/plugins/follow_config/CMakeLists.txt
@@ -0,0 +1,6 @@
+gz_gui_add_plugin(FollowConfig
+  SOURCES
+    FollowConfig.cc
+  QT_HEADERS
+    FollowConfig.hh
+)