コード例 #1
0
ファイル: Vlaser.cpp プロジェクト: nurv/lirec
	void SamIter(void)
	{
	bool thereIsObservation,hardError;	
	CObservation2DRangeScan myscan;
	
	
    laser.doProcessSimple( thereIsObservation, myscan, hardError );
	Bottle &Send = myfirst.prepare();
	Send.clear();

	if(thereIsObservation )
		{
			//scan only 512 points for 180 deg, ignore the first,last 85 points
			for(int x=0;x<myscan.scan.size()-85;x++)
			{
				//if(myscan.scan[x]<mindistance){myscan.scan[x]=0;}
				if(x>84) 
				{
					Send.addDouble(myscan.scan[x]);

					if(myscan.scan[x]==1.0)
						std::cout << "obst sensed at " << x <<std::endl;
					
					//if(myscan.scan[x]<mindistance){myscan.scan[x]=0;}
				}
				//std::cout << "Scan size Vlaser" << myscan.scan.size() <<std::endl;
			}
		myfirst.writeStrict();
		}
	//std::cout<< " laser size " <<Send.size() <<std::endl;
	}
コード例 #2
0
ファイル: main.cpp プロジェクト: caomw/wysiwyd
    bool updateModule()
    {
        LockGuard lg(mutex);

        double dumpTime=Time::now();
        Bottle &bDump=dumpPort.prepare();
        bDump.clear();

        bDump.addString(actionTag);
        bDump.addString(objectTag);

        opc.checkout();        

        // agent body + position
        agentName = getPartnerName();
        if (Entity *e=opc.getEntity(agentName))
        {
            if (Agent *agent=dynamic_cast<Agent*>(e))
            {
                bDump.addList()=agent->m_body.asBottle();
                Bottle &bAgent=bDump.addList();
                bAgent.addString(agent->name());
                bAgent.addDouble(agent->m_ego_position[0]);
                bAgent.addDouble(agent->m_ego_position[1]);
                bAgent.addDouble(agent->m_ego_position[2]);
                bAgent.addInt(agent->m_present==1.0?1:0);
            }
        }

        // objects position
        list<Entity*> lEntity=opc.EntitiesCache();
        for (list<Entity*>::iterator itEnt=lEntity.begin(); itEnt!=lEntity.end(); itEnt++)
        {
            string entityName=(*itEnt)->name();
            string entityType=(*itEnt)->entity_type();
            if (entityType==EFAA_OPC_ENTITY_OBJECT)
            {
                if (Object *object=dynamic_cast<Object*>(*itEnt))
                {
                    Bottle &bObject=bDump.addList();
                    bObject.addString(object->name());
                    bObject.addDouble(object->m_ego_position[0]);
                    bObject.addDouble(object->m_ego_position[1]);
                    bObject.addDouble(object->m_ego_position[2]);
                    bObject.addInt(object->m_present==1.0?1:0);
                }
            }
        }

        bDump.addInt(gate);        

        dumpStamp.update(dumpTime);
        dumpPort.setEnvelope(dumpStamp);
        dumpPort.writeStrict();

        yInfo()<<bDump.toString();

        return true;
    }
コード例 #3
0
ファイル: PhidgetSensor.cpp プロジェクト: nurv/lirec
	void SamIter(void)
	{
		Bottle *b = bRead.read(false);

		//while (bRead.getPendingReads() > 0)
		//	b = bRead.read(false); // get in the input from the port, if you want it to wait use true, else use false

		if(b!=NULL)						 // check theres data
		{
			
			if(b->size()>0)
			{
				task = b->get(0).asString();
				int iStatus=atoi(b->get(1).asString().c_str());
				//check if message is related to charging
				if(task.compare("ChargingStatus")==0)
				{
					
					//relay switch: off mobile base and close modules
					if(iStatus==0)
					{
						yarp::os::Time::delay(10);
						CPhidgetInterfaceKit_setOutputState (ifKit, 6, 0);
						system("stop.bat");
						bCharging=true;
					}
					else if(iStatus==1 && bCharging==true)//relay switch: on mobile base and start modules
					{
						CPhidgetInterfaceKit_setOutputState (ifKit, 6, 1);
						yarp::os::Time::delay(2);
						system("start.bat");
						bCharging=false;
					}
				}
				else
				{
					iflag = iStatus;
					//start timer 
					if(iflag==1)
						time (&start);
				}
			
			}

			std::cout << "got a task " << b->toString().c_str() << std::endl;
		}

		//send phone ring sensor data only if sensor value is changed
		if(bPhoneRingChanged)
		{
			// send back a bottle with current voltage value
			Bottle& b3 = bPhone.prepare();	  // prepare the bottle/port
			b3.clear();
			b3.addInt( iPhoneRing ); // indicates robot voltage		
			bPhone.writeStrict();
			bPhoneRingChanged=false;
		}

	}
コード例 #4
0
ファイル: main.cpp プロジェクト: robotology/attention
 void saythis(std::string text)
 {
     Bottle& b=speechOut.prepare();
     b.clear();
     b.add(text.c_str());
     speechOut.writeStrict();
     log(now, text.c_str());
     return;
 }
コード例 #5
0
ファイル: main.cpp プロジェクト: robotology/attention
 void log(double time, string strng)
 {
     stringstream logstream;
     logstream << setprecision(15) << time << " " << strng.c_str();
     cout << logstream.str() << endl;
     Bottle& b=logOut.prepare();
     b.clear();
     b.add(logstream.str().c_str());
     logOut.writeStrict();
 }
コード例 #6
0
ファイル: VStarGazer.cpp プロジェクト: nurv/lirec
	void SamIter(void)
	{
		Bottle& B = bStarSend.prepare();		// prepare the bottle/port
		B.clear();
		B.addInt(i_ID);
		B.addDouble(i_X);
		B.addDouble(i_Y);
		B.addDouble(i_Angle);
		bStarSend.writeStrict();					// add stuff then send
	}
コード例 #7
0
ファイル: buffered_port.cpp プロジェクト: AbuMussabRaja/yarp
int main() {
    // Initialize YARP - some OSes need network and time service initialization
    Network yarp;

    // Work locally - don't rely on name server (just for this example).
    // If you have a YARP name server running, you can remove this line.
    Network::setLocalMode(true);

    // Create two ports that we'll be using to transmit "Bottle" objects.
    // The ports are buffered, so that sending and receiving can happen
    // in the background.
    BufferedPort<Bottle> in;
    BufferedPort<Bottle> out;

    // we will want to read every message, with no skipping of "old" messages
    // when new ones come in
    in.setStrict();

    // Name the ports
    in.open("/in");
    out.open("/out");

    // Connect the ports so that anything written from /out arrives to /in
    Network::connect("/out","/in");

    // Send one "Bottle" object.  The port is responsible for creating
    // and reusing/destroying that object, since it needs to be sure
    // it exists until communication to all recipients (just one in
    // this case) is complete.
    Bottle& outBot1 = out.prepare();   // Get the object
    outBot1.fromString("hello world"); // Set it up the way we want
    printf("Writing bottle 1 (%s)\n", outBot1.toString().c_str());
    out.write();                       // Now send it on its way

    // Send another "Bottle" object
    Bottle& outBot2 = out.prepare();
    outBot2.fromString("2 3 5 7 11");
    printf("Writing bottle 2 (%s)\n", outBot2.toString().c_str());
    out.writeStrict();                 // writeStrict() will wait for any
                                       // previous communication to finish;
                                       // write() would skip sending if
                                       // there was something being sent

    // Read the first object
    Bottle *inBot1 = in.read();
    printf("Bottle 1 is: %s\n", inBot1->toString().c_str());

    // Read the second object
    Bottle *inBot2 = in.read();
    printf("Bottle 2 is: %s\n", inBot2->toString().c_str());

    return 0;
}
コード例 #8
0
int LookAt(Point& center, Mat& CircleMat)													// returning a number because we want the return code to be nice
{
	Bottle *RiKinIn = iKinInPort.read();													// At first we read the Input Bottle (it will be a vector of doubles)
	if (RiKinIn != NULL)																	// Check if we got something
	{
		// ##########################
		// Current Focus of Attention
		// ##########################

		double z = RiKinIn->get(0).asDouble();												// read the coordinates where the robot currently looks
		double x = RiKinIn->get(1).asDouble();
		double y = RiKinIn->get(2).asDouble();

		circle(CircleMat, Point((160.0 + 640.0 * x), (360.0 - 600.0 * y)), 3, Scalar(0, 0, 255), 3, 8, 0);	// Painting a red dot where the robot looks (and converting to the move_head reference)

		printf("Looking at: %.2f - %.2f \n", x, y);											// printing where the robot looks

		Bottle &RiKinOut = iKinOutPort.prepare();											// prepare sending to the robot (and get a reference to the bottle)
		RiKinOut.clear();																	// clear the cache

		// ######################
		// New Focus of Attention
		// ######################

		double NewX = (static_cast<double>(center.x) - 160.0) / 640.0;						// convert where the Robot should look into the move_head reference
		double NewY = (static_cast<double>(center.y) - 360.0) / -600.0;
		double NewZ = -3.0;

		if ((abs(NewX - x) > 0.02) || (abs(NewY - y) > 0.02))								// We move the head only if the difference between the point where the robot currently looks
		{																					// and where we want the robot to look is big enough (this ensures not flooding the controller)
			RiKinOut.addDouble(NewZ);														// Add our coordinates to the bottle
			RiKinOut.addDouble(NewX);
			RiKinOut.addDouble(NewY);
			iKinOutPort.writeStrict();														// writing to the output port and wait until writing is done
			printf("Look at: %.2f(%i) - %.2f(%i)\n", NewX, center.x, NewY, center.y);		// inform the user were we want the robot to look at
		}

	}
	else
	{																						// if our bottle is empty :( then we should deal with it
		printf("Error in the iKinGazeCtrl Interface! Please restart the program and the iKinGazeCtrl Interface.\n");
		printf("Press Return to Exit\n");
		std::cin.get();
		return ERROR_DEV_NOT_EXIST;
	}
	return ERROR_SUCCESS;
}
コード例 #9
0
ファイル: PhidgetSensor.cpp プロジェクト: nurv/lirec
	void SamSendVoltage(void)
	{
		/*
		int iVolt=0;
		if(dVoltageNow<=12.4999)//require recharge
			iVolt=0;
		else if(dVoltageNow>=12.5000 && dVoltageNow<14.0000)//fine
			iVolt=1;
		else if (dVoltageNow>=14.0000)//fully charged
			iVolt=2;
		*/

		
		std::string s;
 
		// convert double b to string s
		std::ostringstream ss;
		ss << dVoltageNow;
		s = ss.str();

		
		//if(iVoltage!=iVolt)
		//{
			// send back a bottle with current voltage value
			Bottle& b2 = bRead.prepare();// prepare the bottle/port
			b2.clear();
			//b2.addInt( iVolt );// indicates robot voltage		
			b2.addString(s.c_str());
			bRead.writeStrict();
			
			//iVoltage=iVolt;
		//}
		std::cout << " voltage " << dVoltageNow << std::endl; 


	}
コード例 #10
0
ファイル: main.cpp プロジェクト: pattacini/icub-contrib
    bool updateModule()
    {
        ImageOf<PixelRgb> *imgIn=imgInPort.read(true);
        if (imgIn==NULL)
            return false;

        ImageOf<PixelRgb> &imgOut=imgOutPort.prepare();
        imgOut=*imgIn;

        cv::Mat img=cv::cvarrToMat(imgOut.getIplImage());

        Vector xa,oa;
        iarm->getPose(xa,oa);

        Matrix Ha=axis2dcm(oa);
        xa.push_back(1.0);
        Ha.setCol(3,xa);

        Vector pc;
        igaze->get2DPixel(camSel,xa,pc);

        cv::Point point_c((int)pc[0],(int)pc[1]);
        cv::circle(img,point_c,4,cv::Scalar(0,255,0),4);

        Vector analogs,encs(nEncs),joints;
        if (ianalog!=NULL)
            ianalog->read(analogs);
        iencs->getEncoders(encs.data());

        for (int i=0; i<3; i++)
        {
            if (ianalog!=NULL)
                finger[i].getChainJoints(encs,analogs,joints);
            else
                finger[i].getChainJoints(encs,joints);
            finger[i].setAng(CTRL_DEG2RAD*joints);
        }

        for (int fng=0; fng<3; fng++)
        {
            deque<cv::Point> point_f;
            for (int i=-1; i<(int)finger[fng].getN(); i++)
            {
                Vector fc;
                igaze->get2DPixel(camSel,Ha*(i<0?finger[fng].getH0().getCol(3):
                                                 finger[fng].getH(i,true).getCol(3)),fc);
                point_f.push_front(cv::Point((int)fc[0],(int)fc[1]));
                cv::circle(img,point_f.front(),3,cv::Scalar(0,0,255),4);

                if (i>=0)
                {
                    cv::line(img,point_f.front(),point_f.back(),cv::Scalar(255,255,255),2);
                    point_f.pop_back();
                }
                else
                    cv::line(img,point_c,point_f.front(),cv::Scalar(255,0,0),2);
            }
        }

        imgOutPort.writeStrict();
        return true;
    }
コード例 #11
0
/*
SendImage(address to input image)

This send a Mat to the Yarpview - waits until sending is finished
*/
void SendImage(Mat& input)
{
	ImageOf<PixelBgr>& outp = imageOutPort.prepare();										// we prepare our output image and get a pointer to an image object
	outp.wrapIplImage(ConvertImage(input).getIplImage());									// At first we convert the image, the we getting an ipl image out of this and wrapping this into our output image
	imageOutPort.writeStrict();																// now we send the image, AND wait until its sent
}
コード例 #12
0
ファイル: GoToNavigation.cpp プロジェクト: nurv/lirec
	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");}
	}