Changeset 15 in flair-src for trunk/lib/FlairSensorActuator/src/VrpnClient_impl.cpp
- Timestamp:
- Apr 8, 2016, 3:40:57 PM (9 years ago)
- File:
-
- 1 edited
Legend:
- Unmodified
- Added
- Removed
-
trunk/lib/FlairSensorActuator/src/VrpnClient_impl.cpp
r3 r15 41 41 using namespace flair::sensor; 42 42 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(); 43 VrpnClient_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 62 VrpnClient_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 82 VrpnClient_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 106 void VrpnClient_impl::ComputeRotations(Vector3D &point) { 107 rotation_1->ComputeRotation(point); 108 rotation_2->ComputeRotation(point); 109 } 110 111 void VrpnClient_impl::ComputeRotations(Quaternion &quat) { 112 rotation_1->ComputeRotation(quat); 113 rotation_2->ComputeRotation(quat); 114 } 115 116 void VrpnClient_impl::AddTrackable(VrpnObject *obj) { 117 mutex->GetMutex(); 118 trackables.push_back(obj); 119 mutex->ReleaseMutex(); 120 } 121 122 void 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; 90 129 } 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 134 void 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 143 void 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; 96 150 } 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 155 bool VrpnClient_impl::UseXbee(void) { 156 if (connection == NULL) { 157 return true; 158 } else { 159 return false; 160 } 161 } 162 163 void 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); 104 220 } 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.