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

DatabaseViewer: added exporting of a specific session with or without user_data. MainWindow: added action to send goal to rtabmap. Rtabmap: Saving goals in user_data of the current signature, modified parameter Mem/RehearsedNodesKept to Mem/NotLinkedNodesKept. DBReader: added sending goals when detected while playing back a database.

Browse Source
This commit is contained in:
Mathieu Labbe
2015-04-17 18:30:12 -04:00
parent b48bd7cf70
commit 5700c14bcb
25 changed files with 325 additions and 49 deletions
Download Patch File Download Diff File Expand all files Collapse all files

51
corelib/src/DBReader.cpp
View File

@@ -31,8 +31,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UFile.h>
#include <rtabmap/utilite/UStl.h>
#include <rtabmap/utilite/UConversion.h>
#include "rtabmap/core/CameraEvent.h"
#include "rtabmap/core/RtabmapEvent.h"
#include "rtabmap/core/OdometryEvent.h"
#include "rtabmap/core/util3d.h"
#include "rtabmap/core/Compression.h"
@@ -135,6 +138,23 @@ void DBReader::mainLoop()
SensorData data = this->getNextData();
if(data.isValid())
{
int goalId = 0;
double previousStamp = data.stamp();
data.setStamp(UTimer::now());
if(data.userData().size() >= 6 && memcmp(data.userData().data(), "GOAL:", 5) == 0)
{
//GOAL format detected, remove it from the user data and send it as goal event
std::string goalStr = uBytes2Str(data.userData());
if(!goalStr.empty())
{
std::list<std::string> strs = uSplit(goalStr, ':');
if(strs.size() == 2)
{
goalId = atoi(strs.rbegin()->c_str());
data.setUserData(std::vector<unsigned char>());
}
}
}
if(!_odometryIgnored)
{
if(data.pose().isNull())
@@ -150,6 +170,35 @@ void DBReader::mainLoop()
this->post(new CameraEvent(data));
}
if(goalId > 0)
{
this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, "", goalId));
if(_currentId != _ids.end())
{
// get stamp for the next signature to compute the delay
// that was used originally for planning
int weight;
std::string label;
double stamp;
int mapId;
Transform localTransform, pose;
std::vector<unsigned char> userData;
_dbDriver->getNodeInfo(*_currentId, pose, mapId, weight, label, stamp, userData);
if(previousStamp && stamp && stamp > previousStamp)
{
double delay = stamp - previousStamp;
UWARN("Goal %d detected, posting it! Waiting %f seconds before sending next data...",
goalId, delay);
uSleep(delay*1000);
}
else
{
UWARN("stamps = %d=%f %d=%f ", data.id(), data.stamp(), *_currentId, stamp);
}
}
}
}
else if(!this->isKilled())
{
@@ -246,7 +295,7 @@ SensorData DBReader::getNextData()
rotVariance,
transVariance,
seq,
UTimer::now(),
stamp,
userData);
UDEBUG("Laser=%d RGB/Left=%d Depth=%d Right=%d",
data.laserScan().empty()?0:1,

16
corelib/src/Memory.cpp
View File

@@ -59,7 +59,7 @@ Memory::Memory(const ParametersMap & parameters) :
_similarityThreshold(Parameters::defaultMemRehearsalSimilarity()),
_rawDataKept(Parameters::defaultMemImageKept()),
_binDataKept(Parameters::defaultMemBinDataKept()),
_keepRehearsedNodesInDb(Parameters::defaultMemRehearsedNodesKept()),
_notLinkedNodesKeptInDb(Parameters::defaultMemNotLinkedNodesKept()),
_incrementalMemory(Parameters::defaultMemIncrementalMemory()),
_maxStMemSize(Parameters::defaultMemSTMSize()),
_recentWmRatio(Parameters::defaultMemRecentWmRatio()),
@@ -379,7 +379,7 @@ void Memory::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kMemImageKept(), _rawDataKept);
Parameters::parse(parameters, Parameters::kMemBinDataKept(), _binDataKept);
Parameters::parse(parameters, Parameters::kMemRehearsedNodesKept(), _keepRehearsedNodesInDb);
Parameters::parse(parameters, Parameters::kMemNotLinkedNodesKept(), _notLinkedNodesKeptInDb);
Parameters::parse(parameters, Parameters::kMemRehearsalIdUpdatedToNewOne(), _idUpdatedToNewOneRehearsal);
Parameters::parse(parameters, Parameters::kMemGenerateIds(), _generateIds);
Parameters::parse(parameters, Parameters::kMemBadSignaturesIgnored(), _badSignaturesIgnored);
@@ -1499,13 +1499,13 @@ std::list<Signature *> Memory::getRemovableSignatures(int count, const std::set<
/**
* If saveToDatabase=false, deleted words are filled in deletedWords.
*/
void Memory::moveToTrash(Signature * s, bool saveToDatabase, std::list<int> * deletedWords)
void Memory::moveToTrash(Signature * s, bool keepLinkedToGraph, std::list<int> * deletedWords)
{
UDEBUG("id=%d", s?s->id():0);
if(s)
{
// If not saved to database or it is a bad signature (not saved), remove links!
if(!saveToDatabase || (!s->isSaved() && s->isBadSignature() && _badSignaturesIgnored))
if(!keepLinkedToGraph || (!s->isSaved() && s->isBadSignature() && _badSignaturesIgnored))
{
UASSERT_MSG(this->isInSTM(s->id()),
uFormat("Deleting location (%d) outside the STM is not implemented!", s->id()).c_str());
@@ -1537,6 +1537,7 @@ void Memory::moveToTrash(Signature * s, bool saveToDatabase, std::list<int> * de
}
s->removeLinks(); // remove all links
s->setWeight(0);
s->setLabel(""); // reset label
}
else
{
@@ -1561,7 +1562,7 @@ void Memory::moveToTrash(Signature * s, bool saveToDatabase, std::list<int> * de
}
this->disableWordsRef(s->id());
if(!saveToDatabase)
if(!keepLinkedToGraph)
{
std::list<int> keys = uUniqueKeys(s->getWords());
for(std::list<int>::const_iterator i=keys.begin(); i!=keys.end(); ++i)
@@ -1599,7 +1600,7 @@ void Memory::moveToTrash(Signature * s, bool saveToDatabase, std::list<int> * de
}
}
if( saveToDatabase &&
if( (_notLinkedNodesKeptInDb || keepLinkedToGraph) &&
_dbDriver &&
s->id()>0)
{
@@ -2711,8 +2712,7 @@ bool Memory::rehearsalMerge(int oldId, int newId)
}
// remove location
bool saveToDb = _keepRehearsedNodesInDb;
moveToTrash(_idUpdatedToNewOneRehearsal?oldS:newS, saveToDb);
moveToTrash(_idUpdatedToNewOneRehearsal?oldS:newS, _notLinkedNodesKeptInDb);
return true;
}

1
corelib/src/Odometry.cpp
View File

@@ -177,6 +177,7 @@ OdometryBOW::OdometryBOW(const ParametersMap & parameters) :
customParameters.insert(ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0")); // desactivate rehearsal
customParameters.insert(ParametersPair(Parameters::kMemBinDataKept(), "false"));
customParameters.insert(ParametersPair(Parameters::kMemSTMSize(), "0"));
customParameters.insert(ParametersPair(Parameters::kMemNotLinkedNodesKept(), "false"));
int nn = Parameters::defaultOdomBowNNType();
float nndr = Parameters::defaultOdomBowNNDR();
int featureType = Parameters::defaultOdomFeatureType();

20
corelib/src/Rtabmap.cpp
View File

@@ -109,6 +109,7 @@ Rtabmap::Rtabmap() :
_startNewMapOnLoopClosure(Parameters::defaultRtabmapStartNewMapOnLoopClosure()),
_goalReachedRadius(Parameters::defaultRGBDGoalReachedRadius()),
_planWithNearNodesLinked(Parameters::defaultRGBDPlanWithNearNodesLinked()),
_goalsSavedInUserData(Parameters::defaultRGBDGoalsSavedInUserData()),
_loopClosureHypothesis(0,0.0f),
_highestHypothesis(0,0.0f),
_lastProcessTime(0.0),
@@ -395,6 +396,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kRtabmapStartNewMapOnLoopClosure(), _startNewMapOnLoopClosure);
Parameters::parse(parameters, Parameters::kRGBDGoalReachedRadius(), _goalReachedRadius);
Parameters::parse(parameters, Parameters::kRGBDPlanWithNearNodesLinked(), _planWithNearNodesLinked);
Parameters::parse(parameters, Parameters::kRGBDGoalsSavedInUserData(), _goalsSavedInUserData);
// RGB-D SLAM stuff
if((iter=parameters.find(Parameters::kLccIcpType())) != parameters.end())
@@ -1504,7 +1506,7 @@ bool Rtabmap::process(const SensorData & data)
rejectedHypothesis = transform.isNull();
if(rejectedHypothesis)
{
UWARN("Rejected loop closure transform between %d and %d: %s",
UWARN("Rejected loop closure %d -> %d: %s",
_loopClosureHypothesis.first, signature->id(), rejectedMsg.c_str());
}
}
@@ -2069,16 +2071,6 @@ void Rtabmap::setWorkingDirectory(std::string path)
}
}
void Rtabmap::deleteLocation(int locationId)
{
if(_memory)
{
_memory->deleteLocation(locationId);
}
}
void Rtabmap::rejectLoopClosure(int oldId, int newId)
{
UDEBUG("_loopClosureHypothesis.first=%d", _loopClosureHypothesis.first);
@@ -2596,6 +2588,12 @@ bool Rtabmap::computePath(
}
UINFO("Path = [%s]", stream.str().c_str());
}
if(_goalsSavedInUserData)
{
// set goal to latest signature
std::string goalStr = uFormat("GOAL:%d", targetNode);
setUserData(0, uStr2Bytes(goalStr));
}
}
return _path.size()>0;

22
corelib/src/RtabmapThread.cpp
View File

@@ -33,6 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/ParamEvent.h"
#include "rtabmap/core/OdometryEvent.h"
#include "rtabmap/core/UserDataEvent.h"
#include "rtabmap/core/Memory.h"
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UEventsManager.h>
@@ -194,6 +195,7 @@ void RtabmapThread::mainLoop()
}
_stateMutex.unlock();
int id = 0;
std::vector<unsigned char> userData;
switch(state)
{
@@ -272,6 +274,18 @@ void RtabmapThread::mainLoop()
_userDataMutex.unlock();
_rtabmap->setUserData(0, userData);
break;
case kStateSettingGoal:
id = atoi(parameters.at("goal_id").c_str());
if(id == 0 && !parameters.at("goal_label").empty() && _rtabmap->getMemory())
{
id = _rtabmap->getMemory()->getSignatureIdByLabel(parameters.at("goal_label"));
}
if(id <= 0 || !_rtabmap->computePath(id, true))
{
UERROR("Failed to set a goal to location=%d.", id);
}
this->post(new RtabmapGlobalPathEvent(id, _rtabmap->getPath()));
break;
default:
UFATAL("Invalid state !?!?");
break;
@@ -448,6 +462,14 @@ void RtabmapThread::handleEvent(UEvent* event)
ULOGGER_DEBUG("CMD_PAUSE");
_paused = !_paused;
}
else if(cmd == RtabmapEventCmd::kCmdGoal)
{
ULOGGER_DEBUG("CMD_GOAL");
ParametersMap param;
param.insert(ParametersPair("goal_label", rtabmapEvent->getStr()));
param.insert(ParametersPair("goal_id", uNumber2Str(rtabmapEvent->getInt())));
pushNewState(kStateSettingGoal, param);
}
else
{
UWARN("Cmd %d unknown!", cmd);