i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-09-13 06:40:19 +08:00
Code Issues Packages Projects Releases Wiki Activity

load_database srv: fixed Odom parameters missing warnings, fixed warning when existing parameters are the the same than loaded ones from new database.

Browse Source
This commit is contained in:
matlabbe
2025-05-06 16:14:58 -07:00
parent b7ce0b19c2
commit e9fa89110c
1 changed files with 28 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rtabmap_slam/src/CoreWrapper.cpp
+28 -10
View File
@@ -359,7 +359,7 @@ void CoreWrapper::onInit()
double vDouble;
if(pnh.getParam(iter->first, vStr))
{
NODELET_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\"", iter->first.c_str(), vStr.c_str());
NODELET_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\" (rosparam)", iter->first.c_str(), vStr.c_str());
if(iter->first.compare(Parameters::kRtabmapWorkingDirectory()) == 0)
{
@@ -373,17 +373,17 @@ void CoreWrapper::onInit()
}
else if(pnh.getParam(iter->first, vBool))
{
NODELET_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\"", iter->first.c_str(), uBool2Str(vBool).c_str());
NODELET_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\" (rosparam)", iter->first.c_str(), uBool2Str(vBool).c_str());
uInsert(parameters_, ParametersPair(iter->first, uBool2Str(vBool)));
}
else if(pnh.getParam(iter->first, vDouble))
{
NODELET_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vDouble).c_str());
NODELET_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\" (rosparam)", iter->first.c_str(), uNumber2Str(vDouble).c_str());
uInsert(parameters_, ParametersPair(iter->first, uNumber2Str(vDouble)));
}
else if(pnh.getParam(iter->first, vInt))
{
NODELET_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vInt).c_str());
NODELET_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\" (rosparam)", iter->first.c_str(), uNumber2Str(vInt).c_str());
uInsert(parameters_, ParametersPair(iter->first, uNumber2Str(vInt)));
}
}
@@ -2942,14 +2942,16 @@ bool CoreWrapper::loadDatabaseCallback(rtabmap_msgs::LoadDatabase::Request& req,
// Open new database
databasePath_ = newDatabasePath;
// modify default parameters with those in the database
// Warn if database's parameters are different than the current ones we are using
if(!req.clear && UFile::exists(databasePath_))
{
ParametersMap dbParameters;
rtabmap::DBDriver * driver = rtabmap::DBDriver::create();
std::string databaseVersion = "0.0.0";
if(driver->openConnection(databasePath_))
{
dbParameters = driver->getLastParameters(); // parameter migration is already done
databaseVersion = driver->getDatabaseVersion();
}
delete driver;
for(ParametersMap::iterator iter=dbParameters.begin(); iter!=dbParameters.end(); ++iter)
@@ -2959,15 +2961,31 @@ bool CoreWrapper::loadDatabaseCallback(rtabmap_msgs::LoadDatabase::Request& req,
// ignore working directory
continue;
}
if(parameters_.find(iter->first) == parameters_.end() &&
parameters_.find(iter->first)->second.compare(iter->second) !=0)
if(iter->first.find("Odom") == 0)
{
NODELET_WARN("RTAB-Map parameter \"%s\" from database (%s) is different "
"from the current used one (%s). We still keep the "
// ignore odometry params
continue;
}
if(parameters_.find(iter->first) == parameters_.end())
{
NODELET_WARN("RTAB-Map parameter \"%s\" from database (%s, version \"%s\") doesn't exist "
"in current rtabmap version (\"%s\"). The parameter is ignored.",
iter->first.c_str(),
iter->second.c_str(),
databaseVersion.c_str(),
RTABMAP_VERSION);
}
else if(parameters_.find(iter->first)->second.compare(iter->second) !=0)
{
NODELET_WARN("RTAB-Map parameter \"%s\" from database (%s, version=\"%s\") is different "
"from the current used one (%s, version=\"%s\"). We still keep the "
"current parameter value (%s). If you want to switch between databases "
"with different configurations, restart rtabmap node instead of using this service.",
iter->first.c_str(), iter->second.c_str(),
iter->first.c_str(),
iter->second.c_str(),
databaseVersion.c_str(),
parameters_.find(iter->first)->second.c_str(),
RTABMAP_VERSION,
parameters_.find(iter->first)->second.c_str());
}
}