Ignore:
Timestamp:
Feb 13, 2017, 11:38:33 AM (5 years ago)
Author:
Sanahuja Guillaume
Message:

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

File:
1 edited

Legend:

Unmodified
Added
Removed
  • trunk/lib/FlairSensorActuator/src/VrpnObject_impl.cpp

    r122 r135  
    6767
    6868  // state
    69   cvmatrix_descriptor *desc = new cvmatrix_descriptor(6, 1);
    70   desc->SetElementName(0, 0, "roll");
    71   desc->SetElementName(1, 0, "pitch");
    72   desc->SetElementName(2, 0, "yaw");
    73   desc->SetElementName(3, 0, "x");
    74   desc->SetElementName(4, 0, "y");
    75   desc->SetElementName(5, 0, "z");
     69  cvmatrix_descriptor *desc = new cvmatrix_descriptor(7, 1);
     70  desc->SetElementName(0, 0, "q0");
     71  desc->SetElementName(1, 0, "q1");
     72  desc->SetElementName(2, 0, "q2");
     73  desc->SetElementName(3, 0, "q3");
     74  desc->SetElementName(4, 0, "x");
     75  desc->SetElementName(5, 0, "y");
     76  desc->SetElementName(6, 0, "z");
    7677  output = new cvmatrix(self, desc, floatType);
    7778
     
    8586  plot_tab = new Tab(tab, "Mesures " + name);
    8687  x_plot = new DataPlot1D(plot_tab->NewRow(), "x", -10, 10);
    87   x_plot->AddCurve(output->Element(3));
     88  x_plot->AddCurve(output->Element(4));
    8889  y_plot = new DataPlot1D(plot_tab->LastRowLastCol(), "y", -10, 10);
    89   y_plot->AddCurve(output->Element(4));
     90  y_plot->AddCurve(output->Element(5));
    9091  z_plot = new DataPlot1D(plot_tab->LastRowLastCol(), "z", -2, 0);
    91   z_plot->AddCurve(output->Element(5));
     92  z_plot->AddCurve(output->Element(6));
    9293}
    9394
     
    122123}
    123124
    124 void VrpnObject_impl::GetEuler(Euler &euler) {
    125   output->GetMutex();
    126   euler.roll = output->ValueNoMutex(0, 0);
    127   euler.pitch = output->ValueNoMutex(1, 0);
    128   euler.yaw = output->ValueNoMutex(2, 0);
    129   output->ReleaseMutex();
    130 }
    131 
    132125void VrpnObject_impl::GetQuaternion(Quaternion &quaternion) {
    133126  output->GetMutex();
    134   quaternion.q0 = this->quaternion.q0;
    135   quaternion.q1 = this->quaternion.q1;
    136   quaternion.q2 = this->quaternion.q2;
    137   quaternion.q3 = this->quaternion.q3;
     127  quaternion.q0 = output->ValueNoMutex(0, 0);
     128  quaternion.q1 = output->ValueNoMutex(1, 0);
     129  quaternion.q2 = output->ValueNoMutex(2, 0);
     130  quaternion.q3 = output->ValueNoMutex(3, 0);
    138131  output->ReleaseMutex();
    139132}
     
    141134void VrpnObject_impl::GetPosition(Vector3D &point) {
    142135  output->GetMutex();
    143   point.x = output->ValueNoMutex(3, 0);
    144   point.y = output->ValueNoMutex(4, 0);
    145   point.z = output->ValueNoMutex(5, 0);
     136  point.x = output->ValueNoMutex(4, 0);
     137  point.y = output->ValueNoMutex(5, 0);
     138  point.z = output->ValueNoMutex(6, 0);
    146139  output->ReleaseMutex();
    147140}
     
    172165  // warning: t.quat is defined as (qx,qy,qz,qw), which is different from
    173166  // flair::core::Quaternion
    174   caller->quaternion.q0 = t.quat[3];
    175   caller->quaternion.q1 = t.quat[0];
    176   caller->quaternion.q2 = t.quat[1];
    177   caller->quaternion.q3 = t.quat[2];
     167        Quaternion quaternion(t.quat[3],t.quat[0],t.quat[1],t.quat[2]);
    178168  Vector3D pos((float)t.pos[0], (float)t.pos[1], (float)t.pos[2]);
    179169
    180170  // on effectue les rotation
    181171  caller->parent->pimpl_->ComputeRotations(pos);
    182   caller->parent->pimpl_->ComputeRotations(caller->quaternion);
     172  caller->parent->pimpl_->ComputeRotations(quaternion);
    183173
    184   Euler euler;
    185   caller->quaternion.ToEuler(euler);
    186   caller->output->SetValueNoMutex(0, 0, euler.roll);
    187   caller->output->SetValueNoMutex(1, 0, euler.pitch);
    188   caller->output->SetValueNoMutex(2, 0, euler.yaw);
    189   caller->output->SetValueNoMutex(3, 0, pos.x);
    190   caller->output->SetValueNoMutex(4, 0, pos.y);
    191   caller->output->SetValueNoMutex(5, 0, pos.z);
     174  caller->output->SetValueNoMutex(0, 0, quaternion.q0);
     175  caller->output->SetValueNoMutex(1, 0, quaternion.q1);
     176  caller->output->SetValueNoMutex(2, 0, quaternion.q2);
     177  caller->output->SetValueNoMutex(3, 0, quaternion.q3);
     178  caller->output->SetValueNoMutex(4, 0, pos.x);
     179  caller->output->SetValueNoMutex(5, 0, pos.y);
     180  caller->output->SetValueNoMutex(6, 0, pos.z);
    192181
    193182  caller->output->SetDataTime(time);
    194183  caller->output->ReleaseMutex();
    195184
     185        Euler euler=quaternion.ToEuler();
    196186  caller->state->GetMutex();
    197187  caller->state->SetValueNoMutex(0, 0, Euler::ToDegree(euler.roll));
Note: See TracChangeset for help on using the changeset viewer.