Ignore:
Timestamp:
04/08/16 15:40:57 (8 years ago)
Author:
Bayard Gildas
Message:

sources reformatted with flair-format-dir script

File:
1 edited

Legend:

Unmodified
Added
Removed

  • trunk/lib/FlairSensorActuator/src/VrpnObject_impl.cpp

    r3 r15  
    3939using namespace flair::sensor;
    4040
    41 VrpnObject_impl::VrpnObject_impl(VrpnObject* self,const VrpnClient *parent,string name, int id,const TabWidget* tab)
    42 {
    43     this->parent=parent;
    44     this->self=self;
     41VrpnObject_impl::VrpnObject_impl(VrpnObject *self, const VrpnClient *parent,
     42                                 string name, int id, const TabWidget *tab) {
     43  this->parent = parent;
     44  this->self = self;
    4545
    46     if(id==-1 && parent->UseXbee())
    47     {
    48         self->Err("erreur aucun identifiant specifie pour la connexion Xbee\n");
    49     }
    50     if(id!=-1 && !parent->UseXbee())
    51     {
    52         self->Warn("identifiant pour la connexion Xbee ignore car pas en mode Xbee\n");
    53     }
     46  if (id == -1 && parent->UseXbee()) {
     47    self->Err("erreur aucun identifiant specifie pour la connexion Xbee\n");
     48  }
     49  if (id != -1 && !parent->UseXbee()) {
     50    self->Warn(
     51        "identifiant pour la connexion Xbee ignore car pas en mode Xbee\n");
     52  }
    5453
    55         if(parent->UseXbee())
    56         {
    57         parent->pimpl_->AddTrackable(this,id);
    58         tracker=NULL;
    59         }
    60         else
    61         {
    62             parent->pimpl_->AddTrackable(self);
    63         tracker = new vrpn_Tracker_Remote(name.c_str(), parent->pimpl_->connection);
    64         tracker->register_change_handler(this,handle_pos);
    65         tracker->shutup=true;
    66         }
     54  if (parent->UseXbee()) {
     55    parent->pimpl_->AddTrackable(this, id);
     56    tracker = NULL;
     57  } else {
     58    parent->pimpl_->AddTrackable(self);
     59    tracker = new vrpn_Tracker_Remote(name.c_str(), parent->pimpl_->connection);
     60    tracker->register_change_handler(this, handle_pos);
     61    tracker->shutup = true;
     62  }
    6763
    68         //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");
    76         output=new cvmatrix(self,desc,floatType);
     64  // state
     65  cvmatrix_descriptor *desc = new cvmatrix_descriptor(6, 1);
     66  desc->SetElementName(0, 0, "roll");
     67  desc->SetElementName(1, 0, "pitch");
     68  desc->SetElementName(2, 0, "yaw");
     69  desc->SetElementName(3, 0, "x");
     70  desc->SetElementName(4, 0, "y");
     71  desc->SetElementName(5, 0, "z");
     72  output = new cvmatrix(self, desc, floatType);
    7773
    78         desc=new cvmatrix_descriptor(3,1);
    79         desc->SetElementName(0,0,"roll");
    80         desc->SetElementName(1,0,"pitch");
    81         desc->SetElementName(2,0,"yaw");
    82         state=new cvmatrix(self,desc,floatType);
     74  desc = new cvmatrix_descriptor(3, 1);
     75  desc->SetElementName(0, 0, "roll");
     76  desc->SetElementName(1, 0, "pitch");
     77  desc->SetElementName(2, 0, "yaw");
     78  state = new cvmatrix(self, desc, floatType);
    8379
    84         //ui
    85         plot_tab=new Tab(tab,"Mesures "+ name);
    86         x_plot=new DataPlot1D(plot_tab->NewRow(),"x",-10,10);
    87             x_plot->AddCurve(output->Element(3));
    88         y_plot=new DataPlot1D(plot_tab->LastRowLastCol(),"y",-10,10);
    89             y_plot->AddCurve(output->Element(4));
    90         z_plot=new DataPlot1D(plot_tab->LastRowLastCol(),"z",-2,0);
    91             z_plot->AddCurve(output->Element(5));
     80  // ui
     81  plot_tab = new Tab(tab, "Mesures " + name);
     82  x_plot = new DataPlot1D(plot_tab->NewRow(), "x", -10, 10);
     83  x_plot->AddCurve(output->Element(3));
     84  y_plot = new DataPlot1D(plot_tab->LastRowLastCol(), "y", -10, 10);
     85  y_plot->AddCurve(output->Element(4));
     86  z_plot = new DataPlot1D(plot_tab->LastRowLastCol(), "z", -2, 0);
     87  z_plot->AddCurve(output->Element(5));
    9288}
    9389
    94 VrpnObject_impl::~VrpnObject_impl(void)
    95 {
    96     if(tracker!=NULL)//normal
    97         {
    98             parent->pimpl_->RemoveTrackable(self);
    99         tracker->unregister_change_handler(this,handle_pos);
    100         delete tracker;
    101         }
    102         else//xbee
    103         {
    104         parent->pimpl_->RemoveTrackable(this);
    105         }
    106         delete plot_tab;
     90VrpnObject_impl::~VrpnObject_impl(void) {
     91  if (tracker != NULL) // normal
     92  {
     93    parent->pimpl_->RemoveTrackable(self);
     94    tracker->unregister_change_handler(this, handle_pos);
     95    delete tracker;
     96  } else // xbee
     97  {
     98    parent->pimpl_->RemoveTrackable(this);
     99  }
     100  delete plot_tab;
    107101}
    108102
    109 void VrpnObject_impl::mainloop(void)
    110 {
    111         tracker->mainloop();
     103void VrpnObject_impl::mainloop(void) { tracker->mainloop(); }
     104
     105bool VrpnObject_impl::IsTracked(unsigned int timeout_ms) {
     106  output->GetMutex();
     107  Time a = GetTime();
     108  Time dt = a - output->DataTime();
     109  output->ReleaseMutex();
     110
     111  if (dt > (Time)(timeout_ms * 1000000)) {
     112    // self->Printf("%lld %lld %lld
     113    // %lld\n",a,output->DataTime(),dt,(Time)(timeout_ms*1000000));
     114    return false;
     115  } else {
     116    return true;
     117  }
    112118}
    113119
    114 bool VrpnObject_impl::IsTracked(unsigned int timeout_ms)
    115 {
    116     output->GetMutex();
    117     Time a=GetTime();
    118     Time dt=a-output->DataTime();
    119     output->ReleaseMutex();
    120 
    121     if(dt>(Time)(timeout_ms*1000000))
    122     {
    123         //self->Printf("%lld %lld %lld %lld\n",a,output->DataTime(),dt,(Time)(timeout_ms*1000000));
    124         return false;
    125     }
    126     else
    127     {
    128         return true;
    129     }
    130 }
    131 
    132 void VrpnObject_impl::GetEuler(Euler &euler)
    133 {
    134     output->GetMutex();
    135     euler.roll=output->ValueNoMutex(0,0);
    136     euler.pitch=output->ValueNoMutex(1,0);
    137     euler.yaw=output->ValueNoMutex(2,0);
    138     output->ReleaseMutex();
     120void VrpnObject_impl::GetEuler(Euler &euler) {
     121  output->GetMutex();
     122  euler.roll = output->ValueNoMutex(0, 0);
     123  euler.pitch = output->ValueNoMutex(1, 0);
     124  euler.yaw = output->ValueNoMutex(2, 0);
     125  output->ReleaseMutex();
    139126}
    140127
    141128void VrpnObject_impl::GetQuaternion(Quaternion &quaternion) {
    142     output->GetMutex();
    143     quaternion.q0=this->quaternion.q0;
    144     quaternion.q1=this->quaternion.q1;
    145     quaternion.q2=this->quaternion.q2;
    146     quaternion.q3=this->quaternion.q3;
    147     output->ReleaseMutex();
     129  output->GetMutex();
     130  quaternion.q0 = this->quaternion.q0;
     131  quaternion.q1 = this->quaternion.q1;
     132  quaternion.q2 = this->quaternion.q2;
     133  quaternion.q3 = this->quaternion.q3;
     134  output->ReleaseMutex();
    148135}
    149136
    150 void VrpnObject_impl::GetPosition(Vector3D &point)
    151 {
    152     output->GetMutex();
    153     point.x=output->ValueNoMutex(3,0);
    154     point.y=output->ValueNoMutex(4,0);
    155     point.z=output->ValueNoMutex(5,0);
    156     output->ReleaseMutex();
     137void VrpnObject_impl::GetPosition(Vector3D &point) {
     138  output->GetMutex();
     139  point.x = output->ValueNoMutex(3, 0);
     140  point.y = output->ValueNoMutex(4, 0);
     141  point.z = output->ValueNoMutex(5, 0);
     142  output->ReleaseMutex();
    157143}
    158144
    159 void VRPN_CALLBACK VrpnObject_impl::handle_pos(void *userdata, const vrpn_TRACKERCB t)
    160 {
    161     bool is_nan=false;
    162         VrpnObject_impl* caller= reinterpret_cast<VrpnObject_impl*>(userdata);
    163         Time time=GetTime();
     145void VRPN_CALLBACK
     146VrpnObject_impl::handle_pos(void *userdata, const vrpn_TRACKERCB t) {
     147  bool is_nan = false;
     148  VrpnObject_impl *caller = reinterpret_cast<VrpnObject_impl *>(userdata);
     149  Time time = GetTime();
    164150
    165     //check if something is nan
    166     for(int i=0;i<3;i++)
    167     {
    168         if(isnan(t.pos[i])==true) is_nan=true;
    169     }
    170     for(int i=0;i<4;i++)
    171     {
    172         if(isnan(t.quat[i])==true) is_nan=true;
    173     }
    174     if(is_nan==true)
    175     {
    176         caller->self->Warn("data is nan, skipping it (time %lld)\n",time);
    177         return;
    178     }
     151  // check if something is nan
     152  for (int i = 0; i < 3; i++) {
     153    if (isnan(t.pos[i]) == true)
     154      is_nan = true;
     155  }
     156  for (int i = 0; i < 4; i++) {
     157    if (isnan(t.quat[i]) == true)
     158      is_nan = true;
     159  }
     160  if (is_nan == true) {
     161    caller->self->Warn("data is nan, skipping it (time %lld)\n", time);
     162    return;
     163  }
    179164
    180     //on prend une fois pour toute le mutex et on fait des accès directs
    181     caller->output->GetMutex();
     165  // on prend une fois pour toute le mutex et on fait des accès directs
     166  caller->output->GetMutex();
    182167
    183     //warning: t.quat is defined as (qx,qy,qz,qw), which is different from flair::core::Quaternion
    184     caller->quaternion.q0=t.quat[3];
    185     caller->quaternion.q1=t.quat[0];
    186     caller->quaternion.q2=t.quat[1];
    187     caller->quaternion.q3=t.quat[2];
    188     Vector3D pos((float)t.pos[0],(float)t.pos[1],(float)t.pos[2]);
     168  // warning: t.quat is defined as (qx,qy,qz,qw), which is different from
     169  // flair::core::Quaternion
     170  caller->quaternion.q0 = t.quat[3];
     171  caller->quaternion.q1 = t.quat[0];
     172  caller->quaternion.q2 = t.quat[1];
     173  caller->quaternion.q3 = t.quat[2];
     174  Vector3D pos((float)t.pos[0], (float)t.pos[1], (float)t.pos[2]);
    189175
    190     //on effectue les rotation
    191     caller->parent->pimpl_->ComputeRotations(pos);
    192     caller->parent->pimpl_->ComputeRotations(caller->quaternion);
     176  // on effectue les rotation
     177  caller->parent->pimpl_->ComputeRotations(pos);
     178  caller->parent->pimpl_->ComputeRotations(caller->quaternion);
    193179
    194     Euler euler;
    195     caller->quaternion.ToEuler(euler);
    196     caller->output->SetValueNoMutex( 0, 0,euler.roll);
    197     caller->output->SetValueNoMutex( 1, 0,euler.pitch);
    198     caller->output->SetValueNoMutex( 2, 0,euler.yaw);
    199     caller->output->SetValueNoMutex( 3, 0,pos.x);
    200     caller->output->SetValueNoMutex( 4, 0,pos.y);
    201     caller->output->SetValueNoMutex( 5, 0,pos.z);
     180  Euler euler;
     181  caller->quaternion.ToEuler(euler);
     182  caller->output->SetValueNoMutex(0, 0, euler.roll);
     183  caller->output->SetValueNoMutex(1, 0, euler.pitch);
     184  caller->output->SetValueNoMutex(2, 0, euler.yaw);
     185  caller->output->SetValueNoMutex(3, 0, pos.x);
     186  caller->output->SetValueNoMutex(4, 0, pos.y);
     187  caller->output->SetValueNoMutex(5, 0, pos.z);
    202188
    203     caller->output->SetDataTime(time);
    204     caller->output->ReleaseMutex();
     189  caller->output->SetDataTime(time);
     190  caller->output->ReleaseMutex();
    205191
    206     caller->state->GetMutex();
    207     caller->state->SetValueNoMutex( 0, 0,Euler::ToDegree(euler.roll));
    208     caller->state->SetValueNoMutex( 1, 0,Euler::ToDegree(euler.pitch));
    209     caller->state->SetValueNoMutex(2, 0,Euler::ToDegree(euler.yaw));
    210     caller->state->ReleaseMutex();
     192  caller->state->GetMutex();
     193  caller->state->SetValueNoMutex(0, 0, Euler::ToDegree(euler.roll));
     194  caller->state->SetValueNoMutex(1, 0, Euler::ToDegree(euler.pitch));
     195  caller->state->SetValueNoMutex(2, 0, Euler::ToDegree(euler.yaw));
     196  caller->state->ReleaseMutex();
    211197
    212     caller->self->ProcessUpdate(caller->output);
     198  caller->self->ProcessUpdate(caller->output);
    213199}
    214 
Note: See TracChangeset for help on using the changeset viewer.