added follow camera offset service (#855)

Signed-off-by: Jenn Nguyen <jenn@openrobotics.org>

src/gui/plugins/scene3d/Scene3D.cc

@@ -244,12 +244,15 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
     /// \brief Wait for follow target
     public: bool followTargetWait = false;
 
-    /// \brief Offset of camera from taget being followed
+    /// \brief Offset of camera from target being followed
     public: math::Vector3d followOffset = math::Vector3d(-5, 0, 3);
 
     /// \brief Flag to indicate the follow offset needs to be updated
     public: bool followOffsetDirty = false;
 
+    /// \brief Flag to indicate the follow offset has been updated
+    public: bool newFollowOffset = true;
+
     /// \brief Follow P gain
     public: double followPGain = 0.01;
 
@@ -405,6 +408,9 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
     /// \brief Follow service
     public: std::string followService;
 
+    /// \brief Follow offset service
+    public: std::string followOffsetService;
+
     /// \brief View angle service
     public: std::string viewAngleService;
 
@@ -693,7 +699,8 @@ void IgnRenderer::Render()
           this->dataPtr->followTarget);
       if (target)
       {
-        if (!followTarget || target != followTarget)
+        if (!followTarget || target != followTarget
+              || this->dataPtr->newFollowOffset)
         {
           this->dataPtr->camera->SetFollowTarget(target,
               this->dataPtr->followOffset,
@@ -703,6 +710,7 @@ void IgnRenderer::Render()
           this->dataPtr->camera->SetTrackTarget(target);
           // found target, no need to wait anymore
           this->dataPtr->followTargetWait = false;
+          this->dataPtr->newFollowOffset = false;
         }
         else if (this->dataPtr->followOffsetDirty)
         {
@@ -2000,6 +2008,9 @@ void IgnRenderer::SetFollowOffset(const math::Vector3d &_offset)
 {
   std::lock_guard<std::mutex> lock(this->dataPtr->mutex);
   this->dataPtr->followOffset = _offset;
+
+  if (!this->dataPtr->followTarget.empty())
+    this->dataPtr->newFollowOffset = true;
 }
 
 /////////////////////////////////////////////////
@@ -2663,6 +2674,13 @@ void Scene3D::LoadConfig(const tinyxml2::XMLElement *_pluginElem)
   ignmsg << "Follow service on ["
          << this->dataPtr->followService << "]" << std::endl;
 
+  // follow offset
+  this->dataPtr->followOffsetService = "/gui/follow/offset";
+  this->dataPtr->node.Advertise(this->dataPtr->followOffsetService,
+      &Scene3D::OnFollowOffset, this);
+  ignmsg << "Follow offset service on ["
+         << this->dataPtr->followOffsetService << "]" << std::endl;
+
   // view angle
   this->dataPtr->viewAngleService =
       "/gui/view_angle";
@@ -2809,6 +2827,19 @@ bool Scene3D::OnFollow(const msgs::StringMsg &_msg,
   return true;
 }
 
+/////////////////////////////////////////////////
+bool Scene3D::OnFollowOffset(const msgs::Vector3d &_msg,
+  msgs::Boolean &_res)
+{
+  auto renderWindow = this->PluginItem()->findChild<RenderWindowItem *>();
+
+  math::Vector3d offset = msgs::Convert(_msg);
+  renderWindow->SetFollowOffset(offset);
+
+  _res.set_data(true);
+  return true;
+}
+
 /////////////////////////////////////////////////
 bool Scene3D::OnViewAngle(const msgs::Vector3d &_msg,
   msgs::Boolean &_res)

src/gui/plugins/scene3d/Scene3D.hh

@@ -140,6 +140,13 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
     private: bool OnFollow(const msgs::StringMsg &_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
+    private: bool OnFollowOffset(const msgs::Vector3d &_msg,
+        msgs::Boolean &_res);
+
     /// \brief Callback for a view angle request
     /// \param[in] _msg Request message to set the camera to.
     /// \param[in] _res Response data