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;
}
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 look()
{
Bottle cmd;
Bottle response;
cmd.addString("look");
port.write(cmd,response);
// pick out map part
world= response.findGroup("look").findGroup("map");
Bottle &users = response.findGroup("look").findGroup("players");
Bottle *player = users.get(1).asList();
if (player!=0)
{
Bottle &location = player->findGroup("location");
Value &life = player->find("life");
std::string playerName = player->get(0).asString();
myX=location.get(1).asInt32(),
myY=location.get(2).asInt32(),
myLife=life.asInt32();
}
}
//---------------------------------------------------------
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 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;
}
}
// TODO use it!!
//#warning "Use extractGroup to verify size of parameters matches with number of joints, this will avoid crashes"
static bool extractGroup(Bottle &input, Bottle &out, const std::string &key1, const std::string &txt, int size)
{
size++; // size includes also the name of the parameter
Bottle &tmp=input.findGroup(key1.c_str(), txt.c_str());
if (tmp.isNull())
{
yError () << key1.c_str() << " not found\n";
return false;
}
if(tmp.size()!=size)
{
yError () << key1.c_str() << " incorrect number of entries in board.";
return false;
}
out=tmp;
return true;
}
PidData getPidData(Bottle &bGroup, const int i)
{
PidData pid;
ostringstream joint;
joint<<"joint_"<<i;
Bottle &bJoint=bGroup.findGroup(joint.str().c_str());
pid.Kp=bJoint.check("Kp",Value(0.0)).asDouble();
pid.Ki=bJoint.check("Ki",Value(0.0)).asDouble();
pid.Kd=bJoint.check("Kd",Value(0.0)).asDouble();
pid.scale=bJoint.check("scale",Value(0.0)).asDouble();
pid.st_up=bJoint.check("st_up",Value(0.0)).asDouble();
pid.st_down=bJoint.check("st_down",Value(0.0)).asDouble();
pid.encs_ratio=bJoint.check("encs_ratio",Value(1.0)).asDouble();
pid.status=(bJoint.check("status",Value("download")).asString()=="download"?download:upload);
if (bJoint.check("idling"))
{
if (Bottle *bIdlingJoints=bJoint.find("idling").asList())
{
pid.idling_joints.clear();
for (int j=0; j<bIdlingJoints->size(); j++)
{
int k=bIdlingJoints->get(j).asInt();
int l;
for (l=0; l<rJoints.size(); l++)
{
if (rJoints.get(l).asInt()==k)
{
pid.idling_joints.push_back(k);
break;
}
}
if (l>=rJoints.size())
yError("unrecognized joint %d to put in idle",k);
}
}
}
return pid;
}
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();
}
}
}
//generic function that check is key1 is present in input bottle and that the result has size elements
// return true/false
bool RobotranYarpMotionControl::extractGroup(Bottle &input, Bottle &out, const std::string &key1, const std::string &txt, int size)
{
Bottle &tmp=input.findGroup(key1.c_str(), txt.c_str());
if (tmp.isNull())
{
//yError () << key1.c_str() << " not found\n"; //yError() will be included in the next version of yarp.
std::cout << key1.c_str() << " not found\n";
return false;
}
if(tmp.size()!=size)
{
// yError () << key1.c_str() << " incorrect number of entries";
std::cout << key1.c_str() << " incorrect number of entries";
return false;
}
out=tmp;
return true;
}
//generic function that check is key1 is present in input bottle and that the result has size elements
// return true/false
static inline bool extractGroup(Bottle &input, Bottle &out, const std::string &key1, const std::string &txt, int size)
{
size++; // size includes also the name of the parameter
Bottle &tmp=input.findGroup(key1.c_str(), txt.c_str());
if (tmp.isNull())
{
fprintf(stderr, "%s not found\n", key1.c_str());
return false;
}
if(tmp.size()!=size)
{
fprintf(stderr, "%s incorrect number of entries\n", key1.c_str());
return false;
}
out=tmp;
return true;
}
static bool checkParam(const Bottle& input, RGBDSensorParamParser::RGBDParam& param, bool& found)
{
bool ret = false;
Bottle bt=input.findGroup(param.name).tail(); // the first element is the name of the parameter
if (!bt.isNull())
{
Bottle* b;
if (param.size>1 && bt.size()==1)
{
b = bt.get(0).asList();
}
else
b = &bt;
if (b->isNull())
{
yError()<<"RGBDSensorParamParser: check"<<param.name<<"in config file";
return false;
}
if (b->size() != param.size)
{
yError() << "RGBDSensorParamParser: parameter" << param.name << "size should be" << param.size;
return false;
}
param.val.resize(param.size);
for (size_t i=0;i<b->size();i++)
{
ret = true;
param.val[i] = b->get(i);
found = true;
}
}
else
{
ret = true;
found = false;
}
return ret;
}
void fromCommand(int argc, char *argv[],bool wipe=true) {
String tag = "";
Bottle accum;
Bottle total;
bool qualified = false;
for (int i=0; i<argc; i++) {
String work = argv[i];
bool isTag = false;
if (work.length()>=2) {
if (work[0]=='-'&&work[1]=='-') {
work = work.substr(2,work.length()-2);
isTag = true;
if (work.find("::")!=String::npos) {
qualified = true;
}
}
}
if (isTag) {
if (tag!="") {
total.addList().copy(accum);
}
tag = work;
accum.clear();
} else {
if (work.find("\\")!=String::npos) {
// Specifically when reading from the command
// line, we will allow windows-style paths.
// Hence we have to break the "\" character
String buf = "";
for (unsigned int i=0; i<work.length(); i++) {
buf += work[i];
if (work[i]=='\\') {
buf += work[i];
}
}
work = buf;
}
}
accum.add(Value::makeValue(work.c_str()));
}
if (tag!="") {
total.addList().copy(accum);
}
if (!qualified) {
fromBottle(total,wipe);
return;
}
if (wipe) {
clear();
}
Bottle *cursor = NULL;
for (int i=0; i<total.size(); i++) {
cursor = NULL;
Bottle *term = total.get(i).asList();
if (!term) continue;
ConstString key = term->get(0).asString();
ConstString base = key;
while (key.length()>0) {
int at = key.find("::");
base = key;
if (at>=0) {
base = key.substr(0,at);
key = key.substr(at+2);
} else {
key = "";
}
Bottle& result = (cursor!=NULL)? (cursor->findGroup(base.c_str())) : owner.findGroup(base.c_str());
if (result.isNull()) {
if (!cursor) {
cursor = &putBottle((base).c_str());
} else {
cursor = &cursor->addList();
}
cursor->addString(base);
} else {
cursor = &result;
}
}
if (cursor) {
cursor->copy(*term);
cursor->get(0) = Value(base);
}
}
}
bool embObjInertials::sendConfig2MTBboards(Searchable& globalConfig)
{
eOprotID32_t id32 = eo_prot_ID32dummy;
// configuration specific for skin-inertial device
Bottle config = globalConfig.findGroup("GENERAL");
// prepare the config of the inertial sensors: datarate and the mask of the enabled ones.
eOas_inertial_sensorsconfig_t inertialSensorsConfig = {0};
inertialSensorsConfig.accelerometers = 0;
inertialSensorsConfig.gyroscopes = 0;
inertialSensorsConfig.datarate = _period;
// meglio sarebbe mettere un controllo sul fatto che ho trovato enabledAccelerometers e che ha size coerente
Bottle sensors;
sensors = config.findGroup("enabledAccelerometers");
int numofaccelerometers = sensors.size()-1; // sensors holds strings "enabledAccelerometers" and then all the others, thus i need a -1
for(int i=0; i<numofaccelerometers; i++)
{
eOas_inertial_position_t pos = eoas_inertial_pos_none;
yarp::os::ConstString strpos = sensors.get(i+1).asString();
pos = getLocationOfInertialSensor(strpos); // prendi la posizione dalla stringa strpos: fai una funzione apposita
if(eoas_inertial_pos_none != pos)
{
_fromInertialPos2DataIndexAccelerometers[pos] = i;
inertialSensorsConfig.accelerometers |= EOAS_ENABLEPOS(pos);
}
}
sensors = config.findGroup("enabledGyroscopes");
int numofgyroscopess = sensors.size()-1; // sensors holds strings "enabledGyroscopes" and then all the others, thus i need a -1
for(int i=0; i<numofgyroscopess; i++)
{
eOas_inertial_position_t pos = eoas_inertial_pos_none;
yarp::os::ConstString strpos = sensors.get(i+1).asString();
pos = getLocationOfInertialSensor(strpos); // prendi la posizione dalla stringa strpos: fai una funzione apposita
if(eoas_inertial_pos_none != pos)
{
_fromInertialPos2DataIndexGyroscopes[pos] = numofaccelerometers+i;
inertialSensorsConfig.gyroscopes |= EOAS_ENABLEPOS(pos);
}
}
// configure the sensors (datarate and position)
id32 = eoprot_ID_get(eoprot_endpoint_analogsensors, eoprot_entity_as_inertial, 0, eoprot_tag_as_inertial_config_sensors);
if(false == res->setRemoteValueUntilVerified(id32, &inertialSensorsConfig, sizeof(inertialSensorsConfig), 10, 0.010, 0.050, 2))
{
yError() << "FATAL: embObjInertials::sendConfig2MTBboards() had an error while calling setRemoteValueUntilVerified() for config in BOARD" << res->getName() << "with IP" << res->getIPv4string();
return false;
}
else
{
if(verbosewhenok)
{
yDebug() << "embObjInertials::sendConfig2MTBboards() correctly configured enabled sensors with period" << _period << "in BOARD" << res->getName() << "with IP" << res->getIPv4string();
}
}
// // start the configured sensors
// eOmc_inertial_commands_t startCommand = {0};
// startCommand.enable = 1;
// id32 = eoprot_ID_get(eoprot_endpoint_analogsensors, eoprot_entity_as_inertial, 0, eoprot_tag_as_inertial_cmmnds_enable);
// if(!res->addSetMessage(id32, (uint8_t*) &startCommand))
// {
// yError() << "ethResources::sendConfig2MTBboards() fails to command the start transmission of the inertials";
// return false;
// }
return true;
}
ethResources* TheEthManager::getResource(yarp::os::Searchable &config)
{
std::string str;
Bottle xtmp2;
ACE_TCHAR remote_address[64];
ACE_TCHAR address_tmp_string[64];
ACE_UINT16 rem_port;
ACE_UINT32 rem_ip1,rem_ip2,rem_ip3,rem_ip4;
if(config.findGroup("GENERAL").find("Verbose").asInt())
{str=config.toString().c_str();}
else
{str=" ";}
yTrace() << str;
/* Get a Socket for the PC104 This needs a reading from config file, at least for the port to use...
* If the socket is already initialized the createSocket function does nothing, it is also thread safe */
ACE_UINT32 loc_ip1,loc_ip2,loc_ip3,loc_ip4;
xtmp2 = config.findGroup("PC104IpAddress");
strcpy(address_tmp_string, xtmp2.get(1).asString().c_str());
// ACE format
sscanf(address_tmp_string,"%d.%d.%d.%d",&loc_ip1, &loc_ip2, &loc_ip3, &loc_ip4);
ACE_INET_Addr loc_dev(rem_port, (loc_ip1<<24)|(loc_ip2<<16)|(loc_ip3<<8)|loc_ip4);
if(!createSocket(loc_dev))
{return false;}
//
// Get EMS ip addresses from config file, to see if we need to instantiate a new Resources or simply return
// a pointer to an already existing object
//
Bottle xtmp = Bottle(config.findGroup("ETH"));
xtmp2 = xtmp.findGroup("IpAddress");
strcpy(remote_address, xtmp2.get(1).asString().c_str());
sscanf(remote_address,"%d.%d.%d.%d",&rem_ip1, &rem_ip2, &rem_ip3, &rem_ip4);
// Get EMS CmdPort from config file
xtmp2 = xtmp.findGroup("CmdPort");
rem_port = xtmp2.get(1).asInt();
// ACE format
ACE_INET_Addr remote_addr_tmp;
remote_addr_tmp.set(rem_port, (rem_ip1<<24)|(rem_ip2<<16)|(rem_ip3<<8)|rem_ip4);
ethResources *newRes = NULL;
managerMutex.wait();
ethResIt iterator = EMS_list.begin();
while(iterator != EMS_list.end())
{
if((*iterator)->getRemoteAddress() == remote_addr_tmp)
{
// device already exist.
newRes = (*iterator);
break;
}
iterator++;
}
if(NULL == newRes)
{
// device doesn't exist yet, create it
yTrace() << "Creating EMS device with IP " << remote_address;
newRes = new ethResources;
if(!newRes->open(config))
{
printf("Error creating new EMS!!");
if(NULL != newRes)
delete newRes;
newRes = NULL;
}
else
{
nBoards++;
EMS_list.push_back(newRes);
}
}
managerMutex.post();
return newRes;
}
void YarpPluginSelector::scan() {
config.clear();
plugins.clear();
search_path.clear();
NetworkBase::lock();
Property& state = NameClient::getNameClient().getPluginState();
config = state;
NetworkBase::unlock();
bool need_scan = true;
if (config.check("last_update_time")) {
if (Time::now()-config.find("last_update_time").asDouble()<5) {
need_scan = false;
}
}
if (need_scan) {
YARP_SPRINTF0(Logger::get(),
debug,
"Scanning. I'm scanning. I hope you like scanning too.");
NetworkBase::lock();
ResourceFinder& rf = ResourceFinder::getResourceFinderSingleton();
if (!rf.isConfigured()) {
rf.configure(0,YARP_NULLPTR);
}
rf.setQuiet(true);
Bottle plugins = rf.findPaths("plugins");
if (plugins.size()==0) {
plugins = rf.findPaths("share/yarp/plugins");
}
if (plugins.size()>0) {
for (int i=0; i<plugins.size(); i++) {
ConstString target = plugins.get(i).asString();
YARP_SPRINTF1(Logger::get(),
debug,
"Loading configuration files related to plugins from %s.", target.c_str());
config.fromConfigDir(target, "inifile", false);
}
} else {
YARP_SPRINTF0(Logger::get(),
debug,
"Plugin directory not found");
}
config.put("last_update_time",Time::now());
state = config;
NetworkBase::unlock();
}
Bottle inilst = config.findGroup("inifile").tail();
for (int i=0; i<inilst.size(); i++) {
ConstString inifile = inilst.get(i).asString();
Bottle inigroup = config.findGroup(inifile);
Bottle lst = inigroup.findGroup("plugin").tail();
for (int i=0; i<lst.size(); i++) {
ConstString plugin_name = lst.get(i).asString();
Bottle group = inigroup.findGroup(plugin_name);
group.add(Value::makeValue(ConstString("(inifile \"") + inifile + "\")"));
if (select(group)) {
plugins.addList() = group;
}
}
lst = inigroup.findGroup("search").tail();
for (int i=0; i<lst.size(); i++) {
ConstString search_name = lst.get(i).asString();
Bottle group = inigroup.findGroup(search_name);
search_path.addList() = group;
}
}
}
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;
}
bool homeostaticModule::respond(const Bottle& cmd, Bottle& reply)
{
if (cmd.get(0).asString() == "help" )
{ string help = "\n";
help += " ['par'] ['drive'] ['val'/'min'/'max'/'dec'] [value] : Assigns a value to a specific parameter \n";
help += " ['delta'] ['drive'] ['val'/'min'/'max'/'dec'] [value] : Adds a value to a specific parameter \n";
help += " ['add'] ['conf'] [drive Bottle] : Adds a drive to the manager as a drive directly read from conf-file \n";
help += " ['add'] ['botl'] [drive Bottle] : Adds a drive to the manager as a Bottle of values of shape \n";
help += " ['add'] ['new'] [drive name] : Adds a default drive to the manager \n";
help += " ['rm'] [drive name] : removes a drive from the manager \n";
help += " ['names'] : returns an ordered list of the drives in the manager \n";
help += " : (string name, double value, double homeo_min, double homeo_max, double decay = 0.05, bool gradient = true) \n";
reply.addString(help);
/*cout << " [par] [drive] [val/min/max/dec] [value] : Assigns a value to a specific parameter \n"<<
" [delta] [drive] [val/min/max/dec] [value] : Adds a value to a specific parameter \n"<<
" [add] [conf] [drive Bottle] : Adds a drive to the manager as a drive directly read from conf-file \n"<<
" [add] [botl] [drive Bottle] : Adds a drive to the manager as a Bottle of values of shape \n"<<
" : (string name, double value, double homeo_min, double homeo_max, double decay = 0.05, bool gradient = true) \n"<<endl;
*/
}
else if (cmd.get(0).asString() == "par" )
{
for (unsigned int d = 0; d<manager.drives.size();d++)
{
if (cmd.get(1).asString() == manager.drives[d]->name)
{
if (cmd.get(2).asString()=="val")
{
manager.drives[d]->setValue(cmd.get(3).asDouble());
}
else if (cmd.get(2).asString()=="min")
{
manager.drives[d]->setHomeostasisMin(cmd.get(3).asDouble());
}
else if (cmd.get(2).asString()=="max")
{
manager.drives[d]->setHomeostasisMax(cmd.get(3).asDouble());
}
else if (cmd.get(2).asString()=="dec")
{
manager.drives[d]->setDecay(cmd.get(3).asDouble());
}
else
{
reply.addString("Format is: \n - ['par'] [drive_name] [val/min/max/dec] [value]");
}
reply.addString("ack");
cout<<"Received a order"<<endl;
}
}
reply.addString("nack: wrong drive name");
}
else if (cmd.get(0).asString() == "delta" )
{
for (unsigned int d = 0; d<manager.drives.size();d++)
{
if (cmd.get(1).asString() == manager.drives[d]->name)
{
if (cmd.get(2).asString()=="val")
{
manager.drives[d]->deltaValue(cmd.get(3).asDouble());
}
else if (cmd.get(2)=="min")
{
manager.drives[d]->deltaHomeostasisMin(cmd.get(3).asDouble());
}
else if (cmd.get(2)=="max")
{
manager.drives[d]->deltaHomeostasisMax(cmd.get(3).asDouble());
}
else if (cmd.get(2)=="dec")
{
manager.drives[d]->deltaDecay(cmd.get(3).asDouble());
}
else
{
reply.addString("nack");
cout << "Format is: \n - ['par'] [drive_name] [val/min/max/dec] [value]"<<endl;
}
reply.addString("ack");
cout<<"Received a order"<<endl;
}
}
}
else if (cmd.get(0).asString()=="add")
{
if (cmd.get(1).asString()=="conf")
{
Bottle *ga = cmd.get(2).asList();
// cout <<"HOMEOSTATIC"<< ga->toString() << endl;
Bottle grpAllostatic = ga->findGroup("ALLOSTATIC");
cout <<"HOMEOSTATIC"<< grpAllostatic.toString() << endl;
cout << "Bottle allo check: " << grpAllostatic.find("test-decay").asDouble() << endl;
addNewDrive(cmd.get(2).check("name",yarp::os::Value("")).asString(), grpAllostatic);
reply.addString("add drive from config bottle: ack");
}
else if (cmd.get(1).asString()=="botl")
{
Drive d = bDrive(cmd.get(2).asList());
manager.addDrive(&d);
openPorts(d.name);
reply.addString("add drive from bottle: ack");
}
else if (cmd.get(1).asString()=="new")
{
string d_name = cmd.get(2).asString();
bool b = addNewDrive(d_name);
if (b)
reply.addString("add new drive: ack");
else
reply.addString("ack. drive not created");
}
else
{
reply.addString("nack");
}
}
else if (cmd.get(0).asString()=="rm")
{
for (unsigned int d = 0; d<manager.drives.size();d++)
{
if (cmd.get(1).asString() == manager.drives[d]->name)
{
bool b = removeDrive(d);
if (b)
reply.addString("ack: Successfully removed");
else
reply.addString("ack: Could not remove the drive");
}
}
reply.addString("nack");
}
else if (cmd.get(0).asString()=="names")
{
Bottle nms;
nms.clear();
for (unsigned int i=0;i<manager.drives.size();i++)
{
nms.addString(manager.drives[i]->name);
}
reply.addList()=nms;
}
else
{
reply.addString("nack");
}
return true;
}
bool embObjInertials::configServiceInertials(Searchable& globalConfig)
{
// find SERVICES-INERTIALS. if not found, exit mildly from function. in a first stage the remote eth board will already be configured.
Bottle tmp = globalConfig.findGroup("SERVICES");
Bottle config = tmp.findGroup("INERTIALS");
// prepare the config of the inertial sensors: datarate and the mask of the enabled ones.
eOas_inertial_serviceconfig_t inertialServiceConfig = {0}; // by this initialisation, there is no sensor at all in the two can buses
// meglio sarebbe mettere un controllo sul fatto che ho trovato InertialsCAN1mapping e che ha size coerente
Bottle canmap;
int numofentries = 0;
// CAN1
canmap = config.findGroup("InertialsCAN1mapping");
numofentries = canmap.size()-1;
for(int i=0; i<numofentries; i++)
{
eOas_inertial_position_t pos = eoas_inertial_pos_none;
yarp::os::ConstString strpos = canmap.get(i+1).asString();
pos = getLocationOfInertialSensor(strpos); // prendi la posizione dalla stringa strpos: fai una funzione apposita
inertialServiceConfig.canmapofsupportedsensors[0][i] = pos;
}
// CAN2
canmap = config.findGroup("InertialsCAN2mapping");
numofentries = canmap.size()-1;
for(int i=0; i<numofentries; i++)
{
eOas_inertial_position_t pos = eoas_inertial_pos_none;
yarp::os::ConstString strpos = canmap.get(i+1).asString();
pos = getLocationOfInertialSensor(strpos); // prendi la posizione dalla stringa strpos: fai una funzione apposita
inertialServiceConfig.canmapofsupportedsensors[1][i] = pos;
}
// configure the service
eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_analogsensors, eoprot_entity_as_inertial, 0, eoprot_tag_as_inertial_config_service);
if(false == res->setRemoteValueUntilVerified(id32, &inertialServiceConfig, sizeof(inertialServiceConfig), 10, 0.010, 0.050, 2))
{
yError() << "FATAL: embObjInertials::configServiceInertials() had an error while calling setRemoteValueUntilVerified() for config in BOARD" << res->getName() << "with IP" << res->getIPv4string();
return false;
}
else
{
if(verbosewhenok)
{
yDebug() << "embObjInertials::configServiceInertials() correctly configured the service in BOARD" << res->getName() << "with IP" << res->getIPv4string();
}
}
return true;
}
bool embObjInertials::fromConfig(yarp::os::Searchable &_config)
{
Bottle xtmp;
// Analog Sensor stuff
Bottle config = _config.findGroup("GENERAL");
if (!extractGroup(config, xtmp, "Period","transmitting period of the sensors", 1))
{
yError() << "embObjInertials Using default value = 0 (disabled)";
_period = 0;
}
else
{
_period = xtmp.get(1).asInt();
yDebug() << "embObjInertials::fromConfig() detects embObjInertials Using value of" << _period;
}
#if 0
if (!extractGroup(config, xtmp, "NumberOfSensors","Number of sensors managed", 1))
{
yWarning("embObjInertials: Using default value = 1 for _numofsensors\n");
_numofsensors = 1;
}
else
{
_numofsensors = xtmp.get(1).asInt();
}
#endif
_numofsensors = 1;
// if we have a AS_Type_INERTIAL_MTB, then we may have more than one. for sure not zero.
{
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 expressed as strings", 1))
{
yWarning("embObjInertials: cannot find enabledAccelerometers\n");
_numofsensors = 1;
}
else
{
_numofsensors = xtmp.size();
}
#endif
}
return true;
}
bool FrameTransformServer::parseStartingTf(yarp::os::Searchable &config)
{
if (config.check("USER_TF"))
{
Bottle group = config.findGroup("USER_TF").tail();
for (size_t i = 0; i < group.size(); i++)
{
string tfName;
FrameTransform t;
Bottle b;
Bottle* list = group.get(i).asList();
if(!list)
{
yError() << "no entries in USER_TF group";
return false;
}
tfName = list->get(0).asString();
b = group.findGroup(tfName).tail();
string s = b.toString();
if(b.size() == 18)
{
bool r(true);
Matrix m(4, 4);
for(int i = 0; i < 16; i++)
{
if(!b.get(i).isFloat64())
{
yError() << "transformServer: param " << tfName << ", element " << i << " is not a double.";
r = false;
}
else
{
m.data()[i] = b.get(i).asFloat64();
}
}
if(!b.get(16).isString() || !b.get(17).isString())
{
r = false;
}
if(!r)
{
yError() << "transformServer: param" << tfName << "not correct.. for the 4x4 matrix mode" <<
"you must provide 18 parameter. the matrix, the source frame(string) and the destination frame(string)";
return false;
}
t.fromMatrix(m);
t.src_frame_id = b.get(16).asString();
t.dst_frame_id = b.get(17).asString();
}
else if( b.size() == 8 &&
b.get(0).isFloat64() &&
b.get(1).isFloat64() &&
b.get(2).isFloat64() &&
b.get(3).isFloat64() &&
b.get(4).isFloat64() &&
b.get(5).isFloat64() &&
b.get(6).isString() &&
b.get(7).isString())
{
t.translation.set(b.get(0).asFloat64(), b.get(1).asFloat64(), b.get(2).asFloat64());
t.rotFromRPY(b.get(3).asFloat64(), b.get(4).asFloat64(), b.get(5).asFloat64());
t.src_frame_id = b.get(6).asString();
t.dst_frame_id = b.get(7).asString();
}
else
{
yError() << "transformServer: param" << tfName << "not correct.. a tf requires 8 param in the format:" <<
"x(dbl) y(dbl) z(dbl) r(dbl) p(dbl) y(dbl) src(str) dst(str)";
return false;
}
if(m_yarp_static_transform_storage->set_transform(t))
{
yInfo() << tfName << "from" << t.src_frame_id << "to" << t.dst_frame_id << "succesfully set";
}
}
return true;
}
else
{
yInfo() << "transformServer: no starting tf found";
}
return true;
}
