00001 #include <playerc.h> 00002 00003 #include <phPlayerPosition.h> 00004 #include <phPlayer.h> 00005 #include <phbase.h> 00006 00007 /* ------------------------------------------------------------------------ */ 00008 struct ph_player_position_t 00009 { 00010 playerc_position_t *m_player_position; 00011 }; 00012 00013 /* ------------------------------------------------------------------------ */ 00014 phPlayerPosition::phPlayerPosition( ) 00015 { 00016 phFUNCTION("phPlayerPosition::phPlayerPosition") 00017 int locked = 0; 00018 00019 phTHIS_LOOSE_LOCK(locked); 00020 00021 this->setName("phPlayerPosition"); 00022 this->m_enabled = 0; 00023 this->m_last_vx = 0; 00024 this->m_last_vy = 0; 00025 this->m_last_va = 0; 00026 this->m_last_qx = 0; 00027 this->m_last_qy = 0; 00028 this->m_last_qa = 0; 00029 this->m_last_state = 0; 00030 this->m_position = (struct ph_player_position_t *)phCalloc(1, 00031 sizeof(struct ph_player_position_t)); 00032 phPRINT_PTR(this->m_position,"phCalloc","phCalloc failed"); 00033 00034 this->m_source = this->m_data = new phPositionData(); 00035 phPRINT_PTR(this->m_source,"new", "new failed"); 00036 00037 phTHIS_LOOSE_UNLOCK(locked); 00038 } 00039 00040 /* ------------------------------------------------------------------------ */ 00041 phPlayerPosition::phPlayerPosition( int index, phPlayerClient *client ) 00042 { 00043 phFUNCTION("phPlayerPosition::phPlayerPosition") 00044 int locked = 0; 00045 00046 phTHIS_LOOSE_LOCK(locked); 00047 00048 phPlayerPosition(); 00049 00050 rc = this->set(index,client); 00051 phPRINT_RC(rc,NULL,"this->set(index,client)"); 00052 00053 phTHIS_LOOSE_UNLOCK(locked); 00054 } 00055 00056 /* ------------------------------------------------------------------------ */ 00057 phPlayerPosition::~phPlayerPosition( ) 00058 { 00059 phFUNCTION("phPlayerPositon::~phPlayerPosition") 00060 int locked = 0; 00061 00062 phTHIS_LOOSE_LOCK(locked); 00063 00064 if (this->m_position != NULL) 00065 { 00066 if (this->m_is_subscribed) 00067 { 00068 rc = this->unsubscribe(); 00069 phPRINT_RC(rc,NULL,"this->unsubscribe()"); 00070 } 00071 00072 if (this->m_position->m_player_position != NULL) 00073 { 00074 playerc_position_destroy(this->m_position->m_player_position); 00075 this->m_position->m_player_position = NULL; 00076 } 00077 00078 phFree(this->m_position); 00079 } 00080 00081 this->m_enabled = 0; 00082 this->m_last_vx = 0; 00083 this->m_last_vy = 0; 00084 this->m_last_va = 0; 00085 this->m_last_qx = 0; 00086 this->m_last_qy = 0; 00087 this->m_last_qa = 0; 00088 this->m_last_state = 0; 00089 this->m_position = NULL; 00090 phDelete(this->m_data); 00091 00092 phTHIS_LOOSE_UNLOCK(locked); 00093 } 00094 00095 /* ------------------------------------------------------------------------ */ 00096 int phPlayerPosition::set( int index, phPlayerClient *client ) 00097 { 00098 phFUNCTION("phPlayerPosition::set") 00099 int locked = 0; 00100 00101 if (this->m_position == NULL) return phFAIL; 00102 00103 phTHIS_LOCK(locked); 00104 00105 if (index >= 0) 00106 this->m_index = index; 00107 if (client != NULL) 00108 this->m_client = client; 00109 00110 if (this->m_is_subscribed) 00111 { 00112 rc = this->unsubscribe(); 00113 phPRINT_RC(rc,NULL,"this->unsubscribe()"); 00114 } 00115 00116 if (this->m_position->m_player_position != NULL) 00117 { 00118 playerc_position_destroy(this->m_position->m_player_position); 00119 this->m_position->m_player_position = NULL; 00120 this->m_is_subscribed = 0; 00121 } 00122 00123 if ((this->m_position->m_player_position == NULL) && 00124 (this->m_client != NULL)) 00125 { 00126 this->m_position->m_player_position = 00127 playerc_position_create((playerc_client_t*)this->m_client->getClientPtr(), 00128 this->m_index); 00129 phPRINT_PTR(this->m_position->m_player_position, 00130 "playerc_position_create","playerc_position_create"); 00131 if (this->m_position->m_player_position != NULL) 00132 { 00133 this->m_is_subscribed = 0; 00134 } 00135 } 00136 00137 this->m_last_vx = 0; 00138 this->m_last_vy = 0; 00139 this->m_last_va = 0; 00140 this->m_last_qx = 0; 00141 this->m_last_qy = 0; 00142 this->m_last_qa = 0; 00143 this->m_last_state = 0; 00144 this->m_enabled = 0; 00145 00146 phTHIS_UNLOCK(locked); 00147 00148 return phSUCCESS; 00149 error: 00150 phTHIS_ERROR_UNLOCK(locked); 00151 00152 return phFAIL; 00153 } 00154 00155 /* ------------------------------------------------------------------------ */ 00156 int phPlayerPosition::subscribe( int access, int index, phPlayerClient *client ) 00157 { 00158 phFUNCTION("phPlayerPosition::subscribe") 00159 int locked = 0; 00160 int retrc = phSUCCESS; 00161 00162 phTHIS_LOCK(locked); 00163 00164 rc = this->set(index,client); 00165 phPRINT_RC(rc,NULL,"this->set(index:%d,client:%p)",index,client); 00166 00167 retrc = this->subscribe( access ); 00168 00169 phTHIS_UNLOCK(locked); 00170 00171 return retrc; 00172 error: 00173 phTHIS_ERROR_UNLOCK(locked); 00174 00175 return phFAIL; 00176 } 00177 00178 /* ------------------------------------------------------------------------ */ 00179 int phPlayerPosition::subscribe( int phaccess ) 00180 { 00181 phFUNCTION("phPlayerPosition::subscribe") 00182 int locked = 0; 00183 int access = 0; 00184 int tries = 0; 00185 int max_tries = 35; 00186 00187 if (this->m_position == NULL) return phFAIL; 00188 00189 phCHECK_PTR(this->m_client,NULL,"No client to subscribe to; call 'set'"); 00190 00191 phTHIS_LOCK(locked); 00192 00193 if (this->m_is_subscribed) 00194 { 00195 this->unsubscribe(); 00196 } 00197 00198 if ((this->m_position->m_player_position != NULL) && 00199 (!this->m_is_subscribed)) 00200 { 00201 switch(phaccess) 00202 { 00203 case phMODE_READ: 00204 access = PLAYER_READ_MODE; 00205 break; 00206 case phMODE_WRITE: 00207 access = PLAYER_WRITE_MODE; 00208 break; 00209 case phMODE_ALL: 00210 access = PLAYER_ALL_MODE; 00211 break; 00212 case phMODE_CLOSE: 00213 access = PLAYER_CLOSE_MODE; 00214 break; 00215 case phMODE_ERROR: 00216 access = PLAYER_ERROR_MODE; 00217 break; 00218 default: 00219 access = PLAYER_ALL_MODE; 00220 break; 00221 } 00222 00223 do 00224 { 00225 rc = playerc_position_subscribe(this->m_position->m_player_position,access); 00226 tries++; 00227 } while ((rc != phSUCCESS) && (tries <= max_tries)); 00228 PLAYER_PRINT_RC(rc,"playerc_position_subscribe","playerc_position_subscribe"); 00229 00230 if (rc == 0) 00231 this->m_is_subscribed = 1; 00232 else 00233 this->m_is_subscribed = 0; 00234 00235 if ((this->m_client != NULL) && (this->m_is_subscribed)) 00236 { 00237 rc = this->m_client->subscribeDevice(this); 00238 phPRINT_RC(rc,NULL,"this->m_client->subsribeDevice"); 00239 00240 this->m_last_vx = 0; 00241 this->m_last_vy = 0; 00242 this->m_last_va = 0; 00243 this->m_last_qx = 0; 00244 this->m_last_qy = 0; 00245 this->m_last_qa = 0; 00246 this->m_last_state = 0; 00247 this->m_enabled = 0; 00248 } 00249 } 00250 00251 phTHIS_UNLOCK(locked); 00252 00253 return phSUCCESS; 00254 error: 00255 phTHIS_ERROR_UNLOCK(locked); 00256 00257 return phFAIL; 00258 } 00259 00260 /* ------------------------------------------------------------------------ */ 00261 int phPlayerPosition::unsubscribe() 00262 { 00263 phFUNCTION("phPlayerPosition::unsubscribe") 00264 int locked = 0; 00265 00266 if (this->m_position == NULL) return phFAIL; 00267 00268 phTHIS_LOCK(locked); 00269 00270 if ((this->m_client != NULL) && (this->m_is_subscribed)) 00271 { 00272 rc = this->m_client->unsubscribeDevice(this); 00273 phPRINT_RC(rc,NULL,"this->m_client->unsubsribeDevice"); 00274 } 00275 00276 if (this->m_position->m_player_position != NULL) 00277 { 00278 rc = playerc_position_unsubscribe(this->m_position->m_player_position); 00279 PLAYER_PRINT_RC(rc,"playerc_position_unsubscribe","playerc_position_unsubscribe"); 00280 } 00281 00282 this->m_is_subscribed = 0; 00283 this->m_last_vx = 0; 00284 this->m_last_vy = 0; 00285 this->m_last_va = 0; 00286 this->m_last_qx = 0; 00287 this->m_last_qy = 0; 00288 this->m_last_qa = 0; 00289 this->m_last_state = 0; 00290 this->m_enabled = 0; 00291 00292 phTHIS_UNLOCK(locked); 00293 00294 return phSUCCESS; 00295 error: 00296 phTHIS_ERROR_UNLOCK(locked); 00297 00298 return phFAIL; 00299 } 00300 00301 /* ------------------------------------------------------------------------ */ 00302 #define USE_ENABLE() 0 00303 int phPlayerPosition::enable( int enable ) 00304 { 00305 phFUNCTION("phPlayerPosition::enable") 00306 int locked = 0; 00307 00308 if (this->m_position == NULL) return phFAIL; 00309 00310 phTHIS_LOCK(locked); 00311 00312 if (this->m_position->m_player_position != NULL) 00313 { 00314 #if USE_ENABLE() 00315 rc = playerc_position_enable(this->m_position->m_player_position, 00316 enable); 00317 PLAYER_PRINT_RC(rc,"playerc_position_enable","playerc_position_enable"); 00318 #endif 00319 this->m_enabled = enable; 00320 if (enable == 0) this->stop(); 00321 } 00322 00323 phTHIS_UNLOCK(locked); 00324 00325 return phSUCCESS; 00326 error: 00327 phTHIS_ERROR_UNLOCK(locked); 00328 00329 return phFAIL; 00330 } 00331 00332 /* ------------------------------------------------------------------------ */ 00333 int phPlayerPosition::disable(int disable ) 00334 { 00335 phFUNCTION("phPlayerPosition::disable") 00336 int locked = 0; 00337 00338 if (this->m_position == NULL) return phFAIL; 00339 00340 phTHIS_LOCK(locked); 00341 00342 if (this->m_position->m_player_position != NULL) 00343 { 00344 #if USE_ENABLE() 00345 rc = playerc_position_enable(this->m_position->m_player_position, 00346 (disable) ? 0 : 1); 00347 PLAYER_PRINT_RC(rc,"playerc_position_enable","playerc_position_enable"); 00348 #endif 00349 this->m_enabled = (disable ? 0 : 1); 00350 if (disable != 0) this->stop(); 00351 } 00352 00353 phTHIS_UNLOCK(locked); 00354 00355 return phSUCCESS; 00356 error: 00357 phTHIS_ERROR_UNLOCK(locked); 00358 00359 return phFAIL; 00360 } 00361 00362 /* ------------------------------------------------------------------------ */ 00363 int phPlayerPosition::isEnabled() 00364 { 00365 phFUNCTION("phPlayerPosition::isEnabled") 00366 int locked = 0; 00367 int value = 0; 00368 00369 phTHIS_LOCK(locked); 00370 00371 value = this->m_enabled; 00372 00373 phTHIS_UNLOCK(locked); 00374 00375 return value; 00376 error: 00377 phTHIS_ERROR_UNLOCK(locked); 00378 00379 return phFAIL; 00380 } 00381 /* ------------------------------------------------------------------------ */ 00382 int phPlayerPosition::getGeometry() 00383 { 00384 phFUNCTION("phPlayerPosition::getGeometry") 00385 int locked = 0; 00386 00387 if (this->m_position == NULL) return phFAIL; 00388 00389 phTHIS_LOCK(locked); 00390 00391 if (this->m_position->m_player_position != NULL) 00392 { 00393 rc = playerc_position_get_geom(this->m_position->m_player_position); 00394 PLAYER_PRINT_RC(rc,"playerc_position_get_geom","playerc_position_get_geom"); 00395 } 00396 00397 phTHIS_UNLOCK(locked); 00398 00399 return phSUCCESS; 00400 error: 00401 phTHIS_ERROR_UNLOCK(locked); 00402 00403 return phFAIL; 00404 } 00405 00406 /* ------------------------------------------------------------------------ */ 00407 int phPlayerPosition::setCmdVel( double vx, double vy, double va, int state ) 00408 { 00409 phFUNCTION("phPlayerPosition::setCmdVel") 00410 int locked = 0; 00411 int motor_state = 0; 00412 00413 if (this->m_position == NULL) return phFAIL; 00414 00415 phTHIS_LOCK(locked); 00416 00417 if (this->m_position->m_player_position != NULL) 00418 { 00419 state = (state == 1) ? 1 : 0; 00420 00421 if ((this->m_last_vx != vx) || 00422 (this->m_last_vy != vy) || 00423 (this->m_last_va != va) || 00424 ((this->m_last_state != state) && (state >= 0))) 00425 { 00426 motor_state = ((state == 1) && (this->m_enabled)) ? 1 : 0; 00427 //phPRINT("VELO=(%0.2f:%0.2f:%0.2f:%d)\n",vx,vy,va,motor_state); 00428 00429 if (motor_state == 0) 00430 { 00431 vx = 0; 00432 vy = 0; 00433 va = 0; 00434 } 00435 00436 rc = playerc_position_set_cmd_vel(this->m_position->m_player_position, 00437 vx, 00438 vy, 00439 va, 00440 motor_state); 00441 PLAYER_PRINT_RC(rc,"playerc_position_set_cmd_vel","playerc_position_set_cmd_vel"); 00442 00443 this->m_last_vx = vx; 00444 this->m_last_vy = vy; 00445 this->m_last_va = va; 00446 this->m_last_state = state; 00447 } 00448 } 00449 00450 phTHIS_UNLOCK(locked); 00451 00452 return phSUCCESS; 00453 error: 00454 phTHIS_ERROR_UNLOCK(locked); 00455 00456 return phFAIL; 00457 } 00458 00459 /* ------------------------------------------------------------------------ */ 00460 int phPlayerPosition::setCmdPose( double qx, double qy, double qa, int state ) 00461 { 00462 phFUNCTION("phPlayerPosition::setCmdPose") 00463 int locked = 0; 00464 00465 if (this->m_position == NULL) return phFAIL; 00466 00467 phTHIS_LOCK(locked); 00468 00469 if (this->m_position->m_player_position != NULL) 00470 { 00471 state = (state == 1) ? 1 : 0; 00472 00473 if ((this->m_last_qx != qx) || 00474 (this->m_last_qy != qy) || 00475 (this->m_last_qa != qa) || 00476 ((this->m_last_state != state) && (state >= 0))) 00477 { 00478 if ((state == 1) && (this->m_enabled)) 00479 { 00480 //phPRINT("POSE=(%0.2f:%0.2f:%0.2f:%d)\n",qx,qy,qa,state); 00481 } 00482 00483 rc = playerc_position_set_cmd_pose(this->m_position->m_player_position, 00484 qx, 00485 qy, 00486 qa, 00487 ((state == 1) && (this->m_enabled)) ? 1 : 0); 00488 PLAYER_PRINT_RC(rc,"playerc_position_set_cmd_pose","playerc_position_set_cmd_pose"); 00489 00490 this->m_last_qx = qx; 00491 this->m_last_qy = qy; 00492 this->m_last_qa = qa; 00493 this->m_last_state = state; 00494 } 00495 } 00496 00497 phTHIS_UNLOCK(locked); 00498 00499 return phSUCCESS; 00500 error: 00501 phTHIS_ERROR_UNLOCK(locked); 00502 00503 return phFAIL; 00504 } 00505 00506 /* ------------------------------------------------------------------------ */ 00507 int phPlayerPosition::move(double trans, double rot) 00508 { 00509 phFUNCTION("phPlayerPosition::move") 00510 int locked = 0; 00511 00512 if (this->m_position == NULL) return phFAIL; 00513 00514 phTHIS_LOCK(locked); 00515 00516 this->setCmdVel(trans,0.0,rot,1); 00517 00518 phTHIS_UNLOCK(locked); 00519 00520 return phSUCCESS; 00521 error: 00522 phTHIS_ERROR_UNLOCK(locked); 00523 00524 return phFAIL; 00525 } 00526 00527 /* ------------------------------------------------------------------------ */ 00528 int phPlayerPosition::stop(int state) 00529 { 00530 phFUNCTION("phPlayerPosition::stop") 00531 int locked = 0; 00532 00533 if (this->m_position == NULL) return phFAIL; 00534 00535 phTHIS_LOCK(locked); 00536 00537 this->setCmdVel(0.0,0.0,0.0,state); 00538 00539 phTHIS_UNLOCK(locked); 00540 00541 return phSUCCESS; 00542 error: 00543 phTHIS_ERROR_UNLOCK(locked); 00544 00545 return phFAIL; 00546 } 00547 00548 /* ------------------------------------------------------------------------ */ 00549 int phPlayerPosition::getData( phPositionData &data ) 00550 { 00551 phFUNCTION("phPlayerPosition::getData") 00552 int locked = 0; 00553 playerc_position_t *pposition; 00554 00555 if (this->m_position == NULL) return phFAIL; 00556 00557 phTHIS_LOCK(locked); 00558 00559 if (this->m_position->m_player_position != NULL) 00560 { 00561 pposition = this->m_position->m_player_position; 00562 rc = data.set( pposition->pose, 00563 pposition->size, 00564 pposition->px, 00565 pposition->py, 00566 pposition->pa, 00567 pposition->vx, 00568 pposition->vy, 00569 pposition->va, 00570 pposition->stall ); 00571 phCHECK_RC(rc,NULL,"data.set()"); 00572 00573 } 00574 00575 phTHIS_UNLOCK(locked); 00576 00577 return phSUCCESS; 00578 00579 error: 00580 phTHIS_ERROR_UNLOCK(locked); 00581 00582 return phFAIL; 00583 } 00584 00585 /* ------------------------------------------------------------------------ */ 00586 int phPlayerPosition::updateData() 00587 { 00588 phFUNCTION("phPlayerPosition::updateData") 00589 int locked = 0; 00590 int data_locked = 0; 00591 int data_rwlocked = 0; 00592 playerc_position_t *pposition = NULL; 00593 double pose[3] = {0,0,0}; 00594 double size[2] = {0,0}; 00595 double px = 0.0; 00596 double py = 0.0; 00597 double pa = 0.0; 00598 double vx = 0.0; 00599 double vy = 0.0; 00600 double va = 0.0; 00601 int stall = 0; 00602 int i = 0; 00603 int changed = 0; 00604 double **s = (double **)&size; 00605 double **p = (double **)&pose; 00606 00607 if (this->m_position == NULL) return phFAIL; 00608 00609 phTHIS_LOCK(locked); 00610 00611 if (this->m_position->m_player_position != NULL) 00612 { 00613 phMUTEX_LOCK((*(this->m_data)),data_locked); 00614 phWRITELOCK((*(this->m_data)),data_rwlocked); 00615 00616 rc = this->m_data->get(p,s,&px,&py,&pa,&vx,&vy,&va,&stall); 00617 phCHECK_RC(rc,NULL,"data.get()"); 00618 00619 pposition = this->m_position->m_player_position; 00620 00621 for (i = 0; i < 3; i++) 00622 if (pposition->pose[i] != pose[i]) changed = 1; 00623 00624 for (i = 0; i < 2; i++) 00625 if (pposition->size[i] != size[i]) changed = 1; 00626 00627 if (!changed) 00628 { 00629 if ((pposition->px != px) || 00630 (pposition->py != py) || 00631 (pposition->pa != pa) || 00632 (pposition->vx != vx) || 00633 (pposition->vy != vy) || 00634 (pposition->va != va) || 00635 (pposition->stall != stall)) 00636 { 00637 changed = 1; 00638 } 00639 } 00640 00641 if (changed) 00642 { 00643 rc = this->m_data->set( pposition->pose, 00644 pposition->size, 00645 pposition->px, 00646 pposition->py, 00647 pposition->pa, 00648 pposition->vx, 00649 pposition->vy, 00650 pposition->va, 00651 pposition->stall ); 00652 phCHECK_RC(rc,NULL,"this->m_data->set()"); 00653 } 00654 00655 phRWUNLOCK((*(this->m_data)),data_rwlocked); 00656 phMUTEX_UNLOCK((*(this->m_data)),data_locked); 00657 } 00658 00659 phTHIS_UNLOCK(locked); 00660 00661 return phSUCCESS; 00662 00663 error: 00664 phRWUNLOCK_ERROR((*(this->m_data)),data_rwlocked); 00665 phMUTEX_ERROR_UNLOCK((*(this->m_data)),data_locked); 00666 00667 phTHIS_ERROR_UNLOCK(locked); 00668 00669 return phFAIL; 00670 } 00671 00672 /* ------------------------------------------------------------------------ */ 00673 void *phPlayerPosition::getId() 00674 { 00675 void *retrc = NULL; 00676 if (this->m_position != NULL) 00677 { 00678 retrc = this->m_position->m_player_position->info.id; 00679 } 00680 return retrc; 00681 } 00682 00683
Copyright (C) 2002 - 2007 |
Philip D.S. Thoren ( pthoren@users.sourceforge.net ) University Of Massachusetts at Lowell Robotics Lab |