Пример #1
0
    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();
    }
Пример #3
0
    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");
        }
    }
Пример #4
0
	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");}
	} 
Пример #5
0
    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;
        }
    }