m5p3nc3r/agv_pro_ros2
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

feat(slam): add rtabmap_ros

Browse Source
This commit is contained in:
X-lanni
2025-07-14 11:34:38 +08:00
parent 3b6641c1fb
commit 943ce5b06f
1635 changed files with 603092 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rtabmap/utilite/CMakeLists.txt
+10
View File
@@ -0,0 +1,10 @@
SET(UTILITE_VERSION "0.3.0")
ADD_DEFINITIONS(-DUTILITE_VERSION="${UTILITE_VERSION}")
if(UNIX AND NOT ANDROID)
FIND_PACKAGE(Pthreads REQUIRED)
ENDIF(UNIX AND NOT ANDROID)
ADD_SUBDIRECTORY( src )
ADD_SUBDIRECTORY( resource_generator )
rtabmap/utilite/include/rtabmap/utilite/Posix/UThreadC.h
+411
View File
@@ -0,0 +1,411 @@
/////////////////////////////////////////////////////////////////////
// Written by Phillip Sitbon
// Copyright 2003
//
// Modified by Mathieu Labbe
//
// Posix/Thread.h
// - Posix thread
//
/////////////////////////////////////////////////////////////////////
#ifndef _U_Thread_Posix_
#define _U_Thread_Posix_
#include "rtabmap/utilite/USemaphore.h"
#include "rtabmap/utilite/UMutex.h"
#include <pthread.h>
/**
* Calling thread sleeps for some milliseconds.
*/
inline void uSleep(unsigned int ms)
{
struct timespec req;
struct timespec rem;
req.tv_sec = ms / 1000;
req.tv_nsec = (ms - req.tv_sec * 1000) * 1000 * 1000;
nanosleep (&req, &rem);
}
/**
* Calling thread sleeps for some microseconds.
*/
inline void uSleepMicro(unsigned int us)
{
struct timespec req;
struct timespec rem;
req.tv_sec = us / 1000000;
req.tv_nsec = (us - req.tv_sec * 1000000) * 1000;
nanosleep (&req, &rem);
}
/**
* Calling thread sleeps for some nanoseconds.
*/
inline void uSleepNano(unsigned int ns)
{
struct timespec req;
struct timespec rem;
req.tv_sec = ns / 1000000000;
req.tv_nsec = (ns - req.tv_sec * 1000000000);
nanosleep (&req, &rem);
}
#define InvalidHandle 0
#define THREAD_HANDLE pthread_t
typedef void *( * pthread_fn )( void * );
template
<
typename Thread_T
>
class UThreadC
{
private:
struct Instance;
public:
typedef Thread_T & Thread_R;
typedef const Thread_T & Thread_C_R;
typedef THREAD_HANDLE Handle;
typedef void ( *Handler)( Thread_R );
virtual ~UThreadC() {}
protected:
UThreadC() {}
virtual void ThreadMain( Thread_R ) = 0;
static void Exit()
{ pthread_exit(0); }
#ifndef ANDROID
static void TestCancel()
{ pthread_testcancel(); }
#endif
static Handle Self()
{ return (Handle)pthread_self(); }
public:
static int Create(
const Handler & Function,
Thread_C_R Param,
Handle * const & H = 0,
const bool & CreateDetached = false,
const unsigned int & StackSize = 0,
const bool & CancelEnable = false,
const bool & CancelAsync = false
)
{
M_Create().lock();
pthread_attr_t attr;
pthread_attr_init(&attr);
if ( CreateDetached )
pthread_attr_setdetachstate(&attr,PTHREAD_CREATE_DETACHED);
if ( StackSize )
pthread_attr_setstacksize(&attr,StackSize);
Instance I(Param,0,Function,CancelEnable,CancelAsync);
Handle h=InvalidHandle;
int R = pthread_create((pthread_t *)&h,&attr,(pthread_fn)ThreadMainHandler,(void *)&I);
pthread_attr_destroy(&attr);
if(H) *H = h;
if ( !R ) S_Create().acquire();
M_Create().unlock();
return R;
}
int Create(
Thread_C_R Param,
Handle * const & H = 0,
const bool & CreateDetached = false,
const unsigned int & StackSize = 0,
const bool & CancelEnable = false,
const bool & CancelAsync = false
) const
{
M_Create().lock();
pthread_attr_t attr;
pthread_attr_init(&attr);
if ( CreateDetached )
pthread_attr_setdetachstate(&attr,PTHREAD_CREATE_DETACHED);
if ( StackSize )
pthread_attr_setstacksize(&attr,StackSize);
Instance I(Param,const_cast<UThreadC *>(this),0,CancelEnable,CancelAsync);
Handle h=InvalidHandle;
int R = pthread_create((pthread_t *)&h,&attr,(pthread_fn)ThreadMainHandler,(void *)&I);
pthread_attr_destroy(&attr);
if(H) *H = h;
if ( !R ) S_Create().acquire();
M_Create().unlock();
return R;
}
static int Join( Handle H )
{ return pthread_join(H,0); }
#ifndef ANDROID
static int Kill( Handle H )
{ return pthread_cancel(H); }
#endif
static int Detach( Handle H )
{ return pthread_detach(H); }
private:
static const UMutex &M_Create() { static UMutex M; return M; }
static USemaphore &S_Create() { static USemaphore S; return S; }
static void *ThreadMainHandler( Instance *Param )
{
Instance I(*Param);
Thread_T Data(I.Data);
S_Create().release();
#ifndef ANDROID
if ( I.Flags & 1 /*CancelEnable*/ )
{
pthread_setcancelstate(PTHREAD_CANCEL_ENABLE,NULL);
if ( I.Flags & 2 /*CancelAsync*/ )
pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS,NULL);
else
pthread_setcanceltype(PTHREAD_CANCEL_DEFERRED,NULL);
}
else
{
pthread_setcancelstate(PTHREAD_CANCEL_DISABLE,NULL);
}
#endif
if ( I.Owner )
I.Owner->ThreadMain(Data);
else
I.pFN(Data);
return 0;
}
struct Instance
{
Instance( Thread_C_R P, UThreadC<Thread_T> *const &O, const UThreadC<Thread_T>::Handler &pH = 0, const bool &CE=false, const bool &CA=false )
: Data(P), Owner(O), pFN(pH), Flags(0) { if ( CE ) Flags|=1; if ( CA ) Flags|=2; }
Thread_C_R Data;
UThreadC<Thread_T> * Owner;
Handler pFN;
unsigned char Flags;
};
};
/////////////////////////////////////////////////////////////////////
// Explicit specialization, no thread parameters
//
template<>
class UThreadC<void>
{
private:
struct Instance;
public:
typedef THREAD_HANDLE Handle;
typedef void ( *Handler)();
virtual ~UThreadC() {}
protected:
UThreadC() {}
virtual void ThreadMain() = 0;
static void Exit()
{ pthread_exit(0); }
#ifndef ANDROID
static void TestCancel()
{ pthread_testcancel(); }
#endif
static Handle Self()
{ return (Handle)pthread_self(); }
public:
static int Create(
const Handler & Function,
Handle * const & H = 0,
const bool & CreateDetached = false,
const unsigned int & StackSize = 0,
const bool & CancelEnable = false,
const bool & CancelAsync = false
)
{
M_Create().lock();
pthread_attr_t attr;
pthread_attr_init(&attr);
if ( CreateDetached )
pthread_attr_setdetachstate(&attr,PTHREAD_CREATE_DETACHED);
if ( StackSize )
pthread_attr_setstacksize(&attr,StackSize);
Instance I(0,Function,CancelEnable,CancelAsync);
Handle h=InvalidHandle;
int R = pthread_create((pthread_t *)&h,&attr,(pthread_fn)ThreadMainHandler,(void *)&I);
pthread_attr_destroy(&attr);
if(H) *H = h;
if ( !R ) S_Create().acquire();
M_Create().unlock();
return R;
}
int Create(
Handle * const & H = 0,
const bool & CreateDetached = false,
const unsigned int & StackSize = 0,
const bool & CancelEnable = false,
const bool & CancelAsync = false
) const
{
M_Create().lock();
pthread_attr_t attr;
pthread_attr_init(&attr);
if ( CreateDetached )
pthread_attr_setdetachstate(&attr,PTHREAD_CREATE_DETACHED);
if ( StackSize )
pthread_attr_setstacksize(&attr,StackSize);
Instance I(const_cast<UThreadC *>(this),0,CancelEnable,CancelAsync);
Handle h=InvalidHandle;
int R = pthread_create((pthread_t *)&h,&attr,(pthread_fn)ThreadMainHandler,(void *)&I);
pthread_attr_destroy(&attr);
if(H) *H = h;
if ( !R ) S_Create().acquire();
M_Create().unlock();
return R;
}
int Create(
unsigned long & ThreadId,
Handle * const & H = 0,
const bool & CreateDetached = false,
const unsigned int & StackSize = 0,
const bool & CancelEnable = false,
const bool & CancelAsync = false
) const
{
M_Create().lock();
pthread_attr_t attr;
pthread_attr_init(&attr);
if ( CreateDetached )
pthread_attr_setdetachstate(&attr,PTHREAD_CREATE_DETACHED);
if ( StackSize )
pthread_attr_setstacksize(&attr,StackSize);
Instance I(const_cast<UThreadC *>(this),0,CancelEnable,CancelAsync);
*H = InvalidHandle;
int R = pthread_create((pthread_t *)&(*H),&attr,(pthread_fn)ThreadMainHandler,(void *)&I);
ThreadId = (unsigned long)*H;
pthread_attr_destroy(&attr);
if ( !R ) S_Create().acquire();
M_Create().unlock();
return R;
}
static int Join( Handle H )
{ return pthread_join(H,0); }
#ifndef ANDROID
static int Kill( Handle H )
{ return pthread_cancel(H); }
#endif
static int Detach( Handle H )
{ return pthread_detach(H); }
private:
static const UMutex &M_Create() { static UMutex M; return M; }
static USemaphore &S_Create() { static USemaphore S; return S; }
static void *ThreadMainHandler( Instance *Param )
{
Instance I(*Param);
S_Create().release();
#ifndef ANDROID
if ( I.Flags & 1 /*CancelEnable*/ )
{
pthread_setcancelstate(PTHREAD_CANCEL_ENABLE,NULL);
if ( I.Flags & 2 /*CancelAsync*/ )
pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS,NULL);
else
pthread_setcanceltype(PTHREAD_CANCEL_DEFERRED,NULL);
}
else
{
pthread_setcancelstate(PTHREAD_CANCEL_DISABLE,NULL);
}
#endif
if ( I.Owner )
I.Owner->ThreadMain();
else
I.pFN();
return 0;
}
struct Instance
{
Instance( UThreadC<void> *const &O, const UThreadC<void>::Handler &pH = 0, const bool &CE=false, const bool &CA=false )
: pFN(pH), Owner(O), Flags(0) { if ( CE ) Flags|=1; if ( CA ) Flags|=2; }
UThreadC<void>::Handler pFN;
UThreadC<void> * Owner;
unsigned char Flags;
};
};
#endif // !_U_Thread_Posix_
rtabmap/utilite/include/rtabmap/utilite/UConversion.h
+290
View File
@@ -0,0 +1,290 @@
/*
* utilite is a cross-platform library with
* useful utilities for fast and small developing.
* Copyright (C) 2010 Mathieu Labbe
*
* utilite is free library: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* utilite is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef UCONVERSION_H
#define UCONVERSION_H
#include "rtabmap/utilite/utilite_export.h" // DLL export/import defines
#include <string>
#include <vector>
#include <stdarg.h>
/**
* \file UConversion.h
* \brief Some conversion functions
*
* This contains functions to do some convenient conversion like
* uNumber2str(), uBytes2Hex() or uHex2Bytes().
*/
/**
* Replace old characters in a string to new ones.
* Example :
* @code
* std::string str = "Hello";
* uReplaceChar(str, 'l', 'p');
* // The results is str = "Heppo";
* @endcode
*
* @param str the string
* @param before the character to be replaced by the new one
* @param after the new character replacing the old one
* @return the modified string
*/
std::string UTILITE_EXPORT uReplaceChar(const std::string & str, char before, char after);
/**
* Replace old characters in a string with the specified string.
* Example :
* @code
* std::string str = "Hello";
* uReplaceChar(str, 'o', "oween");
* // The results is str = "Helloween";
* @endcode
*
* @param str the string
* @param before the character to be replaced by the new one
* @param after the new string replacing the old character
* @return the modified string
*/
std::string UTILITE_EXPORT uReplaceChar(const std::string & str, char before, const std::string & after);
/**
* Transform characters from a string to upper case.
* Example :
* @code
* std::string str = "hello!";
* str = uToUpperCase(str);
* //str is now equal to "HELLO!"
* @endcode
* @param str the string
* @return the modified string
*/
std::string UTILITE_EXPORT uToUpperCase(const std::string & str);
/**
* Transform characters from a string to lower case.
* Example :
* @code
* std::string str = "HELLO!";
* str = uToLowerCase(str, false);
* //str is now equal to "hello!"
* @endcode
* @param str the string
* @return the modified string
*/
std::string UTILITE_EXPORT uToLowerCase(const std::string & str);
/**
* Convert a number (unsigned int) to a string.
* @param number the number to convert in a string
* @return the string
*/
std::string UTILITE_EXPORT uNumber2Str(unsigned int number);
/**
* Convert a number (int) to a string.
* @param number the number to convert in a string
* @return the string
*/
std::string UTILITE_EXPORT uNumber2Str(int number);
/**
* Convert a number (float) to a string.
* @param number the number to convert in a string
* @return the string
*/
std::string UTILITE_EXPORT uNumber2Str(float number, int precision=6, bool fixed = false);
/**
* Convert a number (double) to a string.
* @param number the number to convert in a string
* @return the string
*/
std::string UTILITE_EXPORT uNumber2Str(double number, int precision=6, bool fixed = false);
/**
* Convert a string to an integer.
* @param the string
* @return the number
*/
int UTILITE_EXPORT uStr2Int(const std::string & str);
/**
* Convert a string to a float independent of the locale (comma/dot).
* @param the string
* @return the number
*/
float UTILITE_EXPORT uStr2Float(const std::string & str);
/**
* Convert a string to a double independent of the locale (comma/dot).
* @param the string
* @return the number
*/
double UTILITE_EXPORT uStr2Double(const std::string & str);
/**
* Convert a bool to a string.
* The format used is "true" and "false".
* @param boolean the boolean to convert in a string
* @return the string
*/
std::string UTILITE_EXPORT uBool2Str(bool boolean);
/**
* Convert a string to a boolean.
* The format used is :
* "false", "FALSE" or "0" give false. All others give true.
* @param str the string to convert in a boolean
* @return the boolean
*/
bool UTILITE_EXPORT uStr2Bool(const char * str);
bool UTILITE_EXPORT uStr2Bool(const std::string & str);
/**
* Convert a string to an array of bytes including the null character ('\0').
* @param str the string
* @return the array of bytes
*/
std::vector<unsigned char> UTILITE_EXPORT uStr2Bytes(const std::string & str);
/**
* Convert an array of bytes to string, the array of bytes must end with the null character ('\0').
* @param bytes the array of bytes
* @return the string
*/
std::string UTILITE_EXPORT uBytes2Str(const std::vector<unsigned char> & bytes);
/**
* Convert a bytes array to an hexadecimal string.
* The resulting string is twice the size of the bytes array. The hexadecimal
* Characters are in upper case.
* Example :
* @code
* char bytes[] = {0x3F};
* std::string hex = uBytes2Hex(bytes, 1);
* // The string constains "3F".
* @endcode
*
* @param bytes the bytes array
* @param bytesLen the length of the bytes array
* @return the hexadecimal string
*/
std::string UTILITE_EXPORT uBytes2Hex(const char * bytes, unsigned int bytesLen);
/**
* Convert an hexadecimal string to a bytes array.
* The string must be pair length. The hexadecimal
* Characters can be in upper or lower case.
* Example :
* @code
* std::string hex = "1f3B";
* std::vector<char> bytes = uHex2Bytes(hex);
* // The array contains {0x1F, 0x3B}.
* @endcode
*
* @param hex the hexadecimal string
* @return the bytes array
*/
std::vector<char> UTILITE_EXPORT uHex2Bytes(const std::string & hex);
/**
* Convert an hexadecimal string to a bytes array.
* The string must be pair length. The hexadecimal
* Characters can be in upper or lower case.
* Example :
* @code
* std::vector<char> bytes = uHex2Bytes("1f3B", 4);
* // The array contains {0x1F, 0x3B}.
* @endcode
*
* @param hex the hexadecimal string
* @param bytesLen the hexadecimal string length
* @return the bytes array
*/
std::vector<char> UTILITE_EXPORT uHex2Bytes(const char * hex, int hexLen);
/**
* Convert an hexadecimal string to an ascii string. A convenient way
* when using only strings.
* The hexadecimal str MUST not contains any null values 0x00 ("00").
* Think to use of hex2bytes() to handle 0x00 values.
* Characters can be in upper or lower case.
* Example :
* @code
* std::string str = uHex2Str("48656C6C4F21");
* // The string contains "Hello!".
* @endcode
*
* @see hex2bytes
* @param hex the hexadecimal string
* @return the ascii string
*/
std::string UTILITE_EXPORT uHex2Str(const std::string & hex);
/**
* Convert hexadecimal (left or right part) value to an ascii character.
* Example :
* @code
* unsigned char F = uHex2Ascii(0xFA, false);
* unsigned char A = uHex2Ascii(0xFA, true);
* @endcode
* @see ascii2hex
* @param c the hexadecimal value
* @param rightPart If we want the character corresponding to the right of left part (4 bits) of the byte value.
* @return the ascii character (in upper case)
*/
unsigned char UTILITE_EXPORT uHex2Ascii(const unsigned char & c, bool rightPart);
/**
* Convert an ascii character to an hexadecimal value (right 4 bits).
* Characters can be in upper or lower case.
* Example :
* @code
* unsigned char hex = uAscii2Hex('F');
* // The results is hex = 0x0F;
* @endcode
* @see hex2ascii
* @param c the ascii character
* @return the hexadecimal value
*/
unsigned char UTILITE_EXPORT uAscii2Hex(const unsigned char & c);
/**
* Format a string like printf, and return it as a std::string
*/
std::string UTILITE_EXPORT uFormatv (const char *fmt, va_list ap);
/**
* Format a string like printf, and return it as a std::string
*/
std::string UTILITE_EXPORT uFormat (const char *fmt, ...);
#ifdef _WIN32
/**
* Convert multi-byte string to unicode (wide-char) string.
* Note that returned whar_t * must be deleted : delete [] wText;
*/
UTILITE_EXPORT wchar_t * createWCharFromChar(const char * text);
/**
* Convert unicode (wide-char) string to multi-byte string.
* Note that returned char * must be deleted : delete [] text;
*/
UTILITE_EXPORT char * createCharFromWChar(const wchar_t * wText);
#endif
#endif /* UCONVERSION_H */
rtabmap/utilite/include/rtabmap/utilite/UDestroyer.h
+75
View File
@@ -0,0 +1,75 @@
/*
* utilite is a cross-platform library with
* useful utilities for fast and small developing.
* Copyright (C) 2010 Mathieu Labbe
*
* utilite is free library: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* utilite is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef UDESTROYER_H
#define UDESTROYER_H
/**
* This class is used to delete a dynamically created
* objects. It was mainly designed to remove dynamically created Singleton.
* Created on the stack of a Singleton, when the
* application is finished, his destructor make sure that the
* Singleton is deleted.
*
*/
template <class T>
class UDestroyer
{
public:
/**
* The constructor. Set the doomed object (take ownership of the object). The object is deleted
* when this object is deleted.
*/
UDestroyer(T* doomed = 0) : doomed_(doomed) {}
~UDestroyer()
{
if(doomed_)
{
delete doomed_;
doomed_ = 0;
}
}
/**
* Set the doomed object. If a doomed object is already set, the function returns false.
* @param doomed the doomed object
* @return false if an object is already set and the new object is not null, otherwise true
*/
bool setDoomed(T* doomed)
{
if(doomed_ && doomed)
{
return false;
}
doomed_ = doomed;
return true;
}
private:
// Prevent users from making copies of a
// Destroyer to avoid double deletion:
UDestroyer(const UDestroyer<T>&);
void operator=(const UDestroyer<T>&);
private:
T* doomed_;
};
#endif // UDESTROYER_H
rtabmap/utilite/include/rtabmap/utilite/UDirectory.h
+143
View File
@@ -0,0 +1,143 @@
/*
* utilite is a cross-platform library with
* useful utilities for fast and small developing.
* Copyright (C) 2010 Mathieu Labbe
*
* utilite is free library: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* utilite is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef UDIRECTORY_H
#define UDIRECTORY_H
#include "rtabmap/utilite/utilite_export.h" // DLL export/import defines
#include <string>
#include <vector>
#include <list>
/**
* Class UDirectory.
*
* This class can be used to get file names in a directory.
*/
class UTILITE_EXPORT UDirectory
{
public:
/**
* Check if a directory exists.
* @param dirPath the directory path
* @return true if the directory exists
*/
static bool exists(const std::string & dirPath);
/**
* Get the directory path of a file path.
* @param filePath the file path
* @return the directory path of the file
*/
static std::string getDir(const std::string & filePath);
/**
* Get the current directory.
* @param trailingSeparator If true, a '/' is added to the path.
* @return the current directory
*/
static std::string currentDir(bool trailingSeparator = false);
/**
* Make a directory.
* @param dirPath the directory path
* @return true on success, false otherwise.
*/
static bool makeDir(const std::string & dirPath);
/**
* Remove a directory.
* @param dirPath the directory path
* @return true on success, false otherwise.
*/
static bool removeDir(const std::string & dirPath);
/**
* Return the "home" directory.
* @return the directory path.
*/
static std::string homeDir();
/**
* Return \ (Win32) or / (Unix) depending of the platform.
*/
static std::string separator();
public:
/**
* Create a UDirectory object with path initialized to an existing "path" and with filter "extensions".
* @param path the path to an existing directory
* @param extensions filter to get only file names with the extensions specified, format is a
* list of extensions separated by a space: "jpg bmp" get only file names finishing by jpg or bmp.
*/
UDirectory(const std::string & path = "", const std::string & extensions = "");
UDirectory(const UDirectory & dir);
UDirectory & operator=(const UDirectory & dir);
~UDirectory();
/**
* Set path of the directory.
* @param path the new directory path.
*/
void setPath(const std::string & path, const std::string & extensions = "");
/**
* Update indexed file names (if the directory changed).
*/
void update();
/**
* Check is the directory exists.
* @return if directory exists.
*/
bool isValid();
/**
* Get the next file name.
* @return the next file name
*/
std::string getNextFileName();
/**
* Get the next file path.
* @return the next file path
*/
std::string getNextFilePath();
/**
* Get all file names.
* @see UDirectory()
* @return all the file names in directory matching the set extensions.
*/
const std::list<std::string> & getFileNames() const {return fileNames_;}
/**
* Return the pointer of file names to beginning.
*/
void rewind();
private:
std::string path_;
std::vector<std::string> extensions_;
std::list<std::string> fileNames_;
std::list<std::string>::iterator iFileName_;
};
#endif /* UDIRECTORY_H */
rtabmap/utilite/include/rtabmap/utilite/UEvent.h
+87
View File
@@ -0,0 +1,87 @@
/*
* utilite is a cross-platform library with
* useful utilities for fast and small developing.
* Copyright (C) 2010 Mathieu Labbe
*
* utilite is free library: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* utilite is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef UEVENT_H
#define UEVENT_H
#include "rtabmap/utilite/utilite_export.h" // DLL export/import defines
#include <string>
class UEventsHandler;
/**
* This is the base class for all events used
* with the UEventsManager. Inherited classes
* must redefined the virtual method getClassName()
* to return their class name.
*
* Example:
* @code
* class MyEvent : public UEvent
* {
* public:
* MyEvent() {}
* virtual ~MyEvent() {}
* std::string getClassName() const {return "MyEvent";}
* };
*
* int main(int argc, char * argv[])
* {
* ...
* UEventsManager::post(new MyEvent()); // UEventsManager take ownership of the event (deleted by UEventsManager).
* ...
* }
* @endcode
*
* @see UEventsManager
* @see UEventsHandler
* @see getClassName()
*/
class UTILITE_EXPORT UEvent{
public:
virtual ~UEvent() {}
/**
* This method is used to get the class name
* of the event. For example, if a class MouseEvent
* inherits from UEvent, it must return the
* "MouseEvent" string.
* @return the class name
*/
virtual std::string getClassName() const = 0; // TODO : macro?
/**
* Get event's code.
* @return the code
*/
int getCode() const {return code_;}
protected:
/**
* @param code the event code.
* TODO : Remove the code, not required for most of all implemented events
*/
UEvent(int code = 0) : code_(code) {}
private:
int code_; /**< The event's code. */
};
#endif // UEVENT_H
rtabmap/utilite/include/rtabmap/utilite/UEventsHandler.h
+188
View File
@@ -0,0 +1,188 @@
/*
* utilite is a cross-platform library with
* useful utilities for fast and small developing.
* Copyright (C) 2010 Mathieu Labbe
*
* utilite is free library: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* utilite is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef UEVENTSHANDLER_H
#define UEVENTSHANDLER_H
#include "rtabmap/utilite/utilite_export.h" // DLL export/import defines
#include "rtabmap/utilite/UEventsSender.h"
class UEvent;
/**
* The class UEventsHandler is an abstract class for
* handling events.
*
* Inherited classes must implement handleEvent() function, which
* is called by the UEventsManager when an event is dispatched. Once the handler is
* created, it must be added to events manager with UEventsManager::addHandler() function.
* Note that it is not safe to automatically add the handler to UEventsManager in the handler's constructor.
*
* Note for multi-threading: the handleEvent() method is called
* inside the UEventsManager thread. If the inherited class also inherits
* from UThreadNode, handleEvent() is done as well outside the thread's main loop, so
* be careful to protect private data of the thread used in its main loop.
*
* Example for a useful combination of an UEventsHandler and a UThreadNode, with safe data
* modification while not blocking the handleEvent() call on a mutex:
* @code
* #include "utilite/UThreadNode.h"
* #include "utilite/UEventsHandler.h"
* #include "utilite/UEventsManager.h"
* #include "utilite/UEvent.h"
*
* // Implement a simple event
* class ResetEvent : public UEvent {
* public:
* ResetEvent() {}
* virtual ~ResetEvent() {}
* virtual std::string getClassName() const {return "ResetEvent";} // Must be implemented
* };
*
* // There is the thread counting indefinitely, the count can be reseted by sending a ResetEvent.
* class CounterThread : public UThreadNode, public UEventsHandler {
* public:
* CounterThread() : state_(0), count_(0) {}
* virtual ~CounterThread() {this->join(true);}
*
* protected:
* virtual void mainLoop() {
* if(state_ == 1) {
* state_ = 0;
* // Do a long initialization, reset memory or other special long works... here
* // we reset the count. This could be done in the handleEvent() but
* // with many objects, it is more safe to do it here (in the thread's loop). A safe
* // way could be also to use a UMutex to protect this initialization in
* // the handleEvent(), but it is not recommended to do long works in handleEvent()
* // because this will add latency in the UEventsManager dispatching events loop.
* count_ = 0; // Reset the count
* printf("Reset!\n");
* }
*
* // Do some works...
* printf("count=%d\n", count_++);
* uSleep(100); // wait 100 ms
* }
* virtual void handleEvent(UEvent * event) {
* if(event->getClassName().compare("ResetEvent") == 0) {
* state_ = 1;
* }
* }
* private:
* int state_;
* int count_;
* };
*
* int main(int argc, char * argv[])
* {
* CounterThread counter;
* counter.start();
* UEventsManager::addHandler(&counter);
*
* uSleep(500); // wait 500 ms before sending a reset event
* UEventsManager::post(new ResetEvent());
* uSleep(500); // wait 500 ms before termination
*
* UEventsManager::removeHandler(&counter);
* counter.join(true); // Kill and wait to finish
* return 0;
* }
* @endcode
*
* The output is:
* @code
* count=0
* count=1
* count=2
* count=3
* count=4
* Reset!
* count=0
* count=1
* count=2
* count=3
* count=4
* @endcode
*
* @see UEventsManager
* @see UEvent
* @see UThreadNode
*
*/
class UTILITE_EXPORT UEventsHandler : public UEventsSender {
public:
void registerToEventsManager();
void unregisterFromEventsManager();
protected:
/**
* Only the UEventsManager has access
* to the handleEvent() method.
*/
friend class UEventsManager;
/**
* Method called by the UEventsManager
* to handle an event. Important : this method
* must do a minimum of work because the faster
* the dispatching loop is done; the faster the
* events are received. If a handling function
* takes too much time, the events list can grow
* faster than it is emptied. The event can be
* modified.
* @return "true" to notify UEventsManager that this handler took ownership of the
* event (meaning it must delete it). The event will
* not be dispatched to next handlers.
* @return "false" to let event be dispatched to next handlers (default behavior). UEventsManager
* will take care of deleting the event.
*
*/
virtual bool handleEvent(UEvent * event) = 0;
protected:
/**
* UEventsHandler constructor.
*
* Note : You can call EventsManager::addHandler(this) at
* the end of the constructor of the inherited class where the virtual
* method handleEvent(...) is defined. If so, the UEventsHandler doesn't
* need to be manually added to the EventsManager where the handler
* is instantiated. We decided to not include UEventsManager::addHandler(this)
* in this abstract class constructor because an event can be handled (calling
* the pure virtual method) while the concrete class is constructed.
*/
UEventsHandler() {}
/**
* UEventsHandler destructor.
*
* By default, it removes the handler reference from the UEventsManager. To be thread-safe,
* the inherited class must remove itself from the UEventsManager before it is deleted because
* an event can be handled (calling the pure virtual method handleEvent()) after the concrete class
* is deleted.
*/
virtual ~UEventsHandler();
private:
};
#endif // UEVENTSHANDLER_H
rtabmap/utilite/include/rtabmap/utilite/UEventsManager.h
+253
View File
@@ -0,0 +1,253 @@
/*
* utilite is a cross-platform library with
* useful utilities for fast and small developing.
* Copyright (C) 2010 Mathieu Labbe
*
* utilite is free library: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* utilite is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef UEVENTSMANAGER_H
#define UEVENTSMANAGER_H
#include "rtabmap/utilite/utilite_export.h" // DLL export/import defines
#include "rtabmap/utilite/UEventsHandler.h"
#include "rtabmap/utilite/UThreadNode.h"
#include "rtabmap/utilite/ULogger.h"
#include "rtabmap/utilite/UDestroyer.h"
#include <list>
#include <map>
// TODO Not implemented... for multithreading event handling
class UEventDispatcher : public UThread
{
public:
virtual ~UEventDispatcher();
protected:
friend class UEventsManager;
UEventDispatcher();
virtual void mainLoop();
private:
virtual void killCleanup();
private:
UEvent * _event;
std::vector<UEventsHandler*> _handlers;
};
/**
* This class is used to post events between threads
* in the application. It is Thread-Safe and the events are sent
* to receivers in the same order they are posted (FIFO). It works
* like the design pattern Mediator. It is also a Singleton, so
* it can be used anywhere in the application.
*
* To send an event, use UEventsManager::post().
* Events are automatically deleted after they are posted.
*
* The EventsManager have a list of handlers to which
* it sends posted events. To add an handler, use
* UEventsManager::addHandler(). To remove, use
* UEventsManager::removeHandler().
*
* @code
* // Anywhere in the code:
* UEventsManager::post(new MyEvent()); // where MyEvent is an implemented UEvent
* @endcode
*
* @see UEvent
* @see UEventsHandler
* @see post()
* @see addHandler()
* @see removeHandler()
*/
class UTILITE_EXPORT UEventsManager : public UThread{
public:
/**
* This method is used to add an events
* handler to the list of handlers.
*
* @param handler the handler to be added.
*/
static void addHandler(UEventsHandler* handler);
/**
* This method is used to remove an events
* handler from the list of handlers.
*
* @param handler the handler to be removed.
*/
static void removeHandler(UEventsHandler* handler);
/**
* This method is used to post an event to
* handlers.
*
* Event can be posted asynchronously or not. In the first case,
* the event is dispatched by the UEventsManager's thread. In the
* second case, the event is handled immediately by event's
* receivers, thus in the sender thread.
*
* @param event the event to be posted.
* @param async if true, the event is dispatched by the UEventsManager thread, otherwise it's in the caller thread (synchronous).
*/
static void post(UEvent * event, bool async = true, const UEventsSender * sender = 0);
static void createPipe(
const UEventsSender * sender,
const UEventsHandler * receiver,
const std::string & eventName);
static void removePipe(
const UEventsSender * sender,
const UEventsHandler * receiver,
const std::string & eventName);
static void removeAllPipes(const UEventsSender * sender);
static void removeNullPipes(const UEventsSender * sender);
protected:
/*
* This method is used to have a reference on the
* EventsManager. When no EventsManager exists, one is
* created. There is only one instance in the application.
* See the Singleton pattern further explanation.
*
* @return the reference on the EventsManager
*/
static UEventsManager* getInstance();
/*
* Called only once in getInstance(). It can't be instantiated
* by the user.
*
*/
UEventsManager();
/*
* Only called by a Destroyer.
*/
virtual ~UEventsManager();
/*
* A Destroyer is used to remove a dynamically created
* Singleton. It is friend here to have access to the
* destructor.
*
*/
friend class UDestroyer<UEventsManager>;
/**
* The UEventsManager's main loop.
*/
virtual void mainLoop();
private:
/**
* Reimplemented to wake up UEventsManager on termination.
*/
virtual void mainLoopKill();
/*
* This method dispatches asynchronized events to all handlers.
* FIFO (first in first out) dispatching is used.
*/
virtual void dispatchEvents();
/*
* This method dispatches an event to all handlers.
*/
virtual bool dispatchEvent(UEvent * event, const UEventsSender * sender);
/*
* This method is used to add an events
* handler to the list of handlers.
*
* @param handler the handler to be added.
*/
void _addHandler(UEventsHandler* handler);
/*
* This method is used to remove an events
* handler from the list of handlers.
*
* @param handler the handler to be removed.
*/
void _removeHandler(UEventsHandler* handler);
/*
* This method is used to post an event to
* handlers.
*
* Event can be posted asynchronously or not. In the first case,
* the event is dispatched by the UEventsManager's thread. In the
* second case, the event is handled immediately by event's
* receivers, thus in the sender thread.
*
* @param event the event to be posted.
* @param async if true, the event is dispatched by the UEventsManager thread, otherwise it's in the caller thread (synchronous).
*/
void _postEvent(UEvent * event, bool async = true, const UEventsSender * sender = 0);
std::list<UEventsHandler*> getPipes(
const UEventsSender * sender,
const std::string & eventName);
void _createPipe(
const UEventsSender * sender,
const UEventsHandler * receiver,
const std::string & eventName);
void _removePipe(
const UEventsSender * sender,
const UEventsHandler * receiver,
const std::string & eventName);
void _removeAllPipes(const UEventsSender * sender);
void _removeNullPipes(const UEventsSender * sender);
private:
class Pipe
{
public:
Pipe(const UEventsSender * sender, const UEventsHandler * receiver, const std::string & eventName) :
sender_(sender),
receiver_(receiver),
eventName_(eventName)
{}
const UEventsSender * sender_;
const UEventsHandler * receiver_;
const std::string eventName_;
};
static UEventsManager* instance_; /* The EventsManager instance pointer. */
static UDestroyer<UEventsManager> destroyer_; /* The EventsManager's destroyer. */
std::list<std::pair<UEvent*, const UEventsSender * > > events_; /* The events list. */
std::list<UEventsHandler*> handlers_; /* The handlers list. */
UMutex eventsMutex_; /* The mutex of the events list, */
UMutex handlersMutex_; /* The mutex of the handlers list. */
USemaphore postEventSem_; /* Semaphore used to signal when an events is posted. */
std::list<Pipe> pipes_;
UMutex pipesMutex_;
};
#endif // UEVENTSMANAGER_H
rtabmap/utilite/include/rtabmap/utilite/UEventsSender.h
+31
View File
@@ -0,0 +1,31 @@
/*
* UEventsSender.h
*
* Created on: 2013-10-14
* Author: Mathieu
*/
#ifndef UEVENTSSENDER_H_
#define UEVENTSSENDER_H_
#include "rtabmap/utilite/utilite_export.h" // DLL export/import defines
class UEvent;
class UTILITE_EXPORT UEventsSender
{
public:
UEventsSender(){}
virtual ~UEventsSender();
protected:
/**
* For convenience to post an event. This is the same than calling UEventsManager::post()
* with the sender reference.
*/
void post(UEvent * event, bool async = true) const;
};
#endif /* UEVENTSSENDER_H_ */
rtabmap/utilite/include/rtabmap/utilite/UException.h
+33
View File
@@ -0,0 +1,33 @@
/*
* utilite is a cross-platform library with
* useful utilities for fast and small developing.
* Copyright (C) 2010 Mathieu Labbe
*
* utilite is free library: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* utilite is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef UEXCEPTION_H_
#define UEXCEPTION_H_
#include <stdexcept>
class UException: public std::runtime_error {
public:
UException(const std::string & what) :
std::runtime_error(what)
{}
};
#endif /* UEXCEPTION_H_ */
rtabmap/utilite/include/rtabmap/utilite/UFile.h
+152
View File
@@ -0,0 +1,152 @@
/*
* utilite is a cross-platform library with
* useful utilities for fast and small developing.
* Copyright (C) 2010 Mathieu Labbe
*
* utilite is free library: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* utilite is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef FILE_H
#define FILE_H
#include "rtabmap/utilite/utilite_export.h" // DLL export/import defines
#include "rtabmap/utilite/UDirectory.h"
#include <string>
/**
* Class UFile.
*
* This class can be used to modify/erase files on hard drive.
*/
class UTILITE_EXPORT UFile
{
public:
/**
* Check if a file exists.
* @param filePath the file path
* @return true if the file exists, otherwise false.
*/
static bool exists(const std::string &filePath);
/**
* Get the file length.
* @param filePath the file path
* @return long the length of the file in bytes. Return -1 if the file doesn't exist.
*/
static long length(const std::string &filePath);
/**
* Erase a file.
* @param filePath the file path
* @return 0 if success.
*/
static int erase(const std::string &filePath);
/**
* Rename a file.
* @param oldFilePath the old file path
* @param newFilePath the new file path
* @return 0 if success.
*/
static int rename(const std::string &oldFilePath,
const std::string &newFilePath);
/**
* Get the file name from a file path (with extension).
* @param filePath the file path
* @return the file name.
*/
static std::string getName(const std::string & filePath);
/**
* Get the file extension.
* @return the file extension
*/
static std::string getExtension(const std::string &filePath);
/**
* Copy a file.
* @param from the file path
* @param to destination file path
*/
static void copy(const std::string & from, const std::string & to);
public:
/**
* Create a UFile object with path initialized to an existing file .
* @param path the path to an existing file
*/
UFile(const std::string & path) : path_(path) {}
~UFile() {}
/**
* Check if the file exists. Same as exists().
* @return true if the path exits
*/
bool isValid() {return exists(path_);}
/**
* Check if the file exists.
* @return true if the path exits
*/
bool exists() {return exists(path_);}
/**
* Get the length of the file.
* @return long the length of the file in bytes. Return -1 if the file doesn't exist.
*/
long length() {return length(path_);}
/**
* Rename the file name. The path stays the same.
* @param the new name
*/
int rename(const std::string &newName)
{
std::string ext = this->getExtension();
std::string newPath = UDirectory::getDir(path_) + std::string("/") + newName;
if(ext.size())
{
newPath += std::string(".") + getExtension(path_);
}
int result = rename(path_, newPath);
if(result == 0)
{
path_ = newPath;
}
return result;
}
/**
* Get the file name without the path.
* @return the file name
*/
std::string getName() {return getName(path_);}
/**
* Get the file extension.
* @return the file extension
*/
std::string getExtension() {return getExtension(path_);}
/**
* Copy a file.
* @param to destination file path
*/
void copy(const std::string & to) {copy(path_, to);}
private:
std::string path_;
};
#endif
rtabmap/utilite/include/rtabmap/utilite/ULogger.h
+575
View File
@@ -0,0 +1,575 @@
/*
* utilite is a cross-platform library with
* useful utilities for fast and small developing.
* Copyright (C) 2010 Mathieu Labbe
*
* utilite is free library: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* utilite is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef ULOGGER_H
#define ULOGGER_H
#include "rtabmap/utilite/utilite_export.h" // DLL export/import defines
#include "rtabmap/utilite/UMutex.h"
#include "rtabmap/utilite/UDestroyer.h"
#include "rtabmap/utilite/UEvent.h"
#include "rtabmap/utilite/UException.h"
#include <stdio.h>
#include <time.h>
#include <string>
#include <vector>
#include <map>
#include <set>
#include <stdarg.h>
/**
* \file ULogger.h
* \brief ULogger class and convenient macros
*
* This contains macros useful for logging a message anywhere in the
* application. Once the ULogger is set, use these macros like a printf to
* print debug messages.
*/
/*
* Convenient macros for logging...
*/
#define ULOGGER_LOG(level, ...) ULogger::write(level, __FILE__, __LINE__, __FUNCTION__, __VA_ARGS__)
#define ULOGGER_DEBUG(...) ULOGGER_LOG(ULogger::kDebug, __VA_ARGS__)
#define ULOGGER_INFO(...) ULOGGER_LOG(ULogger::kInfo, __VA_ARGS__)
#define ULOGGER_WARN(...) ULOGGER_LOG(ULogger::kWarning, __VA_ARGS__)
#define ULOGGER_ERROR(...) ULOGGER_LOG(ULogger::kError, __VA_ARGS__)
#define ULOGGER_FATAL(...) ULOGGER_LOG(ULogger::kFatal, __VA_ARGS__) // Throw UException
#define UDEBUG(...) ULOGGER_DEBUG(__VA_ARGS__)
#define UINFO(...) ULOGGER_INFO(__VA_ARGS__)
#define UWARN(...) ULOGGER_WARN(__VA_ARGS__)
#define UERROR(...) ULOGGER_ERROR(__VA_ARGS__)
#define UFATAL(...) ULOGGER_FATAL(__VA_ARGS__) // Throw UException
// Throw UException
#define UASSERT(condition) if(!(condition)) ULogger::write(ULogger::kFatal, __FILE__, __LINE__, __FUNCTION__, "Condition (%s) not met!", #condition)
#define UASSERT_MSG(condition, msg_str) if(!(condition)) ULogger::write(ULogger::kFatal, __FILE__, __LINE__, __FUNCTION__, "Condition (%s) not met! [%s]", #condition, msg_str)
/**
* \def UDEBUG(...)
* Print a debug level message in the logger. Format is the same as a printf:
* @code
* UDEBUG("This is a debug message with the number %d", 42);
* @endcode
*/
/**
* \def UINFO(...)
* Print a information level message in the logger. Format is the same as a printf:
* @code
* UINFO("This is a information message with the number %d", 42);
* @endcode
*/
/**
* \def UWARN(...)
* Print a warning level message in the logger. Format is the same as a printf:
* @code
* UWARN("This is a warning message with the number %d", 42);
* @endcode
*/
/**
* \def UERROR(...)
* Print an error level message in the logger. Format is the same as a printf:
* @code
* UERROR("This is an error message with the number %d", 42);
* @endcode
*/
/**
* \def UFATAL(...)
* Print a fatal error level message in the logger. The application will exit on
* fatal error. Format is the same as a printf:
* @code
* UFATAL("This is a fatal error message with the number %d", 42);
* @endcode
*/
/**
* \def UASSERT(condition, ...)
* Print a fatal error level message in the logger if condition is not met. The application will exit on
* fatal error. Format is the same as a printf:
* @code
* UASSERT(a!=42, "This is a fatal error message with the number %d", 42);
* @endcode
*/
/**
* This class is used by the ULogger to send logged messages like events. Messages with level
* over the event level set in ULogger::setEventLevel() are sent like ULogEvent with the message and its level.
* The default event level of ULogger is kFatal (see ULogger::Level).
*/
class ULogEvent : public UEvent
{
public:
/**
* ULogEvent constructor. Note that to retrieve the message level, use UEvent::getCode().
* @param msg the message already formatted to a full string.
* @param level the severity of the message, @see ULogger::Level.
*/
ULogEvent(const std::string & msg, int level) :
UEvent(level),
msg_(msg)
{}
virtual ~ULogEvent() {}
/**
* Get the message from the event.
*/
const std::string & getMsg() const {return msg_;}
/**
* @return string "ULogEvent"
*/
virtual std::string getClassName() const {return "ULogEvent";}
private:
std::string msg_;
};
/**
* This class is used to log messages with time on a console, in a file
* and/or with an event. At the start of the application, call
* ULogger::setType() with the type of the logger you want (see ULogger::Type, the type of the output
* can be changed at the run-time.). To use it,
* simply call the convenient macros UDEBUG(), UINFO(), UWARN(), UERROR(), UFATAL() depending of
* the severity of the message. You can disable some messages by setting the logger
* level ULogger::setLevel() to severity you want, defined by ULogger::Level. A fatal message
* will make the application to exit, printing the message on console (whatever the logger type) and
* posting a ULogEvent (synchronously... see UEventsManager::post()) before exiting.
*
* The display of the logged messages can be modified:
* - If you don't want the level label, set ULogger::setPrintLevel() to false.
* - If you don't want the time label, set ULogger::setPrintTime() to false.
* - If you don't want the end of line added, set ULogger::setPrintEndline() to false.
* - If you don't the full path of the message, set ULogger::setPrintWhereFullPath() to false.
* - If you don't the path of the message, set ULogger::setPrintWhere() to false.
*
* When using a file logger (kTypeLogger), it can be useful in some
* application to buffer messages before writing them to hard drive (avoiding
* hard drive latencies). You can set ULogger::setBuffered() to true to do that. When the
* buffered messages will be written to file on appllciation exit (ULogger destructor) or when
* ULogger::flush() is called.
*
* If you want the application to exit on a lower severity level than kFatal,
* you can set ULogger::setExitLevel() to any ULogger::Type you want.
*
* Example:
* @code
* #include <utilite/ULogger.h>
* int main(int argc, char * argv[])
* {
* // Set the logger type. The choices are kTypeConsole,
* // kTypeFile or kTypeNoLog (nothing is logged).
* ULogger::setType(ULogger::kTypeConsole);
*
* // Set the logger severity level (kDebug, kInfo, kWarning, kError, kFatal).
* // All log entries under the severity level are not logged. Here,
* // only debug messages are not logged.
* ULogger::setLevel(ULogger::kInfo);
*
* // Use a predefined Macro to easy logging. It can be
* // called anywhere in the application as the logger is
* // a Singleton.
* UDEBUG("This message won't be logged because the "
* "severity level of the logger is set to kInfo.");
*
* UINFO("This message is logged.");
*
* UWARN("A warning message...");
*
* UERROR("An error message with code %d.", 42);
*
* return 0;
* }
* @endcode
* Output:
* @code
* [ INFO] (2010-09-25 18:08:20) main.cpp:18::main() This message is logged.
* [ WARN] (2010-09-25 18:08:20) main.cpp:20::main() A warning message...
* [ERROR] (2010-09-25 18:08:20) main.cpp:22::main() An error message with code 42.
* @endcode
*
* Another useful form of the ULogger is to use it with the UTimer class. Here an example:
* @code
* #include <utilite/ULogger.h>
* #include <utilite/UTimer.h>
* ...
* UTimer timer; // automatically starts
* // do some works for part A
* UINFO("Time for part A = %f s", timer.ticks());
* // do some works for part B
* UINFO("Time for part B = %f s", timer.ticks());
* // do some works for part C
* UINFO("Time for part C = %f s", timer.ticks());
* ...
* @endcode
*
* @see setType()
* @see setLevel()
* @see UDEBUG(), UINFO(), UWARN(), UERROR(), UFATAL()
*
*/
class UTILITE_EXPORT ULogger
{
public:
/**
* The default log file name.
*/
static const std::string kDefaultLogFileName;
/**
* Loggers available:
* @code
* kTypeNoLog, kTypeConsole, kTypeFile
* @endcode
*/
enum Type{kTypeNoLog, kTypeConsole, kTypeFile};
/**
* Logger levels, from lowest severity to highest:
* @code
* kDebug, kInfo, kWarning, kError, kFatal
* @endcode
*/
enum Level{kDebug, kInfo, kWarning, kError, kFatal};
/**
* Set the type of the logger. When using kTypeFile, the parameter "fileName" would be
* changed (default is "./ULog.txt"), and optionally "append" if we want the
* logger to append messages to file or to overwrite the file.
* @param type the ULogger::Type of the logger.
* @param fileName file name used with a file logger type.
* @param append if true, the file isn't overwritten, otherwise it is.
*
* TODO : Can it be useful to have 2 or more types at the same time ? Print
* in console and file at the same time.
*/
static void setType(Type type, const std::string &fileName = kDefaultLogFileName, bool append = true);
static Type type() {return type_;}
// Setters
/**
* Print time: default true.
* @param printTime true to print time, otherwise set to false.
*/
static void setPrintTime(bool printTime) {printTime_ = printTime;}
static bool isPrintTime() {return printTime_;}
/**
* Print level: default true.
* @param printLevel true to print level, otherwise set to false.
*/
static void setPrintLevel(bool printLevel) {printLevel_ = printLevel;}
static bool isPrintLevel() {return printLevel_;}
/**
* Print end of line: default true.
* @param printLevel true to print end of line, otherwise set to false.
*/
static void setPrintEndline(bool printEndline) {printEndline_ = printEndline;}
static bool isPrintEndLine() {return printEndline_;}
/**
* Print text with color: default true.
* Dark green for Debug, white for Info, yellow for Warning, red for Error and Fatal.
* @param printColored true to print text with color, otherwise set to false.
*/
static void setPrintColored(bool printColored) {printColored_ = printColored;}
static bool isPrintColored() {return printColored_;}
/**
* Print where is this message in source code: default true.
* @param printWhere true to print where, otherwise set to false.
*/
static void setPrintWhere(bool printWhere) {printWhere_ = printWhere;}
static bool isPrintWhere() {return printWhere_;}
/**
* Print thread ID: default false.
* @param printThreadId true to print where, otherwise set to false.
*/
static void setPrintThreadId(bool printThreadId) {printThreadID_ = printThreadId;}
static bool isPrintThreadId() {return printThreadID_;}
/**
* Print the full path: default true. ULogger::setPrintWhere() must be true to have path printed.
* @param printWhereFullPath true to print the full path, otherwise set to false.
*/
static void setPrintWhereFullPath(bool printWhereFullPath) {printWhereFullPath_ = printWhereFullPath;}
static bool isPrintWhereFullPath() {return printWhereFullPath_;}
/**
* Set is the logger buffers messages, default false. When true, the messages are
* buffered until the application is closed or ULogger::flush() is called.
* @see ULogger::flush()
* @param buffered true to buffer messages, otherwise set to false.
*/
static void setBuffered(bool buffered);
static bool isBuffered() {return buffered_;}
/**
* Set logger level: default kInfo. All messages over the severity set
* are printed, other are ignored. The severity is from the lowest to
* highest:
* - kDebug
* - kInfo
* - kWarning
* - kError
* - kFatal
* @param level the minimum level of the messages printed.
*/
static void setLevel(ULogger::Level level) {level_ = level;}
static ULogger::Level level() {return level_;}
/**
* An ULogEvent is sent on each message logged at the specified level.
* Note : On message with level >= exitLevel, the event is sent synchronously (see UEventsManager::post()).
* @see ULogEvent
* @see setExitLevel()
*/
static void setEventLevel(ULogger::Level eventSentLevel) {eventLevel_ = eventSentLevel;}
static ULogger::Level eventLevel() {return eventLevel_;}
/**
* If not empty, only show log messages from threads included in this list.
*/
static void setTreadIdFilter(const std::set<unsigned long> & ids) {threadIdFilter_ = ids;}
static void setTreadIdFilter(const std::vector<std::string> & ids);
static const std::set<unsigned long> & getTreadIdFilter() {return threadIdFilter_;}
/**
* Threads can register to this list. If name is empty, it will
* clear the thread in the list. Should be called from the thread itself.
*/
static void registerCurrentThread(const std::string & name);
static void unregisterCurrentThread();
static std::map<std::string, unsigned long> getRegisteredThreads();
/**
* Reset to default parameters.
*/
static void reset();
/**
* Flush buffered messages.
* @see setBuffered()
*/
static void flush();
/**
* Write a message directly to logger without level handling.
* @param msg the message to write.
* @param ... the variable arguments
* @deprecated use UDEBUG(), UINFO(), UWARN(), UERROR() or UFATAL()
*/
static void write(const char* msg, ...);
/*
* Write a message to logger: use UDEBUG(), UINFO(), UWARN(), UERROR() or UFATAL() instead.
* @param level the log level of this message
* @param file the file path
* @param line the line in the file
* @param function the function name in which the message is logged
* @param msg the message to write
* @param ... the variable arguments
*/
static void write(ULogger::Level level,
const char * file,
int line,
const char *function,
const char* msg,
...);
/**
* Get the time in the format "2008-7-13 12:23:44".
* @param timeStr string were the time will be copied.
* @return the number of characters written, or 0 if an error occurred.
*/
static int getTime(std::string &timeStr);
protected:
/*
* This method is used to have a reference on the
* Logger. When no Logger exists, one is
* created. There is only one instance in the application.
* Must be protected by loggerMutex_.
* See the Singleton pattern for further explanation.
*
* @return the reference on the Logger
*/
static ULogger* getInstance();
/*
* Called only once in getInstance(). It can't be instanciated
* by the user.
*
* @see getInstance()
*/
ULogger() {}
/*
* Only called by a Destroyer.
* @see Destroyer
*/
virtual ~ULogger();
/*
* Flush buffered messages
*/
void _flush();
/*
* A Destroyer is used to remove a dynamicaly created
* Singleton. It is friend here to have access to the
* destructor.
*
* @see Destroyer
*/
friend class UDestroyer<ULogger>;
/*
* The log file name.
*/
static std::string logFileName_;
/*
* Default true, it doesn't overwrite the file.
*/
static bool append_;
private:
/*
* Create an instance according to type. See the Abstract factory
* pattern for further explanation.
* @see type_
* @return the reference on the new logger
*/
static ULogger* createInstance();
/*
* Write a message on the output with the format :
* "A message". Inherited class
* must override this method to output the message. It
* does nothing by default.
* @param msg the message to write.
* @param arg the variable arguments
*/
virtual void _write(const char*, va_list) {} // Do nothing by default
virtual void _writeStr(const char*) {} // Do nothing by default
private:
/*
* The Logger instance pointer.
*/
static ULogger* instance_;
/*
* The Logger's destroyer
*/
static UDestroyer<ULogger> destroyer_;
/*
* If the logger prints the time for each message.
* Default is true.
*/
static bool printTime_;
/*
* If the logger prints the level for each message.
* Default is true.
*/
static bool printLevel_;
/*
* If the logger prints the end line for each message.
* Default is true.
*/
static bool printEndline_;
/*
* If the logger prints text with color.
* Default is true.
*/
static bool printColored_;
/*
* If the logger prints where the message is logged (fileName::function():line).
* Default is true.
*/
static bool printWhere_;
/*
* If the logger prints the full path of the source file
* where the message is written. Only works when
* "printWhere_" is true.
* Default is false.
*/
static bool printWhereFullPath_;
/*
* If the logger prints the thread ID.
* Default is false.
*/
static bool printThreadID_;
/*
* If the logger limit the size of the "where" path to
* characters. If the path is over 8 characters, a "~"
* is added. Only works when "printWhereFullPath_" is false.
* Default is false.
*/
static bool limitWhereLength_;
/*
* The type of the logger.
*/
static Type type_;
/*
* The severity of the log.
*/
static Level level_;
/*
* The severity at which the message is also sent in a ULogEvent.
*/
static Level eventLevel_;
static const char * levelName_[5];
/*
* Mutex used when accessing public functions.
*/
static UMutex loggerMutex_;
/*
* If the logger prints messages only when ULogger::flush() is called.
* Default is false.
*/
static bool buffered_;
static std::string bufferedMsgs_;
static std::set<unsigned long> threadIdFilter_;
static std::map<std::string, unsigned long> registeredThreads_;
};
#endif // ULOGGER_H
rtabmap/utilite/include/rtabmap/utilite/UMath.h
+937
View File
@@ -0,0 +1,937 @@
/*
* utilite is a cross-platform library with
* useful utilities for fast and small developing.
* Copyright (C) 2010 Mathieu Labbe
*
* utilite is free library: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* utilite is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef UMATH_H
#define UMATH_H
/** \file UMath.h
\brief Basic mathematics functions
*/
#include <cmath>
#include <list>
#include <vector>
#if _MSC_VER
#undef min
#undef max
#endif
/**
* Return true if the number is NAN.
*/
template<class T>
inline bool uIsNan(const T & value)
{
#if _MSC_VER
return _isnan(value) != 0;
#else
return std::isnan(value);
#endif
}
/**
* Return true if the number is finite.
*/
template<class T>
inline bool uIsFinite(const T & value)
{
#if _MSC_VER
return _finite(value) != 0;
#else
return std::isfinite(value);
#endif
}
/**
* Get the minimum of the 3 values.
* @return the minimum value
*/
template<class T>
inline T uMin3( const T& a, const T& b, const T& c)
{
float m=a<b?a:b;
return m<c?m:c;
}
/**
* Get the maximum of the 3 values.
* @return the maximum value
*/
template<class T>
inline T uMax3( const T& a, const T& b, const T& c)
{
float m=a>b?a:b;
return m>c?m:c;
}
/**
* Get the maximum of a vector.
* @param v the array
* @param size the size of the array
* @param index the index of the maximum value in the vector.
* @return the maximum value of the array
*/
template<class T>
inline T uMax(const T * v, unsigned int size, unsigned int & index)
{
T max = 0;
index = 0;
if(!v || size == 0)
{
return max;
}
max = v[0];
for(unsigned int i=1; i<size; ++i)
{
if(uIsNan(max) || (max < v[i] && !uIsNan(v[i])))
{
max = v[i];
index = i;
}
}
return max;
}
/**
* Get the maximum of a vector.
* @param v the array
* @param index the index of the maximum value in the vector.
* @return the maximum value of the array
*/
template<class T>
inline T uMax(const std::vector<T> & v, unsigned int & index)
{
return uMax(v.data(), v->size(), index);
}
/**
* Get the maximum of a vector.
* @param v the array
* @param size the size of the array
* @return the maximum value of the array
*/
template<class T>
inline T uMax(const T * v, unsigned int size)
{
unsigned int index;
return uMax(v, size, index);
}
/**
* Get the maximum of a vector.
* @param v the array
* @return the maximum value of the array
*/
template<class T>
inline T uMax(const std::vector<T> & v)
{
return uMax(v.data(), v.size());
}
/**
* Get the minimum of a vector.
* @param v the array
* @param size the size of the array
* @param index the index of the minimum value in the vector.
* @return the minimum value of the array
*/
template<class T>
inline T uMin(const T * v, unsigned int size, unsigned int & index)
{
T min = 0;
index = 0;
if(!v || size == 0)
{
return min;
}
min = v[0];
for(unsigned int i=1; i<size; ++i)
{
if(uIsNan(min) || (min > v[i] && !uIsNan(v[i])))
{
min = v[i];
index = i;
}
}
return min;
}
/**
* Get the minimum of a vector.
* @param v the array
* @param index the index of the minimum value in the vector.
* @return the minimum value of the array
*/
template<class T>
inline T uMin(const std::vector<T> & v, unsigned int & index)
{
return uMin(v.data(), v.size(), index);
}
/**
* Get the minimum of a vector.
* @param v the array
* @param size the size of the array
* @return the minimum value of the array
*/
template<class T>
inline T uMin(const T * v, unsigned int size)
{
unsigned int index;
return uMin(v, size, index);
}
/**
* Get the minimum of a vector.
* @param v the array
* @return the minimum value of the array
*/
template<class T>
inline T uMin(const std::vector<T> & v)
{
return uMin(v.data(), v.size());
}
/**
* Get the minimum and maximum of a vector.
* @param v the array
* @param size the size of the array
* @param min reference to output minimum
* @param max reference to output maximum
* @param min reference to output minimum index
* @param max reference to output maximum index
*/
template<class T>
inline void uMinMax(const T * v, unsigned int size, T & min, T & max, unsigned int & indexMin, unsigned int & indexMax)
{
min = 0;
max = 0;
indexMin = 0;
indexMax = 0;
if(!v || size == 0)
{
return;
}
min = v[0];
max = v[0];
for(unsigned int i=1; i<size; ++i)
{
if(uIsNan(min) || (min > v[i] && !uIsNan(v[i])))
{
min = v[i];
indexMin = i;
}
if(uIsNan(max) || (max < v[i] && !uIsNan(v[i])))
{
max = v[i];
indexMax = i;
}
}
}
/**
* Get the minimum and maximum of a vector.
* @param v the array
* @param min reference to output minimum
* @param max reference to output maximum
* @param min reference to output minimum index
* @param max reference to output maximum index
*/
template<class T>
inline void uMinMax(const std::vector<T> & v, T & min, T & max, unsigned int & indexMin, unsigned int & indexMax)
{
uMinMax(v.data(), v.size(), min, max, indexMin, indexMax);
}
/**
* Get the minimum and maximum of a vector.
* @param v the array
* @param size the size of the array
* @param min reference to output minimum
* @param max reference to output maximum
*/
template<class T>
inline void uMinMax(const T * v, unsigned int size, T & min, T & max)
{
unsigned int indexMin;
unsigned int indexMax;
uMinMax(v, size, min, max, indexMin, indexMax);
}
/**
* Get the minimum and maximum of a vector.
* @param v the array
* @param min reference to output minimum
* @param max reference to output maximum
*/
template<class T>
inline void uMinMax(const std::vector<T> & v, T & min, T & max)
{
uMinMax(v.data(), v.size(), min, max);
}
/**
* Get the sign of value.
* @param v the value
* @return -1 if v<0, otherwise 1
*/
template<class T>
inline int uSign(const T & v)
{
if(v < 0)
{
return -1;
}
else
{
return 1;
}
}
/**
* Get the sum of all values contained in a list. Provided for convenience.
* @param list the list
* @return the sum of values of the list
*/
template<class T>
inline T uSum(const std::list<T> & list)
{
T sum = 0;
for(typename std::list<T>::const_iterator i=list.begin(); i!=list.end(); ++i)
{
sum += *i;
}
return sum;
}
/**
* Get the sum of all values contained in an array: sum(x).
* @param v the array
* @param size the size of the array
* @return the sum of values of the array
*/
template<class T>
inline T uSum(const T * v, unsigned int size)
{
T sum = 0;
if(v && size)
{
for(unsigned int i=0; i<size; ++i)
{
sum += v[i];
}
}
return sum;
}
/**
* Get the sum of all values contained in a vector. Provided for convenience.
* @param v the vector
* @return the sum of values of the vector
*/
template<class T>
inline T uSum(const std::vector<T> & v)
{
return uSum(v.data(), (int)v.size());
}
/**
* Get the sum of all squared values contained in an array: sum(x.^2).
* @param v the array
* @param size the size of the array
* @param subtract an optional value to remove to v before squaring v: sum((x-sub).^2)
* @return the sum of values of the array
*/
template<class T>
inline T uSumSquared(const T * v, unsigned int size, T subtract = T())
{
T sum = 0;
if(v && size)
{
for(unsigned int i=0; i<size; ++i)
{
sum += (v[i]-subtract)*(v[i]-subtract);
}
}
return sum;
}
/**
* Get the sum of all squared values contained in an array: sum(x.^2).
* @param v the array
* @param subtract an optional value to remove to v before squaring v: sum((x-sub).^2)
* @return the sum of values of the array
*/
template<class T>
inline T uSumSquared(const std::vector<T> & v, T subtract = T())
{
return uSumSquared(v.data(), v.size(), subtract);
}
/**
* Compute the mean of an array.
* @param v the array
* @param size the size of the array
* @return the mean
*/
template<class T>
inline T uMean(const T * v, unsigned int size)
{
T buf = 0;
if(v && size)
{
for(unsigned int i=0; i<size; ++i)
{
buf += v[i];
}
buf /= size;
}
return buf;
}
/**
* Get the mean of a list. Provided for convenience.
* @param list the list
* @return the mean
*/
template<class T>
inline T uMean(const std::list<T> & list)
{
T m = 0;
if(list.size())
{
for(typename std::list<T>::const_iterator i=list.begin(); i!=list.end(); ++i)
{
m += *i;
}
m /= list.size();
}
return m;
}
/**
* Get the mean of a vector. Provided for convenience.
* @param v the vector
* @return the mean
*/
template<class T>
inline T uMean(const std::vector<T> & v)
{
return uMean(v.data(), v.size());
}
/**
* Compute mean squared error between two arrays: mean((x-y).^2).
* @param x the first array
* @param sizeX the size of the array x
* @param y the second array
* @param sizeY the size of the array y (must be same size as x)
* @return the mean squared error (return -1 if error cannot be computed)
*/
template<class T>
inline T uMeanSquaredError(const T * x, unsigned int sizeX, const T * y, unsigned int sizeY)
{
T sum = 0;
if(x && y && sizeX == sizeY)
{
for(unsigned int i=0; i<sizeX; ++i)
{
T diff = x[i]-y[i];
sum += diff*diff;
}
return sum/(T)sizeX;
}
return (T)-1;
}
/**
* Compute mean squared error between two arrays: mean((x-y).^2).
* @param x the first array
* @param y the second array (must be same size as x)
* @return the mean squared error (return -1 if error cannot be computed)
*/
template<class T>
inline T uMeanSquaredError(const std::vector<T> & x, const std::vector<T> & y)
{
return uMeanSquaredError(x.data(), x.size(), y.data(), y.size());
}
/**
* Compute the variance of an array.
* @param v the array
* @param size the size of the array
* @param meanV the mean of the array
* @return the variance
* @see mean()
*/
template<class T>
inline T uVariance(const T * v, unsigned int size, T meanV)
{
T buf = 0;
if(v && size>1)
{
float sum = 0;
for(unsigned int i=0; i<size; ++i)
{
sum += (v[i]-meanV)*(v[i]-meanV);
}
buf = sum/(size-1);
}
return buf;
}
/**
* Get the variance of a list. Provided for convenience.
* @param list the list
* @param m the mean of the list
* @return the variance
* @see mean()
*/
template<class T>
inline T uVariance(const std::list<T> & list, const T & m)
{
T buf = 0;
if(list.size()>1)
{
float sum = 0;
for(typename std::list<T>::const_iterator i=list.begin(); i!=list.end(); ++i)
{
sum += (*i-m)*(*i-m);
}
buf = sum/(list.size()-1);
}
return buf;
}
/**
* Compute the variance of an array.
* @param v the array
* @param size the size of the array
* @return the variance
*/
template<class T>
inline T uVariance(const T * v, unsigned int size)
{
T m = uMean(v, size);
return uVariance(v, size, m);
}
/**
* Get the variance of a vector. Provided for convenience.
* @param v the vector
* @param m the mean of the vector
* @return the variance
* @see mean()
*/
template<class T>
inline T uVariance(const std::vector<T> & v, const T & m)
{
return uVariance(v.data(), v.size(), m);
}
/**
* Get the squared norm of the vector : return x1*x1 + x2*x2 + x3*x3 + ...
* @return the squared norm of the vector
*/
template<class T>
inline T uNormSquared(const std::vector<T> & v)
{
float sum = 0.0f;
for(unsigned int i=0; i<v.size(); ++i)
{
sum += v[i]*v[i];
}
return sum;
}
/**
* Get the norm of the vector : return sqrt(x1*x1 + x2*x2 + x3*x3 + ...)
* @return the norm of the vector
*/
template<class T>
inline T uNorm(const std::vector<T> & v)
{
return std::sqrt(uNormSquared(v));
}
/**
* Get the squared norm of the vector : return x1*x1 + x2*x2
* @return the squared norm of the vector
*/
template<class T>
inline T uNormSquared(const T & x1, const T & x2)
{
return x1*x1 + x2*x2;
}
/**
* Get the norm of the vector : return sqrt(x1*x1 + x2*x2 + x3*x3)
* @return the norm of the vector
*/
template<class T>
inline T uNorm(const T & x1, const T & x2)
{
return std::sqrt(uNormSquared(x1, x2));
}
/**
* Get the squared norm of the vector : return x1*x1 + x2*x2 + x3*x3
* @return the squared norm of the vector
*/
template<class T>
inline T uNormSquared(const T & x1, const T & x2, const T & x3)
{
return x1*x1 + x2*x2 + x3*x3;
}
/**
* Get the norm of the vector : return sqrt(x1*x1 + x2*x2 + x3*x3)
* @return the norm of the vector
*/
template<class T>
inline T uNorm(const T & x1, const T & x2, const T & x3)
{
return std::sqrt(uNormSquared(x1, x2, x3));
}
/**
* Normalize the vector : [x1 x2 x3 ...] ./ uNorm([x1 x2 x3 ...])
* @return the vector normalized
*/
template<class T>
inline std::vector<T> uNormalize(const std::vector<T> & v)
{
float norm = uNorm(v);
if(norm == 0)
{
return v;
}
else
{
std::vector<T> r(v.size());
for(unsigned int i=0; i<v.size(); ++i)
{
r[i] = v[i]/norm;
}
return r;
}
}
/**
* Find all local maxima.
*/
template<class T>
inline std::list<unsigned int> uLocalMaxima(const T * v, unsigned int size)
{
std::list<unsigned int> maxima;
if(size)
{
for(unsigned int i=0; i<size; ++i)
{
if(i == 0)
{
// first item
if((i+1 < size && v[i] > v[i+1]) ||
i+1 >= size)
{
maxima.push_back(i);
}
}
else if(i == size - 1)
{
//last item
if((i >= 1 && v[i] > v[i-1]) ||
i == 0)
{
maxima.push_back(i);
}
}
else
{
//all others, check previous and next
if(v[i] > v[i-1] && v[i] > v[i+1])
{
maxima.push_back(i);
}
}
}
}
return maxima;
}
/**
* Find all local maxima.
*/
template<class T>
inline std::list<unsigned int> uLocalMaxima(const std::vector<T> & v)
{
return uLocalMaxima(v.data(), v.size());
}
/**
* Enum of cross matching methods (cross-correlation, cross-covariance) :
* UXCorrRaw, UXCorrBiased, UXCorrUnbiased, UXCorrCoeff, UXCovRaw, UXCovBiased, UXCovUnbiased, UXCovCoeff.
*/
enum UXMatchMethod{UXCorrRaw, UXCorrBiased, UXCorrUnbiased, UXCorrCoeff, UXCovRaw, UXCovBiased, UXCovUnbiased, UXCovCoeff};
/**
* Do a full cross-correlation or cross-covariance between 2 arrays.
* @param vA the first array
* @param vB the second array
* @param sizeA the size of the first array
* @param sizeB the size of the second array
* @param method see UXMatchMethod
* @return the resulting correlation/covariance vector of size = sizeA + sizeB - 1
*/
template<class T>
inline std::vector<T> uXMatch(const T * vA, const T * vB, unsigned int sizeA, unsigned int sizeB, UXMatchMethod method)
{
if(!vA || !vB || sizeA == 0 || sizeB == 0)
{
return std::vector<T>();
}
std::vector<T> result(sizeA + sizeB - 1);
T meanA = 0;
T meanB = 0;
if(method > UXCorrCoeff)
{
meanA = uMean(vA, sizeA);
meanB = uMean(vB, sizeB);
}
T den = 1;
if(method == UXCorrCoeff || method == UXCovCoeff)
{
den = std::sqrt(uSumSquared(vA, sizeA, meanA) * uSumSquared(vB, sizeB, meanB));
}
else if(method == UXCorrBiased || method == UXCovBiased)
{
den = (T)std::max(sizeA, sizeB);
}
if(sizeA == sizeB)
{
T resultA;
T resultB;
int posA;
int posB;
unsigned int j;
// Optimization, filling two results at once
for(unsigned int i=0; i<sizeA; ++i)
{
if(method == UXCorrUnbiased || method == UXCovUnbiased)
{
den = 0;
}
posA = sizeA - i - 1;
posB = sizeB - i - 1;
resultA = 0;
resultB = 0;
for(j=0; (j + posB) < sizeB && (j + posA) < sizeA; ++j)
{
resultA += (vA[j] - meanA) * (vB[j + posB] - meanB);
resultB += (vA[j + posA] - meanA) * (vB[j] - meanB);
if(method == UXCorrUnbiased || method == UXCovUnbiased)
{
++den;
}
}
result[i] = resultA / den;
result[result.size()-1 -i] = resultB / den;
}
}
else
{
for(unsigned int i=0; i<result.size(); ++i)
{
if(method == UXCorrUnbiased || method == UXCovUnbiased)
{
den = 0;
}
int posB = sizeB - i - 1;
T r = 0;
if(posB >= 0)
{
for(unsigned int j=0; (j + posB) < sizeB && j < sizeA; ++j)
{
r += (vA[j] - meanA) * (vB[j + posB] - meanB);
if(method == UXCorrUnbiased || method == UXCovUnbiased)
{
++den;
}
}
}
else
{
int posA = posB*-1;
for(unsigned int i=0; (i+posA) < sizeA && i < sizeB; ++i)
{
r += (vA[i+posA] - meanA) * (vB[i] - meanB);
if(method == UXCorrUnbiased || method == UXCovUnbiased)
{
++den;
}
}
}
result[i] = r / den;
}
}
return result;
}
/**
* Do a full cross-correlation or cross-covariance between 2 arrays.
* @param vA the first array
* @param vB the second array
* @param method see UXMatchMethod
* @return the resulting correlation/covariance vector of size = sizeA + sizeB - 1
*/
template<class T>
inline std::vector<T> uXMatch(const std::vector<T> & vA, const std::vector<T> & vB, UXMatchMethod method)
{
return uXMatch(vA.data(), vB.data(), vA.size(), vB.size(), method);
}
/**
* Do a cross correlation between 2 arrays at a specified index.
* @param vA the first array
* @param vB the second array
* @param sizeA the size of the first array
* @param sizeB the size of the second array
* @param index the index to correlate
* @param method see UXMatchMethod
* @return the resulting correlation value
*/
template<class T>
inline T uXMatch(const T * vA, const T * vB, unsigned int sizeA, unsigned int sizeB, unsigned int index, UXMatchMethod method)
{
T result = 0;
if(!vA || !vB || sizeA == 0 || sizeB == 0)
{
return result;
}
T meanA = 0;
T meanB = 0;
if(method > UXCorrCoeff)
{
meanA = uMean(vA, sizeA);
meanB = uMean(vB, sizeB);
}
unsigned int size = sizeA + sizeB - 1;
T den = 1;
if(method == UXCorrCoeff || method == UXCovCoeff)
{
den = std::sqrt(uSumSquared(vA, sizeA, meanA) * uSumSquared(vB, sizeB, meanB));
}
else if(method == UXCorrBiased || method == UXCovBiased)
{
den = (T)std::max(sizeA, sizeB);
}
else if(method == UXCorrUnbiased || method == UXCovUnbiased)
{
den = 0;
}
if(index < size)
{
int posB = sizeB - index - 1;
unsigned int i;
if(posB >= 0)
{
for(i=0; (i + posB) < sizeB && i < sizeA; ++i)
{
result += (vA[i] - meanA) * (vB[i + posB] - meanB);
if(method == UXCorrUnbiased || method == UXCovUnbiased)
{
++den;
}
}
}
else
{
int posA = posB*-1;
for(i=0; (i+posA) < sizeA && i < sizeB; ++i)
{
result += (vA[i+posA] - meanA) * (vB[i] - meanB);
if(method == UXCorrUnbiased || method == UXCovUnbiased)
{
++den;
}
}
}
}
return result / den;
}
/**
* Do a cross correlation between 2 arrays at a specified index.
* @param vA the first array
* @param vB the second array
* @param sizeA the size of the first array
* @param sizeB the size of the second array
* @param index the index to correlate
* @param method see UXMatchMethod
* @return the resulting correlation value
*/
template<class T>
inline T uXMatch(const std::vector<T> & vA, const std::vector<T> & vB, unsigned int index, UXMatchMethod method)
{
return uXMatch(vA.data(), vB.data(), vA.size(), vB.size(), index, method);
}
/**
* Return Hamming window of length L.
* @param L the window length
* @return the Hamming window (values are between 0 and 1)
*/
inline std::vector<float> uHamming(unsigned int L)
{
std::vector<float> w(L);
unsigned int N = L-1;
float pi = 3.14159265f;
for(unsigned int n=0; n<N; ++n)
{
w[n] = 0.54f-0.46f*std::cos(2.0f*pi*float(n)/float(N));
}
return w;
}
template <typename T>
bool uIsInBounds(const T& value, const T& low, const T& high)
{
return uIsFinite(value) && !(value < low) && !(value >= high);
}
#endif // UMATH_H
rtabmap/utilite/include/rtabmap/utilite/UMutex.h
+179
View File
@@ -0,0 +1,179 @@
/*
* utilite is a cross-platform library with
* useful utilities for fast and small developing.
* Copyright (C) 2010 Mathieu Labbe
*
* utilite is free library: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* utilite is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef UMUTEX_H
#define UMUTEX_H
#include <errno.h>
#ifdef _WIN32
#include "rtabmap/utilite/Win32/UWin32.h"
#else
#include <pthread.h>
#endif
/**
* A mutex class.
*
* On a lock() call, the calling thread is blocked if the
* UMutex was previously locked by another thread. It is unblocked when unlock() is called.
*
* On Unix (not yet tested on Windows), UMutex is recursive: the same thread can
* call multiple times lock() without being blocked.
*
* Example:
* @code
* UMutex m; // Mutex shared with another thread(s).
* ...
* m.lock();
* // Data is protected here from the second thread
* //(assuming the second one protects also with the same mutex the same data).
* m.unlock();
*
* @endcode
*
* @see USemaphore
*/
class UMutex
{
public:
/**
* The constructor.
*/
UMutex()
{
#ifdef _WIN32
InitializeCriticalSection(&C);
#else
pthread_mutexattr_t attr;
pthread_mutexattr_init(&attr);
pthread_mutexattr_settype(&attr,PTHREAD_MUTEX_RECURSIVE);
pthread_mutex_init(&M,&attr);
pthread_mutexattr_destroy(&attr);
#endif
}
virtual ~UMutex()
{
#ifdef _WIN32
DeleteCriticalSection(&C);
#else
pthread_mutex_unlock(&M); pthread_mutex_destroy(&M);
#endif
}
/**
* Lock the mutex.
*/
int lock() const
{
#ifdef _WIN32
EnterCriticalSection(&C); return 0;
#else
return pthread_mutex_lock(&M);
#endif
}
#ifdef _WIN32
#if(_WIN32_WINNT >= 0x0400)
int lockTry() const
{
return (TryEnterCriticalSection(&C)?0:EBUSY);
}
#endif
#else
int lockTry() const
{
return pthread_mutex_trylock(&M);
}
#endif
/**
* Unlock the mutex.
*/
int unlock() const
{
#ifdef _WIN32
LeaveCriticalSection(&C); return 0;
#else
return pthread_mutex_unlock(&M);
#endif
}
private:
#ifdef _WIN32
mutable CRITICAL_SECTION C;
#else
mutable pthread_mutex_t M;
#endif
void operator=(UMutex &) {}
UMutex( const UMutex & ) {}
};
/**
* Automatically lock the referenced mutex on constructor and unlock mutex on destructor.
*
* Example:
* @code
* UMutex m; // Mutex shared with another thread(s).
* ...
* int myMethod()
* {
* UScopeMutex sm(m); // automatically lock the mutex m
* if(cond1)
* {
* return 1; // automatically unlock the mutex m
* }
* else if(cond2)
* {
* return 2; // automatically unlock the mutex m
* }
* return 0; // automatically unlock the mutex m
* }
*
* @endcode
*
* @see UMutex
*/
class UScopeMutex
{
public:
UScopeMutex(const UMutex & mutex) :
mutex_(mutex)
{
mutex_.lock();
}
// backward compatibility
UScopeMutex(UMutex * mutex) :
mutex_(*mutex)
{
mutex_.lock();
}
~UScopeMutex()
{
mutex_.unlock();
}
private:
const UMutex & mutex_;
};
#endif // UMUTEX_H
rtabmap/utilite/include/rtabmap/utilite/UObjDeletionThread.h
+129
View File
@@ -0,0 +1,129 @@
/*
* utilite is a cross-platform library with
* useful utilities for fast and small developing.
* Copyright (C) 2010 Mathieu Labbe
*
* utilite is free library: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* utilite is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef UOBJDELETIONTHREAD_H
#define UOBJDELETIONTHREAD_H
#include "rtabmap/utilite/UThreadNode.h"
#include "rtabmap/utilite/UEvent.h"
#include "rtabmap/utilite/UEventsManager.h"
/**
* Event used by UObjDeletionThread to notify when its object is deleted. It contains
* the object id used for deletion (can be retrieved by UEvent::getCode()).
*/
class UObjDeletedEvent : public UEvent
{
public:
UObjDeletedEvent(int objDeletionThreadId) : UEvent(objDeletionThreadId) {}
virtual ~UObjDeletedEvent() {}
/**
* @return string "UObjDeletedEvent"
*/
virtual std::string getClassName() const {return std::string("UObjDeletedEvent");}
};
/**
* This class can be used to delete a dynamically created object in another thread. Give the
* dynamic reference to object to it and it will notify with a UObjDeletedEvent when the object is deleted.
* The deletion can be delayed on startDeletion(), the thread will wait the time given before deleting the object.
*/
template<class T>
class UObjDeletionThread : public UThread
{
public:
/**
* The constructor.
* @param obj the object to delete
* @param id the custom id which will be sent in a event UObjDeletedEvent after the object is deleted
*/
UObjDeletionThread(T * obj, int id=0) :
obj_(obj),
id_(id),
waitMs_(0) {}
/**
* The destructor. If this thread is not started but with an object set, the
* object is deleted. If the thread has not finished to delete the object, the
* calling thread will wait (on a UThreadNode::join()) until the object is deleted.
* @param obj the object to delete
* @param id the custom id which will be sent in a event UObjDeletedEvent after the object is deleted
*/
virtual ~UObjDeletionThread()
{
join(true);
if(obj_)
{
delete obj_;
}
}
/**
* Start the thread after optional delay.
* @param waitMs the delay before deletion
*/
void startDeletion(int waitMs = 0) {waitMs_ = waitMs; this->start();}
/**
* Get id of the deleted object.
* @return the id
*/
int id() const {return id_;}
/**
* Set a new object, if one was already set, the old one is deleted.
* @param obj the object to delete
*/
void setObj(T * obj)
{
join();
if(obj_)
{
delete obj_;
obj_ = 0;
}
obj_ = obj;
}
private:
/**
* Thread main loop...
*/
virtual void mainLoop()
{
if(waitMs_)
{
uSleep(waitMs_);
}
if(obj_)
{
delete obj_;
obj_ = 0;
}
this->kill();
UEventsManager::post(new UObjDeletedEvent(id_), false);
}
private:
T * obj_;
int id_;
int waitMs_;
};
#endif /* UOBJDELETIONTHREAD_H */
rtabmap/utilite/include/rtabmap/utilite/UProcessInfo.h
+41
View File
@@ -0,0 +1,41 @@
/*
* utilite is a cross-platform library with
* useful utilities for fast and small developing.
* Copyright (C) 2010 Mathieu Labbe
*
* utilite is free library: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* utilite is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef UPROCESSINFO_H
#define UPROCESSINFO_H
#include "rtabmap/utilite/utilite_export.h" // DLL export/import defines
/**
* This class is used to get some informations
* about the current process.
*/
class UTILITE_EXPORT UProcessInfo {
public:
UProcessInfo();
virtual ~UProcessInfo();
/**
* Get the memory used by the current process.
* @return the number of bytes used by the current process.
*/
static long int getMemoryUsage();
};
#endif /* UPROCESSINFO_H */
rtabmap/utilite/include/rtabmap/utilite/USemaphore.h
+223
View File
@@ -0,0 +1,223 @@
/*
* utilite is a cross-platform library with
* useful utilities for fast and small developing.
* Copyright (C) 2010 Mathieu Labbe
*
* utilite is free library: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* utilite is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
/*
* Originally written by Phillip Sitbon
* Copyright 2003
*/
#ifndef USEMAPHORE_H
#define USEMAPHORE_H
#include <errno.h>
#ifdef _WIN32
#include "rtabmap/utilite/Win32/UWin32.h"
#define SEM_VALUE_MAX ((int) ((~0u) >> 1))
#else
#include <pthread.h>
#include <sys/time.h>
#endif
/**
* A semaphore class.
*
* On an acquire() call, the calling thread is blocked if the
* USemaphore's value is <= 0. It is unblocked when release() is called.
* The function acquire() decreases by 1 (default) the
* semaphore's value and release() increases it by 1 (default).
*
* Example:
* @code
* USemaphore s;
* s.acquire(); // Will wait until s.release() is called by another thread.
* @endcode
*
* @see UMutex
*/
class USemaphore
{
public:
/**
* The constructor. The semaphore waits on acquire() when its value is <= 0.
* @param n number to initialize
*/
USemaphore( int initValue = 0 )
{
#ifdef _WIN32
S = CreateSemaphore(0,initValue,SEM_VALUE_MAX,0);
#else
_available = initValue;
pthread_mutex_init(&_waitMutex, NULL);
pthread_cond_init(&_cond, NULL);
#endif
}
virtual ~USemaphore()
{
#ifdef _WIN32
CloseHandle(S);
#else
pthread_cond_destroy(&_cond);
pthread_mutex_destroy(&_waitMutex);
#endif
}
/**
* Acquire the semaphore. If semaphore's value is <=0, the
* calling thread will wait until the count acquired is released.
* @see release()
* @param n number to acquire
* @param t time to wait (ms), a value <=0 means infinite
* @return true on success, false on error/timeout
*/
#ifdef _WIN32
bool acquire(int n = 1, int ms = 0) const
{
int rt = 0;
while(n-- > 0 && rt==0)
{
rt = WaitForSingleObject((HANDLE)S, ms<=0?INFINITE:ms);
}
return rt == 0;
}
#else
bool acquire(int n = 1, int ms = 0)
{
int rt = 0;
pthread_mutex_lock(&_waitMutex);
while (n > _available && rt == 0)
{
if(ms > 0)
{
struct timespec timeToWait;
struct timeval now;
gettimeofday(&now,NULL);
timeToWait.tv_sec = now.tv_sec + ms/1000;
timeToWait.tv_nsec = (now.tv_usec+1000UL*(ms%1000))*1000UL;
rt = pthread_cond_timedwait(&_cond, &_waitMutex, &timeToWait);
}
else
{
rt = pthread_cond_wait(&_cond, &_waitMutex);
}
}
if(rt == 0)
{
// only remove them if waiting did not fail
_available -= n;
}
pthread_mutex_unlock(&_waitMutex);
return rt == 0;
}
#endif
/*
* Try to acquire the semaphore, not a blocking call.
* @return false if the semaphore can't be taken without waiting (value <= 0), true otherwise
*/
#ifdef _WIN32
int acquireTry() const
{
return ((WaitForSingleObject((HANDLE)S,INFINITE)==WAIT_OBJECT_0)?0:EAGAIN);
}
#else
int acquireTry(int n)
{
pthread_mutex_lock(&_waitMutex);
if(n > _available)
{
pthread_mutex_unlock(&_waitMutex);
return false;
}
_available -= n;
pthread_mutex_unlock(&_waitMutex);
return true;
}
#endif
/**
* Release the semaphore, increasing its value by 1 and
* signaling waiting threads (which called acquire()).
*/
#ifdef _WIN32
int release(int n = 1) const
{
return (ReleaseSemaphore((HANDLE)S,n,0)?0:ERANGE);
}
#else
void release(int n = 1)
{
pthread_mutex_lock(&_waitMutex);
_available += n;
pthread_cond_broadcast(&_cond);
pthread_mutex_unlock(&_waitMutex);
}
#endif
/**
* Get the USempahore's value.
* @return the semaphore's value
*/
#ifdef _WIN32
int value() const
{
LONG V = -1; ReleaseSemaphore((HANDLE)S,0,&V); return V;
}
#else
int value()
{
int value = 0;
pthread_mutex_lock(&_waitMutex);
value = _available;
pthread_mutex_unlock(&_waitMutex);
return value;
}
#endif
#ifdef _WIN32
/*
* Reset the semaphore count.
* @param init the initial value
* TODO implement on posix ?
*/
void reset( int init = 0 )
{
CloseHandle(S);
S = CreateSemaphore(0,init,SEM_VALUE_MAX,0);
}
#endif
private:
void operator=(const USemaphore &){}
#ifdef _WIN32
USemaphore(const USemaphore &S){}
HANDLE S;
#else
USemaphore(const USemaphore &):_available(0){}
pthread_mutex_t _waitMutex;
pthread_cond_t _cond;
int _available;
#endif
};
#endif // USEMAPHORE_H
rtabmap/utilite/include/rtabmap/utilite/UStl.h
+822
View File
@@ -0,0 +1,822 @@
/*
* utilite is a cross-platform library with
* useful utilities for fast and small developing.
* Copyright (C) 2010 Mathieu Labbe
*
* utilite is free library: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* utilite is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef USTL_H
#define USTL_H
#include <list>
#include <map>
#include <set>
#include <vector>
#include <string>
#include <algorithm>
#include <stdlib.h>
/**
* \file UStl.h
* \brief Wrappers of STL for convenient functions.
*
* All functions you will find here are here
* for the use of STL in a more convenient way.
*/
/**
* Get unique keys from a std::multimap.
* @param mm the multimap
* @return the list which contains unique keys
*/
template<class K, class V>
inline std::list<K> uUniqueKeys(const std::multimap<K, V> & mm)
{
std::list<K> l;
typename std::list<K>::reverse_iterator lastValue;
for(typename std::multimap<K, V>::const_iterator iter = mm.begin(); iter!=mm.end(); ++iter)
{
if(iter == mm.begin() || (iter != mm.begin() && *lastValue != iter->first))
{
l.push_back(iter->first);
lastValue = l.rbegin();
}
}
return l;
}
/**
* Get all keys from a std::multimap.
* @param mm the multimap
* @return the vector which contains all keys (may contains duplicated keys)
*/
template<class K, class V>
inline std::vector<K> uKeys(const std::multimap<K, V> & mm)
{
std::vector<K> v(mm.size());
int i=0;
for(typename std::multimap<K, V>::const_iterator iter = mm.begin(); iter!=mm.end(); ++iter)
{
v[i++] = iter->first;
}
return v;
}
/**
* Get all keys from a std::multimap.
* @param mm the multimap
* @return the list which contains all keys (may contains duplicated keys)
*/
template<class K, class V>
inline std::list<K> uKeysList(const std::multimap<K, V> & mm)
{
std::list<K> l;
for(typename std::multimap<K, V>::const_iterator iter = mm.begin(); iter!=mm.end(); ++iter)
{
l.push_back(iter->first);
}
return l;
}
/**
* Get all values from a std::multimap.
* @param mm the multimap
* @return the vector which contains all values (contains values from duplicated keys)
*/
template<class K, class V>
inline std::vector<V> uValues(const std::multimap<K, V> & mm)
{
std::vector<V> v(mm.size());
int i=0;
for(typename std::multimap<K, V>::const_iterator iter = mm.begin(); iter!=mm.end(); ++iter)
{
v[i++] = iter->second;
}
return v;
}
/**
* Get all values from a std::multimap.
* @param mm the multimap
* @return the list which contains all values (contains values from duplicated keys)
*/
template<class K, class V>
inline std::list<V> uValuesList(const std::multimap<K, V> & mm)
{
std::list<V> l;
for(typename std::multimap<K, V>::const_iterator iter = mm.begin(); iter!=mm.end(); ++iter)
{
l.push_back(iter->second);
}
return l;
}
/**
* Get values for a specified key from a std::multimap.
* @param mm the multimap
* @param key the key
* @return the list which contains the values of the key
*/
template<class K, class V>
inline std::list<V> uValues(const std::multimap<K, V> & mm, const K & key)
{
std::list<V> l;
std::pair<typename std::multimap<K, V>::const_iterator, typename std::multimap<K, V>::const_iterator> range;
range = mm.equal_range(key);
for(typename std::multimap<K, V>::const_iterator iter = range.first; iter!=range.second; ++iter)
{
l.push_back(iter->second);
}
return l;
}
/**
* Get all keys from a std::map.
* @param m the map
* @return the vector of keys
*/
template<class K, class V>
inline std::vector<K> uKeys(const std::map<K, V> & m)
{
std::vector<K> v(m.size());
int i=0;
for(typename std::map<K, V>::const_iterator iter = m.begin(); iter!=m.end(); ++iter)
{
v[i] = iter->first;
++i;
}
return v;
}
/**
* Get all keys from a std::map.
* @param m the map
* @return the list of keys
*/
template<class K, class V>
inline std::list<K> uKeysList(const std::map<K, V> & m)
{
std::list<K> l;
for(typename std::map<K, V>::const_iterator iter = m.begin(); iter!=m.end(); ++iter)
{
l.push_back(iter->first);
}
return l;
}
/**
* Get all keys from a std::map.
* @param m the map
* @return the set of keys
*/
template<class K, class V>
inline std::set<K> uKeysSet(const std::map<K, V> & m)
{
std::set<K> s;
for(typename std::map<K, V>::const_iterator iter = m.begin(); iter!=m.end(); ++iter)
{
s.insert(s.end(), iter->first);
}
return s;
}
/**
* Get all values from a std::map.
* @param m the map
* @return the vector of values
*/
template<class K, class V>
inline std::vector<V> uValues(const std::map<K, V> & m)
{
std::vector<V> v(m.size());
int i=0;
for(typename std::map<K, V>::const_iterator iter = m.begin(); iter!=m.end(); ++iter)
{
v[i] = iter->second;
++i;
}
return v;
}
/**
* Get all values from a std::map.
* @param m the map
* @return the list of values
*/
template<class K, class V>
inline std::list<V> uValuesList(const std::map<K, V> & m)
{
std::list<V> l;
for(typename std::map<K, V>::const_iterator iter = m.begin(); iter!=m.end(); ++iter)
{
l.push_back(iter->second);
}
return l;
}
/**
* Get the value of a specified key from a std::map.
* @param m the map
* @param key the key
* @param defaultValue the default value used if the key is not found
* @return the value
*/
template<class K, class V>
inline V uValue(const std::map<K, V> & m, const K & key, const V & defaultValue = V())
{
V v = defaultValue;
typename std::map<K, V>::const_iterator i = m.find(key);
if(i != m.end())
{
v = i->second;
}
return v;
}
/**
* Get the value of a specified key from a std::map. This will
* remove the value from the map;
* @param m the map
* @param key the key
* @param defaultValue the default value used if the key is not found
* @return the value
*/
template<class K, class V>
inline V uTake(std::map<K, V> & m, const K & key, const V & defaultValue = V())
{
V v;
typename std::map<K, V>::iterator i = m.find(key);
if(i != m.end())
{
v = i->second;
m.erase(i);
}
else
{
v = defaultValue;
}
return v;
}
/**
* Get the iterator at a specified position in a std::list. If the position
* is out of range, the result is the end iterator of the list.
* @param list the list
* @param pos the index position in the list
* @return the iterator at the specified index
*/
template<class V>
inline typename std::list<V>::iterator uIteratorAt(std::list<V> & list, const unsigned int & pos)
{
typename std::list<V>::iterator iter = list.begin();
for(unsigned int i = 0; i<pos && iter != list.end(); ++i )
{
++iter;
}
return iter;
}
/**
* Get the iterator at a specified position in a std::list. If the position
* is out of range, the result is the end iterator of the list.
* @param list the list
* @param pos the index position in the list
* @return the iterator at the specified index
*/
template<class V>
inline typename std::list<V>::const_iterator uIteratorAt(const std::list<V> & list, const unsigned int & pos)
{
typename std::list<V>::const_iterator iter = list.begin();
for(unsigned int i = 0; i<pos && iter != list.end(); ++i )
{
++iter;
}
return iter;
}
/**
* Get the iterator at a specified position in a std::set. If the position
* is out of range, the result is the end iterator of the set.
* @param set the set
* @param pos the index position in the set
* @return the iterator at the specified index
*/
template<class V>
inline typename std::set<V>::iterator uIteratorAt(std::set<V> & set, const unsigned int & pos)
{
typename std::set<V>::iterator iter = set.begin();
for(unsigned int i = 0; i<pos && iter != set.end(); ++i )
{
++iter;
}
return iter;
}
/**
* Get the iterator at a specified position in a std::set. If the position
* is out of range, the result is the end iterator of the set.
* @param set the set
* @param pos the index position in the set
* @return the iterator at the specified index
*/
template<class V>
inline typename std::set<V>::const_iterator uIteratorAt(const std::set<V> & set, const unsigned int & pos)
{
typename std::set<V>::const_iterator iter = set.begin();
for(unsigned int i = 0; i<pos && iter != set.end(); ++i )
{
++iter;
}
return iter;
}
/**
* Get the iterator at a specified position in a std::vector. If the position
* is out of range, the result is the end iterator of the vector.
* @param v the vector
* @param pos the index position in the vector
* @return the iterator at the specified index
*/
template<class V>
inline typename std::vector<V>::iterator uIteratorAt(std::vector<V> & v, const unsigned int & pos)
{
return v.begin() + pos;
}
/**
* Get the iterator at a specified position in a std::vector. If the position
* is out of range, the result is the end iterator of the vector.
* @param v the vector
* @param pos the index position in the vector
* @return the iterator at the specified index
*/
template<class V>
inline typename std::vector<V>::const_iterator uIteratorAt(const std::vector<V> & v, const unsigned int & pos)
{
return v.begin() + pos;
}
/**
* Get the value at a specified position in a std::list. If the position
* is out of range, the result is undefined.
* @param list the list
* @param pos the index position in the list
* @return the value at the specified index
*/
template<class V>
inline V & uValueAt(std::list<V> & list, const unsigned int & pos)
{
typename std::list<V>::iterator iter = uIteratorAt(list, pos);
return *iter;
}
/**
* Get the value at a specified position in a std::list. If the position
* is out of range, the result is undefined.
* @param list the list
* @param pos the index position in the list
* @return the value at the specified index
*/
template<class V>
inline const V & uValueAt(const std::list<V> & list, const unsigned int & pos)
{
typename std::list<V>::const_iterator iter = uIteratorAt(list, pos);
return *iter;
}
/**
* Check if the list contains the specified value.
* @param list the list
* @param value the value
* @return true if the value is found in the list, otherwise false
*/
template<class V>
inline bool uContains(const std::list<V> & list, const V & value)
{
return std::find(list.begin(), list.end(), value) != list.end();
}
/**
* Check if the map contains the specified key.
* @param map the map
* @param key the key
* @return true if the value is found in the map, otherwise false
*/
template<class K, class V>
inline bool uContains(const std::map<K, V> & map, const K & key)
{
return map.find(key) != map.end();
}
/**
* Check if the multimap contains the specified key.
* @param map the map
* @param key the key
* @return true if the value is found in the map, otherwise false
*/
template<class K, class V>
inline bool uContains(const std::multimap<K, V> & map, const K & key)
{
return map.find(key) != map.end();
}
/**
* Insert an item in the map. Contrary to the insert in the STL,
* if the key already exists, the value will be replaced by the new one.
*/
template<class K, class V>
inline void uInsert(std::map<K, V> & map, const std::pair<K, V> & pair)
{
std::pair<typename std::map<K, V>::iterator, bool> inserted = map.insert(pair);
if(inserted.second == false)
{
inserted.first->second = pair.second;
}
}
/**
* Insert items in the map. Contrary to the insert in the STL,
* if the key already exists, the value will be replaced by the new one.
*/
template<class K, class V>
inline void uInsert(std::map<K, V> & map, const std::map<K, V> & items)
{
for(typename std::map<K, V>::const_iterator iter=items.begin(); iter!=items.end(); ++iter)
{
std::pair<typename std::map<K, V>::iterator, bool> inserted = map.insert(*iter);
if(inserted.second == false)
{
inserted.first->second = iter->second;
}
}
}
/**
* Convert a std::list to a std::vector.
* @param list the list
* @return the vector
*/
template<class V>
inline std::vector<V> uListToVector(const std::list<V> & list)
{
return std::vector<V>(list.begin(), list.end());
}
/**
* Convert a std::vector to a std::list.
* @param v the vector
* @return the list
*/
template<class V>
inline std::list<V> uVectorToList(const std::vector<V> & v)
{
return std::list<V>(v.begin(), v.end());
}
/**
* Convert a std::multimap to a std::map
* @see uMultimapToMapUnique to keep only unique keys
*/
template<class K, class V>
inline std::map<K, V> uMultimapToMap(const std::multimap<K, V> & m)
{
return std::map<K, V>(m.begin(), m.end());
}
/**
* Convert a std::multimap to a std::map, keeping only unique keys!
*/
template<class K, class V>
inline std::map<K, V> uMultimapToMapUnique(const std::multimap<K, V> & m)
{
std::map<K, V> mapOut;
std::list<K> uniqueKeys = uUniqueKeys(m);
for(typename std::list<K>::const_iterator iter = uniqueKeys.begin(); iter!=uniqueKeys.end(); ++iter)
{
if(m.count(*iter) == 1)
{
typename std::multimap<K, V>::const_iterator jter=m.find(*iter);
mapOut.insert(mapOut.end(), std::pair<K,V>(jter->first, jter->second));
}
}
return mapOut;
}
/**
* Append a list to another list.
* @param list the list on which the other list will be appended
* @param newItems the list of items to be appended
*/
template<class V>
inline void uAppend(std::list<V> & list, const std::list<V> & newItems)
{
list.insert(list.end(), newItems.begin(), newItems.end());
}
/**
* Get the index in the list of the specified value. S negative index is returned
* if the value is not found.
* @param list the list
* @param value the value
* @return the index of the value in the list
*/
template<class V>
inline int uIndexOf(const std::vector<V> & list, const V & value)
{
int index=-1;
int i=0;
for(typename std::vector<V>::const_iterator iter = list.begin(); iter!=list.end(); ++iter)
{
if(*iter == value)
{
index = i;
break;
}
++i;
}
return index;
}
/**
* Split a string into multiple string around the specified separator.
* Example:
* @code
* std::list<std::string> v = split("Hello the world!", ' ');
* @endcode
* The list v will contain {"Hello", "the", "world!"}
* @param str the string
* @param separator the separator character
* @return the list of strings
*/
inline std::list<std::string> uSplit(const std::string & str, char separator = ' ')
{
std::list<std::string> v;
std::string buf;
for(unsigned int i=0; i<str.size(); ++i)
{
if(str[i] != separator)
{
buf += str[i];
}
else if(buf.size())
{
v.push_back(buf);
buf = "";
}
}
if(buf.size())
{
v.push_back(buf);
}
return v;
}
/**
* Join multiple strings into one string with optional separator.
* Example:
* @code
* std::list<std::string> v;
* v.push_back("Hello");
* v.push_back("world!");
* std::string joined = split(v, " ");
* @endcode
* The output string is "Hello world!"
* @param strings a list of strings
* @param separator the separator string
* @return the joined string
*/
inline std::string uJoin(const std::list<std::string> & strings, const std::string & separator = "")
{
std::string out;
for(std::list<std::string>::const_iterator iter = strings.begin(); iter!=strings.end(); ++iter)
{
if(iter!=strings.begin() && !separator.empty())
{
out += separator;
}
out+=*iter;
}
return out;
}
/**
* Check if a character is a digit.
* @param c the character
* @return true if the character is a digit (if c >= '0' && c <= '9')
*/
inline bool uIsDigit(const char c)
{
return c >= '0' && c <= '9';
}
/**
* Check if a string is a integer number.
* @param str the string
* @return true if the string is a integer number
*/
inline bool uIsInteger(const std::string & str, bool checkForSign = true)
{
bool isInteger = str.size()!=0;
for(unsigned int i=0; i<str.size() && isInteger; ++i)
{
isInteger = (checkForSign && i==0 && str[i]=='-') || uIsDigit(str[i]);
}
return isInteger;
}
/**
* Check if a string is a number (integer or float).
* @param str the string
* @return true if the string is a number
*/
inline bool uIsNumber(const std::string & str)
{
std::list<std::string> list = uSplit(str, '.');
if(list.size() == 1)
{
return uIsInteger(str);
}
else if(list.size() == 2)
{
return uIsInteger(list.front()) && uIsInteger(list.back(), false);
}
return false;
}
/**
* Split a string into number and character strings.
* Example:
* @code
* std::list<std::string> v = uSplit("Hello 03 my 65 world!");
* @endcode
* The list v will contain {"Hello ", "03", " my ", "65", " world!"}
* @param str the string
* @return the list of strings
*/
inline std::list<std::string> uSplitNumChar(const std::string & str)
{
std::list<std::string> list;
std::string buf;
bool num = false;
for(unsigned int i=0; i<str.size(); ++i)
{
if(uIsDigit(str[i]))
{
if(!num && buf.size())
{
list.push_back(buf);
buf.clear();
}
buf += str[i];
num = true;
}
else
{
if(num)
{
list.push_back(buf);
buf.clear();
}
buf += str[i];
num = false;
}
}
if(buf.size())
{
list.push_back(buf);
}
return list;
}
/**
* Compare two alphanumeric strings. Useful to sort filenames (human-like sorting).
* Example:
* @code
* std::string a = "Image9.jpg";
* std::string b = "Image10.jpg";
* int r = uStrNumCmp(a, b); // r returns -1 (a is smaller than b). In contrast, std::strcmp(a, b) would return 1.
* @endcode
* @param a the first string
* @param b the second string
* @return -1 if a<b, 0 if a=b and 1 if a>b
*/
inline int uStrNumCmp(const std::string & a, const std::string & b)
{
std::vector<std::string> listA;
std::vector<std::string> listB;
listA = uListToVector(uSplitNumChar(a));
listB = uListToVector(uSplitNumChar(b));
unsigned int i;
int result = 0;
for(i=0; i<listA.size() && i<listB.size(); ++i)
{
if(uIsDigit(listA[i].at(0)) && uIsDigit(listB[i].at(0)))
{
//padding if zeros at the beginning
if(listA[i].at(0) == '0' && listB[i].size() < listA[i].size())
{
while(listB[i].size() < listA[i].size())
{
listB[i] += '0';
}
}
else if(listB[i].at(0) == '0' && listA[i].size() < listB[i].size())
{
while(listA[i].size() < listB[i].size())
{
listA[i] += '0';
}
}
if(listB[i].size() < listA[i].size())
{
result = 1;
}
else if(listB[i].size() > listA[i].size())
{
result = -1;
}
else
{
result = listA[i].compare(listB[i]);
}
}
else if(uIsDigit(listA[i].at(0)))
{
result = -1;
}
else if(uIsDigit(listB[i].at(0)))
{
result = 1;
}
else
{
result = listA[i].compare(listB[i]);
}
if(result != 0)
{
break;
}
}
return result;
}
/**
* Check if a string contains a specified substring.
*/
inline bool uStrContains(const std::string & string, const std::string & substring)
{
return string.find(substring) != std::string::npos;
}
inline int uCompareVersion(const std::string & version, int major, int minor=-1, int patch=-1)
{
std::vector<std::string> v = uListToVector(uSplit(version, '.'));
if(v.size() == 3)
{
int vMajor = atoi(v[0].c_str());
int vMinor = atoi(v[1].c_str());
int vPatch = atoi(v[2].c_str());
if(vMajor > major ||
(vMajor == major && minor!=-1 && vMinor > minor) ||
(vMajor == major && minor!=-1 && vMinor == minor && patch!=-1 && vPatch > patch))
{
return 1;
}
else if(vMajor == major && (minor == -1 || (vMinor == minor && (patch == -1 || vPatch == patch))))
{
return 0;
}
}
return -1;
}
inline std::string uPad(const std::string & title, int padding = 20)
{
int emptySize = padding - (int)title.size();
if(emptySize>0)
{
return title + std::string(emptySize, ' ');
}
return title;
}
#endif /* USTL_H */
rtabmap/utilite/include/rtabmap/utilite/UThread.h
+270
View File
@@ -0,0 +1,270 @@
/*
* utilite is a cross-platform library with
* useful utilities for fast and small developing.
* Copyright (C) 2010 Mathieu Labbe
*
* utilite is free library: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* utilite is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef UTHREADNODE_H
#define UTHREADNODE_H
#include "rtabmap/utilite/utilite_export.h" // DLL export/import defines
#include "rtabmap/utilite/UThreadC.h"
/**
* The class UThread is an abstract class for creating thread objects.
* A UThread provides methods to create threads as an object-style fashion.
*
* For most of inherited classes, only mainLoop() needs to be implemented, then only start() needs
* to be called from the outside.
* The main loop is called until the thread itself calls kill() or another thread
* calls kill() or join() (with parameter to true) on this thread. Unlike kill(), join() is a blocking call:
* the calling thread will wait until this thread has finished, thus join() must not be
* called inside the mainLoop().
*
* If inside the mainLoop(), at some time, the thread needs to wait on a mutex/semaphore
* (like for the acquisition of a resource), the function mainLoopKill() should be
* implemented to release the mutex/semaphore when the thread is killed, to avoid a deadlock.
* The function killCleanup() is called after the thread's state is set to kSKilled.
* After the mutex/semaphore is released in killCleanup(), on wake up, the thread can know if
* it needs to stop by calling isKilled().
*
* To do an initialization process (executed by the worker thread) just one time before
* entering the mainLoop(), mainLoopBegin() can be implemented.
*
* Example:
* @code
* #include "utilite/UThread.h"
* class SimpleThread : public UThread
* {
* public:
* SimpleThread() {}
* virtual ~SimpleThread() {
* // The calling thread will wait until this thread has finished.
* this->join(true);
* }
*
* protected:
* virtual void mainLoop() {
* // Do some works...
*
* // This will stop the thread, otherwise the mainLoop() is recalled.
* this->kill();
* }
* };
*
* int main(int argc, char * argv[])
* {
* SimpleThread t;
* t.start();
* t.join(); // Wait until the thread has finished.
* return 0;
* }
* @endcode
*
* @see start()
* @see kill()
* @see join()
* @see mainLoopBegin()
* @see mainLoopKill()
* @see mainLoop()
*
*/
class UTILITE_EXPORT UThread : public UThreadC<void>
{
public:
/**
* Enum of priorities : kPLow, kPBelowNormal, kPNormal, kPAboveNormal, kPRealTime.
*/
enum Priority{kPLow, kPBelowNormal, kPNormal, kPAboveNormal, kPRealTime};
public:
//return caller thread id
static unsigned long currentThreadId() {return (unsigned long)UThreadC<void>::Self();}
public:
/**
* The constructor.
* @see Priority
* @param priority the thread priority
*/
UThread(Priority priority = kPNormal);
/**
* The destructor. Inherited classes must call join(true) inside their destructor
* to avoid memory leaks where the underlying c-thread is still running.
*
* Note: not safe to delete a thread while other threads are joining it.
*/
virtual ~UThread();
/**
* Start the thread. Once the thread is started, subsequent calls
* to start() are ignored until the thread is killed.
* @see kill()
*/
void start();
/**
* Kill the thread.
* This functions does nothing if the thread is not started or is killed.
*
* Note : not a blocking call
*/
void kill();
/**
* The caller thread will wait until the thread has finished.
*
* Note : blocking call
* @param killFirst if you want kill() to be called before joining (default false), otherwise not.
*/
void join(bool killFirst = false);
/**
* Set the thread priority.
* @param priority the priority
*/
void setPriority(Priority priority);
/**
* Set the thread affinity. This is applied during start of the thread.
*
* MAC OS X : http://developer.apple.com/library/mac/#releasenotes/Performance/RN-AffinityAPI/_index.html.
* @param cpu the cpu id (start at 1), 0 means no affinity (default).
*/
void setAffinity(int cpu = 0);
/**
* @return if the state of the thread is kSCreating (after start() is called but before entering the mainLoop()).
*/
bool isCreating() const;
/**
* @return if the state of the thread is kSRunning (it is executing the mainLoop()) or kSCreating.
*/
bool isRunning() const;
/**
* @return if the state of the thread is kSIdle (before start() is called and after the thread is totally killed (or after join(true))).
*/
bool isIdle() const;
/**
* @return if the state of the thread is kSKilled (after kill() is called and before the thread is totally killed).
*/
bool isKilled() const;
Handle getThreadHandle() const {return handle_;}
unsigned long getThreadId() const {return threadId_;}
protected:
private:
/**
* Virtual method mainLoopBegin().
* User can implement this function to add a behavior
* before the main loop is started. It is
* called once (before entering mainLoop()).
*/
virtual void mainLoopBegin() {}
/**
* Pure virtual method mainLoop().
* The inner loop of the thread. This method is called repetitively
* until the thread is killed. Note that if kill() is called in mainLoopBegin(),
* mainLoop() is not called, terminating immediately the thread.
*
* @see mainLoop()
* @see kill()
*/
virtual void mainLoop() = 0;
/**
* Virtual method mainLoopKill().
* User can implement this function to add a behavior
* before the thread is killed. When this
* function is called, the state of the thread is set to kSKilled. It is useful to
* wake up a sleeping thread to finish his loop and to avoid a deadlock.
*/
virtual void mainLoopKill() {}
/**
* Virtual method mainLoopEnd().
* User can implement this function to add a behavior
* after the thread is killed (after exiting the mainLoop(), work is
* still done in the thread before exiting).
*/
virtual void mainLoopEnd() {}
/*
* Inherited method ThreadMain() from Thread.
* @see Thread<void>
*/
void ThreadMain();
/*
* Apply thread priority. This is called when starting the thread.
* *@todo : Support pthread
*/
void applyPriority();
/*
* Apply cpu affinity. This is called when starting the thread.
* *@todo : Support Windows
*/
void applyAffinity();
/*
* Inherited method Create() from Thread.
* Here we force this function to be private so the
* inherited class can't have access to it.
* @see Thread<void>
*/
int Create(
Handle * const & H = 0,
const bool & CreateDetached = false,
const unsigned int & StackSize = 0,
const bool & CancelEnable = false, // UNUSED
const bool & CancelAsync = false // UNUSED
) const;
//Methods from UThread<void> class hided
static int Join( Handle H )
{ return UThreadC<void>::Join(H); }
#ifndef ANDROID
static int Kill( Handle H )
{ return UThreadC<void>::Kill(H); }
#endif
static int Detach( Handle H )
{ return UThreadC<void>::Detach(H); }
private:
void operator=(UThread &) {}
UThread( const UThread &) : state_(kSIdle) {}
private:
enum State{kSIdle, kSCreating, kSRunning, kSKilled}; /* Enum of states. */
State state_; /* The thread state. */
Priority priority_; /* The thread priority. */
Handle handle_; /* The thread handle. */
unsigned long threadId_; /* The thread id. */
int cpuAffinity_; /* The cpu affinity. */
UMutex killSafelyMutex_; /* Mutex used to protect the kill() method. */
UMutex runningMutex_; /* Mutex used to notify the join method when the thread has finished. */
};
#endif // UTHREADNODE_H
rtabmap/utilite/include/rtabmap/utilite/UThreadC.h
+36
View File
@@ -0,0 +1,36 @@
/*
* utilite is a cross-platform library with
* useful utilities for fast and small developing.
* Copyright (C) 2010 Mathieu Labbe
*
* utilite is free library: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* utilite is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef UTHREADC_H
#define UTHREADC_H
#include <errno.h>
/*
* Use of StateThread is safer. The Thread class in his
* base form is not supported.
* @see StateThread
*/
#ifdef _WIN32
#include "rtabmap/utilite/Win32/UThreadC.h"
#else
#include "rtabmap/utilite/Posix/UThreadC.h"
#endif
#endif // UTHREADC_H
rtabmap/utilite/include/rtabmap/utilite/UThreadNode.h
+29
View File
@@ -0,0 +1,29 @@
/*
* utilite is a cross-platform library with
* useful utilities for fast and small developing.
* Copyright (C) 2010 Mathieu Labbe
*
* utilite is free library: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* utilite is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef UTHREADNODE_H_
#define UTHREADNODE_H_
#include "rtabmap/utilite/UThread.h"
//For backward compatibility
typedef UThread UThreadNode;
#endif /* UTHREADNODE_H_ */
rtabmap/utilite/include/rtabmap/utilite/UTimer.h
+109
View File
@@ -0,0 +1,109 @@
/*
* utilite is a cross-platform library with
* useful utilities for fast and small developing.
* Copyright (C) 2010 Mathieu Labbe
*
* utilite is free library: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* utilite is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef UTIMER_H
#define UTIMER_H
#include "rtabmap/utilite/utilite_export.h" // DLL export/import defines
#ifdef _WIN32
#include <windows.h>
#else
#include <sys/time.h>
#include <time.h>
#endif
/**
* This class is used to time some codes (in seconds).
* On Unix, the resolution is up to microseconds (see gettimeofday()).
* On Windows, the performance counter is used (see QueryPerformanceCounter() and QueryPerformanceFrequency()).
* Example:
* @code
* UTimer timer;
* timer.start();
* ... (do some work)
* timer.stop();
* double seconds = timer.getInterval();
* ...
* @endcode
*/
class UTILITE_EXPORT UTimer
{
public:
UTimer();
~UTimer();
/**
* This method is used to get
* the time of the system right now.
* @return double the time in seconds.
*/
static double now();
/**
* This method starts the timer.
*/
void start();
/**
* This method stops the timer.
*/
void stop();
/**
* This method is used to get the elapsed time
* between now and the start(). If timer is stopped, the interval time
* between stop() and the start() is returned.
* @return double the interval in seconds.
*/
double elapsed() {return getElapsedTime();}
double getElapsedTime();
/**
* This method is used to get the interval time
* between stop() and the start().
* @return double the interval in seconds.
* @deprecated use elapsed() instead.
*/
UTILITE_DEPRECATED double getInterval();
/**
* This method is used to get the interval of
* the timer while it is running. It's automatically
* stop the timer, get the interval and restart
* the timer. It's the same of calling stop(),
* elapsed() and start(). Method restart() does the same thing, for convenience.
* @return double the interval in seconds.
*/
double restart() {return ticks();}
double ticks();
private:
#ifdef _WIN32
LARGE_INTEGER startTimeRecorded_; /* When we start the timer, timeRecorded is copied over lastTimeRecorded.*/
LARGE_INTEGER stopTimeRecorded_; /* When we stop the timer. */
LARGE_INTEGER frequency_; /* Keep the frequency of the counter */
#else
struct timeval startTimeRecorded_; /* When we start the timer, timeRecorded is copied over lastTimeRecorded.*/
struct timeval stopTimeRecorded_; /* When we stop the timer. */
#endif
};
#endif //UTIMER_H
rtabmap/utilite/include/rtabmap/utilite/UVariant.h
+124
View File
@@ -0,0 +1,124 @@
/*
* utilite is a cross-platform library with
* useful utilities for fast and small developing.
* Copyright (C) 2010 Mathieu Labbe
*
* utilite is free library: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* utilite is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef UVARIANT_H
#define UVARIANT_H
#include "rtabmap/utilite/utilite_export.h" // DLL export/import defines
#include <string>
#include <vector>
/**
* Experimental class...
*/
class UTILITE_EXPORT UVariant
{
public:
enum Type{
kBool,
kChar,
kUChar,
kShort,
kUShort,
kInt,
kUInt,
kFloat,
kDouble,
kStr,
kCharArray,
kUCharArray,
kShortArray,
kUShortArray,
kIntArray,
kUIntArray,
kFloatArray,
kDoubleArray,
kUndef
};
public:
UVariant();
UVariant(const bool & value);
UVariant(const signed char & value);
UVariant(const unsigned char & value);
UVariant(const short & value);
UVariant(const unsigned short & value);
UVariant(const int & value);
UVariant(const unsigned int & value);
UVariant(const float & value);
UVariant(const double & value);
UVariant(const char * value);
UVariant(const std::string & value);
UVariant(const std::vector<signed char> & value);
UVariant(const std::vector<unsigned char> & value);
UVariant(const std::vector<short> & value);
UVariant(const std::vector<unsigned short> & value);
UVariant(const std::vector<int> & value);
UVariant(const std::vector<unsigned int> & value);
UVariant(const std::vector<float> & value);
UVariant(const std::vector<double> & value);
Type type() const {return type_;}
bool isUndef() const {return type_ == kUndef;}
bool isBool() const {return type_ == kBool;}
bool isChar() const {return type_ == kChar;}
bool isUChar() const {return type_ == kUChar;}
bool isShort() const {return type_ == kShort;}
bool isUShort() const {return type_ == kUShort;}
bool isInt() const {return type_ == kInt;}
bool isUInt() const {return type_ == kUInt;}
bool isFloat() const {return type_ == kFloat;}
bool isDouble() const {return type_ == kDouble;}
bool isStr() const {return type_ == kStr;}
bool isCharArray() const {return type_ == kCharArray;}
bool isUCharArray() const {return type_ == kUCharArray;}
bool isShortArray() const {return type_ == kShortArray;}
bool isUShortArray() const {return type_ == kUShortArray;}
bool isIntArray() const {return type_ == kIntArray;}
bool isUIntArray() const {return type_ == kUIntArray;}
bool isFloatArray() const {return type_ == kFloatArray;}
bool isDoubleArray() const {return type_ == kDoubleArray;}
bool toBool() const;
signed char toChar(bool * ok = 0) const;
unsigned char toUChar(bool * ok = 0) const;
short toShort(bool * ok = 0) const;
unsigned short toUShort(bool * ok = 0) const;
int toInt(bool * ok = 0) const;
unsigned int toUInt(bool * ok = 0) const;
float toFloat(bool * ok = 0) const;
double toDouble(bool * ok = 0) const;
std::string toStr(bool * ok = 0) const;
std::vector<signed char> toCharArray(bool * ok = 0) const;
std::vector<unsigned char> toUCharArray(bool * ok = 0) const;
std::vector<short> toShortArray(bool * ok = 0) const;
std::vector<unsigned short> toUShortArray(bool * ok = 0) const;
std::vector<int> toIntArray(bool * ok = 0) const;
std::vector<unsigned int> toUIntArray(bool * ok = 0) const;
std::vector<float> toFloatArray(bool * ok = 0) const;
std::vector<double> toDoubleArray(bool * ok = 0) const;
virtual ~UVariant() {}
private:
Type type_;
std::vector<unsigned char> data_;
};
#endif /* UVARIANT_H */
rtabmap/utilite/include/rtabmap/utilite/UtiLite.h
+253
View File
@@ -0,0 +1,253 @@
/*
* utilite is a cross-platform library with
* useful utilities for fast and small developing.
* Copyright (C) 2010 Mathieu Labbe
*
* utilite is free library: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* utilite is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef UTILITE_H
#define UTILITE_H
/** \mainpage UtiLite
*
* \section intro Introduction
* <a href="http://utilite.googlecode.com">UtiLite</a> is a simple library to create small cross-platform
* applications using <b>threads</b>, <b>events-based communication</b> and a powerful <b>logger</b>. The first three
* sections show the core classes of the library, then last sections show some useful functions added through time.
*
* UtiLite provides a utility application called \ref uResourceGeneratorPage "uResourceGenerator" to generate resources to include in an executable. For example:
* @code
* $ ./uresourcegenerator DatabaseSchema.sql
* @endcode
* This will generate a HEX file "DatabaseSchema_sql.h" which can be included in source files.
* Data of the file is global and can be accessed by the generated const char * DATABASESCHEMA_SQL.
* @code
* #include "DatabaseSchema_sql.h"
* ...
* std::string hex = DATABASESCHEMA_SQL;
* // Assuming there are only ASCII characters, we can directly convert to a string:
* std::string schema = uHex2Str(hex);
* // For binary data:
* std::vector<char> bytes = uHex2Bytes(hex);
* @endcode
*
* A generated \ref findUtilitePage "FindUtiLite.cmake" is also provided for easy linking with the library.
*
*
* \section logger ULogger
* The ULogger can be used anywhere in the application to log messages (formated like a printf). The
* logger can be set (ULogger::setType()) to print in a file or in the console (with colors depending on the severity). Convenient
* macros are given, working like a printf(), as UDEBUG(), UINFO(), UWARN(), UERROR(), UFATAL(), UASSERT(). Small example:
* @code
* ...
* UINFO("A simple message with number %d", 42);
* UDEBUG("A debug message");
* ...
* @endcode
* This will print: [Severity] (Time) File:Line:Function() "The message"
* @code
* [ INFO] (2010-09-25 18:08:20) main.cpp:18::main() A simple message with number 42
* [DEBUG] (2010-09-25 18:08:20) main.cpp:18::main() A debug message
* @endcode
*
* \section eventsmanager UEventsManager, UEventsHandler, UEvent
* The events-based communication framework helps to communicate between objects/threads.
* The UEventsManager is a singleton with which we can post events anywhere in the
* application by calling UEventsManager::post(). All UEventsHandler will then receive the
* event with their protected function UEventsHandler::handleEvent(). Handlers are added to UEventsManager by
* calling UEventsManager::addHandler(). The UEvent provides an abstract class to implement any event
* implementations. The only requirement is that the event must implements UEvent::getClassName() to know the event's type.
* @code
* ...
* MyHandler handler; // MyHandler is an implementation of UEventsHandler
* UEventsManager::addHandler(&handler);
* UEventsManager::post(new MyEvent()); // MyEvent is an implementation of UEvent
* // The UEventsHandler::handleEvent() of "handler" will then be called by the UEventsManager's events dispatching thread.
* ...
* @endcode
* Look at the <b>full example</b> in page of UEventsHandler on how communication works with threads (UThread).
*
* \section thread UThread, UMutex, USemaphore
* The multi-threading framework use a UThread as an abstract class to implement a
* thread in object-style. Reimplement UThread::mainLoop() then call UThread::start() to
* start the main loop of the thread. Threads can be joined by calling UThread::join() and
* killed by calling UThread::kill(). Classes UMutex and USemaphore provide blocking mechanisms to
* protect data between threads.
* @code
* ...
* MyThread t; // MyThread is an implementation of UThread
* t.start();
* t.join(); // Wait the thread to finish
* ...
* @endcode
*
* \section timer UTimer
* A useful class to compute processing time:
* - UTimer::start(),
* - UTimer::stop(),
* - UTimer::restart(),
* - UTimer::elapsed(),
* - UTimer::now().
*
* \section filedir UDirectory, UFile
* For files and directories manipulations :
* - UFile::exists(),
* - UFile::length(),
* - UFile::rename(),
* - UFile::erase(),
* - UDirectory::exists(),
* - UDirectory::getFileNames(),
* - UDirectory::makeDir(),
* - UDirectory::removeDir(),
* - UDirectory::currentDir(),
* - UDirectory::homeDir(),
*
* \section stl Convenient use of STL
* The library provides some simple wrappers of the STL like:
* - uUniqueKeys() to get unique keys from a std::multimap,
* - uKeys() to get all keys of a std::multimap or std::map,
* - uValues() to get all values of a std::multimap or std::map,
* - uValue() to get the value of a key (with a default argument if the key is not found),
* - uTake() to take the value of a key (with a default argument if the key is not found),
* - uIteratorAt() to get iterator at a specified position in a std::list,
* - uValueAt() to get value at a specified position in a std::list,
* - uContains() to know if a key/value exists in a std::multimap, std::map, std::list,
* - uInsert() to insert a value in a std::map and overwriting the value if the key already exists,
* - uListToVector(),
* - uVectorToList(),
* - uAppend() to append a list to another list,
* - uIndexOf() to get index of a value in a std::list,
* - uSplit() to split a string into a std::list of strings on the specified separator.
*
*
* \section math Basic mathematic operations
* A library of basic array manipulations:
* - uMax(),
* - uSign(),
* - uSum(),
* - uMean(),
* - uStdDev(),
* - uNorm(),
* - uNormalize(),
* - uXMatch().
*
* \section conversion Conversion
* A library of convenient functions to convert some data into another like:
* - uReplaceChar(),
* - uToUpperCase(),
* - uToLowerCase(),
* - uNumber2Str(),
* - uBool2Str(),
* - uStr2Bool(),
* - uBytes2Hex(),
* - uHex2Bytes(),
* - uHex2Bytes(),
* - uHex2Str(),
* - uHex2Ascii(),
* - uAscii2Hex(),
* - uFormatv(),
* - uFormat().
*
* \section processinfo UProcessInfo
* This class can be used to get the process memory usage: UProcessInfo::getMemoryUsage().
*
* \section qtLib Qt Widgets (libutilite_qt.so : OPTIONAL)
* If Qt is found on the system, the UtiLite Qt library (libutilite_qt.so, libutilite_qt.dll) with
* useful widgets is built. Use class UPlot to create a plot like MATLAB, and incrementally add
* new values like a scope. USpectrogram is used to
* show audio frequency frames.
* - UPlot,
* - USpectrogram,
* - UImageView.
* @image html UPlot.gif
* @image html USpectrogram.png
*
* \section audioLib Audio stuff (libutilite_audio.so : OPTIONAL)
* If FMOD is found on the system, the UtiLite audio
* library is built (libutilite_audio.so, libutilite_audio.dll). It is a wrapper
* of FMOD methods with a convenient interface to extract audio frames.
* - UAudioCapture,
* - UAudioCaptureFile,
* - UAudioCaptureMic,
* - UAudioCaptureFFT,
* - UAudioPlayer,
* - UAudioPlayerTone,
* - UWav,
* - UMp3Encoder (only if Lame is also found on the system).
*
* \section cvLib OpenCV stuff (libutilite_cv.so : OPTIONAL)
* If OpenCV is found on the system, the UtiLite cv
* library is built (libutilite_cv.so, libutilite_cv.dll). It provides
* image capture classes used to read from a webcam, a video file
* or a directory of images. If UtiLite is also built with Qt, a
* convenient function uCvMat2QImage() can be used to convert a cv::Mat
* image to a QImage.
* - UVideoCapture,
* - UImageFolderCapture,
* - UColorTable,
* - uCvMat2QImage() (only if Qt is also found on the system).
*/
/*! \page uResourceGeneratorPage uResourceGenerator
* UtiLite provides a utility application called \ref uResourceGeneratorPage "uResourceGenerator" to generate resources to include in an executable. For example:
* @code
* $ ./uresourcegenerator DatabaseSchema.sql
* @endcode
* This will generate a HEX file "DatabaseSchema_sql.h" which can be included in source files.
* Data of the file is global and can be accessed by the generated const char * DATABASESCHEMA_SQL.
* @code
* #include "DatabaseSchema_sql.h"
* ...
* std::string hex = DATABASESCHEMA_SQL;
* // Assuming there are only ASCII characters, we can directly convert to a string:
* std::string schema = uHex2Str(hex);
* // For binary data:
* std::vector<char> bytes = uHex2Bytes(hex);
* @endcode
*
* The generator can be automated in a CMake build like:
* @code
* ADD_CUSTOM_COMMAND(
* OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/DatabaseSchema_sql.h
* COMMAND ${URESOURCEGENERATOR_EXEC} -n my_namespace -p ${CMAKE_CURRENT_BINARY_DIR} ${CMAKE_CURRENT_SOURCE_DIR}/DatabaseSchema.sql
* COMMENT "[Creating database resource]"
* DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/DatabaseSchema.sql
* )
* SET(RESOURCES
* ${CMAKE_CURRENT_BINARY_DIR}/DatabaseSchema_sql.h
* )
* ADD_LIBRARY(mylib ${SRC_FILES} ${RESOURCES})
* ADD_EXECUTABLE(myexecutable ${SRC_FILES} ${RESOURCES})
* @endcode
* The variable URESOURCEGENERATOR_EXEC is set when FIND_PACKAGE(UtiLite) is done, you would need to add \ref findUtilitePage "FindUtiLite.cmake".
*/
#include "rtabmap/utilite/UStl.h"
#include "rtabmap/utilite/UConversion.h"
#include "rtabmap/utilite/UDirectory.h"
#include "rtabmap/utilite/UFile.h"
#include "rtabmap/utilite/ULogger.h"
#include "rtabmap/utilite/UEventsManager.h"
#include "rtabmap/utilite/UEventsHandler.h"
#include "rtabmap/utilite/UEvent.h"
#include "rtabmap/utilite/UProcessInfo.h"
#include "rtabmap/utilite/UMutex.h"
#include "rtabmap/utilite/USemaphore.h"
#include "rtabmap/utilite/UThreadNode.h"
#include "rtabmap/utilite/UTimer.h"
#include "rtabmap/utilite/UVariant.h"
#include "rtabmap/utilite/UMath.h"
#endif /* UTILITE_H */
rtabmap/utilite/include/rtabmap/utilite/Win32/UThreadC.h
+385
View File
@@ -0,0 +1,385 @@
/////////////////////////////////////////////////////////////////////
// Written by Phillip Sitbon
// Copyright 2003
//
// Modified by Mathieu Labbe
//
// Win32/Thread.h
// - Windows thread
//
// - From CreateThread Platform SDK Documentation:
//
// "A thread that uses functions from the static C run-time
// libraries should use the beginthread and endthread C run-time
// functions for thread management rather than CreateThread and
// ExitThread. Failure to do so results in small memory leaks
// when ExitThread is called. Note that this is not a problem
// with the C run-time in a DLL."
//
// With regards to this, I have decided to use the CreateThread
// API, unless you define _CRT_ in which case there are two
// possibilities:
//
// 1. Define _USE_BEGINTHREAD: Uses _beginthread/_endthread
// (said to be *unreliable* in the SDK docs)
//
// 2. Don't - Uses _beginthreaded/_endthreadex
//
// A note about _endthread:
//
// It will call CloseHandle() on exit, and if it was already
// closed then you will get an exception. To prevent this, I
// removed the CloseHandle() functionality - this means that
// a Join() WILL wait on a Detach()'ed thread.
//
/////////////////////////////////////////////////////////////////////
#ifndef _U_Thread_Win32_
#define _U_Thread_Win32_
#include "rtabmap/utilite/utilite_export.h"
#include "rtabmap/utilite/Win32/UWin32.h"
#include "rtabmap/utilite/USemaphore.h"
#include "rtabmap/utilite/UMutex.h"
inline void uSleep(unsigned int ms)
{
Sleep(ms);
}
#ifdef _CRT_
# include <process.h>
# ifdef _USE_BEGINTHREAD
# define THREAD_CALL __cdecl
# define THREAD_HANDLE uintptr_t
# define THREAD_RET_T void
# define CREATE_THREAD_FAILED (-1L)
# define CREATE_THREAD_ERROR (errno)
# define CREATE_THREAD(_S,_F,_P) ((Handle)_beginthread((void (__cdecl *)(void *))_F,_S,(void *)_P))
# define EXIT_THREAD _endthread()
# define CLOSE_HANDLE(x) 1
# define THREAD_RETURN(x) return
# else
# define THREAD_CALL WINAPI
# define THREAD_HANDLE HANDLE
# define THREAD_RET_T UINT
# define CREATE_THREAD_FAILED (0L)
# define CREATE_THREAD_ERROR (errno)
# define CREATE_THREAD(_S,_F,_P) ((Handle)_beginthreadex(0,_S,(UINT (WINAPI *)(void *))_F,(void *)_P,0,0))
# define EXIT_THREAD _endthreadex(0)
# define CLOSE_HANDLE(x) CloseHandle(x)
# define THREAD_RETURN(x) return(x)
# endif
#else
# define THREAD_CALL WINAPI
# define THREAD_HANDLE HANDLE
# define THREAD_RET_T DWORD
# define CREATE_THREAD_FAILED (0L)
# define CREATE_THREAD_ERROR GetLastError()
# define CREATE_THREAD(_S,_F,_P) ((Handle)CreateThread(0,_S,(DWORD (WINAPI *)(void *))_F,(void *)_P,0,0))
# define CREATE_THREAD2(_S,_F,_P,_ID) ((Handle)CreateThread(0,_S,(DWORD (WINAPI *)(void *))_F,(void *)_P,0,_ID))
# define EXIT_THREAD ExitThread(0)
# define CLOSE_HANDLE(x) CloseHandle(x)
# define THREAD_RETURN(x) return(x)
#endif
#define InvalidHandle 0
template
<
typename Thread_T
>
class UTILITE_EXPORT UThreadC
{
private:
struct Instance;
public:
typedef Thread_T & Thread_R;
typedef const Thread_T & Thread_C_R;
typedef THREAD_HANDLE Handle;
typedef void (* Handler)( Thread_R );
protected:
UThreadC() {}
virtual void ThreadMain( Thread_R ) = 0;
static void Exit()
{ EXIT_THREAD; }
static void TestCancel()
{ Sleep(0); }
static int Self()
{
Handle Hnd = InvalidHandle;
DuplicateHandle(GetCurrentProcess(),GetCurrentThread(),GetCurrentProcess(),(LPHANDLE)&Hnd,NULL,0,NULL);
return Hnd;
// only a pseudo-handle!
//return (Handle)GetCurrentThread();
}
public:
static int Create(
const Handler & Function,
Thread_C_R Param,
Handle * const & H = 0,
const bool & CreateDetached = false,
const unsigned int & StackSize = 0,
const bool & CancelEnable = false, // UNUSED
const bool & CancelAsync = false // UNUSED
)
{
M_Create().lock();
Instance I(Param,0,Function);
Handle Hnd(CREATE_THREAD(StackSize,ThreadMainHandler,&I));
if ( Hnd == CREATE_THREAD_FAILED )
{
if ( H ) *H = InvalidHandle;
M_Create().unlock();
return CREATE_THREAD_ERROR;
}
if ( H ) *H = Hnd;
S_Create().Wait();
M_Create().unlock();
if ( CreateDetached ) CLOSE_HANDLE(Hnd);
return 0;
}
int Create(
Thread_C_R Param,
Handle * const & H = 0,
const bool & CreateDetached = false,
const unsigned int & StackSize = 0,
const bool & CancelEnable = false, // UNUSED
const bool & CancelAsync = false // UNUSED
) const
{
M_Create().lock();
Instance I(Param,const_cast<UThreadC *>(this));
Handle Hnd(CREATE_THREAD(StackSize,ThreadMainHandler,&I));
if ( Hnd == CREATE_THREAD_FAILED )
{
if ( H ) *H = InvalidHandle;
M_Create().unlock();
return CREATE_THREAD_ERROR;
}
if ( H ) *H = Hnd;
S_Create().Wait();
M_Create().unlock();
if ( CreateDetached ) CLOSE_HANDLE(Hnd);
return 0;
}
static int Join( const Handle &H )
{
DWORD R = WaitForSingleObject((HANDLE)H,INFINITE);
if ( (R == WAIT_OBJECT_0) || (R == WAIT_ABANDONED) )
{
CLOSE_HANDLE(H);
return 0;
}
if ( R == WAIT_TIMEOUT ) return EAGAIN;
return EINVAL;
}
static int Kill( const Handle &H )
{ return TerminateThread((HANDLE)H,0) ? 0 : EINVAL; }
static int Detach( const Handle &H )
{ return (CLOSE_HANDLE(H)?0:EINVAL); }
private:
static const UMutex &M_Create() { static UMutex M; return M; }
static const USemaphore &S_Create() { static USemaphore S; return S; }
static THREAD_RET_T THREAD_CALL ThreadMainHandler( Instance *Param )
{
Instance I(*Param);
Thread_T Data(I.Data);
S_Create().Post();
if ( I.Owner )
I.Owner->ThreadMain(Data);
else
I.pFN(Data);
Exit();
THREAD_RETURN(0);
}
struct Instance
{
Instance( Thread_C_R P, UThreadC<Thread_T> *const &O, const typename UThreadC<Thread_T>::Handler &pH = 0 )
: pFN(pH), Data(P), Owner(O) {}
typename UThreadC<Thread_T>::Handler pFN;
typename UThreadC<Thread_T>::Thread_C_R Data;
UThreadC<Thread_T> * Owner;
};
};
/////////////////////////////////////////////////////////////////////
// Explicit Specialization of void
//
template<>
class UTILITE_EXPORT UThreadC<void>
{
private:
struct Instance;
public:
typedef THREAD_HANDLE Handle;
typedef void ( *Handler)();
virtual ~UThreadC<void>() {}
protected:
UThreadC<void>() {}
virtual void ThreadMain() = 0;
static void Exit()
{ EXIT_THREAD; }
static void TestCancel()
{ Sleep(0); }
static int Self()
{
return (int)GetCurrentThreadId();
//Handle Hnd = InvalidHandle;
//DuplicateHandle(GetCurrentProcess(),GetCurrentThread(),GetCurrentProcess(),(LPHANDLE)&Hnd,NULL,0,NULL);
//return Hnd;
// only a pseudo-handle!
//return (Handle)GetCurrentThread();
}
public:
static int Create(
const Handler & Function,
Handle * const & H = 0,
const bool & CreateDetached = false,
const unsigned int & StackSize = 0,
const bool & CancelEnable = false, // UNUSED
const bool & CancelAsync = false // UNUSED
)
{
Handle Hnd(CREATE_THREAD(StackSize,ThreadMainHandler_S,Function));
if ( Hnd == CREATE_THREAD_FAILED )
{
if ( H ) *H = InvalidHandle;
return (int)CREATE_THREAD_ERROR;
}
if ( H ) *H = Hnd;
if ( CreateDetached ) CLOSE_HANDLE(Hnd);
return 0;
}
int Create(
Handle * const & H = 0,
const bool & CreateDetached = false,
const unsigned int & StackSize = 0,
const bool & CancelEnable = false, // UNUSED
const bool & CancelAsync = false // UNUSED
) const
{
Handle Hnd(CREATE_THREAD(StackSize,ThreadMainHandler,this));
if ( Hnd == CREATE_THREAD_FAILED )
{
if ( H ) *H = InvalidHandle;
return (int)CREATE_THREAD_ERROR;
}
if ( H ) *H = Hnd;
if ( CreateDetached ) CLOSE_HANDLE(Hnd);
Self();
return 0;
}
int Create(
unsigned long & ThreadId,
Handle * const & H = 0,
const bool & CreateDetached = false,
const unsigned int & StackSize = 0,
const bool & CancelEnable = false, // UNUSED
const bool & CancelAsync = false // UNUSED
) const
{
*H = InvalidHandle;
int id;
*H = CREATE_THREAD2(StackSize,ThreadMainHandler,this, (LPDWORD)&id);
ThreadId = (unsigned long)id;
if ( *H == CREATE_THREAD_FAILED )
{
*H = InvalidHandle;
return (int)CREATE_THREAD_ERROR;
}
if ( CreateDetached ) CLOSE_HANDLE(*H);
return 0;
}
static int Join( const Handle &H )
{
DWORD R = WaitForSingleObject((HANDLE)H,INFINITE);
if ( (R == WAIT_OBJECT_0) || (R == WAIT_ABANDONED) )
{
CLOSE_HANDLE(H);
return 0;
}
if ( R == WAIT_TIMEOUT ) return EAGAIN;
return EINVAL;
}
static int Kill( const Handle &H )
{ return TerminateThread((HANDLE)H,0) ? 0 : EINVAL; }
static int Detach( const Handle &H )
{ return (CLOSE_HANDLE(H)?0:EINVAL); }
private:
static THREAD_RET_T THREAD_CALL ThreadMainHandler( UThreadC<void> *Param )
{
Param->ThreadMain();
Exit();
THREAD_RETURN(0);
}
static THREAD_RET_T THREAD_CALL ThreadMainHandler_S( Handler Param )
{
Param();
Exit();
THREAD_RETURN(0);
}
};
#endif // !_U_Thread_Win32_
rtabmap/utilite/include/rtabmap/utilite/Win32/UWin32.h
+62
View File
@@ -0,0 +1,62 @@
/////////////////////////////////////////////////////////////////////
// Written by Phillip Sitbon
// Copyright 2003
//
// Win32.h
// - Windows includes
//
/////////////////////////////////////////////////////////////////////
#ifndef _U_Win32_
#define _U_Win32_
#if !defined(_WINDOWS_)
// WIN32 Excludes
#ifdef WIN32_LEAN_AND_MEAN
# define VC_EXTRALEAN
# define WIN32_LEAN_AND_MEAN
# define _PRSHT_H_
# define NOGDICAPMASKS // CC_*, LC_*, PC_*, CP_*, TC_*, RC_
# define NOVIRTUALKEYCODES // VK_*
# define NOWINMESSAGES // WM_*, EM_*, LB_*, CB_*
# define NOWINSTYLES // WS_*, CS_*, ES_*, LBS_*, SBS_*, CBS_*
# define NOSYSMETRICS // SM_*
# define NOMENUS // MF_*
# define NOICONS // IDI_*
# define NOKEYSTATES // MK_*
# define NOSYSCOMMANDS // SC_*
# define NORASTEROPS // Binary and Tertiary raster ops
# define NOSHOWWINDOW // SW_*
# define OEMRESOURCE // OEM Resource values
# define NOATOM // Atom Manager routines
# define NOCLIPBOARD // Clipboard routines
# define NOCOLOR // Screen colors
# define NOCTLMGR // Control and Dialog routines
# define NODRAWTEXT // DrawText() and DT_*
# define NOGDI // All GDI defines and routines
# define NOKERNEL // All KERNEL defines and routines
# define NOUSER // All USER defines and routines
# define NONLS // All NLS defines and routines
# define NOMB // MB_* and MessageBox()
# define NOMEMMGR // GMEM_*, LMEM_*, GHND, LHND, associated routines
# define NOMETAFILE // typedef METAFILEPICT
# define NOMINMAX // Macros min(a,b) and max(a,b)
# define NOMSG // typedef MSG and associated routines
# define NOOPENFILE // OpenFile(), OemToAnsi, AnsiToOem, and OF_*
# define NOSCROLL // SB_* and scrolling routines
# define NOSERVICE // All Service Controller routines, SERVICE_ equates, etc.
# define NOSOUND // Sound driver routines
# define NOTEXTMETRIC // typedef TEXTMETRIC and associated routines
# define NOWH // SetWindowsHook and WH_*
# define NOWINOFFSETS // GWL_*, GCL_*, associated routines
# define NOCOMM // COMM driver routines
# define NOKANJI // Kanji support stuff.
# define NOHELP // Help engine interface.
# define NOPROFILER // Profiler interface.
# define NODEFERWINDOWPOS // DeferWindowPos routines
# define NOMCX // Modem Configuration Extensions
#endif // WIN32_LEAN_AND_MEAN
//
# include <windows.h>
#endif
#endif // !_U_Win32_
rtabmap/utilite/resource_generator/CMakeLists.txt
+62
View File
@@ -0,0 +1,62 @@
if (CMAKE_CROSSCOMPILING OR ANDROID OR IOS)
# See this page about tools being required in the build:
# https://gitlab.kitware.com/cmake/community/-/wikis/doc/cmake/CrossCompiling#using-executables-in-the-build-created-during-the-build
# Some ideas there were used, not all.
# The target named 'res_tool' can be used elsewhere in all cases, when cross compiling or not.
IF(NOT RTABMAP_RES_TOOL)
IF (COMMAND find_host_program)
# On android docker build
MESSAGE(STATUS "Looking for ${PROJECT_PREFIX}-res tool in ${PROJECT_BINARY_DIR}/../bin")
FIND_HOST_PROGRAM( RTABMAP_RES_TOOL ${PROJECT_PREFIX}-res_tool PATHS ${PROJECT_BINARY_DIR}/../bin NO_DEFAULT_PATH)
IF(NOT RTABMAP_RES_TOOL)
MESSAGE(STATUS "Looking for ${PROJECT_PREFIX}-res tool on host system")
FIND_HOST_PROGRAM( RTABMAP_RES_TOOL ${PROJECT_PREFIX}-res_tool)
ENDIF(NOT RTABMAP_RES_TOOL)
ELSE()
MESSAGE(STATUS "Looking for ${PROJECT_PREFIX}-res tool on host system")
FIND_PROGRAM( RTABMAP_RES_TOOL ${PROJECT_PREFIX}-res_tool )
ENDIF()
IF(NOT RTABMAP_RES_TOOL)
MESSAGE( FATAL_ERROR "RTABMAP_RES_TOOL is not defined (it is the path to \"${PROJECT_PREFIX}-res_tool\" application created by a non-Android build)." )
ENDIF(NOT RTABMAP_RES_TOOL)
ENDIF(NOT RTABMAP_RES_TOOL)
MESSAGE(STATUS "Using res_tool at ${RTABMAP_RES_TOOL}")
ADD_EXECUTABLE(res_tool IMPORTED GLOBAL)
SET_TARGET_PROPERTIES(res_tool PROPERTIES IMPORTED_LOCATION "${RTABMAP_RES_TOOL}")
else()
SET(SRC_FILES
main.cpp
)
SET(INCLUDE_DIRS
../include
)
# Make sure the compiler can find include files from our library.
INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
# Add binary called "resource_tool" that is built from the source file "main.cpp".
# The extension is automatically found.
ADD_EXECUTABLE(res_tool ${SRC_FILES})
TARGET_LINK_LIBRARIES(res_tool rtabmap_utilite)
SET_TARGET_PROPERTIES(
res_tool
PROPERTIES
VERSION ${UTILITE_VERSION}
SOVERSION ${UTILITE_VERSION}
OUTPUT_NAME ${PROJECT_PREFIX}-res_tool
)
INSTALL(TARGETS res_tool
RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT runtime
BUNDLE DESTINATION "${CMAKE_BUNDLE_LOCATION}" COMPONENT runtime)
endif()
rtabmap/utilite/resource_generator/main.cpp
+170
View File
@@ -0,0 +1,170 @@
/*
* utilite is a cross-platform library with
* useful utilities for fast and small developing.
* Copyright (C) 2010 Mathieu Labbe
*
* utilite is free library: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* utilite is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include "rtabmap/utilite/UtiLite.h"
#include <fstream>
#include <iostream>
#include <string.h>
void showUsage()
{
printf("Usage:\n"
"uresourcegenerator.exe [option] \"file1\" \"file2\" ... \n"
" Create a file named \"file\".h with string\n"
" variable named \"file\" which contains the data of the file.\n"
" Warning, it overwrites the target file\n"
" Options:\n"
" -n \"namespace\" namespace used\n"
" -p \"targetPath\" target path where the file is created\n"
" -v version of the UtiLite library\n");
exit(1);
}
int main(int argc, char * argv[])
{
if(argc < 2)
{
showUsage();
}
else if(argc == 2 && strcmp(argv[1], "-v") == 0)
{
printf("%s\n", UTILITE_VERSION);
exit(0);
}
std::string targetDir = UDirectory::currentDir(); // By default, use the current directory
std::string nspace; // namespace
int k;
for(k=1; k<(argc-1); ++k)
{
if(strcmp(argv[k], "-n") == 0)
{
if(!(k+1<(argc-1)))
{
showUsage();
}
nspace = argv[k+1];
printf(" Using namespace=%s\n", nspace.c_str());
++k;
}
else if(strcmp(argv[k], "-p") == 0)
{
if(!(k+1<(argc-1)))
{
showUsage();
}
targetDir = argv[k+1];
printf(" Using target directory=%s\n", targetDir.c_str());
++k;
}
else
{
break;
}
}
while(k < argc)
{
std::string filePath = argv[k];
std::string varName = UFile::getName(argv[k]);
// replace '_'
for(unsigned int i=0; i<varName.size(); ++i)
{
if(!((varName[i] >= '0' && varName[i] <= '9') ||
(varName[i] >= 'A' && varName[i] <= 'Z') ||
(varName[i] >= 'a' && varName[i] <= 'z')))
{
varName[i] = '_';
}
}
std::string targetFileName = varName + ".h";
// upper case
for(unsigned int i=0; i<varName.size(); ++i)
{
if(varName[i] >= 'a' && varName[i] <= 'z')
{
varName[i] -= 32; // upper case
}
}
std::fstream outFile;
std::fstream inFile;
outFile.open(((targetDir + "/") + targetFileName).c_str(), std::fstream::out);
inFile.open(filePath.c_str(), std::fstream::in | std::fstream::binary);
printf("Input file \"%s\" size = %ld bytes\n", filePath.c_str(), UFile::length(filePath));
if(outFile.is_open() && inFile.is_open())
{
outFile << "/*This is a generated file...*/\n\n";
outFile << "#ifndef " << varName << "_H\n";
outFile << "#define " << varName << "_H\n\n";
if(!nspace.empty())
{
outFile << "namespace " << nspace.c_str() << "\n{\n\n";
}
outFile << "static const char * " << varName.c_str() << " = ";
if(!inFile.good())
{
outFile << "\"\""; //empty string
}
else
{
std::string startLine = "\n \"";
std::string endLine = "\"";
std::vector<char> buffer(1024);
while(inFile.good())
{
inFile.read(buffer.data(), 1024);
std::streamsize count = inFile.gcount();
if(count)
{
outFile.write(startLine.c_str(), startLine.size());
std::string hex = uBytes2Hex(buffer.data(), count);
outFile.write(hex.c_str(), hex.size());
outFile.write(endLine.c_str(), endLine.size());
}
}
}
std::string endOfVar = ";\n\n";
outFile.write(endOfVar.c_str(), endOfVar.size());
if(!nspace.empty())
{
outFile << "}\n\n";
}
outFile << "#endif //" << varName << "_H\n\n";
}
outFile.close();
inFile.close();
printf("Output file \"%s\" size = %ld bytes\n", ((targetDir + "/") + targetFileName).c_str(), UFile::length(((targetDir + "/") + targetFileName).c_str()));
++k;
}
}
rtabmap/utilite/src/CMakeLists.txt
+74
View File
@@ -0,0 +1,74 @@
SET(SRC_FILES
UEventsManager.cpp
UEventsHandler.cpp
UEventsSender.cpp
UFile.cpp
UDirectory.cpp
UConversion.cpp
ULogger.cpp
UThread.cpp
UTimer.cpp
UProcessInfo.cpp
UVariant.cpp
)
ADD_LIBRARY(rtabmap_utilite ${SRC_FILES})
ADD_LIBRARY(rtabmap::utilite ALIAS rtabmap_utilite)
generate_export_header(rtabmap_utilite
BASE_NAME utilite)
target_include_directories(rtabmap_utilite PUBLIC
"$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/../include;${CMAKE_CURRENT_BINARY_DIR}/include;${PTHREADS_INCLUDE_DIR}>"
"$<INSTALL_INTERFACE:${INSTALL_INCLUDE_DIR};${PTHREADS_INCLUDE_DIR}>")
IF(MINGW)
TARGET_LINK_LIBRARIES(rtabmap_utilite PRIVATE ${PTHREADS_LIBRARY} "-lpsapi")
ELSEIF(WIN32 OR MSVC)
FIND_LIBRARY(PSAPI_LIBRARIES NAMES psapi libpsapi.dll.a libpsapi.a libpsapi.lib )
TARGET_LINK_LIBRARIES(rtabmap_utilite PRIVATE ${PSAPI_LIBRARIES})
ELSE()
TARGET_LINK_LIBRARIES(rtabmap_utilite PRIVATE ${PTHREADS_LIBRARY})
ENDIF()
SET_TARGET_PROPERTIES(
rtabmap_utilite
PROPERTIES
VERSION ${RTABMAP_VERSION}
SOVERSION ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}
EXPORT_NAME "utilite"
)
INSTALL(TARGETS rtabmap_utilite EXPORT rtabmap_utiliteTargets
RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT runtime
LIBRARY DESTINATION "${CMAKE_INSTALL_LIBDIR}" COMPONENT devel
ARCHIVE DESTINATION "${CMAKE_INSTALL_LIBDIR}" COMPONENT devel)
configure_file(
${CMAKE_CURRENT_BINARY_DIR}/utilite_export.h
${CMAKE_CURRENT_BINARY_DIR}/include/${PROJECT_PREFIX}/utilite/utilite_export.h
COPYONLY)
install(
DIRECTORY
${CMAKE_CURRENT_SOURCE_DIR}/../include/${PROJECT_PREFIX}
${CMAKE_CURRENT_BINARY_DIR}/include/${PROJECT_PREFIX}
DESTINATION
"${INSTALL_INCLUDE_DIR}"
COMPONENT
devel
FILES_MATCHING PATTERN "*.h" PATTERN "*.hpp")
export(EXPORT rtabmap_utiliteTargets
FILE "${CMAKE_CURRENT_BINARY_DIR}/../../${PROJECT_NAME}_utiliteTargets.cmake"
NAMESPACE rtabmap::
)
install(EXPORT rtabmap_utiliteTargets
FILE
${PROJECT_NAME}_utiliteTargets.cmake
DESTINATION
${INSTALL_CMAKE_DIR}
NAMESPACE rtabmap::
COMPONENT
devel
)
rtabmap/utilite/src/UConversion.cpp
+392
View File
@@ -0,0 +1,392 @@
/*
* utilite is a cross-platform library with
* useful utilities for fast and small developing.
* Copyright (C) 2010 Mathieu Labbe
*
* utilite is free library: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* utilite is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include "rtabmap/utilite/UConversion.h"
#include "rtabmap/utilite/UStl.h"
#include <sstream>
#include <string.h>
#include <stdio.h>
#include <stdlib.h>
#include <iomanip>
#ifdef _WIN32
#include <windows.h>
#endif
std::string uReplaceChar(const std::string & str, char before, char after)
{
std::string result = str;
for(unsigned int i=0; i<result.size(); ++i)
{
if(result[i] == before)
{
result[i] = after;
}
}
return result;
}
std::string uReplaceChar(const std::string & str, char before, const std::string & after)
{
std::string s;
for(unsigned int i=0; i<str.size(); ++i)
{
if(str.at(i) != before)
{
s.push_back(str.at(i));
}
else
{
s.append(after);
}
}
return s;
}
std::string uToUpperCase(const std::string & str)
{
std::string result = str;
for(unsigned int i=0; i<result.size(); ++i)
{
// only change case of ascii characters ('a' to 'z')
if(result[i] >= 'a' && result[i]<='z')
{
result[i] = result[i] - 'a' + 'A';
}
}
return result;
}
std::string uToLowerCase(const std::string & str)
{
std::string result = str;
for(unsigned int i=0; i<result.size(); ++i)
{
// only change case of ascii characters ('A' to 'Z')
if(result[i] >= 'A' && result[i]<='Z')
{
result[i] = result[i] - 'A' + 'a';
}
}
return result;
}
std::string uNumber2Str(unsigned int number)
{
std::stringstream s;
s << number;
return s.str();
}
std::string uNumber2Str(int number)
{
std::stringstream s;
s << number;
return s.str();
}
std::string uNumber2Str(float number, int precision, bool fixed)
{
std::stringstream s;
s << std::setprecision(precision);
if(fixed)
s << std::fixed;
s << number;
return s.str();
}
std::string uNumber2Str(double number, int precision, bool fixed)
{
std::stringstream s;
s << std::setprecision(precision);
if(fixed)
s << std::fixed;
s << number;
return s.str();
}
int uStr2Int(const std::string & str)
{
if(uStrContains(uToLowerCase(str), "true"))
{
return 1;
}
else if(uStrContains(uToLowerCase(str), "false"))
{
return 0;
}
return atoi(str.c_str());
}
float uStr2Float(const std::string & str)
{
float value = 0.0f;
std::istringstream istr(uReplaceChar(str, ',', '.').c_str());
istr.imbue(std::locale("C"));
istr >> value;
return value;
}
double uStr2Double(const std::string & str)
{
double value = 0.0;
std::istringstream istr(uReplaceChar(str, ',', '.').c_str());
istr.imbue(std::locale("C"));
istr >> value;
return value;
}
std::string uBool2Str(bool boolean)
{
std::string s;
if(boolean)
{
s = "true";
}
else
{
s = "false";
}
return s;
}
bool uStr2Bool(const char * str)
{
return !(str && (uStrContains(uToLowerCase(str), "false") || strcmp(str, "0") == 0));
}
bool uStr2Bool(const std::string & str)
{
return !(uStrContains(uToLowerCase(str), "false") || str.compare("0") == 0);
}
std::vector<unsigned char> uStr2Bytes(const std::string & str)
{
std::vector<unsigned char> bytes(str.size()+1);
memcpy(bytes.data(), str.data(), str.size());
bytes[bytes.size()-1] = '\0'; // null character
return bytes;
}
std::string uBytes2Str(const std::vector<unsigned char> & bytes)
{
if(bytes.size())
{
if(bytes[bytes.size()-1] != '\0')
{
std::vector<unsigned char> tmp = bytes;
tmp.push_back('\0');
return std::string((const char *)tmp.data());
}
return std::string((const char *)bytes.data());
}
return std::string();
}
std::string uBytes2Hex(const char * bytes, unsigned int bytesLen)
{
std::string hex;
if(!bytes || bytesLen == 0)
{
return hex;
}
const unsigned char * bytes_u = (const unsigned char*)(bytes);
hex.resize(bytesLen*2);
char * pHex = &hex[0];
const unsigned char * pEnd = (bytes_u + bytesLen);
for(const unsigned char * pChar = bytes_u; pChar != pEnd; ++pChar, pHex += 2)
{
pHex[0] = uHex2Ascii(*pChar, 0);
pHex[1] = uHex2Ascii(*pChar, 1);
}
return hex;
}
std::vector<char> uHex2Bytes(const std::string & hex)
{
return uHex2Bytes(&hex[0], (int)hex.length());
}
std::vector<char> uHex2Bytes(const char * hex, int hexLen)
{
std::vector<char> bytes;
if(!hex || hexLen % 2 || hexLen == 0)
{
return bytes; // must be pair
}
unsigned int bytesLen = hexLen / 2;
bytes.resize(bytesLen);
unsigned char * pBytes = (unsigned char *)&bytes[0];
const unsigned char * pHex = (const unsigned char *)hex;
unsigned char * pEnd = (pBytes + bytesLen);
for(unsigned char * pChar = pBytes; pChar != pEnd; pChar++, pHex += 2)
{
*pChar = (uAscii2Hex(pHex[0]) << 4) | uAscii2Hex(pHex[1]);
}
return bytes;
}
// The hex str MUST not contains any null values (0x00)
std::string uHex2Str(const std::string & hex)
{
std::vector<char> bytes = uHex2Bytes(hex);
return std::string(&bytes[0], bytes.size());
}
static const char HEX2ASCII[256][2] =
{
{'0','0'},{'0','1'},{'0','2'},{'0','3'},{'0','4'},{'0','5'},{'0','6'},{'0','7'},{'0','8'},{'0','9'},{'0','A'},{'0','B'},{'0','C'},{'0','D'},{'0','E'},{'0','F'},
{'1','0'},{'1','1'},{'1','2'},{'1','3'},{'1','4'},{'1','5'},{'1','6'},{'1','7'},{'1','8'},{'1','9'},{'1','A'},{'1','B'},{'1','C'},{'1','D'},{'1','E'},{'1','F'},
{'2','0'},{'2','1'},{'2','2'},{'2','3'},{'2','4'},{'2','5'},{'2','6'},{'2','7'},{'2','8'},{'2','9'},{'2','A'},{'2','B'},{'2','C'},{'2','D'},{'2','E'},{'2','F'},
{'3','0'},{'3','1'},{'3','2'},{'3','3'},{'3','4'},{'3','5'},{'3','6'},{'3','7'},{'3','8'},{'3','9'},{'3','A'},{'3','B'},{'3','C'},{'3','D'},{'3','E'},{'3','F'},
{'4','0'},{'4','1'},{'4','2'},{'4','3'},{'4','4'},{'4','5'},{'4','6'},{'4','7'},{'4','8'},{'4','9'},{'4','A'},{'4','B'},{'4','C'},{'4','D'},{'4','E'},{'4','F'},
{'5','0'},{'5','1'},{'5','2'},{'5','3'},{'5','4'},{'5','5'},{'5','6'},{'5','7'},{'5','8'},{'5','9'},{'5','A'},{'5','B'},{'5','C'},{'5','D'},{'5','E'},{'5','F'},
{'6','0'},{'6','1'},{'6','2'},{'6','3'},{'6','4'},{'6','5'},{'6','6'},{'6','7'},{'6','8'},{'6','9'},{'6','A'},{'6','B'},{'6','C'},{'6','D'},{'6','E'},{'6','F'},
{'7','0'},{'7','1'},{'7','2'},{'7','3'},{'7','4'},{'7','5'},{'7','6'},{'7','7'},{'7','8'},{'7','9'},{'7','A'},{'7','B'},{'7','C'},{'7','D'},{'7','E'},{'7','F'},
{'8','0'},{'8','1'},{'8','2'},{'8','3'},{'8','4'},{'8','5'},{'8','6'},{'8','7'},{'8','8'},{'8','9'},{'8','A'},{'8','B'},{'8','C'},{'8','D'},{'8','E'},{'8','F'},
{'9','0'},{'9','1'},{'9','2'},{'9','3'},{'9','4'},{'9','5'},{'9','6'},{'9','7'},{'9','8'},{'9','9'},{'9','A'},{'9','B'},{'9','C'},{'9','D'},{'9','E'},{'9','F'},
{'A','0'},{'A','1'},{'A','2'},{'A','3'},{'A','4'},{'A','5'},{'A','6'},{'A','7'},{'A','8'},{'A','9'},{'A','A'},{'A','B'},{'A','C'},{'A','D'},{'A','E'},{'A','F'},
{'B','0'},{'B','1'},{'B','2'},{'B','3'},{'B','4'},{'B','5'},{'B','6'},{'B','7'},{'B','8'},{'B','9'},{'B','A'},{'B','B'},{'B','C'},{'B','D'},{'B','E'},{'B','F'},
{'C','0'},{'C','1'},{'C','2'},{'C','3'},{'C','4'},{'C','5'},{'C','6'},{'C','7'},{'C','8'},{'C','9'},{'C','A'},{'C','B'},{'C','C'},{'C','D'},{'C','E'},{'C','F'},
{'D','0'},{'D','1'},{'D','2'},{'D','3'},{'D','4'},{'D','5'},{'D','6'},{'D','7'},{'D','8'},{'D','9'},{'D','A'},{'D','B'},{'D','C'},{'D','D'},{'D','E'},{'D','F'},
{'E','0'},{'E','1'},{'E','2'},{'E','3'},{'E','4'},{'E','5'},{'E','6'},{'E','7'},{'E','8'},{'E','9'},{'E','A'},{'E','B'},{'E','C'},{'E','D'},{'E','E'},{'E','F'},
{'F','0'},{'F','1'},{'F','2'},{'F','3'},{'F','4'},{'F','5'},{'F','6'},{'F','7'},{'F','8'},{'F','9'},{'F','A'},{'F','B'},{'F','C'},{'F','D'},{'F','E'},{'F','F'}
};
unsigned char uHex2Ascii(const unsigned char & c, bool rightPart)
{
if(rightPart)
{
return HEX2ASCII[c][1];
}
else
{
return HEX2ASCII[c][0];
}
}
unsigned char uAscii2Hex(const unsigned char & c)
{
switch(c)
{
case '0':
case '1':
case '2':
case '3':
case '4':
case '5':
case '6':
case '7':
case '8':
case '9':
return c-'0';
case 'A':
case 'B':
case 'C':
case 'D':
case 'E':
case 'F':
return c-'A'+10;
case 'a':
case 'b':
case 'c':
case 'd':
case 'e':
case 'f':
return c-'a'+10;
default:
return 0x00;
}
}
std::string uFormatv (const char *fmt, va_list args)
{
// Allocate a buffer on the stack that's big enough for us almost
// all the time. Be prepared to allocate dynamically if it doesn't fit.
size_t size = 1024;
std::vector<char> dynamicbuf(size);
char *buf = &dynamicbuf[0];
va_list argsTmp;
while (1) {
#if defined(_WIN32) && !defined(__MINGW32__)
argsTmp = args;
#else
va_copy(argsTmp, args);
#endif
// Try to vsnprintf into our buffer.
#ifdef _MSC_VER
int needed = vsnprintf_s(buf, size, size, fmt, argsTmp);
#else
int needed = vsnprintf (buf, size, fmt, argsTmp);
#endif
va_end(argsTmp);
// NB. C99 (which modern Linux and OS X follow) says vsnprintf
// failure returns the length it would have needed. But older
// glibc and current Windows return -1 for failure, i.e., not
// telling us how much was needed.
if (needed < (int)size-1 && needed >= 0) {
// It fit fine so we're done.
return std::string (buf, (size_t) needed);
}
// vsnprintf reported that it wanted to write more characters
// than we allotted. So try again using a dynamic buffer. This
// doesn't happen very often if we chose our initial size well.
size = needed>=0?needed+2:size*2;
dynamicbuf.resize (size);
buf = &dynamicbuf[0];
}
return std::string(); // would not reach this, but for compiler complaints...
}
std::string uFormat (const char *fmt, ...)
{
va_list args;
va_start(args, fmt);
std::string buf = uFormatv(fmt, args);
va_end(args);
return buf;
}
#ifdef _WIN32
// returned whar_t * must be deleted : delete [] wText;
wchar_t * createWCharFromChar(const char * text)
{
DWORD length = MultiByteToWideChar (CP_ACP, 0, text, -1, NULL, 0);
wchar_t * wText = new wchar_t[length];
MultiByteToWideChar (CP_ACP, 0, text, -1, wText, length );
return wText;
}
// returned char * must be deleted : delete [] text;
char * createCharFromWChar(const wchar_t * wText)
{
DWORD length = WideCharToMultiByte (CP_ACP, 0, wText, -1, NULL, 0, NULL, NULL);
char * text = new char[length];
WideCharToMultiByte (CP_ACP, 0, wText, -1, text, length, NULL, NULL);
return text;
}
#endif
rtabmap/utilite/src/UDirectory.cpp
+398
View File
@@ -0,0 +1,398 @@
/*
* utilite is a cross-platform library with
* useful utilities for fast and small developing.
* Copyright (C) 2010 Mathieu Labbe
*
* utilite is free library: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* utilite is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include "rtabmap/utilite/UDirectory.h"
#ifdef _WIN32
#include <Windows.h>
#include <direct.h>
#include <algorithm>
#include <conio.h>
#else
#include <dirent.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <sys/param.h>
#include <unistd.h>
#include <stdlib.h>
#include <string.h>
#include <pwd.h>
#endif
#include "rtabmap/utilite/UStl.h"
#include "rtabmap/utilite/UFile.h"
#include "rtabmap/utilite/UDirectory.h"
#include "rtabmap/utilite/UConversion.h"
#include "rtabmap/utilite/ULogger.h"
#ifdef _WIN32
bool sortCallback(const std::string & a, const std::string & b)
{
return uStrNumCmp(a,b) < 0;
}
#elif __APPLE__
int sortCallback(const struct dirent ** a, const struct dirent ** b)
{
return uStrNumCmp((*a)->d_name, (*b)->d_name);
}
#else
int sortCallback( const dirent ** a, const dirent ** b)
{
return uStrNumCmp((*a)->d_name, (*b)->d_name);
}
#endif
UDirectory::UDirectory(const std::string & path, const std::string & extensions)
{
extensions_ = uListToVector(uSplit(extensions, ' '));
path_ = path;
iFileName_ = fileNames_.begin();
this->update();
}
UDirectory::UDirectory(const UDirectory & dir)
{
*this = dir;
}
UDirectory & UDirectory::operator=(const UDirectory & dir)
{
extensions_ = dir.extensions_;
path_ = dir.path_;
fileNames_ = dir.fileNames_;
for(iFileName_=fileNames_.begin(); iFileName_!=fileNames_.end(); ++iFileName_)
{
if(iFileName_->compare(*dir.iFileName_) == 0)
{
break;
}
}
return *this;
}
UDirectory::~UDirectory()
{
}
void UDirectory::setPath(const std::string & path, const std::string & extensions)
{
extensions_ = uListToVector(uSplit(extensions, ' '));
path_ = path;
fileNames_.clear();
iFileName_ = fileNames_.begin();
this->update();
}
void UDirectory::update()
{
if(exists(path_))
{
std::string lastName;
bool endOfDir = false;
if(iFileName_ != fileNames_.end())
{
//Record the last file name
lastName = *iFileName_;
}
else if(fileNames_.size())
{
lastName = *fileNames_.rbegin();
endOfDir = true;
}
fileNames_.clear();
#ifdef _WIN32
WIN32_FIND_DATA fileInformation;
#ifdef UNICODE
wchar_t * pathAll = createWCharFromChar((path_+"\\*").c_str());
HANDLE hFile = ::FindFirstFile(pathAll, &fileInformation);
delete [] pathAll;
#else
HANDLE hFile = ::FindFirstFile((path_+"\\*").c_str(), &fileInformation);
#endif
if(hFile != INVALID_HANDLE_VALUE)
{
do
{
#ifdef UNICODE
char * fileName = createCharFromWChar(fileInformation.cFileName);
fileNames_.push_back(fileName);
delete [] fileName;
#else
fileNames_.push_back(fileInformation.cFileName);
#endif
} while(::FindNextFile(hFile, &fileInformation) == TRUE);
::FindClose(hFile);
std::vector<std::string> vFileNames = uListToVector(fileNames_);
std::sort(vFileNames.begin(), vFileNames.end(), sortCallback);
fileNames_ = uVectorToList(vFileNames);
}
#else
int nameListSize;
struct dirent ** nameList = 0;
nameListSize = scandir(path_.c_str(), &nameList, 0, sortCallback);
if(nameList && nameListSize>0)
{
for (int i=0;i<nameListSize;++i)
{
fileNames_.push_back(nameList[i]->d_name);
free(nameList[i]);
}
free(nameList);
}
#endif
//filter extensions...
std::list<std::string>::iterator iter = fileNames_.begin();
bool valid;
while(iter!=fileNames_.end())
{
valid = false;
if(extensions_.size() == 0 &&
iter->compare(".") != 0 &&
iter->compare("..") != 0)
{
valid = true;
}
for(unsigned int i=0; i<extensions_.size(); ++i)
{
if(UFile::getExtension(*iter).compare(extensions_[i]) == 0)
{
valid = true;
break;
}
}
if(!valid)
{
iter = fileNames_.erase(iter);
}
else
{
++iter;
}
}
iFileName_ = fileNames_.begin();
if(!lastName.empty())
{
bool found = false;
for(std::list<std::string>::iterator iter=fileNames_.begin(); iter!=fileNames_.end(); ++iter)
{
if(lastName.compare(*iter) == 0)
{
found = true;
iFileName_ = iter;
break;
}
}
if(endOfDir && found)
{
++iFileName_;
}
else if(endOfDir && fileNames_.size())
{
iFileName_ = --fileNames_.end();
}
}
}
}
bool UDirectory::isValid()
{
return exists(path_);
}
std::string UDirectory::getNextFileName()
{
std::string fileName;
if(iFileName_ != fileNames_.end())
{
fileName = *iFileName_;
++iFileName_;
}
return fileName;
}
std::string UDirectory::getNextFilePath()
{
std::string filePath;
if(iFileName_ != fileNames_.end())
{
filePath = path_+separator()+*iFileName_;
++iFileName_;
}
return filePath;
}
void UDirectory::rewind()
{
iFileName_ = fileNames_.begin();
}
bool UDirectory::exists(const std::string & dirPath)
{
bool r = false;
#ifdef _WIN32
#ifdef UNICODE
wchar_t * wDirPath = createWCharFromChar(dirPath.c_str());
DWORD dwAttrib = GetFileAttributes(wDirPath);
delete [] wDirPath;
#else
DWORD dwAttrib = GetFileAttributes(dirPath.c_str());
#endif
r = (dwAttrib != INVALID_FILE_ATTRIBUTES && (dwAttrib & FILE_ATTRIBUTE_DIRECTORY));
#else
DIR *dp;
if((dp = opendir(dirPath.c_str())) != NULL)
{
r = true;
closedir(dp);
}
#endif
return r;
}
// return the directory path of the file
std::string UDirectory::getDir(const std::string & filePath)
{
std::string dir = filePath;
int i=(int)dir.size()-1;
for(; i>=0; --i)
{
if(dir[i] == '/' || dir[i] == '\\')
{
//remove separators...
dir[i] = 0;
--i;
while(i>=0 && (dir[i] == '/' || dir[i] == '\\'))
{
dir[i] = 0;
--i;
}
break;
}
else
{
dir[i] = 0;
}
}
if(i<0)
{
dir = ".";
}
else
{
dir.resize(i+1);
}
return dir;
}
std::string UDirectory::currentDir(bool trailingSeparator)
{
std::string dir;
char * buffer;
#ifdef _WIN32
buffer = _getcwd(NULL, 0);
#else
buffer = getcwd(NULL, MAXPATHLEN);
#endif
if( buffer != NULL )
{
dir = buffer;
free(buffer);
if(trailingSeparator)
{
dir += separator();
}
}
return dir;
}
bool UDirectory::makeDir(const std::string & dirPath)
{
int status;
#ifdef _WIN32
status = _mkdir(dirPath.c_str());
#else
status = mkdir(dirPath.c_str(), S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH);
#endif
return status==0;
}
bool UDirectory::removeDir(const std::string & dirPath)
{
int status;
#ifdef _WIN32
status = _rmdir(dirPath.c_str());
#else
status = rmdir(dirPath.c_str());
#endif
return status==0;
}
std::string UDirectory::homeDir()
{
std::string path;
#ifdef _WIN32
#ifdef UNICODE
wchar_t wProfilePath[250];
ExpandEnvironmentStrings(L"%userprofile%",wProfilePath,250);
char * profilePath = createCharFromWChar(wProfilePath);
path = profilePath;
delete [] profilePath;
#else
char profilePath[250];
ExpandEnvironmentStrings("%userprofile%",profilePath,250);
path = profilePath;
#endif
#else
char * pathstr = getenv("HOME");
if(pathstr)
{
path = pathstr;
}
if(path.empty())
{
struct passwd *pw = getpwuid(getuid());
if(pw) {
path = pw->pw_dir;
}
if(path.empty())
{
UFATAL("Environment variable HOME is not set, cannot get home directory! Please set HOME environment variable to a valid directory.");
}
}
#endif
return path;
}
std::string UDirectory::separator()
{
#ifdef _WIN32
return "\\";
#else
return "/";
#endif
}
rtabmap/utilite/src/UEventsHandler.cpp
+36
View File
@@ -0,0 +1,36 @@
/*
* utilite is a cross-platform library with
* useful utilities for fast and small developing.
* Copyright (C) 2010 Mathieu Labbe
*
* utilite is free library: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* utilite is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include "rtabmap/utilite/UEventsHandler.h"
#include "rtabmap/utilite/UEventsManager.h"
UEventsHandler::~UEventsHandler()
{
unregisterFromEventsManager();
}
void UEventsHandler::registerToEventsManager()
{
UEventsManager::addHandler(this);
}
void UEventsHandler::unregisterFromEventsManager()
{
UEventsManager::removeHandler(this);
}
rtabmap/utilite/src/UEventsManager.cpp
+476
View File
@@ -0,0 +1,476 @@
/*
* utilite is a cross-platform library with
* useful utilities for fast and small developing.
* Copyright (C) 2010 Mathieu Labbe
*
* utilite is free library: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* utilite is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include "rtabmap/utilite/UEventsManager.h"
#include "rtabmap/utilite/UEvent.h"
#include <list>
#include "rtabmap/utilite/UStl.h"
UEventsManager* UEventsManager::instance_ = 0;
UDestroyer<UEventsManager> UEventsManager::destroyer_;
void UEventsManager::addHandler(UEventsHandler* handler)
{
if(!handler)
{
UERROR("Handler is null!");
return;
}
else
{
UEventsManager::getInstance()->_addHandler(handler);
}
}
void UEventsManager::removeHandler(UEventsHandler* handler)
{
if(!handler)
{
UERROR("Handler is null!");
return;
}
else
{
UEventsManager::getInstance()->_removeHandler(handler);
}
}
void UEventsManager::post(UEvent * event, bool async, const UEventsSender * sender)
{
if(!event)
{
UERROR("Event is null!");
return;
}
else
{
UEventsManager::getInstance()->_postEvent(event, async, sender);
}
}
void UEventsManager::createPipe(
const UEventsSender * sender,
const UEventsHandler * receiver,
const std::string & eventName)
{
if(!sender || !receiver)
{
UERROR("Sender and/or receiver is null!");
return;
}
else
{
UEventsManager::getInstance()->_createPipe(sender, receiver, eventName);
}
}
void UEventsManager::removePipe(
const UEventsSender * sender,
const UEventsHandler * receiver,
const std::string & eventName)
{
if(!sender || !receiver)
{
UERROR("Sender and/or receiver is null!");
return;
}
else
{
UEventsManager::getInstance()->_removePipe(sender, receiver, eventName);
}
}
void UEventsManager::removeAllPipes(const UEventsSender * sender)
{
if(!sender)
{
UERROR("Sender is null!");
return;
}
else
{
UEventsManager::getInstance()->_removeAllPipes(sender);
}
}
void UEventsManager::removeNullPipes(const UEventsSender * sender)
{
if(!sender)
{
UERROR("Sender is null!");
return;
}
else
{
UEventsManager::getInstance()->_removeNullPipes(sender);
}
}
UEventsManager* UEventsManager::getInstance()
{
if(!instance_)
{
instance_ = new UEventsManager();
destroyer_.setDoomed(instance_);
instance_->start(); // Start the thread
}
return instance_;
}
UEventsManager::UEventsManager()
{
}
UEventsManager::~UEventsManager()
{
join(true);
// Free memory
for(std::list<std::pair<UEvent*, const UEventsSender*> >::iterator it=events_.begin(); it!=events_.end(); ++it)
{
delete it->first;
}
events_.clear();
handlers_.clear();
instance_ = 0;
}
void UEventsManager::mainLoop()
{
postEventSem_.acquire();
if(!this->isKilled())
{
dispatchEvents();
}
}
void UEventsManager::mainLoopKill()
{
postEventSem_.release();
}
void UEventsManager::dispatchEvents()
{
if(events_.size() == 0)
{
return;
}
std::list<std::pair<UEvent*, const UEventsSender*> >::iterator it;
std::list<std::pair<UEvent*, const UEventsSender*> > eventsBuf;
// Copy events in a buffer :
// Other threads can post events
// while events are handled.
eventsMutex_.lock();
{
eventsBuf = events_;
events_.clear();
}
eventsMutex_.unlock();
// Past events to handlers
for(it=eventsBuf.begin(); it!=eventsBuf.end(); ++it)
{
if(!dispatchEvent(it->first, it->second))
{
delete it->first;
}
}
eventsBuf.clear();
}
bool UEventsManager::dispatchEvent(UEvent * event, const UEventsSender * sender)
{
std::list<UEventsHandler*> handlers;
// Verify if there are pipes with the sender for his type of event
if(sender)
{
handlers = getPipes(sender, event->getClassName());
}
handlersMutex_.lock();
if(handlers.size() == 0)
{
//No pipes, send to all handlers
handlers = handlers_;
}
bool handled = false;
for(std::list<UEventsHandler*>::iterator it=handlers.begin(); it!=handlers.end() && !handled; ++it)
{
// Check if the handler is still in the
// handlers_ list (may be changed if addHandler() or
// removeHandler() is called in EventsHandler::handleEvent())
if(std::find(handlers_.begin(), handlers_.end(), *it) != handlers_.end())
{
UEventsHandler * handler = *it;
handlersMutex_.unlock();
// Don't process event if the handler is the same as the sender
if(handler != sender)
{
// To be able to add/remove an handler in a handleEvent call (without a deadlock)
// @see _addHandler(), _removeHandler()
handled = handler->handleEvent(event);
}
handlersMutex_.lock();
}
}
handlersMutex_.unlock();
return handled;
}
void UEventsManager::_addHandler(UEventsHandler* handler)
{
if(!this->isKilled())
{
handlersMutex_.lock();
{
//make sure it is not already in the list
bool handlerFound = false;
for(std::list<UEventsHandler*>::iterator it=handlers_.begin(); it!=handlers_.end(); ++it)
{
if(*it == handler)
{
handlerFound = true;
}
}
if(!handlerFound)
{
handlers_.push_back(handler);
}
}
handlersMutex_.unlock();
}
}
void UEventsManager::_removeHandler(UEventsHandler* handler)
{
if(!this->isKilled())
{
handlersMutex_.lock();
{
for (std::list<UEventsHandler*>::iterator it = handlers_.begin(); it!=handlers_.end(); ++it)
{
if(*it == handler)
{
handlers_.erase(it);
break;
}
}
}
handlersMutex_.unlock();
pipesMutex_.lock();
{
for(std::list<Pipe>::iterator iter=pipes_.begin(); iter!= pipes_.end(); ++iter)
{
if(iter->receiver_ == handler)
{
iter->receiver_ = 0; // set to null
}
}
}
pipesMutex_.unlock();
}
}
void UEventsManager::_postEvent(UEvent * event, bool async, const UEventsSender * sender)
{
if(!this->isKilled())
{
if(async)
{
eventsMutex_.lock();
{
events_.push_back(std::make_pair(event, sender));
}
eventsMutex_.unlock();
// Signal the EventsManager that an Event is added
postEventSem_.release();
}
else
{
if(!dispatchEvent(event, sender))
{
delete event;
}
}
}
else
{
delete event;
}
}
std::list<UEventsHandler*> UEventsManager::getPipes(
const UEventsSender * sender,
const std::string & eventName)
{
std::list<UEventsHandler*> pipes;
pipesMutex_.lock();
for(std::list<Pipe>::iterator iter=pipes_.begin(); iter!= pipes_.end(); ++iter)
{
if(iter->sender_ == sender && iter->eventName_.compare(eventName) == 0)
{
bool added = false;
if(iter->receiver_)
{
handlersMutex_.lock();
for(std::list<UEventsHandler*>::iterator jter=handlers_.begin(); jter!=handlers_.end(); ++jter)
{
if(*jter == iter->receiver_)
{
pipes.push_back(*jter);
added = true;
break;
}
}
handlersMutex_.unlock();
}
if(!added)
{
// Add nulls
pipes.push_back(0);
}
}
}
pipesMutex_.unlock();
return pipes;
}
void UEventsManager::_createPipe(
const UEventsSender * sender,
const UEventsHandler * receiver,
const std::string & eventName)
{
pipesMutex_.lock();
bool exist = false;
for(std::list<Pipe>::iterator iter=pipes_.begin(); iter!= pipes_.end();++iter)
{
if(iter->sender_ == sender && iter->receiver_ == receiver && iter->eventName_.compare(eventName) == 0)
{
exist = true;
break;
}
}
if(!exist)
{
bool handlerFound = false;
handlersMutex_.lock();
for(std::list<UEventsHandler*>::iterator iter=handlers_.begin(); iter!=handlers_.end(); ++iter)
{
if(*iter == receiver)
{
handlerFound = true;
break;
}
}
handlersMutex_.unlock();
if(handlerFound)
{
pipes_.push_back(Pipe(sender, receiver, eventName));
}
else
{
UERROR("Cannot create the pipe because the receiver is not yet "
"added to UEventsManager's handlers list.");
}
}
else
{
UWARN("Pipe between sender %p and receiver %p with event %s was already created.",
sender, receiver, eventName.c_str());
}
pipesMutex_.unlock();
}
void UEventsManager::_removePipe(
const UEventsSender * sender,
const UEventsHandler * receiver,
const std::string & eventName)
{
pipesMutex_.lock();
bool removed = false;
for(std::list<Pipe>::iterator iter=pipes_.begin(); iter!= pipes_.end();)
{
if(iter->sender_ == sender && iter->receiver_ == receiver && iter->eventName_.compare(eventName) == 0)
{
iter = pipes_.erase(iter);
removed = true;
}
else
{
++iter;
}
}
if(!removed)
{
UWARN("Pipe between sender %p and receiver %p with event %s didn't exist.",
sender, receiver, eventName.c_str());
}
pipesMutex_.unlock();
}
void UEventsManager::_removeAllPipes(const UEventsSender * sender)
{
pipesMutex_.lock();
for(std::list<Pipe>::iterator iter=pipes_.begin(); iter!=pipes_.end();)
{
if(iter->sender_ == sender)
{
iter = pipes_.erase(iter);
}
else
{
++iter;
}
}
pipesMutex_.unlock();
}
void UEventsManager::_removeNullPipes(const UEventsSender * sender)
{
pipesMutex_.lock();
for(std::list<Pipe>::iterator iter=pipes_.begin(); iter!=pipes_.end();)
{
if(iter->receiver_ == 0)
{
iter = pipes_.erase(iter);
}
else
{
++iter;
}
}
pipesMutex_.unlock();
}
rtabmap/utilite/src/UEventsSender.cpp
+31
View File
@@ -0,0 +1,31 @@
/*
* utilite is a cross-platform library with
* useful utilities for fast and small developing.
* Copyright (C) 2010 Mathieu Labbe
*
* utilite is free library: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* utilite is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include "rtabmap/utilite/UEventsSender.h"
#include "rtabmap/utilite/UEventsManager.h"
UEventsSender::~UEventsSender()
{
UEventsManager::removeAllPipes(this);
}
void UEventsSender::post(UEvent * event, bool async) const
{
UEventsManager::post(event, async, this);
}
rtabmap/utilite/src/UFile.cpp
+103
View File
@@ -0,0 +1,103 @@
/*
* utilite is a cross-platform library with
* useful utilities for fast and small developing.
* Copyright (C) 2010 Mathieu Labbe
*
* utilite is free library: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* utilite is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include "rtabmap/utilite/UFile.h"
#include <fstream>
#include "rtabmap/utilite/UStl.h"
bool UFile::exists(const std::string &filePath)
{
bool fileExists = false;
std::ifstream in(filePath.c_str(), std::ios::in);
if (in.is_open())
{
fileExists = true;
in.close();
}
return fileExists;
}
long UFile::length(const std::string &filePath)
{
long fileSize = 0;
FILE* fp = 0;
#ifdef _MSC_VER
fopen_s(&fp, filePath.c_str(), "rb");
#else
fp = fopen(filePath.c_str(), "rb");
#endif
if(fp == NULL)
{
return 0;
}
fseek(fp , 0 , SEEK_END);
fileSize = ftell(fp);
fclose(fp);
return fileSize;
}
int UFile::erase(const std::string &filePath)
{
return std::remove(filePath.c_str());
}
int UFile::rename(const std::string &oldFilePath,
const std::string &newFilePath)
{
return std::rename(oldFilePath.c_str(), newFilePath.c_str());
}
std::string UFile::getName(const std::string & filePath)
{
std::string fullPath = filePath;
std::string name;
for(int i=(int)fullPath.size()-1; i>=0; --i)
{
if(fullPath[i] == '/' || fullPath[i] == '\\')
{
break;
}
else
{
name.insert(name.begin(), fullPath[i]);
}
}
return name;
}
std::string UFile::getExtension(const std::string &filePath)
{
std::list<std::string> list = uSplit(filePath, '.');
if(list.size())
{
return list.back();
}
return "";
}
void UFile::copy(const std::string & from, const std::string & to)
{
std::ifstream src(from.c_str());
std::ofstream dst(to.c_str());
dst << src.rdbuf();
}
rtabmap/utilite/src/ULogger.cpp
+669
View File
@@ -0,0 +1,669 @@
/*
* utilite is a cross-platform library with
* useful utilities for fast and small developing.
* Copyright (C) 2010 Mathieu Labbe
*
* utilite is free library: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* utilite is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include "rtabmap/utilite/ULogger.h"
#include "rtabmap/utilite/UConversion.h"
#include "rtabmap/utilite/UFile.h"
#include "rtabmap/utilite/UStl.h"
#include "rtabmap/utilite/UEventsManager.h"
#include <fstream>
#include <string>
#include <string.h>
#ifndef _WIN32
#include <sys/time.h>
#endif
#ifdef _WIN32
#include <Windows.h>
#define COLOR_NORMAL FOREGROUND_BLUE | FOREGROUND_GREEN | FOREGROUND_RED
#define COLOR_RED FOREGROUND_RED | FOREGROUND_INTENSITY
#define COLOR_GREEN FOREGROUND_GREEN
#define COLOR_YELLOW FOREGROUND_GREEN | FOREGROUND_RED
#else
#define COLOR_NORMAL "\033[0m"
#define COLOR_RED "\033[31m"
#define COLOR_GREEN "\033[32m"
#define COLOR_YELLOW "\033[33m"
#endif
bool ULogger::append_ = true;
bool ULogger::printTime_ = true;
bool ULogger::printLevel_ = true;
bool ULogger::printEndline_ = true;
bool ULogger::printColored_ = true;
bool ULogger::printWhere_ = true;
bool ULogger::printWhereFullPath_ = false;
bool ULogger::printThreadID_ = false;
bool ULogger::limitWhereLength_ = false;
bool ULogger::buffered_ = false;
ULogger::Level ULogger::level_ = kInfo; // By default, we show all info msgs + upper level (Warning, Error)
ULogger::Level ULogger::eventLevel_ = kFatal;
const char * ULogger::levelName_[5] = {"DEBUG", " INFO", " WARN", "ERROR", "FATAL"};
ULogger* ULogger::instance_ = 0;
UDestroyer<ULogger> ULogger::destroyer_;
ULogger::Type ULogger::type_ = ULogger::kTypeNoLog; // Default nothing
UMutex ULogger::loggerMutex_;
const std::string ULogger::kDefaultLogFileName = "./ULog.txt";
std::string ULogger::logFileName_;
std::string ULogger::bufferedMsgs_;
std::set<unsigned long> ULogger::threadIdFilter_;
std::map<std::string, unsigned long> ULogger::registeredThreads_;
/**
* This class is used to write logs in the console. This class cannot
* be directly used, use ULogger::setType() to console type to print in
* console and use macro UDEBUG(), UINFO()... to print messages.
* @see ULogger
*/
class UConsoleLogger : public ULogger
{
public :
virtual ~UConsoleLogger() {this->_flush();}
protected:
/**
* Only the Logger can create inherited
* loggers according to the Abstract factory patterns.
*/
friend class ULogger;
UConsoleLogger() {}
private:
virtual void _write(const char* msg, va_list arg)
{
vprintf(msg, arg);
}
virtual void _writeStr(const char* msg)
{
printf("%s", msg);
}
};
/**
* This class is used to write logs in a file. This class cannot
* be directly used, use ULogger::setType() to file type to print in
* a file and use macro UDEBUG(), UINFO()... to print messages.
* @see ULogger
*/
class UFileLogger : public ULogger
{
public:
virtual ~UFileLogger()
{
this->_flush();
if(fout_)
{
fclose(fout_);
}
}
protected:
/**
* Only the Logger can create inherited
* loggers according to the Abstract factory patterns.
*/
friend class ULogger;
/**
* The UFileLogger constructor.
* @param fileName the file name
* @param append if true append logs in the file,
* ortherwise it overrides the file.
*
*/
UFileLogger(const std::string &fileName, bool append)
{
fileName_ = fileName;
if(!append) {
std::ofstream fileToClear(fileName_.c_str(), std::ios::out);
fileToClear.clear();
fileToClear.close();
}
#ifdef _MSC_VER
fopen_s(&fout_, fileName_.c_str(), "a");
#else
fout_ = fopen(fileName_.c_str(), "a");
#endif
if(!fout_) {
printf("FileLogger : Cannot open file : %s\n", fileName_.c_str()); // TODO send Event instead, or return error code
return;
}
}
private:
virtual void _write(const char* msg, va_list arg)
{
if(fout_)
{
vfprintf(fout_, msg, arg);
}
}
virtual void _writeStr(const char* msg)
{
if(fout_)
{
fprintf(fout_, "%s", msg);
}
}
private:
std::string fileName_; ///< the file name
FILE* fout_;
std::string bufferedMsgs_;
};
void ULogger::setType(Type type, const std::string &fileName, bool append)
{
ULogger::flush();
loggerMutex_.lock();
{
// instance not yet created
if(!instance_)
{
type_ = type;
logFileName_ = fileName;
append_ = append;
instance_ = createInstance();
}
// type changed
else if(type_ != type || (type_ == kTypeFile && logFileName_.compare(fileName)!=0))
{
destroyer_.setDoomed(0);
delete instance_;
instance_ = 0;
type_ = type;
logFileName_ = fileName;
append_ = append;
instance_ = createInstance();
}
}
loggerMutex_.unlock();
}
void ULogger::setTreadIdFilter(const std::vector<std::string> & ids)
{
loggerMutex_.lock();
threadIdFilter_.clear();
for(unsigned int i=0;i<ids.size();++i)
{
if(registeredThreads_.find(ids[i]) != registeredThreads_.end())
{
threadIdFilter_.insert(registeredThreads_.at(ids[i]));
}
}
loggerMutex_.unlock();
}
void ULogger::registerCurrentThread(const std::string & name)
{
loggerMutex_.lock();
UASSERT(!name.empty());
uInsert(registeredThreads_, std::make_pair(name, UThread::currentThreadId()));
loggerMutex_.unlock();
}
void ULogger::unregisterCurrentThread()
{
loggerMutex_.lock();
unsigned long id = UThread::currentThreadId();
for(std::map<std::string, unsigned long>::iterator iter=registeredThreads_.begin(); iter!=registeredThreads_.end();)
{
if(iter->second == id)
{
registeredThreads_.erase(iter++);
threadIdFilter_.erase(id);
}
else
{
++iter;
}
}
loggerMutex_.unlock();
}
std::map<std::string, unsigned long> ULogger::getRegisteredThreads()
{
loggerMutex_.lock();
std::map<std::string, unsigned long> out = registeredThreads_;
loggerMutex_.unlock();
return out;
}
void ULogger::reset()
{
ULogger::setType(ULogger::kTypeNoLog);
append_ = true;
printTime_ = true;
printLevel_ = true;
printEndline_ = true;
printColored_ = true;
printWhere_ = true;
printWhereFullPath_ = false;
printThreadID_ = false;
limitWhereLength_ = false;
level_ = kInfo; // By default, we show all info msgs + upper level (Warning, Error)
logFileName_ = ULogger::kDefaultLogFileName;
}
void ULogger::setBuffered(bool buffered)
{
if(!buffered)
{
ULogger::flush();
}
buffered_ = buffered;
}
void ULogger::flush()
{
loggerMutex_.lock();
if(!instance_ || bufferedMsgs_.size()==0)
{
loggerMutex_.unlock();
return;
}
instance_->_flush();
loggerMutex_.unlock();
}
void ULogger::_flush()
{
ULogger::getInstance()->_writeStr(bufferedMsgs_.c_str());
bufferedMsgs_.clear();
}
void ULogger::write(const char* msg, ...)
{
loggerMutex_.lock();
if(!instance_)
{
loggerMutex_.unlock();
return;
}
std::string endline = "";
if(printEndline_) {
endline = "\r\n";
}
std::string time = "";
if(printTime_)
{
getTime(time);
time.append(" - ");
}
if(printTime_)
{
if(buffered_)
{
bufferedMsgs_.append(time.c_str());
}
else
{
ULogger::getInstance()->_writeStr(time.c_str());
}
}
va_list args;
va_start(args, msg);
if(buffered_)
{
bufferedMsgs_.append(uFormatv(msg, args));
}
else
{
ULogger::getInstance()->_write(msg, args);
}
va_end(args);
if(printEndline_)
{
if(buffered_)
{
bufferedMsgs_.append(endline.c_str());
}
else
{
ULogger::getInstance()->_writeStr(endline.c_str());
}
}
loggerMutex_.unlock();
}
void ULogger::write(ULogger::Level level,
const char * file,
int line,
const char * function,
const char* msg,
...)
{
loggerMutex_.lock();
if(type_ == kTypeNoLog && level < kFatal && level < eventLevel_)
{
loggerMutex_.unlock();
return;
}
if(strlen(msg) == 0 && !printWhere_ && level < kFatal)
{
loggerMutex_.unlock();
// No need to show an empty message if we don't print where.
return;
}
if(level < kFatal &&
threadIdFilter_.size() &&
threadIdFilter_.find(UThread::currentThreadId()) == threadIdFilter_.end())
{
loggerMutex_.unlock();
return;
}
if(level >= level_ || level >= eventLevel_)
{
#ifdef _WIN32
int color = 0;
#else
const char* color = NULL;
#endif
switch(level)
{
case kDebug:
color = COLOR_GREEN;
break;
case kInfo:
color = COLOR_NORMAL;
break;
case kWarning:
color = COLOR_YELLOW;
break;
case kError:
case kFatal:
color = COLOR_RED;
break;
default:
break;
}
std::string endline = "";
if(printEndline_) {
endline = "\r\n";
}
std::string time = "";
if(printTime_ || level == kFatal)
{
time.append("(");
getTime(time);
time.append(") ");
}
std::string levelStr = "";
if(printLevel_ || level == kFatal)
{
const int bufSize = 30;
char buf[bufSize] = {0};
#ifdef _MSC_VER
sprintf_s(buf, bufSize, "[%s]", levelName_[level]);
#else
snprintf(buf, bufSize, "[%s]", levelName_[level]);
#endif
levelStr = buf;
levelStr.append(" ");
}
std::string pidStr;
if(printThreadID_)
{
pidStr = uFormat("{%lu} ", UThread::currentThreadId());
}
std::string whereStr = "";
if(printWhere_ || level == kFatal)
{
whereStr.append("");
//File
if(printWhereFullPath_)
{
whereStr.append(file);
}
else
{
std::string fileName = UFile::getName(file);
if(limitWhereLength_ && fileName.size() > 8)
{
fileName.erase(8);
fileName.append("~");
}
whereStr.append(fileName);
}
//Line
whereStr.append(":");
std::string lineStr = uNumber2Str(line);
whereStr.append(lineStr);
//Function
whereStr.append("::");
std::string funcStr = function;
if(!printWhereFullPath_ && limitWhereLength_ && funcStr.size() > 8)
{
funcStr.erase(8);
funcStr.append("~");
}
funcStr.append("()");
whereStr.append(funcStr);
whereStr.append(" ");
}
va_list args;
if(type_ != kTypeNoLog)
{
va_start(args, msg);
#ifdef _WIN32
HANDLE H = GetStdHandle(STD_OUTPUT_HANDLE);
#endif
if(type_ == ULogger::kTypeConsole && printColored_)
{
#ifdef _WIN32
SetConsoleTextAttribute(H,color);
#else
if(buffered_)
{
bufferedMsgs_.append(color);
}
else
{
ULogger::getInstance()->_writeStr(color);
}
#endif
}
if(buffered_)
{
bufferedMsgs_.append(levelStr.c_str());
bufferedMsgs_.append(pidStr.c_str());
bufferedMsgs_.append(time.c_str());
bufferedMsgs_.append(whereStr.c_str());
bufferedMsgs_.append(uFormatv(msg, args));
}
else
{
ULogger::getInstance()->_writeStr(levelStr.c_str());
ULogger::getInstance()->_writeStr(pidStr.c_str());
ULogger::getInstance()->_writeStr(time.c_str());
ULogger::getInstance()->_writeStr(whereStr.c_str());
ULogger::getInstance()->_write(msg, args);
}
if(type_ == ULogger::kTypeConsole && printColored_)
{
#ifdef _WIN32
SetConsoleTextAttribute(H,COLOR_NORMAL);
#else
if(buffered_)
{
bufferedMsgs_.append(COLOR_NORMAL);
}
else
{
ULogger::getInstance()->_writeStr(COLOR_NORMAL);
}
#endif
}
if(buffered_)
{
bufferedMsgs_.append(endline.c_str());
}
else
{
ULogger::getInstance()->_writeStr(endline.c_str());
}
va_end (args);
}
if(level >= eventLevel_)
{
std::string fullMsg = uFormat("%s%s%s%s", levelStr.c_str(), pidStr.c_str(), time.c_str(), whereStr.c_str());
va_start(args, msg);
fullMsg.append(uFormatv(msg, args));
va_end(args);
if(level >= kFatal)
{
// Send it synchronously, then receivers
// can do something before the code (exiting) below is executed.
UEventsManager::post(new ULogEvent(fullMsg, kFatal), false);
}
else
{
UEventsManager::post(new ULogEvent(fullMsg, level));
}
}
if(level >= kFatal)
{
std::string fullMsg = uFormat("%s%s%s%s", levelStr.c_str(), pidStr.c_str(), time.c_str(), whereStr.c_str());
va_start(args, msg);
fullMsg.append(uFormatv(msg, args));
va_end(args);
if(instance_)
{
destroyer_.setDoomed(0);
delete instance_; // If a FileLogger is used, this will close the file.
instance_ = 0;
}
//========================================================================
// Throw exception
loggerMutex_.unlock();
throw UException(fullMsg);
//========================================================================
}
}
loggerMutex_.unlock();
}
int ULogger::getTime(std::string &timeStr)
{
struct tm timeinfo;
const int bufSize = 30;
char buf[bufSize] = {0};
#if _MSC_VER
time_t rawtime;
time(&rawtime);
localtime_s (&timeinfo, &rawtime );
int result = sprintf_s(buf, bufSize, "%d-%s%d-%s%d %s%d:%s%d:%s%d",
timeinfo.tm_year+1900,
(timeinfo.tm_mon+1) < 10 ? "0":"", timeinfo.tm_mon+1,
(timeinfo.tm_mday) < 10 ? "0":"", timeinfo.tm_mday,
(timeinfo.tm_hour) < 10 ? "0":"", timeinfo.tm_hour,
(timeinfo.tm_min) < 10 ? "0":"", timeinfo.tm_min,
(timeinfo.tm_sec) < 10 ? "0":"", timeinfo.tm_sec);
#elif WIN32
time_t rawtime;
time(&rawtime);
timeinfo = *localtime (&rawtime);
int result = snprintf(buf, bufSize, "%d-%s%d-%s%d %s%d:%s%d:%s%d",
timeinfo.tm_year+1900,
(timeinfo.tm_mon+1) < 10 ? "0":"", timeinfo.tm_mon+1,
(timeinfo.tm_mday) < 10 ? "0":"", timeinfo.tm_mday,
(timeinfo.tm_hour) < 10 ? "0":"", timeinfo.tm_hour,
(timeinfo.tm_min) < 10 ? "0":"", timeinfo.tm_min,
(timeinfo.tm_sec) < 10 ? "0":"", timeinfo.tm_sec);
#else
struct timeval rawtime;
gettimeofday(&rawtime, NULL);
localtime_r (&rawtime.tv_sec, &timeinfo);
int result = snprintf(buf, bufSize, "%d-%s%d-%s%d %s%d:%s%d:%s%d.%s%d",
timeinfo.tm_year+1900,
(timeinfo.tm_mon+1) < 10 ? "0":"", timeinfo.tm_mon+1,
(timeinfo.tm_mday) < 10 ? "0":"", timeinfo.tm_mday,
(timeinfo.tm_hour) < 10 ? "0":"", timeinfo.tm_hour,
(timeinfo.tm_min) < 10 ? "0":"", timeinfo.tm_min,
(timeinfo.tm_sec) < 10 ? "0":"", timeinfo.tm_sec,
(rawtime.tv_usec/1000) < 10 ? "00":(rawtime.tv_usec/1000) < 100?"0":"", int(rawtime.tv_usec/1000));
#endif
if(result)
{
timeStr.append(buf);
}
return result;
}
ULogger* ULogger::getInstance()
{
if(!instance_)
{
instance_ = createInstance();
}
return instance_;
}
ULogger* ULogger::createInstance()
{
ULogger* instance = 0;
if(type_ == ULogger::kTypeConsole)
{
instance = new UConsoleLogger();
}
else if(type_ == ULogger::kTypeFile)
{
instance = new UFileLogger(logFileName_, append_);
}
destroyer_.setDoomed(instance);
return instance;
}
ULogger::~ULogger()
{
instance_ = 0;
//printf("Logger is destroyed...\n\r");
}
rtabmap/utilite/src/UProcessInfo.cpp
+78
View File
@@ -0,0 +1,78 @@
/*
* utilite is a cross-platform library with
* useful utilities for fast and small developing.
* Copyright (C) 2010 Mathieu Labbe
*
* utilite is free library: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* utilite is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include "rtabmap/utilite/UProcessInfo.h"
#ifdef _WIN32
#include "Windows.h"
#include "Psapi.h"
#elif __APPLE__
#include <sys/resource.h>
#else
#include <fstream>
#include <stdlib.h>
#include "rtabmap/utilite/UStl.h"
#endif
UProcessInfo::UProcessInfo() {}
UProcessInfo::~UProcessInfo() {}
// return in bytes
long int UProcessInfo::getMemoryUsage()
{
long int memoryUsage = -1;
#ifdef _WIN32
HANDLE hProc = GetCurrentProcess();
PROCESS_MEMORY_COUNTERS info;
BOOL okay = GetProcessMemoryInfo(hProc, &info, sizeof(info));
if(okay)
{
memoryUsage = info.WorkingSetSize;
}
#elif __APPLE__
rusage u;
if(getrusage(RUSAGE_SELF, &u) == 0)
{
memoryUsage = u.ru_maxrss;
}
#else
std::fstream file("/proc/self/status", std::fstream::in);
if(file.is_open())
{
std::string bytes;
while(std::getline(file, bytes))
{
if(bytes.find("VmRSS") != bytes.npos)
{
std::list<std::string> strs = uSplit(bytes, ' ');
if(strs.size()>1)
{
memoryUsage = atol(uValueAt(strs,1).c_str()) * 1024;
}
break;
}
}
file.close();
}
#endif
return memoryUsage;
}
rtabmap/utilite/src/UThread.cpp
+301
View File
@@ -0,0 +1,301 @@
/*
* utilite is a cross-platform library with
* useful utilities for fast and small developing.
* Copyright (C) 2010 Mathieu Labbe
*
* utilite is free library: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* utilite is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include "rtabmap/utilite/UThread.h"
#include "rtabmap/utilite/ULogger.h"
#ifdef __APPLE__
#include <mach/thread_policy.h>
#include <mach/mach.h>
#endif
#define PRINT_DEBUG 0
////////////////////////////
// public:
////////////////////////////
UThread::UThread(Priority priority) :
state_(kSIdle),
priority_(priority),
handle_(0),
threadId_(0),
cpuAffinity_(-1)
{}
UThread::~UThread()
{
#if PRINT_DEBUG
ULOGGER_DEBUG("");
#endif
}
void UThread::kill()
{
#if PRINT_DEBUG
ULOGGER_DEBUG("");
#endif
killSafelyMutex_.lock();
{
if(this->isRunning())
{
// Thread is creating
while(state_ == kSCreating)
{
uSleep(1);
}
if(state_ == kSRunning)
{
state_ = kSKilled;
// Call function to do something before wait
mainLoopKill();
}
else
{
UERROR("thread (%d) is supposed to be running...", threadId_);
}
}
else
{
#if PRINT_DEBUG
UDEBUG("thread (%d) is not running...", threadId_);
#endif
}
}
killSafelyMutex_.unlock();
}
void UThread::join(bool killFirst)
{
//make sure the thread is created
while(this->isCreating())
{
uSleep(1);
}
#ifdef _WIN32
#if PRINT_DEBUG
UDEBUG("Thread %d joining %d", UThreadC<void>::Self(), threadId_);
#endif
if(UThreadC<void>::Self() == threadId_)
#else
#if PRINT_DEBUG
UDEBUG("Thread %d joining %d", UThreadC<void>::Self(), handle_);
#endif
if(UThreadC<void>::Self() == handle_)
#endif
{
UERROR("Thread cannot join itself");
return;
}
if(killFirst)
{
this->kill();
}
runningMutex_.lock();
runningMutex_.unlock();
#if PRINT_DEBUG
UDEBUG("Join ended for %d", UThreadC<void>::Self());
#endif
}
void UThread::start()
{
#if PRINT_DEBUG
ULOGGER_DEBUG("");
#endif
if(state_ == kSIdle || state_ == kSKilled)
{
if(state_ == kSKilled)
{
// make sure it is finished
runningMutex_.lock();
runningMutex_.unlock();
}
state_ = kSCreating;
int r = UThreadC<void>::Create(threadId_, &handle_, true); // Create detached
if(r)
{
UERROR("Failed to create a thread! errno=%d", r);
threadId_=0;
handle_=0;
state_ = kSIdle;
}
else
{
#if PRINT_DEBUG
ULOGGER_DEBUG("StateThread::startThread() thread id=%d _handle=%d", threadId_, handle_);
#endif
}
}
}
//TODO : Support pThread
void UThread::setPriority(Priority priority)
{
priority_ = priority;
}
//TODO : Support pThread
void UThread::applyPriority()
{
if(handle_)
{
#ifdef _WIN32
int p = THREAD_PRIORITY_NORMAL;
switch(priority_)
{
case kPLow:
p = THREAD_PRIORITY_LOWEST;
break;
case kPBelowNormal:
p = THREAD_PRIORITY_BELOW_NORMAL;
break;
case kPNormal:
p = THREAD_PRIORITY_NORMAL;
break;
case kPAboveNormal:
p = THREAD_PRIORITY_ABOVE_NORMAL;
break;
case kPRealTime:
p = THREAD_PRIORITY_TIME_CRITICAL;
break;
default:
break;
}
SetThreadPriority(handle_, p);
#endif
}
}
void UThread::setAffinity(int cpu)
{
cpuAffinity_ = cpu;
if(cpuAffinity_<0)
{
cpuAffinity_ = 0;
}
}
//TODO : Support Windows and linux
void UThread::applyAffinity()
{
if(cpuAffinity_>0)
{
#ifdef _WIN32
#elif __APPLE__
thread_affinity_policy_data_t affPolicy;
affPolicy.affinity_tag = cpuAffinity_;
kern_return_t ret = thread_policy_set(
mach_thread_self(),
THREAD_AFFINITY_POLICY,
(integer_t*) &affPolicy,
THREAD_AFFINITY_POLICY_COUNT);
if(ret != KERN_SUCCESS)
{
UERROR("thread_policy_set returned %d", ret);
}
#else
/*unsigned long mask = cpuAffinity_;
if (pthread_setaffinity_np(
pthread_self(),
sizeof(mask),
&mask) <0)
{
UERROR("pthread_setaffinity_np failed");
}
}*/
#endif
}
}
bool UThread::isCreating() const
{
return state_ == kSCreating;
}
bool UThread::isRunning() const
{
return state_ == kSRunning || state_ == kSCreating;
}
bool UThread::isIdle() const
{
return state_ == kSIdle;
}
bool UThread::isKilled() const
{
return state_ == kSKilled;
}
////////////////////////////
// private:
////////////////////////////
void UThread::ThreadMain()
{
runningMutex_.lock();
applyPriority();
applyAffinity();
#if PRINT_DEBUG
ULOGGER_DEBUG("before mainLoopBegin()");
#endif
state_ = kSRunning;
mainLoopBegin();
#if PRINT_DEBUG
ULOGGER_DEBUG("before mainLoop()");
#endif
while(state_ == kSRunning)
{
mainLoop();
}
#if PRINT_DEBUG
ULOGGER_DEBUG("before mainLoopEnd()");
#endif
mainLoopEnd();
handle_ = 0;
threadId_ = 0;
state_ = kSIdle;
runningMutex_.unlock();
#if PRINT_DEBUG
ULOGGER_DEBUG("Exiting thread loop");
#endif
}
rtabmap/utilite/src/UTimer.cpp
+122
View File
@@ -0,0 +1,122 @@
/*
* utilite is a cross-platform library with
* useful utilities for fast and small developing.
* Copyright (C) 2010 Mathieu Labbe
*
* utilite is free library: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* utilite is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include "rtabmap/utilite/UTimer.h"
#include "rtabmap/utilite/ULogger.h"
///////////////////////
// public:
///////////////////////
UTimer::UTimer()
{
#ifdef _WIN32
QueryPerformanceFrequency(&frequency_);
#endif
start(); // This will initialize the private counters
}
UTimer::~UTimer() {}
#ifdef _WIN32
double UTimer::now()
{
#if defined(_WIN32_WINNT) && (_WIN32_WINNT >= _WIN32_WINNT_WIN8)
FILETIME ft;
GetSystemTimePreciseAsFileTime(&ft);
__int64* val = (__int64*)&ft;
return static_cast<double>(*val) / 10000000.0 - 11644473600.0; // The Windows epoch is Jan 1 1601, the Unix epoch Jan 1 1970.
#else
LARGE_INTEGER count, freq;
QueryPerformanceFrequency(&freq);
QueryPerformanceCounter(&count);
return double(count.QuadPart) / freq.QuadPart;
#endif
}
void UTimer::start()
{
QueryPerformanceCounter(&startTimeRecorded_);
stopTimeRecorded_ = startTimeRecorded_;
}
void UTimer::stop()
{
QueryPerformanceCounter(&stopTimeRecorded_);
}
double UTimer::getElapsedTime()
{
LARGE_INTEGER now;
QueryPerformanceCounter(&now);
return double(now.QuadPart - startTimeRecorded_.QuadPart) / frequency_.QuadPart;
}
double UTimer::getInterval()
{
if(stopTimeRecorded_.QuadPart == startTimeRecorded_.QuadPart)
{
return getElapsedTime();
}
else
{
return double(stopTimeRecorded_.QuadPart - startTimeRecorded_.QuadPart) / frequency_.QuadPart;
}
}
#else
double UTimer::now()
{
struct timeval tv;
gettimeofday(&tv, NULL);
return double(tv.tv_sec) + double(tv.tv_usec) / 1000000.0;
}
void UTimer::start()
{
gettimeofday(&startTimeRecorded_, NULL);
stopTimeRecorded_ = startTimeRecorded_;
}
void UTimer::stop()
{
gettimeofday(&stopTimeRecorded_, NULL);
}
double UTimer::getElapsedTime()
{
return UTimer::now() - (double(startTimeRecorded_.tv_sec) + double(startTimeRecorded_.tv_usec) / 1000000.0);
}
double UTimer::getInterval()
{
if(startTimeRecorded_.tv_sec == stopTimeRecorded_.tv_sec && startTimeRecorded_.tv_usec == stopTimeRecorded_.tv_usec)
{
return getElapsedTime();
}
else
{
double start = double(startTimeRecorded_.tv_sec) + double(startTimeRecorded_.tv_usec) / 1000000.0;
double stop = double(stopTimeRecorded_.tv_sec) + double(stopTimeRecorded_.tv_usec) / 1000000.0;
return stop - start;
}
}
#endif
double UTimer::ticks() // Stop->start and return Interval
{
double inter = elapsed();
start();
return inter;
}
rtabmap/utilite/src/UVariant.cpp
+870
View File
@@ -0,0 +1,870 @@
/*
* utilite is a cross-platform library with
* useful utilities for fast and small developing.
* Copyright (C) 2010 Mathieu Labbe
*
* utilite is free library: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* utilite is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include "rtabmap/utilite/UVariant.h"
#include "rtabmap/utilite/UConversion.h"
#include <limits>
#include <string.h>
UVariant::UVariant() :
type_(kUndef)
{
}
UVariant::UVariant(const bool & value) :
type_(kBool),
data_(1)
{
data_[0] = value?1:0;
}
UVariant::UVariant(const signed char & value) :
type_(kChar),
data_(sizeof(signed char))
{
memcpy(data_.data(), &value, sizeof(signed char));
}
UVariant::UVariant(const unsigned char & value) :
type_(kUChar),
data_(sizeof(unsigned char))
{
memcpy(data_.data(), &value, sizeof(unsigned char));
}
UVariant::UVariant(const short & value) :
type_(kShort),
data_(sizeof(short))
{
memcpy(data_.data(), &value, sizeof(short));
}
UVariant::UVariant(const unsigned short & value) :
type_(kUShort),
data_(sizeof(unsigned short))
{
memcpy(data_.data(), &value, sizeof(unsigned short));
}
UVariant::UVariant(const int & value) :
type_(kInt),
data_(sizeof(int))
{
memcpy(data_.data(), &value, sizeof(int));
}
UVariant::UVariant(const unsigned int & value) :
type_(kUInt),
data_(sizeof(unsigned int))
{
memcpy(data_.data(), &value, sizeof(unsigned int));
}
UVariant::UVariant(const float & value) :
type_(kFloat),
data_(sizeof(float))
{
memcpy(data_.data(), &value, sizeof(float));
}
UVariant::UVariant(const double & value) :
type_(kDouble),
data_(sizeof(double))
{
memcpy(data_.data(), &value, sizeof(double));
}
UVariant::UVariant(const char * value) :
type_(kStr)
{
std::string str(value);
data_.resize(str.size()+1);
memcpy(data_.data(), str.data(), str.size()+1);
}
UVariant::UVariant(const std::string & value) :
type_(kStr),
data_(value.size()+1) // with null character
{
memcpy(data_.data(), value.data(), value.size()+1);
}
UVariant::UVariant(const std::vector<signed char> & value) :
type_(kCharArray),
data_(sizeof(signed char)*value.size())
{
memcpy(data_.data(), value.data(), sizeof(signed char)*value.size());
}
UVariant::UVariant(const std::vector<unsigned char> & value) :
type_(kUCharArray),
data_(sizeof(unsigned char)*value.size())
{
memcpy(data_.data(), value.data(), sizeof(unsigned char)*value.size());
}
UVariant::UVariant(const std::vector<short> & value) :
type_(kShortArray),
data_(sizeof(short)*value.size())
{
memcpy(data_.data(), value.data(), sizeof(short)*value.size());
}
UVariant::UVariant(const std::vector<unsigned short> & value) :
type_(kUShortArray),
data_(sizeof(unsigned short)*value.size())
{
memcpy(data_.data(), value.data(), sizeof(unsigned short)*value.size());
}
UVariant::UVariant(const std::vector<int> & value) :
type_(kIntArray),
data_(sizeof(int)*value.size())
{
memcpy(data_.data(), value.data(), sizeof(int)*value.size());
}
UVariant::UVariant(const std::vector<unsigned int> & value) :
type_(kUIntArray),
data_(sizeof(unsigned int)*value.size())
{
memcpy(data_.data(), value.data(), sizeof(unsigned int)*value.size());
}
UVariant::UVariant(const std::vector<float> & value) :
type_(kFloatArray),
data_(sizeof(float)*value.size())
{
memcpy(data_.data(), value.data(), sizeof(float)*value.size());
}
UVariant::UVariant(const std::vector<double> & value) :
type_(kDoubleArray),
data_(sizeof(double)*value.size())
{
memcpy(data_.data(), value.data(), sizeof(double)*value.size());
}
bool UVariant::toBool() const
{
if(type_ ==kStr)
{
return uStr2Bool(toStr().c_str());
}
else if(data_.size())
{
return memcmp(data_.data(), std::vector<unsigned char>(data_.size(), 0).data(), data_.size()) != 0;
}
return false;
}
signed char UVariant::toChar(bool * ok) const
{
if(ok)
{
*ok = false;
}
signed char v = 0;
if(type_ == kChar)
{
memcpy(&v, data_.data(), sizeof(signed char));
if(ok)
{
*ok = true;
}
}
else if(type_ == kUChar)
{
unsigned char tmp = toUChar();
if(tmp <= std::numeric_limits<signed char>::max())
{
v = (signed char)tmp;
if(ok)
{
*ok = true;
}
}
}
else if(type_ == kShort)
{
short tmp = toShort();
if(tmp >= std::numeric_limits<signed char>::min() && tmp <= std::numeric_limits<signed char>::max())
{
v = (signed char)tmp;
if(ok)
{
*ok = true;
}
}
}
else if(type_ == kUShort)
{
unsigned short tmp = toUShort();
if(tmp <= std::numeric_limits<signed char>::max())
{
v = (signed char)tmp;
if(ok)
{
*ok = true;
}
}
}
else if(type_ == kInt)
{
int tmp = toInt();
if(tmp >= std::numeric_limits<signed char>::min() && tmp <= std::numeric_limits<signed char>::max())
{
v = (signed char)tmp;
if(ok)
{
*ok = true;
}
}
}
else if(type_ == kUInt)
{
unsigned int tmp = toUInt();
if(tmp <= (unsigned int)std::numeric_limits<signed char>::max())
{
v = (signed char)tmp;
if(ok)
{
*ok = true;
}
}
}
return v;
}
unsigned char UVariant::toUChar(bool * ok) const
{
if(ok)
{
*ok = false;
}
unsigned char v = 0;
if(type_ == kUChar)
{
memcpy(&v, data_.data(), sizeof(unsigned char));
if(ok)
{
*ok = true;
}
}
else if(type_ == kChar)
{
char tmp = toChar();
if(tmp >= std::numeric_limits<unsigned char>::min())
{
v = (unsigned char)tmp;
if(ok)
{
*ok = true;
}
}
}
else if(type_ == kShort)
{
short tmp = toShort();
if(tmp >= std::numeric_limits<unsigned char>::min() && tmp <= std::numeric_limits<unsigned char>::max())
{
v = (unsigned char)tmp;
if(ok)
{
*ok = true;
}
}
}
else if(type_ == kUShort)
{
unsigned short tmp = toUShort();
if(tmp >= std::numeric_limits<unsigned char>::min() && tmp <= std::numeric_limits<unsigned char>::max())
{
v = (unsigned char)tmp;
if(ok)
{
*ok = true;
}
}
}
else if(type_ == kInt)
{
int tmp = toInt();
if(tmp >= std::numeric_limits<unsigned char>::min() && tmp <= std::numeric_limits<unsigned char>::max())
{
v = (unsigned char)tmp;
if(ok)
{
*ok = true;
}
}
}
else if(type_ == kUInt)
{
unsigned int tmp = toUInt();
if(tmp <= std::numeric_limits<unsigned char>::max())
{
v = (unsigned char)tmp;
if(ok)
{
*ok = true;
}
}
}
return v;
}
short UVariant::toShort(bool * ok) const
{
if(ok)
{
*ok = false;
}
short v = 0;
if(type_ == kShort)
{
memcpy(&v, data_.data(), sizeof(short));
if(ok)
{
*ok = true;
}
}
else if(type_ == kChar)
{
v = (short)toChar();
if(ok)
{
*ok = true;
}
}
else if(type_ == kUChar)
{
v = (short)toUChar();
if(ok)
{
*ok = true;
}
}
else if(type_ == kUShort)
{
unsigned short tmp = toUShort();
if(tmp <= std::numeric_limits<short>::max())
{
v = (short)tmp;
if(ok)
{
*ok = true;
}
}
}
else if(type_ == kInt)
{
int tmp = toInt();
if(tmp >= std::numeric_limits<short>::min() && tmp <= std::numeric_limits<short>::max())
{
v = (short)tmp;
if(ok)
{
*ok = true;
}
}
}
else if(type_ == kUInt)
{
unsigned int tmp = toUInt();
if(tmp <= (unsigned int)std::numeric_limits<short>::max())
{
v = (short)tmp;
if(ok)
{
*ok = true;
}
}
}
return v;
}
unsigned short UVariant::toUShort(bool * ok) const
{
if(ok)
{
*ok = false;
}
unsigned short v = 0;
if(type_ == kUShort)
{
memcpy(&v, data_.data(), sizeof(unsigned short));
if(ok)
{
*ok = true;
}
}
else if(type_ == kChar)
{
signed char tmp = toChar();
if(tmp >= 0)
{
v = (unsigned short)tmp;
if(ok)
{
*ok = true;
}
}
}
else if(type_ == kUChar)
{
v = (unsigned short)toUChar();
if(ok)
{
*ok = true;
}
}
else if(type_ == kShort)
{
short tmp = toShort();
if(tmp >= std::numeric_limits<unsigned short>::min())
{
v = (unsigned short)tmp;
if(ok)
{
*ok = true;
}
}
}
else if(type_ == kInt)
{
int tmp = toInt();
if(tmp >= std::numeric_limits<unsigned short>::min() && tmp <= std::numeric_limits<unsigned short>::max())
{
v = (unsigned short)tmp;
if(ok)
{
*ok = true;
}
}
}
else if(type_ == kUInt)
{
unsigned int tmp = toUInt();
if(tmp <= std::numeric_limits<unsigned short>::max())
{
v = (unsigned short)tmp;
if(ok)
{
*ok = true;
}
}
}
return v;
}
int UVariant::toInt(bool * ok) const
{
if(ok)
{
*ok = false;
}
int v = 0;
if(type_ == kInt)
{
memcpy(&v, data_.data(), sizeof(int));
if(ok)
{
*ok = true;
}
}
else if(type_ == kChar)
{
v = (int)toChar();
if(ok)
{
*ok = true;
}
}
else if(type_ == kUChar)
{
v = (int)toUChar();
if(ok)
{
*ok = true;
}
}
else if(type_ == kShort)
{
v = (int)toShort();
if(ok)
{
*ok = true;
}
}
else if(type_ == kUShort)
{
v = (int)toUShort();
if(ok)
{
*ok = true;
}
}
else if(type_ == kUInt)
{
unsigned int tmp = toUInt();
if(tmp <= (unsigned int)std::numeric_limits<int>::max())
{
v = (int)tmp;
if(ok)
{
*ok = true;
}
}
}
return v;
}
unsigned int UVariant::toUInt(bool * ok) const
{
if(ok)
{
*ok = false;
}
unsigned int v = 0;
if(type_ == kUInt)
{
memcpy(&v, data_.data(), sizeof(unsigned int));
if(ok)
{
*ok = true;
}
}
else if(type_ == kChar)
{
signed char tmp = toChar();
if(tmp >= 0)
{
v = (unsigned int)tmp;
if(ok)
{
*ok = true;
}
}
}
else if(type_ == kUChar)
{
v = (unsigned int)toUChar();
if(ok)
{
*ok = true;
}
}
else if(type_ == kShort)
{
short tmp = toShort();
if(tmp >= 0)
{
v = (unsigned int)tmp;
if(ok)
{
*ok = true;
}
}
}
else if(type_ == kUShort)
{
v = (unsigned int)toUShort();
if(ok)
{
*ok = true;
}
}
else if(type_ == kInt)
{
int tmp = toInt();
if(tmp >= 0)
{
v = (unsigned int)tmp;
if(ok)
{
*ok = true;
}
}
}
return v;
}
float UVariant::toFloat(bool * ok) const
{
if(ok)
{
*ok = false;
}
float v = 0;
if(type_ == kFloat)
{
memcpy(&v, data_.data(), sizeof(float));
if(ok)
{
*ok = true;
}
}
else if(type_ == kDouble)
{
double tmp = toDouble();
if(tmp >= std::numeric_limits<float>::min() && tmp <= std::numeric_limits<float>::max())
{
v = (float)tmp;
if(ok)
{
*ok = true;
}
}
}
return v;
}
double UVariant::toDouble(bool * ok) const
{
if(ok)
{
*ok = false;
}
double v = 0;
if(type_ == kDouble)
{
memcpy(&v, data_.data(), sizeof(double));
if(ok)
{
*ok = true;
}
}
else if(type_ == kFloat)
{
v = (double)toFloat(ok);
}
return v;
}
std::string UVariant::toStr(bool * ok) const
{
if(ok)
{
*ok = false;
}
std::string v;
if(type_ == kStr)
{
v = std::string((const char *)data_.data());
if(ok)
{
*ok = true;
}
}
else if(type_ == kBool)
{
v = toBool()?"true":"false";
if(ok)
{
*ok = true;
}
}
else if(type_ == kChar)
{
v = " ";
v.at(0) = toChar(ok);
}
else if(type_ == kUChar)
{
v = uNumber2Str(toUChar(ok));
}
else if(type_ == kShort)
{
v = uNumber2Str(toShort(ok));
}
else if(type_ == kUShort)
{
v = uNumber2Str(toUShort(ok));
}
else if(type_ == kInt)
{
v = uNumber2Str(toInt(ok));
}
else if(type_ == kUInt)
{
v = uNumber2Str(toUInt(ok));
}
else if(type_ == kFloat)
{
v = uNumber2Str(toFloat(ok));
}
else if(type_ == kDouble)
{
v = uNumber2Str(toDouble(ok));
}
return v;
}
std::vector<signed char> UVariant::toCharArray(bool * ok) const
{
if(ok)
{
*ok = false;
}
std::vector<signed char> v;
if(type_ == kCharArray)
{
if(ok)
{
*ok = true;
}
if(data_.size())
{
v.resize(data_.size() / sizeof(signed char));
memcpy(v.data(), data_.data(), data_.size());
}
}
return v;
}
std::vector<unsigned char> UVariant::toUCharArray(bool * ok) const
{
if(ok)
{
*ok = false;
}
std::vector<unsigned char> v;
if(type_ == kUCharArray)
{
if(ok)
{
*ok = true;
}
if(data_.size())
{
v.resize(data_.size() / sizeof(unsigned char));
memcpy(v.data(), data_.data(), data_.size());
}
}
return v;
}
std::vector<short> UVariant::toShortArray(bool * ok) const
{
if(ok)
{
*ok = false;
}
std::vector<short> v;
if(type_ == kShortArray)
{
if(ok)
{
*ok = true;
}
if(data_.size())
{
v.resize(data_.size() / sizeof(short));
memcpy(v.data(), data_.data(), data_.size());
}
}
return v;
}
std::vector<unsigned short> UVariant::toUShortArray(bool * ok) const
{
if(ok)
{
*ok = false;
}
std::vector<unsigned short> v;
if(type_ == kUShortArray)
{
if(ok)
{
*ok = true;
}
if(data_.size())
{
v.resize(data_.size() / sizeof(unsigned short));
memcpy(v.data(), data_.data(), data_.size());
}
}
return v;
}
std::vector<int> UVariant::toIntArray(bool * ok) const
{
if(ok)
{
*ok = false;
}
std::vector<int> v;
if(type_ == kIntArray)
{
if(ok)
{
*ok = true;
}
if(data_.size())
{
v.resize(data_.size() / sizeof(int));
memcpy(v.data(), data_.data(), data_.size());
}
}
return v;
}
std::vector<unsigned int> UVariant::toUIntArray(bool * ok) const
{
if(ok)
{
*ok = false;
}
std::vector<unsigned int> v;
if(type_ == kUIntArray)
{
if(ok)
{
*ok = true;
}
if(data_.size())
{
v.resize(data_.size() / sizeof(unsigned int));
memcpy(v.data(), data_.data(), data_.size());
}
}
return v;
}
std::vector<float> UVariant::toFloatArray(bool * ok) const
{
if(ok)
{
*ok = false;
}
std::vector<float> v;
if(type_ == kFloatArray)
{
if(ok)
{
*ok = true;
}
if(data_.size())
{
v.resize(data_.size() / sizeof(float));
memcpy(v.data(), data_.data(), data_.size());
}
}
return v;
}
std::vector<double> UVariant::toDoubleArray(bool * ok) const
{
if(ok)
{
*ok = false;
}
std::vector<double> v;
if(type_ == kDoubleArray)
{
if(ok)
{
*ok = true;
}
if(data_.size())
{
v.resize(data_.size() / sizeof(double));
memcpy(v.data(), data_.data(), data_.size());
}
}
return v;
}