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

phPositionData.cpp

Go to the documentation of this file.
00001 #include <phPositionData.h>
00002 #include <phbase.h>
00003 
00004 /* ------------------------------------------------------------------------ */
00005 phPositionData::phPositionData()
00006 {
00007     phFUNCTION("phPositionData::phPositionData")
00008     int locked = 0;
00009     int rwlocked = 0;
00010     
00011     phTHIS_LOOSE_LOCK(locked);
00012     phTHIS_WRITELOCK_LOOSE(rwlocked);
00013 
00014     this->setName("phPositionData");
00015     
00016     memset(this->m_pose,0,sizeof(double)*3);
00017     memset(this->m_size,0,sizeof(double)*2);
00018     this->m_px = 0.0;
00019     this->m_py = 0.0;
00020     this->m_pa = 0.0;
00021     this->m_vx = 0.0;
00022     this->m_vy = 0.0;
00023     this->m_va = 0.0;
00024     this->m_stall = 0;
00025 
00026     phTHIS_RWUNLOCK_LOOSE(rwlocked);
00027     phTHIS_LOOSE_UNLOCK(locked);
00028 }
00029 
00030 /* ---------------------------------------------------------------------- */
00031 int phPositionData::copy( phObject *copyto_object )
00032 {
00033     phFUNCTION("phPositionData::copy")
00034     
00035     if (this->isNamed(((phObject *)copyto_object)->getName()))
00036     {
00037         phPositionData *copyto_data = (phPositionData *)copyto_object;
00038         return copyto_data->copyData(*this);
00039     }
00040     /* Otherwise I'll assume they are incompatible */
00041     else
00042     {
00043         phPRINT_RC(-1,NULL, "Invalid object pairing in update."
00044                           "[ this:%s != target:%s ]",
00045                  this->getName(), ((phObject *)copyto_object)->getName());
00046         return phFAIL;
00047     }
00048 }
00049 
00050 /* ---------------------------------------------------------------------- */
00051 int phPositionData::swap( phObject *object )
00052 {
00053     phFUNCTION("phPositionData::swap")
00054 
00055     if (this->isNamed(((phObject *)object)->getName()))
00056     {
00057         phPositionData *data = (phPositionData *)object;
00058 
00059         return data->swapData(*this);
00060     }
00061     /* Otherwise I'll assume they are incompatible */
00062     else
00063     {
00064         phPRINT_RC(-1,NULL, "Invalid object pairing in update."
00065                           "[ this:%s != target:%s ]",
00066                  this->getName(), ((phObject *)object)->getName());
00067         return phFAIL;
00068     }    
00069 }
00070 
00071 /* ---------------------------------------------------------------------- */
00072 int phPositionData::copyData( phPositionData &copyfrom )
00073 {
00074     phFUNCTION("phPositionData::copyData")
00075     int rwlocked = 0;
00076     int other_rwlocked = 0;
00077     
00078     phTHIS_WRITELOCK(rwlocked);
00079     phREADLOCK(copyfrom,other_rwlocked);
00080 
00081     phMemcpy(this->m_pose,  copyfrom.m_pose, sizeof(double) * 3);
00082     phMemcpy(this->m_size,  copyfrom.m_size, sizeof(double) * 2);
00083     this->m_px = copyfrom.m_px;
00084     this->m_py = copyfrom.m_py;
00085     this->m_pa = copyfrom.m_pa;
00086     this->m_vx = copyfrom.m_vx;
00087     this->m_vy = copyfrom.m_vy;
00088     this->m_va = copyfrom.m_va;
00089     this->m_stall = copyfrom.m_stall;
00090 
00091     rc = this->notify();
00092     phPRINT_RC(rc,NULL,"this->notify()");
00093 
00094     phRWUNLOCK(copyfrom,other_rwlocked);
00095     phTHIS_RWUNLOCK(rwlocked);
00096 
00097     return phSUCCESS;
00098 error:
00099     phRWUNLOCK_ERROR(copyfrom,other_rwlocked);
00100     phTHIS_RWUNLOCK_ERROR(rwlocked);
00101     
00102     return phFAIL;    
00103 }
00104 
00105 /* ---------------------------------------------------------------------- */
00106 int phPositionData::swapData( phPositionData &data )
00107 {
00108     phFUNCTION("phPositionData::swapData")
00109     int rwlocked = 0;
00110     int other_rwlocked = 0;
00111     double  t_pose[3];
00112     double  t_size[2];
00113     double  t_px;
00114     double  t_py;
00115     double  t_pa;
00116     double  t_vx;
00117     double  t_vy;
00118     double  t_va;
00119     int     t_stall;
00120 
00121     phTHIS_WRITELOCK(rwlocked);
00122         
00123     if (this->isSwapEnabled())
00124     {
00125         phWRITELOCK(data,other_rwlocked);
00126     
00127         phMemcpy(t_pose,   data.m_pose, sizeof(double) * 3);
00128         phMemcpy(t_size,   data.m_size, sizeof(double) * 2);
00129         t_px = data.m_px;
00130         t_py = data.m_py;
00131         t_pa = data.m_pa;
00132         t_vx = data.m_vx;
00133         t_vy = data.m_vy;
00134         t_va = data.m_va;
00135         t_stall = data.m_stall;
00136  
00137         phMemcpy(data.m_pose,   this->m_pose, sizeof(double) * 3);
00138         phMemcpy(data.m_size,this->m_size, sizeof(double) * 2);
00139         data.m_px = this->m_px;
00140         data.m_py = this->m_py;
00141         data.m_pa = this->m_pa;
00142         data.m_vx = this->m_vx;
00143         data.m_vy = this->m_vy;
00144         data.m_va = this->m_va;
00145         data.m_stall = this->m_stall;
00146 
00147         phMemcpy(this->m_pose,   t_pose, sizeof(double) * 3);
00148         phMemcpy(this->m_size,t_size, sizeof(double) * 2);
00149         this->m_px = t_px;
00150         this->m_py = t_py;
00151         this->m_pa = t_pa;
00152         this->m_vx = t_vx;
00153         this->m_vy = t_vy;
00154         this->m_va = t_va;
00155         this->m_stall = t_stall;
00156 
00157         rc = this->notify();
00158         phPRINT_RC(rc,NULL,"this->notify()");
00159     
00160         rc = data.notify();
00161         phPRINT_RC(rc,NULL,"data.notify()");
00162 
00163         phRWUNLOCK(data,other_rwlocked);
00164     }
00165     else
00166     {
00167         rc = this->copyData(data);
00168         phCHECK_RC(rc,NULL,"this->copyData(data)");
00169     }
00170 
00171     phTHIS_RWUNLOCK(rwlocked);
00172 
00173     return phSUCCESS;
00174 error:
00175     phRWUNLOCK_ERROR(data,other_rwlocked);
00176     phTHIS_RWUNLOCK_ERROR(rwlocked);
00177     
00178     return phFAIL;    
00179 }
00180 
00181 /* ---------------------------------------------------------------------- */
00182 int phPositionData::set(double pose[3],double size[2],
00183                         double px,     double py,     double pa,
00184                         double vx,     double vy,     double va,
00185                         int    stall )
00186 {
00187     phFUNCTION("phPositionData::set")
00188     int rwlocked = 0;
00189 
00190     phTHIS_WRITELOCK(rwlocked);
00191     
00192     phMemcpy(this->m_pose,pose, sizeof(double) * 3);
00193     phMemcpy(this->m_size,size, sizeof(double) * 2);
00194     this->m_px = px;
00195     this->m_py = py;
00196     this->m_pa = pa;
00197     this->m_vx = vx;
00198     this->m_vy = vy;
00199     this->m_va = va;
00200     this->m_stall = stall;
00201 
00202     rc = this->notify();
00203     phPRINT_RC(rc,NULL,"this->notify()");
00204 
00205     phTHIS_RWUNLOCK_RET(rwlocked,phSUCCESS,phFAIL);
00206 }
00207 
00208 /* ---------------------------------------------------------------------- */
00209 int phPositionData::get(double    *pose[3],      double    *size[2],
00210                         double *px, double *py, double *pa,
00211                         double *vx, double *vy, double *va,
00212                         int    *stall )
00213 {
00214     phFUNCTION("phPositionData::get")
00215     int rwlocked = 0;
00216     
00217     phTHIS_READLOCK(rwlocked);
00218 
00219     if (pose != NULL)
00220         phMemcpy(pose,this->m_pose, sizeof(double) * 3);
00221     if (size != NULL)
00222         phMemcpy(size,this->m_size, sizeof(double) * 2);
00223     if (px != NULL) *px = this->m_px;
00224     if (py != NULL) *py = this->m_py;
00225     if (pa != NULL) *pa = this->m_pa;
00226     if (vx != NULL) *vx = this->m_vx;
00227     if (vy != NULL) *vy = this->m_vy;
00228     if (va != NULL) *va = this->m_va;
00229     if (stall != NULL) *stall = this->m_stall;
00230 
00231     phTHIS_RWUNLOCK_RET(rwlocked,phSUCCESS,phFAIL);
00232 }
00233 
00234 /* ---------------------------------------------------------------------- */
00235 int phPositionData::getPoseArray( double *pose[3] )
00236 {
00237     phFUNCTION("phPositionData::getPoseArray")
00238     int rwlocked = 0;
00239     
00240     phTHIS_READLOCK(rwlocked);
00241 
00242     phMemcpy(pose,this->m_pose, sizeof(double) * 3);
00243 
00244     phTHIS_RWUNLOCK_RET(rwlocked,phSUCCESS,phFAIL);
00245 }
00246 
00247 /* ---------------------------------------------------------------------- */
00248 int phPositionData::getSize     ( double *size[2] )
00249 {
00250     phFUNCTION("phPositionData::getSize")
00251     int rwlocked = 0;
00252     
00253     phTHIS_READLOCK(rwlocked);
00254 
00255     phMemcpy(size,this->m_size, sizeof(double) * 2);
00256 
00257     phTHIS_RWUNLOCK_RET(rwlocked,phSUCCESS,phFAIL);
00258 }
00259 
00260 /* ---------------------------------------------------------------------- */
00261 int phPositionData::getPose( double *px, double *py, double *pa )
00262 {
00263     phFUNCTION("phPositionData::getPose")
00264     int rwlocked = 0;
00265     
00266     phTHIS_READLOCK(rwlocked);
00267 
00268     *px = this->m_px;
00269     *py = this->m_py;
00270     *pa = this->m_pa;
00271     
00272     phTHIS_RWUNLOCK_RET(rwlocked,phSUCCESS,phFAIL);
00273 }
00274 
00275 /* ---------------------------------------------------------------------- */
00276 double phPositionData::get_px()
00277 {
00278     phFUNCTION("phPositionData::get_px")
00279     int rwlocked = 0;
00280     double px = 0.0;
00281     
00282     phTHIS_READLOCK(rwlocked);
00283 
00284     px = this->m_px;
00285     
00286     phTHIS_RWUNLOCK_RET(rwlocked,px,phFAIL);
00287 }
00288 
00289 /* ---------------------------------------------------------------------- */
00290 double phPositionData::get_py()
00291 {
00292     phFUNCTION("phPositionData::get_py")
00293     int rwlocked = 0;
00294     double py = 0.0;
00295     
00296     phTHIS_READLOCK(rwlocked);
00297 
00298     py = this->m_py;
00299 
00300     phTHIS_RWUNLOCK_RET(rwlocked,py,phFAIL);
00301 }
00302 
00303 /* ---------------------------------------------------------------------- */
00304 double phPositionData::get_pa()
00305 {
00306     phFUNCTION("phPositionData::get_pa")
00307     int rwlocked = 0;
00308     double pa = 0.0;
00309     
00310     phTHIS_READLOCK(rwlocked);
00311 
00312     pa = this->m_pa;
00313 
00314     phTHIS_RWUNLOCK_RET(rwlocked,pa,phFAIL);
00315 }
00316 
00317 /* ---------------------------------------------------------------------- */
00318 int phPositionData::getVelocity( double *vx, double *vy, double *va )
00319 {
00320     phFUNCTION("phPositionData::getVelocity")
00321     int rwlocked = 0;
00322     
00323     phTHIS_READLOCK(rwlocked);
00324 
00325     *vx = this->m_vx;
00326     *vy = this->m_vy;
00327     *va = this->m_va;
00328     
00329     phTHIS_RWUNLOCK_RET(rwlocked,phSUCCESS,phFAIL);
00330 }
00331 
00332 /* ---------------------------------------------------------------------- */
00333 double phPositionData::get_vx()
00334 {
00335     phFUNCTION("phPositionData::get_vx")
00336     int rwlocked = 0;
00337     double vx = 0.0;
00338     
00339     phTHIS_READLOCK(rwlocked);
00340 
00341     vx = this->m_vx;
00342     
00343     phTHIS_RWUNLOCK_RET(rwlocked,vx,phFAIL);
00344 }
00345 
00346 /* ---------------------------------------------------------------------- */
00347 double phPositionData::get_vy()
00348 {
00349     phFUNCTION("phPositionData::get_vy")
00350     int rwlocked = 0;
00351     double vy = 0.0;
00352     
00353     phTHIS_READLOCK(rwlocked);
00354 
00355     vy = this->m_vy;
00356     
00357     phTHIS_RWUNLOCK_RET(rwlocked,vy,phFAIL);
00358 }
00359 
00360 /* ---------------------------------------------------------------------- */
00361 double phPositionData::get_va()
00362 {
00363     phFUNCTION("phPositionData::get_va")
00364     int rwlocked = 0;
00365     double va = 0.0;
00366     
00367     phTHIS_READLOCK(rwlocked);
00368 
00369     va = this->m_va;
00370 
00371     phTHIS_RWUNLOCK_RET(rwlocked,va,phFAIL);
00372 }
00373 
00374 /* ---------------------------------------------------------------------- */
00375 int phPositionData::get_stall()
00376 {
00377     phFUNCTION("phPositionData::get_stall")
00378     int rwlocked = 0;
00379     int stall = 0;
00380     
00381     phTHIS_READLOCK(rwlocked);
00382 
00383     stall = this->m_stall;
00384 
00385     phTHIS_RWUNLOCK_RET(rwlocked,stall,phFAIL);
00386 }
00387 
00388 /* ---------------------------------------------------------------------- */
00389 void phPositionData::print_data( )
00390 {
00391     phFUNCTION("phPositionData::print_data")
00392     int rwlocked = 0;
00393 
00394     phTHIS_READLOCK_LOOSE(rwlocked);
00395     
00396     fprintf(stderr,"----------------------------------------------------\n");
00397     fprintf(stderr,"%s\n",this->getName());
00398     fprintf(stderr,"----------------------------------------------------\n");
00399     fprintf(stderr,"pose = ( %0.8f, %0.8f, %0.8f )\n",
00400           this->m_pose[0], this->m_pose[1], this->m_pose[2]);
00401     fprintf(stderr,"size = ( %0.8f, %0.8f )\n",
00402           this->m_size[0], this->m_size[1] );
00403     fprintf(stderr,"p ( x:%0.8f y:%0.8f a%0.8f )\n",
00404           this->m_px, this->m_py, this->m_pa );
00405     fprintf(stderr,"v ( x:%0.8f y:%0.8f a%0.8f )\n",
00406           this->m_vx, this->m_vy, this->m_va );
00407     fprintf(stderr,"stall: %d\n",
00408           this->m_stall );
00409     fprintf(stderr,"----------------------------------------------------\n");
00410     
00411     phTHIS_RWUNLOCK_LOOSE(rwlocked);
00412 }




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