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

Merged pcl_integration branch to trunk

Browse Source 
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1014 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2013-12-11 00:12:44 +00:00
parent 97c70d394e
commit 8b8511e154
124 changed files with 21692 additions and 4458 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
utilite/src/CMakeLists.txt
View File

@@ -2,6 +2,7 @@
SET(SRC_FILES
UEventsManager.cpp
UEventsHandler.cpp
UEventsSender.cpp
UFile.cpp
UDirectory.cpp
UConversion.cpp
@@ -34,9 +35,10 @@ PROPERTIES
)
INSTALL(TARGETS rtabmap_utilite
RUNTIME DESTINATION bin COMPONENT runtime
LIBRARY DESTINATION lib COMPONENT devel
ARCHIVE DESTINATION lib COMPONENT devel)
EXPORT RTABMapTargets
RUNTIME DESTINATION "${INSTALL_BIN_DIR}" COMPONENT runtime
LIBRARY DESTINATION "${INSTALL_LIB_DIR}" COMPONENT devel
ARCHIVE DESTINATION "${INSTALL_LIB_DIR}" COMPONENT devel)
install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/../include/ DESTINATION include/ COMPONENT devel FILES_MATCHING PATTERN "*.h" PATTERN ".svn" EXCLUDE)
install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/../include/ DESTINATION "${INSTALL_INCLUDE_DIR}" COMPONENT devel FILES_MATCHING PATTERN "*.h" PATTERN ".svn" EXCLUDE)

11
utilite/src/UEventsHandler.cpp
View File

@@ -22,10 +22,15 @@
UEventsHandler::~UEventsHandler()
{
UEventsManager::removeHandler(this);
unregisterFromEventsManager();
}
void UEventsHandler::post(UEvent * event, bool async)
void UEventsHandler::registerToEventsManager()
{
UEventsManager::post(event, async);
UEventsManager::addHandler(this);
}
void UEventsHandler::unregisterFromEventsManager()
{
UEventsManager::removeHandler(this);
}

262
utilite/src/UEventsManager.cpp
View File

@@ -51,7 +51,7 @@ void UEventsManager::removeHandler(UEventsHandler* handler)
}
}
void UEventsManager::post(UEvent * event, bool async)
void UEventsManager::post(UEvent * event, bool async, const UEventsSender * sender)
{
if(!event)
{
@@ -60,7 +60,65 @@ void UEventsManager::post(UEvent * event, bool async)
}
else
{
UEventsManager::getInstance()->_postEvent(event, async);
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);
}
}
@@ -84,9 +142,9 @@ UEventsManager::~UEventsManager()
join(true);
// Free memory
for(std::list<UEvent*>::iterator it=events_.begin(); it!=events_.end(); ++it)
for(std::list<std::pair<UEvent*, const UEventsSender*> >::iterator it=events_.begin(); it!=events_.end(); ++it)
{
delete *it;
delete it->first;
}
events_.clear();
@@ -116,8 +174,8 @@ void UEventsManager::dispatchEvents()
return;
}
std::list<UEvent*>::iterator it;
std::list<UEvent*> eventsBuf;
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
@@ -132,17 +190,29 @@ void UEventsManager::dispatchEvents()
// Past events to handlers
for(it=eventsBuf.begin(); it!=eventsBuf.end(); ++it)
{
dispatchEvent(*it);
delete *it;
dispatchEvent(it->first, it->second);
delete it->first;
}
eventsBuf.clear();
}
void UEventsManager::dispatchEvent(UEvent * event)
void UEventsManager::dispatchEvent(UEvent * event, const UEventsSender * sender)
{
UEventsHandler * handler;
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();
std::list<UEventsHandler*> handlers = handlers_;
if(handlers.size() == 0)
{
//No pipes, send to all handlers
handlers = handlers_;
}
for(std::list<UEventsHandler*>::iterator it=handlers.begin(); it!=handlers.end(); ++it)
{
// Check if the handler is still in the
@@ -150,7 +220,7 @@ void UEventsManager::dispatchEvent(UEvent * event)
// removeHandler() is called in EventsHandler::handleEvent())
if(std::find(handlers_.begin(), handlers_.end(), *it) != handlers_.end())
{
handler = *it;
UEventsHandler * handler = *it;
handlersMutex_.unlock();
// To be able to add/remove an handler in a handleEvent call (without a deadlock)
@@ -161,7 +231,6 @@ void UEventsManager::dispatchEvent(UEvent * event)
}
}
handlersMutex_.unlock();
}
void UEventsManager::_addHandler(UEventsHandler* handler)
@@ -204,10 +273,22 @@ void UEventsManager::_removeHandler(UEventsHandler* handler)
}
}
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)
void UEventsManager::_postEvent(UEvent * event, bool async, const UEventsSender * sender)
{
if(!this->isKilled())
{
@@ -215,7 +296,7 @@ void UEventsManager::_postEvent(UEvent * event, bool async)
{
eventsMutex_.lock();
{
events_.push_back(event);
events_.push_back(std::make_pair(event, sender));
}
eventsMutex_.unlock();
@@ -224,7 +305,7 @@ void UEventsManager::_postEvent(UEvent * event, bool async)
}
else
{
dispatchEvent(event);
dispatchEvent(event, sender);
delete event;
}
}
@@ -233,3 +314,152 @@ void UEventsManager::_postEvent(UEvent * event, bool async)
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();
}

31
utilite/src/UEventsSender.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/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);
}

70
utilite/src/ULogger.cpp
View File

@@ -88,15 +88,12 @@ protected:
private:
virtual void _write(const char* msg, va_list arg)
{
if(arg != 0)
{
vprintf(msg, arg);
}
else
{
printf("%s", msg);
}
vprintf(msg, arg);
}
virtual void _writeStr(const char* msg)
{
printf("%s", msg);
}
};
/**
@@ -158,16 +155,16 @@ private:
{
if(fout_)
{
if(arg != 0)
{
vfprintf(fout_, msg, arg);
}
else
{
fprintf(fout_, "%s", msg);
}
vfprintf(fout_, msg, arg);
}
}
virtual void _writeStr(const char* msg)
{
if(fout_)
{
fprintf(fout_, "%s", msg);
}
}
private:
std::string fileName_; ///< the file name
@@ -243,7 +240,7 @@ void ULogger::flush()
void ULogger::_flush()
{
ULogger::getInstance()->_write(bufferedMsgs_.c_str(), 0);
ULogger::getInstance()->_writeStr(bufferedMsgs_.c_str());
bufferedMsgs_.clear();
}
@@ -277,7 +274,7 @@ void ULogger::write(const char* msg, ...)
}
else
{
ULogger::getInstance()->_write(time.c_str(), 0);
ULogger::getInstance()->_writeStr(time.c_str());
}
}
@@ -300,7 +297,7 @@ void ULogger::write(const char* msg, ...)
}
else
{
ULogger::getInstance()->_write(endline.c_str(), 0);
ULogger::getInstance()->_writeStr(endline.c_str());
}
}
loggerMutex_.unlock();
@@ -444,7 +441,7 @@ void ULogger::write(ULogger::Level level,
}
else
{
ULogger::getInstance()->_write(color, 0);
ULogger::getInstance()->_writeStr(color);
}
#endif
}
@@ -458,9 +455,9 @@ void ULogger::write(ULogger::Level level,
}
else
{
ULogger::getInstance()->_write(levelStr.c_str(), 0);
ULogger::getInstance()->_write(time.c_str(), 0);
ULogger::getInstance()->_write(whereStr.c_str(), 0);
ULogger::getInstance()->_writeStr(levelStr.c_str());
ULogger::getInstance()->_writeStr(time.c_str());
ULogger::getInstance()->_writeStr(whereStr.c_str());
ULogger::getInstance()->_write(msg, args);
}
if(type_ == ULogger::kTypeConsole && printColored_)
@@ -474,7 +471,7 @@ void ULogger::write(ULogger::Level level,
}
else
{
ULogger::getInstance()->_write(COLOR_NORMAL, 0);
ULogger::getInstance()->_writeStr(COLOR_NORMAL);
}
#endif
}
@@ -484,7 +481,7 @@ void ULogger::write(ULogger::Level level,
}
else
{
ULogger::getInstance()->_write(endline.c_str(), 0);
ULogger::getInstance()->_writeStr(endline.c_str());
}
va_end (args);
}
@@ -493,14 +490,7 @@ void ULogger::write(ULogger::Level level,
{
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);
}
fullMsg.append(uFormatv(msg, args));
va_end(args);
if(level >= exitLevel_)
{
@@ -517,18 +507,14 @@ void ULogger::write(ULogger::Level 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)
printf("\n*******\n%s message occurred! Application will now exit.\n", levelName_[level]);
if(type_ != kTypeConsole)
{
printf(" %s%s%s", levelStr.c_str(), time.c_str(), whereStr.c_str());
va_start(args, msg);
vprintf(msg, args);
va_end(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.