/*
* Dump in the port DumperPort the human skeleton, and the object of the OPC: sObjectToDump
*
*/
void humanRobotDump::DumpHumanObject()
{
Agent* ag = iCub->opc->addOrRetrieveEntity<Agent>(sAgentName);
Bottle bDump;
bDump.addString(sActionName);
bDump.addString(sObjectToDump);
bDump.addList() = ag->m_body.asBottle();
iCub->opc->checkout();
list<Entity*> lEntity = iCub->opc->EntitiesCacheCopy();
for (list<Entity*>::iterator itEnt = lEntity.begin(); itEnt != lEntity.end(); itEnt++)
{
if ((*itEnt)->entity_type() == EFAA_OPC_ENTITY_AGENT ||
(*itEnt)->entity_type() == EFAA_OPC_ENTITY_OBJECT ||
(*itEnt)->entity_type() == EFAA_OPC_ENTITY_RTOBJECT)
{
if ((*itEnt)->name() != "icub")
{
Object* ob = iCub->opc->addOrRetrieveEntity<Object>((*itEnt)->name());
Bottle bObject;
bObject.addString(ob->name());
bObject.addDouble(ob->m_ego_position[0]);
bObject.addDouble(ob->m_ego_position[1]);
bObject.addDouble(ob->m_ego_position[2]);
bObject.addInt(ob->m_present);
bDump.addList() = bObject;
}
}
}
bDump.addInt(m_iterator);
DumperPort.write(bDump);
}
/*
* Message handler. Just echo all received messages.
*/
bool respond(const Bottle& command, Bottle& reply)
{
cout<<"Got "<<command.toString().c_str()<<endl;
if(command.size() < 3) {
reply.addString("Command error! example: 'sum 3 4'");
return true;
}
int a = command.get(1).asInt();
int b = command.get(2).asInt();
if( command.get(0).asString() == "sum") {
int c = sum(a, b);
reply.addInt(c);
return true;
}
if( command.get(0).asString() == "sub") {
int c = sub(a, b);
reply.addInt(c);
return true;
}
reply.addString("Unknown command");
reply.addString(command.get(0).asString().c_str());
return true;
}
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 NameServer::botify(const Contact& address) {
Bottle result;
if (address.isValid()) {
Bottle bname;
bname.addString("name");
bname.addString(address.getRegName().c_str());
Bottle bip;
bip.addString("ip");
bip.addString(address.getHost().c_str());
Bottle bnum;
bnum.addString("port_number");
bnum.addInt(address.getPort());
Bottle bcarrier;
bcarrier.addString("carrier");
bcarrier.addString(address.getCarrier().c_str());
result.addString("port");
result.addList() = bname;
result.addList() = bip;
result.addList() = bnum;
result.addList() = bcarrier;
} else {
Bottle bstate;
bstate.addString("error");
bstate.addInt(-2);
bstate.addString("port not known");
result.addString("port");
result.addList() = bstate;
}
return result;
}
bool SubscriberOnSql::listSubscriptions(const ConstString& port,
yarp::os::Bottle& reply) {
mutex.wait();
sqlite3_stmt *statement = NULL;
char *query = NULL;
if (ConstString(port)!="") {
query = sqlite3_mprintf("SELECT s.srcFull, s.DestFull, EXISTS(SELECT topic FROM topics WHERE topic = s.src), EXISTS(SELECT topic FROM topics WHERE topic = s.dest), s.mode FROM subscriptions s WHERE s.src = %Q OR s.dest= %Q ORDER BY s.src, s.dest",port.c_str(),port.c_str());
} else {
query = sqlite3_mprintf("SELECT s.srcFull, s.destFull, EXISTS(SELECT topic FROM topics WHERE topic = s.src), EXISTS(SELECT topic FROM topics WHERE topic = s.dest), s.mode FROM subscriptions s ORDER BY s.src, s.dest");
}
if (verbose) {
printf("Query: %s\n", query);
}
int result = sqlite3_prepare_v2(SQLDB(implementation),query,-1,&statement,
NULL);
if (result!=SQLITE_OK) {
const char *msg = sqlite3_errmsg(SQLDB(implementation));
if (msg!=NULL) {
fprintf(stderr,"Error: %s\n", msg);
}
}
reply.addString("subscriptions");
while (result == SQLITE_OK && sqlite3_step(statement) == SQLITE_ROW) {
char *src = (char *)sqlite3_column_text(statement,0);
char *dest = (char *)sqlite3_column_text(statement,1);
int srcTopic = sqlite3_column_int(statement,2);
int destTopic = sqlite3_column_int(statement,3);
char *mode = (char *)sqlite3_column_text(statement,4);
Bottle& b = reply.addList();
b.addString("subscription");
Bottle bsrc;
bsrc.addString("src");
bsrc.addString(src);
Bottle bdest;
bdest.addString("dest");
bdest.addString(dest);
b.addList() = bsrc;
b.addList() = bdest;
if (mode!=NULL) {
if (mode[0]!='\0') {
Bottle bmode;
bmode.addString("mode");
bmode.addString(mode);
b.addList() = bmode;
}
}
if (srcTopic||destTopic) {
Bottle btopic;
btopic.addString("topic");
btopic.addInt(srcTopic);
btopic.addInt(destTopic);
b.addList() = btopic;
}
}
sqlite3_finalize(statement);
sqlite3_free(query);
mutex.post();
return true;
}
void CB::YARPConfigurationVariables::setVelocityControlMode(bool mode, string portName) {
bool ok = true;
// specify the local port names that connect to the velocityControl module
string velocityOutputPortName = "/cb/configuration" + deviceName + "/vel:o";
string velocityRPCOutputPortName = "/cb/configuration" + deviceName + "/vel/rpc:o";
// velocityPortName = portName + "/command";
velocityPortName = portName + "/fastCommand";
velocityRPCPortName = portName + "/input";
Bottle b;
if(mode && !velocityControlMode) {
// if we're turning on the velocityControlMode (and it wasnt previously on)
// open and connect the data and config portsport
ok &= velocityPort.open(velocityOutputPortName.c_str());
ok &= Network::connect(velocityOutputPortName.c_str(),velocityPortName.c_str(),"tcp");
Time::delay(0.5);
ok &= velocityRPCPort.open(velocityRPCOutputPortName.c_str());
ok &= Network::connect(velocityRPCOutputPortName.c_str(),velocityRPCPortName.c_str(),"tcp");
// send gain and maxVel paramaters to the vc module
for(int i=0; i<mask.size(); i++) {
if(mask[i]) {
cout << "setting vc params joint[" << i << "]: gain=" << velocityGain << ", svel=" << maxSetVal << endl;
b.clear();
b.addString("gain");
b.addInt(i);
b.addDouble(velocityGain);
velocityRPCPort.write(b);
Time::delay(0.1);
b.clear();
b.addString("svel");
b.addInt(i);
b.addDouble(maxSetVal);
velocityRPCPort.write(b);
Time::delay(0.1);
}
}
} else if(!mode && velocityControlMode) {
// turning off velocity control mode by disconnecting and closing ports
ok &= Network::disconnect(velocityOutputPortName.c_str(),velocityPortName.c_str());
ok &= Network::disconnect(velocityRPCOutputPortName.c_str(),velocityRPCPortName.c_str());
velocityRPCPort.close();
velocityPort.close();
}
// set the current mode
velocityControlMode = mode;
}
bool utManagerThread::getPointFromStereo()
{
Bottle cmdSFM;
Bottle respSFM;
cmdSFM.clear();
respSFM.clear();
cmdSFM.addString("Root");
cmdSFM.addInt(int(templatePFTrackerPos(0)));
cmdSFM.addInt(int(templatePFTrackerPos(1)));
SFMrpcPort.write(cmdSFM, respSFM);
// Read the 3D coords and compute the distance to the set reference frame origin
if (respSFM.size() == 3)
{
Vector SFMtmp(3,0.0);
SFMtmp(0) = respSFM.get(0).asDouble(); // Get the X coordinate
SFMtmp(1) = respSFM.get(1).asDouble(); // Get the Y coordinate
SFMtmp(2) = respSFM.get(2).asDouble(); // Get the Z coordinate
if (SFMtmp(0) == 0.0 && SFMtmp(1) == 0.0 && SFMtmp(2) == 0.0)
{
return false;
}
SFMPos = SFMtmp;
return true;
}
return false;
}
bool updateModule()
{
if (imgOutPort.getOutputCount()>0)
{
if (ImageOf<PixelBgr> *pImgBgrIn=imgInPort.read(false))
{
Vector xa,oa;
iarm->getPose(xa,oa);
Matrix Ha=axis2dcm(oa);
Ha.setSubcol(xa,0,3);
Vector v(4,0.0); v[3]=1.0;
Vector c=Ha*v;
v=0.0; v[0]=0.05; v[3]=1.0;
Vector x=Ha*v;
v=0.0; v[1]=0.05; v[3]=1.0;
Vector y=Ha*v;
v=0.0; v[2]=0.05; v[3]=1.0;
Vector z=Ha*v;
v=solution; v.push_back(1.0);
Vector t=Ha*v;
Vector pc,px,py,pz,pt;
int camSel=(eye=="left")?0:1;
igaze->get2DPixel(camSel,c,pc);
igaze->get2DPixel(camSel,x,px);
igaze->get2DPixel(camSel,y,py);
igaze->get2DPixel(camSel,z,pz);
igaze->get2DPixel(camSel,t,pt);
CvPoint point_c=cvPoint((int)pc[0],(int)pc[1]);
CvPoint point_x=cvPoint((int)px[0],(int)px[1]);
CvPoint point_y=cvPoint((int)py[0],(int)py[1]);
CvPoint point_z=cvPoint((int)pz[0],(int)pz[1]);
CvPoint point_t=cvPoint((int)pt[0],(int)pt[1]);
cvCircle(pImgBgrIn->getIplImage(),point_c,4,cvScalar(0,255,0),4);
cvCircle(pImgBgrIn->getIplImage(),point_t,4,cvScalar(255,0,0),4);
cvLine(pImgBgrIn->getIplImage(),point_c,point_x,cvScalar(0,0,255),2);
cvLine(pImgBgrIn->getIplImage(),point_c,point_y,cvScalar(0,255,0),2);
cvLine(pImgBgrIn->getIplImage(),point_c,point_z,cvScalar(255,0,0),2);
cvLine(pImgBgrIn->getIplImage(),point_c,point_t,cvScalar(255,255,255),2);
tip.clear();
tip.addInt(point_t.x);
tip.addInt(point_t.y);
imgOutPort.prepare()=*pImgBgrIn;
imgOutPort.write();
}
}
return true;
}
void testUnbufferedSubscriber() {
report(0,"Unbuffereded Subscriber test");
Node n("/node");
BufferedPort<Bottle> pout;
pout.setWriteOnly();
pout.open("very_interesting_topic");
{
Node n2("/node2");
Subscriber<Bottle> pin("/very_interesting_topic");
waitForOutput(pout,10);
Bottle& b = pout.prepare();
b.clear();
b.addInt(42);
pout.write();
Bottle bin;
bin.addInt(99);
pin.read(bin);
pout.waitForWrite();
checkEqual(bin.get(0).asInt(),42,"message is correct");
}
}
bool Sound::write(ConnectionWriter& connection) {
// lousy format - fix soon!
FlexImage& img = HELPER(implementation);
Bottle bot;
bot.addInt(frequency);
return PortablePair<FlexImage,Bottle>::writePair(connection,img,bot);
}
void testUnbufferedPublisher() {
report(0,"Unbuffered Publisher test");
Node n("/node");
Publisher<Bottle> p("/very_interesting_topic");
{
Node n2("/node2");
BufferedPort<Bottle> pin;
pin.setReadOnly();
pin.setStrict();
pin.open("very_interesting_topic");
waitForOutput(p,10);
Bottle b;
b.addInt(42);
p.write(b);
Bottle *bin = pin.read();
checkTrue(bin!=NULL,"message arrived");
if (!bin) return;
checkEqual(bin->get(0).asInt(),42,"message is correct");
}
}
void getObjectList(Bottle &reply)
{
double t0=Time::now();
int max_size=0;
vector<Blob> max_blobs;
while(Time::now()-t0<0.5)
{
mutex.wait();
reply.clear();
int size=0;
for(unsigned int i=0; i<blobs.size(); i++)
if(blobs[i].getBest()!="")
size++;
if(max_size<size)
{
max_size=size;
max_blobs=blobs;
}
mutex.post();
Time::delay(0.05);
}
reply.addInt(max_size);
for(unsigned int i=0; i<max_blobs.size(); i++)
if(max_blobs[i].getBest()!="")
reply.addString(max_blobs[i].getBest().c_str());
}
bool skinCalibrationClient::quitModule(){
Bottle b;
b.addInt(quit);
if(!skinCalibrationRpcPort.write(b))
return false;
return true;
}
bool skinCalibrationClient::startCalibration(){
Bottle b;
b.addInt(start_calibration);
if(!skinCalibrationRpcPort.write(b))
return false;
return true;
}
bool updateModule()
{
double verg = 0;
Bottle bin;
Bottle bout;
inport.read(bin);
if (reset_offset)
{
initial_offset[0] = bin.get(0).asDouble();
initial_offset[1] = bin.get(1).asDouble();
initial_offset[2] = bin.get(2).asDouble();
initial_offset[3] = bin.get(3).asDouble();
initial_offset[4] = bin.get(4).asDouble();
initial_offset[5] = bin.get(5).asDouble();
reset_offset = false;
}
double x_torque = fabs(bin.get(3).asDouble()) - initial_offset [3];
double y_force = fabs(bin.get(1).asDouble()) - initial_offset[1];
bout.addInt(2);
double j_torque = x_torque + y_force * 0.70; //Nm + N * m
bout.addDouble(j_torque); //j0
bout.addDouble(j_torque); //j1
bout.addDouble(j_torque); //j2
bout.addDouble(j_torque); //j3
bout.addDouble(j_torque); //j4
bout.addDouble(j_torque); //j5
outport.write(bout);
return true;
}
void testModify() {
report(0,"test bottle modification...");
Bottle b;
b.addInt(3);
b.get(0) = 5;
b.hasChanged();
checkEqual(b.get(0).asInt(),5,"assignment works");
}
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);
}
Bottle TRACKERManager::getPausedIDs()
{
Bottle listID;
for (int i = 0; i < pausedThreads.size(); i++ )
listID.addInt(pausedThreads[i]);
return listID;
}
void matrixOfIntIntoBottle(const yarp::sig::Matrix m, Bottle &b)
{
Vector v = toVector(m);
for (unsigned int i = 0; i < v.size(); i++)
{
b.addInt(int(v[i]));
}
}
//*************************************************************************************************************************
bool ParamHelperBase::checkParamConstraints(int id, const Bottle &v, Bottle &reply)
{
if(!hasParam(id))
{
reply.addString("There is no parameter with the specified id: ");
reply.addInt(id);
return false;
}
return paramList[id]->checkConstraints(v, &reply);
}
Bottle TRACKERManager::getIDs()
{
Bottle listID;
std::map< unsigned int, ParticleThread* >::iterator itr;
for (itr = workerThreads.begin(); itr!=workerThreads.end(); itr++)
listID.addInt(itr->first);
return listID;
}
virtual bool read(ConnectionReader& connection) {
Bottle receive;
receive.read(connection);
receive.addInt(5);
ConnectionWriter *writer = connection.getWriter();
if (writer!=NULL) {
receive.write(*writer);
}
return true;
}
void vtWThread::sendGuiEvents()
{
if (outPortGui.getOutputCount()>0)
{
int counter = 1;
Bottle obj;
for (std::vector<IncomingEvent>::const_iterator it = events.begin() ; it != events.end(); ++it){
stringstream ss;
obj.clear();
obj.addString("object");
ss << "obstacle" << counter;
obj.addString(ss.str());
// size
obj.addDouble(1000.0* (*it).Radius);
obj.addDouble(1000.0* (*it).Radius);
obj.addDouble(1000.0* (*it).Radius);
// positions
obj.addDouble(1000.0 * (*it).Pos[0]);
obj.addDouble(1000.0 * (*it).Pos[1]);
obj.addDouble(1000.0 * (*it).Pos[2]);
// orientation
obj.addDouble(0.0);
obj.addDouble(0.0);
obj.addDouble(0.0);
// color
obj.addInt(50 + (*it).Threat*200.0); //threatening objects will be more red
obj.addInt(50);
obj.addInt(50);
// transparency
obj.addDouble(0.9);
outPortGui.write(obj);
counter++;
}
}
}
Bottle SimBox::releaseObjectBottle(iCubArm arm) {
Bottle cmd;
cmd.addString("world");
cmd.addString("grab");
cmd.addString("box");
cmd.addInt (objSubIndex);
switch(arm) {
case RIGHT:
cmd.addString("right");
break;
case LEFT:
cmd.addString("left");
break;
default:
cmd.addString("right");
}
cmd.addInt(0);
return cmd;
}
Bottle SimModel::grabObjectBottle(iCubArm arm) {
Bottle cmd;
cmd.addString("world");
cmd.addString("grab");
cmd.addString("model");
cmd.addInt (objSubIndex);
switch(arm) {
case RIGHT:
cmd.addString("right");
break;
case LEFT:
cmd.addString("left");
break;
default:
cmd.addString("right");
}
cmd.addInt(1);
return cmd;
}
Bottle SimBox::moveObjectBottle() {
Bottle cmd;
cmd.addString("world");
cmd.addString("set");
cmd.addString("box");
cmd.addInt (objSubIndex);
cmd.addDouble(positionX);
cmd.addDouble(positionY);
cmd.addDouble(positionZ);
return cmd;
}
Bottle SimBox::rotateObjectBottle() {
Bottle cmd;
cmd.addString("world");
cmd.addString("rot");
cmd.addString("box");
cmd.addInt (objSubIndex);
cmd.addDouble(rotationX);
cmd.addDouble(rotationY);
cmd.addDouble(rotationZ);
return cmd;
}
int main(int argc, char *argv[]) {
Network yarp;
Port output;
string from_port;
string to_port;
if (argc >=3) {
from_port = string(argv[1]);
to_port = string(argv[2]);
}
else {
from_port = "/sender";
to_port = "/timer";
}
output.open(from_port.c_str());
yarp::os::Network::connect(from_port.c_str(),to_port.c_str());
int top = 100000;
Random::seed(std::clock());
for (int i=1; i<=top; i++) {
// prepare a message
Bottle bot;
bot.addString("testing");
bot.addInt(i); //id
bot.addInt(i); //dev type
bot.addInt(i); //server type
bot.addInt(i); //time stamp
for (int j=0; j<15; j++) { //15 sensor values
bot.addDouble(Random::uniform());
}
// send the message
output.write(bot);
//printf("Sent message: %s\n", bot.toString().c_str());
// wait a while
//Time::delay(1);
}
output.close();
return 0;
}
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++;
}
}
