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

phSonarData.cpp

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




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