00001 /**************************************************************** 00002 * 00003 * Copyright (c) 2010 00004 * 00005 * Fraunhofer Institute for Manufacturing Engineering 00006 * and Automation (IPA) 00007 * 00008 * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 00009 * 00010 * Project name: care-o-bot 00011 * ROS stack name: cob3_common 00012 * ROS package name: generic_can 00013 * Description: 00014 * 00015 * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 00016 * 00017 * Author: Christian Connette, email:christian.connette@ipa.fhg.de 00018 * Supervised by: Christian Connette, email:christian.connette@ipa.fhg.de 00019 * 00020 * Date of creation: Feb 2009 00021 * ToDo: Check if this is still neccessary. Can we use the ROS-Infrastructure within the implementation? 00022 * 00023 * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 00024 * 00025 * Redistribution and use in source and binary forms, with or without 00026 * modification, are permitted provided that the following conditions are met: 00027 * 00028 * * Redistributions of source code must retain the above copyright 00029 * notice, this list of conditions and the following disclaimer. 00030 * * Redistributions in binary form must reproduce the above copyright 00031 * notice, this list of conditions and the following disclaimer in the 00032 * documentation and/or other materials provided with the distribution. 00033 * * Neither the name of the Fraunhofer Institute for Manufacturing 00034 * Engineering and Automation (IPA) nor the names of its 00035 * contributors may be used to endorse or promote products derived from 00036 * this software without specific prior written permission. 00037 * 00038 * This program is free software: you can redistribute it and/or modify 00039 * it under the terms of the GNU Lesser General Public License LGPL as 00040 * published by the Free Software Foundation, either version 3 of the 00041 * License, or (at your option) any later version. 00042 * 00043 * This program is distributed in the hope that it will be useful, 00044 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00045 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00046 * GNU Lesser General Public License LGPL for more details. 00047 * 00048 * You should have received a copy of the GNU Lesser General Public 00049 * License LGPL along with this program. 00050 * If not, see <http://www.gnu.org/licenses/>. 00051 * 00052 ****************************************************************/ 00053 00054 #include <cob_utilities/TimeStamp.h> 00055 00056 //----------------------------------------------------------------------------- 00057 00058 TimeStamp::TimeStamp() 00059 { 00060 m_TimeStamp.tv_sec = 0; 00061 m_TimeStamp.tv_nsec = 0; 00062 } 00063 00064 void TimeStamp::SetNow() 00065 { 00066 ::clock_gettime(CLOCK_REALTIME, &m_TimeStamp); 00067 } 00068 00069 double TimeStamp::TimespecToDouble(const ::timespec& LargeInt) 00070 { 00071 return double(LargeInt.tv_sec) + double(LargeInt.tv_nsec) / 1e9; 00072 } 00073 00074 ::timespec TimeStamp::DoubleToTimespec(double TimeS) 00075 { 00076 ::timespec DeltaTime; 00077 if (! ( TimeS < 4e9 && TimeS > 0.0 )) 00078 { 00079 DeltaTime.tv_sec = 0; 00080 DeltaTime.tv_nsec = 0; 00081 return DeltaTime; 00082 } 00083 00084 DeltaTime.tv_sec = ::time_t(TimeS); 00085 DeltaTime.tv_nsec 00086 = static_cast<long int>((TimeS - double(DeltaTime.tv_sec)) * 1e9); 00087 00088 return DeltaTime; 00089 } 00090 00091 double TimeStamp::operator-(const TimeStamp& EarlierTime) const 00092 { 00093 ::timespec Res; 00094 00095 Res.tv_sec = m_TimeStamp.tv_sec - EarlierTime.m_TimeStamp.tv_sec; 00096 Res.tv_nsec = m_TimeStamp.tv_nsec - EarlierTime.m_TimeStamp.tv_nsec; 00097 00098 if (Res.tv_nsec < 0) { 00099 Res.tv_sec--; 00100 Res.tv_nsec += 1000000000; 00101 } 00102 00103 return TimespecToDouble(Res); 00104 } 00105 00106 void TimeStamp::operator+=(double TimeS) 00107 { 00108 ::timespec Dbl = DoubleToTimespec(TimeS); 00109 m_TimeStamp.tv_sec += Dbl.tv_sec; 00110 m_TimeStamp.tv_nsec += Dbl.tv_nsec; 00111 if (m_TimeStamp.tv_nsec > 1000000000) 00112 { 00113 m_TimeStamp.tv_sec++; 00114 m_TimeStamp.tv_nsec -= 1000000000; 00115 } 00116 } 00117 00118 void TimeStamp::operator-=(double TimeS) 00119 { 00120 ::timespec Dbl = DoubleToTimespec(TimeS); 00121 m_TimeStamp.tv_sec -= Dbl.tv_sec; 00122 m_TimeStamp.tv_nsec -= Dbl.tv_nsec; 00123 if (m_TimeStamp.tv_nsec < 0.0) 00124 { 00125 m_TimeStamp.tv_sec--; 00126 m_TimeStamp.tv_nsec += 1000000000; 00127 } 00128 } 00129 00130 bool TimeStamp::operator>(const TimeStamp& Time) 00131 { 00132 if (m_TimeStamp.tv_sec > Time.m_TimeStamp.tv_sec) return true; 00133 if ((m_TimeStamp.tv_sec == Time.m_TimeStamp.tv_sec) && 00134 (m_TimeStamp.tv_nsec > Time.m_TimeStamp.tv_nsec)) return true; 00135 return false; 00136 } 00137 00138 bool TimeStamp::operator<(const TimeStamp& Time) 00139 { 00140 if (m_TimeStamp.tv_sec < Time.m_TimeStamp.tv_sec) return true; 00141 if ((m_TimeStamp.tv_sec == Time.m_TimeStamp.tv_sec) && 00142 (m_TimeStamp.tv_nsec < Time.m_TimeStamp.tv_nsec)) return true; 00143 return false; 00144 } 00145 00146 void TimeStamp::getTimeStamp(long& lSeconds, long& lNanoSeconds) 00147 { 00148 lSeconds = m_TimeStamp.tv_sec; 00149 lNanoSeconds = m_TimeStamp.tv_nsec; 00150 }; 00151 00152 void TimeStamp::setTimeStamp(const long& lSeconds, const long& lNanoSeconds) 00153 { 00154 m_TimeStamp.tv_sec = lSeconds; 00155 m_TimeStamp.tv_nsec = lNanoSeconds; 00156 }; 00157 00158 std::string TimeStamp::CurrentToString() 00159 { 00160 # define TIME_SIZE 400 00161 00162 const struct tm *tm; 00163 size_t len; 00164 time_t now; 00165 char pres[TIME_SIZE]; 00166 std::string s; 00167 00168 now = time ( NULL ); 00169 tm = localtime ( &now ); 00170 len = strftime ( pres, TIME_SIZE, "%Y-%m-%d %H:%M:%S.", tm ); 00171 00172 s = (std::string)pres + NumToString(m_TimeStamp.tv_nsec / 1000); 00173 00174 return s; 00175 # undef TIME_SIZE 00176 } 00177 00178 std::string TimeStamp::ToString() 00179 { 00180 # define TIME_SIZE 4000 00181 00182 const struct tm *tm; 00183 size_t len; 00184 //time_t now; 00185 char pres[TIME_SIZE]; 00186 std::string s; 00187 00188 tm = localtime ( &m_TimeStamp.tv_sec ); 00189 len = strftime ( pres, TIME_SIZE, "%Y-%m-%d %H:%M:%S.", tm ); 00190 00191 s = (std::string)pres + NumToString(m_TimeStamp.tv_nsec / 1000); 00192 00193 return s; 00194 # undef TIME_SIZE 00195 }