Bottle SimCyl::rotateObjectBottle() {
Bottle cmd;
cmd.addString("world");
cmd.addString("rot");
cmd.addString("cyl");
cmd.addInt (objSubIndex);
cmd.addDouble(rotationX);
cmd.addDouble(rotationY);
cmd.addDouble(rotationZ);
return cmd;
}
Bottle SimCyl::moveObjectBottle() {
Bottle cmd;
cmd.addString("world");
cmd.addString("set");
cmd.addString("cyl");
cmd.addInt (objSubIndex);
cmd.addDouble(positionX);
cmd.addDouble(positionY);
cmd.addDouble(positionZ);
return cmd;
}
void Controller::notifyEvent(const string &event, const double checkPoint)
{
if (port_event.getOutputCount()>0)
{
Bottle bottle;
bottle.addString(event.c_str());
bottle.addDouble(q_stamp);
if (checkPoint>=0.0)
bottle.addDouble(checkPoint);
port_event.write(bottle);
}
}
// if the object is located in both cameras, return its stereo position
bool VisuoThread::getObject(const std::string &object_name, Bottle &bStereo)
{
Vector stereo;
bool ok=opcPort.getStereoPosition(object_name,stereo);
if(!ok)
{
double t=Time::now();
while(Time::now()-t<objectWaitThresh && !interrupted)
{
MILMutex.wait();
if(locations.count(object_name)>0)
{
stereo.resize(4);
stereo[0]=locations[object_name].x;
stereo[1]=locations[object_name].y;
stereo[2]=160.0;
stereo[3]=120.0;
ok=true;
}
MILMutex.post();
}
}
for(size_t i=0; i<stereo.size(); i++)
bStereo.addDouble(stereo[i]);
return ok;
}
bool VisuoThread::getTrack(Bottle &bStereo)
{
Vector stereo;
bool ok=false;
trackMutex.wait();
if(stereoTracker.vec.size()==12)
{
stereo.resize(4);
stereo[0]=stereoTracker.vec[0];
stereo[1]=stereoTracker.vec[1];
stereo[2]=stereoTracker.vec[6];
stereo[3]=stereoTracker.vec[7];
}
trackMutex.post();
for(size_t i=0; i<stereo.size(); i++)
bStereo.addDouble(stereo[i]);
trackMutex.wait();
tracking=true;
trackMutex.post();
trackMode=MODE_TRACK_TEMPLATE;
return ok;
}
bool VisuoThread::getFixation(Bottle &bStereo)
{
Vector stereo(4);
imgMutex.wait();
if(img[LEFT]!=NULL)
{
stereo[0]=stereo[2]=0.5*img[LEFT]->width();
stereo[1]=stereo[3]=0.5*img[LEFT]->height();
}
else
{
stereo[0]=stereo[2]=160;
stereo[1]=stereo[3]=120;
}
imgMutex.post();
for(size_t i=0; i<stereo.size(); i++)
bStereo.addDouble(stereo[i]);
int side=40;
startTracker(stereo,side);
return true;
}
int main(int argc, char *argv[]) {
Network yarp;
Port portRpc, portCommand, portState;
if (!portRpc.open("/motor/server/rpc:i")) {
fprintf(stderr,"Failed to open a port, maybe run: yarpserver\n");
return 1;
}
portCommand.open("/motor/server/command:i");
portState.open("/motor/server/state:o");
MotorImpl motor;
motor.serve(portRpc);
//motor.serve(portCommand);
double start = Time::now();
while (true) {
double now = Time::now();
if (now-start>10) {
printf("Motor server running happily\n");
start += 10;
if (now-start>10) start = now;
}
// stream motor state every now and then
Bottle b;
b.addDouble(motor.get_enc(0));
b.addDouble(motor.get_enc(1));
portState.write(b);
Time::delay(0.02);
}
return 0;
}
virtual ArActionDesired *fire (ArActionDesired currentDesired)
{
if(BeenCorrectedByStar || !usestar) // if we have updated odo from star or is star isn't being used
{
ArPose MyPose;
MyPose = myRobot->getPose();
Bottle PoseBottle;
PoseBottle.addDouble(MyPose.getX()/1000);
PoseBottle.addDouble(MyPose.getY()/1000);
PoseBottle.addDouble(MyPose.getTh());
Mycopyofmodule->SendBottleData("MAPout",PoseBottle);
printf("my x:%f y:%f rot:%f \n",MyPose.getX()/1000,MyPose.getY()/1000,MyPose.getTh());
}
return &myDesired;
}
void boardDumperThread::run()
{
//printf("Entering the main thread\n");
//enc->getEncoders(data);
//Bottle bData;
//for (int i = 0; i < numberOfJointsRead; i++)
// {
// dataRead[i] = data[dataMap[i]];
// bData.addDouble(dataRead[i]);
// }
//port->write(bData);
if (getter)
{
//printf("Getter is getting something\n");
getter -> getData(data);
//fprintf(stderr, "Time is %lf \n", stmp.getTime());
Bottle bData;
for (int i = 0; i < numberOfJointsRead; i++)
{
//printf("%.2f \n", data[dataMap[i]]);
dataRead[i] = data[dataMap[i]];
bData.addDouble(dataRead[i]);
}
if (getter->getStamp(stmp))
{
if (stmp.isValid())
{
port->setEnvelope(stmp);
}
else
{
//stmp.update();
stmp=Stamp(-1,0.0);
port->setEnvelope(stmp);
}
}
else
{
fprintf(stderr, "boardDumperThread::warning. Trying to get a stamp without a proper IPreciselyTimed defined. \n");
}
if (logFile)
{
char buff [20];
sprintf(buff,"%d ",stmp.getCount());
fputs (buff,logFile);
sprintf(buff,"%f ",stmp.getTime());
fputs (buff,logFile);
fputs (bData.toString().c_str(),logFile);
fputs ("\n",logFile);
}
port->write(bData);
}
}
void skinEventsAggregThread::run()
{
int indexOfBiggestContact = -1;
skinContact biggestContactInSkinPart;
Vector geoCenter(3,0.0), normalDir(3,0.0);
double activation = 0.0;
Bottle & out = skinEvAggregPortOut.prepare(); out.clear();
Bottle b;
b.clear();
ts.update();
skinContactList *scl = skinEventsPortIn.read(false);
if(scl)
{
if(!(scl->empty()))
{
//Probably source of crazy inefficiencies, here just to reach a working state as soon as possible \todo TODO
map<SkinPart, skinContactList> contactsPerSkinPart = scl->splitPerSkinPart();
for(map<SkinPart,skinContactList>::iterator it=contactsPerSkinPart.begin(); it!=contactsPerSkinPart.end(); it++)
{
indexOfBiggestContact = getIndexOfBiggestContactInList(it->second);
if (indexOfBiggestContact != -1)
{
b.clear();
biggestContactInSkinPart = (it->second)[indexOfBiggestContact];
//the output prepared should have identical format to the one prepared in void vtRFThread::manageSkinEvents()
b.addInt(biggestContactInSkinPart.getSkinPart());
vectorIntoBottle(biggestContactInSkinPart.getGeoCenter(),b);
vectorIntoBottle(biggestContactInSkinPart.getNormalDir(),b);
//we add dummy geoCenter and normalDir in Root frame to keep same format as vtRFThread manageSkinEvents
b.addDouble(0.0); b.addDouble(0.0); b.addDouble(0.0);
b.addDouble(0.0); b.addDouble(0.0); b.addDouble(0.0);
b.addDouble(std::max(1.0,(biggestContactInSkinPart.getPressure()/SKIN_ACTIVATION_MAX))); // % pressure "normalized" with ad hoc constant
b.addString(biggestContactInSkinPart.getSkinPartName()); //this one just for readability
out.addList().read(b);
}
}
skinEvAggregPortOut.setEnvelope(ts);
skinEvAggregPortOut.write(); // send something anyway (if there is no contact the bottle is empty)
}
}
}
int TRACKERModule::track(const double fix_x, const double fix_y)
{
Bottle b;
b.addDouble(fix_x);
b.addDouble(fix_y);
int rep = trackerManager->processFixationPoint(b);
return rep;
}
Bottle IncomingEvent::toBottle()
{
Bottle b;
b.clear();
b.addDouble(Pos[0]);
b.addDouble(Pos[1]);
b.addDouble(Pos[2]);
b.addDouble(Vel[0]);
b.addDouble(Vel[1]);
b.addDouble(Vel[2]);
b.addDouble(Radius);
b.addString(Src);
b.addDouble(Threat); //keep it last for now - for backward compatibility
return b;
}
Bottle PMPthread::initTorso(Bottle angles)
{
Bottle reply;
reply.clear();
string msg;
//check if the connection is established. If not, try to connect to DevDriver input ports
if (!Network::exists(("/" + rpcServerName + "/rpc").c_str()) )
{
//printf("Error: device ports not active\n");
reply.addString("Error: device ports not active");
return reply;
}
if( !Network::isConnected(rpcPort.getName().c_str(), ("/" + rpcServerName + "/rpc").c_str()) )
{
if(!Network::connect(rpcPort.getName().c_str(), ("/" + rpcServerName + "/rpc").c_str()) )
{
//printf("Error: unable to connect to device port %s\n", ("/" + rpcServerName + "/head:i").c_str());
msg = "Error: unable to connect to device port /" + rpcServerName + "/rpc";
reply.addString(msg.c_str());
return reply;
}
}
double t[3] = {0.0, 0.0, 0.0};
// default initialization
if (angles.size() == 1)
{
for (int i=0; i<3; i++) angles.addDouble(t[i]);
}
// else user defined initialization
updateSem.wait();
rpcPort.write(angles,reply);
// update PMP joint angles
if (reply.get(0).asString() == "Done")
{
PMP->q_rightArm(0) = PMP->q_leftArm(0) = angles.get(0).asDouble()*M_PI/180;
PMP->q_rightArm(1) = PMP->q_leftArm(1) = angles.get(1).asDouble()*M_PI/180;
PMP->q_rightArm(2) = PMP->q_leftArm(2) = angles.get(2).asDouble()*M_PI/180;
PMP->Rchain->setAng(PMP->q_rightArm);
PMP->Lchain->setAng(PMP->q_leftArm);
PMP->x_0R = PMP->get_EEPose("right");
PMP->x_0L = PMP->get_EEPose("left");
//update VTGS starting position
VTGS_r->setStartingPoint(PMP->x_0R);
VTGS_l->setStartingPoint(PMP->x_0L);
}
updateSem.post();
return reply;
}
void vtWThread::sendGuiTarget()
{
if (outPortGui.getOutputCount()>0)
{
Bottle obj;
obj.addString("object");
obj.addString("Target");
// size
obj.addDouble(50.0);
obj.addDouble(50.0);
obj.addDouble(50.0);
// positions
obj.addDouble(1000.0*events[0].Pos[0]);
obj.addDouble(1000.0*events[0].Pos[1]);
obj.addDouble(1000.0*events[0].Pos[2]);
// orientation
obj.addDouble(0.0);
obj.addDouble(0.0);
obj.addDouble(0.0);
// color
obj.addInt(255);
obj.addInt(125);
obj.addInt(125);
// transparency
obj.addDouble(0.9);
outPortGui.write(obj);
}
}
void GuiUpdaterModule::addObject(Object* o, const string &opcTag)
{
//Get the position of the object in the current reference frame of the robot (not the initial one)
Vector inCurrentRootReference = iCub->getSelfRelativePosition(o->m_ego_position);
//cout<<o->name()<<" init Root: \t \t"<<o->m_ego_position.toString(3,3)<<endl
// <<o->name()<<" current Root: \t \t"<<inCurrentRootReference.toString(3,3)<<endl;
Bottle cmd;
cmd.addString("object");
cmd.addString(opcTag.c_str());
cmd.addDouble(o->m_dimensions[0] *1000.0); // dimX in [mm]
cmd.addDouble(o->m_dimensions[1] *1000.0); // dimY in [mm]
cmd.addDouble(o->m_dimensions[2] *1000.0); // dimZ in [mm]
cmd.addDouble(inCurrentRootReference[0] *1000.0); // posX in [mm]
cmd.addDouble(inCurrentRootReference[1] *1000.0); // posY in [mm]
cmd.addDouble(inCurrentRootReference[2] *1000.0); // posZ in [mm]
cmd.addDouble(o->m_ego_orientation[0] - iCub->m_ego_orientation[0]); // Deal with the object orientation that is moving with the base
cmd.addDouble(o->m_ego_orientation[1] - iCub->m_ego_orientation[1]); // "
cmd.addDouble(o->m_ego_orientation[2] - iCub->m_ego_orientation[2]); // "
cmd.addInt((int)o->m_color[0]); // color R
cmd.addInt((int)o->m_color[1]); // color G
cmd.addInt((int)o->m_color[2]); // color B
cmd.addDouble(1); // alpha coefficient [0,1]
toGui.write(cmd);
}
Bottle PMPthread::Vector2Bottle(Vector v)
{
Bottle bot;
for (unsigned int i=0; i<v.size(); i++)
{
bot.addDouble(v(i));
}
return bot;
}
bool AvrgForceEstimator::estimateContactCondition(Bottle &tactileData, Bottle &contactConditionEstimate){
contactConditionEstimate.clear();
if (tactileData.size() == 0){
contactConditionEstimate.addDouble(0);
return false;
}
double avrg = 0;
for(int i =0; i < tactileData.size(); i++){
avrg += tactileData.get(i).asDouble();
}
avrg = avrg/tactileData.size();
contactConditionEstimate.addDouble(avrg);
return true;
}
void utManagerThread::sendData()
{
Bottle b;
b.clear();
for (size_t i = 0; i < 3; i++)
{
b.addDouble(kalOut(i));
}
outPortEvents.write(b);
}
Bottle SimModel::makeObjectBottle(vector<int>& ind, bool collision) {
Bottle cmd;
cmd.addString("world");
cmd.addString("mk");
cmd.addString("model");
cmd.addString(mesh.c_str());
cmd.addString(texture.c_str());
cmd.addDouble(positionX);
cmd.addDouble(positionY);
cmd.addDouble(positionZ);
if (collision == false) {
cmd.addString("false");
}
ind[MODEL]++;
objSubIndex=ind[MODEL];
return cmd;
}
Bottle Controller::listMotionOngoingEvents()
{
Bottle events;
mutexData.lock();
for (multiset<double>::iterator itr=motionOngoingEvents.begin(); itr!=motionOngoingEvents.end(); itr++)
events.addDouble(*itr);
mutexData.unlock();
return events;
}
void SegmentationPoint::segment(Bottle &b)
{
if (getOutputCount()>0)
{
//send 2D x y coordinates to segmentator
Bottle request;
request.addDouble(b.get(0).asDouble());
request.addDouble(b.get(1).asDouble());
write(request);
}
}
Bottle SimSBox::makeObjectBottle(vector<int>& ind, bool collision) {
Bottle cmd;
cmd.addString("world");
cmd.addString("mk");
cmd.addString("sbox");
cmd.addDouble(sizeX);
cmd.addDouble(sizeY);
cmd.addDouble(sizeZ);
cmd.addDouble(positionX);
cmd.addDouble(positionY);
cmd.addDouble(positionZ);
cmd.addDouble(colorR);
cmd.addDouble(colorG);
cmd.addDouble(colorB);
if (collision == false) {
cmd.addString("false");
cout << "Collision with Static box set to False" << endl;
}
ind[SBOX]++;
objSubIndex=ind[SBOX];
return cmd;
}
void display_cog(string text,yarp::sig::Vector v, int r, int g, int b)
{
Bottle obj;
obj.clear();
obj.addString("object"); // command to add/update an object
obj.addString(text.c_str());
// object dimensions in millimiters
// (it will be displayed as an ellipsoid with the tag "my_object_name")
bool fixed_size = false;
if (fixed_size)
{
obj.addDouble(20);
obj.addDouble(20);
obj.addDouble(20);
}
else
{
obj.addDouble(pow(v[3],1.0/3.0)*20.0);
obj.addDouble(pow(v[3],1.0/3.0)*20.0);
obj.addDouble(pow(v[3],1.0/3.0)*20.0);
}
// object position in millimiters
// reference frame: X=fwd, Y=left, Z=up
obj.addDouble(v[0]*1000);
obj.addDouble(v[1]*1000);
obj.addDouble(v[2]*1000);
// object orientation (roll, pitch, yaw) in degrees
obj.addDouble(0);
obj.addDouble(0);
obj.addDouble(0);
// object color (0-255)
obj.addInt(r);
obj.addInt(g);
obj.addInt(b);
// transparency (0.0=invisible 1.0=solid)
obj.addDouble(0.9);
port_out.prepare() = obj;
port_out.write();
Time::delay(0.1);
}
void Controller::stopLimb(const bool execStopPosition)
{
if (commData->neckPosCtrlOn)
{
if (execStopPosition)
posHead->stop(neckJoints.size(),neckJoints.getFirst());
// note: vel==0.0 is always achievable
velHead->velocityMove(eyesJoints.size(),eyesJoints.getFirst(),
Vector(eyesJoints.size(),0.0).data());
}
else
velHead->velocityMove(Vector(nJointsHead,0.0).data());
if (commData->debugInfoEnabled && (port_debug.getOutputCount()>0))
{
Bottle info;
int j=0;
if (commData->neckPosCtrlOn)
{
if (execStopPosition)
{
for (size_t i=0; i<neckJoints.size(); i++)
{
ostringstream ss;
ss<<"pos_"<<neckJoints[i];
info.addString(ss.str().c_str());
info.addString("stop");
}
}
j=eyesJoints[0];
}
for (int i=j; i<nJointsHead; i++)
{
ostringstream ss;
ss<<"vel_"<<i;
info.addString(ss.str().c_str());
info.addDouble(0.0);
}
port_debug.prepare()=info;
txInfo_debug.update(q_stamp);
port_debug.setEnvelope(txInfo_debug);
port_debug.writeStrict();
}
commData->ctrlActive=false;
motionDone=true;
}
Bottle SimSph::makeObjectBottle(vector<int>& ind, bool collision) {
Bottle cmd;
cmd.addString("world");
cmd.addString("mk");
cmd.addString("sph");
cmd.addDouble(radius);
cmd.addDouble(positionX);
cmd.addDouble(positionY);
cmd.addDouble(positionZ);
cmd.addDouble(colorR);
cmd.addDouble(colorG);
cmd.addDouble(colorB);
if (collision == false) {
cmd.addString("false");
}
ind[SPH]++;
objSubIndex=ind[SPH];
return cmd;
}
void SimoxHandEyeCalibrationWindow::segThresholdChanged( int i )
{
float t = (float)UI->horizontalSliderSegThreshold->value() / (float)UI->horizontalSliderSegThreshold->maximum();
if (t<0)
t = 0;
if (t>1.0f)
t = 1.0f;
t = t*255.0f;
Bottle cmd;
cmd.addString("set");
cmd.addString("threshold");
cmd.addDouble((double)t);
sendToHandTracker(cmd);
updateSegmentationInfo();
}
void GuiUpdaterModule::addDrives(Agent* a)
{
Vector driveDimension(3);
driveDimension[0] = 10;
driveDimension[1] = 500;
driveDimension[2] = 10;
//Create drives
Vector overHeadPos = iCub->getSelfRelativePosition(a->m_ego_position);
int driveCount = 0;
for(map<string,Drive>::iterator drive = a->m_drives.begin(); drive != a->m_drives.end(); drive++)
{
double ySize = driveDimension[1] * drive->second.value + 10;
ostringstream opcTag;
opcTag<<a->name()<<"_"<<drive->second.name;
Bottle cmd;
cmd.addString("object");
cmd.addString(opcTag.str().c_str());
cmd.addDouble(driveDimension[0]);
cmd.addDouble(ySize);
cmd.addDouble(driveDimension[2]);
cmd.addDouble(overHeadPos[0] *1000.0);
cmd.addDouble(overHeadPos[1] *1000.0 - ySize / 2.0 + driveDimension[1] / 2.0);
cmd.addDouble(overHeadPos[2] *1000.0 + 500 + driveCount * (driveDimension[2]+30) );
cmd.addDouble(a->m_ego_orientation[0]);
cmd.addDouble(a->m_ego_orientation[1]);
cmd.addDouble(a->m_ego_orientation[2]);
Vector color = getDriveColor(drive->second);
cmd.addInt((int)color[0]); // color R
cmd.addInt((int)color[1]); // color G
cmd.addInt((int)color[2]); // color B
cmd.addDouble(1); // alpha coefficient [0,1]
toGui.write(cmd);
driveCount++;
}
}
bool VisuoThread::getRaw(Bottle &bStereo)
{
Vector stereo;
bool ok=false;
//empty the buffers
Bottle *bL=rawInPort[LEFT].read(false);
Bottle *bR=rawInPort[RIGHT].read(false);
double t=Time::now();
while(stereo.size()!=4 && Time::now()-t<rawWaitThresh && !interrupted)
{
Bottle *bL=rawInPort[LEFT].read(false);
Bottle *bR=rawInPort[RIGHT].read(false);
if(bL!=NULL || bR!=NULL)
stereo.resize(4,0.0);
if(bL!=NULL)
{
stereo[0]=bL->get(0).asInt();
stereo[1]=bL->get(1).asInt();
}
if(bR!=NULL)
{
stereo[2]=bR->get(0).asInt();
stereo[3]=bR->get(1).asInt();
}
}
if(stereo.size()==4)
{
int side=40;
startTracker(stereo,side);
ok=true;
}
for(size_t i=0; i<stereo.size(); i++)
bStereo.addDouble(stereo[i]);
return ok;
}
bool CRMainEstimation::respond(const Bottle& command, Bottle& reply) {
reply.clear();
if (command.get(0).asString()=="getPoses") {
reply.addString("[ack]");
reply.addInt(g_means_last.size());
for(unsigned int i=0; i<g_means_last.size(); i++) {
for(unsigned int j=0; j<POSE_SIZE; j++) { // 6DOF
reply.addDouble(g_means_last[i][j]);
}
}
}
else {
reply.addString("[nack]");
}
return true;
}
bool configure(ResourceFinder &rf)
{
moduleName = rf.find("moduleName").asString().c_str();
string inputPortName = rf.find("inputPortName").asString().c_str();
string outputPortName = rf.find("outputPortName").asString().c_str();
primitives = rf.findGroup("primitives");
grasp = new Grasper(("/"+moduleName+"/"+outputPortName));
if(!rpcPort.open(("/"+moduleName+"/"+inputPortName).c_str()))
{
cout << "Error: unable to open rpc port, closing module" << endl;
return false;
}
// set grasp speeds
Bottle *speeds =NULL;
if (!rf.check("speeds") || (rf.find("speeds").asList())->size()!= 9)
{
for (int i=0; i<9; i++)
speeds->addDouble(0.0);
}
else
speeds = rf.find("speeds").asList();
grasp->setGraspSpeeds(*speeds);
Bottle *offset=NULL;
// set tightening offset
if (!rf.check("offset") || (rf.find("offset").asList())->size()!= 9)
{
for (int i=0; i<9; i++)
offset->addDouble(0.0);
}
else
offset = rf.find("offset").asList();
grasp->setOffset(*offset);
graspSucceeded = false;
cout << "--> module successfully configured" << endl;
bool done = grasp->StartPorts();
if(done)
attach(rpcPort);
return done;
}
