UDirectory::UDirectory(const std::string & path, const std::string & extensions)
{
extensions_ = uListToVector(uSplit(extensions, ' '));
path_ = path;
iFileName_ = fileNames_.begin();
this->update();
}
void UDir::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();
}
}
}
}
OdometryROS::OdometryROS(int argc, char * argv[], bool stereo) :
odometry_(0),
frameId_("base_link"),
odomFrameId_("odom"),
groundTruthFrameId_(""),
publishTf_(true),
waitForTransform_(true),
waitForTransformDuration_(0.1), // 100 ms
paused_(false)
{
ros::NodeHandle nh;
odomPub_ = nh.advertise<nav_msgs::Odometry>("odom", 1);
odomInfoPub_ = nh.advertise<rtabmap_ros::OdomInfo>("odom_info", 1);
odomLocalMap_ = nh.advertise<sensor_msgs::PointCloud2>("odom_local_map", 1);
odomLastFrame_ = nh.advertise<sensor_msgs::PointCloud2>("odom_last_frame", 1);
ros::NodeHandle pnh("~");
Transform initialPose = Transform::getIdentity();
std::string initialPoseStr;
std::string tfPrefix;
std::string configPath;
pnh.param("frame_id", frameId_, frameId_);
pnh.param("odom_frame_id", odomFrameId_, odomFrameId_);
pnh.param("publish_tf", publishTf_, publishTf_);
pnh.param("tf_prefix", tfPrefix, tfPrefix);
pnh.param("wait_for_transform", waitForTransform_, waitForTransform_);
pnh.param("wait_for_transform_duration", waitForTransformDuration_, waitForTransformDuration_);
pnh.param("initial_pose", initialPoseStr, initialPoseStr); // "x y z roll pitch yaw"
pnh.param("ground_truth_frame_id", groundTruthFrameId_, groundTruthFrameId_);
pnh.param("config_path", configPath, configPath);
configPath = uReplaceChar(configPath, '~', UDirectory::homeDir());
if(configPath.size() && configPath.at(0) != '/')
{
configPath = UDirectory::currentDir(true) + configPath;
}
if(!tfPrefix.empty())
{
if(!frameId_.empty())
{
frameId_ = tfPrefix + "/" + frameId_;
}
if(!odomFrameId_.empty())
{
odomFrameId_ = tfPrefix + "/" + odomFrameId_;
}
if(!groundTruthFrameId_.empty())
{
groundTruthFrameId_ = tfPrefix + "/" + groundTruthFrameId_;
}
}
if(initialPoseStr.size())
{
std::vector<std::string> values = uListToVector(uSplit(initialPoseStr, ' '));
if(values.size() == 6)
{
initialPose = Transform(
uStr2Float(values[0]), uStr2Float(values[1]), uStr2Float(values[2]),
uStr2Float(values[3]), uStr2Float(values[4]), uStr2Float(values[5]));
}
else
{
ROS_ERROR("Wrong initial_pose format: %s (should be \"x y z roll pitch yaw\" with angle in radians). "
"Identity will be used...", initialPoseStr.c_str());
}
}
//parameters
parameters_ = Parameters::getDefaultOdometryParameters(stereo);
if(!configPath.empty())
{
if(UFile::exists(configPath.c_str()))
{
ROS_INFO("Odometry: Loading parameters from %s", configPath.c_str());
rtabmap::ParametersMap allParameters;
Parameters::readINI(configPath.c_str(), allParameters);
// only update odometry parameters
for(ParametersMap::iterator iter=parameters_.begin(); iter!=parameters_.end(); ++iter)
{
ParametersMap::iterator jter = allParameters.find(iter->first);
if(jter!=allParameters.end())
{
iter->second = jter->second;
}
}
}
else
{
ROS_ERROR("Config file \"%s\" not found!", configPath.c_str());
}
}
for(rtabmap::ParametersMap::iterator iter=parameters_.begin(); iter!=parameters_.end(); ++iter)
{
std::string vStr;
bool vBool;
int vInt;
double vDouble;
if(pnh.getParam(iter->first, vStr))
{
ROS_INFO("Setting odometry parameter \"%s\"=\"%s\"", iter->first.c_str(), vStr.c_str());
iter->second = vStr;
}
else if(pnh.getParam(iter->first, vBool))
{
ROS_INFO("Setting odometry parameter \"%s\"=\"%s\"", iter->first.c_str(), uBool2Str(vBool).c_str());
iter->second = uBool2Str(vBool);
}
else if(pnh.getParam(iter->first, vDouble))
{
ROS_INFO("Setting odometry parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vDouble).c_str());
iter->second = uNumber2Str(vDouble);
}
else if(pnh.getParam(iter->first, vInt))
{
ROS_INFO("Setting odometry parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vInt).c_str());
iter->second = uNumber2Str(vInt);
}
if(iter->first.compare(Parameters::kVisMinInliers()) == 0 && atoi(iter->second.c_str()) < 8)
{
ROS_WARN("Parameter min_inliers must be >= 8, setting to 8...");
iter->second = uNumber2Str(8);
}
}
rtabmap::ParametersMap parameters = rtabmap::Parameters::parseArguments(argc, argv);
for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
{
rtabmap::ParametersMap::iterator jter = parameters_.find(iter->first);
if(jter!=parameters_.end())
{
ROS_INFO("Update odometry parameter \"%s\"=\"%s\" from arguments", iter->first.c_str(), iter->second.c_str());
jter->second = iter->second;
}
}
// Backward compatibility
for(std::map<std::string, std::pair<bool, std::string> >::const_iterator iter=Parameters::getRemovedParameters().begin();
iter!=Parameters::getRemovedParameters().end();
++iter)
{
std::string vStr;
if(pnh.getParam(iter->first, vStr))
{
if(iter->second.first)
{
// can be migrated
parameters_.at(iter->second.second)= vStr;
ROS_WARN("Odometry: Parameter name changed: \"%s\" -> \"%s\". Please update your launch file accordingly. Value \"%s\" is still set to the new parameter name.",
iter->first.c_str(), iter->second.second.c_str(), vStr.c_str());
}
else
{
if(iter->second.second.empty())
{
ROS_ERROR("Odometry: Parameter \"%s\" doesn't exist anymore!",
iter->first.c_str());
}
else
{
ROS_ERROR("Odometry: Parameter \"%s\" doesn't exist anymore! You may look at this similar parameter: \"%s\"",
iter->first.c_str(), iter->second.second.c_str());
}
}
}
}
int odomStrategy = 0; // BOW
Parameters::parse(parameters_, Parameters::kOdomStrategy(), odomStrategy);
odometry_ = Odometry::create(parameters_);
if(!initialPose.isIdentity())
{
odometry_->reset(initialPose);
}
resetSrv_ = nh.advertiseService("reset_odom", &OdometryROS::reset, this);
resetToPoseSrv_ = nh.advertiseService("reset_odom_to_pose", &OdometryROS::resetToPose, this);
pauseSrv_ = nh.advertiseService("pause_odom", &OdometryROS::pause, this);
resumeSrv_ = nh.advertiseService("resume_odom", &OdometryROS::resume, this);
setLogDebugSrv_ = pnh.advertiseService("log_debug", &OdometryROS::setLogDebug, this);
setLogInfoSrv_ = pnh.advertiseService("log_info", &OdometryROS::setLogInfo, this);
setLogWarnSrv_ = pnh.advertiseService("log_warning", &OdometryROS::setLogWarn, this);
setLogErrorSrv_ = pnh.advertiseService("log_error", &OdometryROS::setLogError, this);
}
