void
freeDataContent (TuiWinContent content, int contentSize)
{
int i;
/*
** Remember that data window content elements are of type TuiGenWinInfoPtr,
** each of which whose single element is a data element.
*/
for (i = 0; i < contentSize; i++)
{
TuiGenWinInfoPtr genericWin = &content[i]->whichElement.dataWindow;
if (genericWin != (TuiGenWinInfoPtr) NULL)
{
tuiDelwin (genericWin->handle);
genericWin->handle = (WINDOW *) NULL;
freeWinContent (genericWin);
}
}
freeContent (content,
contentSize,
DATA_WIN);
return;
} /* freeDataContent */
//free : liberation memoire ST_CONTENT
ST_CONTENT* freeContent(ST_CONTENT* content){
if(content == NULL){
return NULL;
}else{
ST_CONTENT* tmp;
tmp = content->suiv;
free(content);
return freeContent(tmp);
}
}
void FSDir::readContent()
{
freeContent();
std::string command = "ls -lh";
command += " " + getAbsolutePath() + "/";
command += " | tail -n +2 ";
command += " | ";
command += AWK;
std::string ls = exec(command);
std::istringstream iss(ls);
std::string line;
while ( line.clear(), getline(iss, line) )
{
std::istringstream linestream(line);
std::string filename;
std::string date;
std::string size;
std::string owner;
std::string perms;
getline(linestream, filename, '#');
getline(linestream, date, '#');
getline(linestream, size, '#');
getline(linestream, owner, '#');
getline(linestream, perms);
if ( ! hidden && filename[0] == '.' )
continue;
if ( perms[0] == 'd' )
{
FSDir * d = new FSDir( getAbsolutePath() + PATH_SEPARATOR + filename);
d->setAttrs(date, owner, size, perms);
content.push_back((FSObject*) d);
}
else if ( perms[0] == 'l' )
{
FSSymlink * d = new FSSymlink( getAbsolutePath() + PATH_SEPARATOR + filename);
d->setAttrs(date, owner, size, perms);
content.push_back((FSObject*) d);
}
else
{
FSFile * f = new FSFile(getAbsolutePath() + PATH_SEPARATOR + filename);
f->setAttrs(date, owner, size, perms);
content.push_back((FSObject*) f);
}
}
std::sort(content.begin(), content.end(), directoriesFirst);
}
/*
** freeWinContent().
*/
void
freeWinContent (TuiGenWinInfoPtr winInfo)
{
if (winInfo->content != (OpaquePtr) NULL)
{
freeContent ((TuiWinContent) winInfo->content,
winInfo->contentSize,
winInfo->type);
winInfo->content = (OpaquePtr) NULL;
}
winInfo->contentSize = 0;
return;
} /* freeWinContent */
void ICLSLAM::handleScan(double timestamp, const PointScan &pscan)
{
doBackup(timestamp);
if(doICL(timestamp)) {
SegmentScan sm(pscan);
bool skip = false;
if(outdoor) {
Rototranslation rt(currentPose);
freeContent();
fforeach(const LineSegment &s, sm.getSegments()) {
segments.append(new LineSegment(rt * s));
}
fforeach(const Frontier &f, sm.getFrontiers()) {
if(f.length() > 0.2) frontiers.append(new Frontier(rt * f));
}
if(takeAMeasure(timestamp)) {
poses.append(TimedPose(timestamp, currentPose));
lastMeasurePose = lastPose;
lastMeasureTimestamp = timestamp;
}
return;
}
ICLImpl icl(sm, segments, currentPose);
icl.run();
Eigen::Vector3d z = icl.measure();
#ifndef SLAM_SKIP_DEBUG
ldbg << "ins: " << ins.getPose() << endl;
ldbg << "currentPose: " << currentPose << endl;
ldbg << "z: " << z << endl;
#endif
Eigen::Vector3d diff = currentPose.vectorForm() - z;
if(std::abs(wrap(diff[2])) >= M_PI / 12 || SQUARE(diff[0]) + SQUARE(diff[1]) >= 3) {
for(int i = 0; i < 10; i++) {
Rototranslation perturbation(
Random::normal(0, 1 / 3.),
Random::normal(0, 1 / 3.),
Random::normal(0, SQUARE(M_PI / 6) / 3.));
ICLImpl icl(sm, segments, perturbation * currentPose);
icl.run();
const Eigen::Vector3d &z1 = icl.measure();
if(poseDistance(z1, currentPose) < poseDistance(z, currentPose)) {
#ifndef SLAM_SKIP_DEBUG
ldbg << "Cambiato" << endl;
#endif
z = z1;
}
}
}
diff = currentPose.vectorForm() - z;
if(std::abs(wrap(diff[2])) >= M_PI / 12 || SQUARE(diff[0]) + SQUARE(diff[1]) >= 3) {
z = currentPose;
skipCount++;
skip = true;
}
if(skip && skipCount <= MAX_SKIP_COUNT) {
return;
} else {
skipCount = 0;
}
#ifndef SLAM_SKIP_DEBUG
ldbg << "ins: " << ins.getPose() << endl;
ldbg << "currentPose: " << currentPose << endl;
ldbg << "z: " << z << endl;
#endif
Pose guess = currentPose;
currentPose = z;
if(!almostEqual(ins.getPose().theta(), 0, 0.02) ||
!almostEqual(ins.getPose().phi(), 0, 0.02)) {
return;
}
SegmentScan rotoscan = Rototranslation(z) * sm;
#ifndef SLAM_SKIP_DEBUG
ldbg << endl << "counter=" << counter++;
ldbg << endl << "guess=" << guess << endl;
#endif
//printMap(rotoscan.getSegments(), "scan");
//printMap(segments, "walls");
//printMap(frontiers, "frontiers");
//ldbg << "plot(xwalls, ywalls, 'b', xfrontiers, yfrontiers, 'g', xscan, yscan, 'r');" << endl;
//printMapCPP(sm.getSegments(), "scan");
//printMapCPP(segments, "segments");
//ldbg << "Graphics[" << rotoscan.getSegments() << ",Dashed," << rotoscan.getFrontiers() << "]" << endl;
//ldbg << "Graphics[{" << segments << ",Dashed," << frontiers << ",Opacity[0.1],"
// << rotoscan.toPolygon() << "}]" << endl;
mergeSegments(rotoscan);
if(takeAMeasure(timestamp)) {
mergeFrontiers(rotoscan);
poses.append(TimedPose(timestamp, currentPose));
lastMeasurePose = lastPose;
lastMeasureTimestamp = timestamp;
}
if(timestamp - lastThinningTimestamp >= SLAM_MAP_THINNING_INTERVAL) {
#ifndef SLAM_SKIP_DEBUG
ldbg << "Map Thinning" << endl;
#endif
mapThinning();
lastThinningTimestamp = timestamp;
}
#ifndef SLAM_SKIP_DEBUG
//ldbg << endl << "ListPlot[" << pscan << "]" << endl;
ldbg << endl << "Graphics[" << sm.getSegments() << "]";
ldbg << endl << "Graphics[{" << segments << ",Dashed," << frontiers << ",Opacity[0.1],"
<< rotoscan.toPolygon() << "}]" << endl;
#endif
/*printMap(rotoscan.getSegments(), "scan");
printMap(segments, "walls");
printMap(frontiers, "frontiers");
ldbg << "plot(xwalls, ywalls, 'b', xfrontiers, yfrontiers, 'g', xscan, yscan, 'r');" << endl;
*/
//if(counter == 1508)
// exit(0);
Pose p = Rototranslation(initialPose) * currentPose;
emit newRobotPose(TimedPose(lastPose.timestamp(), p));
}
}
File::~File(){ close(); freeContent(); }
void File::allocContent(int n){
if(mContent) freeContent();
mContent = new char[n+1];
mContent[n] = '\0';
}
