void partMover::home_click(GtkButton *button, gtkClassData* currentClassData)
{
partMover *currentPart = currentClassData->partPointer;
int * joint = currentClassData->indexPointer;
IPositionControl *ipos = currentPart->pos;
IEncoders *iiencs = currentPart->iencs;
IAmplifierControl *iamp = currentPart->amp;
IPidControl *ipid = currentPart->pid;
IControlCalibration2 *ical = currentPart->cal;
int NUMBER_OF_JOINTS;
ipos->getAxes(&NUMBER_OF_JOINTS);
//fprintf(stderr, "Retrieving finder \n");
ResourceFinder *fnd = currentPart->finder;
//fprintf(stderr, "Retrieved finder: %p \n", fnd);
char buffer1[800];
char buffer2[800];
strcpy(buffer1, currentPart->partLabel);
strcpy(buffer2, strcat(buffer1, "_zero"));
//fprintf(stderr, "Finder retrieved %s\n", buffer2);
if (!fnd->findGroup(buffer2).isNull() && !fnd->isNull())
{
//fprintf(stderr, "Home group was not empty \n");
bool ok = true;
Bottle xtmp;
xtmp = fnd->findGroup(buffer2).findGroup("PositionZero");
ok = ok && (xtmp.size() == NUMBER_OF_JOINTS+1);
double positionZero = xtmp.get(*joint+1).asDouble();
//fprintf(stderr, "%f\n", positionZero);
xtmp = fnd->findGroup(buffer2).findGroup("VelocityZero");
//fprintf(stderr, "VALUE VEL is %d \n", fnd->findGroup(buffer2).find("VelocityZero").toString().c_str());
ok = ok && (xtmp.size() == NUMBER_OF_JOINTS+1);
double velocityZero = xtmp.get(*joint+1).asDouble();
//fprintf(stderr, "%f\n", velocityZero);
if(!ok)
dialog_message(GTK_MESSAGE_ERROR,(char *) "Check the number of entries in the group", buffer2, true);
else
{
ipos->setRefSpeed(*joint, velocityZero);
ipos->positionMove(*joint, positionZero);
}
}
else
{
// currentPart->dialog_message(GTK_MESSAGE_ERROR,"No calib file found", strcat("Define a suitable ", strcat(currentPart->partLabel, "Calib")), true);
dialog_message(GTK_MESSAGE_ERROR,(char *) "No zero group found in the supplied file. Define a suitable", buffer2, true);
}
return;
}
void fingerDetector::setModel(Bottle q_0, Bottle q_1, double m, double M, double t, double T)
{
q0.resize(q_0.size());
q1.resize(q_1.size());
for(int i =0; i < q_0.size(); i++)
q0(i) = q_0.get(i).asDouble();
for(int i =0; i < q_1.size(); i++)
q1(i) = q_1.get(i).asDouble();
min = m;
max = M;
minT = t;
maxT = T;
}
bool embObjMultiEnc::fromConfig(yarp::os::Searchable &_config)
{
yDebug()<< "configurazione: ";;
yDebug() << _config.toString();
Bottle general = _config.findGroup("JOINTS");
if(general.isNull())
{
yError() << "embObjMultiEnc cannot find general group";
return false;
}
Bottle jointsbottle = general.findGroup("listofjoints");
if (jointsbottle.isNull())
{
yError() << "embObjMultiEnc cannot find listofjoints param";
return false;
}
Bottle encsbottle = general.findGroup("encoderConversionFactor");
if (encsbottle.isNull())
{
yError() << "embObjMultiEnc cannot find encoderConversionFactor param";
return false;
}
//jointsbottle contains: "listofjoints 0 1 2 3. So the num of joints is jointsbottle->size() -1 "
numofjoints = jointsbottle.size() -1;
listofjoints.clear();
for (int i = 1; i < jointsbottle.size(); i++) listofjoints.push_back(jointsbottle.get(i).asInt());
yDebug()<< " embObjMultiEnc List of joints: " << numofjoints;
for(int i=0; i<numofjoints; i++) yDebug() << "pos="<< i << "val="<< listofjoints[i];
analogdata.resize(numofencperjoint*numofjoints, 0.0);
encoderConversionFactor.resize(numofencperjoint*numofjoints, 1.0);
if (numofencperjoint*numofjoints!=encsbottle.size()-1)
{
yError() << "embObjMultiEnc invalid size of encoderConversionFactor param";
return false;
}
for (int i=0; i<encsbottle.size()-1; i++)
{
encoderConversionFactor[i]=encsbottle.get(i+1).asDouble();
}
return true;
}
virtual ArActionDesired *fire (ArActionDesired currentDesired)
{
Bottle SoundBottle;
if(Mycopyofmodule->GetBottleData("SOUNDin",&SoundBottle,SamgarModule::NoStep)==true)
{
char tune[40];
for(int uu = 0;uu<SoundBottle.size();uu++)
{
tune[uu]=SoundBottle.get(uu).asInt();
}
robot.comStrN(ArCommands::SAY,tune,SoundBottle.size());
}
return &myDesired;
}
void toXmlRpcValue(Value& vin, XmlRpcValue& vout) {
if (vin.isInt()) {
vout = vin.asInt();
} else if (vin.isDouble()) {
vout = vin.asDouble();
} else if (vin.isString()) {
vout = vin.asString();
} else if (vin.isVocab()) {
vout = ConstString("[") + vin.toString() + "]";
} else if (vin.isList()) {
Bottle *bot = vin.asList();
bool struc = true;
int offset = 0;
ConstString tag = bot->get(0).asString();
if (tag=="list") {
struc = false;
offset = 1;
} else if (tag=="dict") {
struc = true;
offset = 1;
} else {
// auto-detect
for (int i=0; i<bot->size(); i++) {
Value& vi = bot->get(i);
if (!vi.isList()) {
struc = false;
break;
}
if (vi.asList()->size()!=2) {
struc = false;
break;
}
}
}
if (struc) {
vout = XmlRpcValue();
for (int i=offset; i<bot->size(); i++) {
Bottle *boti = bot->get(i).asList();
XmlRpcValue& vouti=vout[boti->get(0).toString()]=XmlRpcValue();
toXmlRpcValue(boti->get(1),vouti);
}
} else {
vout = XmlRpcValue();
for (int i=offset; i<bot->size(); i++) {
XmlRpcValue& vouti = vout[i] = XmlRpcValue();
toXmlRpcValue(bot->get(i),vouti);
}
}
}
}
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);
}
void Manager::processMotionPoints(Bottle &b)
{
// fprintf(stdout,"create mat\n");
//create MAT image
cv::Mat imgMat(Size(320,240),CV_8UC3);
cv::Mat imgClean(Size(320,240),CV_8UC3);
//vector<Point> points;
imgMat = Scalar::all(0);
imgClean = Scalar::all(255);
// fprintf(stdout,"filling up the data\n");
for (int x=1; x<b.size(); x++)
{
Point pt;
pt.x = b.get(x).asList()->get(0).asInt();
pt.y = b.get(x).asList()->get(1).asInt();
imgMat.at<cv::Vec3b>(pt.y,pt.x)[0] = 255 ;
imgMat.at<cv::Vec3b>(pt.y,pt.x)[1] = 0 ;
imgMat.at<cv::Vec3b>(pt.y,pt.x)[2] = 0 ;
imgClean.at<cv::Vec3b>(pt.y,pt.x)[0] = 255 ;
imgClean.at<cv::Vec3b>(pt.y,pt.x)[1] = 0 ;
imgClean.at<cv::Vec3b>(pt.y,pt.x)[2] = 0 ;
}
//imgClean = imgMat;
int n = 10;
int an = n > 0 ? n : -n;
int element_shape = MORPH_RECT;
Mat element = getStructuringElement(element_shape, Size(an*2+1, an*2+1), Point(an, an) );
morphologyEx(imgMat, imgMat, CV_MOP_CLOSE, element);
Bottle data;
//fprintf(stdout,"before process image\n");
data = processImage(b, imgMat, imgClean, lineDetails); //image analisis and bottle cleaning
//fprintf(stdout,"before process blobs\n");
if (data.size() > 0)
processBlobs(data, imgClean, lineDetails); // kmeans
ImageOf<PixelRgb> outImg;// = new ImageOf<PixelRgb>;
//fprintf(stdout,"done1 with data %d %d\n", imgClean.cols, imgClean.rows);
outImg.resize( imgClean.cols, imgClean.rows );
IplImage ipl_img = imgClean;
cvCopyImage(&ipl_img, (IplImage*)outImg.getIplImage());
imgOutPort.prepare() = outImg;
imgOutPort.write();
//delete[] outImg;
//fprintf(stdout,"ok\n");
}
/* Connect the opc client to an OPC */
Bottle opcManager::connect(Bottle bInput)
{
Bottle bOutput;
if (bInput.size() != 2) {
bOutput.addString("Error in connect, wrong number of input");
}
if (!bInput.get(1).isString()){
bOutput.addString("Error in connect, wrong format of input");
}
realOPC = new OPCClient(moduleName.c_str());
int iTry = 0;
while (!realOPC->isConnected())
{
cout << "Connecting to realOPC..." << realOPC->connect("OPC") << endl;
Time::delay(0.5);
iTry++;
if (iTry > 20)
{
bOutput.addString("Connection failed, please check your port");
return bOutput;
}
}
realOPC->update();
bOutput.addString("Connection done");
return bOutput;
}
bool respond(const Bottle& command, Bottle& reply) {
//handle information requests
string msg(command.get(0).asString().c_str());
if (msg == "rate") {
if (command.size() < 2) {
reply.add(-1);
}
else {
int newrate = command.get(1).asInt();
bool rssuc;
if (newrate > 0) {
rssuc = thr->setRate(newrate);
if (rssuc)
reply.add(1);
else
reply.add(-1);
}
}
}
else {
reply.add(-1);
}
return true;
}
void OpenNI2SkeletonData::storeData(Bottle& b){
Bottle *list;
int userID;
UserSkeleton *tuserSkeleton;
if(b.get(0).isString()){
userID = b.get(1).asInt();
string vocab = b.get(0).asString().c_str();
if(vocab.compare("CALIBRATING FOR USER") == 0){
userSkeleton[userID-1].skeletonState = nite::SKELETON_CALIBRATING;
}
}
else if(b.get(0).isList()){
list = b.get(0).asList();
userID = list->get(1).asInt();
userSkeleton[userID-1].skeletonState = nite::SKELETON_TRACKED;//USER STATUS
tuserSkeleton = &(userSkeleton[userID-1]);
int jointIndex = 0;
for(int i = 1; i < b.size(); i+=6){
list = b.get(i+1).asList();// position elements
tuserSkeleton->skeletonPointsPos[jointIndex][0] = list->get(0).asDouble();
tuserSkeleton->skeletonPointsPos[jointIndex][1] = list->get(1).asDouble();
tuserSkeleton->skeletonPointsPos[jointIndex][2] = list->get(2).asDouble();
tuserSkeleton->skeletonPosConf[jointIndex] = b.get(i+2).asDouble();
list = b.get(i+4).asList();// orientation elements
tuserSkeleton->skeletonPointsOri[jointIndex][0] = list->get(0).asDouble();
tuserSkeleton->skeletonPointsOri[jointIndex][1] = list->get(1).asDouble();
tuserSkeleton->skeletonPointsOri[jointIndex][2] = list->get(2).asDouble();
tuserSkeleton->skeletonPointsOri[jointIndex][3] = list->get(3).asDouble();
tuserSkeleton->skeletonOriConf[jointIndex] = b.get(i+5).asDouble();
jointIndex++;
}
}
}
bool utManagerThread::getPointFromStereo()
{
Bottle cmdSFM;
Bottle respSFM;
cmdSFM.clear();
respSFM.clear();
cmdSFM.addString("Root");
cmdSFM.addInt(int(templatePFTrackerPos(0)));
cmdSFM.addInt(int(templatePFTrackerPos(1)));
SFMrpcPort.write(cmdSFM, respSFM);
// Read the 3D coords and compute the distance to the set reference frame origin
if (respSFM.size() == 3)
{
Vector SFMtmp(3,0.0);
SFMtmp(0) = respSFM.get(0).asDouble(); // Get the X coordinate
SFMtmp(1) = respSFM.get(1).asDouble(); // Get the Y coordinate
SFMtmp(2) = respSFM.get(2).asDouble(); // Get the Z coordinate
if (SFMtmp(0) == 0.0 && SFMtmp(1) == 0.0 && SFMtmp(2) == 0.0)
{
return false;
}
SFMPos = SFMtmp;
return true;
}
return false;
}
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
bool iCub::skinForceControl::identifyCommand(const Bottle &commandBot, const string *cmdList, unsigned int cmdListSize, unsigned int &cmdId, Bottle ¶ms)
{
for(unsigned int i=0; i<cmdListSize; i++){
stringstream stream(cmdList[i]);
string word;
int wordCounter=0;
bool found = true;
while(stream>>word){
if (commandBot.get(wordCounter).asString() != word.c_str()){
found=false;
break;
}
//printf("%s = %s\n", commandBot.get(wordCounter).asString().c_str(), word.c_str());
wordCounter++;
}
if(found){
cmdId = i;
for(int k=wordCounter; k<commandBot.size(); k++)
params.add(commandBot.get(k));
return true;
}
}
return false;
}
bool SerialDeviceDriver::send(const Bottle& msg)
{
if (msg.size() > 0) {
int message_size = msg.get(0).asString().length();
if (message_size > 0) {
if (verbose)
{
yDebug("Sending string: %s", msg.get(0).asString().c_str());
}
// Write message to the serial device
ssize_t bytes_written = _serial_dev.send_n((void *) msg.get(0).asString().c_str(), message_size);
if (bytes_written == -1) {
ACE_ERROR((LM_ERROR, ACE_TEXT ("%p\n"), ACE_TEXT ("send")));
return false;
}
} else {
if (verbose) yDebug("The input command bottle contains an empty string. \n");
return false;
}
} else {
if (verbose) yDebug("The input command bottle is empty. \n");
return false;
}
return true;
}
string toString()
{
ostringstream stream;
stream<<"["<<part<<"]"<<endl;
stream<<"device "<<device<<endl;
for (int i=0; i<rJoints.size(); i++)
{
int j=rJoints.get(i).asInt();
PidData &pid=pids[j];
Property prop;
prop.put("Kp",pid.Kp);
prop.put("Ki",pid.Ki);
prop.put("Kd",pid.Kd);
prop.put("scale",pid.scale);
prop.put("st_up",pid.st_up);
prop.put("st_down",pid.st_down);
prop.put("encs_ratio",pid.encs_ratio);
prop.put("status",pid.status==download?"download":"upload");
stream<<"joint_"<<j<<" ";
stream<<prop.toString().c_str()<<endl;
}
return stream.str();
}
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");
}
void ICubClient::LoadChoregraphies(yarp::os::ResourceFinder &rf)
{
choregraphiesKnown.clear();
int posCount = rf.check("choregraphiesCount", yarp::os::Value(0)).asInt();
cout<<"Loading Choregraphies: "<<endl;
for (int i = 0; i < posCount; i++)
{
std::stringstream ss;
ss<<"chore_" << i;
Bottle postureGroup = rf.findGroup(ss.str().c_str());
std::string name = postureGroup.find("name").asString().c_str();
std::cout<<"\t"<<name<<std::endl;
Bottle* sequence = postureGroup.find("sequence").asList();
std::list< std::pair<std::string, double> > seq;
for(int s=0; s<sequence->size(); s++)
{
Bottle* element = sequence->get(s).asList();
std::string elementName = element->get(0).asString().c_str();
double elementTime = element->get(1).asDouble();
seq.push_back(std::pair<std::string,double>(elementName,elementTime));
//std::cout<<"\t \t"<<elementName<< "\t" << elementTime << std::endl;
}
choregraphiesKnown[name] = seq;
}
}
static bool matchCarrier(const Bytes *header, Bottle& code) {
int at = 0;
bool success = true;
bool done = false;
for (int i=0; i<code.size() && !done; i++) {
Value& v = code.get(i);
if (v.isString()) {
ConstString str = v.asString();
for (int j=0; j<(int)str.length(); j++) {
if ((int)header->length()<=at) {
success = false;
done = true;
break;
}
if (str[j] != header->get()[at]) {
success = false;
done = true;
break;
}
at++;
}
} else {
at++;
}
}
return success;
}
//********************************************
bool read(ConnectionReader &connection)
{
Bottle input;
input.read(connection);
if (input.size()>=7)
{
string arm=input.get(0).asString().c_str();
transform(arm.begin(),arm.end(),arm.begin(),::tolower);
mutex.lock();
map<string,Data>::iterator it=data.find(arm);
if (it!=data.end())
{
Data &d=it->second;
d.point[0]=input.get(1).asDouble();
d.point[1]=input.get(2).asDouble();
d.point[2]=input.get(3).asDouble();
d.dir[0]=input.get(4).asDouble();
d.dir[1]=input.get(5).asDouble();
d.dir[2]=input.get(6).asDouble();
d.persistence=PPS_AVOIDANCE_PERSISTENCE;
d.timeout=PPS_AVOIDANCE_TIMEOUT;
}
mutex.unlock();
}
return true;
}
bool respond(const Bottle &command, Bottle &reply)
{
int cmd0=command.get(0).asVocab();
if (cmd0==Vocab::encode("stat"))
{
reply.addString(speaker.isSpeaking()?"speaking":"quiet");
return true;
}
if (command.size()>1)
{
int cmd1=command.get(1).asVocab();
if (cmd1==Vocab::encode("opt"))
{
if (cmd0==Vocab::encode("get"))
{
reply.addString(speaker.get_package_options().c_str());
return true;
}
if (cmd0==Vocab::encode("set"))
{
string cmd2=command.get(2).asString().c_str();
speaker.set_package_options(cmd2);
reply.addString("ack");
return true;
}
}
}
return RFModule::respond(command,reply);
}
void Bottle::append(const Bottle& alt)
{
implementation->edit();
for (size_t i = 0; i < alt.size(); i++) {
add(alt.get(i));
}
}
vector<Vector> YarpAdapter::readKinectInput() {
Bottle* in = inPort[KINECT_IN].read(false);
if (in == NULL) {
return oldKinectData;
}
int isize = in->size();
// read body count
bodyCount = in->get(0).asInt();
// collect joints
vector<Vector> outVal(isize - 3); //= new vector<Vector>[isize-3];
int k = 0;
for (int i = 1; i < isize - 2; i = i + 3) {
outVal[k] = Vector(in->get(i).asDouble(), in->get(i + 1).asDouble(),
in->get(i + 2).asDouble());
k++;
}
// get hand state
leftHandClosed = in->get(isize - 2).asInt() == 1 ? true : false;
rightHandClosed = in->get(isize - 1).asInt() == 1 ? true : false;
oldKinectData = outVal;
kinectInitialized = true;
return outVal;
}
bool respond(const Bottle& command, Bottle& reply)
{
LockGuard lg(mutex);
int cmd=command.get(0).asVocab();
if (cmd==Vocab::encode("stop"))
{
state=idle;
reply.addVocab(Vocab::encode("ack"));
return true;
}
else if (cmd==Vocab::encode("start"))
{
if (command.size()>=5)
{
initRect.x=command.get(1).asInt();
initRect.y=command.get(2).asInt();
initRect.width=command.get(3).asInt();
initRect.height=command.get(4).asInt();
state=init;
reply.addVocab(Vocab::encode("ack"));
}
else
reply.addVocab(Vocab::encode("nack"));
return true;
}
else
return RFModule::respond(command,reply);
}
virtual void run() {
printf("Listening to terminal (type \"quit\" to stop module)\n");
bool isEof = false;
while (!(isEof||isStopping()||owner.isStopping())) {
ConstString str = NetworkBase::readString(&isEof);
if (!isEof) {
Bottle cmd(str.c_str());
Bottle reply;
bool ok = owner.safeRespond(cmd,reply);
if (ok) {
//printf("ALL: %s\n", reply.toString().c_str());
//printf("ITEM 1: %s\n", reply.get(0).toString().c_str());
if (reply.get(0).toString()=="help") {
for (int i=0; i<reply.size(); i++) {
ACE_OS::printf("%s\n",
reply.get(i).toString().c_str());
}
} else {
ACE_OS::printf("%s\n", reply.toString().c_str());
}
} else {
ACE_OS::printf("Command not understood -- %s\n", str.c_str());
}
}
}
//printf("terminal shutting down\n");
//owner.interruptModule();
}
virtual void onRead(Bottle &b)
{
Stamp info;
BufferedPort<Bottle>::getEnvelope(info);
size_t sz = b.size();
Vector x(sz);
Vector inertial(sz);
for(unsigned int i=0;i<sz;i++)
{
x[i]=b.get(i).asDouble();
inertial(i)=lpf_ord1_3hz(x(i), i);
}
g[0] = inertial[3];
g[1] = inertial[4];
g[2] = inertial[5];
g[3] = inertial[6];
g[4] = inertial[7];
g[5] = inertial[8];
//g = (9.81/norm(g))*g;
port_filtered_output.prepare() = g;
port_filtered_output.setEnvelope(info);
port_filtered_output.write();
}
bool cartControlReachAvoidThread::getAvoidanceVectorsFromPort()
{
avoidanceStruct_t avoidanceStruct;
avoidanceStruct.skin_part = SKIN_PART_UNKNOWN;
avoidanceStruct.x.resize(3,0.0);
avoidanceStruct.n.resize(3,0.0);
Bottle* avoidanceMultiBottle = inportAvoidanceVectors.read(false);
if(avoidanceMultiBottle != NULL){
yDebug("getAvoidanceVectorsFromPort(): There were %d bottles on the port.\n",avoidanceMultiBottle->size());
for(int i=0; i< avoidanceMultiBottle->size();i++){
Bottle* avoidanceStructBottle = avoidanceMultiBottle->get(i).asList();
yDebug("Bottle %d contains %s", i,avoidanceStructBottle->toString().c_str());
avoidanceStruct.skin_part = (SkinPart)(avoidanceStructBottle->get(0).asInt());
avoidanceStruct.x(0) = avoidanceStructBottle->get(1).asDouble();
avoidanceStruct.x(1) = avoidanceStructBottle->get(2).asDouble();
avoidanceStruct.x(2) = avoidanceStructBottle->get(3).asDouble();
avoidanceStruct.n(0) = avoidanceStructBottle->get(4).asDouble();
avoidanceStruct.n(1) = avoidanceStructBottle->get(5).asDouble();
avoidanceStruct.n(2) = avoidanceStructBottle->get(6).asDouble();
avoidanceVectors.push_back(avoidanceStruct);
}
return true;
}
else{
yDebug("getAvoidanceVectorsFromPort(): no avoidance vectors on the port.") ;
return false;
};
}
/*
* Message handler. Just echo all received messages.
*/
bool respond(const Bottle& command, Bottle& reply)
{
cout<<"Got "<<command.toString().c_str()<<endl;
if(command.size() < 3) {
reply.addString("Command error! example: 'sum 3 4'");
return true;
}
int a = command.get(1).asInt();
int b = command.get(2).asInt();
if( command.get(0).asString() == "sum") {
int c = sum(a, b);
reply.addInt(c);
return true;
}
if( command.get(0).asString() == "sub") {
int c = sub(a, b);
reply.addInt(c);
return true;
}
reply.addString("Unknown command");
reply.addString(command.get(0).asString().c_str());
return true;
}
bool DeviceGroup::open(yarp::os::Searchable& config) {
if (implementation==NULL) {
implementation = new DeviceGroupHelper;
}
if (implementation==NULL) {
printf("Out of memory\n");
return false;
}
if (config.check("part","a list of section names, with each section containing a device")) {
Bottle bot = config.findGroup("part").tail();
printf("Assembly of: %s\n", bot.toString().c_str());
for (int i=0; i<bot.size(); i++) {
ConstString name = bot.get(i).asString();
printf(" %s -> %s\n", name.c_str(),
config.findGroup(name).toString().c_str());
bool result = HELPER(implementation).add(name,
config.findGroup(name));
if (!result) {
HELPER(implementation).close();
return false;
}
}
return true;
}
return false;
}
int getUsedJointsMap(Property p, int n, int* thetaMap)
{
if (!p.check("joints"))
{
yError("Missing option 'joints' in given config file\n");
return 0;
}
Value& joints = p.find("joints");
Bottle *pJoints = joints.asList();
if (pJoints == 0)
{
yError("Error in option 'joints'\n");
return 0;
}
if (pJoints->size()!=n)
{
yError("The 'nJoints' and 'joints' params are incompatible");
return 0;
}
for (int i = 0; i < n; i++)
{
thetaMap[i] = pJoints->get(i).asInt();
}
return 1;
}
Bottle abmReasoning::connectOPC(Bottle bInput)
{
Bottle bOutput;
if (bInput.size() != 2)
{
bOutput.addString("Error in connect, wrong number of input");
}
if (!bInput.get(1).isString())
{
bOutput.addString("Error in connect, wrong format of input");
}
string sPortTemp = moduleName + "/" + abmReasoningFunction::s_realOPC;
realOPC = new OPCClient(sPortTemp.c_str());
int iTry = 0;
while (!realOPC->isConnected())
{
yInfo() << "\t" << "abmReasoning Connecting to " << abmReasoningFunction::s_realOPC << "..." << realOPC->connect(abmReasoningFunction::s_realOPC);
if (!realOPC->isConnected())
Time::delay(0.5);
iTry++;
if (iTry > 1)
{
yInfo() << "\t" << "abmReasoning failed to connect to " << abmReasoningFunction::s_realOPC;
bOutput.addString("Connection failed, please check your port");
break;
}
}
if (realOPC->isConnected())
{
realOPC->checkout();
realOPC->update();
}
sPortTemp = moduleName + "/" + abmReasoningFunction::s_mentalOPC;
mentalOPC = new OPCClient(sPortTemp.c_str());
iTry = 0;
while (!mentalOPC->isConnected())
{
yInfo() << "\t" << "abmReasoning Connecting to " << abmReasoningFunction::s_mentalOPC << "..." << mentalOPC->connect(abmReasoningFunction::s_mentalOPC);
if (!mentalOPC->isConnected())
Time::delay(0.5);
iTry++;
if (iTry > 1)
{
yInfo() << "\t" << "abmReasoning failed to connect to " << abmReasoningFunction::s_mentalOPC;
bOutput.addString("Connection failed, please check your port");
return bOutput;
}
mentalOPC->isVerbose = false;
}
mentalOPC->checkout();
mentalOPC->update();
bOutput.addString("Connection done");
return bOutput;
}
void ArbitratorPropertyWindow::setExcitation(ArrowModel* arrow, const char* szLink, int value)
{
Connection con = *arrow->getConnection();
string carrier = con.carrier();
//printf("Connection: %s\n",arrow->getId());
string excitation;
size_t start = carrier.find("+(ex");
if(start != std::string::npos)
{
size_t end = carrier.find("+", start+1);
if(end == std::string::npos)
end = carrier.length();
excitation = carrier.substr(start+1, end-(start+1));
carrier.erase(start, end-start);
}
//printf("\tvalue:%d, excitation: %s\n", value, excitation.c_str());
Property options;
options.fromString(excitation.c_str());
Bottle exc = options.findGroup("ex");
string strLink = "+(ex";
bool bEmpty = true;
for(int i=0; i<exc.size(); i++)
{
Value v = exc.get(i);
if(v.isList() && (v.asList()->size()>=2))
{
Bottle* b = v.asList();
if(!compareString(b->get(0).asString().c_str(), szLink))
{
strLink += string(" (") + b->get(0).asString().c_str();
char dummy[64];
sprintf(dummy, " %d)", (int)b->get(1).asDouble());
strLink += dummy;
bEmpty = false;
}
}
}
if(value != 0)
{
strLink += string(" (") + szLink;
char dummy[64];
sprintf(dummy, " %d)", value);
strLink += dummy;
bEmpty = false;
}
strLink += ")";
if(!bEmpty)
carrier += strLink;
// updating carrier
con.setCarrier(carrier.c_str());
Application* application = m_pManager->getKnowledgeBase()->getApplication();
m_pManager->getKnowledgeBase()->updateConnectionOfApplication(application, *arrow->getConnection(), con);
arrow->getConnection()->setCarrier(carrier.c_str());
arrow->setLabel(carrier.c_str());
}