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;
}
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;
}
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;
}
}
void saythis(std::string text)
{
Bottle& b=speechOut.prepare();
b.clear();
b.add(text.c_str());
speechOut.writeStrict();
log(now, text.c_str());
return;
}
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();
}
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
}
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;
}
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;
}
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;
}
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;
}
/*
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
}
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");}
}
