bool Action::fromBottle(const Bottle &b)
{
if (!this->Entity::fromBottle(b))
return false;
if (!b.check("description")||!b.check("subactions"))
return false;
Bottle* bDesc = b.find("description").asList();
initialDescription.fromBottle(*bDesc);
this->subActions.clear();
Bottle* bSub = b.find("subactions").asList();
for(int i=0; i<bSub->size(); i++)
{
Action a;
a.fromBottle(*bSub->get(i).asList());
this->subActions.push_back(a);
}
this->estimatedDriveEffects.clear();
bSub = b.find("estimatedDriveEffects").asList();
for(int i=0; i<bSub->size(); i++)
{
string driveName = bSub->get(i).asList()->get(0).asString().c_str();
double driveEffect = bSub->get(i).asList()->get(1).asDouble();
this->estimatedDriveEffects[driveName] = driveEffect;
}
return true;
}
bool MapGrid2D::loadROSParams(string ros_yaml_filename, string& pgm_occ_filename, double& resolution, double& orig_x, double& orig_y, double& orig_t )
{
std::string file_string;
std::ifstream file;
file.open(ros_yaml_filename.c_str());
if (!file.is_open())
{
yError() << "failed to open file" << ros_yaml_filename;
return false;
}
string line;
while (getline(file, line))
{
if (line.find("origin") != std::string::npos)
{
std::replace(line.begin(), line.end(), ',', ' ');
std::replace(line.begin(), line.end(), '[', '(');
std::replace(line.begin(), line.end(), ']', ')');
/*
auto it = line.find('[');
if (it != string::npos) line.replace(it, 1, "(");
it = line.find(']');
if(it != string::npos) line.replace(it, 1, ")");*/
}
file_string += (line + '\n');
}
file.close();
bool ret = true;
Bottle bbb;
bbb.fromString(file_string);
string debug_s = bbb.toString();
if (bbb.check("image:") == false) { yError() << "missing image"; ret = false; }
pgm_occ_filename = bbb.find("image:").asString();
//ppm_flg_filename = (pgm_occ_filename.substr(0, pgm_occ_filename.size()-4))+"_yarpflags"+".ppm";
if (bbb.check("resolution:") == false) { yError() << "missing resolution"; ret = false; }
resolution = bbb.find("resolution:").asDouble();
if (bbb.check("origin:") == false) { yError() << "missing origin"; ret = false; }
Bottle* b = bbb.find("origin:").asList();
if (b)
{
orig_x = b->get(0).asDouble();
orig_y = b->get(1).asDouble();
orig_t = b->get(2).asDouble();
}
if (bbb.check("occupied_thresh:"))
{m_occupied_thresh = bbb.find("occupied_thresh:").asDouble();}
if (bbb.check("free_thresh:"))
{m_free_thresh = bbb.find("free_thresh:").asDouble();}
return ret;
}
void testFindPlugins() {
report(0,"test get 'home' dirs for writing");
setUpTestArea(false);
YarpPluginSelector selector;
selector.scan();
Bottle lst = selector.getSelectedPlugins();
checkTrue(lst.check("fakedev1"),"first device present");
checkTrue(lst.check("fakedev2"),"second device present");
checkFalse(lst.check("fakedev3"),"non-existent device absent");
breakDownTestArea();
}
void ReactiveLayer::configureSalutation(yarp::os::ResourceFinder &rf)
{
;
//Initialise the gestures response
Bottle grpSocial = rf.findGroup("SOCIAL");
salutationLifetime = grpSocial.check("salutationLifetime", Value(15.0)).asDouble();
Bottle *socialStimulus = grpSocial.find("stimuli").asList();
if (socialStimulus)
{
for (int d = 0; d<socialStimulus->size(); d++)
{
string socialStimulusName = socialStimulus->get(d).asString().c_str();
StimulusEmotionalResponse response;
Bottle * bSentences = grpSocial.find((socialStimulusName + "-sentence").c_str()).asList();
for (int s = 0; s<bSentences->size(); s++)
{
response.m_sentences.push_back(bSentences->get(s).asString().c_str());
}
std::string sGroupTemp = socialStimulusName;
sGroupTemp += "-effect";
Bottle *bEffects = grpSocial.find(sGroupTemp.c_str()).asList();
for (int i = 0; bEffects && i<bEffects->size(); i += 2)
{
response.m_emotionalEffect[bEffects->get(i).asString().c_str()] = bEffects->get(i + 1).asDouble();
}
salutationEffects[socialStimulusName] = response;
}
}
//Add the relevant Entities for handling salutation
iCub->opc->addOrRetrieveAction("is");
iCub->opc->addOrRetrieveAdjective("saluted");
}
bool RosNameSpace::connectTopic(Bottle& cmd,
bool srcIsTopic,
const Contact& src,
const Contact& dest,
ContactStyle style,
bool activeRegistration) {
Bottle reply;
Contact dynamicSrc = src;
Contact dynamicDest = dest;
if (style.carrier!="") {
if (srcIsTopic) {
dynamicDest.setCarrier(style.carrier);
} else {
dynamicSrc.setCarrier(style.carrier);
}
}
Contact base = getNameServerContact();
bool ok = NetworkBase::write(base,
cmd,
reply);
bool fail = (reply.check("faultCode", Value(0)).asInt()!=0)||!ok;
if (fail) {
if (!style.quiet) {
fprintf(stderr, "Failure: name server did not accept connection to topic.\n");
if (reply.check("faultString")) {
fprintf(stderr, "Cause: %s\n", reply.check("faultString", Value("")).asString().c_str());
}
}
}
if (!fail) {
if (activeRegistration) {
Bottle *lst = reply.get(2).asList();
Bottle cmd2;
if (lst!=nullptr) {
cmd2.addString("publisherUpdate");
cmd2.addString("/yarp");
cmd2.addString(dynamicSrc.getName());
cmd2.addList() = *lst;
NetworkBase::write(dynamicDest,
cmd2,
reply,
true);
}
}
}
return !fail;
}
bool Agent::fromBottle(Bottle b)
{
if (!this->Object::fromBottle(b))
return false;
if (!b.check("belief")||!b.check("emotions"))
return false;
m_belief.clear();
Bottle* beliefs = b.find("belief").asList();
for(int i=0; i<beliefs->size() ; i++)
{
Bottle* bRelation = beliefs->get(i).asList();
Relation r(*bRelation);
m_belief.push_back(r);
}
m_emotions_intrinsic.clear();
Bottle* emotions = b.find("emotions").asList();
for(int i=0; i<emotions->size() ; i++)
{
Bottle* bEmo = emotions->get(i).asList();
string emotionName = bEmo->get(0).asString().c_str();
double emotionValue = bEmo->get(1).asDouble();
m_emotions_intrinsic[emotionName.c_str()] = emotionValue;
}
m_drives.clear();
Bottle* drivesProperty = b.find("drives").asList();
string drivesDebug = drivesProperty->toString().c_str();
for(int i=0; i<drivesProperty->size() ; i++)
{
Bottle* bD = drivesProperty->get(i).asList();
string drivesDebug1 = bD->toString().c_str();
Drive currentDrive;
currentDrive.fromBottle(*bD);
m_drives[currentDrive.name] = currentDrive;
}
Bottle* bodyProperty = b.find("body").asList();
m_body.fromBottle(*bodyProperty);
return true;
}
/**
Add an interaction in the adjKnowledge
bInput format:
(action sAction_name)
(timing timing)
(coordinate X Y dX dY)
*/
void adjKnowledge::addInteraction(Bottle bInput)
{
string sAction = bInput.check("action", Value("none")).asString();
vdGnlTiming.push_back(bInput.check("timing", Value(0.)).asDouble());
Bottle bTemp = *bInput.get(2).asList();
pair<double, double> pXY(bTemp.get(1).asDouble(), bTemp.get(2).asDouble());
pair<double, double> pDXY(bTemp.get(3).asDouble(), bTemp.get(4).asDouble());
// mActionTiming[sAction].first.push_back(bInput.check("timing", Value(0.)).asDouble());
vdGnlXY.push_back(pXY);
vdGnlDelta.push_back(pDXY);
mActionAbsolut[sAction].push_back(pXY);
mActionDelta[sAction].push_back(pDXY);
}
bool Adjective::fromBottle(Bottle b)
{
if (!this->Entity::fromBottle(b))
return false;
if (!b.check("qualityType"))
return false;
m_quality = b.find("qualityType").asString().c_str();
return true;
}
bool getProjectionMatrix(Bottle b, Matrix &P)
{
double fx,fy,cx,cy;
if(b.check("fx"))
fx = b.find("fx").asDouble();
else
return 0;
if(b.check("fy"))
fy = b.find("fy").asDouble();
else
return 0;
// we suppose that the center distorsion is already compensated
if(b.check("w"))
cx = b.find("w").asDouble()/2.0;
else
return 0;
if(b.check("h"))
cy = b.find("h").asDouble()/2.0;
else
return 0;
double sth= 0.0; double sx = fx; double sy = fy;
double ox = cx; double oy = cy;
double f = 1.0;
Matrix K = eye(3,3);
K(0,0)=sx*f; K(1,1)=sy*f; K(0,1)=sth*f; K(0,2)=ox; K(1,2)=oy;
Matrix Pi = zeros(3,4);
Pi(0,0)=1.0; Pi(1,1)=1.0; Pi(2,2)=1.0;
P = K*Pi;
//fprintf(stderr, "Working with Projection %s\n", P.toString().c_str());
return 1;
}
//---------------------------------------------------------
void readInt(Bottle &rf, string name, int &v, int vdefault)
{
if(rf.check(name.c_str()))
{
v = rf.find(name.c_str()).asInt();
}
else
{
v = vdefault;
cout<<"Could not find value for "<<name<<". "
<<"Setting default "<<vdefault<<endl;
}
displayNameValue(name,v);
}
void cartControlReachAvoidThread::getArmHomeOptions(Bottle &b, Vector &poss, Vector &vels)
{
if (b.check("poss","Getting arm home poss"))
{
Bottle &grp=b.findGroup("poss");
int sz=grp.size()-1;
int len=sz>7?7:sz;
for (int i=0; i<len; i++){
poss[i]=grp.get(1+i).asDouble();
}
}
if (b.check("vels","Getting arm home vels"))
{
Bottle &grp=b.findGroup("vels");
int sz=grp.size()-1;
int len=sz>7?7:sz;
for (int i=0; i<len; i++)
vels[i]=grp.get(1+i).asDouble();
}
}
void Localizer::setPidOptions(const Bottle &options)
{
mutex.lock();
pid->setOptions(options);
if (options.check("dominantEye"))
{
string domEye=options.find("dominantEye").asString().c_str();
if ((domEye=="left") || (domEye=="right"))
dominantEye=domEye;
}
mutex.unlock();
}
//---------------------------------------------------------
void readVector(Bottle &rf, string name, Vector &v, int len)
{
v.resize(len,0.0);
if(rf.check(name.c_str()))
{
Bottle &grp = rf.findGroup(name.c_str());
for (int i=0; i<len; i++)
v[i]=grp.get(1+i).asDouble();
}
else
{
cout<<"Could not find parameters for "<<name<<". "
<<"Setting everything to zero by default"<<endl;
}
displayNameVector(name,v);
}
//---------------------------------------------------------
void readBool(Bottle &rf, string name, bool &v, bool vdefault)
{
if(rf.check(name.c_str()))
{
if((rf.find(name.c_str()).asString()=="true")||(rf.find(name.c_str()).asString()=="on"))
v = true;
else
v = false;
}
else
{
v = vdefault;
cout<<"Could not find value true/false for "<<name<<". "
<<"Setting default "<<((vdefault==true)?"true":"false")<<endl;
}
displayNameValue(name,((v==true)?"true":"false"));
}
void readBridgeHeaderVector(Bottle &rf, string name, field &groups, int size)
{
groups.clear();
if ( rf.check( name.c_str() ) )
{
Bottle &grp = rf.findGroup(name.c_str());
for ( int i = 0; i < size; i++)
{
groups.push_back({grp.get(1+i).asString().c_str(),""});
}
}
else
{
cout << "Could not find parameters for " << name << ". "
<< "Setting everything to null by default" << endl;
}
}
bool NetworkProfiler::yarpNameList(ports_name_set &ports, bool complete) {
ports.clear();
ContactStyle style;
style.quiet = true;
style.timeout = 3.0;
string nameserver = NetworkBase::getNameServerName();
Bottle msg, reply;
msg.addString("bot");
msg.addString("list");
if(!NetworkBase::write(Contact(nameserver), msg, reply, style)) {
yError() << "Cannot write to yarp name server";
return false;
}
if(reply.size() == 0) {
yError() << "Empty reply from yarp name server";
return false;
}
for (int i=1; i<reply.size(); i++) {
Bottle *entry = reply.get(i).asList();
if(entry != nullptr) {
bool shouldTake = false;
ConstString portname = entry->check("name", Value("")).asString();
if(complete)
{
shouldTake = portname != "";
}
else
{
shouldTake = portname != "" && portname != "fallback" && portname != nameserver;
}
if (shouldTake) {
Contact c = Contact::fromConfig(*entry);
if(c.getCarrier() != "mcast")
ports.push_back(*entry);
}
}
}
return true;
}
bool helperPID::getVectorFromOption(const Bottle &options, const char *key,
Vector &val, int &size)
{
if (options.check(key))
{
if (Bottle *b=options.find(key).asList())
{
int len=(int)val.length();
int bSize=b->size();
size=bSize<len?bSize:len;
for (int i=0; i<size; i++)
val[i]=b->get(i).asDouble();
return true;
}
}
return false;
}
void cartControlReachAvoidThread::getTorsoOptions(Bottle &b, const char *type, const int i, Vector &sw, Matrix &lim)
{
if (b.check(type))
{
Bottle &grp=b.findGroup(type);
sw[i]=grp.get(1).asString()=="on"?1.0:0.0;
if (grp.check("min","Getting minimum value"))
{
lim(i,0)=1.0;
lim(i,1)=grp.find("min").asDouble();
}
if (grp.check("max","Getting maximum value"))
{
lim(i,2)=1.0;
lim(i,3)=grp.find("max").asDouble();
}
}
}
Relation AdaptiveLayer::getRelationFromSemantic(Bottle b)
{
string s = "none";
string v = "none";
string o = "none";
string p = "none";
string t = "none";
string m = "none";
if (b.check("groupSubject"))
s = getEntityFromWordGroup(b.find("groupSubject").asList());
if (b.check("groupVerbal"))
v = getEntityFromWordGroup(b.find("groupVerbal").asList());
if (b.check("groupObject"))
o = getEntityFromWordGroup(b.find("groupObject").asList());
if (b.check("groupPlace"))
p = getEntityFromWordGroup(b.find("groupPlace").asList());
if (b.check("groupTime"))
t = getEntityFromWordGroup(b.find("groupTime").asList());
if (b.check("groupManner"))
m = getEntityFromWordGroup(b.find("groupManner").asList());
return Relation(s,v,o,p,t,m);
}
Bottle autobiographicalMemory::snapshot(const Bottle &bInput)
{
/*
format of input bottle :
snapshot (action name_action type_action) (time t_time) (arguments (arg1) (arg2, role) ... argN) (begin 0/1) (mental)
Arguments are from 2 types:
- something from the OPC : just argN is enough
- something external : argN + role is mandatory
*/
// run only one snapshot at a time
LockGuard lg(mutexSnapshot);
//get Instance of the next opc
string sRequest_instance;
// get the OPC to check en bois
bool bMental = bInput.check("mental");
if (bMental)
{
// change the providers !!
}
if (isconnected2reasoning)
{
Bottle b2reasoning;
b2reasoning.addString("updateObjectLocation");
b2reasoning.addString(s_real_OPC.c_str());
abm2reasoning.write(b2reasoning);
}
sRequest_instance = "SELECT instance FROM main ORDER BY instance DESC LIMIT 1;";
Bottle bRequest, bTemp;
bRequest.addString("request");
bRequest.addString(sRequest_instance.c_str());
bRequest = request(bRequest);
int instance;
if (bRequest.toString() != "NULL") {
instance = atoi(bRequest.get(0).asList()->get(0).toString().c_str()) + 1;
}
else {
instance = 0;
}
OPCEARS.setInstance(instance);
currentInstance = instance;
// Filling table main:
Bottle bMain;
bMain.addString("request");
ostringstream osMain;
osMain << "INSERT INTO main(activityname, activitytype, time, instance, begin, opcname) VALUES ('";
string sName;
//for streaming image
bool isStreamActivity = false;
string fullSentence = "defaultLabel";
string activityType;
//Action
bool done = false;
for (int i = 1; i < bInput.size(); i++)
{
bTemp = *(bInput.get(i).asList());
if (bTemp.get(0) == "action" && !done)
{
osMain << bTemp.get(1).asString() << "' , '"; // activityname
sName = bTemp.get(1).asString();
imgLabel = bTemp.get(1).asString();
//used to name the single image
ostringstream labelImg;
labelImg << imgLabel << "_" << instance;
fullSentence = labelImg.str();
activityType = bTemp.get(2).asString();
//if activity is an action -> stream
if (activityType == "action") {
isStreamActivity = true;
}
osMain << bTemp.get(2).asString() << "' , '"; // activitytype
done = true;
}
}
if (!done) {
osMain << "unknown' , 'unknown', '";
}
// Time
string sTime = getCurrentTime();
osMain << sTime << "' , " << instance << " , "; // time + instance
//Begin
done = false;
bool bBegin = false;
//yDebug() << "bInput has a size of " << bInput.size() << " and is : " << bInput.toString().c_str();
for (int i = 1; i < bInput.size(); i++)
{
bTemp = *(bInput.get(i).asList());
if (bTemp.get(0) == "begin" && !done)
{
if (bTemp.get(1).asInt() == 1)
{
osMain << "TRUE ";
bBegin = true;
}
else
{
osMain << "FALSE ";
bBegin = false;
}
done = true;
}
}
if (!done) {
osMain << "FALSE";
}
if (isStreamActivity == true && !bBegin) { //just stop stream images stores when relevant activity
yDebug() << "[mutexChangeover] try locking in snapshot end";
mutexChangeover.lock();
yDebug() << "[mutexChangeover] locked in snapshot end";
streamStatus = "end"; //is done here (before the OPC snapshot), because the snapshot is slowing everything down
}
if(bMental) {
osMain << " , '" << s_mental_OPC << "' ) ; ";
} else {
osMain << " , '" << s_real_OPC << "' ) ; ";
}
bMain.addString(string(osMain.str()).c_str());
request(bMain);
OPCClient *opcWorld;
//Connection to the OPC
(bMental) ? opcWorld = opcWorldMental : opcWorld = opcWorldReal;
Bottle bOPCEearsResponse = OPCEARS.snapshot(bInput, opcWorld);
if(bOPCEearsResponse.get(0).asString() == "nack") {
return bOPCEearsResponse;
}
ostringstream osName;
osName << sName << instance;
sName += osName.str(); //I dont understand this Gregoire : you concatenate the name with nameInstance with itself, producing namenameinstance
//yDebug() << "OPCEARS: " << sName;
Bottle bSnapShot = OPCEARS.insertOPC(sName);
if(bSnapShot.get(0).asString() == "nack") {
return bSnapShot;
}
ostringstream osAllArg;
Bottle bRecogSemantic;
bool bShouldSend = true;
// Filling contentArg
if (opcWorld->isConnected()) {
for (int i = 1; i < bInput.size(); i++)
{
bTemp = *(bInput.get(i).asList());
if (bTemp.get(0) == "arguments" && bTemp.size() > 1)
{
for (int j = 1; j < bTemp.size(); j++)
{
ostringstream osArg;
string cArgArgument, cArgType, cArgSubtype, cArgRole;
//check if the argument is an entity in OPC
Entity* currentEntity = opcWorld->getEntity(bTemp.get(j).asList()->get(0).toString().c_str());
if (currentEntity == NULL) {
cArgArgument = bTemp.get(j).asList()->get(0).asString();
cArgType = "external";
cArgSubtype = "default";
}
else {
cArgArgument = currentEntity->name();
cArgType = "entity";
cArgSubtype = currentEntity->entity_type();
}
if (bTemp.get(j).asList()->size() > 1) {
cArgRole = bTemp.get(j).asList()->get(1).asString();
//add sentence for single img label
if (cArgRole == "sentence"){
fullSentence = cArgArgument;
}
if (cArgRole == "semantic"){
yInfo() << " " << bTemp.get(j).asList()->get(0).toString();
bRecogSemantic.fromString(bTemp.get(j).asList()->get(0).toString());
yInfo() << " " << bRecogSemantic.toString();
yInfo() << " bRecog size: " << bRecogSemantic.size();
bShouldSend = false;
}
}
else {
cArgRole = "unknown";
}
std::replace( cArgArgument.begin(), cArgArgument.end(), '\'', ' ');
if (bShouldSend){
osArg << "INSERT INTO contentarg(instance, argument, type, subtype, role) VALUES ( " << instance << ", '" << cArgArgument << "', " << "'" << cArgType << "', '" << cArgSubtype << "', '" << cArgRole << "') ; ";
}
else{
bShouldSend = true;
}
// one stringstream with all argments
osAllArg << osArg.str().c_str();
}
}
}
// add the snapshot of the OPC
osAllArg << bSnapShot.get(0).asString();
bRequest.clear();
if(osAllArg.str() != "") {
bRequest.addString("request");
bRequest.addString(osAllArg.str().c_str());
request(bRequest);
}
}
if ((!bBegin) && isconnected2reasoning)
{
Bottle b2reasoning;
b2reasoning.addString("addLastActivity");
b2reasoning.addString("action");
abm2reasoning.write(b2reasoning);
}
if (isStreamActivity == true) { //just launch stream images stores when relevant activity
if (bBegin) {
while(!mutexStreamRecord.tryLock()) {
yDebug() << "[mutexStreamRecord] tryLock";
yarp::os::Time::delay(0.1);
}
yDebug() << "[mutexStreamRecord] unlock";
mutexStreamRecord.unlock();
yDebug() << "[mutexChangeover] try locking in snapshot begin";
mutexChangeover.lock();
yDebug() << "[mutexChangeover] locked in snapshot begin";
streamStatus = "begin"; //streamStatus = "end" is done before the OPC snapshot, because the snapshot is slowing everything down
}
}
else
{ //just one image (sentence?)
while(streamStatus == "send") { // when it's sending, don't record new images!
yarp::os::Time::delay(0.1);
}
if(streamStatus != "record") { // only change instance number if we are not recording already
imgInstance = currentInstance;
}
string synchroTime = getCurrentTime();
frameNb = 0;
storeImagesAndData(synchroTime, true, fullSentence);
//if activity = say, we have to take one image/data + the sound that is coming from another port and catch in the update method of ABM (store in /tmp/sound/default.wav
if (activityType == "recog") {
string sndName;
//take the full sentence, replace space by _ to have the sound name
replace(fullSentence.begin(), fullSentence.end(), ' ', '_');
sndName = fullSentence + ".wav";
//read sound from file and put data in yarp::sig::Sound to store properly with semantic/instance that we are now aware of
yarp::sig::Sound s;
string defaultSoundFullPath = storingPath + "/" + storingTmpSuffix + "/sound/" + "default.wav";
yDebug() << "opening sound file " << defaultSoundFullPath.c_str();
if (yarp::sig::file::read(s, defaultSoundFullPath.c_str()) == false) {
yWarning() << "Cannot open the default sound file: check " << defaultSoundFullPath;
}
else {
yInfo() << "Default sound file loaded from " << defaultSoundFullPath;
stringstream sInstance;
sInstance << currentInstance;
//build the path and the name of the sound according to the instance and sentence said
string relativePath = sInstance.str() + "/" + sndName;
string fullPath = storingPath + "/" + relativePath;
if (yarp::sig::file::write(s, fullPath.c_str()) == false) {
yError() << "Cannot save the default sound file to " << fullPath;
}
else {
yInfo() << "Default sound file renamed and moved to " << fullPath;
database_mutex.lock();
//add the sound into the large_objects table of ABM
unsigned int snd_oid = ABMDataBase->lo_import(fullPath.c_str());
database_mutex.unlock();
Bottle bRequest;
ostringstream osArg;
//Populate the sounddata table with the infos
bRequest.addString("request");
osArg << "INSERT INTO sounddata(instance, relative_path, time, snd_provider_port, snd_oid) VALUES ('" << currentInstance << "', '" << relativePath << "', '" << synchroTime << "', '" << portSoundStreamInput.getName() << "', '" << snd_oid << "');";
bRequest.addString(osArg.str());
request(bRequest);
}
}
osInsertTemp.str("");
recogFromGrammarSemantic(bRecogSemantic, "", 1, currentInstance);
requestFromString(osInsertTemp.str());
}
}
bSnapShot.addInt(instance);
return bSnapShot;
}
bool embObjAnalogSensor::fromConfig(yarp::os::Searchable &_config)
{
Bottle xtmp;
int format=0;
// embObj parameters, in ETH group
Value val =_config.findGroup("ETH").check("Ems",Value(1), "Board number");
if(val.isInt())
_fId.boardNumber = val.asInt();
else
{
_as_type=AS_NONE;
yError () << "embObjAnalogSensor: EMS Board number identifier not found\n";
return false;
}
// Analog Sensor stuff
Bottle config = _config.findGroup("GENERAL");
if (!extractGroup(config, xtmp, "Period","transmitting period of the sensors", 1))
{
yError() << "embObjAnalogSensor Using default value = 0 (disabled)";
_period = 0;
_as_type=AS_NONE;
}
else
{
_period = xtmp.get(1).asInt();
yDebug() << "embObjAnalogSensor::fromConfig() detects embObjAnalogSensor Using value of" << _period;
}
if (!extractGroup(config, xtmp, "Channels","Number of channels of the sensor", 1))
{
yWarning("embObjAnalogSensor: Using default value = 0 (disabled)\n");
_channels = 0;
_numofsensors = 0;
_period = 0;
_as_type=AS_NONE;
}
else
{
_channels = xtmp.get(1).asInt();
}
#if 0
if (!extractGroup(config, xtmp, "NumberOfSensors","Number of sensors managed", 1))
{
yWarning("embObjAnalogSensor: Using default value = 1 for _numofsensors\n");
_numofsensors = 1;
}
else
{
_numofsensors = xtmp.get(1).asInt();
}
#endif
// Type is a new parameter describing the sensor Type using human readable string
if(config.check("Type") )
{
yDebug() << "Using new syntax";
yarp::os::ConstString type = config.find("Type").asString();
if(type == "inertial")
{
_as_type=AS_INERTIAL_MTB;
}
else if(type == "strain")
{
_as_type=AS_STRAIN;
}
else if(type == "mais")
{
_as_type=AS_MAIS;
}
else
{
_as_type=AS_NONE;
yError() << "embObjAnalogSensor: unknown device " << type << ". Supported devices are 'mais', 'strain' and 'inertial'";
return false;
}
}
else
{
// Infer the sensor type by combining other parameters
yDebug() << "Using old syntax";
if (!extractGroup(config, xtmp, "Format","data format of the Analog Sensor", 1))
{
yWarning("embObjAnalogSensor: Using default value = 0 (disabled)");
_channels = 0;
_period = 0;
_as_type=AS_NONE;
}
else
{
format = xtmp.get(1).asInt();
}
if((_channels==NUMCHANNEL_STRAIN) && (format==FORMATDATA_STRAIN))
{
_as_type=AS_STRAIN;
}
else if((_channels==NUMCHANNEL_MAIS) && (format==FORMATDATA_MAIS))
{
_as_type=AS_MAIS;
}
else
{
_as_type=AS_NONE;
yError() << "embObjAnalogSensor incorrect config!channels="<< _channels <<" format="<< format;
cleanup();
return false;
}
}
_numofsensors = 1;
// however, if we have a AS_INERTIAL_MTB, then we may have more than one. for sure not zero.
if(AS_INERTIAL_MTB == _as_type)
{
Bottle tmp;
_numofsensors = 0;
tmp = config.findGroup("enabledAccelerometers");
_numofsensors += (tmp.size()-1); // sensors holds strings "enabledAccelerometers" and then all the others, thus i need a -1
tmp = config.findGroup("enabledGyroscopes");
_numofsensors += (tmp.size()-1); // sensors holds strings "enabledGyroscopes" and then all the others, thus i need a -1
#if 0
if (!extractGroup(config, xtmp, "enabledAccelerometers", "Position of managed sensors axpressed as strings", 1))
{
yWarning("embObjAnalogSensor: cannot find enabledAccelerometers\n");
_numofsensors = 1;
}
else
{
_numofsensors = xtmp.size();
}
#endif
}
if(AS_STRAIN == _as_type)
{
if (!extractGroup(config, xtmp, "UseCalibration","Calibration parameters are needed", 1))
{
return false;
}
else
{
_useCalibration = xtmp.get(1).asInt();
}
}
return true;
}
/**
Add an other interaction in the adjKnowledge (event adj is not this one, this is used for a comparaison)
bInput format:
(action sAction_name)
(timing timing)
(coordinate X Y dX dY)
*/
void adjKnowledge::addOtherInteraction(Bottle bInput)
{
string sAction = bInput.check("action", Value("none")).asString();
// vdNoGnlTiming.push_back(bInput.check("timing", Value(0.)).asDouble());
// mActionTiming[sAction].second.push_back(bInput.check("timing", Value(0.)).asDouble());
}
bool ClientCartesianController::open(Searchable &config)
{
ConstString remote, local, carrier;
if (config.check("remote"))
remote=config.find("remote").asString();
else
return false;
if (config.check("local"))
local=config.find("local").asString();
else
return false;
carrier=config.check("carrier",Value("udp")).asString();
if (config.check("timeout"))
timeout=config.find("timeout").asDouble();
portCmd.open((local+"/command:o").c_str());
portState.open((local+"/state:i").c_str());
portEvents.open((local+"/events:i").c_str());
portRpc.open((local+"/rpc:o").c_str());
bool ok=true;
ok&=Network::connect(portRpc.getName().c_str(),(remote+"/rpc:i").c_str());
if (ok)
{
Bottle info;
getInfoHelper(info);
if (info.check("server_version"))
{
double server_version=info.find("server_version").asDouble();
if (server_version!=CARTCTRL_CLIENT_VER)
{
yError("version mismatch => server(%g) != client(%g); please update accordingly",
server_version,CARTCTRL_CLIENT_VER);
return false;
}
}
else
yWarning("unable to retrieve server version; please update the server");
}
else
{
yError("unable to connect to the server rpc port!");
return false;
}
ok&=Network::connect(portCmd.getName().c_str(),(remote+"/command:i").c_str(),carrier.c_str());
ok&=Network::connect((remote+"/state:o").c_str(),portState.getName().c_str(),carrier.c_str());
ok&=Network::connect((remote+"/events:o").c_str(),portEvents.getName().c_str(),carrier.c_str());
// check whether the solver is alive and connected
if (ok)
{
Bottle command, reply;
command.addVocab(IKINCARTCTRL_VOCAB_CMD_GET);
command.addVocab(IKINCARTCTRL_VOCAB_OPT_ISSOLVERON);
if (!portRpc.write(command,reply))
{
yError("unable to get reply from server!");
close();
return false;
}
if (reply.get(0).asVocab()==IKINCARTCTRL_VOCAB_REP_ACK)
if (reply.size()>1)
if (reply.get(1).asVocab()==IKINCARTCTRL_VOCAB_VAL_TRUE)
return connected=true;
yError("unable to connect to solver!");
close();
return false;
}
else
{
yError("unable to connect to server!");
close();
return false;
}
}
void AllostaticController::configureAllostatic(yarp::os::ResourceFinder &rf)
{
//The homeostatic module should be running in parallel, independent from this, so the objective of
//this config would be to have a proper list and connect to each port
homeo_name = "homeostasis";
string homeo_rpc_name = "/" + homeo_name + "/rpc";
string to_h_rpc_name="/"+moduleName+"/toHomeoRPC:o";
to_homeo_rpc.open(to_h_rpc_name);
while(!Network::connect(to_h_rpc_name,homeo_rpc_name))
{
yDebug()<<"Trying to connect to homeostasis...";
yDebug() << "from " << to_h_rpc_name << " to " << homeo_rpc_name;
yarp::os::Time::delay(0.2);
}
yInfo() << "Initializing drives...";//<<endl;
Bottle grpAllostatic = rf.findGroup("ALLOSTATIC");
drivesList = *grpAllostatic.find("drives").asList();
Bottle cmd;
priority_sum = 0.;
double priority;
for (int d = 0; d<drivesList.size(); d++)
{
cmd.clear();
string driveName = drivesList.get(d).asString();
yInfo() << ("Initializing drive " + driveName);
cmd.addString("add");
cmd.addString("conf");
Bottle drv;
drv.clear();
Bottle aux;
aux.clear();
aux.addString("name");
aux.addString(driveName);
drv.addList()=aux;
aux.clear();
drv.addList()=grpAllostatic;
cmd.addList()=drv;
Bottle rply;
rply.clear();
rply.get(0).asString();
to_homeo_rpc.write(cmd,rply);
AllostaticDrive alloDrive;
alloDrive.name = driveName;
Value cmds = grpAllostatic.check((driveName + "-sensation-on"), Value("None"));
alloDrive.sensationOnCmd = *cmds.asList();
cmds = grpAllostatic.check((driveName + "-sensation-off"), Value("None"));
alloDrive.sensationOffCmd = *cmds.asList();
cmds = grpAllostatic.check((driveName + "-before-trigger"), Value("None"));
if (!(cmds.isString() && cmds.asString() == "None")) {
alloDrive.beforeTriggerCmd = *cmds.asList();
}
cmds = grpAllostatic.check((driveName + "-after-trigger"), Value("None"));
if (!(cmds.isString() && cmds.asString() == "None")) {
alloDrive.afterTriggerCmd = *cmds.asList();
}
alloDrive.homeoPort = &to_homeo_rpc;
alloDrive.inputSensationPort = new BufferedPort<Bottle>;
string portName = "/" + moduleName + "/" + driveName + "/sensation:i";
alloDrive.inputSensationPort->open(portName);
openPorts(driveName);
string sensationPort = grpAllostatic.check((driveName + "-sensation-port"), Value("None")).asString();
string pn = "/" + moduleName + "/" + driveName + "/sensation:i";
while(!Network::connect(sensationPort, pn)) {
yDebug()<<"Connecting " << sensationPort << " to " << pn;
yarp::os::Time::delay(0.5);
}
// set drive priorities. Default to 1.
priority = grpAllostatic.check((driveName + "-priority"), Value(1.)).asDouble();
priority_sum += priority;
drivePriorities.push_back(priority);
//Under effects
string under_port_name = grpAllostatic.check((driveName + "-under-behavior-port"), Value("None")).asString();
string under_cmd_name = grpAllostatic.check((driveName + "-under-behavior"), Value("None")).asString();
bool active = false;
if (under_port_name != "None" && under_cmd_name != "None")
{
active = true;
string out_port_name = "/" + moduleName + "/" + driveName + "/under_action:o";
alloDrive.behaviorUnderPort = new Port();
alloDrive.behaviorUnderPort->open(out_port_name);
yDebug() << "trying to connect to" << under_port_name;
while(!Network::connect(out_port_name,under_port_name))
{
yarp::os::Time::delay(0.5);
}
alloDrive.behaviorUnderCmd = Bottle(under_cmd_name);
}else{
yInfo() << "No port name for" << driveName << "under-behavior-port";
}
//Over effects
string over_port_name = grpAllostatic.check((driveName + "-over-behavior-port"), Value("None")).asString();
string over_cmd_name = grpAllostatic.check((driveName + "-over-behavior"), Value("None")).asString();
if (over_port_name != "None" && over_cmd_name != "None")
{
active=true;
string out_port_name = "/" + moduleName + "/" + driveName + "/over_action:o";
alloDrive.behaviorOverPort = new Port();
alloDrive.behaviorOverPort->open(out_port_name);
yDebug() << "trying to connect to" << over_port_name;
while(!Network::connect(out_port_name, over_port_name))
{
yarp::os::Time::delay(0.5);
}
alloDrive.behaviorOverCmd = Bottle(over_cmd_name);
} else {
yInfo() << "No port name for" << driveName << "over-behavior-port";
}
alloDrive.active = active;
allostaticDrives[driveName] = alloDrive;
}
if (! Normalize(drivePriorities))
yDebug() << "Error: Drive priorities sum up to 0.";
yInfo() << "done.";
}
cartesianMover::cartesianMover(GtkWidget *vbox_d, PolyDriver *partDd_d, char *partName, ResourceFinder *fnd)
{
finder = fnd;
if (!finder->isNull())
fprintf(stderr, "Setting a valid finder \n");
partLabel = partName;
partDd = partDd_d;
vbox = vbox_d;
interfaceError = false;
if (!partDd->isValid()) {
fprintf(stderr, "Device given to the cartesian interface is not available.\n");
interfaceError = true;
}
fprintf(stderr, "Opening crt interface...");
bool ok;
ok = partDd->view(crt);
if ((!ok) || (crt==0))
fprintf(stderr, "...crt was not ok...ok=%d", ok);
if (!partDd->isValid()) {
fprintf(stderr, "Cartesian device driver was not valid! \n");
dialog_severe_error(GTK_MESSAGE_ERROR,(char *) "Cartesian device not available.", (char *) "Check available devices", true);
interfaceError = true;
}
else if (!ok) {
fprintf(stderr, "Error while acquiring cartesian interfaces \n");
dialog_severe_error(GTK_MESSAGE_ERROR,(char *) "Problems acquiring cartesian interface", (char *) "Check if cartesian interface is running", true);
interfaceError = true;
}
if (interfaceError == false)
{
fprintf(stderr, "Allocating memory \n");
int j,k;
index = new int [MAX_NUMBER_OF_JOINTS];
entry_id = new guint [0];
*entry_id = -1;
frame_slider1 = new GtkWidget* [MAX_NUMBER_OF_JOINTS];
sliderArray = new GtkWidget* [NUMBER_OF_CARTESIAN_COORDINATES];
currPosArray = new GtkWidget* [NUMBER_OF_CARTESIAN_COORDINATES];
//fprintf(stderr, "sliderArray has address 0x%x\n", (unsigned int) sliderArray);
GtkWidget *top_hbox = NULL;
GtkWidget *bottom_hbox = NULL;
GtkWidget *panel_hbox = NULL;
GtkWidget *inv1 = NULL;
GtkWidget *invArray[NUMBER_OF_CARTESIAN_COORDINATES];
GtkWidget *homeArray[NUMBER_OF_CARTESIAN_COORDINATES];
GtkWidget *framesArray[NUMBER_OF_CARTESIAN_COORDINATES];
GtkWidget *sw = NULL;
//creation of the top_hbox
top_hbox = gtk_hbox_new (FALSE, 0);
gtk_container_set_border_width (GTK_CONTAINER (top_hbox), 10);
gtk_container_add (GTK_CONTAINER (vbox), top_hbox);
inv1 = gtk_fixed_new ();
gtk_container_add (GTK_CONTAINER (top_hbox), inv1);
Vector o;
Vector x;
while (!crt->getPose(x,o))
Time::delay(0.001);
Matrix R = axis2dcm(o);
Vector eu = dcm2euler(R);
//x(0) = 1; x(1) = 1; x(2) = 1;
//o(0) = 1; o(1) = 0; o(2) = 0; o(3) = 0;
char buffer[40] = {'i', 'n', 'i', 't'};
int numberOfRows = 3;
int height, width;
height = 100;
width = 180;
double min,max;
std::string limitString=partLabel;
limitString=limitString+"_workspace";
Bottle bCartesianLimits;
if (finder->check(limitString.c_str()))
{
//fprintf(stderr, "There seem limits for %s", partLabel);
bCartesianLimits = finder->findGroup(limitString.c_str());
//fprintf(stderr, "...got: %s", bCartesianLimits.toString().c_str());
}
fprintf(stderr, "Starting embedding cartesian GUI widgets \n");
for (k = 0; k<NUMBER_OF_CARTESIAN_COORDINATES; k++)
{
//fprintf(stderr, "Adding invArray \n");
invArray[k] = gtk_fixed_new ();
index[k]=k;
j = k/numberOfRows;
if (k==0)
{
sprintf(buffer, "x");
if (bCartesianLimits.check("xmin") && bCartesianLimits.check("xmax"))
{
min = bCartesianLimits.find("xmin").asDouble();
max = bCartesianLimits.find("xmax").asDouble();
}
else
{
min = x(0) - 0.1 * fabs(x(0));
max = x(0) + 0.1 * fabs(x(0));
}
}
if (k==1)
{
sprintf(buffer, "y");
if (bCartesianLimits.check("ymin") && bCartesianLimits.check("ymax"))
{
min = bCartesianLimits.find("ymin").asDouble();
max = bCartesianLimits.find("ymax").asDouble();
}
else
{
min = x(1) - 0.1 * fabs(x(1));
max = x(1) + 0.1 * fabs(x(1));
}
}
if (k==2)
{
sprintf(buffer, "z");
if (bCartesianLimits.check("zmin") && bCartesianLimits.check("zmax"))
{
min = bCartesianLimits.find("zmin").asDouble();
max = bCartesianLimits.find("zmax").asDouble();
}
else
{
min = x(2) - 0.1 * fabs(x(2));
max = x(2) + 0.1 * fabs(x(2));
}
}
if (k==3)
{
sprintf(buffer, "euler-alpha");
min = -180;
max = 180;
}
if (k==4)
{
sprintf(buffer, "euler-beta");
min = -180;
max = 180;
}
if (k==5)
{
sprintf(buffer, "euler-gamma");
min = -180;
max = 180;
}
frame_slider1[k] = gtk_frame_new ("Value:");
//fprintf(stderr, "Initializing sliders %d \n",k);
if (min<max)
{
sliderArray[k] = gtk_hscale_new_with_range(min, max, 1);
if (k<3)
gtk_scale_set_digits((GtkScale*) sliderArray[k],2);
else
gtk_scale_set_digits((GtkScale*) sliderArray[k],1);
}
else
{
sliderArray[k] = gtk_hscale_new_with_range(1, 2, 1);
if (k<3)
gtk_scale_set_digits((GtkScale*) sliderArray[k],2);
else
gtk_scale_set_digits((GtkScale*) sliderArray[k],1);
}
currPosArray[k] = gtk_entry_new();
//fprintf(stderr, "Initializing the buttons %d \n", k);
homeArray[k] = gtk_button_new_with_mnemonic ("Home");
//fprintf(stderr, "Initializing frames %d \n", k);
framesArray[k] = gtk_frame_new (buffer);
gtk_fixed_put (GTK_FIXED(inv1), invArray[k], 0+(k%numberOfRows)*width, 0+ j*height);
//Positions
//fprintf(stderr, "Positioning buttons %d \n", k);
gtk_fixed_put (GTK_FIXED(invArray[k]), frame_slider1[k], 60, 10 );
gtk_fixed_put (GTK_FIXED(invArray[k]), sliderArray[k], 65, 20 );
gtk_fixed_put (GTK_FIXED(invArray[k]), currPosArray[k], 95, 70);
int buttonDist= 24;
int buttonOffset = 13;
gtk_fixed_put (GTK_FIXED(invArray[k]), homeArray[k], 6, buttonOffset);
gtk_fixed_put (GTK_FIXED(invArray[k]), framesArray[k], 0, 0);
//Dimensions
//fprintf(stderr, "Dimensioning buttons %d \n", k);
gtk_widget_set_size_request (frame_slider1[k], 110, 50);
gtk_widget_set_size_request (sliderArray[k], 90, 40);
gtk_widget_set_size_request (currPosArray[k], 70, 20);
gtk_widget_set_size_request (homeArray[k], 50, 25);
gtk_widget_set_size_request (framesArray[k], width, height);
/*
* Positions commands
*/
//fprintf(stderr, "Assinging callback %d \n", k);
gtk_range_set_update_policy ((GtkRange *) (sliderArray[k]), GTK_UPDATE_DISCONTINUOUS);
if (k<3)
gtk_range_set_value ((GtkRange *) (sliderArray[k]), x(k));
if (k>=3 && k <= 5)
gtk_range_set_value ((GtkRange *) (sliderArray[k]), eu(k-3) * 180/M_PI);
g_signal_connect (sliderArray[k], "value-changed", G_CALLBACK(position_slider_changed), this);
}
/*
* Display current position
*/
*entry_id = gtk_timeout_add(UPDATE_TIME, (GtkFunction) display_cartesian_pose, this);
for (k = 0; k<NUMBER_OF_CARTESIAN_COORDINATES; k++)
gtk_editable_set_editable ((GtkEditable*) currPosArray[k], FALSE);
/*
* Common commands
*/
GtkWidget *frame3;
frame3 = gtk_frame_new ("Commands:");
gtk_fixed_put (GTK_FIXED(inv1), frame3, (NUMBER_OF_CARTESIAN_COORDINATES%numberOfRows)*width, (NUMBER_OF_CARTESIAN_COORDINATES/numberOfRows)*height);
gtk_widget_set_size_request (frame3, 180, 240);
//Button 0 in the panel
GtkWidget *button0 = gtk_button_new_with_mnemonic ("Open sequence tab");
gtk_fixed_put (GTK_FIXED (inv1), button0, 10+(NUMBER_OF_CARTESIAN_COORDINATES%numberOfRows)*width, 20+(NUMBER_OF_CARTESIAN_COORDINATES/numberOfRows)*height);
gtk_widget_set_size_request (button0, 150, 25);
fprintf(stderr, "Initializing the table \n");
init_cartesian_table();
fprintf(stderr, "Connecting the callbacks for the table \n");
g_signal_connect (button0, "clicked", G_CALLBACK (cartesian_table_open), this);
//Button1 in the panel
GtkWidget *button1 = gtk_button_new_with_mnemonic ("Stop");
gtk_fixed_put (GTK_FIXED (inv1), button1, 10+(NUMBER_OF_CARTESIAN_COORDINATES%numberOfRows)*width, 45+(NUMBER_OF_CARTESIAN_COORDINATES/numberOfRows)*height);
gtk_widget_set_size_request (button1, 150, 25);
//g_signal_connect (button1, "clicked", G_CALLBACK (stop_motion), crt);
//Velocity
GtkWidget *frame7;
frame7 = gtk_frame_new ("Time[sec]:");
gtk_fixed_put (GTK_FIXED(inv1), frame7, 5+(NUMBER_OF_CARTESIAN_COORDINATES%numberOfRows)*width, 70 + (NUMBER_OF_CARTESIAN_COORDINATES/numberOfRows)*height);
gtk_widget_set_size_request (frame7, 160, 50);
sliderVelocity = new GtkWidget;
sliderVelocity = gtk_hscale_new_with_range(1, 10, 1);
gtk_scale_set_digits((GtkScale*) sliderVelocity,2);
gtk_fixed_put (GTK_FIXED(inv1), sliderVelocity, 60+(NUMBER_OF_CARTESIAN_COORDINATES%numberOfRows)*width, 80 + (NUMBER_OF_CARTESIAN_COORDINATES/numberOfRows)*height);
gtk_widget_set_size_request (sliderVelocity, 90, 40);
gtk_range_set_update_policy ((GtkRange *) (sliderVelocity), GTK_UPDATE_DISCONTINUOUS);
gtk_range_set_value ((GtkRange *) (sliderVelocity), 2);
//Diplay axis
po = new GtkWidget*[4];
for (int i=0; i < 4; i++)
{
po[i] = gtk_entry_new();
}
//Display axis
GtkWidget *frame5;
frame5 = gtk_frame_new ("Axis:");
gtk_fixed_put (GTK_FIXED(inv1), frame5, 5+(NUMBER_OF_CARTESIAN_COORDINATES%numberOfRows)*width, 140 + (NUMBER_OF_CARTESIAN_COORDINATES/numberOfRows)*height);
gtk_widget_set_size_request (frame5, 80, 85);
gtk_fixed_put (GTK_FIXED(inv1), po[0], 10+(NUMBER_OF_CARTESIAN_COORDINATES%numberOfRows)*width, 155+(NUMBER_OF_CARTESIAN_COORDINATES/numberOfRows)*height);
gtk_fixed_put (GTK_FIXED(inv1), po[1], 10+(NUMBER_OF_CARTESIAN_COORDINATES%numberOfRows)*width, 175+(NUMBER_OF_CARTESIAN_COORDINATES/numberOfRows)*height);
gtk_fixed_put (GTK_FIXED(inv1), po[2], 10+(NUMBER_OF_CARTESIAN_COORDINATES%numberOfRows)*width, 195+(NUMBER_OF_CARTESIAN_COORDINATES/numberOfRows)*height);
//Display angle
GtkWidget *frame6;
frame6 = gtk_frame_new ("Angle:");
gtk_fixed_put (GTK_FIXED(inv1), frame6, 85+(NUMBER_OF_CARTESIAN_COORDINATES%numberOfRows)*width, 155 + (NUMBER_OF_CARTESIAN_COORDINATES/numberOfRows)*height);
gtk_widget_set_size_request (frame6, 80, 45);
gtk_fixed_put (GTK_FIXED(inv1), po[3], 90+(NUMBER_OF_CARTESIAN_COORDINATES%numberOfRows)*width, 175+(NUMBER_OF_CARTESIAN_COORDINATES/numberOfRows)*height);
for (int i=0; i < 4; i++)
{
gtk_widget_set_size_request (po[i], 70, 20);
gtk_editable_set_editable ((GtkEditable*) po[i], FALSE);
}
*entry_id = gtk_timeout_add(UPDATE_TIME, (GtkFunction) display_axis_pose, this);
//CheckButton in the panel
GtkWidget *check= gtk_check_button_new_with_mnemonic ("Tracking Mode");
gtk_fixed_put (GTK_FIXED (inv1), check, 10+(NUMBER_OF_CARTESIAN_COORDINATES%numberOfRows)*width, 120+(NUMBER_OF_CARTESIAN_COORDINATES/numberOfRows)*height);
gtk_widget_set_size_request (check, 150, 25);
//g_signal_connect (check, "clicked", G_CALLBACK (toggle_tracking_mode), crt);
}
}
/* ******* Configure module ********************************************** */
bool RPCControllerModule::configure(ResourceFinder &rf){
using std::string;
using yarp::os::Network;
cout << dbgTag << "Starting. \n";
/* ****** Configure the Module ****** */
// Get resource finder and extract properties
moduleName = rf.check("name", Value("RPCController"), "The module name.").asString().c_str();
period = rf.check("period", 1.0).asDouble();
robotName = rf.check("robot", Value("icub"), "The robot name.").asString().c_str();
string portNameRoot = "/" + moduleName + "/";
/* ******* Ports. ******* */
rpcModule.open((portNameRoot + "cmd:io").c_str());
portExperimentStepsOut.open((portNameRoot + "experiment/status:o").c_str());
/* ******* Read RPC port parameters. ******* */
Bottle parGroup = rf.findGroup("rpc");
if (!parGroup.isNull()) {
if (parGroup.check("rpcServer")) {
string rpcServer = parGroup.find("rpcServer").asString().c_str();
// Connect to RPC server
if (!Network::connect(rpcModule.getName(), rpcServer)) {
cerr << dbgTag << "Could not connect to the specified RPC server: " << rpcServer << ". \n";
return false;
}
} else {
cerr << dbgTag << "Could not find the rpcServer name in the specified configuration file. \n";
return false;
}
} else {
cerr << dbgTag << "Could not find the [rpc] parameter group in the specified configuration file. \n";
return false;
}
/* ******* Read experiment parameters. ******* */
parGroup = rf.findGroup("experiment");
if (!parGroup.isNull()) {
// Number of columns in array
if (parGroup.check("nCols")) {
int nCols = parGroup.find("nCols").asInt();
// Experiment parameters
Bottle *confExpParams = parGroup.find("cmdT").asList();
if (confExpParams != NULL) {
for (int i = 0; i < confExpParams->size(); ++i) {
ExperimentParams tmpPar;
tmpPar.cmd = confExpParams->get(i).asString().c_str();
tmpPar.time = confExpParams->get(i + 1).asDouble();
expParams.push_back(tmpPar);
i++;
}
} else {
cerr << dbgTag << "Could not find the experiment parameter list in the supplied configuration file. \n";
return false;
}
} else {
cerr << dbgTag << "Could not find nCols parameter in supplied configuration file. \n";
return false;
}
} else {
cerr << dbgTag << "Could not find the experiment parameter group [experiment] in the supplied configuration file. \n";
return false;
}
#ifndef NODEBUG
cout << "DEBUG: " << dbgTag << "Experiment parameters are: \n";
for (size_t i = 0; i < expParams.size(); ++i) {
cout << "DEBUG " << dbgTag << expParams[i].cmd << " " << expParams[i].time << "\n";
i++;
}
#endif
cout << dbgTag << "Started correctly. \n";
return true;
}
static void myMain2(GtkButton *button, int *position)
{
std::string robotName;
std::string portLocalName;
std::string portLocalName2;
GtkWidget *main_vbox1 = NULL;
GtkWidget *main_vbox2 = NULL;
GtkWidget *main_vbox3 = NULL;
GtkWidget *main_vbox4 = NULL;
GtkWidget *main_vbox5 = NULL;
Property options;
yarp::os::Network::init();
//retrieve robot name
robotName = gtk_entry_get_text((GtkEntry *)(robotNameBox));
//sprintf(&robotName[0], "%s", gtk_entry_get_text((GtkEntry *)(robotNameBox)));
gtk_widget_destroy (window);
window = NULL;
window = gtk_window_new (GTK_WINDOW_TOPLEVEL);
//Creation of the notebook
GtkWidget* nb1 = gtk_notebook_new();
gtk_container_add (GTK_CONTAINER (window), nb1);
g_signal_connect (window, "destroy",G_CALLBACK (destroy_main), &window);
gtk_window_set_title (GTK_WINDOW (window), "Robot Motor GUI V1.10");
char legsLabel[]= "Legs";
GtkWidget *label;
int n;
PolyDriver *partsdd[MAX_NUMBER_ACTIVATED];
PolyDriver *debugdd[MAX_NUMBER_ACTIVATED];
partMover *partMoverList[MAX_NUMBER_ACTIVATED];
partMover *currentPartMover;
#ifdef CARTESIAN_MOVER
PolyDriver *cartesiandd[MAX_NUMBER_ACTIVATED];
cartesianMover *cartesianMoverList[MAX_NUMBER_ACTIVATED];
cartesianMover *currentCartesianMover;
#endif
for (n = 0; n < NUMBER_OF_AVAILABLE_PARTS; n++)
{
if(*ENA[n] == 1)
{
label = gtk_label_new(partsName[n]);
main_vbox4 = gtk_vbox_new (FALSE, 0);
gint note4 = gtk_notebook_append_page((GtkNotebook*) nb1, main_vbox4, label);
std::string robotPartPort= "/";
robotPartPort += robotName.c_str();
robotPartPort += "/";
robotPartPort += partsName[n];
std::string robotPartDebugPort= "/";
robotPartDebugPort += robotName.c_str();
robotPartDebugPort += "/debug/";
robotPartDebugPort += partsName[n];
//checking existence of the port
int ind = 0;
portLocalName="/";
portLocalName+=robotName.c_str();
portLocalName+="/yarpmotorgui";
char tmp[80];
sprintf(tmp, "%d", ind);
portLocalName+=tmp;
portLocalName+="/";
portLocalName+=partsName[n];
// sprintf(&portLocalName[0], "/%s/gui%d/%s", robotName.c_str(), ind, partsName[n]);
std::string nameToCheck = portLocalName;
nameToCheck += "/rpc:o";
// NameClient &nic=NameClient::getNameClient();
fprintf(stderr, "Checking the existence of: %s \n", nameToCheck.c_str());
// Address adr=nic.queryName(nameToCheck.c_str());
Contact adr=Network::queryName(nameToCheck.c_str());
//Contact c = yarp::os::Network::queryName(portLocalName.c_str());
fprintf(stderr, "ADDRESS is: %s \n", adr.toString().c_str());
while(adr.isValid())
{
ind++;
portLocalName="/";
portLocalName+=robotName.c_str();
portLocalName+="/yarpmotorgui";
char tmp[80];
sprintf(tmp, "%d", ind);
portLocalName+=tmp;
portLocalName+="/";
portLocalName+=partsName[n];
// sprintf(&portLocalName[0], "/%s/gui%d/%s", robotName.c_str(), ind, partsName[n]);
nameToCheck = portLocalName;
nameToCheck += "/rpc:o";
// adr=nic.queryName(nameToCheck.c_str());
adr=Network::queryName(nameToCheck.c_str());
}
options.put("local", portLocalName.c_str());
options.put("device", "remote_controlboard");
options.put("remote", robotPartPort.c_str());
options.put("carrier", "udp");
partsdd[n] = new PolyDriver(options);
if (debug_param_enabled)
{
Property debugOptions;
portLocalName2=portLocalName;
// the following complex line of code performs a substring substitution (see example below)
// "/icub/yarpmotorgui2/right_arm" -> "/icub/yarpmotorgui2/debug/right_arm"
portLocalName2.replace(portLocalName2.find(partsName[n]),strlen(partsName[n]),std::string("debug/")+std::string(partsName[n]));
debugOptions.put("local", portLocalName2.c_str());
debugOptions.put("device", "debugInterfaceClient");
debugOptions.put("remote", robotPartPort.c_str());
debugOptions.put("carrier", "udp");
debugdd[n] = new PolyDriver(debugOptions);
if(debugdd[n]->isValid() == false)
{
fprintf(stderr, "Problems opening the debug client \n");
}
}
else
{
debugdd[n]=0;
}
currentPartMover = new partMover(main_vbox4, partsdd[n], debugdd[n], partsName[n], finder,speedview_param_enabled, enable_calib_all);
if(!(currentPartMover->interfaceError))
{
partMoverList[NUMBER_OF_ACTIVATED_PARTS] = currentPartMover;
NUMBER_OF_ACTIVATED_PARTS++;
}
else
fprintf(stderr, "Trying to exit without starting the GUI \n");
}
}
if (NUMBER_OF_ACTIVATED_PARTS > 0)
{
#ifdef CARTESIAN_MOVER
for (n = 0; n < NUMBER_OF_AVAILABLE_PARTS; n++)
{
if(*ENA[n] == 1)
{
if (finder->check("cartesian"))
{
Bottle bCartesianParts = finder->findGroup("cartesian");
if (bCartesianParts.check(partsName[n]))
{
fprintf(stderr, "Adding cartesian tab %d\n", n);
std::string cartesianPartName;
cartesianPartName = partsName[n];
cartesianPartName += "_cartesian";
label = gtk_label_new(cartesianPartName.c_str());
main_vbox4 = gtk_vbox_new (FALSE, 0);
gint note4 = gtk_notebook_append_page((GtkNotebook*) nb1, main_vbox4, label);
std::string robotPartPort= "/";
robotPartPort = robotPartPort + robotName.c_str() + "/cartesianController/" + partsName[n];
//checking eixstence of the port
int ind = 0;
sprintf(&portLocalName[0], "/%s/yarpmotorgui%d/cartesian/%s", robotName.c_str(), ind, partsName[n]);
// NameClient &nic=NameClient::getNameClient();
std::string nameToCheck = portLocalName.c_str();
nameToCheck += "/rpc:o";
fprintf(stderr, "Checking the existence of: %s \n", nameToCheck.c_str());
// Address adr=nic.queryName(nameToCheck.c_str());
Contact adr = yarp::os::Network::queryName(nameToCheck.c_str());
fprintf(stderr, "ADDRESS is: %s \n", adr.toString().c_str());
while(adr.isValid())
{
ind++;
sprintf(&portLocalName[0], "/%s/yarpmotorgui%d/cartesian/%s", robotName.c_str(), ind, partsName[n]);
nameToCheck=portLocalName.c_str();
nameToCheck += "/rpc:o";
//Contact adr=yarp::os::Network::queryName(portLocalName.c_str());
fprintf(stderr, "Checking the existence of: %s \n", nameToCheck.c_str());
adr=yarp::os::Network::queryName(nameToCheck.c_str());
//adr=nic.queryName(nameToCheck.c_str());
}
options.put("local", portLocalName.c_str()); //local port names
options.put("device", "cartesiancontrollerclient");
options.put("remote", robotPartPort.c_str());
fprintf(stderr, "Trying to open the cartesian PolyDriver...\n");
cartesiandd[n] = new PolyDriver(options);
fprintf(stderr, "Checking the validity of the cartesian PolyDriver...\n");
if(cartesiandd[n]->isValid())
{
currentCartesianMover = new cartesianMover(main_vbox4, cartesiandd[n], partsName[n], finder);
if(!(currentCartesianMover->interfaceError))
{
cartesianMoverList[NUMBER_OF_ACTIVATED_CARTESIAN] = currentCartesianMover;
NUMBER_OF_ACTIVATED_CARTESIAN++;
}
else
fprintf(stderr, "One of the requested cartesian interfaces was not available\n");
}
else
fprintf(stderr, "Cartesian Poly Driver was not valid \n");
}
else
fprintf(stderr, "A cartesian interface was requested but no part was associated \n");
}
else
fprintf(stderr, "GUI was not configured for cartesian \n");
}
}
#endif
if (NUMBER_OF_ACTIVATED_PARTS>0)
{
fprintf(stderr, "Activating tabs \n");
g_signal_connect (nb1, "switch-page",G_CALLBACK(notebook_change), partMoverList);
main_vbox5 = gtk_fixed_new ();
fprintf(stderr, "Created all tab box\n");
label = gtk_label_new("all");
fprintf(stderr, "Appending all tab \n");
gint note5 = gtk_notebook_append_page((GtkNotebook*) nb1, main_vbox5, label);
//Frame
fprintf(stderr, "Appending all frame \n");
GtkWidget *frame1 = gtk_frame_new ("Global joint commands");
gtk_widget_set_size_request (frame1, 250, 550);
gtk_fixed_put (GTK_FIXED (main_vbox5), frame1, 10, 10);
//Button 1 in the panel
buttonGoAll = gtk_button_new_with_mnemonic ("Go ALL!");
gtk_fixed_put (GTK_FIXED(main_vbox5), buttonGoAll, 30, 50);
g_signal_connect (buttonGoAll, "clicked", G_CALLBACK (go_all_click), partMoverList);
gtk_widget_set_size_request(buttonGoAll, 190, 30);
//Button 2 in the panel
buttonSeqAll = gtk_button_new_with_mnemonic ("Run ALL Sequence");
gtk_fixed_put (GTK_FIXED(main_vbox5), buttonSeqAll, 30, 100);
g_signal_connect (buttonSeqAll, "clicked",G_CALLBACK(sequence_all_click), partMoverList);
gtk_widget_set_size_request (buttonSeqAll, 190, 30);
//Button 2time in the panel
buttonSeqAllTime = gtk_button_new_with_mnemonic ("Run ALL Sequence (time)");
gtk_fixed_put (GTK_FIXED(main_vbox5), buttonSeqAllTime, 30, 150);
g_signal_connect (buttonSeqAllTime, "clicked",G_CALLBACK(sequence_all_click_time), partMoverList);
gtk_widget_set_size_request (buttonSeqAllTime, 190, 30);
//Button 3 in the panel
buttonSeqAllSave = gtk_button_new_with_mnemonic ("Save ALL Sequence");
gtk_fixed_put (GTK_FIXED(main_vbox5), buttonSeqAllSave, 30, 200);
g_signal_connect (buttonSeqAllSave, "clicked", G_CALLBACK(sequence_all_save), partMoverList);
gtk_widget_set_size_request (buttonSeqAllSave, 190, 30);
//Button 4 in the panel
buttonSeqAllLoad = gtk_button_new_with_mnemonic ("Load ALL Sequence");
gtk_fixed_put (GTK_FIXED(main_vbox5), buttonSeqAllLoad, 30, 250);
g_signal_connect (buttonSeqAllLoad, "clicked", G_CALLBACK (sequence_all_load), partMoverList);
gtk_widget_set_size_request (buttonSeqAllLoad, 190, 30);
//Button 5 in the panel
buttonSeqAllCycle = gtk_button_new_with_mnemonic ("Cycle ALL Sequence");
gtk_fixed_put (GTK_FIXED(main_vbox5), buttonSeqAllCycle, 30, 300);
g_signal_connect (buttonSeqAllCycle, "clicked", G_CALLBACK (sequence_all_cycle), partMoverList);
gtk_widget_set_size_request (buttonSeqAllCycle, 190, 30);
//Button 5time in the panel
buttonSeqAllCycleTime = gtk_button_new_with_mnemonic ("Cycle ALL Sequence (time)");
gtk_fixed_put (GTK_FIXED(main_vbox5), buttonSeqAllCycleTime, 30, 350);
g_signal_connect (buttonSeqAllCycleTime, "clicked", G_CALLBACK (sequence_all_cycle_time), partMoverList);
gtk_widget_set_size_request (buttonSeqAllCycleTime, 190, 30);
//Button 6 in the panel
buttonSeqAllStop = gtk_button_new_with_mnemonic ("Stop ALL Sequence");
gtk_fixed_put (GTK_FIXED(main_vbox5), buttonSeqAllStop, 30, 400);
g_signal_connect (buttonSeqAllStop, "clicked", G_CALLBACK (sequence_all_stop), partMoverList);
gtk_widget_set_size_request (buttonSeqAllStop, 190, 30);
//Button 6time in the panel
//buttonSeqAllStopTime = gtk_button_new_with_mnemonic ("Stop ALL Sequence (time)");
//gtk_fixed_put (GTK_FIXED(main_vbox5), buttonSeqAllStopTime, 120, 450);
//g_signal_connect (buttonSeqAllStopTime, "clicked", G_CALLBACK (sequence_all_stop_time), partMoverList);
//gtk_widget_set_size_request (buttonSeqAllStopTime, 190, 30);
//Button 6time in the panel
buttonRunAllParts = gtk_button_new_with_mnemonic ("Run ALL Parts");
gtk_fixed_put (GTK_FIXED(main_vbox5), buttonRunAllParts, 30, 450);
g_signal_connect (buttonRunAllParts, "clicked", G_CALLBACK (run_all_parts), partMoverList);
gtk_widget_set_size_request (buttonRunAllParts, 190, 30);
//Button 7 in the panel
buttonHomeAllParts = gtk_button_new_with_mnemonic ("Home ALL Parts");
gtk_fixed_put (GTK_FIXED(main_vbox5), buttonHomeAllParts, 30, 500);
g_signal_connect (buttonHomeAllParts, "clicked", G_CALLBACK (home_all_parts), partMoverList);
gtk_widget_set_size_request (buttonHomeAllParts, 190, 30);
//Global cartesian commands
if(NUMBER_OF_ACTIVATED_CARTESIAN>0)
{
//Frame
fprintf(stderr, "Appending all cartesian frame \n");
GtkWidget *frame2 = gtk_frame_new ("Global cartesian commands");
gtk_widget_set_size_request (frame2, 250, 550);
gtk_fixed_put (GTK_FIXED (main_vbox5), frame2, 300, 10);
//for(int i = 0; i < NUMBER_OF_ACTIVATED_CARTESIAN; i++)
// add_enabled_joints(cartesianMoverList[i], main_vbox5);
//Button 3 in the panel
#ifdef CARTESIAN_MOVER
buttonCrtSeqAllSave = gtk_button_new_with_mnemonic ("Save ALL Cartesian Seq");
gtk_fixed_put (GTK_FIXED(main_vbox5), buttonCrtSeqAllSave, 320, 200);
g_signal_connect (buttonCrtSeqAllSave, "clicked", G_CALLBACK(sequence_crt_all_save), cartesianMoverList);
gtk_widget_set_size_request (buttonCrtSeqAllSave, 190, 30);
//Button 4 in the panel
buttonCrtSeqAllLoad = gtk_button_new_with_mnemonic ("Load ALL Cartesian Seq");
gtk_fixed_put (GTK_FIXED(main_vbox5), buttonCrtSeqAllLoad, 320, 250);
g_signal_connect (buttonCrtSeqAllLoad, "clicked", G_CALLBACK (sequence_crt_all_load), cartesianMoverList);
gtk_widget_set_size_request (buttonCrtSeqAllLoad, 190, 30);
//Button 5time in the panel
buttonCrtSeqAllCycleTime = gtk_button_new_with_mnemonic ("Cycle ALL Cartesian Seq");
gtk_fixed_put (GTK_FIXED(main_vbox5), buttonCrtSeqAllCycleTime, 320, 350);
g_signal_connect (buttonCrtSeqAllCycleTime, "clicked", G_CALLBACK (sequence_crt_all_cycle_time), cartesianMoverList);
gtk_widget_set_size_request (buttonCrtSeqAllCycleTime, 190, 30);
//Button 6 in the panel
buttonCrtSeqAllStop = gtk_button_new_with_mnemonic ("Stop ALL Cartsian Seq");
gtk_fixed_put (GTK_FIXED(main_vbox5), buttonCrtSeqAllStop, 320, 400);
g_signal_connect (buttonCrtSeqAllStop, "clicked", G_CALLBACK (sequence_crt_all_stop), cartesianMoverList);
gtk_widget_set_size_request (buttonCrtSeqAllStop, 190, 30);
#endif
}
// finish & show
// connected_status();
fprintf(stderr, "Resizing window \n");
gtk_window_set_default_size (GTK_WINDOW (window), 480, 250);
gtk_window_set_resizable (GTK_WINDOW (window), true);
}
}
fprintf(stderr, "Making the window visible \n");
if (!GTK_WIDGET_VISIBLE (window))
gtk_widget_show_all (window);
else
{
gtk_widget_destroy (window);
window = NULL;
}
gtk_main ();
fprintf(stderr, "Closing the partMovers. Number of activated parts was %d. \n", NUMBER_OF_ACTIVATED_PARTS);
for (int i = 0; i < NUMBER_OF_ACTIVATED_PARTS; i++)
{
fprintf(stderr, "Closing part number %d \n", i);
partMoverList[i]->releaseDriver();
fprintf(stderr, "Deleting part number %d \n", i);
delete partMoverList[i];
Time::delay(0.1);
}
#if CARTESIAN_MOVER
fprintf(stderr, "Closing the cartesianMovers. Number of activated parts was %d. \n", NUMBER_OF_ACTIVATED_CARTESIAN);
for (int i = 0; i < NUMBER_OF_ACTIVATED_CARTESIAN; i++)
{
fprintf(stderr, "Closing part number %d \n", i);
cartesianMoverList[i]->releaseDriver();
fprintf(stderr, "Deleting part number %d \n", i);
delete cartesianMoverList[i];
Time::delay(0.1);
}
#endif
fprintf(stderr, "Closing the main GUI \n");
Network::fini();
return;
}
int yarp_readdir(const char *path, void *buf, fuse_fill_dir_t filler,
off_t offset, struct fuse_file_info *fi)
{
(void) offset;
(void) fi;
printf(">>>>>>>>>>>READING DIR\n");
filler(buf, ".", NULL, 0);
filler(buf, "..", NULL, 0);
YPath ypath(path);
if (ypath.isPort()) {
filler(buf, "rw", NULL, 0);
//filler(buf, "write", NULL, 0); //deprecated
//filler(buf, "status", NULL, 0);
return 0;
}
NameConfig nc;
ConstString name = nc.getNamespace();
Bottle msg, reply;
msg.addString("bot");
msg.addString("list");
Network::write(name.c_str(),
msg,
reply);
printf("Got %s\n", reply.toString().c_str());
ACE_Ordered_MultiSet<ConstString> lines;
for (int i=1; i<reply.size(); i++) {
Bottle *entry = reply.get(i).asList();
string rpath = path;
if (rpath[rpath.length()-1]!='/') {
rpath = rpath + "/";
}
if (entry!=NULL) {
ConstString name = entry->check("name",Value("")).asString();
if (name!="") {
if (strstr(name.c_str(),rpath.c_str())==
name.c_str()) {
printf(">>> %s is in path %s\n", name.c_str(),
rpath.c_str());
ConstString part(name.c_str()+rpath.length());
if (part[0]=='/') {
part = part.substr(1,part.length()-1);
}
printf(" %s is the addition\n", part.c_str());
char *brk = (char*)strstr(part.c_str(),"/");
if (brk!=NULL) {
*brk = '\0';
}
ConstString item(part.c_str());
printf(" %s is the item\n", item.c_str());
if (item!="") {
lines.remove(item);
lines.insert(item);
}
}
}
}
}
// return result in alphabetical order
ACE_Ordered_MultiSet_Iterator<ConstString> iter(lines);
iter.first();
while (!iter.done()) {
printf("adding item %s\n", (*iter).c_str());
filler(buf, (*iter).c_str(), NULL, 0);
iter.advance();
}
return 0;
}
void seriesPID::setOptions(const Bottle &options)
{
Vector satLimVect(satLim.rows()*satLim.cols());
for (int r=0; r<satLim.rows(); r++)
for (int c=0; c<satLim.cols(); c++)
satLimVect[r*satLim.cols()+c]=satLim(r,c);
bool recomputeQuantities=false;
int size;
if (getVectorFromOption(options,"Kp",Kp,size))
recomputeQuantities=true;
if (getVectorFromOption(options,"Ti",Ti,size))
recomputeQuantities=true;
if (getVectorFromOption(options,"Kd",Kd,size))
recomputeQuantities=true;
if (getVectorFromOption(options,"N",N,size))
recomputeQuantities=true;
if (getVectorFromOption(options,"satLim",satLimVect,size))
{
for (int r=0; r<satLim.rows(); r++)
for (int c=0; c<satLim.cols(); c++)
satLimVect[r*satLim.cols()+c]=satLim(r,c);
recomputeQuantities=true;
}
if (options.check("Ts"))
{
double _Ts=options.find("Ts").asDouble();
if (_Ts>0.0)
{
Ts=_Ts;
recomputeQuantities=true;
}
}
if (recomputeQuantities)
{
for (unsigned int i=0; i<dim; i++)
uSat[i]=PID_SAT(uSat[i],satLim(i,0),satLim(i,1));
Vector u0(1,0.0);
Vector num(2),den(2);
for (unsigned int i=0; i<dim; i++)
{
num[0]=Ts; num[1]=Ts;
den[0]=Ts+2.0*Ti[i]; den[1]=Ts-2.0*Ti[i];
Int[i]->adjustCoeffs(num,den);
Int[i]->init(u0);
if ((Kp[i]!=0.0) && (N[i]!=0.0))
{
double tau=Kd[i]/(Kp[i]*N[i]);
num[0]=2.0; num[1]=-2.0;
den[0]=Ts+2.0*tau; den[1]=Ts-2.0*tau;
}
else
{
num[0]=num[1]=den[1]=0.0;
den[0]=1.0;
}
Der[i]->adjustCoeffs(num,den);
Der[i]->init(u0);
}
}
if (options.check("reset"))
reset();
}
bool yarp::os::impl::HttpCarrier::reply(Protocol& proto, SizedWriter& writer) {
DummyConnector con;
con.setTextMode(true);
for (size_t i=writer.headerLength(); i<writer.length(); i++) {
con.getWriter().appendBlock(writer.data(i),writer.length(i));
}
Bottle b;
b.read(con.getReader());
ConstString mime = b.check("mime",Value("text/html")).asString();
ConstString body;
bool using_json = false;
if (stream!=NULL) {
if (stream->useJson()) {
mime = "text/json";
asJson(body,&b,stream->typeHint());
using_json = true;
}
}
if (b.check("web")&&!using_json) {
body = b.find("web").toString();
}
if (b.check("stream")&&!using_json) {
String header("HTTP/1.1 200 OK\r\nContent-Type: ");
header += mime;
header += "\r\n";
header += "Transfer-Encoding: chunked\r\n";
header += "\r\n";
int N = 2*1024;
header += NetType::toHexString(body.length()+N);
header += "\r\n";
Bytes b2((char*)header.c_str(),header.length());
proto.os().write(b2);
// chrome etc won't render until enough chars are received.
for (int i=0; i<N; i++) {
proto.os().write(' ');
}
Bytes b3((char*)body.c_str(),body.length());
proto.os().write(b3);
proto.os().write('\r');
proto.os().write('\n');
if (stream!=NULL) {
stream->flip();
}
return true;
}
if (stream!=NULL) {
stream->finish();
}
// Could check response codes, mime types here.
if (body.length()!=0 || using_json) {
ConstString mime = b.check("mime",Value("text/html")).asString();
String header("HTTP/1.1 200 OK\nContent-Type: ");
header += mime;
header += "\n";
header += "Access-Control-Allow-Origin: *\n";
header += "\n";
Bytes b2((char*)header.c_str(),header.length());
proto.os().write(b2);
//body = b.toString();
Bytes b3((char*)body.c_str(),body.length());
proto.os().write(b3);
} else {
writer.write(proto.os());
}
proto.os().flush();
return proto.os().isOk();
}