University Of Massachusetts at Lowell
Robotics Lab
00001 #include <playerc.h> 00002 00003 #include <phPlayerSonar.h> 00004 #include <phPlayer.h> 00005 #include <phbase.h> 00006 00007 /* ------------------------------------------------------------------------ */ 00008 struct ph_player_sonar_t 00009 { 00010 playerc_sonar_t *m_player_sonar; 00011 }; 00012 00013 /* ------------------------------------------------------------------------ */ 00014 phPlayerSonar::phPlayerSonar( ) 00015 { 00016 phFUNCTION("phPlayerSonar::phPlayerSonar") 00017 int locked = 0; 00018 00019 phTHIS_LOOSE_LOCK(locked); 00020 00021 this->setName("phPlayerSonar"); 00022 00023 this->m_sonar = (struct ph_player_sonar_t *)phCalloc(1, 00024 sizeof(struct ph_player_sonar_t)); 00025 phPRINT_PTR(this->m_sonar,"phCalloc","phCalloc failed"); 00026 00027 this->m_source = this->m_data = new phSonarData(); 00028 phPRINT_PTR(this->m_source,"new", "new failed"); 00029 00030 phTHIS_LOOSE_UNLOCK(locked); 00031 } 00032 00033 /* ------------------------------------------------------------------------ */ 00034 phPlayerSonar::phPlayerSonar( int index, phPlayerClient *client ) 00035 { 00036 phFUNCTION("phPlayerSonar::phPlayerSonar") 00037 int locked = 0; 00038 00039 phTHIS_LOOSE_LOCK(locked); 00040 00041 phPlayerSonar(); 00042 00043 rc = this->set(index,client); 00044 phPRINT_RC(rc,NULL,"this->set(%d,%p)\n",index,client); 00045 00046 phTHIS_LOOSE_UNLOCK(locked); 00047 } 00048 00049 /* ------------------------------------------------------------------------ */ 00050 phPlayerSonar::~phPlayerSonar( ) 00051 { 00052 phFUNCTION("phPlayerPositon::~phPlayerSonar") 00053 int locked = 0; 00054 00055 phTHIS_LOOSE_LOCK(locked); 00056 00057 if (this->m_sonar != NULL) 00058 { 00059 if (this->m_is_subscribed) 00060 { 00061 rc = this->unsubscribe(); 00062 phPRINT_RC(rc,NULL,"this->unsubscribe()"); 00063 } 00064 00065 if (this->m_sonar->m_player_sonar != NULL) 00066 { 00067 playerc_sonar_destroy(this->m_sonar->m_player_sonar); 00068 this->m_sonar->m_player_sonar = NULL; 00069 } 00070 00071 phFree(this->m_sonar); 00072 } 00073 00074 this->m_sonar = NULL; 00075 phDelete(this->m_data); 00076 00077 phTHIS_LOOSE_UNLOCK(locked); 00078 } 00079 00080 /* ------------------------------------------------------------------------ */ 00081 int phPlayerSonar::set( int index, phPlayerClient *client ) 00082 { 00083 phFUNCTION("phPlayerSonar::set") 00084 int locked = 0; 00085 00086 phTHIS_LOCK(locked); 00087 00088 if (index >= 0) 00089 this->m_index = index; 00090 00091 if (client != NULL) 00092 this->m_client = client; 00093 00094 if (this->m_is_subscribed) 00095 { 00096 rc = this->unsubscribe(); 00097 phPRINT_RC(rc,NULL,"this->unsubscribe()"); 00098 } 00099 00100 if (this->m_sonar->m_player_sonar != NULL) 00101 { 00102 playerc_sonar_destroy(this->m_sonar->m_player_sonar); 00103 this->m_sonar->m_player_sonar = NULL; 00104 this->m_is_subscribed = 0; 00105 } 00106 00107 if ((this->m_sonar->m_player_sonar == NULL) && 00108 (this->m_client != NULL)) 00109 { 00110 this->m_sonar->m_player_sonar = 00111 playerc_sonar_create((playerc_client_t*)this->m_client->getClientPtr(), 00112 index); 00113 phPRINT_PTR(this->m_sonar->m_player_sonar, 00114 "playerc_sonar_create","playerc_sonar_create"); 00115 if (this->m_sonar->m_player_sonar != NULL) 00116 { 00117 this->m_is_subscribed = 0; 00118 } 00119 } 00120 00121 if (this->m_sonar->m_player_sonar != NULL) 00122 { 00123 rc = this->m_data->reset(); 00124 phPRINT_RC(rc,NULL,"this->m_data->reset()"); 00125 } 00126 00127 phTHIS_UNLOCK(locked); 00128 00129 return phSUCCESS; 00130 error: 00131 phTHIS_ERROR_UNLOCK(locked); 00132 00133 return phFAIL; 00134 } 00135 00136 /* ------------------------------------------------------------------------ */ 00137 int phPlayerSonar::subscribe( int access, int index, phPlayerClient *client ) 00138 { 00139 phFUNCTION("phPlayerSonar::subscribe") 00140 int locked = 0; 00141 int retrc = phSUCCESS; 00142 00143 phTHIS_LOCK(locked); 00144 00145 rc = this->set(index,client); 00146 phPRINT_RC(rc,NULL,"this->set(client:%p)",client); 00147 00148 retrc = this->subscribe( access ); 00149 00150 phTHIS_UNLOCK(locked); 00151 00152 return retrc; 00153 error: 00154 phTHIS_ERROR_UNLOCK(locked); 00155 00156 return phFAIL; 00157 } 00158 00159 /* ------------------------------------------------------------------------ */ 00160 int phPlayerSonar::subscribe( int phaccess ) 00161 { 00162 phFUNCTION("phPlayerSonar::subscribe") 00163 int locked = 0; 00164 int access = 0; 00165 int tries = 0; 00166 int max_tries = 35; 00167 00168 if (this->m_sonar == NULL) return phFAIL; 00169 00170 phCHECK_PTR(this->m_client,NULL,"No client to subscribe to; call 'set'"); 00171 00172 phTHIS_LOCK(locked); 00173 00174 if (this->m_is_subscribed) 00175 { 00176 this->unsubscribe(); 00177 } 00178 00179 if ((this->m_sonar->m_player_sonar != NULL) && 00180 (!this->m_is_subscribed)) 00181 { 00182 switch(phaccess) 00183 { 00184 case phMODE_READ: 00185 access = PLAYER_READ_MODE; 00186 break; 00187 case phMODE_WRITE: 00188 access = PLAYER_WRITE_MODE; 00189 break; 00190 case phMODE_ALL: 00191 access = PLAYER_ALL_MODE; 00192 break; 00193 case phMODE_CLOSE: 00194 access = PLAYER_CLOSE_MODE; 00195 break; 00196 case phMODE_ERROR: 00197 access = PLAYER_ERROR_MODE; 00198 break; 00199 default: 00200 access = PLAYER_ALL_MODE; 00201 break; 00202 } 00203 00204 do 00205 { 00206 rc = playerc_sonar_subscribe(this->m_sonar->m_player_sonar,access); 00207 tries++; 00208 } while ((rc != phSUCCESS) && (tries <= max_tries)); 00209 PLAYER_PRINT_RC(rc,"playerc_sonar_subscribe","playerc_sonar_subscribe"); 00210 00211 if (rc == 0) 00212 this->m_is_subscribed = 1; 00213 else 00214 this->m_is_subscribed = 0; 00215 00216 rc = this->m_data->reset(); 00217 phPRINT_RC(rc,NULL,"this->m_data->reset()"); 00218 00219 if ((this->m_client != NULL) && (this->m_is_subscribed)) 00220 { 00221 rc = this->m_client->subscribeDevice(this); 00222 phPRINT_RC(rc,NULL,"this->m_client->subsribeDevice"); 00223 } 00224 } 00225 00226 phTHIS_UNLOCK(locked); 00227 00228 return phSUCCESS; 00229 error: 00230 phTHIS_ERROR_UNLOCK(locked); 00231 00232 return phFAIL; 00233 } 00234 00235 /* ------------------------------------------------------------------------ */ 00236 int phPlayerSonar::unsubscribe() 00237 { 00238 phFUNCTION("phPlayerSonar::unsubscribe") 00239 int locked = 0; 00240 00241 if (this->m_sonar == NULL) return phFAIL; 00242 00243 phTHIS_LOCK(locked); 00244 00245 if ((this->m_client != NULL) && (this->m_is_subscribed)) 00246 { 00247 rc = this->m_client->unsubscribeDevice(this); 00248 phPRINT_RC(rc,NULL,"this->m_client->unsubsribeDevice"); 00249 } 00250 00251 if (this->m_sonar->m_player_sonar != NULL) 00252 { 00253 rc = playerc_sonar_unsubscribe(this->m_sonar->m_player_sonar); 00254 PLAYER_PRINT_RC(rc,"playerc_sonar_unsubscribe","playerc_sonar_unsubscribe"); 00255 00256 this->m_is_subscribed = 0; 00257 00258 rc = this->m_data->reset(); 00259 phPRINT_RC(rc,NULL,"this->m_data->reset()"); 00260 } 00261 00262 phTHIS_UNLOCK(locked); 00263 00264 return phSUCCESS; 00265 error: 00266 phTHIS_ERROR_UNLOCK(locked); 00267 00268 return phFAIL; 00269 } 00270 00271 /* ------------------------------------------------------------------------ */ 00272 int phPlayerSonar::getGeometry() 00273 { 00274 phFUNCTION("phPlayerSonar::getGeometry") 00275 int locked = 0; 00276 00277 if (this->m_sonar == NULL) return phFAIL; 00278 00279 phTHIS_LOCK(locked); 00280 00281 if (this->m_sonar->m_player_sonar != NULL) 00282 { 00283 rc = playerc_sonar_get_geom(this->m_sonar->m_player_sonar); 00284 PLAYER_PRINT_RC(rc,"playerc_sonar_get_geom","playerc_sonar_get_geom"); 00285 } 00286 00287 phTHIS_UNLOCK(locked); 00288 00289 return phSUCCESS; 00290 error: 00291 phTHIS_ERROR_UNLOCK(locked); 00292 00293 return phFAIL; 00294 } 00295 00296 /* ------------------------------------------------------------------------ */ 00297 int phPlayerSonar::getData( phSonarData &data ) 00298 { 00299 phFUNCTION("phPlayerSonar::getData") 00300 int locked = 0; 00301 playerc_sonar_t *psonar; 00302 00303 if (this->m_sonar == NULL) return phFAIL; 00304 00305 phTHIS_LOCK(locked); 00306 00307 if (this->m_sonar->m_player_sonar != NULL) 00308 { 00309 psonar = this->m_sonar->m_player_sonar; 00310 rc = data.set( psonar->pose_count, 00311 3, 00312 (double *)psonar->poses, 00313 psonar->scan_count, 00314 (double *)psonar->scan 00315 ); 00316 phCHECK_RC(rc,NULL,"data.set()"); 00317 00318 } 00319 00320 phTHIS_UNLOCK(locked); 00321 00322 return phSUCCESS; 00323 00324 error: 00325 phTHIS_ERROR_UNLOCK(locked); 00326 00327 return phFAIL; 00328 } 00329 00330 /* ------------------------------------------------------------------------ */ 00331 int phPlayerSonar::updateData() 00332 { 00333 phFUNCTION("phPlayerSonar::updateData") 00334 int locked = 0; 00335 int data_locked = 0; 00336 int data_rwlocked = 0; 00337 playerc_sonar_t *psonar = NULL; 00338 00339 int32_t pose_count = 0; 00340 int32_t pose_dim = 0; 00341 double pose[PLAYERC_SONAR_MAX_SAMPLES][3] = {{0,0,0}}; 00342 int32_t scan_count = 0; 00343 double scan[PLAYERC_SONAR_MAX_SAMPLES] = {0}; 00344 00345 int i = 0; 00346 int changed = 0; 00347 double *s = (double *)&scan; 00348 double *p = (double *)&pose; 00349 00350 if (this->m_sonar == NULL) return phFAIL; 00351 00352 phTHIS_LOCK(locked); 00353 00354 /*phPROGRESS("\n");*/ 00355 if (this->m_sonar->m_player_sonar != NULL) 00356 { 00357 phMUTEX_LOCK((*(this->m_data)),data_locked); 00358 phWRITELOCK((*(this->m_data)),data_rwlocked); 00359 00360 rc = this->m_data->get(&pose_count,&pose_dim,p, 00361 &scan_count,s); 00362 phCHECK_RC(rc,NULL,"data.get()"); 00363 00364 psonar = this->m_sonar->m_player_sonar; 00365 00366 if (psonar->pose_count > PLAYERC_SONAR_MAX_SAMPLES) 00367 { 00368 phPRINT_RC(-1,NULL,"Warning: psonar->pose_count:%d > PLAYERC_SONAR_MAX_SAMPLES:%d\n", 00369 psonar->pose_count,PLAYERC_SONAR_MAX_SAMPLES); 00370 } 00371 00372 if (pose_count != psonar->pose_count) changed = 1; 00373 if (scan_count != psonar->scan_count) changed = 1; 00374 00375 if (changed == 0) 00376 { 00377 for (i = 0; (i < pose_count) && (changed == 0); i++) 00378 if (psonar->poses[i] != pose[i]) changed = 1; 00379 00380 for (i = 0; (i < scan_count) && (changed == 0); i++) 00381 if (psonar->scan[i] != scan[i]) changed = 1; 00382 } 00383 00384 if (changed) 00385 { 00386 rc = this->m_data->set( psonar->pose_count, 00387 3, 00388 (double *)psonar->poses, 00389 psonar->scan_count, 00390 (double *)psonar->scan 00391 ); 00392 phCHECK_RC(rc,NULL,"this->m_data->set()"); 00393 } 00394 00395 phRWUNLOCK((*(this->m_data)),data_rwlocked); 00396 phMUTEX_UNLOCK((*(this->m_data)),data_locked); 00397 } 00398 00399 phTHIS_UNLOCK(locked); 00400 00401 return phSUCCESS; 00402 00403 error: 00404 phRWUNLOCK_ERROR((*(this->m_data)),data_rwlocked); 00405 phMUTEX_ERROR_UNLOCK((*(this->m_data)),data_locked); 00406 00407 phTHIS_ERROR_UNLOCK(locked); 00408 00409 return phFAIL; 00410 } 00411 00412 /* ------------------------------------------------------------------------ */ 00413 void *phPlayerSonar::getId() 00414 { 00415 void *retrc = NULL; 00416 if (this->m_sonar != NULL) 00417 { 00418 retrc = this->m_sonar->m_player_sonar->info.id; 00419 } 00420 return retrc; 00421 } 00422 00423
| Copyright (C) 2002 - 2007 |
Philip D.S. Thoren ( pthoren@users.sourceforge.net ) University Of Massachusetts at Lowell Robotics Lab |
|
1.4.4