void onRead(Sound& sound) {
int ct = port.getPendingReads();
//printf("pending reads %d\n", ct);
while (ct>padding) {
ct = port.getPendingReads();
printf("Dropping sound packet -- %d packet(s) behind\n", ct);
port.read();
}
mutex.wait();
/*
if (muted) {
for (int i=0; i<sound.getChannels(); i++) {
for (int j=0; j<sound.getSamples(); j++) {
sound.put(0,j,i);
}
}
}
*/
if (!muted) {
if (put!=NULL) {
put->renderSound(sound);
}
}
if (saving) {
saveFrame(sound);
}
mutex.post();
Time::yield();
}
void onRead(Sound& sound)
{
#ifdef TEST
//this block can be used to measure time elapsed between two sound packets
static double t1= yarp::os::Time::now();
static double t2= yarp::os::Time::now();
t1= yarp::os::Time::now();
printf("onread %f\n", t2-t1);
t2 = yarp::os::Time::now();
#endif
int ct = port.getPendingReads();
//printf("pending reads %d\n", ct);
while (ct>padding) {
ct = port.getPendingReads();
printf("Dropping sound packet -- %d packet(s) behind\n", ct);
port.read();
}
mutex.wait();
/*
if (muted) {
for (int i=0; i<sound.getChannels(); i++) {
for (int j=0; j<sound.getSamples(); j++) {
sound.put(0,j,i);
}
}
}
*/
if (!muted) {
if (put!=NULL) {
put->renderSound(sound);
}
}
if (saving) {
saveFrame(sound);
}
mutex.post();
Time::yield();
}
void testReopen() {
report(0,"checking opening/closing/reopening ports...");
BufferedPort<Bottle> port2;
port2.open("/test2");
BufferedPort<Bottle> port;
port.open("/test");
Network::connect("/test2", "/test");
Network::sync("/test");
Network::sync("/test2");
port2.prepare().fromString("1 msg");
port2.write();
while (port.getPendingReads()<1) {
Time::delay(0.1);
}
checkFalse(port.isClosed(),"port tagged as open");
port.close();
checkTrue(port.isClosed(),"port tagged as closed");
port.open("/test");
checkFalse(port.isClosed(),"port tagged as open");
Bottle *bot = port.read(false);
checkTrue(bot==NULL,"reader correctly reset");
Network::connect("/test2", "/test");
Network::sync("/test");
Network::sync("/test2");
port2.prepare().fromString("2 msg");
port2.write();
bot = port.read();
checkFalse(bot==NULL,"reader working");
if (bot) {
checkEqual(bot->get(0).asInt(),2,"reader read correct message");
}
}
void SamIter(void)
{
Bottle *goal = GoalIn.read(false);
//puts("running reader");
//while (GoalIn.getPendingReads() > 0)
// goal = GoalIn.read(false);
if(goal!=NULL) // check theres data
{
std::cout << "size of bottle is " << goal->size() << " data " << goal->toString().c_str() << std::endl;
if(goal->size()>0)
{
//iGoalPostion =GetGoalFromString(goal->get(1).asString().c_str());
iGoalPostion=atoi(goal->get(1).asString().c_str());
SetGoalPos(iGoalPostion);
bReached=false;
std::cout<< "got a goal, location " << iGoalPostion <<std::endl;
iStuckCount=0;
loopCnt=0;
robot->enableMotors();
robot->enableSonar();
}
}
//// send back a bottle with current voltage value
//Bottle& b2 = PwrOut.prepare(); // prepare the bottle/port
//b2.clear();
//b2.addDouble(robot->getRealBatteryVoltage()); // indicates robot voltage avg
//PwrOut.writeStrict();
Bottle *input = NULL;
//puts("running reader");
while (StarIn.getPendingReads() > 0)
input = StarIn.read(false);
if(input!=NULL) // check theres data
{
dStarId = input->get(0).asDouble();
//std::cout << "star id " << starID << std::endl;
dX = input->get(1).asDouble();
dY = input->get(2).asDouble();
dTh = input->get(3).asDouble();
if(firstStarReading==false)
{
CurrentPose.setX(dX);
CurrentPose.setY(dY);
CurrentPose.setTh(dTh);
OldPose = CurrentPose;
firstStarReading=true;
}
iStarCntr++;
if((OldPose.findDistanceTo(CurrentPose)<1.5 || iStarCntr>3) && dStarId!=0)
{
CurrentPose.setX(dX);
CurrentPose.setY(dY);
CurrentPose.setTh(dTh);
OldPose = CurrentPose;
iStarCntr=0;
}
//puts("got a msg");
//puts(input->toString());
//std::cout << " robot->getRealBatteryVoltageNow() " << " " << robot->getRealBatteryVoltageNow()<<std::endl;
}
if(bReached==false)
{
loopCnt++;
double ang = atan2(GoalPose.getY() - CurrentPose.getY(), GoalPose.getX() - CurrentPose.getX());
double dist = GoalPose.findDistanceTo(CurrentPose);
//check if the robot is stuck while navigating
if(dist>0)
{
//first distance reading
if(iStuckCount==0)
OldDist=dist;
DiffDist=OldDist-dist;
if(loopCnt%5==0 && abs(DiffDist)<0.1)
{
iStuckCount++;
std::cout << "diff dist, dist, stuck count " << DiffDist << " " << dist << " " << iStuckCount <<std::endl;
}
else
OldDist=dist;
//recover motion
if(iStuckCount>6)
{
robot->lock();
robot->setVel(-100);//move back
yarp::os::Time::delay(5);
robot->unlock();
//robot->setVel(0);//move back
iStuckCount=0;
}
}
//calculate_distance(dX, dY, GoalPose.getX(), GoalPose.getY());//ar.findDistanceTo(ArPose(-0.75,3.0,4.5));
ang-=dTh;
while(ang>3.14159265){ang-=6.28318531;} // normalise PI ie if over 180, then minus 360
while(ang<-3.14159265){ang+=6.28318531;}
ang = Rad2Deg(ang);
//std::cout << "required angle, dist " << ang << " " << dist <<std::endl;
//if the robot has to turn more then turn threshold for obst avoidance is less and speed is 0
if(abs(ang)>55)
{
myTurnThreshold = 250;
myStopDistance = 200;
speed=0;
}
else
{
speed=220;
myTurnThreshold = 550;
myStopDistance = 450;
}
bool obst= ObstacleAvoidance();
if(dist>=0.7 && obst==false)//&& abs(ang)>10
{
robot->lock();
std::cout <<"navigating to goal " << iGoalPostion<<std::endl;
/* if(abs(ang)<45)
speed=200;
else
speed=0;*/
robot->setDeltaHeading(ang/2.0);
robot->setVel(speed);
robot->unlock();
}
else if(dist<0.7)
{
robot->lock();
double rotFinal = GoalPose.getTh();
rotFinal-=dTh;
std::cout <<"goal rot " << rotFinal <<std::endl;
while(rotFinal>3.14159265){rotFinal-=6.28318531;} // normalise PI ie if over 180, then minus 360
while(rotFinal<-3.14159265){rotFinal+=6.28318531;}
rotFinal = Rad2Deg(rotFinal);
robot->setDeltaHeading(rotFinal);
robot->setVel(0);
std::cout <<"final rot " << rotFinal <<std::endl;
//robot->setDeltaHeading(0);
ang=0;
speed=0;
puts("reached ");
bReached=true;
robot->unlock();
// send back a bottle with goal position reached
Bottle& b2 = GoalIn.prepare(); // prepare the bottle/port
b2.clear();
b2.addInt(iGoalPostion); // indicates robot reached goal location
GoalIn.writeStrict();
//wait for some time for rotation to complete
yarp::os::Time::delay(5);
robot->disableMotors();
robot->disableSonar();
}
/*myfile.open ("motion.txt");
if (myfile.is_open())
{
myfile << ang << "|" << speed << "\n";
myfile.close();
}*/
}
//else{puts("didn't get a msg");}
}
bool respond(const Bottle& cmd, Bottle& reply)
{
if (cmd.get(0).asString()=="save")
{
Bottle bot1,bot2;
bot1.addVocab(VOCAB_CMD_STOP);
outGestRecModule.write(bot1,bot2);
string a="Ok, show me this gesture";
Bottle w;
w.addString(a.c_str());
outspeak.write(w);
Bottle bot5,bot6;
bot5.addString("execute");
bot5.addInt(cmd.get(1).asInt());
outExecModule.write(bot5,bot6);
Bottle bot7,bot8;
bot7.addVocab(VOCAB_CMD_UPDATE_INITIAL);
outGestRecModule.write(bot7,bot8);
Bottle bot3,bot4;
bot3.addVocab(VOCAB_CMD_SAVE);
stringstream ss;
ss << cmd.get(1).asInt();
string str = "action"+ss.str();
bot3.addString(str.c_str());
outGestRecModule.write(bot3,bot4);
reply.addString("Starting saving features");
return true;
}
if (cmd.get(0).asString()=="trained")
{
string a="Training";
Bottle w;
w.addString(a.c_str());
outspeak.write(w);
Bottle bot1,bot2;
bot1.addVocab(VOCAB_CMD_TRAIN);
outGestRecModule.write(bot1,bot2);
string b="Thanks, now I know the gesture";
Bottle w1;
w1.addString(b.c_str());
outspeak.write(w1);
reply.addString("Training done");
return true;
}
if (cmd.get(0).asString()=="start")
{
count=1;
Bottle bot1,bot2;
bot1.addVocab(VOCAB_CMD_UPDATE_INITIAL);
outGestRecModule.write(bot1,bot2);
Bottle bot3,bot4;
bot3.addVocab(VOCAB_CMD_REC);
outGestRecModule.write(bot3,bot4);
double number=randGen.get(0,1);
string turn;
if (number<=0.5)
turn="your turn";
else
turn="my turn";
string a="Ok, let's start the game, it is "+turn;
reply.addString("Ok, let's start the game ");
reply.addString(yourturn?"your turn":"my turn");
Bottle w;
w.addString(a.c_str());
outspeak.write(w);
if(number>0.5)
Time::delay(3.5);
int pending=inSequence.getPendingReads();
for (int i=0; i<pending; i++)
inSequence.read(false);
gameEnding=false;
if (number>0.5)
myturn=true;
else
yourturn=true;
return true;
}
else if (cmd.get(0).asString()=="win"||cmd.get(0).asString()=="lose"||cmd.get(0).asString()=="over")
{
reply.addString("Quitting game");
Bottle bot1,bot2;
bot1.addString("stop");
outExecModule.write(bot1,bot2);
Bottle bot3,bot4;
bot3.addVocab(VOCAB_CMD_STOP);
outGestRecModule.write(bot3,bot4);
sequence="";
currentSequence="";
gameEnding=true;
myturn=false;
yourturn=false;
fprintf(stdout, "Quitting game\n");
if (cmd.get(0).asString()=="win")
{
Bottle w;
w.addString("Yay! I'm happy!");
outspeak.write(w);
fprintf(stdout, "yay! I'm happy!\n");
}
else if (cmd.get(0).asString()=="lose")
{
Bottle w;
w.addString("I'm really sad!");
outspeak.write(w);
fprintf(stdout, "Oh No!\n");
}
else
{
Bottle w;
w.addString("Game over!");
outspeak.write(w);
fprintf(stdout, "Quitting!\n");
}
return true;
}
else if (cmd.get(0).asString()=="done"&&!gameEnding)
{
Bottle bot1,bot2;
bot1.addVocab(VOCAB_CMD_UPDATE_INITIAL);
outGestRecModule.write(bot1,bot2);
Bottle bot3,bot4;
bot3.addVocab(VOCAB_CMD_REC);
outGestRecModule.write(bot3,bot4);
yourturn=true;
int pending=inSequence.getPendingReads();
for (int i=0; i<pending; i++)
inSequence.read(false);
reply.addString("ack");
Bottle w;
w.addString("It's your turn");
outspeak.write(w);
return true;
}
else if(cmd.get(0).asString()=="turn")
{
Bottle bot3,bot4;
bot3.addVocab(VOCAB_CMD_STOP);
outGestRecModule.write(bot3,bot4);
if(!gameEnding)
{
myturn=true;
yourturn=false;
reply.addString("It is my turn now");
}
return true;
}
else
{
reply.addString("command not recognized");
return true;
}
}
