int main(int argc, char *argv[])
{
Network yarp;
int c=0;
BufferedPort<ImageOf<PixelRgb> > right;
BufferedPort<ImageOf<PixelRgb> > left;
BufferedPort<ImageOf<PixelRgb> > out;
right.open("/teststereomatch/right");
left.open("/teststereomatch/left");
out.open("/teststereomatch/o");
while(true)
{
ImageOf< PixelRgb> *imgR=right.read(true);
ImageOf< PixelRgb> *imgL=left.read(true);
ImageOf< PixelRgb> &outImg=out.prepare();
if (imgR!=0 && imgL!=0)
{
merge(*imgR, *imgL, outImg);
out.write();
c++;
}
printFrame(outImg.height(), outImg.width(), c);
}
return 0;
}
virtual void testStrictWriter() {
report(0,"check strict writer...");
BufferedPort<Bottle> in;
BufferedPort<Bottle> out;
in.setStrict();
in.open("/in");
out.open("/out");
Network::connect("/out","/in");
Bottle& outBot1 = out.prepare();
outBot1.fromString("hello world");
printf("Writing bottle 1: %s\n", outBot1.toString().c_str());
out.write(true);
Bottle& outBot2 = out.prepare();
outBot2.fromString("2 3 5 7 11");
printf("Writing bottle 2: %s\n", outBot2.toString().c_str());
out.write(true);
Bottle *inBot1 = in.read();
checkTrue(inBot1!=NULL,"got 1 of 2 items");
if (inBot1!=NULL) {
printf("Bottle 1 is: %s\n", inBot1->toString().c_str());
checkEqual(inBot1->size(),2,"match for item 1");
}
Bottle *inBot2 = in.read();
checkTrue(inBot2!=NULL,"got 2 of 2 items");
if (inBot2!=NULL) {
printf("Bottle 2 is: %s\n", inBot2->toString().c_str());
checkEqual(inBot2->size(),5,"match for item 1");
}
}
bool updateModule()
{
// get fresh images
ImageOf<PixelRgb> *imgL=imgLPortIn.read();
ImageOf<PixelRgb> *imgR=imgRPortIn.read();
// interrupt sequence detected
if ((imgL==NULL) || (imgR==NULL))
return false;
// compute the center-of-mass of pixels of our color
mutex.lock();
okL=getCOG(*imgL,cogL);
okR=getCOG(*imgR,cogR);
mutex.unlock();
PixelRgb color;
color.r=255; color.g=0; color.b=0;
if (okL)
draw::addCircle(*imgL,color,(int)cogL[0],(int)cogL[1],5);
if (okR)
draw::addCircle(*imgR,color,(int)cogR[0],(int)cogR[1],5);
imgLPortOut.write(*imgL);
imgRPortOut.write(*imgR);
return true;
}
virtual void run()
{
// get inputs
ImageOf<PixelFloat> *pSegIn=portImgIn->read(false);
//if there is a seg image, use that
if (pSegIn)
{
ImageOf<PixelRgb> *pImgIn=portRefIn->read(false);
ImageOf<PixelRgb> &imgOut= portImgOut->prepare();
Mat * M = new Mat(pSegIn->height(), pSegIn->width(), CV_32F, (void *)pSegIn->getRawImage());
Mat X;
vector<vector<Point> > contours;
vector<Vec4i> hierarchy;
M->convertTo(X,CV_8UC1);
findContours(X, contours, hierarchy, CV_RETR_LIST, CV_CHAIN_APPROX_NONE);
//pick a random segmented object
int iBlob = -1;
if (contours.size() > 0) {
iBlob = rand() % contours.size();
}
if (pImgIn) {
imgOut.copy(*pImgIn);
} else {
imgOut.copy(*pSegIn);
}
//write the contour pixels of the object to port
Matrix &object = portObjOut->prepare();
yarp::sig::Vector xs, ys;
vector<Point> cts = contours[iBlob];
for (int i = 0; i < cts.size(); i++) {
xs.push_back(cts.at(i).y);
ys.push_back(cts.at(i).x);
imgOut.pixel(cts.at(i).x,cts.at(i).y) = PixelRgb(255, 0, 0);
}
object.resize(xs.size(),2);
object.setCol(0,xs);
object.setCol(1,ys);
portObjOut->write();
portImgOut->write();
delete M;
}
}
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 Repo::mygetch() {
// can substitute next block for a timer:
Time::delay(0.1); // seconds
///////////// [begin] The getch() implementation ///////////
struct termios oldt, newt;
int ch;
tcgetattr( STDIN_FILENO, &oldt );
newt = oldt;
newt.c_lflag &= ~( ICANON | ECHO );
tcsetattr( STDIN_FILENO, TCSANOW, &newt );
ch = getchar();
tcsetattr( STDIN_FILENO, TCSANOW, &oldt );
///////////// [end] The getch() implementation ///////////
Vector* qRead = port_q.read(false);
if (qRead) {
q = *qRead; // joint position values
} else {
printf("Get joint values failed!\n");
return -1;
}
Vector* f_eeRead = port_f.read(false);
if (f_eeRead) {
f_ee = *f_eeRead; // EE force in end effector frame
} else {
printf("Get external contacts failed!\n");
return -1;
}
d = q.subVector(0,4);
for (int k=0;k<6;k++)
d.push_back(f_ee[k]);
printf("Sample %d: ",counter+1); // Get on screen
for (int k=0;k<11;k++) {
// fprintf(stderr,"%f ",d[k]); // Get on screen through stderr
printf("%f ",d[k]); // Sent to stdout to pass to file using operator >
logger << d[k] << " "; // Sent to logger file
}
// fprintf(stderr,"\n"); // Get on screen through stderr
logger << std::endl; // Sent to logger file
printf("now I grabbed data\n");
counter++;
// return ch;
return 0;
}
virtual void testAcquire() {
report(0, "checking acquire/release...");
BufferedPort<Bottle> in;
BufferedPort<Bottle> out;
in.setStrict();
out.setStrict();
in.open("/in");
out.open("/out");
Network::connect("/out","/in");
out.prepare().fromString("1");
out.write(true);
Bottle *bot = in.read();
checkTrue(bot!=NULL,"Inserted message received");
if (bot!=NULL) {
checkEqual(bot->size(),1,"right length");
}
out.prepare().fromString("1 2");
out.write(true);
void *key = in.acquire();
Bottle *bot2 = in.read();
checkTrue(bot2!=NULL,"Inserted message received");
if (bot2!=NULL) {
checkEqual(bot2->size(),2,"right length");
}
out.prepare().fromString("1 2 3");
out.write(true);
void *key2 = in.acquire();
Bottle *bot3 = in.read();
checkTrue(bot3!=NULL,"Inserted message received");
if (bot3!=NULL) {
checkEqual(bot3->size(),3,"right length");
}
if (bot2!=NULL) {
checkEqual(bot2->size(),2,"original (2) still ok");
}
if (bot!=NULL) {
checkEqual(bot->size(),1,"original (1) still ok");
}
in.release(key);
in.release(key2);
}
bool getBox()
{
// Crops the image based on user input and creates a template for the tracker with it.
printf("Reading image!!\n");
ImageOf<PixelRgb> *imgIn = imInPort.read(); // read an image
cv::Mat img((IplImage*) imgIn->getIplImage());
printf("Click first top left and then bottom right from the object !!\n");
bool boxOK = false;
//Bottle &out = coordsOutPort.prepare();
cv::Point tl, br;
while (!boxOK){
printf("Click on top left!\n");
Bottle *point1 = coordsInPort.read(true);
tl.x = point1->get(0).asDouble();
tl.y = point1->get(1).asDouble();
printf("Point read at %d, %d!!\n", tl.x, tl.y);
printf("Click on bottom right!\n");
Bottle *point2 = coordsInPort.read(true);
br.x = point2->get(0).asDouble();
br.y = point2->get(1).asDouble();
printf("Point read at %d, %d!!\n", br.x, br.y);
BBox = cv::Rect(tl,br);
if (BBox.area() > 20) {
printf("valid coordinates, cropping image!\n");
boxOK = true;}
else {printf("Coordinates not valid, click again!\n");}
}
printf("Prep out mat !!\n");
ImageOf<PixelRgb> &templateOut = tempOutPort.prepare();
templateOut.resize(BBox.width, BBox.height);
cv::Mat tempOut((IplImage*)templateOut.getIplImage(),false);
img(BBox).copyTo(tempOut);
//cv::GaussianBlur(img(BBox), imOut, cv::Size(1,1), 1, 1);
double t0 = Time::now();
while(Time::now()-t0 < 1) { //send the template for one second
printf("Writing Template!\n");
tempOutPort.write();
Time::delay(0.1);
}
tracking = true;
return true;
}
bool updateModule()
{
if (ImageOf<PixelRgb> *iImg=iPort.read())
{
LockGuard lg(mutex);
ImageOf<PixelRgb> &oImg=oPort.prepare();
oImg=*iImg;
cv::Mat frame=cv::cvarrToMat((IplImage*)oImg.getIplImage());
cv::Rect2d result;
if (state==init)
{
tracker=cv::Tracker::create("BOOSTING");
tracker->init(frame,initRect);
result=initRect;
state=run;
}
else if (state==run)
tracker->update(frame,result);
if (state!=idle)
cv::rectangle(frame,
cv::Point((int)result.x,(int)result.y),
cv::Point((int)(result.x+result.width),(int)(result.y+result.height)),
cv::Scalar(0,255,0),2);
oPort.write();
}
return true;
}
void testPublisherToBufferedPort() {
report(0,"Publisher to BufferedPort 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 = p.prepare();
b.clear();
b.addInt(42);
p.write();
p.waitForWrite();
Bottle *bin = pin.read();
checkTrue(bin!=NULL,"message arrived");
if (!bin) return;
checkEqual(bin->get(0).asInt(),42,"message is correct");
}
}
bool updateModule() {
if (firstRun) {
init();
firstRun = false;
}
// get a target object position from a YARP port
Bottle *b = inPort.read();
if (b != NULL) {
Vector xd(3);
bool f;
xd[0] = b->get(0).asDouble();
xd[1] = b->get(1).asDouble();
xd[2] = b->get(2).asDouble();
// apply systematic offset
// due to uncalibrated kinematic
xd = xd + dOffs;
// safe thresholding
xd[0] = xd[0] > -0.1 ? -0.1 : xd[0];
// grasp (wait until it's done)
action->grasp(xd, graspOrien, graspDisp);
action->checkActionsDone(f, true);
action->areFingersInPosition(f);
// if fingers are not in position,
// it's likely that we've just grasped
// something, so lift it up!
if (!f) {
cout << "Wow, got something!" << endl;
// lift the object (wait until it's done)
action->pushAction(xd + dLift, graspOrien);
action->checkActionsDone(f, true);
}
else
cout << "Sorry :( ... nothing to grasp" << endl;
// release the object or just open the
// hand (wait until it's done)
action->pushAction("open_hand");
action->checkActionsDone(f, true);
// wait until it's done, since
// use two arms that shares the torso
action->pushAction(home_x);
action->checkActionsDone(f, true);
// let the hand wave a bit around home position
// the waving will be disabled before commencing
// a new action
action->enableArmWaving(home_x);
}
return true;
}
bool acquirePoints()
{
cout<<"point_cloud is reading from port..."<<endl;
Bottle *pointIn=portIn.read(true);
Vector point;
point.resize(3,0.0);
if (pointIn!=NULL)
{
int bsize=pointIn->size();
cout<<"size of bottle received: "<<bsize<<endl;
for(size_t i=0; i<bsize; i++)
{
Bottle *pointList=pointIn->get(i).asList();
point[0]=pointList->get(0).asDouble();
point[1]=pointList->get(1).asDouble();
point[2]=pointList->get(2).asDouble();
cout<<"point received: "<<point.toString().c_str()<<endl;
points.push_back(point);
}
return true;
}
else
return false;
}
// Function: mdlOutputs =======================================================
// Abstract:
// In this function, you compute the outputs of your S-function
// block.
static void mdlOutputs(SimStruct *S, int_T tid)
{
BufferedPort<Vector> *toPort = static_cast<BufferedPort<Vector>*>(ssGetPWork(S)[0]);
Vector *v = toPort->read(false); // Read from the port. Waits until data arrives.
if (v!=NULL)
{
for (int i = 0; i < SIZE_READING_PORT; i++)
{
real_T *pY = (real_T *)ssGetOutputPortSignal(S,i);
int_T widthPort = ssGetOutputPortWidth(S,i);
if (widthPort == 1)
{
for(int_T j=0; j<widthPort; j++)
if (i < (v->length())){
pY[j] = v->data()[i];
//cout<<v->data()[i]<<" ";
}
else
pY[j] = 0;
}
else
cout << "ERROR: something wrong with port dimensions \n";
}
}
}
int main(int argc, char *argv[]) {
// Open the network
Network yarp;
BufferedPort<Sound> p;
p.open("/receiver");
Network::connect("/sender", "/receiver");
// Get an audio write device.
Property conf;
conf.put("device","portaudio");
conf.put("samples", "4096");
conf.put("write", "1");
PolyDriver poly(conf);
IAudioRender *put;
// Check it can write the sound
poly.view(put);
if (put==NULL) {
printf("cannot open interface\n");
return 1;
}
//Receive and render the sound
Sound *s;
while (true)
{
s = p.read(false);
if (s!=NULL)
put->renderSound(*s);
}
return 0;
}
bool updateModule()
{
Bottle *inputBottleTactileLeft = gtw_tactileLeftInputPort.read();
Bottle *inputBottleTactileRight = gtw_tactileRightInputPort.read();
Bottle &outputBottleTactileLeft = gtw_tactileLeftOutputPort.prepare();
Bottle &outputBottleTactileRight = gtw_tactileRightOutputPort.prepare();
outputBottleTactileLeft = *inputBottleTactileLeft;
outputBottleTactileRight = *inputBottleTactileRight;
gtw_tactileLeftOutputPort.write();
gtw_tactileRightOutputPort.write();
return true;
}
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;
}
virtual void run()
{
// get both input images
yarp::sig::Vector *actuation=actuationIn->read(false);
//if we have the actuator
if (actuation != NULL)
//if (speaker->NotDone()) // this should actually loop some fixed number of blocks based on the update size
{
// setup output variables
yarp::os::Bottle &areaFunction = areaOut->prepare();
yarp::os::Bottle &acousticSignal = acousticOut->prepare();
// this should run some number of times? maybe...
{
// run next step of control inputs
cout << actuation->data() << std::endl;
for(int k = 0; k<kArt_muscle_MAX; k++){
speaker->art[k] = (*actuation)[k];
//cout << (*actuation)[k] << std::endl; // debug
}
// iterate simulator
speaker->IterateSim();
// loop back to start if we hit the of the buffer
speaker->LoopBack();
}
// resize acousticSignal and put in samples
//acousticSignal.resize(1); // (samples, channels) # of samples should correspond to loop above
//acousticSignal(0) = speaker->getLastSample();
acousticSignal.addDouble(speaker->getLastSample());
// load area function
double temp[89];
speaker->getAreaFcn(temp);
// and pass into output variable
//areaFunction.resize(89);
for(int k=0; k<89; k++){
//areaFunction(k) = temp[k];
areaFunction.addDouble(temp[k]);
}
//send out, cleanup
areaOut->write();
acousticOut->write();
} else {
// speaker->Speak();
}
}
virtual void run()
{
if (Vector *qdNew=qdPort.read(false))
qd=*qdNew;
Vector v=Kp*(qd-q);
for (int i=0; i<joints; i++)
if (v[i]>maxVel[i])
v[i]=maxVel[i];
else if (v[i]<-maxVel[i])
v[i]=-maxVel[i];
q=I->integrate(v);
Vector &qo=qoPort.prepare();
Vector &vo=voPort.prepare();
qo.resize(3+joints,0.0);
vo.resize(3+joints,0.0);
// deal with torso joints
for (int i=3; i<qo.length(); i++)
{
qo[i]=q[i-3];
vo[i]=v[i-3];
}
qoPort.write();
voPort.write();
}
int main(int argc, char *argv[]) {
Network yarp;
BufferedPort<Bottle> inPort;
ResourceFinder parameters;
parameters.configure(argc, argv);
int delay=0;
string portname="/consumer";
if (parameters.check("name"))
portname=parameters.find("name").asString().c_str();
if (parameters.check("delay"))
{
delay=parameters.find("delay").asInt();
printf("Setting delay to %d[ms]\n", delay);
}
inPort.open(portname.c_str());
while (true) {
Bottle *message=inPort.read();
if (message==0)
continue;
int counter=message->get(0).asInt();
string msg=message->get(1).asString().c_str();
printf("Received: %d %s\n", counter, msg.c_str());
Time::delay(delay/1000.0);
}
return 0;
}
int main(int argc, char *argv[]) {
// Initialize network
Network yarp;
// Make a port for reading and writing images
BufferedPort<ImageOf<PixelRgb> > port;
// Get command line options
Property options;
options.fromCommand(argc,argv);
// Set the name of the port (use "/worker" if there is no --name option)
std::string portName = options.check("name",Value("/worker")).asString();
port.open(portName);
int ct = 0;
while (true) {
// read an image from the port
ImageOf<PixelRgb> *img = port.read();
if (img==NULL) continue;
// add a blue circle
PixelRgb blue(0,0,255);
addCircle(*img,blue,ct,50,10);
ct = (ct+5)%img->width();
// output the image
port.prepare() = *img;
port.write();
}
return 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();
}
bool acquirePoints(int &file_provided, ResourceFinder &rf)
{
if(file_provided==1)
{
if (!rf.check("pointCloudFile"))
{
yError()<<"point clouds file not provided!";
return false;
}
pointCloudFileName=rf.find("pointCloudFile"). asString().c_str();
ifstream pointCloudFile(pointCloudFileName.c_str());
if (!pointCloudFile.is_open())
{
yError()<<"problem opening point clouds file!";
pointCloudFile.close();
return false;
}
if (!readPoints(pointCloudFile))
{
yError()<<"problem reading point clouds file!";
pointCloudFile.close();
return false;
}
pointCloudFile.close();
}
else
{
cout<<"point_cloud is reading from port..."<<endl;
Bottle *pointIn=portInPoints.read(true);
if (pointIn!=NULL)
{
int bsize=pointIn->size();
cout<<"size of bottle received: "<<bsize<<endl;
for(size_t i=0; i<bsize; i++)
{
Bottle *pointList=pointIn->get(i).asList();
point[0]=pointList->get(0).asDouble();
point[1]=pointList->get(1).asDouble();
point[2]=pointList->get(2).asDouble();
point_cloud.push_back(point);
cout<<"point received "<<point.toString().c_str()<<endl;
nPointsC++;
}
cout<<"points acquired " << nPointsC<<endl;
return true;
}
portInPoints.close();
}
}
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;
}
}
bool updateModule()
{
if (imageIn.getInputCount()>0)
{
cv::Mat orig = (IplImage *) imageIn.read(true)->getIplImage();
ImageOf<PixelMono> &outImg = imageOut.prepare();
cv::Mat blueOnly = blueFilter(orig);
float sumx=0, sumy=0;
float num_pixel = 0;
for(int x=0; x<blueOnly.cols; x++) {
for(int y=0; y<blueOnly.rows; y++) {
int val = blueOnly.at<uchar>(y,x);
if( val >= 50) {
sumx += x;
sumy += y;
num_pixel++;
}
}
}
cv::Point p(sumx/num_pixel, sumy/num_pixel);
//cout << cv::Mat(p) << endl;
cv::Moments m = cv::moments(blueOnly, false);
cv::Point p1(m.m10/m.m00, m.m01/m.m00);
//cout << cv::Mat(p1) << endl;
cv::circle(blueOnly, p, 5, cv::Scalar(0,0,0), -1);
Bottle &target=targetPort.prepare();
target.clear();
if (p.x < 0 && p.y <0)
{
target.addDouble(0);
target.addDouble(0);
target.addInt(0);
}
else
{
target.addDouble(p.x);
target.addDouble(p.y);
target.addInt(1);
}
IplImage tmp = blueOnly;
outImg.resize(tmp.width, tmp.height);
cvCopyImage( &tmp, (IplImage *) outImg.getIplImage());
imageOut.write();
targetPort.write();
}
return true;
}
// reading modified from react-control reactCtrlThread::getCollisionPointsFromPort
// writing adapted from iCub_Sim.cpp OdeSdlSimulation::inspectWholeBodyContactsAndSendTouch()
bool fillSkinContactFromAggregPort(BufferedPort<Bottle> &inPort, const double amplification, skinContactList &sCL)
{
SkinPart sp = SKIN_PART_UNKNOWN;
//all in the link FoR
Vector geocenter(3,0.0); //geocenter from skin / average activation locus from the pps
Vector normal(3,0.0);
Vector force(3,0.0);
Vector moment(3,0.0); //will remain zero
double normalized_activation = 0.0;
std::vector<unsigned int> taxel_list;
taxel_list.clear(); //we will be always passing empty list
Bottle* collPointsMultiBottle = inPort.read(false);
if(collPointsMultiBottle != NULL)
{
//printf("fillSkinContactFromAggregPort(): There were %d bottles on the port.\n",collPointsMultiBottle->size());
for(int i=0; i< collPointsMultiBottle->size();i++)
{
sp = SKIN_PART_UNKNOWN;
geocenter.zero(); normal.zero(); normalized_activation = 0.0;
Bottle* collPointBottle = collPointsMultiBottle->get(i).asList();
//printf("Bottle %d contains %s \n", i,collPointBottle->toString().c_str());
sp = (SkinPart)(collPointBottle->get(0).asInt());
geocenter(0) = collPointBottle->get(1).asDouble();
geocenter(1) = collPointBottle->get(2).asDouble();
geocenter(2) = collPointBottle->get(3).asDouble();
normal(0) = collPointBottle->get(4).asDouble();
normal(1) = collPointBottle->get(5).asDouble();
normal(2) = collPointBottle->get(6).asDouble();
normalized_activation = collPointBottle->get(13).asDouble();
//hack - in current iCubGui version, in bvhnode.h, ForceArrow,
//there is an atan that gives a wrong direction for force vectors of the form (0 0 1)
force(0)=0.000001; force(1)=0.000001;
force(2) = -1.0*amplification*normalized_activation*normal(2);
//see iCubGui/src/objectsthread.h ObjectsManager::manage(iCub::skinDynLib::skinContactList &forces)
//printf("fillDynContactFromAggregPort: setting dynContact: Body part: %s Linknum: %d CoP: %s F: %s M: %s\n",
//BodyPart_s[SkinPart_2_BodyPart[sp].body].c_str(),getLinkNum(sp),geoCenter.toString(3,3).c_str(),
// (-1.0*normal).toString(3,3).c_str(),moment.toString(3,3).c_str());
skinContact sc(SkinPart_2_BodyPart[sp].body,sp,getLinkNum(sp),geocenter,geocenter,taxel_list,
amplification*normalized_activation,normal,force,moment);
// In skinManager/src/compensator.cpp, Compensator::getContacts():
// set an estimate of the force that is with normal direction and intensity equal to the pressure
// d.setForceModule(-0.05*activeTaxels*pressure*normal);
sCL.push_back(sc);
}
return true;
}
else{
//printf("fillDynContactFromAggregPort(): no tactile/pps vectors on the port.\n") ;
return false;
};
}
int main() {
Network yarp; // set up yarp
BufferedPort<ImageOf<PixelRgb> > imagePort; // make a port for reading images
BufferedPort<Vector> targetPort;
imagePort.open("/tutorial/image/in"); // give the port a name
targetPort.open("/tutorial/target/out");
Network::connect("/icubSim/cam/left","/tutorial/image/in");
while (1) { // repeat forever
ImageOf<PixelRgb> *image = imagePort.read(); // read an image
ImageOf<PixelRgb> *image_cropped;
if (image!=NULL) { // check we actually got something
printf("We got an image of size %dx%d\n", image->width(), image->height());
double xMean = 0;
double yMean = 0;
int ct = 0;
for (int x=0; x<image->width(); x++) {
for (int y=0; y<image->height(); y++) {
PixelRgb& pixel = image->pixel(x,y);
/* very simple test for blueishness */
/* make sure blue level exceeds red and green by a factor of 2 */
if (pixel.b>pixel.r*1.2+10 && pixel.b>pixel.g*1.2+10) {
/* there's a blueish pixel at (x,y)! */
/* let's find the average location of these pixels */
xMean += x;
yMean += y;
ct++;
}
}
}
if (ct>0) {
xMean /= ct;
yMean /= ct;
}
if (ct>(image->width()/20)*(image->height()/20)) {
printf("Best guess at blue target: %g %g\n", xMean, yMean);
Vector& target = targetPort.prepare();
target.resize(3);
target[0] = xMean;
target[1] = yMean;
target[2] = 1;
targetPort.write();
} else {
Vector& target = targetPort.prepare();
target.resize(3);
target[0] = 0;
target[1] = 0;
target[2] = 0;
targetPort.write();
}
}
}
return 0;
}
/*
* This is our main function. Will be called periodically every getPeriod() seconds.
*/
bool updateModule()
{
Bottle *bot=vergencePort.read(false);
if(bot!=NULL && bot->toString()=="vergence_accomplished")
vergence_accomplished=true;
else
vergence_accomplished=false;
return true;
}
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");
}
}
bool getPointTrack(double tableHeight)
{
// Put the input image at the moment of computing out
printf("Propagating image!!\n");
ImageOf<PixelRgb> *imgIn = imInPort.read(); // read an image
if(imgIn == NULL) {
printf("No object tracked \n");
return false;
}
ImageOf<PixelRgb> &imOut = imOutPort.prepare();
imOut = *imgIn;
imOutPort.write();
// Retrieves and relays the 2D coordinates of the object tracked by the tracker
//Bottle &out = coordsOutPort.prepare();
printf("Getting 3D coords of tracked object!!\n");
Bottle *trackCoords = trackInPort.read(true);
coords2D.resize(2,0);
coords2D[0] = trackCoords->get(0).asInt();
coords2D[1] = trackCoords->get(1).asInt();
printf("Point in 2D read: %.2f, %.2f!!\n", coords2D(0), coords2D(1));
Vector table(4); // specify the plane in the root reference frame as ax+by+cz+d=0; z=-tableHeight in this case
table[0] = 0.0; table[1] = 0.0; table[2] = 1.0;
table[3] = -tableHeight; // d -> so the equation of the table plane is z=-h
int camSel;
if (camera != "left") {
camSel = 1;}
else { camSel = 0;}
if(igaze->get3DPointOnPlane(camSel,coords2D, table, coords3D)){
printf("Point in 3D computed: %.2f, %.2f, %.2f!!\n", coords3D(0), coords3D(1), coords3D(2));
return true;
} else {
printf("3D point could not be computed\n");
return false;
}
}
bool getPointClick()
{
printf("Click on the object in the image!!\n");
//Bottle &out = coordsOutPort.prepare();
Bottle *obj2Dcoord = coordsInPort.read(true);
printf("Point read!!\n");
coords2D.resize(2,0.0);
coords2D[0] = obj2Dcoord->get(0).asInt();
coords2D[1] = obj2Dcoord->get(1).asInt();
printf("Point in 2D read: %.2f, %.2f!!\n", coords2D(0), coords2D(1));
return true; // get the projection
}