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 testRecentReader() {
report(0,"check recent reader...");
BufferedPort<Bottle> in;
BufferedPort<Bottle> out;
in.setStrict(false);
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);
Time::delay(0.25);
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");
}
}
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();
}
void checkCallback() {
report(0, "checking callback...");
BufferedPort<Bottle> out;
PortReaderBufferTestHelper in;
out.open("/out");
in.open("/in");
in.useCallback();
Network::connect("/out","/in");
Network::sync("/out");
Network::sync("/in");
out.prepare().fromString("1 2 3");
out.write();
int rep = 0;
while (in.count==0 && rep<50) {
Time::delay(0.1);
rep++;
}
checkEqual(in.count,3,"got message #1");
in.disableCallback();
out.prepare().fromString("1 2 3 4");
out.write(true);
Bottle *datum = in.read();
checkTrue(datum!=NULL, "got message #2");
checkEqual(datum->size(),4,"message is ok");
in.useCallback();
in.count = 0;
out.prepare().fromString("1 2 3 4 5");
out.write(true);
rep = 0;
while (in.count==0 && rep<50) {
Time::delay(0.1);
rep++;
}
checkEqual(in.count,5,"got message #3");
}
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;
}
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;
}
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;
}
}
bool updateModule()
{
Vector *imuData=iPort.read();
if (imuData==NULL)
return false;
Stamp stamp;
iPort.getEnvelope(stamp);
double t0=Time::now();
Vector gyro=imuData->subVector(6,8);
Vector gyro_filt=gyroFilt.filt(gyro);
gyro-=gyroBias;
gyro_filt-=gyroBias;
Vector mag_filt=magFilt.filt(imuData->subVector(9,11));
double magVel=norm(velEst.estimate(AWPolyElement(mag_filt,stamp.getTime())));
adaptGyroBias=adaptGyroBias?(magVel<mag_vel_thres_up):(magVel<mag_vel_thres_down);
gyroBias=biasInt.integrate(adaptGyroBias?gyro_filt:Vector(3,0.0));
double dt=Time::now()-t0;
if (oPort.getOutputCount()>0)
{
Vector &outData=oPort.prepare();
outData=*imuData;
outData.setSubvector(6,gyro);
oPort.setEnvelope(stamp);
oPort.write();
}
if (bPort.getOutputCount()>0)
{
bPort.prepare()=gyroBias;
bPort.setEnvelope(stamp);
bPort.write();
}
if (verbose)
{
yInfo("magVel = %g => [%s]",magVel,adaptGyroBias?"adapt-gyroBias":"no-adaption");
yInfo("gyro = %s",gyro.toString(3,3).c_str());
yInfo("gyroBias = %s",gyroBias.toString(3,3).c_str());
yInfo("dt = %.0f [us]",dt*1e6);
yInfo("\n");
}
return true;
}
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);
}
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;
}
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;
}
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;
}
bool updateModule()
{
ImageOf<PixelRgb> &img = portOut.prepare();
global->readAndShow(img, showImages);
portOut.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");
}
}
virtual void onRead(TestData &datum)
{
if (count==nframes)
return;
double now=Time::now();
double origT=datum.get();
unsigned int pl=datum.getPayloadSize();
#ifdef USE_PARALLEL_PORT
static ppEventDebugger pp(0x378);
pp.set();
pp.reset();
#endif
TestData &nd=outPort.prepare();
nd.set(origT);
nd.resize(pl);
outPort.write(true);
if (wait<=0)
{
double dT=(now-origT)*1000;
delay+=dT;
delaySq+=(dT*dT);
count++;
latencies.push_back(Report(dT, pl));
}
else
wait--;
}
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;
}
void testBufferedPortToSubscriber() {
report(0,"BufferedPort to Subscriber test");
Node n("/node");
BufferedPort<Bottle> pout;
pout.setWriteOnly();
pout.open("very_interesting_topic");
{
Node n2("/node2");
Subscriber<Bottle> pin("/very_interesting_topic");
pin.read(false); // make sure we are in buffered mode
waitForOutput(pout,10);
Bottle& b = pout.prepare();
b.clear();
b.addInt(42);
pout.write();
pout.waitForWrite();
Bottle *bin = pin.read();
checkTrue(bin!=NULL,"message arrived");
if (!bin) return;
checkEqual(bin->get(0).asInt(),42,"message is correct");
}
}
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;
}
bool threadInit()
{
fprintf(stdout,"init \n");
port_in0.open(string("/simCOM0:i").c_str());
port_in1.open(string("/simCOM1:i").c_str());
port_in2.open(string("/simCOM2:i").c_str());
port_in3.open(string("/simCOM3:i").c_str());
port_in4.open(string("/simCOM4:i").c_str());
port_in5.open(string("/simCOM5:i").c_str());
port_in6.open(string("/simCOM6:i").c_str());
port_in7.open(string("/simCOM7:i").c_str());
port_in8.open(string("/simCOM8:i").c_str());
port_out.open(string("/simCOM:o").c_str());
yarp::os::Network::connect("/wholeBodyDynamics/com:o","/simCOM0:i");
yarp::os::Network::connect("/wholeBodyDynamics/left_leg/com:o","/simCOM1:i");
yarp::os::Network::connect("/wholeBodyDynamics/left_arm/com:o","/simCOM2:i");
yarp::os::Network::connect("/wholeBodyDynamics/right_leg/com:o","/simCOM3:i");
yarp::os::Network::connect("/wholeBodyDynamics/right_arm/com:o","/simCOM4:i");
yarp::os::Network::connect("/wholeBodyDynamics/head/com:o","/simCOM5:i");
yarp::os::Network::connect("/wholeBodyDynamics/torso/com:o","/simCOM6:i");
yarp::os::Network::connect("/wholeBodyDynamics/upper_body/com:o","/simCOM7:i");
yarp::os::Network::connect("/wholeBodyDynamics/lower_body/com:o","/simCOM8:i");
yarp::os::Network::connect("/simCOM:o","/iCubGui/objects");
Bottle a;
a.clear();
a.addString("reset");
port_out.prepare() = a;
port_out.write();
return true;
}
void update() {
Bottle& status = port.prepare();
mutex.wait();
status.clear();
addQueue(status);
mutex.post();
port.write();
}
int main(int argc, char *argv[]) {
Network yarp;
//yarp::os::Node *rosNode;
BufferedPort<geometry_msgs_Point> xd_inputPort;
xd_inputPort.setReadOnly();
bool outputOk = xd_inputPort.open("/my_xd_in@/ros_tests");
BufferedPort<geometry_msgs_Point> xd_outputPort;
bool outputOk_3 = xd_outputPort.open("/gaze_point");
BufferedPort<geometry_msgs_Point> receiverBuff1Mux1;
bool receiver1Mux1Ok = receiverBuff1Mux1.open("/my_other_port_in");
BufferedPort<geometry_msgs_Point> outputPort;
outputPort.setWriteOnly();
bool outputOk_1 = outputPort.open("/my_x_out@/ros_tests");
if(!outputOk_3)
{
printf("outputOk_3 failed to open\n");
return -1;
}
if(!outputOk_1)
{
printf("outputOk_1 failed to open\n");
return -1;
}
bool connectSuccess = false;
while(!connectSuccess)
{
printf("\nTrying to connect to /iKinGazeCtrl/x:o ... ");
connectSuccess = yarp.connect("/iKinGazeCtrl/x:o", receiverBuff1Mux1.getName());
yarp::os::Time::delay(1);
}
connectSuccess = false;
while(!connectSuccess)
{
printf("\nTrying to connect to /iKinGazeCtrl/xd:i ... ");
connectSuccess = yarp.connect(xd_outputPort.getName(),"/iKinGazeCtrl/xd:i");
yarp::os::Time::delay(1);
}
while(true){
geometry_msgs_Point *reading1Mux1;
reading1Mux1 = xd_inputPort.read();
geometry_msgs_Point & out = xd_outputPort.prepare();
out = *reading1Mux1;
xd_outputPort.write();
geometry_msgs_Point *reading1Mux;
reading1Mux = receiverBuff1Mux1.read();
geometry_msgs_Point & out_ = outputPort.prepare();
out_ = *reading1Mux;
outputPort.write();
}
return 0;
}
virtual void run() {
Network::connect(name,"/reader","mcast");
for (int i=0; i<50; i++) {
Bottle& b = port.prepare();
b.clear();
b.addString(name);
port.write();
Time::delay(0.1);
}
}
void remove_cog(string text)
{
Bottle obj;
obj.clear();
obj.addString("delete"); // command to add/update an object
obj.addString(text.c_str());
port_out.prepare() = obj;
port_out.write();
Time::delay(0.1);
}
void YarpAdapter::writeVibrationDataToOutput(int* vibData) {
Bottle& out = outPort.prepare();
out.clear();
// add vibration data to output
for (int i = 0; i < GloveAdapter::NUM_OF_FINGERS; i++) {
out.addInt(vibData[i]);
}
outPort.write();
}
void sendContacts(BufferedPort<skinContactList> &outPort, const skinContactList &sCL)
{
ts.update();
skinContactList &sCLout = outPort.prepare();
sCLout.clear();
sCLout = sCL;
outPort.setEnvelope(ts);
outPort.write();
}
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;
}
void compute_and_send_command(int action_id)
{
//prepare the output command
Bottle& bot = port_command_out.prepare();
bot.clear();
bot.addInt(actions.action_vector[action_id].counter);
bot.addDouble(actions.action_vector[action_id].time);
bot.addString(actions.action_vector[action_id].tag.c_str());
//@@@ you can add stuff here...
//send the output command
port_command_out.write();
if (!execute_joint_command(action_id))
{
yError("failed to execute command");
}
//quick reads the current position
double encs[50];
if (driver && driver->ienc_ll)
{
//driver->ienc_ll->getEncoders(encs);
int nj; driver->ienc_ll->getAxes(&nj);
for (int j = 0; j < nj; j++)
{
driver->ienc_ll->getEncoder(j, &encs[j]);
}
}
else
{
//invalid driver
yError("Critical error: invalid driver");
}
//send the joints angles on debug port
double *ll = actions.action_vector[action_id].q_joints;
Bottle& bot2 = this->port_command_joints.prepare();
bot2.clear();
bot2.addInt(actions.action_vector[action_id].counter);
bot2.addDouble(actions.action_vector[action_id].time);
int size = this->actions.action_vector[action_id].get_n_joints();
bot2.addString("commands:");
for (int ix=0;ix<size;ix++)
{
bot2.addDouble(ll[ix]);
}
bot2.addString("encoders:");
for (int ix=0;ix<size;ix++)
{
bot2.addDouble(encs[ix]);
}
this->port_command_joints.write();
}
bool read(ConnectionReader &connection)
{
Bottle data;
data.read(connection);
if ((data.size()>=2) && enabled)
{
Vector xa,oa;
iarm->getPose(xa,oa);
Vector xe,oe;
if (eye=="left")
igaze->getLeftEyePose(xe,oe);
else
igaze->getRightEyePose(xe,oe);
Matrix Ha=axis2dcm(oa);
Ha.setSubcol(xa,0,3);
Matrix He=axis2dcm(oe);
He.setSubcol(xe,0,3);
Matrix H=Prj*SE3inv(He)*Ha;
Vector p(2);
p[0]=data.get(0).asDouble();
p[1]=data.get(1).asDouble();
if (logPort.getOutputCount()>0)
{
Vector &log=logPort.prepare();
log=p;
for (int i=0; i<H.rows(); i++)
log=cat(log,H.getRow(i));
for (int i=0; i<Prj.rows(); i++)
log=cat(log,Prj.getRow(i));
for (int i=0; i<Ha.rows(); i++)
log=cat(log,Ha.getRow(i));
for (int i=0; i<He.rows(); i++)
log=cat(log,He.getRow(i));
logPort.write();
}
mutex.wait();
solver.addItem(p,H);
mutex.post();
}
return true;
}
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() {
ImageOf<PixelRgb> *img = port.read();
if (img==NULL) return false;;
// 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 true;
}