Node *readNewickNode(FILE *infile, Tree *tree, Node *parent, int &depth)
{
char chr, char1;
Node *node;
string token;
// read first character
if (!(char1 = readChar(infile, depth))) {
printError("unexpected end of file");
return NULL;
}
if (char1 == '(') {
// read internal node
int depth2 = depth;
node = tree->addNode(new Node());
if (parent)
parent->addChild(node);
// read all child nodes at this depth
while (depth == depth2) {
Node *child = readNewickNode(infile, tree, node, depth);
if (!child)
return NULL;
}
// read distance for this node
char chr = readUntil(infile, token, "):,;", depth);
if (chr == ':') {
node->dist = readDist(infile, depth);
if (!(chr = readUntil(infile, token, "):,", depth)))
return NULL;
} else {
// node name, if it does not start with a number or ".","-"
if (!isdigit(chr) && chr != '.' && chr != '-') {
node->longname = char1 + trim(token.c_str());
}
}
return node;
} else {
// read leaf
node = tree->addNode(new Node());
if (parent)
parent->addChild(node);
// read name
if (!(chr = readUntil(infile, token, ":),", depth)))
return NULL;
node->longname = char1 + trim(token.c_str());
// read distance for this node
if (chr == ':') {
node->dist = readDist(infile, depth);
if (!(chr = readUntil(infile, token, ":),", depth)))
return NULL;
}
return node;
}
}
void startSlam(int v)
{
FileStorage fs("config.yml", FileStorage::READ);
Mat cam, dist, rpos, q, s;
fs["cameraMatrix"] >> cam;
fs["distMatrix"] >> dist;
fs["sensorPos"] >> rpos;
fs["q"] >> q;
fs["s"] >> s;
fs.release();
ml = new MarkerLocator(cam, dist, rpos, 100.);
// Mat q = (Mat_<double>(2,1) << .01, .1*M_PI/180);
// Mat s = (Mat_<double>(2,1) << 10, .5*M_PI/180);
slam = new EkfSlam(&scan, q, s);
#ifdef GUI
namedWindow("EKFSlam");
disp = new MapDisplay("Map");
waitKey();
#endif
Mat u = (Mat_<double>(2,1) << 0, 0);
int fd = openPort();
oiStart();
setSafeMode();
readDist();
readAngle();
VideoCapture cap(0);
if(!cap.isOpened()) {
printf("Failed capture\n");
return;
}
VideoWriter record("/tmp/demo.avi", CV_FOURCC('D','I','V','X'), 30, Size(1340,600), true);
if(!record.isOpened()) {
printf("Failed writer\n");
return;
}
clock_t t;
time_t stime;
while(1) {
t = clock();
setDrive(v,v);
double mindist = 99999;
while(msElap(t) < 100 && !hitWall()) {
u.at<double>(0) = (double)readDist();
u.at<double>(1) = M_PI*(double)readAngle()/180;
mindist = slamStep(u, cap, record);
}
// backup before turn when running into something
if(hitWall() || mindist < 700) {
if(hitWall()) {
// backup
setDrive(-15,-15);
t = clock();
while(msElap(t) < 500) {
u.at<double>(0) = (double)readDist();
u.at<double>(1) = M_PI*(double)readAngle()/180;
slamStep(u, cap, record);
}
}
// turn
if(time(&stime)%60 < 30) {
setDrive(-5, 5);
}
else
setDrive(5,-5);
t = clock();
while(msElap(t) < 250) {
u.at<double>(0) = 0;
u.at<double>(1) = M_PI*(double)readAngle()/180;
slamStep(u, cap, record);
}
}
if(waitKey(10) == 27)
break;
}
setDrive(0,0);
imwrite("lastFrame.png", g_outFrame);
close(fd);
delete ml;
delete slam;
#ifdef GUI
delete disp;
#endif
}
