Commits

Anonymous committed 0f72581

Adding camera tracking for visuals

  • Participants
  • Parent commits 0dc7f6a
  • Branches camera_follow

Comments (0)

Files changed (7)

File gazebo/gui/ModelRightMenu.cc

   this->moveToAct->setStatusTip(tr("Move camera to the selection"));
   connect(this->moveToAct, SIGNAL(triggered()), this, SLOT(OnMoveTo()));
 
+  this->followAct = new QAction(tr("Follow"), this);
+  this->followAct->setStatusTip(tr("Follow the selection"));
+  connect(this->followAct, SIGNAL(triggered()), this, SLOT(OnFollow()));
+
+
   // \todo Reimplement
   // this->snapBelowAct = new QAction(tr("Snap"), this);
   // this->snapBelowAct->setStatusTip(tr("Snap to object below"));
   this->viewStates.push_back(state);
 
   // \todo Reimplement
-  // this->followAction = new QAction(tr("Follow"), this);
-  // this->followAction->setStatusTip(tr("Follow the selection"));
-  // connect(this->followAction, SIGNAL(triggered()), this, SLOT(OnFollow()));
-
   // this->skeletonAction = new QAction(tr("Skeleton"), this);
   // this->skeletonAction->setStatusTip(tr("Show model skeleton"));
   // this->skeletonAction->setCheckable(true);
 
   QMenu menu;
   menu.addAction(this->moveToAct);
+  menu.addAction(this->followAct);
   // menu.addAction(this->snapBelowAct);
 
   // Create the view menu
   menu.addAction(g_deleteAct);
 
   // \todo Reimplement these features.
-  // menu.addAction(this->followAction);
   // menu.addAction(this->skeletonAction);
 
   menu.exec(_pt);
 }
 
 /////////////////////////////////////////////////
+void ModelRightMenu::OnFollow()
+{
+  rendering::UserCameraPtr cam = gui::get_active_camera();
+  cam->TrackVisual(this->modelName);
+}
+
+/////////////////////////////////////////////////
 // void ModelRightMenu::OnSnapBelow()
 // {
 //   rendering::UserCameraPtr cam = gui::get_active_camera();
 //
 //   this->requestPub->Publish(*this->requestMsg);
 // }
-
-/////////////////////////////////////////////////
-// void ModelRightMenu::OnFollow()
-// {
-//   rendering::UserCameraPtr cam = gui::get_active_camera();
-//   cam->TrackVisual(this->modelName);
-// }

File gazebo/gui/ModelRightMenu.hh

       /// \brief QT callback when move to has been selected.
       private slots: void OnMoveTo();
 
+      /// \brief QT callback when follow has been selected.
+      private slots: void OnFollow();
+
+
       /// \brief QT callback when delete has been selected.
       /// \param[in] _name Name of the model to delete.
       private slots: void OnDelete(const std::string &_name="");
       /// \brief QT callback when snap below has been selected.
       // private slots: void OnSnapBelow();
 
-      // private slots: void OnFollow();
       // private slots: void OnSkeleton();
 
       /// \brief Request callback.
       /// \brief Action for moving the camera to an object.
       private: QAction *moveToAct;
 
+      /// \brief Action for attaching the camera to a model.
+      private: QAction *followAct;
+
       /// \brief Action for snapping an object to another object below the
       /// first.
       // private: QAction *snapBelowAct;
-      // private: QAction *followAct;
       // private: QAction *skeletonAct;
 
       /// \brief The various view states

File gazebo/math/Quaternion.hh

     /// \param[in] _z z
     public: void Set(double _u, double _x, double _y, double _z);
 
-    /// \brief Set the quaternion from Euler angles
+    /// \brief Set the quaternion from Euler angles. The order of operations
+    /// are yaw, pitch, roll.
     /// \param[in] vec  Euler angle
     public: void SetFromEuler(const Vector3 &_vec);
 

File gazebo/rendering/Camera.cc

       }
     }
   }
+  else if (this->trackedVisual)
+  {
+    math::Vector3 direction = this->trackedVisual->GetWorldPose().pos -
+                              this->GetWorldPose().pos;
 
-  // TODO: this doesn't work properly
-  /*if (this->trackedVisual)
-  {
-    math::Pose displacement = this->trackedVisual->GetWorldPose() -
-      this->GetWorldPose();
-    this->sceneNode->translate(Conversions::Convert(displacement.pos));
-  }*/
+    double yaw = atan2(direction.y, direction.x);
+    double pitch = atan2(-direction.z,
+                         sqrt(pow(direction.x, 2) + pow(direction.y, 2)));
+    this->SetWorldRotation(math::Quaternion(0, pitch, yaw));
+
+    double origDistance = 5.0;
+    double distance = direction.GetLength();
+    double error = origDistance - distance;
+
+    double scaling = this->trackVisualPID.Update(error, 0.3);
+
+    math::Vector3 displacement = direction;
+    displacement.Normalize();
+    displacement *= scaling;
+    displacement.z = 0.0;
+
+    this->SetWorldPosition(this->GetWorldPosition() + displacement);
+  }
 }
 
 
   return Conversions::Convert(this->sceneNode->_getDerivedPosition());
 }
 
+//////////////////////////////////////////////////
 math::Quaternion Camera::GetWorldRotation() const
 {
   math::Vector3 sRot, pRot;
 
   if (_visual)
   {
-    this->camera->setAutoTracking(true, _visual->GetSceneNode());
+    this->trackVisualPID.Init(0.025, 0, 0, 0, 0, 1.0, 0.0);
     this->trackedVisual = _visual;
   }
   else
   {
     this->trackedVisual.reset();
-    this->camera->setAutoTracking(false, NULL);
-    // this->camera->setPosition(Ogre::Vector3(0, 0, 0));
-    // this->camera->setOrientation(Ogre::Quaternion(-.5, -.5, .5, .5));
   }
   return true;
 }
 
+//////////////////////////////////////////////////
 Ogre::Texture *Camera::GetRenderTexture() const
 {
   return this->renderTexture;

File gazebo/rendering/Camera.hh

 #include <deque>
 
 #include "common/Event.hh"
+#include "common/PID.hh"
 #include "common/Time.hh"
 
 #include "math/Angle.hh"
 
       /// \brief Render period.
       private: common::Time renderPeriod;
+
+      /// \brief PID used to track a visual smoothly.
+      private: common::PID trackVisualPID;
     };
     /// \}
   }

File gazebo/rendering/OrbitViewController.cc

 
   pos += this->focalPoint;
 
-  this->camera->SetWorldPosition(pos);
+  // this->camera->SetWorldPosition(pos);
 
   math::Quaternion rot;
   math::Vector3 rpy(0, this->pitch, this->yaw);
   rot.SetFromEuler(rpy);
-  this->camera->SetWorldRotation(rot);
+  // this->camera->SetWorldRotation(rot);
 }

File gazebo/rendering/UserCamera.cc

 bool UserCamera::TrackVisualImpl(VisualPtr _visual)
 {
   Camera::TrackVisualImpl(_visual);
-  if (_visual)
+  /*if (_visual)
     this->SetViewController(OrbitViewController::GetTypeString());
   else
     this->SetViewController(FPSViewController::GetTypeString());
+    */
 
   return true;
 }