bool configure(ResourceFinder &rf)
{
string name=rf.check("name",Value("imuFilter")).asString();
string robot=rf.check("robot",Value("icub")).asString();
size_t gyro_order=(size_t)rf.check("gyro-order",Value(5)).asInt();
size_t mag_order=(size_t)rf.check("mag-order",Value(51)).asInt();
mag_vel_thres_up=rf.check("mag-vel-thres-up",Value(0.04)).asDouble();
mag_vel_thres_down=rf.check("mag-vel-thres-down",Value(0.02)).asDouble();
bias_gain=rf.check("bias-gain",Value(0.001)).asDouble();
verbose=rf.check("verbose");
gyroFilt.setOrder(gyro_order);
magFilt.setOrder(mag_order);
biasInt.setTs(bias_gain);
gyroBias.resize(3,0.0);
adaptGyroBias=false;
iPort.open("/"+name+"/inertial:i");
oPort.open("/"+name+"/inertial:o");
bPort.open("/"+name+"/bias:o");
string imuPortName=("/"+robot+"/inertial");
if (!Network::connect(imuPortName,iPort.getName()))
yWarning("Unable to connect to %s",imuPortName.c_str());
return true;
}
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 bool threadInit()
{
name=rf.check("name",Value("randObjSeg")).asString().c_str();
trate = rf.check("rate",Value(50)).asInt();
this->setRate(trate);
portImgIn=new BufferedPort<ImageOf<PixelFloat> >;
string portInName="/"+name+"/img:i";
portImgIn->open(portInName.c_str());
portRefIn=new BufferedPort<ImageOf<PixelRgb> >;
string portRefName="/"+name+"/ref:i";
portRefIn->open(portRefName.c_str());
portImgOut=new BufferedPort<ImageOf<PixelRgb> >;
string portOutName="/"+name+"/img:o";
portImgOut->open(portOutName.c_str());
portObjOut=new BufferedPort<yarp::sig::Matrix>;
string portObjName="/"+name+"/obj:o";
portObjOut->open(portObjName.c_str());
srand(time(NULL));
return true;
}
void SamInit(void)
{
RecognisePort("TTSIn"); // name the port to be shown in the gui
RecognisePort("EmoIn"); // name the port to be shown in the gui
RecognisePort("TrackIn"); // name the port to be shown in the gui
StartModule("/EMYS");
bTTS.open("/EMYS_TTSIn"); // open the port
bEmo.open("/EMYS_EmoIn"); // open the port
bTrack.open("/EMYS_TrackIn"); // open the port
//bTrack.setStrict(true);
//bEmo.setStrict(true);
//bTTS.setStrict(true);
bTrack.setReporter(myPortStatus); // set reporter, this is important
bEmo.setReporter(myPortStatus); // set reporter, this is important
bTTS.setReporter(myPortStatus); // set reporter, this is important
bTalking=false;
puts("started EMYS rcv");
}
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");
}
}
WorkingThread(int period=5): RateThread(period)
{
enable_execute_joint_command = true;
//*** open the output port
port_command_out.open("/trajectoryPlayer/port_command_out:o");
port_command_joints.open("/trajectoryPlayer/port_joints:o");
}
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;
}
virtual bool threadInit()
{
string name=rf.check("name",Value("faceTracker")).asString().c_str();
string robot=rf.check("robot",Value("icub")).asString().c_str();
int period=rf.check("period",Value(50)).asInt();
eye=rf.check("eye",Value("left")).asString().c_str();
arm=rf.check("arm",Value("right")).asString().c_str();
eyeDist=rf.check("eyeDist",Value(1.0)).asDouble();
holdoff=rf.check("holdoff",Value(3.0)).asDouble();
Property optGaze("(device gazecontrollerclient)");
optGaze.put("remote","/iKinGazeCtrl");
optGaze.put("local",("/"+name+"/gaze_client").c_str());
if (!clientGaze.open(optGaze))
return false;
clientGaze.view(igaze);
igaze->storeContext(&startup_gazeContext_id);
igaze->blockNeckRoll(0.0);
if (arm!="none")
{
Property optArm("(device cartesiancontrollerclient)");
optArm.put("remote",("/"+robot+"/cartesianController/"+arm+"_arm").c_str());
optArm.put("local",("/"+name+"/arm_client").c_str());
if (!clientArm.open(optArm))
return false;
clientArm.view(iarm);
iarm->storeContext(&startup_armContext_id);
}
portImgIn.open(("/"+name+"/img:i").c_str());
portImgOut.open(("/"+name+"/img:o").c_str());
portTopDown.open(("/"+name+"/topdown:i").c_str());
portSetFace.open(("/"+name+"/setFace:rpc").c_str());
if (!fd.init(rf.findFile("descriptor").c_str()))
{
fprintf(stdout,"Cannot load descriptor!\n");
return false;
}
Rand::init();
resetState();
armCmdState=0;
queuedFaceExprFlag=false;
setRate(period);
cvSetNumThreads(1);
t0=Time::now();
return true;
}
bool configure(ResourceFinder &rf)
{
iPort.open("/tracker:i");
oPort.open("/tracker:o");
rpcPort.open("/tracker:rpc");
attach(rpcPort);
state=idle;
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;
}
bool configure(ResourceFinder &rf)
{
Property config;
config.fromConfigFile(rf.getContext() + "config.ini");
Bottle &bGeneral = config.findGroup("host_computer");
ip_address = bGeneral.find("ip_address").asString().c_str();
cout << "Host computer ip address: " << ip_address << endl;
gtw_contactTactileLeftInputPort = gtw_contactTactileLeftInputPort.addName("/gtw/telepresence/tactile/left:i");
gtw_contactTactileLeftInputPort = gtw_contactTactileLeftInputPort.addCarrier("tcp");
gtw_contactTactileLeftInputPort = gtw_contactTactileLeftInputPort.addHost(ip_address);
gtw_contactTactileLeftInputPort = gtw_contactTactileLeftInputPort.addPort(80005);
gtw_contactTactileRightInputPort = gtw_contactTactileRightInputPort.addName("/gtw/telepresence/tactile/right:i");
gtw_contactTactileRightInputPort = gtw_contactTactileRightInputPort.addCarrier("tcp");
gtw_contactTactileRightInputPort = gtw_contactTactileRightInputPort.addHost(ip_address);
gtw_contactTactileRightInputPort = gtw_contactTactileRightInputPort.addPort(80006);
gtw_contactTactileLeftOutputPort = gtw_contactTactileLeftOutputPort.addName("/gtw/telepresence/tactile/left:o");
gtw_contactTactileLeftOutputPort = gtw_contactTactileLeftOutputPort.addCarrier("tcp");
gtw_contactTactileLeftOutputPort = gtw_contactTactileLeftOutputPort.addHost(ip_address);
gtw_contactTactileLeftOutputPort = gtw_contactTactileLeftOutputPort.addPort(80007);
gtw_contactTactileRightOutputPort = gtw_contactTactileRightOutputPort.addName("/gtw/telepresence/tactile/right:o");
gtw_contactTactileRightOutputPort = gtw_contactTactileRightOutputPort.addCarrier("tcp");
gtw_contactTactileRightOutputPort = gtw_contactTactileRightOutputPort.addHost(ip_address);
gtw_contactTactileRightOutputPort = gtw_contactTactileRightOutputPort.addPort(80008);
Network::registerContact(gtw_contactTactileLeftInputPort);
Network::registerContact(gtw_contactTactileRightInputPort);
Network::registerContact(gtw_contactTactileLeftOutputPort);
Network::registerContact(gtw_contactTactileRightOutputPort);
gtw_tactileLeftInputPort.open(gtw_contactTactileLeftInputPort, true); // give the port a name
gtw_tactileRightInputPort.open(gtw_contactTactileRightInputPort, true); // give the port a name
gtw_tactileLeftOutputPort.open(gtw_contactTactileLeftOutputPort, true); // give the port a name
gtw_tactileRightOutputPort.open(gtw_contactTactileRightOutputPort, true); // give the port a name
cout << "Waiting for connection: /icub/skin/left_hand_comp -> /gtw/telepresence/tactile/left:i" << endl;
while( !Network::isConnected("/icub/skin/left_hand_comp", "/gtw/telepresence/tactile/left:i") )
{}
cout << "Connection ready: /icub/skin/left_hand_comp -> /gtw/telepresence/tactile/left:i" << endl;
cout << "Waiting for connection: /icub/skin/right_hand_comp -> /gtw/telepresence/tactile/right:i" << endl;
while( !Network::isConnected("/icub/skin/right_hand_comp", "/gtw/telepresence/tactile/right:i") )
{}
cout << "Connection ready: /icub/skin/right_hand_comp -> /gtw/telepresence/tactile/right:i" << endl;
return true;
}
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;
}
bool configure(ResourceFinder &rf)
{
string name=rf.check("name",Value("cmt")).asString().c_str();
imgInPort.open(("/"+name+"/img:i").c_str());
imgOutPort.open(("/"+name+"/img:o").c_str());
dataOutPort.open(("/"+name+"/data:o").c_str());
rpcPort.open(("/"+name+"/rpc").c_str());
attach(rpcPort);
initBoundingBox=doCMT=false;
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[]) {
// 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 threadInit()
{
string name=rf.find("name").asString().c_str();
double period=rf.find("period").asDouble();
setRate(int(1000.0*period));
// open a client interface to connect to the gaze server
// we suppose that:
// 1 - the iCub simulator (icubSim) is running
// 2 - the gaze server iKinGazeCtrl is running and
// launched with the following options:
// --robot icubSim --context cameraCalibration/conf --config icubSimEyes.ini
Property optGaze("(device gazecontrollerclient)");
optGaze.put("remote","/iKinGazeCtrl");
optGaze.put("local",("/"+name+"/gaze").c_str());
if (!clientGaze.open(optGaze))
return false;
// open the view
clientGaze.view(igaze);
// latch the controller context in order to preserve
// it after closing the module
// the context contains the tracking mode, the neck limits and so on.
igaze->storeContext(&startup_context_id);
// set trajectory time:
igaze->setNeckTrajTime(0.8);
igaze->setEyesTrajTime(0.4);
port.open(("/"+name+"/target:i").c_str());
return true;
}
bool open(Searchable& config) {
ct = 0;
port.open(getName());
cmdPort.open(getName("cmd")); // optional command port
attach(cmdPort); // cmdPort will work just like terminal
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");
}
}
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");
}
}
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[]) {
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;
}
bool configure(ResourceFinder &rf)
{
string name="demoActionRecognition";
string rpcName="/"+name+"/rpc";
string outExecName="/"+name+"/exec:o";
string inName="/"+name+"/scores:i";
string outGestRecModName="/"+name+"/gestRec:o";
rpc.open(rpcName.c_str());
attach(rpc);
outExecModule.open(outExecName.c_str());
inSequence.open(inName.c_str());
string outspeakname="/"+name+"/outspeak";
outspeak.open(outspeakname.c_str());
outGestRecModule.open(outGestRecModName.c_str());
string filename=rf.findFile("actions").c_str();
Property config; config.fromConfigFile(filename.c_str());
Bottle& bgeneral=config.findGroup("general");
numActions=bgeneral.find("numActions").asInt();
sequence="";
currentSequence="";
myturn=false;
yourturn=false;
gameEnding=true;
count=0;
temp=0;
return true;
}
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 configure(ResourceFinder &rf)
{
rf1=rf;
if(algorithm==1)
loc5=new UnscentedParticleFilter();
else
loc5=new ScalingSeries();
loc5->configure(rf1);
state=0;
meas_given=1;
if(loc5->measurementsString!=1)
{
namePorts = rf.check("namePorts", Value("visual-localization"), "Getting module name").asString();
portIn.open("/"+namePorts+"/pnt:i");
rpcOut.open("/"+namePorts+"/toolFeat:rpc");
meas_given=0;
}
cout<<"meas_given "<<meas_given<<endl;
error_thres=loc5->error_thres;
cout<<"thresconf "<<error_thres<<endl;
rpcPort.open(("/"+namePorts+"/rpc").c_str());
attach(rpcPort);
delete loc5;
return true;
}
JNIEXPORT jboolean JNICALL Java_com_alecive_yarpdroid_yarpviewFragment_createBufferedImgPortL
(JNIEnv *env, jobject obj, jstring _applicationName)
{
__android_log_print(ANDROID_LOG_DEBUG, LOG_TAG, "I'm creating the image port");
if (putenv("YARP_CONF=/data/data/com.alecive.yarpdroid/files/yarpconf"))
{
__android_log_print(ANDROID_LOG_ERROR, LOG_TAG, "Putenv failed %d", errno);
}
DataProcessorImg* processor = new DataProcessorImg(env, javaObj);
BufferedPort<ImageOf<PixelRgb> > *ImgPortL;
ImgPortL = new BufferedPort<ImageOf<PixelRgb> >;
ImgPortL->useCallback(*processor);
std::string portName=env->GetStringUTFChars(_applicationName, 0);
portName = portName + "/cam/left:i";
if(!ImgPortL->open(portName.c_str()))
{
__android_log_print(ANDROID_LOG_ERROR, LOG_TAG, "Error in opening image port!");
delete ImgPortL;
ImgPortL = 0;
return (jboolean)false;
}
setHandle(env, javaObj, ImgPortL, "viewLeftHandle");
return (jboolean)true;
}
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 open(Searchable& p) {
bool dev = true;
if (p.check("nodevice")) {
dev = false;
}
if (dev) {
poly.open(p);
if (!poly.isValid()) {
printf("cannot open driver\n");
return false;
}
if (!p.check("mute")) {
// Make sure we can write sound
poly.view(put);
if (put==NULL) {
printf("cannot open interface\n");
return false;
}
}
}
port.setStrict(true);
if (!port.open(p.check("name",Value("/yarphear")).asString())) {
printf("Communication problem\n");
return false;
}
if (p.check("remote")) {
Network::connect(p.check("remote",Value("/remote")).asString(),
port.getName());
}
return true;
}
bool configure(ResourceFinder &rf)
{
string name=rf.find("name").asString().c_str();
setName(name.c_str());
Property config; config.fromConfigFile(rf.findFile("from").c_str());
Bottle &bGeneral=config.findGroup("general");
if (bGeneral.isNull())
{
cout<<"Error: group general is missing!"<<endl;
return false;
}
// parsing general config options
Property option(bGeneral.toString().c_str());
option.put("local",name.c_str());
option.put("part","left_arm");
option.put("grasp_model_type",rf.find("grasp_model_type").asString().c_str());
option.put("grasp_model_file",rf.findFile("grasp_model_file").c_str());
option.put("hand_sequences_file",rf.findFile("hand_sequences_file").c_str());
// parsing arm dependent config options
Bottle &bArm=config.findGroup("arm_dependent");
getArmDependentOptions(bArm,graspOrien,graspDisp,dOffs,dLift,home_x);
cout<<"***** Instantiating primitives for left arm"<<endl;
action=new AFFACTIONPRIMITIVESLAYER(option);
if (!action->isValid())
{
delete action;
return false;
}
deque<string> q=action->getHandSeqList();
cout<<"***** List of available hand sequence keys:"<<endl;
for (size_t i=0; i<q.size(); i++)
cout<<q[i]<<endl;
string fwslash="/";
inPort.open((fwslash+name+"/in").c_str());
rpcPort.open((fwslash+name+"/rpc").c_str());
attach(rpcPort);
// check whether the grasp model is calibrated,
// otherwise calibrate it
Model *model; action->getGraspModel(model);
if (model!=NULL)
{
if (!model->isCalibrated())
{
Property prop;
prop.put("finger","all");
model->calibrate(prop);
}
}
return true;
}
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;
}
void SamInit(void)
{
RecognisePort("Out"); // name the port to be shown in the gui
StartModule("/Star");
bStarSend.open("/Star_Out"); // open the port
bStarSend.setReporter(myPortStatus); // set reporter, this is important
puts("started stargazer");
}
