int main(int argc, char *argv[])
{
//open the network
Network yarp;
//open the output port
#ifdef USE_PORTS
Port p; //use port
#else
BufferedPort<Sound> p; // use buffered port
#endif
p.open("/sender");
//get the filename
char filename [255];
if (argc==3)
{
if (strcmp(argv[1],"--file")==0)
strcpy (filename, argv[2]);
else
{
printf ("usage: sound_sender_file --file <filename.wav>\n");
return -1;
}
}
else
{
printf ("usage: sound_sender_file --file <filename.wav>\n");
return -1;
}
//read sound from file and put data in yarp::sig::Sound
Sound s;
yarp::sig::file::soundStreamReader soundReader;
printf("opening file %s\n",filename);
if (soundReader.open(filename)==false)
{
printf("cannot open file\n");
return -1;
}
yarp::os::Network::connect("/sender","/yarphear");
//yarp::os::Time::delay (0.1);
//send data on the network
int CHUNK_SIZE = 4096; //samples
for (int i=0; i<3; i++)
{
printf("sending data...\n",i);
bool complete=false;
soundReader.rewind();
do
{
int current_pos = soundReader.getIndex();
int read_samples = soundReader.readBlock(s,CHUNK_SIZE);
if (read_samples<CHUNK_SIZE) complete=true;
static double old = yarp::os::Time::now();
printf("from sample %d to sample %d, time %.3f\n", current_pos, current_pos+read_samples, yarp::os::Time::now()-old);
old = yarp::os::Time::now();
#ifdef USE_PORTS
p.write(s); //use ports
#else
Sound &r = p.prepare(); //use buffered ports
r=s;
p.write(false);
#endif
}
while (!complete);
printf("loop %d/3 complete, press enter to continue\n", i+1);
while ((getchar()) != '\n');
}
//close file
printf("closing file...\n");
soundReader.close();
printf("finished!\n");
return 0;
}
bool configure(ResourceFinder &rf) override
{
portName=rf.check("name",Value("/dump")).asString();
if (portName[0]!='/')
portName="/"+portName;
bool saveData=true;
bool videoOn=false;
string videoType=rf.check("videoType",Value("mkv")).asString();
if (rf.check("type"))
{
string optTypeName=rf.find("type").asString();
if (optTypeName=="bottle")
type=bottle;
else if (optTypeName=="image")
{
type=image;
#ifdef ADD_VIDEO
if (rf.check("addVideo"))
videoOn=true;
#endif
}
else if (optTypeName == "image_jpg")
{
type=image;
save_jpeg = true;
}
#ifdef ADD_VIDEO
else if (optTypeName=="video")
{
type=image;
videoOn=true;
saveData=false;
}
#endif
else
{
yError() << "Error: invalid type";
return false;
}
}
else
type=bottle;
dwnsample=rf.check("downsample",Value(1)).asInt32();
rxTime=rf.check("rxTime");
txTime=rf.check("txTime");
string templateDirName=rf.check("dir")?rf.find("dir").asString():portName;
if (templateDirName[0]!='/')
templateDirName="/"+templateDirName;
string dirName;
if (rf.check("overwrite"))
dirName="."+templateDirName;
else
{
// look for a proper directory
int i=0;
do
{
ostringstream checkDirName;
if (i>0)
checkDirName << "." << templateDirName << "_" << setw(5) << setfill('0') << i;
else
checkDirName << "." << templateDirName;
dirName=checkDirName.str();
i++;
}
while (!yarp::os::stat(dirName.c_str()));
}
yarp::os::mkdir_p(dirName.c_str());
q=new DumpQueue();
t=new DumpThread(type,*q,dirName,100,saveData,videoOn,videoType);
if (!t->start())
{
delete t;
delete q;
return false;
}
reporter.setThread(t);
if (type==bottle)
{
p_bottle=new DumpPort<Bottle>(*q,dwnsample,rxTime,txTime);
p_bottle->useCallback();
p_bottle->open(portName);
p_bottle->setStrict();
p_bottle->setReporter(reporter);
}
else
{
p_image=new DumpPort<Image>(*q,dwnsample,rxTime,txTime);
p_image->useCallback();
p_image->open(portName);
p_image->setStrict();
p_image->setReporter(reporter);
}
if (rf.check("connect"))
{
string srcPort=rf.find("connect").asString();
bool ok=Network::connect(srcPort.c_str(),
(type==bottle)?p_bottle->getName().c_str():
p_image->getName().c_str(),"tcp");
ostringstream msg;
msg << "Connection to " << srcPort << " " << (ok?"successful":"failed");
if (ok)
yInfo() << msg.str();
else
yWarning() << msg.str();
}
// this port serves to handle the "quit" rpc command
rpcPort.open(portName+"/rpc");
attach(rpcPort);
yInfo() << "Service yarp port: " << portName;
yInfo() << "Data stored in : " << dirName;
return true;
}
bool AwareTouch::close()
{
world->close();
eventsPort.close();
return true;
}
void interrupt()
{
port.interrupt();
}
int setupDatagramSocket(UsageEnvironment& env, Port port) {
if (!initializeWinsockIfNecessary()) {
socketErr(env, "Failed to initialize 'winsock': ");
return -1;
}
int newSocket = createSocket(SOCK_DGRAM);
if (newSocket < 0) {
socketErr(env, "unable to create datagram socket: ");
return newSocket;
}
int reuseFlag = groupsockPriv(env)->reuseFlag;
reclaimGroupsockPriv(env);
if (setsockopt(newSocket, SOL_SOCKET, SO_REUSEADDR,
(const char*)&reuseFlag, sizeof reuseFlag) < 0) {
socketErr(env, "setsockopt(SO_REUSEADDR) error: ");
closeSocket(newSocket);
return -1;
}
#if defined(__WIN32__) || defined(_WIN32)
// Windoze doesn't properly handle SO_REUSEPORT or IP_MULTICAST_LOOP
#else
#ifdef SO_REUSEPORT
if (setsockopt(newSocket, SOL_SOCKET, SO_REUSEPORT,
(const char*)&reuseFlag, sizeof reuseFlag) < 0) {
socketErr(env, "setsockopt(SO_REUSEPORT) error: ");
closeSocket(newSocket);
return -1;
}
#endif
#ifdef IP_MULTICAST_LOOP
const u_int8_t loop = 1;
if (setsockopt(newSocket, IPPROTO_IP, IP_MULTICAST_LOOP,
(const char*)&loop, sizeof loop) < 0) {
socketErr(env, "setsockopt(IP_MULTICAST_LOOP) error: ");
closeSocket(newSocket);
return -1;
}
#endif
#endif
// Note: Windoze requires binding, even if the port number is 0
netAddressBits addr = INADDR_ANY;
#if defined(__WIN32__) || defined(_WIN32)
#else
if (port.num() != 0 || ReceivingInterfaceAddr != INADDR_ANY) {
#endif
if (port.num() == 0) addr = ReceivingInterfaceAddr;
MAKE_SOCKADDR_IN(name, addr, port.num());
if (bind(newSocket, (struct sockaddr*)&name, sizeof name) != 0) {
char tmpBuffer[100];
sprintf(tmpBuffer, "bind() error (port number: %d): ",
ntohs(port.num()));
socketErr(env, tmpBuffer);
closeSocket(newSocket);
return -1;
}
#if defined(__WIN32__) || defined(_WIN32)
#else
}
#endif
// Set the sending interface for multicasts, if it's not the default:
if (SendingInterfaceAddr != INADDR_ANY) {
struct in_addr addr;
addr.s_addr = SendingInterfaceAddr;
if (setsockopt(newSocket, IPPROTO_IP, IP_MULTICAST_IF,
(const char*)&addr, sizeof addr) < 0) {
socketErr(env, "error setting outgoing multicast interface: ");
closeSocket(newSocket);
return -1;
}
}
return newSocket;
}
int yarp::serversql::Server::run(int argc, char** argv)
{
Property options;
bool silent(false);
FILE* out;
options.fromCommand(argc, argv, false);
silent = options.check("silent");
out = silent ? tmpfile() : stdout;
fprintf(out, " __ __ ___ ____ ____\n\
\\ \\/ // || _ \\ | _ \\\n\
\\ // /| || |/ / | |/ /\n\
/ // ___ || _ \\ | _/\n\
/_//_/ |_||_| \\_\\|_|\n\
========================\n\n");
if (options.check("help")) {
printf("Welcome to the YARP name server.\n");
printf(" --write Write IP address and socket on the configuration file.\n");
printf(" --config filename.conf Load options from a file.\n");
printf(" --portdb ports.db Store port information in named database.\n");
printf(" Must not be on an NFS file system.\n");
printf(" Set to :memory: to store in memory (faster).\n");
printf(" --subdb subs.db Store subscription information in named database.\n");
printf(" Must not be on an NFS file system.\n");
printf(" Set to :memory: to store in memory (faster).\n");
printf(" --ip IP.AD.DR.ESS Set IP address of server.\n");
printf(" --socket NNNNN Set port number of server.\n");
printf(" --web dir Serve web resources from given directory.\n");
printf(" --no-web-cache Reload pages from file for each request.\n");
printf(" --ros Delegate pub/sub to ROS name server.\n");
printf(" --silent Start in silent mode.\n");
//this->stop();
if (silent) {
fclose(out);
}
return 0;
} else {
fprintf(out, "Call with --help for information on available options\n");
}
NameServerContainer nc;
if (!nc.open(options)) {
return 1;
}
nc.setSilent(silent);
bool ok = false;
NameServerManager name(nc);
BootstrapServer fallback(name);
Port server;
Contact alt;
yarp::os::Bottle cmd;
yarp::os::Bottle reply;
double messageCounter(0);
double pollingRate(.1);
name.setPort(server);
server.setReaderCreator(name);
ok = server.open(nc.where(),false);
if (!ok) {
fprintf(stderr, "Name server failed to open\n");
return 1;
}
printf("\n");
fallback.start();
// Repeat registrations for the server and fallback server -
// these registrations are more complete.
fprintf(out, "Registering name server with itself:\n");
nc.preregister(nc.where());
nc.preregister(fallback.where());
alt = nc.whereDelegate();
if (alt.isValid()) {
nc.preregister(alt);
}
nc.goPublic();
//Setting nameserver property
cmd.addString("set");
cmd.addString(server.getName());
cmd.addString("nameserver");
cmd.addString("true");
yarp::os::impl::NameClient::getNameClient().send(cmd, reply);
fprintf(out, "Name server can be browsed at http://%s:%d/\n",
nc.where().getHost().c_str(), nc.where().getPort());
fprintf(out, "\nOk. Ready!\n");
while(!shouldStop) {
messageCounter += pollingRate;
SystemClock::delaySystem(pollingRate);
double dummy;
if(std::modf(messageCounter / 600.0, &dummy) < .00001) {
fprintf(out, "Name server running happily\n");
}
}
fprintf(out, "closing yarp server\n");
if (silent) {
fclose(out);
}
server.close();
return 0;
}
Helper()
{
clear();
port.includeNodeInName(false);
}
SparseCoderPort(ResourceFinder &_rf)
:BufferedPort<Image>(),rf(_rf)
{
ipl=NULL;
help=false;
verbose=rf.check("verbose");
grid_step=rf.check("grid_step",Value(8)).asInt();
grid_scale=rf.check("grid_scale",Value(1)).asInt();
contextPath=rf.getHomeContextPath().c_str();
string dictionary_name=rf.check("dictionary_file",Value("dictionary_bow.ini")).asString().c_str();
string dictionary_path=rf.findFile(dictionary_name);
if(dictionary_path=="")
dictionary_path=contextPath+"/"+dictionary_name;
string dictionary_group=rf.check("dictionary_group",Value("DICTIONARY")).asString().c_str();
no_code=rf.check("no_code");
dump_sift=rf.check("dump_sift");
if(dump_sift)
{
string sift_path=rf.check("dump_sift",Value("sift.txt")).asString().c_str();
sift_path=contextPath+"/"+sift_path;
string sift_write_mode=rf.check("append")?"a":"w";
fout_sift=fopen(sift_path.c_str(),sift_write_mode.c_str());
}
rate=rf.check("rate",Value(0.0)).asDouble();
dense=rf.check("useDense",Value(1)).asInt();
int knn=rf.check("KNN",Value(5)).asInt();
last_read=0.0;
pyramidLevels=rf.check("PyramidLevels",Value(3)).asInt();
if(dense)
fprintf(stdout,"Step: %d Scale: %d Pyramid: %d Using Dense SIFT Grid\n",grid_step, grid_scale, pyramidLevels);
else
fprintf(stdout,"Step: %d Scale: %d Pyramid: %d Using Sparse SIFTs \n",grid_step, grid_scale, pyramidLevels);
string code_mode_string=rf.check("code_mode",Value("SC")).asString().c_str();
sparse_coder=NULL;
sparse_coder=new DictionaryLearning(dictionary_path,dictionary_group,code_mode_string,knn);
//set all chars to lower case
for(int i=0; i<code_mode_string.size(); i++)
code_mode_string[i] = std::toupper((unsigned char)code_mode_string[i]);
fprintf(stdout,"%s\n",code_mode_string.c_str());
if(code_mode_string=="SC")
code_mode=CODE_MODE_SC;
if(code_mode_string=="BCE")
code_mode=CODE_MODE_BCE;
if(code_mode_string=="BOW")
code_mode=CODE_MODE_BOW;
string name=rf.find("name").asString().c_str();
port_out_img.open(("/"+name+"/img:o").c_str());
port_out_code.open(("/"+name+"/code:o").c_str());
BufferedPort<Image>::useCallback();
}
int main(int argc, char * argv[])
{
Network yarp;
Time::turboBoost();
ResourceFinder rf;
rf.setVerbose(true);
rf.setDefaultConfigFile("pythonInterface.ini"); //overridden by --from parameter
rf.setDefaultContext("eMorphApplication/conf"); //overridden by --context parameter
rf.configure("ICUB_ROOT", argc, argv);
printf("HELP \n");
printf("--rpcport /icub/rpc \n");
printf("--request 'command:help;command:quit;command:set,int:10;command:set,double:10.0 string:Hello ' \n");
// extracting running paramenter
/* get the module name which will form the stem of all module port names */
ConstString moduleName = rf.check("name",
Value("/logSort"),
"module name (string)").asString();
/* get the module name which will form the stem of all module port names */
ConstString externPort = rf.check("rpcport",
Value("null"),
"rpc port name (string)").asString();
/* get the module name which will form the stem of all module port names */
string requestList = (string) rf.check("request",
Value("null"),
"requests list (string)").asString();
//initialisation
size_t foundColon, foundSemicolon, foundComma;
string subpart, typeCommand, valueCommand;
Bottle in;
Bottle bot; //= _pOutPort->prepare();
bot.clear();
printf("Performing request on %s \n", externPort.c_str());
Port outPort;
outPort.open("/pythonInterface/request");
printf("Connecting ports... \n");
Network::connect("/pythonInterface/request", externPort.c_str());
printf("Connection ultimated \n");
// extracting commands
printf("Request list: %s \n", requestList.c_str());
foundSemicolon = requestList.find(';');
while(foundSemicolon!=string::npos) {
foundComma = requestList.find(',');
while(foundComma<foundSemicolon) {
subpart = requestList.substr(0,foundComma);
printf("subpart : %s \n", subpart.c_str());
requestList = requestList.substr(foundComma + 1);
printf("requestList: %s \n", requestList.c_str());
//interpreting the type of request
foundColon = subpart.find(':');
typeCommand = subpart.substr(0,foundColon);
printf(" typeCommand : %s \n", typeCommand.c_str());
if(!strcmp(typeCommand.c_str(),"command")) {
printf(" this is a command \n");
valueCommand = subpart.substr(foundColon + 1);
printf(" valueCommand : %s \n", valueCommand.c_str());
bot.addVocab(fetchCommand(valueCommand));
}
else if(!strcmp(typeCommand.c_str(),"int")) {
printf(" this is the intege %s. \n", valueCommand.c_str());
bot.addInt(atoi(valueCommand.c_str()));
}
else if(!strcmp(typeCommand.c_str(),"string")) {
valueCommand = subpart.substr(foundColon + 1);
printf(" this is the string %s \n", valueCommand.c_str());
bot.addString(valueCommand.c_str());
}
foundComma = requestList.find(',');
foundSemicolon = requestList.find(';');
}
subpart = requestList.substr(0,foundSemicolon);
printf("subpart : %s \n", subpart.c_str());
requestList = requestList.substr(foundSemicolon + 1);
printf("requestList: %s \n", requestList.c_str());
//interpreting the type of request
foundColon = subpart.find(':');
typeCommand = subpart.substr(0,foundColon);
printf(" typeCommand : %s \n", typeCommand.c_str());
if(!strcmp(typeCommand.c_str(),"command")) {
printf(" this is a command \n");
valueCommand = subpart.substr(foundColon + 1);
printf(" valueCommand : %s \n", valueCommand.c_str());
bot.addVocab(fetchCommand(valueCommand));
}
else if(!strcmp(typeCommand.c_str(),"int")) {
valueCommand = subpart.substr(foundColon + 1);
printf(" this is the integer %s \n", valueCommand.c_str());
bot.addInt(atoi(valueCommand.c_str()));
}
else if(!strcmp(typeCommand.c_str(),"string")) {
valueCommand = subpart.substr(foundColon + 1);
printf(" this is the string %s \n", valueCommand.c_str());
bot.addString(valueCommand.c_str());
}
outPort.write(bot,in);
printf("Answer : %s \n", in.toString().c_str());
bot.clear();
foundSemicolon = requestList.find(';');
}
// interpreting commands
// sending commands
bot.addVocab(COMMAND_VOCAB_RIGHT);
bot.addInt(10);
//_pOutPort->Content() = _outBottle;
//closing the module
printf("Closing the module ..... \n");
outPort.close();
return 0;
}
/** @cond doxygenLibsbmlInternal */
void CompModelPlugin::resetPorts()
{
for (unsigned int p=0; p<getNumPorts(); p++) {
Port* port = getPort(p);
SBase* referenced = port->getReferencedElement();
if (port->isSetSBaseRef()) {
port->unsetSBaseRef();
port->unsetIdRef();
port->unsetMetaIdRef();
port->unsetUnitRef();
int type = referenced->getTypeCode();
if (referenced->isSetId() &&
type != SBML_INITIAL_ASSIGNMENT &&
type != SBML_ASSIGNMENT_RULE &&
type != SBML_RATE_RULE &&
type != SBML_EVENT_ASSIGNMENT) {
if (type==SBML_UNIT_DEFINITION) {
port->setUnitRef(referenced->getId());
}
else {
port->setIdRef(referenced->getId());
}
}
else if (referenced->isSetMetaId()) {
port->setMetaIdRef(referenced->getMetaId());
}
else {
stringstream newname;
newname << "auto_port_" << p;
referenced->setMetaId(newname.str());
port->setMetaIdRef(newname.str());
}
}
port->clearReferencedElement();
}
}
virtual void onRead(Image &img)
{
//read at specified rate
if(Time::now()-last_read<rate)
return;
mutex.wait();
if(ipl==NULL || ipl->width!=img.width() || ipl->height!=img.height())
{
if(ipl!=NULL)
{
cvReleaseImage(&ipl);
}
ipl=cvCreateImage(cvSize(img.width(),img.height()),IPL_DEPTH_8U,1);
siftGPU_extractor.setDenseGrid(ipl,grid_step,grid_scale);
}
cvCvtColor((IplImage*)img.getIplImage(),ipl,CV_RGB2GRAY);
//cvSmooth(ipl,ipl);
if(dense)
siftGPU_extractor.extractDenseSift(ipl,&keypoints,&descriptors);
else
siftGPU_extractor.extractSift(ipl,&keypoints,&descriptors);
if(dump_sift)
{
for(int i=0; i<keypoints.size(); i++)
{
for(int j=0; j<128; j++)
fprintf(fout_sift,"%f ",descriptors[i*128+j]);
fprintf(fout_sift,"\n");
}
}
//code the input vector
if(!no_code)
{
vector<Vector> features(keypoints.size());
Vector coding;
for(unsigned int i=0; i<keypoints.size(); i++)
{
features[i].resize(128);
for(unsigned int j=0; j<128; j++)
features[i][j]=descriptors[i*128+j];
}
switch(code_mode)
{
case CODE_MODE_SC:
{
sparse_coder->maxPooling(features,coding,keypoints,pyramidLevels,ipl->width, ipl->height);
break;
}
case CODE_MODE_BCE:
{
sparse_coder->maxPooling(features,coding,keypoints,pyramidLevels,ipl->width, ipl->height);
break;
}
case CODE_MODE_BOW:
{
sparse_coder->avgPooling(features,coding,keypoints,pyramidLevels,ipl->width, ipl->height);
break;
}
}
if(port_out_code.getOutputCount())
{
port_out_code.write(coding);
}
if(port_out_img.getOutputCount())
{
for(unsigned int i=0; i<keypoints.size(); i++)
{
int x = cvRound(keypoints[i].x);
int y = cvRound(keypoints[i].y);
cvCircle(img.getIplImage(),cvPoint(x,y),3,cvScalar(0,0,255),-1);
}
port_out_img.write(img);
}
}
mutex.post();
}
virtual bool configure(ResourceFinder &rf)
{
Time::turboBoost();
fprintf(stderr, "Getting projections\n");
Matrix PiRight;
Bottle b;
b = rf.findGroup("CAMERA_CALIBRATION_RIGHT");
//fprintf(stderr, "CAMERA_CALIBRATION_RIGHT contains: %s\n", b.toString().c_str());
if (getProjectionMatrix(b, PiRight) == 0)
{
fprintf(stderr, "CAMERA_CALIBRATION_RIGHT was missing some params\n");
return false;
}
else
{
fprintf(stderr, "Working with RightProjection \n");
for (int i=0; i < PiRight.rows(); i++)
fprintf(stderr, "%s\n", PiRight.getRow(i).toString().c_str());
}
Matrix PiLeft;
b = rf.findGroup("CAMERA_CALIBRATION_LEFT");
//fprintf(stderr, "CAMERA_CALIBRATION_LEFT contains: %s\n", b.toString().c_str());
if (getProjectionMatrix(b, PiLeft) == 0)
{
fprintf(stderr, "CAMERA_CALIBRATION_LEFT was missing some params\n");
return false;
}
else
{
fprintf(stderr, "Working with LeftProjection \n");
for (int i=0; i < PiLeft.rows(); i++)
fprintf(stderr, "%s\n", PiLeft.getRow(i).toString().c_str());
}
int period=50;
if (rf.check("period"))
period=rf.find("period").asInt();
bool enableKalman=false;
if (rf.check("kalman"))
enableKalman=true;
string ctrlName=rf.find("name").asString().c_str();
string robotName=rf.find("robot").asString().c_str();
fprintf(stderr, "Initializing eT\n");
eT=new eyeTriangulation(rf, PiLeft, PiRight, enableKalman, period, ctrlName, robotName);
Vector xr(3); xr(0)=PiRight(0,2); xr(1)=PiRight(1,2); xr(2)=1.0;
Vector xl(3); xl(0)=PiLeft(0,2); xl(1)=PiLeft(1,2); xl(2)=1.0;
eT->xInit(xl, xr);
if (rf.check("const"))
eT->xSetConstant(xl, xr);
eT->start();
string rpcPortName="/"+ctrlName+"/rpc";
rpcPort.open(rpcPortName.c_str());
attach(rpcPort);
return true;
}
int main(int argc, char *argv[]){
Network yarp;
//Port<Bottle> armPlan;
//Port<Bottle> armPred;
Port armPlan;
Port armPred;
armPlan.open("/randArm/plan");
armPred.open("/randArm/pred");
bool fwCvOn = 0;
fwCvOn = Network::connect("/randArm/plan","/fwdConv:i");
fwCvOn *= Network::connect("/fwdConv:o","/randArm/pred");
if (!fwCvOn){
printf("Please run command:\n ./fwdConv --input /fwdConv:i --output /fwdConv:o");
return 1;
}
const gsl_rng_type *T;
gsl_rng *r;
gsl_rng_env_setup();
T = gsl_rng_default;
r = gsl_rng_alloc(T);
Property params;
params.fromCommand(argc,argv);
if (!params.check("robot")){
fprintf(stderr, "Please specify robot name");
fprintf(stderr, "e.g. --robot icub");
return -1;
}
std::string robotName = params.find("robot").asString().c_str();
std::string remotePorts = "/";
remotePorts += robotName;
remotePorts += "/";
if (params.check("side")){
remotePorts += params.find("side").asString().c_str();
}
else{
remotePorts += "left";
}
remotePorts += "_arm";
std::string localPorts = "/randArm/cmd";
Property options;
options.put("device", "remote_controlboard");
options.put("local", localPorts.c_str());
options.put("remote", remotePorts.c_str());
PolyDriver robotDevice(options);
if (!robotDevice.isValid()){
printf("Device not available. Here are known devices: \n");
printf("%s", Drivers::factory().toString().c_str());
Network::fini();
return 1;
}
IPositionControl *pos;
IEncoders *enc;
bool ok;
ok = robotDevice.view(pos);
ok = ok && robotDevice.view(enc);
if (!ok){
printf("Problems acquiring interfaces\n");
return 0;
}
int nj = 0;
pos->getAxes(&nj);
Vector encoders;
Vector command;
Vector commandCart;
Vector tmp;
encoders.resize(nj);
tmp.resize(nj);
command.resize(nj);
commandCart.resize(nj);
for (int i = 0; i < nj; i++) {
tmp[i] = 25.0;
}
pos->setRefAccelerations(tmp.data());
for (int i = 0; i < nj; i++) {
tmp[i] = 5.0;
pos->setRefSpeed(i, tmp[i]);
}
command = 0;
//set the arm joints to "middle" values
command[0] = -45;
command[1] = 45;
command[2] = 0;
command[3] = 45;
pos->positionMove(command.data());
bool done = false;
while (!done){
pos->checkMotionDone(&done);
Time::delay(0.1);
}
while (true){
tmp = command;
command[0] += 15*(2*gsl_rng_uniform(r)-1);
command[1] += 15*(2*gsl_rng_uniform(r)-1);
command[2] += 15*(2*gsl_rng_uniform(r)-1);
command[3] += 15*(2*gsl_rng_uniform(r)-1);
printf("%.1lf %.1lf %.1lf %.1lf\n", command[0], command[1], command[2], command[3]);
//above 0 doesn't seem to be safe for joint 0
if (command[0] > 0 || command[0] < -90){
command[0] = tmp[0];
}
if (command[1] > 160 || command[1] < -0){
command[1] = tmp[1];
}
if (command[2] > 100 || command[2] < -35){
command[2] = tmp[2];
}
if (command[3] > 100 || command[3] < 10){
command[3] = tmp[3];
}
//use fwd kin to find end effector position
Bottle plan, pred;
for (int i = 0; i < nj; i++){
plan.add(command[i]);
}
armPlan.write(plan);
armPred.read(pred);
for (int i = 0; i < 3; i++){
commandCart[i] = pred.get(i).asDouble();
}
double rad = sqrt(commandCart[0]*commandCart[0]+commandCart[1]*commandCart[1]);
// safety radius back to 30 cm
if (rad > 0.3){
pos->positionMove(command.data());
done = false;
while(!done){
pos->checkMotionDone(&done);
Time::delay(0.1);
}
}
else{
printf("Self collision detected!\n");
}
}
robotDevice.close();
gsl_rng_free(r);
return 0;
}
// This test ensures we don't break the API when it comes to JSON
// representation of tasks.
TEST(HTTPTest, ModelTask)
{
TaskID taskId;
taskId.set_value("t");
SlaveID slaveId;
slaveId.set_value("s");
ExecutorID executorId;
executorId.set_value("t");
FrameworkID frameworkId;
frameworkId.set_value("f");
TaskState state = TASK_RUNNING;
vector<TaskStatus> statuses;
TaskStatus status;
status.mutable_task_id()->CopyFrom(taskId);
status.set_state(state);
status.mutable_slave_id()->CopyFrom(slaveId);
status.mutable_executor_id()->CopyFrom(executorId);
status.set_timestamp(0.0);
statuses.push_back(status);
Labels labels;
labels.add_labels()->CopyFrom(createLabel("ACTION", "port:7987 DENY"));
Ports ports;
Port* port = ports.add_ports();
port->set_number(80);
port->mutable_labels()->CopyFrom(labels);
DiscoveryInfo discovery;
discovery.set_visibility(DiscoveryInfo::CLUSTER);
discovery.set_name("discover");
discovery.mutable_ports()->CopyFrom(ports);
TaskInfo taskInfo;
taskInfo.set_name("task");
taskInfo.mutable_task_id()->CopyFrom(taskId);
taskInfo.mutable_slave_id()->CopyFrom(slaveId);
taskInfo.mutable_command()->set_value("echo hello");
taskInfo.mutable_discovery()->CopyFrom(discovery);
Task task = createTask(taskInfo, state, frameworkId);
task.add_statuses()->CopyFrom(statuses[0]);
JSON::Value object = model(task);
Try<JSON::Value> expected = JSON::parse(
"{"
" \"executor_id\":\"\","
" \"framework_id\":\"f\","
" \"id\":\"t\","
" \"name\":\"task\","
" \"resources\":"
" {"
" \"cpus\":0,"
" \"disk\":0,"
" \"gpus\":0,"
" \"mem\":0"
" },"
" \"slave_id\":\"s\","
" \"state\":\"TASK_RUNNING\","
" \"statuses\":"
" ["
" {"
" \"state\":\"TASK_RUNNING\","
" \"timestamp\":0"
" }"
" ],"
" \"discovery\":"
" {"
" \"name\":\"discover\","
" \"ports\":"
" {"
" \"ports\":"
" ["
" {"
" \"number\":80,"
" \"labels\":"
" {"
" \"labels\":"
" ["
" {"
" \"key\":\"ACTION\","
" \"value\":\"port:7987 DENY\""
" }"
" ]"
" }"
" }"
" ]"
" },"
" \"visibility\":\"CLUSTER\""
" }"
"}");
ASSERT_SOME(expected);
EXPECT_EQ(expected.get(), object);
}
bool Thread::
setC(ticks_t time, ResourceID resID, uint32_t val)
{
Resource *res = getParent().getResourceByID(resID);
if (!res) {
return false;
}
if (val == SETC_INUSE_OFF || val == SETC_INUSE_ON)
return res->setCInUse(*this, val == SETC_INUSE_ON, time);
if (!res->isInUse())
return false;
if (extractBits(val, SETC_MODE_SHIFT, SETC_MODE_SIZE) == SETC_MODE_LONG) {
uint32_t lmode = extractBits(val, SETC_LMODE_SHIFT, SETC_LMODE_SIZE);
uint32_t valField = extractBits(val, SETC_VALUE_SHIFT, SETC_VALUE_SIZE);
switch (lmode) {
default: break;
case SETC_LMODE_PIN_DELAY:
if (res->getType() == RES_TYPE_PORT) {
Port *port = static_cast<Port*>(res);
return port->setPinDelay(*this, valField, time);
}
return false;
case SETC_LMODE_FALL_DELAY:
case SETC_LMODE_RISE_DELAY:
if (res->getType() == RES_TYPE_CLKBLK) {
ClockBlock *clock = static_cast<ClockBlock*>(res);
return clock->setEdgeDelay(*this, getEdgeTypeFromLMode(lmode), valField,
time);
}
return false;
}
}
switch (val) {
default:
internalError(*this, __FILE__, __LINE__); // TODO
case SETC_IE_MODE_EVENT:
case SETC_IE_MODE_INTERRUPT:
{
if (!res->isEventable())
return false;
EventableResource *ER = static_cast<EventableResource *>(res);
ER->setInterruptMode(*this, val == SETC_IE_MODE_INTERRUPT);
break;
}
case SETC_COND_FULL:
case SETC_COND_AFTER:
case SETC_COND_EQ:
case SETC_COND_NEQ:
{
if (!res->setCondition(*this, setCCondition(val), time))
return false;
break;
}
case SETC_RUN_STARTR:
case SETC_RUN_STOPR:
{
if (res->getType() != RES_TYPE_CLKBLK)
return false;
ClockBlock *clock = static_cast<ClockBlock*>(res);
if (val == SETC_RUN_STARTR)
clock->start(*this, time);
else
clock->stop(*this, time);
break;
}
case SETC_MS_MASTER:
case SETC_MS_SLAVE:
if (res->getType() != RES_TYPE_PORT)
return false;
return static_cast<Port*>(res)->setMasterSlave(*this, getMasterSlave(val),
time);
case SETC_BUF_BUFFERS:
case SETC_BUF_NOBUFFERS:
if (res->getType() != RES_TYPE_PORT)
return false;
return static_cast<Port*>(res)->setBuffered(*this, val == SETC_BUF_BUFFERS,
time);
case SETC_RDY_NOREADY:
case SETC_RDY_STROBED:
case SETC_RDY_HANDSHAKE:
if (res->getType() != RES_TYPE_PORT)
return false;
return static_cast<Port*>(res)->setReadyMode(*this, getPortReadyMode(val),
time);
case SETC_PORT_DATAPORT:
case SETC_PORT_CLOCKPORT:
case SETC_PORT_READYPORT:
if (res->getType() != RES_TYPE_PORT)
return false;
return static_cast<Port*>(res)->setPortType(*this, getPortType(val), time);
case SETC_RUN_CLRBUF:
{
if (res->getType() != RES_TYPE_PORT)
return false;
static_cast<Port*>(res)->clearBuf(*this, time);
break;
}
case SETC_INV_INVERT:
case SETC_INV_NOINVERT:
if (res->getType() != RES_TYPE_PORT)
return false;
return static_cast<Port*>(res)->setPortInv(*this, val == SETC_INV_INVERT,
time);
case SETC_SDELAY_NOSDELAY:
case SETC_SDELAY_SDELAY:
{
if (res->getType() != RES_TYPE_PORT)
return false;
Edge::Type samplingEdge = (val == SETC_SDELAY_SDELAY) ? Edge::FALLING :
Edge::RISING;
static_cast<Port*>(res)->setSamplingEdge(*this, samplingEdge, time);
break;
}
}
return true;
}
static status_t
probe_ports()
{
// Try to determine what ports to use. We use the following heuristic:
// * Check for DisplayPort, these can be more or less detected reliably.
// * Check for HDMI, it'll fail on devices not having HDMI for us to fall
// back to DVI.
// * Assume DVI B if no HDMI and no DisplayPort is present, confirmed by
// reading EDID in the IsConnected() call.
// * Check for analog if possible (there's a detection bit on PCH),
// otherwise the assumed presence is confirmed by reading EDID in
// IsConnected().
TRACE("adpa: %08" B_PRIx32 "\n", read32(INTEL_ANALOG_PORT));
TRACE("dova: %08" B_PRIx32 ", dovb: %08" B_PRIx32
", dovc: %08" B_PRIx32 "\n", read32(INTEL_DIGITAL_PORT_A),
read32(INTEL_DIGITAL_PORT_B), read32(INTEL_DIGITAL_PORT_C));
TRACE("lvds: %08" B_PRIx32 "\n", read32(INTEL_DIGITAL_LVDS_PORT));
gInfo->port_count = 0;
for (int i = INTEL_PORT_A; i <= INTEL_PORT_D; i++) {
Port* displayPort = new(std::nothrow) DisplayPort((port_index)i);
if (displayPort == NULL)
return B_NO_MEMORY;
if (displayPort->IsConnected())
gInfo->ports[gInfo->port_count++] = displayPort;
else
delete displayPort;
}
// Digital Display Interface
if (gInfo->shared_info->device_type.HasDDI()) {
for (int i = INTEL_PORT_A; i <= INTEL_PORT_E; i++) {
Port* ddiPort
= new(std::nothrow) DigitalDisplayInterface((port_index)i);
if (ddiPort == NULL)
return B_NO_MEMORY;
if (ddiPort->IsConnected())
gInfo->ports[gInfo->port_count++] = ddiPort;
else
delete ddiPort;
}
}
// Ensure DP_A isn't already taken (or DDI)
if (!has_connected_port((port_index)INTEL_PORT_A, INTEL_PORT_TYPE_ANY)) {
// also always try eDP, it'll also just fail if not applicable
Port* eDPPort = new(std::nothrow) EmbeddedDisplayPort();
if (eDPPort == NULL)
return B_NO_MEMORY;
if (eDPPort->IsConnected())
gInfo->ports[gInfo->port_count++] = eDPPort;
else
delete eDPPort;
}
for (int i = INTEL_PORT_B; i <= INTEL_PORT_D; i++) {
if (has_connected_port((port_index)i, INTEL_PORT_TYPE_ANY)) {
// Ensure port not already claimed by something like DDI
continue;
}
Port* hdmiPort = new(std::nothrow) HDMIPort((port_index)i);
if (hdmiPort == NULL)
return B_NO_MEMORY;
if (hdmiPort->IsConnected())
gInfo->ports[gInfo->port_count++] = hdmiPort;
else
delete hdmiPort;
}
if (!has_connected_port(INTEL_PORT_ANY, INTEL_PORT_TYPE_ANY)) {
// there's neither DisplayPort nor HDMI so far, assume DVI B
Port* dviPort = new(std::nothrow) DigitalPort(INTEL_PORT_B);
if (dviPort == NULL)
return B_NO_MEMORY;
if (dviPort->IsConnected()) {
gInfo->ports[gInfo->port_count++] = dviPort;
gInfo->head_mode |= HEAD_MODE_B_DIGITAL;
} else
delete dviPort;
}
// always try the LVDS port, it'll simply fail if not applicable
Port* lvdsPort = new(std::nothrow) LVDSPort();
if (lvdsPort == NULL)
return B_NO_MEMORY;
if (lvdsPort->IsConnected()) {
gInfo->ports[gInfo->port_count++] = lvdsPort;
gInfo->head_mode |= HEAD_MODE_LVDS_PANEL;
gInfo->head_mode |= HEAD_MODE_B_DIGITAL;
} else
delete lvdsPort;
// then finally always try the analog port
Port* analogPort = new(std::nothrow) AnalogPort();
if (analogPort == NULL)
return B_NO_MEMORY;
if (analogPort->IsConnected()) {
gInfo->ports[gInfo->port_count++] = analogPort;
gInfo->head_mode |= HEAD_MODE_A_ANALOG;
} else
delete analogPort;
if (gInfo->port_count == 0)
return B_ERROR;
return B_OK;
}
bool NetworkBase::write(const Contact& contact,
PortWriter& cmd,
PortReader& reply,
const ContactStyle& style) {
if (!getNameSpace().serverAllocatesPortNumbers()) {
// switch to more up-to-date method
Port port;
port.setAdminMode(style.admin);
port.openFake("network_write");
Contact ec = contact;
if (style.carrier!="") {
ec = ec.addCarrier(style.carrier);
}
if (!port.addOutput(ec)) {
if (!style.quiet) {
ACE_OS::fprintf(stderr, "Cannot make connection to '%s'\n",
ec.toString().c_str());
}
return false;
}
bool ok = port.write(cmd,reply);
return ok;
}
const char *connectionName = "admin";
ConstString name = contact.getName();
const char *targetName = name.c_str(); // use carefully!
Contact address = contact;
if (!address.isValid()) {
address = getNameSpace().queryName(targetName);
}
if (!address.isValid()) {
if (!style.quiet) {
YARP_SPRINTF1(Logger::get(),error,
"cannot find port %s",
targetName);
}
return false;
}
if (style.timeout>0) {
address.setTimeout((float)style.timeout);
}
OutputProtocol *out = Carriers::connect(address);
if (out==NULL) {
if (!style.quiet) {
YARP_SPRINTF1(Logger::get(),error,
"Cannot connect to port %s",
targetName);
}
return false;
}
if (style.timeout>0) {
out->setTimeout(style.timeout);
}
Route r(connectionName,targetName,
(style.carrier!="")?style.carrier.c_str():"text_ack");
out->open(r);
PortCommand pc(0,style.admin?"a":"d");
BufferedConnectionWriter bw(out->getConnection().isTextMode(),
out->getConnection().isBareMode());
bool ok = true;
if (out->getConnection().canEscape()) {
ok = pc.write(bw);
}
if (!ok) {
if (!style.quiet) {
YARP_ERROR(Logger::get(),"could not write to connection");
}
if (out!=NULL) delete out;
return false;
}
ok = cmd.write(bw);
if (!ok) {
if (!style.quiet) {
YARP_ERROR(Logger::get(),"could not write to connection");
}
if (out!=NULL) delete out;
return false;
}
if (style.expectReply) {
bw.setReplyHandler(reply);
}
out->write(bw);
if (out!=NULL) {
delete out;
out = NULL;
}
return true;
}
destRecord
::destRecord(struct in_addr const& addr, Port const& port, u_int8_t ttl, unsigned sessionId,
destRecord* next)
: fNext(next), fGroupEId(addr, port.num(), ttl), fSessionId(sessionId) {
}
//处理setup命令时使用 void RTSPServer::RTSPClientSession::handleCmd_SETUP()
void OnDemandServerMediaSubsession::getStreamParameters(
unsigned clientSessionId,
netAddressBits clientAddress,
Port const& clientRTPPort,
Port const& clientRTCPPort,
int tcpSocketNum,
unsigned char rtpChannelId,
unsigned char rtcpChannelId,
netAddressBits& destinationAddress,
u_int8_t& /*destinationTTL*/,
Boolean& isMulticast,
Port& serverRTPPort,
Port& serverRTCPPort,
void*& streamToken) {
if (destinationAddress == 0) destinationAddress = clientAddress;
struct in_addr destinationAddr; destinationAddr.s_addr = destinationAddress;
isMulticast = False;
if (fLastStreamToken != NULL && fReuseFirstSource) {
// Special case: Rather than creating a new 'StreamState',
// we reuse the one that we've already created:
DEBUG_LOG(INF, "Reuse first source");
serverRTPPort = ((StreamState*)fLastStreamToken)->serverRTPPort();
serverRTCPPort = ((StreamState*)fLastStreamToken)->serverRTCPPort();
++((StreamState*)fLastStreamToken)->referenceCount();
streamToken = fLastStreamToken;
} else {
// Normal case: Create a new media source:
DEBUG_LOG(INF, "Create a new media source");
unsigned streamBitrate;
FramedSource* mediaSource
= createNewStreamSource(clientSessionId, streamBitrate);
// Create 'groupsock' and 'sink' objects for the destination,
// using previously unused server port numbers:
RTPSink* rtpSink;
BasicUDPSink* udpSink;
Groupsock* rtpGroupsock;
Groupsock* rtcpGroupsock;
portNumBits serverPortNum;
if (clientRTCPPort.num() == 0) {
// We're streaming raw UDP (not RTP). Create a single groupsock:
NoReuse dummy; // ensures that we skip over ports that are already in use
for (serverPortNum = fInitialPortNum; ; ++serverPortNum) {
struct in_addr dummyAddr; dummyAddr.s_addr = 0;
serverRTPPort = serverPortNum;
rtpGroupsock = new Groupsock(envir(), dummyAddr, serverRTPPort, 255);
if (rtpGroupsock->socketNum() >= 0) break; // success
}
rtcpGroupsock = NULL;
rtpSink = NULL;
udpSink = BasicUDPSink::createNew(envir(), rtpGroupsock);
} else {
// Normal case: We're streaming RTP (over UDP or TCP). Create a pair of
// groupsocks (RTP and RTCP), with adjacent port numbers (RTP port number even):
NoReuse dummy; // ensures that we skip over ports that are already in use
for (portNumBits serverPortNum = fInitialPortNum; ; serverPortNum += 2) {
struct in_addr dummyAddr; dummyAddr.s_addr = 0;
serverRTPPort = serverPortNum;
rtpGroupsock = new Groupsock(envir(), dummyAddr, serverRTPPort, 255);
if (rtpGroupsock->socketNum() < 0) {
delete rtpGroupsock;
continue; // try again
}
serverRTCPPort = serverPortNum+1;
rtcpGroupsock = new Groupsock(envir(), dummyAddr, serverRTCPPort, 255);
if (rtcpGroupsock->socketNum() < 0) {
delete rtpGroupsock;
delete rtcpGroupsock;
continue; // try again
}
break; // success
}
unsigned char rtpPayloadType = 96 + trackNumber()-1; // if dynamic
rtpSink = createNewRTPSink(rtpGroupsock, rtpPayloadType, mediaSource);
udpSink = NULL;
}
// Turn off the destinations for each groupsock. They'll get set later
// (unless TCP is used instead):
if (rtpGroupsock != NULL) rtpGroupsock->removeAllDestinations();
if (rtcpGroupsock != NULL) rtcpGroupsock->removeAllDestinations();
if (rtpGroupsock != NULL) {
// Try to use a big send buffer for RTP - at least 0.1 second of
// specified bandwidth and at least 50 KB
unsigned rtpBufSize = streamBitrate * 25 / 2; // 1 kbps * 0.1 s = 12.5 bytes
if (rtpBufSize < 50 * 1024) rtpBufSize = 50 * 1024;
increaseSendBufferTo(envir(), rtpGroupsock->socketNum(), rtpBufSize);
}
// Set up the state of the stream. The stream will get started later:
DEBUG_LOG(INF, "Set up the state of the stream. The stream will get started later");
streamToken = fLastStreamToken
= new StreamState(*this, serverRTPPort, serverRTCPPort, rtpSink, udpSink,
streamBitrate, mediaSource,
rtpGroupsock, rtcpGroupsock);
}
// Record these destinations as being for this client session id:
Destinations* destinations;
if (tcpSocketNum < 0)
{ // UDP
DEBUG_LOG(INF, "Create UDP Destinations: destinationAddr=%s, clientRTPPort=%d, clientRTCPPort=%d",
our_inet_ntoa(destinationAddr), ntohs(clientRTPPort.num()), ntohs(clientRTCPPort.num()));
destinations = new Destinations(destinationAddr, clientRTPPort, clientRTCPPort);
}
else
{ // TCP
DEBUG_LOG(INF, "Create TCP Destinations: tcpSocketNum=%d, rtpChannelId=%d, rtcpChannelId=%d",
tcpSocketNum, rtpChannelId, rtcpChannelId);
destinations = new Destinations(tcpSocketNum, rtpChannelId, rtcpChannelId);
}
fDestinationsHashTable->Add((char const*)clientSessionId, destinations);
}
static bool
ExpectXOff(Port &port, OperationEnvironment &env, unsigned timeout_ms)
{
return port.WaitForChar(0x13, env, timeout_ms) == Port::WaitResult::READY;
}
~Helper()
{
clear();
port.close();
}
Pattern* IRWriter::writePattern(Pattern* pattern, map<string, Port*>* ports){
map<string, Port*>::iterator itPort;
Pattern* newPattern = new Pattern();
//Add number of tokens for each ports
map<Port*, ConstantInt*>::iterator itTokens;
map<Port*, ConstantInt*>* numTokens = pattern->getNumTokensMap();
for (itTokens = numTokens->begin(); itTokens != numTokens->end(); itTokens++) {
Port* src = itTokens->first;
itPort = ports->find(src->getName());
// Find destination port
Port* dst;
itPort = ports->find(src->getName());
if (itPort == ports->end()){
dst = new Port(src->getName(), src->getType(), instance);
ports->insert(pair<string, Port*>(src->getName(), dst));
}else{
dst = itPort->second;
dst->setInstance(instance);
}
newPattern->setNumTokens(dst, itTokens->second);
}
//Add variable map for each ports
map<Port*, Variable*>::iterator itVar;
map<Port*, Variable*>* varMap = pattern->getVariableMap();
for (itVar = varMap->begin(); itVar != varMap->end(); itVar++) {
//Get corresponding port in instance
Port* src = itVar->first;
itPort = ports->find(src->getName());
// Find destination port
Port* dst;
itPort = ports->find(src->getName());
if (itPort == ports->end()){
dst = new Port(src->getName(), src->getType(), instance);
ports->insert(pair<string, Port*>(src->getName(), dst));
}else{
dst = itPort->second;
dst->setInstance(instance);
}
// Add variable to pattern
newPattern->setVariable(dst, dst->getPtrVar());
}
return newPattern;
}
int setupStreamSocket(UsageEnvironment& env,
Port port, Boolean makeNonBlocking) {
if (!initializeWinsockIfNecessary()) {
socketErr(env, "Failed to initialize 'winsock': ");
return -1;
}
int newSocket = createSocket(SOCK_STREAM);
if (newSocket < 0) {
socketErr(env, "unable to create stream socket: ");
return newSocket;
}
int reuseFlag = groupsockPriv(env)->reuseFlag;
reclaimGroupsockPriv(env);
if (setsockopt(newSocket, SOL_SOCKET, SO_REUSEADDR,
(const char*)&reuseFlag, sizeof reuseFlag) < 0) {
socketErr(env, "setsockopt(SO_REUSEADDR) error: ");
closeSocket(newSocket);
return -1;
}
// SO_REUSEPORT doesn't really make sense for TCP sockets, so we
// normally don't set them. However, if you really want to do this
// #define REUSE_FOR_TCP
#ifdef REUSE_FOR_TCP
#if defined(__WIN32__) || defined(_WIN32)
// Windoze doesn't properly handle SO_REUSEPORT
#else
#ifdef SO_REUSEPORT
if (setsockopt(newSocket, SOL_SOCKET, SO_REUSEPORT,
(const char*)&reuseFlag, sizeof reuseFlag) < 0) {
socketErr(env, "setsockopt(SO_REUSEPORT) error: ");
closeSocket(newSocket);
return -1;
}
#endif
#endif
#endif
// Note: Windoze requires binding, even if the port number is 0
#if defined(__WIN32__) || defined(_WIN32)
#else
if (port.num() != 0 || ReceivingInterfaceAddr != INADDR_ANY) {
#endif
MAKE_SOCKADDR_IN(name, ReceivingInterfaceAddr, port.num());
if (bind(newSocket, (struct sockaddr*)&name, sizeof name) != 0) {
char tmpBuffer[100];
sprintf(tmpBuffer, "bind() error (port number: %d): ",
ntohs(port.num()));
socketErr(env, tmpBuffer);
closeSocket(newSocket);
return -1;
}
#if defined(__WIN32__) || defined(_WIN32)
#else
}
#endif
if (makeNonBlocking) {
if (!makeSocketNonBlocking(newSocket)) {
socketErr(env, "failed to make non-blocking: ");
closeSocket(newSocket);
return -1;
}
}
return newSocket;
}
// Parse the blif file and build the circuit
void Circuit::parse_blif(string filename)
{
string latch_str= "1 1\n";
ifile.open(filename.c_str());
if (!ifile) {
throw Error("Cannot open file");
}
while (true) {
int type;
string token;
type= get_blif_token(token);
if (type == 2) {
break;
} else if (type == 4) {
if (token == ".inputs" || token == ".outputs") {
int type= 0;
if (token == ".outputs") {
type= 1;
}
while (get_blif_token(token) != 1) {
// create wire
Wire* nwire= new Wire(token);
++num_wires;
sym_table[token]= nwire;
// create instance
string instname = token;
instname += type == 0 ? "_input" : "_output";
Inst* ninst= new Inst(instname, true);
++num_insts;
sym_table[instname]= ninst;
// create port
Port* nport= new Port(token);
++num_ports;
nport->set_inst(ninst);
nport->set_wire(nwire);
if (type == 0) {
// input, connect port as output
ninst->add_output(nport);
nwire->set_driver(nport);
input_wires.push_back(nwire);
} else {
ninst->add_input(nport);
nwire->add_output_port(nport);
output_wires.push_back(nwire);
}
}
} else if (token == ".latch") {
// .latch input output [<type> <control>] [<init-val>]
// only need input and output
string input, output;
get_blif_token(input);
get_blif_token(output);
// Exhaust other token until end-of-line
while (get_blif_token(token) != 1);
// create instance
string inst_name = input + "_" + output + "_latch";
Inst* ninst= new Inst(inst_name, false, true);
++num_insts;
sym_table[inst_name]= ninst;
// handle input port
Port* nport= new Port(input);
++num_ports;
nport->set_inst(ninst);
// create/find wire
Wire* nwire= find_wire_insert(input);
nwire->add_output_port(nport);
output_wires.push_back(nwire);
ninst->add_input(nport);
// handle output port
nport= new Port(output);
++num_ports;
nport->set_inst(ninst);
// create/find wire
nwire = find_wire_insert(output);
if (nwire->get_driver() != 0) {
cout << "Warning, wire " << output
<< " has multipler drivers."<<endl;
}
input_wires.push_back(nwire);
nwire->set_driver(nport);
ninst->add_output(nport);
// latch always has only one input
lib_cell* cell = library->create_libcell(latch_str, 1);
ninst->add_lib_cell(cell);
} else if (token == ".names") {
// .names <in-1> <in-2> ... <in-n> <output>
vector<string> strvec;
int i;
string inst_name;
Inst* ninst;
Port* nport;
Wire* nwire;
while (get_blif_token(token) != 1) {
strvec.push_back(token);
inst_name= inst_name + token + "_";
}
char chr_temp= ifile.get();
ifile.putback(chr_temp);
if (chr_temp == '.') {
cout<<"Warning, instance "<<inst_name<<" has no truth table."<<endl;
if(strvec.size() == 1) {
zero_list.insert(strvec[0]);
nwire = find_wire_insert(strvec[0]);
throw Error("Found a constant 0");
constants_list.push_back(nwire);
}
continue;
}
if(strvec.size() == 1) {
cout << "Warning, constant wire " << inst_name << endl;
one_list.insert(strvec[0]);
nwire= find_wire_insert(strvec[0]);
throw Error("Found a constant 1");
constants_list.push_back(nwire);
continue;
}
// set instance
inst_name= inst_name + "name";
ninst= new Inst(inst_name);
lib_insts.push_back(ninst);
++num_insts;
++num_gates;
sym_table[inst_name]= ninst;
// first n-1 names are inputs
for (i= 0; i < int(strvec.size()) - 1; i++) {
nport= new Port(strvec[i]);
++num_ports;
nwire= find_wire_insert(strvec[i]);
nwire->add_output_port(nport);
ninst->add_input(nport);
nport->set_inst(ninst);
}
// last name is output
int size= strvec.size();
nport= new Port(strvec[size-1]);
++num_ports;
nport->set_inst(ninst);
nwire= find_wire_insert(strvec[size-1]);
ninst->add_output(nport);
if (nwire->get_driver() != 0) {
cout<<"Warning, wire "<<strvec[size - 1]<<" has multiple drivers."<<endl;
}
nwire->set_driver(nport);
// next one should be truth-table
if (get_blif_ttable(token) == 1) {
cout<<"Warning, instance "<<inst_name<<" has no truth table."<<endl;
}
lib_cell* cell = library->create_libcell(token,
ninst->num_inputs());
ninst->add_lib_cell(cell);
} else {
// Not handled. Skip everything until end of line
while (get_blif_token(token) != 1) ;
}
}
}
ifile.close();
sort(output_wires.begin(), output_wires.end(), sort_wire);
sort(input_wires.begin(), input_wires.end(), sort_wire);
}
SBase*
SBaseRef::getReferencedElementFrom(Model* model)
{
SBMLDocument* doc = getSBMLDocument();
if (!hasRequiredAttributes()) {
if (doc) {
string error = "In SBaseRef::getReferencedElementFrom, unable to find referenced element from <" + getElementName() + "> ";
if (isSetId()) {
error += "with ID '" + getId() + "' ";
}
error += "as it does not have the required attributes.";
int en = CompSBaseRefMustReferenceObject;
switch(getTypeCode()) {
case SBML_COMP_REPLACEDBY:
en = CompReplacedByAllowedAttributes;
break;
case SBML_COMP_REPLACEDELEMENT:
en = CompReplacedElementAllowedAttributes;
break;
case SBML_COMP_PORT:
en = CompPortAllowedAttributes;
break;
case SBML_COMP_DELETION:
en = CompDeletionAllowedAttributes;
}
doc->getErrorLog()->logPackageError("comp", en, getPackageVersion(), getLevel(), getVersion(), error, getLine(), getColumn());
}
return NULL;
}
SBase* referent = NULL;
if (isSetPortRef()) {
CompModelPlugin* mplugin = static_cast<CompModelPlugin*>(model->getPlugin(getPrefix()));
Port* port = mplugin->getPort(getPortRef());
if (port==NULL) {
if (doc) {
string error = "In SBaseRef::getReferencedElementFrom, unable to find referenced element from SBase reference ";
if (isSetId()) {
error += "'" + getId() + "' ";
}
error += "as the port it references ('" + getPortRef() +"') could not be found.";
doc->getErrorLog()->logPackageError("comp", CompPortRefMustReferencePort, getPackageVersion(), getLevel(), getVersion(), error, getLine(), getColumn());
}
return NULL;
}
mDirectReference = port;
referent = port->getReferencedElementFrom(model);
}
else if (isSetIdRef()) {
referent = model->getElementBySId(getIdRef());
if (referent == NULL && doc) {
string error = "In SBaseRef::getReferencedElementFrom, unable to find referenced element: no such SId in the model: '" + getIdRef() + "'.";
if (doc->getErrorLog()->contains(UnrequiredPackagePresent)
|| doc->getErrorLog()->contains(RequiredPackagePresent))
{
doc->getErrorLog()->logPackageError("comp",
CompIdRefMayReferenceUnknownPackage, getPackageVersion(),
getLevel(), getVersion(), error, getLine(), getColumn());
}
else
{
doc->getErrorLog()->logPackageError("comp",
CompIdRefMustReferenceObject, getPackageVersion(),
getLevel(), getVersion(), error, getLine(), getColumn());
}
}
}
else if (isSetUnitRef()) {
referent = model->getUnitDefinition(getUnitRef());
if (referent == NULL && doc) {
string error = "In SBaseRef::getReferencedElementFrom, unable to find referenced element: no such Unit in the model: '" + getUnitRef() + "'.";
doc->getErrorLog()->logPackageError("comp", CompUnitRefMustReferenceUnitDef, getPackageVersion(), getLevel(), getVersion(), error, getLine(), getColumn());
}
}
else if (isSetMetaIdRef()) {
referent = model->getElementByMetaId(getMetaIdRef());
if (referent == NULL && doc) {
string error = "In SBaseRef::getReferencedElementFrom, unable to find referenced element: no such metaid in the model: '" + getMetaIdRef() + "'.";
if (doc->getErrorLog()->contains(UnrequiredPackagePresent)
|| doc->getErrorLog()->contains(RequiredPackagePresent))
{
doc->getErrorLog()->logPackageError("comp",
CompIdRefMayReferenceUnknownPackage, getPackageVersion(),
getLevel(), getVersion(), error, getLine(), getColumn());
}
else
{
doc->getErrorLog()->logPackageError("comp",
CompMetaIdRefMustReferenceObject, getPackageVersion(),
getLevel(), getVersion(), error, getLine(), getColumn());
}
}
}
else {
//This is actually possible if the subclass overrides getNumReferents() (which some do). In that case, we just return NULL and let the overriding function find the referent instead.
return NULL;
}
if (referent == NULL) {
//No need to set an error message--one was already set above.
return NULL;
}
if (isSetSBaseRef()) {
//We're drilling into the submodels here, so our referent must be a submodel.
if (referent->getTypeCode() != SBML_COMP_SUBMODEL) {
if (doc) {
string error = "In SBaseRef::getReferencedElementFrom, unable to find referenced element: the element ";
if (referent->isSetId()) {
error += "'" + referent->getId() + "'";
}
else if (referent->isSetMetaId()) {
error += "with the metaid '" + referent->getMetaId() + "'";
}
error += " is not a submodel, and therefore has no subobjects for the child <sBaseRef> to refer to.";
doc->getErrorLog()->logPackageError("comp", CompParentOfSBRefChildMustBeSubmodel, getPackageVersion(), getLevel(), getVersion(), error, getLine(), getColumn());
}
return NULL;
}
Submodel* subm = static_cast<Submodel*>(referent);
if (subm==NULL) {
//Note: should be impossible.
if (doc) {
string error = "In SBaseRef::getReferencedElementFrom, unable to find referenced element: the element ";
if (referent->isSetId()) {
error += "'" + referent->getId() + "' ";
}
else if (referent->isSetMetaId()) {
error += "with the metaid '" + referent->getMetaId() + "' ";
}
error += "claims to be a Submodel, but could not be programmatically turned into one.";
doc->getErrorLog()->logPackageError("comp", CompParentOfSBRefChildMustBeSubmodel, getPackageVersion(), getLevel(), getVersion(), error, getLine(), getColumn());
}
return NULL;
}
Model* inst = subm->getInstantiation();
if (inst==NULL) {
//No need to set an additional error, as 'getInstantiation' will set one itself.
return NULL;
}
//Recursive, so will set its own error messages:
referent = getSBaseRef()->getReferencedElementFrom(inst);
mDirectReference = getSBaseRef()->getDirectReference();
}
return referent;
}
//Gathering resources for initialization
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 from config.ini file
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 from config.ini file
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;
}
//createing YARP ports and connections
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 for graspCalibration
Model *model; action->getGraspModel(model);
if (model != NULL) {
if (!model->isCalibrated()) {
Property prop("(finger all_parallel)");
model->calibrate(prop);
string fileName = rf.getHomeContextPath();
fileName += "/";
fileName += option.find("grasp_model_file").asString().c_str();
ofstream fout;
fout.open(fileName.c_str());
model->toStream(fout);
fout.close();
}
}
return true;
}
void HTTPSink::appendPortNum(UsageEnvironment& env,
Port const& port) {
char tmpBuf[10]; // large enough to hold a port # string
sprintf(tmpBuf, " %d", ntohs(port.num()));
env.appendToResultMsg(tmpBuf);
}
/// TODO: This code can be simplified. Make a PortContainer class.
/// Plugin and Patch can inherit from PortContainer. We can have a
/// pseudo-PortContainer here to represent the input and output ports
/// Then, all adds/removals will be symetric
/// ^^^: Or, just wrap Plugin and BackendPorts with a class that acts this way
void FxLine::addPlugin(const PluginInfoPtr info, int pos)
{
int idx = pos + 1;
int pluginCnt = m_entries.length() - 2;
// Check for proper position value. TODO: Report error, not fatal
Q_ASSERT(pos <= pluginCnt);
// Create the plugin. TODO: Report error, not fatal
Plugin *plugin = info->createPlugin(48000);
Q_ASSERT(plugin);
plugin->activate(Engine::bufferProvider());
m_parent.add(plugin);
// Verify number of ports. TODO: Report error, not fatal
Q_ASSERT(plugin->audioInputCount() == 2);
Q_ASSERT(plugin->audioOutputCount() == 2);
// Collect ports.
Entry entry;
entry.plugin = plugin;
collectPorts(plugin, &entry.inputPorts, &entry.outputPorts);
Q_ASSERT(entry.inputPorts.length() == 2);
Q_ASSERT(entry.outputPorts.length() == 2);
/*
if (m_plugins.length() == 0) {
// If there are no plugins, we disconnect the backend ports from each
// other. Then, we connect the plugin in-between the backend ports
// TODO: Bring back once backend ports have patch?
// m_inPorts[0]->disconnect(m_outPorts[0]);
// m_inPorts[1]->disconnect(m_outPorts[1]);
m_inPorts[0]->connect(pluginIn.at(0));
m_inPorts[1]->connect(pluginIn.at(1));
m_outPorts[0]->connect(pluginOut.at(0));
m_outPorts[1]->connect(pluginOut.at(1));
}
else if (pos < m_plugins.length()) {
*/
// At this point, there is at least one plugin already in the line, and we
// are trying to insert the new plugin before another one.
Entry producer = m_entries.value(idx-1);
Entry consumer = m_entries.value(idx);
qWarning() << "FX LINE ADD producer port count:" << producer.outputPorts.count();
for (int i=0; i<producer.outputPorts.count(); ++i) {
Port *producerPort = producer.outputPorts.at(i);
Port *consumerPort = consumer.inputPorts.at(i);
// Work around:
if (producerPort->parentPatch() == NULL && consumerPort->parentPatch() == NULL) {
qWarning("Probably disconnecting two backend-ports. Ignoring");
}
else {
producerPort->disconnect(consumerPort);
}
qWarning() << "FX: Connecting: " << producerPort->name() << " TO " << entry.inputPorts.at(i)->name();
qWarning() << "FX: and Connecting: " << consumerPort->name() << " TO " << entry.outputPorts.at(i)->name();
producerPort->connect(entry.inputPorts.at(i));
consumerPort->connect(entry.outputPorts.at(i));
}
m_entries.insert(idx, entry);
}
bool AwareTouch::configure(ResourceFinder &rf)
{
// Defining module
string moduleName = rf.check("name", Value("awareTouch")).asString().c_str(); // Check name of the module
setName(moduleName.c_str()); // Assign this name for ports
cout<< "|| Starting config "<< moduleName <<endl;
printf("Naming the module \n");
string robot=rf.check("robot",Value("icubSim")).asString().c_str(); //type of robot
cout<< "|| Robot :"<< robot <<endl;
// port for skin data
string rpcName="/" ;
rpcName += getName ( rf.check("inPort",Value("/skin_contacts:i")).asString() ); //input port name
// port for output events
string eventsName="/" ;
eventsName += getName ( rf.check("eventPort",Value("/events:o")).asString() ); //events port name
cout<< "|| Opening port :"<< eventsName <<endl;
eventsPort.open(eventsName.c_str());
string skinManagerName = rf.check("skinManagerPort", Value("/skinManager/skin_events:o")).asString().c_str(); // Check name of the module
string opcName = rf.check("opcName", Value("OPC")).asString().c_str();
//Generating a copy of the world from OPC
world = new OPCClient("OPCTouch");
while(!world->connect(opcName)) {
cout<< "Trying to connect OPC Server"<<endl;
}
cout<< "|| Connected OPC :" <<endl;
// Generating type of gestures
cout<< "|| Reading files of gesture types ... :" <<endl;
Bottle * gestureTypes=rf.find("gestureTypes").asList();
cout<< "|| Gesture types ("<< gestureTypes->size()<<") "<<endl;
//Populating the world with gestures and subjects
gestureSet.clear();
string gestureStr;
for (int iGesture=0; iGesture<gestureTypes->size(); iGesture++) {
gestureStr=(gestureTypes->get(iGesture).asString().c_str() );
gestureSet.push_back(gestureStr);
cout<<gestureStr<<endl;
world->addOrRetrieveEntity<Adjective>(gestureStr);
}
world->addOrRetrieveEntity<Agent>("icub");
touchLocation = world->addOrRetrieveEntity<Object>("touchLocation");
touchLocation->m_present = 0.0;
world->commit(touchLocation);
world->addOrRetrieveEntity<Action>("is");
world->addOrRetrieveEntity<Adjective>("none");
// holding time in OPC
recordingPeriod=rf.check("recordingPeriod",Value(3.0)).asDouble(); //type of robot
cout << "|| Recording Period is " << recordingPeriod << endl ;
string pathG(rf.getHomeContextPath().c_str());
cout<< "|| Creating Touch Thread:" <<endl;
estimationThread= new TouchEstimationThread(skinManagerName, rpcName, pathG, gestureSet, 50);
cout<< "|| Starting Touch :" <<endl;
estimationThread->start();
cout<< "|| Started Touch :" <<endl;
return true;
}
int main(int argc, char *argv[])
{
// Object construction
Network yarp;
ConstString bottlestring;
Port trollCmdPort;
Port fumblyRpcPort;
Port learnRpcPort;
BufferedPort<Bottle> cstuffPort;
BufferedPort<Bottle> learnPort;
Bottle trollCmdBottle, fumblyRpcBottleO, fumblyRpcBottleI, learnRpcBottleI, learnRpcBottleO;
Bottle& learnerBottle = learnPort.prepare();
int cmd, objectIndex;
bool connected;
VisionReader vs;
double objX, objY, objZ, objDX, objDY, objDZ;
// Object initialization
trollCmdPort.open("/iFumble/iconTroll/rpc");
fumblyRpcPort.open("/iFumble/iconTroll/toFumblyRpc");
cstuffPort.open("/iFumble/iconTroll/toCStuff");
learnPort.open("/iFumble/iconTroll/toLearnStuff");
learnRpcPort.open("/iFumble/iconTroll/toLearnRpc");
vs.open("/iFumble/iconTroll/fromVS");
connected = false;
objDX = objDY = objDZ = objectIndex = 0;
objX = -0.25;
objY = 0;
objZ = 0.20;
// Main RPC loop
std::cout<<"Module iconTroll ARE GO!"<<std::endl;
std::cout<<"Waiting for a command (calibrate, explore, golf, quit)..."<<std::endl;
do
{
// Get a command (from "/iFumble/iconTroll/rpc")
trollCmdBottle.clear();
trollCmdPort.read(trollCmdBottle);
cmd = trollCmdBottle.get(0).asVocab();
switch (cmd)
{
case CMD_CALI:
{
std::cout<<"Calibrating..."<<std::endl;
fumblyRpcBottleO.clear();
fumblyRpcBottleI.clear();
fumblyRpcBottleO.addVocab(CMD_CALI);
fumblyRpcPort.write(fumblyRpcBottleO,fumblyRpcBottleI);
std::cout<<"REC: "<< fumblyRpcBottleI.toString() << std::endl;
break;
}
case CMD_VISI:
{
std::vector <int> currentObjects = vs.getCurrObjectIndices();
for(int i=0;i<currentObjects.size();i++){
double xx,yy,zz;
vs.getCoords(i, xx, yy, zz);
std::cout<<"VISION PROVIDES LABEL "<<vs.getObjectLabel(i).c_str()<<" x="<<xx<<" y="<<yy<<" z="<<zz<<std::endl;
}
}
case CMD_EXPL:
{
std::cout<<"Exploring..."<<std::endl;
// Get number of trials
int trials = 1;
if (trollCmdBottle.size() > 1)
trials = trollCmdBottle.get(1).asInt();
// For each trial
for (int i = 0; i < trials; i++)
{
double thetai,thetaf;
do{
thetai=2*M_PI*((double)rand())/RAND_MAX;
}while( (thetai) >=5*M_PI/4 && (thetai)<=7*M_PI/4 );
do{
thetaf=thetai+(0.2)*M_PI*((double)rand())/RAND_MAX-0.1;
}while( (thetaf) >=5*M_PI/4 && (thetaf)<=7*M_PI/4 );
double execTime=0.1+2*((double)rand())/RAND_MAX;
std::cout<<" - trial " << i+1 << std::endl;
// Read objects in scene
std::vector <int> currentObjects = vs.getCurrObjectIndices();
// Choose an object
objectIndex = currentObjects[rand()%currentObjects.size()];
// Get object coordinates from vision
vs.getCoords(objectIndex, objX, objY, objZ);
std::cout<<"REC(V): " << objX << " " << objY << " " << objZ << std::endl;
// Select an action (at random or based on learner)
fumblyRpcBottleO.clear();
fumblyRpcBottleO.addVocab(CMD_FIDL);
fumblyRpcBottleO.addDouble(objX); // x (double) from vision [m]
fumblyRpcBottleO.addDouble(objY); // y (double) from vision [m]
fumblyRpcBottleO.addDouble(objZ); // z (double) from vision (given by table height) [m]
fumblyRpcBottleO.addDouble(thetai); // theta (double): the hand angle used initially (orientation given by robot) [radians]
fumblyRpcBottleO.addDouble(0.05); // rstart (double): the initial hand distance to object [m]
fumblyRpcBottleO.addDouble(thetaf); // ftheta (double): the final hand angle (orientation given by robot) [radians]
fumblyRpcBottleO.addDouble(0.05); // rstop (double): the final hand distance relative to the initial object position [m]
fumblyRpcBottleO.addDouble(execTime); // execTime (double): the reference velocity [s]
// ...
//5*pi/4 to 7*pi/4.
/*
* fidl (vocab): VOCAB4('f','i','d','l')
* x (double): target good ol' x [m] (provided by eye2world)
* y (double): target good ol' y [m] (provided by eye2world)
* z (double): target good ol' z [m] (provided by the table height in fact)
* theta (double): the hand angle used initially (orientation given by robot) [radians]
* rstart (double): the initial hand distance to object [m]
* ftheta (double): the final hand angle (orientation given by robot) [radians]
* rstop (double): the final hand distance relative to the initial object position [m]
* execTime (double): the reference velocity [s]
*/
// Execute action
fumblyRpcBottleI.clear();
fumblyRpcPort.write(fumblyRpcBottleO,fumblyRpcBottleI); // Wait here until action is finished?
std::cout<<"REC(F): "<< fumblyRpcBottleI.toString() << std::endl;
// Get (or wait for?) feedback (consequences) from vision
Time::delay(2.0);
objDX = objX;
objDY = objY;
objDZ = objZ;
vs.getCoords(objectIndex, objX, objY, objZ);
objDX = objX - objDX;
objDY = objY - objDY;
objDZ = objZ - objDZ;
// Send data (action and consequences) to learner
learnerBottle.clear();
learnerBottle.addString("DATAPOINT");
//learnerBottle.addString(vs.getObjectLabel(objectIndex).c_str()); // object_label
learnerBottle.addString("0"); // object_label
learnerBottle.addDouble(thetai); // theta
learnerBottle.addDouble(thetaf); // final theta
learnerBottle.addDouble(execTime); // execution time (velocity)
learnerBottle.addDouble(objDX); // x_delta
learnerBottle.addDouble(objDY); // y_delta
learnPort.write();
//others --> category acion--> initial_theta, final_theta, execution_time/velocity, from vision--> x_delta, y_delta
}
break;
}
case CMD_GOLF:
{
std::cout<<"Golfing..."<<std::endl;
// Get number of trials
int trials = 1;
if (trollCmdBottle.size() > 1)
trials = trollCmdBottle.get(1).asInt();
// For each trial
for (int i = 0; i < trials; i++)
{
std::cout<<" - trial " << i+1 << std::endl;
// Locate goal location (using vision?)
const double hardx = -0.2;
const double hardy = 0.0;
double objx,objy,objz;
// Select best action to achieve goal (using Learner)
//vs.getCoords(int index,double &x,double &y,double &z);
vs.getCoords(0,objx,objy,objz);
objx=hardx-objx;
objy=hardy-objy;
learnRpcBottleI.clear();
learnRpcBottleO.clear();
learnRpcBottleO.addString("INFER");
learnRpcBottleO.addString("0");
learnRpcBottleO.addInt(2);
learnRpcBottleO.addInt(3);
learnRpcBottleO.addInt(4);
learnRpcBottleO.addDouble(objx);
learnRpcBottleO.addDouble(objy);
learnRpcPort.write(learnRpcBottleO,learnRpcBottleI); // Wait here until action is finished?
// Execute action
fumblyRpcPort.write(fumblyRpcBottleO,fumblyRpcBottleI); // Wait here until action is finished?
// Repeat...?
}
break;
}
}
} while (cmd != CMD_QUIT);
std::cout<<"Terminating iconTroll uplink..."<<std::endl;
trollCmdPort.close();
fumblyRpcPort.close();
cstuffPort.close();
learnPort.close();
return 0;
}
