i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 01:20:25 +08:00
Code Issues Packages Projects Releases Wiki Activity

Included UtiLite library directly in Rtabmap source (to simplify the installation)

Browse Source 
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@856 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2013-04-30 20:17:42 +00:00
parent b92d0444ac
commit b675d2672b
14 changed files with 8899 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

101
guilib/src/utilite/UImageView.h Normal file
View File

@@ -0,0 +1,101 @@
/*
* ImageView.h
*
* Created on: 2012-06-20
* Author: mathieu
*/
#ifndef IMAGEVIEW_H_
#define IMAGEVIEW_H_
#include <QtGui/QWidget>
#include <QtGui/QPainter>
class UImageView : public QWidget
{
Q_OBJECT;
public:
UImageView(QWidget * parent = 0) : QWidget(parent) {}
~UImageView() {}
void setBackgroundBrush(const QBrush & brush) {brush_ = brush;}
public slots:
void setImage(const QImage & image)
{
if(pixmap_.width() != image.width() || pixmap_.height() != image.height())
{
this->setMinimumSize(image.width(), image.height());
this->setGeometry(this->geometry().x(), this->geometry().y(), image.width(), image.height());
}
pixmap_ = QPixmap::fromImage(image);
this->update();
}
private:
void computeScaleOffsets(float & scale, float & offsetX, float & offsetY)
{
scale = 1.0f;
offsetX = 0.0f;
offsetY = 0.0f;
if(!pixmap_.isNull())
{
float w = pixmap_.width();
float h = pixmap_.height();
float widthRatio = float(this->rect().width()) / w;
float heightRatio = float(this->rect().height()) / h;
if(widthRatio < heightRatio)
{
scale = widthRatio;
}
else
{
scale = heightRatio;
}
w *= scale;
h *= scale;
if(w < this->rect().width())
{
offsetX = (this->rect().width() - w)/2.0f;
}
if(h < this->rect().height())
{
offsetY = (this->rect().height() - h)/2.0f;
}
}
}
protected:
virtual void paintEvent(QPaintEvent *event)
{
QPainter painter(this);
//Draw background
painter.save();
painter.setBrush(brush_);
painter.drawRect(this->rect());
painter.restore();
if(!pixmap_.isNull())
{
painter.save();
//Scale
float ratio, offsetX, offsetY;
this->computeScaleOffsets(ratio, offsetX, offsetY);
painter.translate(offsetX, offsetY);
painter.scale(ratio, ratio);
painter.drawPixmap(QPoint(0,0), pixmap_);
painter.restore();
}
}
private:
QPixmap pixmap_;
QBrush brush_;
};
#endif /* IMAGEVIEW_H_ */

2979
guilib/src/utilite/UPlot.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

624
guilib/src/utilite/UPlot.h Normal file
View File

@@ -0,0 +1,624 @@
/*
* 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 UPLOT_H_
#define UPLOT_H_
#include "rtabmap/utilite/UtiLiteExp.h" // DLL export/import defines
#include <QtGui/QFrame>
#include <QtCore/QList>
#include <QtCore/QMap>
#include <QtGui/QPen>
#include <QtGui/QBrush>
#include <QtGui/QGraphicsEllipseItem>
#include <QtCore/QMutex>
#include <QtGui/QLabel>
#include <QtGui/QPushButton>
#include <QtCore/QTime>
class QGraphicsView;
class QGraphicsScene;
class QGraphicsItem;
class QFormLayout;
/**
* UPlotItem is a QGraphicsEllipseItem and can be inherited to do custom behaviors
* on an hoverEnterEvent() for example.
*/
class UTILITE_EXP UPlotItem : public QGraphicsEllipseItem
{
public:
/**
* Constructor 1.
*/
UPlotItem(qreal dataX, qreal dataY, qreal width=2);
/**
* Constructor 2.
*/
UPlotItem(const QPointF & data, qreal width=2);
virtual ~UPlotItem();
public:
void setNextItem(UPlotItem * nextItem);
void setPreviousItem(UPlotItem * previousItem);
void setData(const QPointF & data);
UPlotItem * nextItem() const {return _nextItem;}
UPlotItem * previousItem() const {return _previousItem;};
const QPointF & data() const {return _data;}
protected:
virtual void hoverEnterEvent(QGraphicsSceneHoverEvent * event);
virtual void hoverLeaveEvent(QGraphicsSceneHoverEvent * event);
virtual void focusInEvent(QFocusEvent * event);
virtual void focusOutEvent(QFocusEvent * event);
virtual void keyReleaseEvent(QKeyEvent * keyEvent);
virtual void showDescription(bool shown);
private:
void init(qreal dataX, qreal dataY);
private:
QPointF _data;
UPlotItem * _previousItem;
UPlotItem * _nextItem;
QGraphicsTextItem * _text;
QGraphicsRectItem * _textBackground;
};
class UPlot;
/**
* UPlotCurve is a curve used to hold data shown in a UPlot.
*/
class UTILITE_EXP UPlotCurve : public QObject
{
Q_OBJECT
public:
/**
* Constructor 1
*/
UPlotCurve(const QString & name, QObject * parent = 0);
/**
* Constructor 2
*/
UPlotCurve(const QString & name, const QVector<UPlotItem *> data, QObject * parent = 0);
/**
* Constructor 3
*/
UPlotCurve(const QString & name, const QVector<float> & x, const QVector<float> & y, QObject * parent = 0);
virtual ~UPlotCurve();
/**
* Get pen.
*/
const QPen & pen() const {return _pen;}
/**
* Get brush.
*/
const QBrush & brush() const {return _brush;}
/**
* Set pen.
*/
void setPen(const QPen & pen);
/**
* Set brush.
*/
void setBrush(const QBrush & brush);
void setItemsColor(const QColor & color);
QColor itemsColor() const {return _itemsColor;}
/**
* Get name.
*/
QString name() const {return _name;}
/**
* Get the number of items in the curve (dot + line items).
*/
int itemsSize() const;
QPointF getItemData(int index);
bool isVisible() const {return _visible;}
void setData(QVector<UPlotItem*> & data); // take the ownership
void getData(QVector<float> & x, QVector<float> & y) const; // only call in Qt MainThread
void draw(QPainter * painter, const QRect & limits);
public slots:
/**
*
* Clear curve's values.
*/
virtual void clear();
/**
*
* Show or hide the curve.
*/
void setVisible(bool visible);
/**
*
* Set increment of the x values (when auto-increment is used).
*/
void setXIncrement(float increment);
/**
*
* Set starting x value (when auto-increment is used).
*/
void setXStart(float val);
/**
*
* Add a single value, using a custom UPlotItem.
*/
void addValue(UPlotItem * data); // take the ownership
/**
*
* Add a single value y, x is auto-incremented by the increment set with setXIncrement().
* @see setXStart()
*/
void addValue(float y);
/**
*
* Add a single value y at x.
*/
void addValue(float x, float y);
/**
*
* For convenience...
* Add a single value y, x is auto-incremented by the increment set with setXIncrement().
* @see setXStart()
*/
void addValue(const QString & y);
/**
*
* For convenience...
* Add multiple values, using custom UPlotItem.
*/
void addValues(QVector<UPlotItem *> & data); // take the ownership
/**
*
* Add multiple values y at x. Vectors must have the same size.
*/
void addValues(const QVector<float> & xs, const QVector<float> & ys);
/**
*
* Add multiple values y, x is auto-incremented by the increment set with setXIncrement().
* @see setXStart()
*/
void addValues(const QVector<float> & ys);
void addValues(const QVector<int> & ys); // for convenience
/**
*
* Add multiple values y, x is auto-incremented by the increment set with setXIncrement().
* @see setXStart()
*/
void addValues(const std::vector<float> & ys); // for convenience
void addValues(const std::vector<int> & ys); // for convenience
void setData(const QVector<float> & x, const QVector<float> & y);
void setData(const std::vector<float> & x, const std::vector<float> & y);
void setData(const QVector<float> & y);
void setData(const std::vector<float> & y);
signals:
/**
*
* emitted when data is changed.
*/
void dataChanged(const UPlotCurve *);
protected:
friend class UPlot;
void attach(UPlot * plot);
void detach(UPlot * plot);
void updateMinMax();
const QVector<float> & getMinMax() const {return _minMax;}
int removeItem(int index);
void _addValue(UPlotItem * data);;
virtual bool isMinMaxValid() const {return _minMax.size();}
virtual void update(float scaleX, float scaleY, float offsetX, float offsetY, float xDir, float yDir, int maxItemsKept);
QList<QGraphicsItem *> _items;
UPlot * _plot;
private:
void removeItem(UPlotItem * item);
private:
QString _name;
QPen _pen;
QBrush _brush;
float _xIncrement;
float _xStart;
bool _visible;
bool _valuesShown;
QVector<float> _minMax; // minX, maxX, minY, maxY
QGraphicsRectItem * _rootItem;
QColor _itemsColor;
};
/**
* A special UPlotCurve that shows as a line at the specified value, spanning all the UPlot.
*/
class UTILITE_EXP UPlotCurveThreshold : public UPlotCurve
{
Q_OBJECT
public:
/**
* Constructor.
*/
UPlotCurveThreshold(const QString & name, float thesholdValue, Qt::Orientation orientation = Qt::Horizontal, QObject * parent = 0);
virtual ~UPlotCurveThreshold();
public slots:
/**
* Set threshold value.
*/
void setThreshold(float threshold);
/**
* Set orientation (Qt::Horizontal or Qt::Vertical).
*/
void setOrientation(Qt::Orientation orientation);
protected:
friend class UPlot;
virtual void update(float scaleX, float scaleY, float offsetX, float offsetY, float xDir, float yDir, int maxItemsKept);
virtual bool isMinMaxValid() const {return false;}
private:
Qt::Orientation _orientation;
};
/**
* The UPlot axis object.
*/
class UTILITE_EXP UPlotAxis : public QWidget
{
public:
/**
* Constructor.
*/
UPlotAxis(Qt::Orientation orientation = Qt::Horizontal, float min=0, float max=1, QWidget * parent = 0);
virtual ~UPlotAxis();
public:
/**
* Set axis minimum and maximum values, compute the resulting
* intervals depending on the size of the axis.
*/
void setAxis(float & min, float & max);
/**
* Size of the border between the first line and the beginning of the widget.
*/
int border() const {return _border;}
/**
* Interval step value.
*/
int step() const {return _step;}
/**
* Number of intervals.
*/
int count() const {return _count;}
/**
* Reverse the axis (for vertical :bottom->up, for horizontal :right->left)
*/
void setReversed(bool reversed); // Vertical :bottom->up, horizontal :right->left
protected:
virtual void paintEvent(QPaintEvent * event);
private:
Qt::Orientation _orientation;
float _min;
float _max;
int _count;
int _step;
bool _reversed;
int _gradMaxDigits;
int _border;
};
/**
* The UPlot legend item. Used internally by UPlot.
*/
class UTILITE_EXP UPlotLegendItem : public QPushButton
{
Q_OBJECT
public:
/**
* Constructor.
*/
UPlotLegendItem(UPlotCurve * curve, QWidget * parent = 0);
virtual ~UPlotLegendItem();
const UPlotCurve * curve() const {return _curve;}
QPixmap createSymbol(const QPen & pen, const QBrush & brush);
signals:
void legendItemRemoved(const UPlotCurve *);
void moveUpRequest(UPlotLegendItem *);
void moveDownRequest(UPlotLegendItem *);
protected:
virtual void contextMenuEvent(QContextMenuEvent * event);
private:
UPlotCurve * _curve;
QMenu * _menu;
QAction * _aChangeText;
QAction * _aResetText;
QAction * _aChangeColor;
QAction * _aCopyToClipboard;
QAction * _aRemoveCurve;
QAction * _aMoveUp;
QAction * _aMoveDown;
};
/**
* The UPlot legend. Used internally by UPlot.
*/
class UTILITE_EXP UPlotLegend : public QWidget
{
Q_OBJECT
public:
/**
* Constructor.
*/
UPlotLegend(QWidget * parent = 0);
virtual ~UPlotLegend();
void setFlat(bool on);
bool isFlat() const {return _flat;}
void addItem(UPlotCurve * curve);
bool remove(const UPlotCurve * curve);
private slots:
void removeLegendItem(const UPlotCurve * curve);
void moveUp(UPlotLegendItem * item);
void moveDown(UPlotLegendItem * item);
signals:
void legendItemRemoved(const UPlotCurve * curve);
void legendItemToggled(const UPlotCurve * curve, bool toggled);
void legendItemMoved(const UPlotCurve * curve, int);
protected:
virtual void contextMenuEvent(QContextMenuEvent * event);
private slots:
void redirectToggled(bool);
private:
bool _flat;
QMenu * _menu;
QAction * _aUseFlatButtons;
};
/**
* Orientable QLabel. Inherit QLabel and let you to specify the orientation.
*/
class UTILITE_EXP UOrientableLabel : public QLabel
{
Q_OBJECT
public:
/**
* Constructor.
*/
UOrientableLabel(const QString & text, Qt::Orientation orientation = Qt::Horizontal, QWidget * parent = 0);
virtual ~UOrientableLabel();
/**
* Get orientation.
*/
Qt::Orientation orientation() const {return _orientation;}
/**
* Set orientation (Qt::Vertical or Qt::Horizontal).
*/
void setOrientation(Qt::Orientation orientation);
QSize sizeHint() const;
QSize minimumSizeHint() const;
protected:
virtual void paintEvent(QPaintEvent* event);
private:
Qt::Orientation _orientation;
};
/**
* UPlot is a QWidget to create a plot like MATLAB, and
* incrementally add new values like a scope using Qt signals/slots.
* Many customizations can be done at runtime with the right-click menu.
* @image html UPlot.gif
* @image html UPlotMenu.png
*
* Example:
* @code
* #include "utilite/UPlot.h"
* #include <QApplication>
*
* int main(int argc, char * argv[])
* {
* QApplication app(argc, argv);
* UPlot plot;
* UPlotCurve * curve = plot.addCurve("My curve");
* float y[10] = {0, 1, 2, 3, -3, -2, -1, 0, 1, 2};
* curve->addValues(std::vector<float>(y, y+10));
* plot.showGrid(true);
* plot.setGraphicsView(true);
* plot.show();
* app.exec();
* return 0;
* }
* @endcode
* @image html SimplePlot.tiff
*
*
*/
class UTILITE_EXP UPlot : public QWidget
{
Q_OBJECT
public:
/**
* Constructor.
*/
UPlot(QWidget * parent = 0);
virtual ~UPlot();
/**
* Add a curve. The returned curve doesn't need to be deallocated (UPlot keeps the ownership).
*/
UPlotCurve * addCurve(const QString & curveName, const QColor & color = QColor());
/**
* Add a curve. Ownership is transferred to UPlot if ownershipTransferred=true.
*/
bool addCurve(UPlotCurve * curve, bool ownershipTransferred = true);
/**
* Get all curve names.
*/
QStringList curveNames();
bool contains(const QString & curveName);
void removeCurves();
/**
* Add a threshold to the plot.
*/
UPlotCurveThreshold * addThreshold(const QString & name, float value, Qt::Orientation orientation = Qt::Horizontal);
QString title() const {return this->objectName();}
QPen getRandomPenColored();
void showLegend(bool shown);
void showGrid(bool shown);
void showRefreshRate(bool shown);
void trackMouse(bool tracking);
void keepAllData(bool kept);
void showXAxis(bool shown) {_horizontalAxis->setVisible(shown);}
void showYAxis(bool shown) {_verticalAxis->setVisible(shown);}
void setVariableXAxis() {_fixedAxis[0] = false;}
void setVariableYAxis() {_fixedAxis[1] = false;}
void setFixedXAxis(float x1, float x2);
void setFixedYAxis(float y1, float y2);
void setMaxVisibleItems(int maxVisibleItems);
void setTitle(const QString & text);
void setXLabel(const QString & text);
void setYLabel(const QString & text, Qt::Orientation orientation = Qt::Vertical);
void setWorkingDirectory(const QString & workingDirectory);
void setGraphicsView(bool on);
void setBackgroundColor(const QColor & color);
QRectF sceneRect() const;
public slots:
/**
*
* Remove a curve. If UPlot is the parent of the curve, the curve is deleted.
*/
void removeCurve(const UPlotCurve * curve);
void showCurve(const UPlotCurve * curve, bool shown);
void updateAxis(); //reset axis and recompute it with all curves minMax
/**
*
* Clear all curves' data.
*/
void clearData();
private slots:
void captureScreen();
void updateAxis(const UPlotCurve * curve);
void moveCurve(const UPlotCurve *, int index);
protected:
virtual void contextMenuEvent(QContextMenuEvent * event);
virtual void paintEvent(QPaintEvent * event);
virtual void resizeEvent(QResizeEvent * event);
virtual void mousePressEvent(QMouseEvent * event);
virtual void mouseMoveEvent(QMouseEvent * event);
virtual void mouseReleaseEvent(QMouseEvent * event);
virtual void mouseDoubleClickEvent(QMouseEvent * event);
private:
friend class UPlotCurve;
void addItem(QGraphicsItem * item);
private:
void replot(QPainter * painter);
bool updateAxis(float x, float y);
bool updateAxis(float x1, float x2, float y1, float y2);
void setupUi();
void createActions();
void createMenus();
void selectScreenCaptureFormat();
bool mousePosToValue(const QPoint & pos, float & x, float & y);
private:
UPlotLegend * _legend;
QGraphicsView * _view;
QGraphicsItem * _sceneRoot;
QWidget * _graphicsViewHolder;
float _axisMaximums[4]; // {x1->x2, y1->y2}
bool _axisMaximumsSet[4]; // {x1->x2, y1->y2}
bool _fixedAxis[2];
UPlotAxis * _verticalAxis;
UPlotAxis * _horizontalAxis;
int _penStyleCount;
int _maxVisibleItems;
QList<QGraphicsLineItem *> hGridLines;
QList<QGraphicsLineItem *> vGridLines;
QList<UPlotCurve*> _curves;
QLabel * _title;
QLabel * _xLabel;
UOrientableLabel * _yLabel;
QLabel * _refreshRate;
QString _workingDirectory;
QTime _refreshIntervalTime;
int _lowestRefreshRate;
QTime _refreshStartTime;
QString _autoScreenCaptureFormat;
QPoint _mousePressedPos;
QPoint _mouseCurrentPos;
QColor _bgColor;
QMenu * _menu;
QAction * _aShowLegend;
QAction * _aShowGrid;
QAction * _aKeepAllData;
QAction * _aLimit0;
QAction * _aLimit10;
QAction * _aLimit50;
QAction * _aLimit100;
QAction * _aLimit500;
QAction * _aLimit1000;
QAction * _aLimitCustom;
QAction * _aAddVerticalLine;
QAction * _aAddHorizontalLine;
QAction * _aChangeTitle;
QAction * _aChangeXLabel;
QAction * _aChangeYLabel;
QAction * _aChangeBackgroundColor;
QAction * _aYLabelVertical;
QAction * _aShowRefreshRate;
QAction * _aMouseTracking;
QAction * _aSaveFigure;
QAction * _aAutoScreenCapture;
QAction * _aClearData;
QAction * _aGraphicsView;
};
#endif /* UPLOT_H_ */

42
utilite/src/CMakeLists.txt Normal file
View File

@@ -0,0 +1,42 @@
SET(SRC_FILES
UEventsManager.cpp
UEventsHandler.cpp
UFile.cpp
UDirectory.cpp
UConversion.cpp
ULogger.cpp
UThread.cpp
UTimer.cpp
UProcessInfo.cpp
)
SET(INCLUDE_DIRS
${CMAKE_CURRENT_SOURCE_DIR}/../include
${PTHREADS_INCLUDE_DIR}
)
# Make sure the compiler can find include files from our library.
INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
ADD_LIBRARY(rtabmap_utilite ${SRC_FILES})
IF(WIN32)
TARGET_LINK_LIBRARIES(rtabmap_utilite ${PTHREADS_LIBRARY} ${LIBRARIES} "-lpsapi")
ELSE(WIN32)
TARGET_LINK_LIBRARIES(rtabmap_utilite ${PTHREADS_LIBRARY} ${LIBRARIES})
ENDIF(WIN32)
SET_TARGET_PROPERTIES(
rtabmap_utilite
PROPERTIES
OUTPUT_NAME ${PROJECT_PREFIX}_utilite
INSTALL_NAME_DIR ${CMAKE_INSTALL_PREFIX}/lib
)
INSTALL(TARGETS rtabmap_utilite
RUNTIME DESTINATION bin COMPONENT runtime
LIBRARY DESTINATION lib COMPONENT devel
ARCHIVE DESTINATION lib COMPONENT devel)
install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/../include/ DESTINATION include/ COMPONENT devel FILES_MATCHING PATTERN "*.h" PATTERN ".svn" EXCLUDE)

320
utilite/src/UConversion.cpp Normal file
View File

@@ -0,0 +1,320 @@
/*
* 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 <sstream>
#include <string.h>
#include "rtabmap/utilite/ULogger.h"
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)
{
std::stringstream s;
s << number;
return s.str();
}
std::string uNumber2Str(double number)
{
std::stringstream s;
s << number;
return s.str();
}
std::string uBool2Str(bool boolean)
{
std::string s;
if(boolean)
{
s = "true";
}
else
{
s = "false";
}
return s;
}
bool uStr2Bool(const char * str)
{
return !(str && (strcmp(str, "false") == 0 || strcmp(str, "FALSE") == 0 || strcmp(str, "0") == 0));
}
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], 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) {
va_copy(argsTmp, args);
// Try to vsnprintf into our buffer.
int needed;
if(argsTmp != 0)
{
needed = vsnprintf (buf, size, fmt, argsTmp);
}
else
{
needed = snprintf (buf, size, "%s", fmt);
}
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

375
utilite/src/UDirectory.cpp Normal file
View File

@@ -0,0 +1,375 @@
/*
* 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 <sys/dir.h>
#include <unistd.h>
#include <stdlib.h>
#include <string.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 void * aa, const void * bb)
{
const struct dirent ** a = (const struct dirent **)aa;
const struct dirent ** b = (const struct dirent **)bb;
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;
}
void UDirectory::rewind()
{
iFileName_ = fileNames_.begin();
}
bool UDirectory::exists(const std::string & dirPath)
{
bool r = false;
#if 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=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;
#if 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;
#if WIN32
status = _rmdir(dirPath.c_str());
#else
status = rmdir(dirPath.c_str());
#endif
return status==0;
}
std::string UDirectory::homeDir()
{
std::string path;
#if 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
path = getenv("HOME");
#endif
return path;
}
std::string UDirectory::separator()
{
#ifdef WIN32
return "\\";
#else
return "/";
#endif
}

31
utilite/src/UEventsHandler.cpp Normal file
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/UEventsHandler.h"
#include "rtabmap/utilite/UEventsManager.h"
UEventsHandler::~UEventsHandler()
{
UEventsManager::removeHandler(this);
}
void UEventsHandler::post(UEvent * event, bool async)
{
UEventsManager::post(event, async);
}

235
utilite/src/UEventsManager.cpp Normal file
View File

@@ -0,0 +1,235 @@
/*
* 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)
{
if(!event)
{
UERROR("Event is null!");
return;
}
else
{
UEventsManager::getInstance()->_postEvent(event, async);
}
}
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<UEvent*>::iterator it=events_.begin(); it!=events_.end(); ++it)
{
delete *it;
}
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<UEvent*>::iterator it;
std::list<UEvent*> 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)
{
dispatchEvent(*it);
delete *it;
}
eventsBuf.clear();
}
void UEventsManager::dispatchEvent(UEvent * event)
{
UEventsHandler * handler;
handlersMutex_.lock();
std::list<UEventsHandler*> handlers = handlers_;
for(std::list<UEventsHandler*>::iterator it=handlers.begin(); it!=handlers.end(); ++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())
{
handler = *it;
handlersMutex_.unlock();
// To be able to add/remove an handler in a handleEvent call (without a deadlock)
// @see _addHandler(), _removeHandler()
handler->handleEvent(event);
handlersMutex_.lock();
}
}
handlersMutex_.unlock();
}
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();
}
}
void UEventsManager::_postEvent(UEvent * event, bool async)
{
if(!this->isKilled())
{
if(async)
{
eventsMutex_.lock();
{
events_.push_back(event);
}
eventsMutex_.unlock();
// Signal the EventsManager that an Event is added
postEventSem_.release();
}
else
{
dispatchEvent(event);
delete event;
}
}
else
{
delete event;
}
}

95
utilite/src/UFile.cpp Normal file
View File

@@ -0,0 +1,95 @@
/*
* 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 remove(filePath.c_str());
}
int UFile::rename(const std::string &oldFilePath,
const std::string &newFilePath)
{
return 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=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 "";
}

624
utilite/src/ULogger.cpp Normal file
View File

@@ -0,0 +1,624 @@
/*
* 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::limitWhereLength_ = false;
bool ULogger::buffered_ = false;
bool ULogger::exitingState_ = false;
ULogger::Level ULogger::level_ = kInfo; // By default, we show all info msgs + upper level (Warning, Error)
ULogger::Level ULogger::exitLevel_ = kFatal;
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_;
/**
* 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)
{
if(arg != 0)
{
vprintf(msg, arg);
}
else
{
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_)
{
if(arg != 0)
{
vfprintf(fout_, msg, arg);
}
else
{
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::reset()
{
ULogger::setType(ULogger::kTypeNoLog);
append_ = true;
printTime_ = true;
printLevel_ = true;
printEndline_ = true;
printColored_ = true;
printWhere_ = true;
printWhereFullPath_ = 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()->_write(bufferedMsgs_.c_str(), 0);
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()->_write(time.c_str(), 0);
}
}
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()->_write(endline.c_str(), 0);
}
}
loggerMutex_.unlock();
}
void ULogger::write(ULogger::Level level,
const char * file,
int line,
const char * function,
const char* msg,
...)
{
if(exitingState_)
{
// Ignore messages after a fatal exit...
return;
}
loggerMutex_.lock();
if(type_ == kTypeNoLog && level < kFatal)
{
loggerMutex_.unlock();
return;
}
if(strlen(msg) == 0 && !printWhere_ && level < exitLevel_)
{
loggerMutex_.unlock();
// No need to show an empty message if we don't print where.
return;
}
if(level >= level_)
{
#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_)
{
time.append("(");
getTime(time);
time.append(") ");
}
std::string levelStr = "";
if(printLevel_)
{
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 whereStr = "";
if(printWhere_)
{
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()->_write(color, 0);
}
#endif
}
if(buffered_)
{
bufferedMsgs_.append(levelStr.c_str());
bufferedMsgs_.append(time.c_str());
bufferedMsgs_.append(whereStr.c_str());
bufferedMsgs_.append(uFormatv(msg, args));
}
else
{
ULogger::getInstance()->_write(levelStr.c_str(), 0);
ULogger::getInstance()->_write(time.c_str(), 0);
ULogger::getInstance()->_write(whereStr.c_str(), 0);
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()->_write(COLOR_NORMAL, 0);
}
#endif
}
if(buffered_)
{
bufferedMsgs_.append(endline.c_str());
}
else
{
ULogger::getInstance()->_write(endline.c_str(), 0);
}
va_end (args);
}
if(level >= eventLevel_)
{
std::string fullMsg = uFormat("%s%s%s", levelStr.c_str(), time.c_str(), whereStr.c_str());
va_start(args, msg);
if(args != 0)
{
fullMsg.append(uFormatv(msg, args));
}
else
{
fullMsg.append(msg);
}
va_end(args);
if(level >= exitLevel_)
{
// Send it synchronously, then receivers
// can do something before the code (exiting) below is executed.
exitingState_ = true;
UEventsManager::post(new ULogEvent(fullMsg, kFatal), false);
}
else
{
UEventsManager::post(new ULogEvent(fullMsg, level));
}
}
if(level >= exitLevel_)
{
printf("\n*******\n%s message occurred!\n", levelName_[level]);
printf(" %s%s%s", levelStr.c_str(), time.c_str(), whereStr.c_str());
va_start(args, msg);
if(args != 0)
{
vprintf(msg, args);
}
else
{
printf("%s", msg);
}
va_end(args);
printf("\n*******\n");
destroyer_.setDoomed(0);
delete instance_; // If a FileLogger is used, this will close the file.
instance_ = 0;
//========================================================================
// EXIT APPLICATION
exit(1);
//========================================================================
}
}
loggerMutex_.unlock();
}
int ULogger::getTime(std::string &timeStr)
{
if(!printTime_) {
return 0;
}
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");
}

2979
utilite/src/UPlot.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

78
utilite/src/UProcessInfo.cpp Normal file
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;
}

301
utilite/src/UThread.cpp Normal file
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);
}
#if 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
}

115
utilite/src/UTimer.cpp Normal file
View File

@@ -0,0 +1,115 @@
/*
* 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()
{
LARGE_INTEGER count, freq;
QueryPerformanceFrequency(&freq);
QueryPerformanceCounter(&count);
return double(count.QuadPart) / freq.QuadPart;
}
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;
}