aseba: aseba: utils.cpp Source File

00001 /*
00002         Aseba - an event-based framework for distributed robot control
00003         Copyright (C) 2007--2012:
00004                 Stephane Magnenat <stephane at magnenat dot net>
00005                 (http://stephane.magnenat.net)
00006                 and other contributors, see authors.txt for details
00007         
00008         This program is free software: you can redistribute it and/or modify
00009         it under the terms of the GNU Lesser General Public License as published
00010         by the Free Software Foundation, version 3 of the License.
00011         
00012         This program is distributed in the hope that it will be useful,
00013         but WITHOUT ANY WARRANTY; without even the implied warranty of
00014         MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00015         GNU Lesser General Public License for more details.
00016         
00017         You should have received a copy of the GNU Lesser General Public License
00018         along with this program. If not, see <http://www.gnu.org/licenses/>.
00019 */
00020 
00021 #include <cstdlib>
00022 #include <cstring>
00023 #ifndef WIN32
00024         #include <sys/time.h>
00025 #else // WIN32
00026         #include <sys/types.h>
00027         #include <windows.h>
00028         #define atoll _atoi64
00029 #endif // WIN32
00030 #include <time.h>
00031 #include <errno.h>
00032 #include <iomanip>
00033 #include <ostream>
00034 #include <sstream>
00035 #include <cassert>
00036 #include <limits>
00037 #include <stdint.h>
00038 #include "utils.h"
00039 
00040 namespace Aseba
00041 {
00044         
00045         UnifiedTime::UnifiedTime()
00046         {
00047                 #ifndef WIN32
00048                 struct timeval tv;
00049                 gettimeofday(&tv, NULL);
00050                 value = (Value(tv.tv_sec) * 1000) + Value(tv.tv_usec) / 1000;
00051                 #else // WIN32
00052                 FILETIME        ft;
00053                 LARGE_INTEGER   li;
00054                 __int64         t;
00055                 GetSystemTimeAsFileTime(&ft);
00056                 li.LowPart = ft.dwLowDateTime;
00057                 li.HighPart = ft.dwHighDateTime;
00058                 t = li.QuadPart;                        // In 100-nanosecond intervals
00059                 value = t / 10000l;                      // In milliseconds
00060                 #endif // WIN32
00061         }
00062         
00063         UnifiedTime::UnifiedTime(Value ms) :
00064                 value(ms)
00065         {
00066         }
00067         
00068         UnifiedTime::UnifiedTime(Value seconds, Value milliseconds) :
00069                 value(seconds * 1000 + milliseconds)
00070         {
00071         }
00072         
00073         void UnifiedTime::sleep() const
00074         {
00075                 #ifndef WIN32
00076                 struct timespec ts;
00077                 ts.tv_sec = (value / 1000);
00078                 ts.tv_nsec = ((value % 1000) * 1000000);
00079                 nanosleep(&ts, 0);
00080                 #else // WIN32
00081                 assert(value < (1 << 32));
00082                 Sleep((DWORD)value);
00083                 #endif // WIN32
00084         }
00085         
00086         std::string UnifiedTime::toHumanReadableStringFromEpoch() const
00087         {
00088                 std::ostringstream oss;
00089                 Value seconds(value / 1000);
00090                 Value milliseconds(value % 1000);
00091                 time_t t(seconds);
00092                 char *timeString = ctime(&t);
00093                 timeString[strlen(timeString) - 1] = 0;
00094                 oss << "[";
00095                 oss << timeString << " ";
00096                 oss << std::setfill('0') << std::setw(3) << milliseconds;
00097                 oss << "]";
00098                 return oss.str();
00099         }
00100         
00101         std::string UnifiedTime::toRawTimeString() const
00102         {
00103                 std::ostringstream oss;
00104                 Value seconds(value / 1000);
00105                 Value milliseconds(value % 1000);
00106                 oss << std::dec << seconds << "." << std::setfill('0') << std::setw(3) << milliseconds;
00107                 return oss.str();
00108         }
00109         
00110         UnifiedTime UnifiedTime::fromRawTimeString(const std::string& rawTimeString)
00111         {
00112                 size_t dotPos(rawTimeString.find('.'));
00113                 assert(dotPos != std::string::npos);
00114                 return UnifiedTime(atoll(rawTimeString.substr(0, dotPos).c_str()), atoll(rawTimeString.substr(dotPos + 1, std::string::npos).c_str()));
00115         }
00116         
00117         
00118         void dumpTime(std::ostream &stream, bool raw)
00119         {
00120                 if (raw)
00121                         stream << UnifiedTime().toRawTimeString();
00122                 else
00123                         stream << UnifiedTime().toHumanReadableStringFromEpoch();
00124                 stream << " ";
00125         }
00126         
00127         std::string WStringToUTF8(const std::wstring& s)
00128         {
00129                 std::string os;
00130                 for (size_t i = 0; i < s.length(); ++i)
00131                 {
00132                         const wchar_t c(s[i]);
00133                         if (c < 0x80)
00134                         {
00135                                 os += static_cast<uint8_t>(c);
00136                         }
00137                         else if (c < 0x800)
00138                         {
00139                                 os += static_cast<uint8_t>(((c>>6)&0x1F)|0xC0);
00140                                 os += static_cast<uint8_t>((c&0x3F)|0x80);
00141                         }
00142                         else if (c < 0xd800)
00143                         {
00144                                 os += static_cast<uint8_t>(((c>>12)&0x0F)|0xE0);
00145                                 os += static_cast<uint8_t>(((c>>6)&0x3F)|0x80);
00146                                 os += static_cast<uint8_t>((c&0x3F)|0x80);
00147                         }
00148                         else
00149                         {
00150                                 os += '?';
00151                         }
00152                 }
00153                 // TODO: add >UTF16 support
00154                 return os;
00155         }
00156         
00157         /*
00158         
00159         This code is heavily inspired by http://www.cplusplus.com/forum/general/7142/
00160         released under the following license:
00161         
00162         * Copyright (c) 2009, Helios (helios.vmg@gmail.com)
00163         * All rights reserved.
00164         *
00165         * Redistribution and use in source and binary forms, with or without
00166         * modification, are permitted provided that the following conditions are met:
00167         *     * Redistributions of source code must retain the above copyright notice,
00168         *       this list of conditions and the following disclaimer.
00169         *     * Redistributions in binary form must reproduce the above copyright
00170         *       notice, this list of conditions and the following disclaimer in the
00171         *       documentation and/or other materials provided with the distribution.
00172         *
00173         * THIS SOFTWARE IS PROVIDED BY HELIOS "AS IS" AND ANY EXPRESS OR IMPLIED
00174         * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
00175         * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO
00176         * EVENT SHALL HELIOS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
00177         * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
00178         * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
00179         * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
00180         * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
00181         * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
00182         * OF THE POSSIBILITY OF SUCH DAMAGE.
00183         */
00184 
00185         std::wstring UTF8ToWString(const std::string& s)
00186         {
00187                 std::wstring res;
00188                 for (size_t i = 0; i < s.length(); ++i)
00189                 {
00190                         const char *a = &s[i];
00191                         if (!(*a&128))
00192                         {
00193                                 //Byte represents an ASCII character. Direct copy will do.
00194                                 res += *a;
00195                         }
00196                         else if ((*a&192)==128)
00197                         {
00198                                 //Byte is the middle of an encoded character. Ignore.
00199                                 continue;
00200                         }
00201                         else if ((*a&224)==192)
00202                         {
00203                                 //Byte represents the start of an encoded character in the range
00204                                 //U+0080 to U+07FF
00205                                 res += ((*a&31)<<6)|(a[1]&63);
00206                         }
00207                         else if ((*a&240)==224)
00208                         {
00209                                 //Byte represents the start of an encoded character in the range
00210                                 //U+07FF to U+FFFF
00211                                 res += ((*a&15)<<12)|((a[1]&63)<<6)|(a[2]&63);
00212                         }
00213                         else if ((*a&248)==240)
00214                         {
00215                                 //Byte represents the start of an encoded character beyond the
00216                                 //U+FFFF limit of 16-bit integers
00217                                 res += '?';
00218                         }
00219                 }
00220                 // TODO: add >UTF16 support
00221                 return res;
00222         }
00223         
00224         /*
00225          * Code from http://www.nongnu.org/avr-libc/
00226          * Released under http://www.nongnu.org/avr-libc/LICENSE.txt
00227          * which is GPL- and DFSG- compatible
00228          */
00229         static uint16 crc_xmodem_update (uint16 crc, uint8 data)
00230         {
00231                 int i;
00232 
00233                 crc = crc ^ ((uint16)data << 8);
00234                 for (i=0; i<8; i++)
00235                 {
00236                         if (crc & 0x8000)
00237                                 crc = (crc << 1) ^ 0x1021;
00238                         else
00239                                 crc <<= 1;
00240                 }
00241 
00242                 return crc;
00243         }
00244         
00245         static uint16 crc_xmodem_update (uint16 crc, const uint8* data, size_t len)
00246         {
00247                 for (size_t i = 0; i < len; ++i)
00248                         crc = crc_xmodem_update(crc, data[i]);
00249                 return crc;
00250         }
00251         
00252         uint16 crcXModem(const uint16 oldCrc, const std::wstring& s)
00253         {
00254                 std::string utf8s(WStringToUTF8(s));
00255                 size_t l = utf8s.size();
00256                 if (l & 0x1)
00257                         ++l;
00258                 return crc_xmodem_update(oldCrc, reinterpret_cast<const uint8*>(utf8s.c_str()), l);
00259         }
00260         
00261         uint16 crcXModem(const uint16 oldCrc, const uint16 v)
00262         {
00263                 return crc_xmodem_update(oldCrc, reinterpret_cast<const uint8*>(&v), 2);
00264         }
00265         
00267 }