Changeset 135 in flair-src for trunk/lib/FlairSensorActuator/src/VrpnObject_impl.cpp

Timestamp:

02/13/17 11:38:33 (7 years ago)

Author:

Sanahuja Guillaume

Message:

VrpnObject no longer outputs Euler (ony Quaternion): warning, output matrix has changed!

File:

• trunk/lib/FlairSensorActuator/src/VrpnObject_impl.cpp (modified) (5 diffs)

trunk/lib/FlairSensorActuator/src/VrpnObject_impl.cpp

@@ -67 +67 @@
 
   // state
-  cvmatrix_descriptor *desc = new cvmatrix_descriptor(6, 1);
-  desc->SetElementName(0, 0, "roll");
-  desc->SetElementName(1, 0, "pitch");
-  desc->SetElementName(2, 0, "yaw");
-  desc->SetElementName(3, 0, "x");
-  desc->SetElementName(4, 0, "y");
-  desc->SetElementName(5, 0, "z");
+  cvmatrix_descriptor *desc = new cvmatrix_descriptor(7, 1);
+  desc->SetElementName(0, 0, "q0");
+  desc->SetElementName(1, 0, "q1");
+  desc->SetElementName(2, 0, "q2");
+  desc->SetElementName(3, 0, "q3");
+  desc->SetElementName(4, 0, "x");
+  desc->SetElementName(5, 0, "y");
+  desc->SetElementName(6, 0, "z");
   output = new cvmatrix(self, desc, floatType);
 
@@ -85 +86 @@
   plot_tab = new Tab(tab, "Mesures " + name);
   x_plot = new DataPlot1D(plot_tab->NewRow(), "x", -10, 10);
-  x_plot->AddCurve(output->Element(3));
+  x_plot->AddCurve(output->Element(4));
   y_plot = new DataPlot1D(plot_tab->LastRowLastCol(), "y", -10, 10);
-  y_plot->AddCurve(output->Element(4));
+  y_plot->AddCurve(output->Element(5));
   z_plot = new DataPlot1D(plot_tab->LastRowLastCol(), "z", -2, 0);
-  z_plot->AddCurve(output->Element(5));
+  z_plot->AddCurve(output->Element(6));
 }
 
@@ -122 +123 @@
 }
 
-void VrpnObject_impl::GetEuler(Euler &euler) {
-  output->GetMutex();
-  euler.roll = output->ValueNoMutex(0, 0);
-  euler.pitch = output->ValueNoMutex(1, 0);
-  euler.yaw = output->ValueNoMutex(2, 0);
-  output->ReleaseMutex();
-}
-
 void VrpnObject_impl::GetQuaternion(Quaternion &quaternion) {
   output->GetMutex();
-  quaternion.q0 = this->quaternion.q0;
-  quaternion.q1 = this->quaternion.q1;
-  quaternion.q2 = this->quaternion.q2;
-  quaternion.q3 = this->quaternion.q3;
+  quaternion.q0 = output->ValueNoMutex(0, 0);
+  quaternion.q1 = output->ValueNoMutex(1, 0);
+  quaternion.q2 = output->ValueNoMutex(2, 0);
+  quaternion.q3 = output->ValueNoMutex(3, 0);
   output->ReleaseMutex();
 }
@@ -141 +134 @@
 void VrpnObject_impl::GetPosition(Vector3D &point) {
   output->GetMutex();
-  point.x = output->ValueNoMutex(3, 0);
-  point.y = output->ValueNoMutex(4, 0);
-  point.z = output->ValueNoMutex(5, 0);
+  point.x = output->ValueNoMutex(4, 0);
+  point.y = output->ValueNoMutex(5, 0);
+  point.z = output->ValueNoMutex(6, 0);
   output->ReleaseMutex();
 }
@@ -172 +165 @@
   // warning: t.quat is defined as (qx,qy,qz,qw), which is different from
   // flair::core::Quaternion
-  caller->quaternion.q0 = t.quat[3];
-  caller->quaternion.q1 = t.quat[0];
-  caller->quaternion.q2 = t.quat[1];
-  caller->quaternion.q3 = t.quat[2];
+        Quaternion quaternion(t.quat[3],t.quat[0],t.quat[1],t.quat[2]);
   Vector3D pos((float)t.pos[0], (float)t.pos[1], (float)t.pos[2]);
 
   // on effectue les rotation
   caller->parent->pimpl_->ComputeRotations(pos);
-  caller->parent->pimpl_->ComputeRotations(caller->quaternion);
+  caller->parent->pimpl_->ComputeRotations(quaternion);
 
-  Euler euler;
-  caller->quaternion.ToEuler(euler);
-  caller->output->SetValueNoMutex(0, 0, euler.roll);
-  caller->output->SetValueNoMutex(1, 0, euler.pitch);
-  caller->output->SetValueNoMutex(2, 0, euler.yaw);
-  caller->output->SetValueNoMutex(3, 0, pos.x);
-  caller->output->SetValueNoMutex(4, 0, pos.y);
-  caller->output->SetValueNoMutex(5, 0, pos.z);
+  caller->output->SetValueNoMutex(0, 0, quaternion.q0);
+  caller->output->SetValueNoMutex(1, 0, quaternion.q1);
+  caller->output->SetValueNoMutex(2, 0, quaternion.q2);
+  caller->output->SetValueNoMutex(3, 0, quaternion.q3);
+  caller->output->SetValueNoMutex(4, 0, pos.x);
+  caller->output->SetValueNoMutex(5, 0, pos.y);
+  caller->output->SetValueNoMutex(6, 0, pos.z);
 
   caller->output->SetDataTime(time);
   caller->output->ReleaseMutex();
 
+        Euler euler=quaternion.ToEuler();
   caller->state->GetMutex();
   caller->state->SetValueNoMutex(0, 0, Euler::ToDegree(euler.roll));