Added getPosition and getTarget for the visualizer camera.

src/visualization/include/iDynTree/Visualizer.h

@@ -92,6 +92,16 @@ class ICamera
      */
     virtual void setTarget(const iDynTree::Position & cameraPos) = 0;
 
+    /**
+     * Get the linear position of the camera w.r.t to the world.
+     */
+    virtual iDynTree::Position getPosition() = 0;
+
+    /**
+     * Get the target of the camera (i.e. the point the camera is looking into) w.r.t. the world.
+     */
+    virtual iDynTree::Position getTarget() = 0;
+
     /**
      * Set the up vector of the camera w.r.t to the world.
      */

src/visualization/src/Camera.cpp

@@ -105,6 +105,32 @@ void Camera::setUpVector(const Direction& cameraUpVector)
     }
 }
 
+Position Camera::getPosition()
+{
+    if(m_irrCamera)
+    {
+        return irr2idyntree_pos(m_irrCamera->getPosition());
+    }
+    else
+    {
+        reportError("Camera","getPosition","Impossible to get the position of a null camera");
+        return iDynTree::Position();
+    }
+}
+
+Position Camera::getTarget()
+{
+    if(m_irrCamera)
+    {
+        return irr2idyntree_pos(m_irrCamera->getTarget());
+    }
+    else
+    {
+        reportError("Camera","getTarget","Impossible to get the target of a null camera");
+        return iDynTree::Position();
+    }
+}
+
 ICameraAnimator *Camera::animator()
 {
     if (!m_animator)

src/visualization/src/Camera.h

@@ -39,6 +39,8 @@ class Camera: public ICamera
     virtual void setPosition(const Position& cameraPos);
     virtual void setTarget(const Position& targetPos);
     virtual void setUpVector(const Direction& upVector);
+    virtual iDynTree::Position getPosition();
+    virtual iDynTree::Position getTarget();
     virtual ICameraAnimator* animator();
 };
 

src/visualization/src/DummyImplementations.h

@@ -28,6 +28,8 @@ class DummyCamera : public ICamera
     virtual void setPosition(const iDynTree::Position &) {};
     virtual void setTarget(const iDynTree::Position &) {};
     virtual void setUpVector(const Direction&) {};
+    virtual iDynTree::Position getPosition() {return iDynTree::Position();};
+    virtual iDynTree::Position getTarget() {return iDynTree::Position();};
     virtual ICameraAnimator* animator() {return nullptr;};
 };
 

src/visualization/tests/VisualizerUnitTest.cpp

@@ -121,6 +121,9 @@ void checkFrameVisualization() {
     viz.init();
 
     viz.camera().setPosition(iDynTree::Position(2.0, 0.0, 2.0));
+    ASSERT_EQUAL_VECTOR(viz.camera().getPosition(), iDynTree::Position(2.0, 0.0, 2.0));
+    viz.camera().setTarget(iDynTree::Position(0.0, 0.0, 0.0));
+    ASSERT_EQUAL_VECTOR(viz.camera().getTarget(), iDynTree::Position(0.0, 0.0, 0.0));
 
     iDynTree::IFrameVisualization& frames = viz.frames();