atlasmv_base: /home/laradmin/lar/bases/atlasmv_base/src/timer.cpp Source File

00001 /**************************************************************************************************
00002  Software License Agreement (BSD License)
00003 
00004  Copyright (c) 2011-2013, LAR toolkit developers - University of Aveiro - http://lars.mec.ua.pt
00005  All rights reserved.
00006 
00007  Redistribution and use in source and binary forms, with or without modification, are permitted
00008  provided that the following conditions are met:
00009 
00010   *Redistributions of source code must retain the above copyright notice, this list of
00011    conditions and the following disclaimer.
00012   *Redistributions in binary form must reproduce the above copyright notice, this list of
00013    conditions and the following disclaimer in the documentation and/or other materials provided
00014    with the distribution.
00015   *Neither the name of the University of Aveiro nor the names of its contributors may be used to
00016    endorse or promote products derived from this software without specific prior written permission.
00017  
00018  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR
00019  IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
00020  FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
00021  CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
00022  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
00023  DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
00024  IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
00025  OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00026 ***************************************************************************************************/
00031 #ifndef _TIMER_CPP_
00032 #define _TIMER_CPP_
00033 
00037 #include <timer.h>
00038 
00044 double lar_get_time()
00045 {
00046         struct timeval tv;
00047         double t;
00048         
00049         if(gettimeofday(&tv, NULL)<0)
00050         {
00051                 printf("\33[31mWARNING:\33[0m problem getting time\n");
00052         }
00053         
00054         t = (tv.tv_sec + (tv.tv_usec/1000000.0));// function gettimeofday returns time in seconds with a precision of 0.01seconds
00055         
00056         return t;
00057 }
00061 c_timer::c_timer()
00062 {
00063         //      printf("class initialized\n");
00064         for (int j=0;j<10;j++)
00065         {
00066                 clock_set[j] = 0; //set all ipc flags to zero
00067                 lar_toc[j] = 0;
00068                 lar_tic[j] = 0.0;
00069         }
00070         //if(lar_ipc_sleep(0.01))
00071         //{
00072                 //ipc_connected=0;
00073                 //}else
00074                 //{
00075                         //ipc_connected=1;
00076                         //};
00077                         
00078                         //pthread_mutex_init(&ipc_connected_mutex, NULL);
00079                         pthread_mutex_init(&clock_tic_mutex, NULL);
00080                         pthread_mutex_init(&clock_set_mutex, NULL);
00081                         pthread_mutex_init(&lar_tic_mutex, NULL);
00082                         pthread_mutex_init(&lar_toc_mutex, NULL);
00083                         
00084 };
00085 
00089 c_timer::~c_timer()
00090 {
00091 }
00092 
00098 void c_timer::set_ipc_sleep(int i)
00099 {
00100         pthread_mutex_lock(&clock_set_mutex);
00101         if(!clock_set[i])
00102         {
00103                 clock_tic[i] = lar_get_time();
00104                 clock_set[i] = 1;
00105         }
00106         pthread_mutex_unlock(&clock_set_mutex);
00107 };
00108 
00115 void c_timer::run_sleep(int i, double desired_freq)
00116 {
00117         double ts = (1/desired_freq);
00118         double tsf;
00119         unsigned int t;
00120         
00121         if(abs(i)<11)
00122         {
00123                 //pthread_mutex_lock(&lar_toc_mutex);
00124                 //if(!lar_toc[i])
00125                 //{
00126                         //pthread_mutex_unlock(&lar_toc_mutex);
00127                         //toc(i);
00128                         //}
00129                         pthread_mutex_lock(&lar_tic_mutex);
00130                         tsf = ts - (ros::Time::now().toSec()-lar_tic[i]);
00131                         pthread_mutex_unlock(&lar_tic_mutex);
00132                         t=(tsf<0?100:tsf*1E6);
00133                         //printf("sleep:%d\n", t);
00134                         usleep(t);
00135 }
00136 
00137 };
00138 
00148 char c_timer::run_ipc_sleep(double desired_freq, int i, char verbose)
00149 {
00150 //      double ts = (1.0/desired_freq);
00151 //      double tsf = ts;
00152 //      
00153 //      pthread_mutex_lock(&clock_set_mutex);
00154 //      if(clock_set[i])
00155 //      {
00156 //              tsf = ts - (ros::time:now().to_sec()-clock_tic[i]);
00157 //              
00158 //              if(tsf <= MINIMUM_IPC_SLEEP)
00159 //              {
00160 //                      if(verbose)
00161 //                      {
00162 //                              printf("\33[31mdesired frequency %3.1f NOK.\33[0m\n", desired_freq);
00163 //                              printf("\33[31mactual frequency %3.2f.\33[0m\n", 1.0/(ros::time:now().to_sec()-clock_tic[i]));
00164 //                      };
00165 //                      tsf = MINIMUM_IPC_SLEEP;
00166 //              }else
00167 //              {
00168 //                      if(verbose)
00169 //                      {
00170 //                              printf("\33[31mdesired frequency %3.1fHz OK.\33[0m\r", desired_freq); fflush(stdout);
00171 //                              printf("\33[31mactual frequency %3.2f.\33[0m\n", 1.0/(ros::time:now().to_sec()-clock_tic[i]));
00172 //                      }
00173 //              };
00174 //      }
00175 //      
00176 //      
00177 //      //carmen_ipc_sleep(tsf);
00178 //      char rt = lar_ipc_sleep(tsf);
00179 //      clock_set[i]=0;
00180 //      pthread_mutex_unlock(&clock_set_mutex);
00181 
00182         return 'A';
00183 }
00184 
00190 void c_timer::tic(int i)
00191 {
00192         pthread_mutex_lock(&lar_tic_mutex);
00193         if(abs(i)<11)
00194         {
00195                 lar_tic[i] = ros::Time::now().toSec();
00196         }
00197         pthread_mutex_unlock(&lar_tic_mutex);
00198 };
00199 
00205 void c_timer::toc(int i)
00206 {
00207         pthread_mutex_lock(&lar_toc_mutex);
00208         pthread_mutex_lock(&lar_tic_mutex);
00209         if(abs(i)<11)
00210                 lar_toc[i] = (ros::Time::now().toSec() - lar_tic[i]);
00211         
00212         pthread_mutex_unlock(&lar_tic_mutex);
00213         pthread_mutex_unlock(&lar_toc_mutex);
00214 };
00215 
00216 //added by dgameiro to allow get time stamp
00217 double c_timer::get_toc(int i)
00218 {
00219         if(abs(i)<11)
00220         {
00221                 pthread_mutex_lock(&lar_toc_mutex);
00222                 if(!lar_toc[i])
00223                 {
00224                         pthread_mutex_unlock(&lar_toc_mutex);
00225                         toc(i);
00226                         pthread_mutex_lock(&lar_toc_mutex);
00227                 }
00228                 double result = lar_toc[i];
00229                 pthread_mutex_unlock(&lar_toc_mutex);
00230                 return result;
00231         }else{
00232                 //              printf("error. can only exist 10");
00233         }
00234         return 0;
00235 }
00236 
00237 #endif
00238