Ignore:
Timestamp:
Apr 8, 2016, 3:40:57 PM (9 years ago)
Author:
Bayard Gildas
Message:

sources reformatted with flair-format-dir script

File:
1 edited

Legend:

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

    r3 r15  
    4141using namespace flair::sensor;
    4242
    43 VrpnClient_impl::VrpnClient_impl(VrpnClient* self,std::string name,std::string address,uint16_t us_period)
    44 {
    45     this->us_period=us_period;
    46     this->self=self;
    47     serialport=NULL;
    48 
    49     mutex=new Mutex(self,name);
    50 
    51     connection = vrpn_get_connection_by_name(address.c_str());
    52 
    53     //station sol
    54     main_tab=new Tab(getFrameworkManager()->GetTabWidget(),name);
    55     tab=new TabWidget(main_tab->NewRow(),name);
    56     setup_tab=new Tab(tab,"Reglages");
    57 
    58     rotation_1=new OneAxisRotation(setup_tab->NewRow(),"post rotation 1");
    59     rotation_2=new OneAxisRotation(setup_tab->NewRow(),"post rotation 2");
    60 }
    61 
    62 VrpnClient_impl::VrpnClient_impl(VrpnClient* self,std::string name,SerialPort* serialport,uint16_t us_period)
    63 {
    64     this->us_period=us_period;
    65     this->self=self;
    66     this->serialport=serialport;
    67     connection=NULL;
    68     mutex=new Mutex(self,name);
    69 
    70     serialport->SetBaudrate(111111);
    71     serialport->SetRxTimeout(us_period*1000);
    72 
    73     //station sol
    74     main_tab=new Tab(getFrameworkManager()->GetTabWidget(),name);
    75     tab=new TabWidget(main_tab->NewRow(),name);
    76     setup_tab=new Tab(tab,"Reglages");
    77 
    78     rotation_1=new OneAxisRotation(setup_tab->NewRow(),"post rotation 1");
    79     rotation_2=new OneAxisRotation(setup_tab->NewRow(),"post rotation 2");
    80 }
    81 
    82 VrpnClient_impl::~VrpnClient_impl()
    83 {
    84     if(!UseXbee())
    85     {
    86         //on fait une copie car le delete touche a xbee_objects_copy via RemoveTrackable
    87         vector<VrpnObject*> trackables_copy=trackables;
    88         for(unsigned int i=0;i<trackables_copy.size();i++) delete trackables_copy.at(i);
    89         //trackables.clear();
     43VrpnClient_impl::VrpnClient_impl(VrpnClient *self, std::string name,
     44                                 std::string address, uint16_t us_period) {
     45  this->us_period = us_period;
     46  this->self = self;
     47  serialport = NULL;
     48
     49  mutex = new Mutex(self, name);
     50
     51  connection = vrpn_get_connection_by_name(address.c_str());
     52
     53  // station sol
     54  main_tab = new Tab(getFrameworkManager()->GetTabWidget(), name);
     55  tab = new TabWidget(main_tab->NewRow(), name);
     56  setup_tab = new Tab(tab, "Reglages");
     57
     58  rotation_1 = new OneAxisRotation(setup_tab->NewRow(), "post rotation 1");
     59  rotation_2 = new OneAxisRotation(setup_tab->NewRow(), "post rotation 2");
     60}
     61
     62VrpnClient_impl::VrpnClient_impl(VrpnClient *self, std::string name,
     63                                 SerialPort *serialport, uint16_t us_period) {
     64  this->us_period = us_period;
     65  this->self = self;
     66  this->serialport = serialport;
     67  connection = NULL;
     68  mutex = new Mutex(self, name);
     69
     70  serialport->SetBaudrate(111111);
     71  serialport->SetRxTimeout(us_period * 1000);
     72
     73  // station sol
     74  main_tab = new Tab(getFrameworkManager()->GetTabWidget(), name);
     75  tab = new TabWidget(main_tab->NewRow(), name);
     76  setup_tab = new Tab(tab, "Reglages");
     77
     78  rotation_1 = new OneAxisRotation(setup_tab->NewRow(), "post rotation 1");
     79  rotation_2 = new OneAxisRotation(setup_tab->NewRow(), "post rotation 2");
     80}
     81
     82VrpnClient_impl::~VrpnClient_impl() {
     83  if (!UseXbee()) {
     84    // on fait une copie car le delete touche a xbee_objects_copy via
     85    // RemoveTrackable
     86    vector<VrpnObject *> trackables_copy = trackables;
     87    for (unsigned int i = 0; i < trackables_copy.size(); i++)
     88      delete trackables_copy.at(i);
     89    // trackables.clear();
     90  } else {
     91    // on fait une copie car le delete touche a xbee_objects_copy via
     92    // RemoveTrackable
     93    vector<xbee_object> xbee_objects_copy = xbee_objects;
     94    for (unsigned int i = 0; i < xbee_objects_copy.size(); i++)
     95      delete xbee_objects_copy.at(i).vrpnobject->self;
     96  }
     97
     98  delete main_tab;
     99
     100  if (!UseXbee()) {
     101    // it will automatically delete connection
     102    connection->removeReference();
     103  }
     104}
     105
     106void VrpnClient_impl::ComputeRotations(Vector3D &point) {
     107  rotation_1->ComputeRotation(point);
     108  rotation_2->ComputeRotation(point);
     109}
     110
     111void VrpnClient_impl::ComputeRotations(Quaternion &quat) {
     112  rotation_1->ComputeRotation(quat);
     113  rotation_2->ComputeRotation(quat);
     114}
     115
     116void VrpnClient_impl::AddTrackable(VrpnObject *obj) {
     117  mutex->GetMutex();
     118  trackables.push_back(obj);
     119  mutex->ReleaseMutex();
     120}
     121
     122void VrpnClient_impl::RemoveTrackable(VrpnObject *obj) {
     123  mutex->GetMutex();
     124  for (vector<VrpnObject *>::iterator it = trackables.begin();
     125       it < trackables.end(); it++) {
     126    if (*it == obj) {
     127      trackables.erase(it);
     128      break;
    90129    }
    91     else
    92     {
    93         //on fait une copie car le delete touche a xbee_objects_copy via RemoveTrackable
    94         vector<xbee_object> xbee_objects_copy=xbee_objects;
    95         for(unsigned int i=0;i<xbee_objects_copy.size();i++) delete xbee_objects_copy.at(i).vrpnobject->self;
     130  }
     131  mutex->ReleaseMutex();
     132}
     133
     134void VrpnClient_impl::AddTrackable(VrpnObject_impl *obj, uint8_t id) {
     135  xbee_object tmp;
     136  tmp.vrpnobject = obj;
     137  tmp.id = id;
     138  mutex->GetMutex();
     139  xbee_objects.push_back(tmp);
     140  mutex->ReleaseMutex();
     141}
     142
     143void VrpnClient_impl::RemoveTrackable(VrpnObject_impl *obj) {
     144  mutex->GetMutex();
     145  for (vector<xbee_object>::iterator it = xbee_objects.begin();
     146       it < xbee_objects.end(); it++) {
     147    if ((*it).vrpnobject == obj) {
     148      xbee_objects.erase(it);
     149      break;
    96150    }
    97 
    98     delete main_tab;
    99 
    100     if(!UseXbee())
    101     {
    102         //it will automatically delete connection
    103         connection->removeReference();
     151  }
     152  mutex->ReleaseMutex();
     153}
     154
     155bool VrpnClient_impl::UseXbee(void) {
     156  if (connection == NULL) {
     157    return true;
     158  } else {
     159    return false;
     160  }
     161}
     162
     163void VrpnClient_impl::Run(void) {
     164  while (!self->ToBeStopped()) {
     165    if (UseXbee()) {
     166      ssize_t read = 0;
     167      uint8_t response[38] = {0};
     168
     169      read = serialport->Read(response, sizeof(response));
     170      if (read > 0 && read != sizeof(response))
     171        read += serialport->Read(&response[read], sizeof(response) - read);
     172      // int temps=(float)self->GetTime()/(1000*1000);
     173      // self->Printf("%i %i %i\n",temps-last,temps,last);
     174      // last=temps;
     175      if (read < 0) {
     176        // self->Warn("erreur rt_dev_read (%s)\n",strerror(-read));
     177      } else if (read != sizeof(response)) {
     178        self->Warn("erreur rt_dev_read %i/%i\n", read, sizeof(response));
     179      } else {
     180        // for(ssize_t i=0;i<read;i++) printf("%x ",response[i]);
     181        // printf("\n");
     182        uint8_t checksum = 0;
     183        for (ssize_t i = 3; i < read; i++)
     184          checksum += response[i];
     185        if (checksum != 255) {
     186          self->Err("checksum error\n");
     187        } else {
     188          vrpn_TRACKERCB t;
     189          float pos[3];
     190          float quat[4];
     191          uint8_t id = response[8];
     192
     193          mutex->GetMutex();
     194          if (id < xbee_objects.size()) {
     195            memcpy(pos, &response[9], sizeof(pos));
     196            memcpy(quat, &response[9] + sizeof(pos), sizeof(quat));
     197            for (int i = 0; i < 3; i++)
     198              t.pos[i] = pos[i];
     199            for (int i = 0; i < 4; i++)
     200              t.quat[i] = quat[i];
     201            if (fabs(pos[0] > 10) || fabs(pos[1] > 10) || fabs(pos[2] > 10)) {
     202              printf("prob pos %f %f %f\n", pos[0], pos[1], pos[2]);
     203            } else {
     204              // self->Printf("%i %f %f %f
     205              // %f\n",id,pos[0],pos[1],pos[2],(float)self->GetTime()/(1000*1000));
     206              VrpnObject_impl::handle_pos(xbee_objects.at(id).vrpnobject, t);
     207            }
     208          }
     209          mutex->ReleaseMutex();
     210        }
     211      }
     212    } else {
     213      connection->mainloop();
     214      mutex->GetMutex();
     215      for (unsigned int i = 0; i < trackables.size(); i++)
     216        trackables.at(i)->mainloop();
     217      mutex->ReleaseMutex();
     218
     219      self->SleepUS(us_period);
    104220    }
    105 }
    106 
    107 void VrpnClient_impl::ComputeRotations(Vector3D& point)
    108 {
    109     rotation_1->ComputeRotation(point);
    110     rotation_2->ComputeRotation(point);
    111 }
    112 
    113 void VrpnClient_impl::ComputeRotations(Quaternion& quat)
    114 {
    115     rotation_1->ComputeRotation(quat);
    116     rotation_2->ComputeRotation(quat);
    117 }
    118 
    119 void VrpnClient_impl::AddTrackable(VrpnObject* obj)
    120 {
    121     mutex->GetMutex();
    122     trackables.push_back(obj);
    123     mutex->ReleaseMutex();
    124 }
    125 
    126 void VrpnClient_impl::RemoveTrackable(VrpnObject* obj)
    127 {
    128     mutex->GetMutex();
    129     for(vector<VrpnObject*>::iterator it=trackables.begin() ; it < trackables.end(); it++ )
    130     {
    131         if(*it==obj)
    132         {
    133             trackables.erase (it);
    134             break;
    135         }
    136     }
    137     mutex->ReleaseMutex();
    138 }
    139 
    140 void VrpnClient_impl::AddTrackable(VrpnObject_impl* obj,uint8_t id)
    141 {
    142     xbee_object tmp;
    143     tmp.vrpnobject=obj;
    144     tmp.id=id;
    145     mutex->GetMutex();
    146     xbee_objects.push_back(tmp);
    147     mutex->ReleaseMutex();
    148 }
    149 
    150 void VrpnClient_impl::RemoveTrackable(VrpnObject_impl* obj)
    151 {
    152     mutex->GetMutex();
    153     for(vector<xbee_object>::iterator it=xbee_objects.begin() ; it < xbee_objects.end(); it++ )
    154     {
    155         if((*it).vrpnobject==obj)
    156         {
    157             xbee_objects.erase (it);
    158             break;
    159         }
    160     }
    161     mutex->ReleaseMutex();
    162 }
    163 
    164 bool VrpnClient_impl::UseXbee(void)
    165 {
    166     if(connection==NULL)
    167     {
    168         return true;
    169     }
    170     else
    171     {
    172         return false;
    173     }
    174 }
    175 
    176 void VrpnClient_impl::Run(void)
    177 {
    178     while(!self->ToBeStopped())
    179     {
    180         if(UseXbee())
    181         {
    182             ssize_t read = 0;
    183             uint8_t response[38] = {0};
    184 
    185             read = serialport->Read(response,sizeof(response));
    186             if(read>0 && read!=sizeof(response)) read += serialport->Read(&response[read],sizeof(response)-read);
    187             //int temps=(float)self->GetTime()/(1000*1000);
    188 //self->Printf("%i %i %i\n",temps-last,temps,last);
    189 //last=temps;
    190             if(read<0)
    191             {
    192                 //self->Warn("erreur rt_dev_read (%s)\n",strerror(-read));
    193             }
    194             else if (read != sizeof(response))
    195             {
    196                 self->Warn("erreur rt_dev_read %i/%i\n",read,sizeof(response));
    197             }
    198             else
    199             {
    200                 //for(ssize_t i=0;i<read;i++) printf("%x ",response[i]);
    201                 //printf("\n");
    202                 uint8_t checksum=0;
    203                 for(ssize_t i=3;i<read;i++) checksum+=response[i];
    204                 if(checksum!=255)
    205                 {
    206                     self->Err("checksum error\n");
    207                 }
    208                 else
    209                 {
    210                     vrpn_TRACKERCB t;
    211                     float pos[3];
    212                     float quat[4];
    213                     uint8_t id=response[8];
    214 
    215                     mutex->GetMutex();
    216                     if(id<xbee_objects.size())
    217                     {
    218                         memcpy(pos,&response[9],sizeof(pos));
    219                         memcpy(quat,&response[9]+sizeof(pos),sizeof(quat));
    220                         for(int i=0;i<3;i++) t.pos[i]=pos[i];
    221                         for(int i=0;i<4;i++) t.quat[i]=quat[i];
    222                         if(fabs(pos[0]>10) || fabs(pos[1]>10) || fabs(pos[2]>10))
    223                         {
    224                             printf("prob pos %f %f %f\n",pos[0],pos[1],pos[2]);
    225                         }
    226                         else
    227                         {
    228                             //self->Printf("%i %f %f %f %f\n",id,pos[0],pos[1],pos[2],(float)self->GetTime()/(1000*1000));
    229                             VrpnObject_impl::handle_pos(xbee_objects.at(id).vrpnobject,t);
    230                         }
    231                     }
    232                     mutex->ReleaseMutex();
    233                 }
    234             }
    235         }
    236         else
    237         {
    238             connection->mainloop();
    239             mutex->GetMutex();
    240             for(unsigned int i=0;i<trackables.size();i++) trackables.at(i)->mainloop();
    241             mutex->ReleaseMutex();
    242 
    243             self->SleepUS(us_period);
    244         }
    245     }
    246 }
     221  }
     222}
Note: See TracChangeset for help on using the changeset viewer.