Main Page | Modules | Namespace List | Class Hierarchy | Alphabetical List | Class List | Directories | File List | Namespace Members | Class Members | File Members | Related Pages

phPlayerPosition.cpp

Go to the documentation of this file.
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
SourceForge.net Logo

Generated on Sat Jun 16 02:44:07 2007 for phission by  doxygen 1.4.4