void configure_search(RosTypeSearch& env, Searchable& p) {
if (p.check("out")) {
env.setTargetDirectory(p.find("out").toString().c_str());
}
if (p.check("web",Value(0)).asInt()!=0 || p.findGroup("web").size()==1) {
env.allowWeb();
}
if (p.check("soft",Value(0)).asInt()!=0 || p.findGroup("soft").size()==1) {
env.softFail();
}
env.lookForService(p.check("service"));
}
bool ServerSerial::open(Searchable& prop)
{
verb = (prop.check("verbose",Value(0),"Specifies if the device is in verbose mode (0/1).").asInt())>0;
if (verb)
printf("running with verbose output\n");
Value *name;
if (prop.check("subdevice",name,"name of specific control device to wrap")) {
printf("Subdevice %s\n", name->toString().c_str());
if (name->isString()) {
// maybe user isn't doing nested configuration
Property p;
p.setMonitor(prop.getMonitor(),
"subdevice"); // pass on any monitoring
p.fromString(prop.toString());
p.put("device",name->toString());
poly.open(p);
} else {
Bottle subdevice = prop.findGroup("subdevice").tail();
poly.open(subdevice);
}
if (!poly.isValid()) {
printf("cannot make <%s>\n", name->toString().c_str());
}
} else {
printf("\"--subdevice <name>\" not set for server_serial\n");
return false;
}
if (!poly.isValid()) {
return false;
}
ConstString rootName =
prop.check("name",Value("/serial"),
"prefix for port names").asString().c_str();
command_buffer.attach(toDevice);
reply_buffer.attach(fromDevice);
command_buffer.useCallback(callback_impl);
toDevice.open((rootName+"/in").c_str());
fromDevice.open((rootName+"/out").c_str());
if (poly.isValid())
poly.view(serial);
if(serial != NULL) {
start();
return true;
}
printf("subdevice <%s> doesn't look like a serial port (no appropriate interfaces were acquired)\n",
name->toString().c_str());
return false;
}
static bool checkForCarrier(const Bytes *header, Searchable& group) {
Bottle code = group.findGroup("code").tail();
if (code.size()==0) return false;
if (matchCarrier(header,code)) {
ConstString name = group.find("name").asString();
if (NetworkBase::registerCarrier(name.c_str(),NULL)) {
return true;
}
}
return false;
}
bool AcousticMap::open(Searchable& config) {
bool ok = true;
if(!config.check("name")) {
std::cout << "AuditoryMap: Error, module base name not found in configuration. Start the module with the --name option.." << std::endl;
return false;
}
// module base name
std::string strModuleName = std::string(config.find("name").asString().c_str());
// look for group EGO_SPHERE_ACOUSTIC_MAP
Bottle botConfigAcoustic(config.toString().c_str());
botConfigAcoustic.setMonitor(config.getMonitor());
if (!config.findGroup("EGO_SPHERE_ACOUSTIC_MAP").isNull()) {
botConfigAcoustic.clear();
botConfigAcoustic.fromString(config.findGroup("EGO_SPHERE_ACOUSTIC_MAP", "Loading visual map configuration from group EGO_SPHERE_ACOUSTIC_MAP.").toString());
}
_salienceDecayRate = botConfigAcoustic.check("decayAcoustic",
Value(0.95),
"Decay for the acoustic saliency map (double).").asDouble();
_resXAcoustic = botConfigAcoustic.check("resXAcoustic",
Value(80),
"Width of internal acoustic map (int)").asInt();
_resYAcoustic = botConfigAcoustic.check("resYAcoustic",
Value(60),
"Height of internal acoustic map (int)").asInt();
_imgCart.resize(_resXAcoustic,_resYAcoustic);
_imgRemapX.resize(_resXAcoustic,_resYAcoustic);
_imgRemapY.resize(_resXAcoustic,_resYAcoustic);
_imgSpher.resize(_resXAcoustic,_resYAcoustic);
_imgMapResA.resize(_resXAcoustic,_resYAcoustic);
ok = ok && _prtVctSound.open(std::string(strModuleName + std::string("/mapAuditory/vct_in")).c_str());
return ok;
}
virtual bool open(Searchable& config)
{
int i;
char portname[10];
if (config.check("help","if present, display usage message")) {
printf("Call with --name </portprefix> --file <configfile.ini>\n");
return false;
}
cols = config.check("width", 640, "Number of image columns").asInt();
lines = config.check("height", 480, "Number of image lines").asInt();
levels = config.check("levels", 4, "Number of scales in the scale space").asInt();
scales = new double[levels];
Bottle &xtmp = config.findGroup("scales");
if(xtmp.size() != levels+1)
for (i = 0; i < levels; i++)
scales[i] = pow(2.0,i+1);
for (i = 1; i < xtmp.size(); i++)
scales[i-1] = xtmp.get(i).asDouble();
ss.AllocateResources(lines, cols, levels, scales);
infloat = cvCreateImage(cvSize(cols,lines), IPL_DEPTH_32F, 1);
ingray = cvCreateImage(cvSize(cols,lines), IPL_DEPTH_8U, 1);
outgray = cvCreateImage(cvSize(cols,lines), IPL_DEPTH_8U, 1);
outfloat = cvCreateImageHeader(cvSize(cols,lines), IPL_DEPTH_32F, 1);
portIn.open(getName("in"));
portOut = new BufferedPort<ImageOf<PixelMono> >[levels];
for( i = 1; i <= levels; i++ )
{
sprintf(portname, "out:%d", i);
portOut[i-1].open(getName(portname));
}
return true;
};
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;
}
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 ServiceParser::parseGlobalBoardVersions(Searchable &config)
{
// format is BOARDVERSIONS{ type, PROTOCOL{ major, minor }, FIRMWARE{ major, minor, build } }
Bottle b_BOARDVERSIONS(config.findGroup("BOARDVERSIONS"));
if(b_BOARDVERSIONS.isNull())
{
yWarning() << "ServiceParser::parseGlobalBoardVersions() cannot find BOARDVERSIONS group. must use locally defined PROTOCOL and FIRMWARE";
boards.resize(0);
return false;
}
// now get type, PROTOCOL.major/minor, FIRMWARE.major/minor/build and see their sizes. the must be all equal.
// for mais and strain and so far for intertials it must be numboards = 1.
Bottle b_BOARDVERSIONS_type = b_BOARDVERSIONS.findGroup("type");
if(b_BOARDVERSIONS_type.isNull())
{
yError() << "ServiceParser::parseGlobalBoardVersions() cannot find BOARDVERSIONS.type";
return false;
}
Bottle b_BOARDVERSIONS_PROTOCOL = Bottle(b_BOARDVERSIONS.findGroup("PROTOCOL"));
if(b_BOARDVERSIONS_PROTOCOL.isNull())
{
yError() << "ServiceParser::parseGlobalBoardVersions() cannot find BOARDVERSIONS.PROTOCOL";
return false;
}
Bottle b_BOARDVERSIONS_PROTOCOL_major = Bottle(b_BOARDVERSIONS_PROTOCOL.findGroup("major"));
if(b_BOARDVERSIONS_PROTOCOL_major.isNull())
{
yError() << "ServiceParser::parseGlobalBoardVersions() cannot find BOARDVERSIONS.PROTOCOL.major";
return false;
}
Bottle b_BOARDVERSIONS_PROTOCOL_minor = Bottle(b_BOARDVERSIONS_PROTOCOL.findGroup("minor"));
if(b_BOARDVERSIONS_PROTOCOL_minor.isNull())
{
yError() << "ServiceParser::parseGlobalBoardVersions() cannot find BOARDVERSIONS.PROTOCOL.minor";
return false;
}
Bottle b_BOARDVERSIONS_FIRMWARE = Bottle(b_BOARDVERSIONS.findGroup("FIRMWARE"));
if(b_BOARDVERSIONS_FIRMWARE.isNull())
{
yError() << "ServiceParser::parseGlobalBoardVersions() cannot find BOARDVERSIONS.FIRMWARE";
return false;
}
Bottle b_BOARDVERSIONS_FIRMWARE_major = Bottle(b_BOARDVERSIONS_FIRMWARE.findGroup("major"));
if(b_BOARDVERSIONS_FIRMWARE_major.isNull())
{
yError() << "ServiceParser::parseGlobalBoardVersions() cannot find BOARDVERSIONS.FIRMWARE.major";
return false;
}
Bottle b_BOARDVERSIONS_FIRMWARE_minor = Bottle(b_BOARDVERSIONS_FIRMWARE.findGroup("minor"));
if(b_BOARDVERSIONS_FIRMWARE_minor.isNull())
{
yError() << "ServiceParser::parseGlobalBoardVersions() cannot find BOARDVERSIONS.FIRMWARE.minor";
return false;
}
Bottle b_BOARDVERSIONS_FIRMWARE_build = Bottle(b_BOARDVERSIONS_FIRMWARE.findGroup("build"));
if(b_BOARDVERSIONS_FIRMWARE_build.isNull())
{
yError() << "ServiceParser::parseGlobalBoardVersions() cannot find BOARDVERSIONS.FIRMWARE.build";
return false;
}
int tmp = b_BOARDVERSIONS_type.size();
int numboards = tmp - 1; // first position of bottle contains the tag "type"
// check if all other fields have the same size.
if( (tmp != b_BOARDVERSIONS_PROTOCOL_major.size()) ||
(tmp != b_BOARDVERSIONS_PROTOCOL_minor.size()) ||
(tmp != b_BOARDVERSIONS_FIRMWARE_major.size()) ||
(tmp != b_BOARDVERSIONS_FIRMWARE_minor.size()) ||
(tmp != b_BOARDVERSIONS_FIRMWARE_build.size())
)
{
yError() << "ServiceParser::parseGlobalBoardVersions() in BOARDVERSIONS some param has inconsistent length";
return false;
}
boards.resize(0);
bool formaterror = false;
for(int i=0; i<numboards; i++)
{
servBoard_t item;
convert(b_BOARDVERSIONS_type.get(i+1).asString(), item.type, formaterror);
convert(b_BOARDVERSIONS_PROTOCOL_major.get(i+1).asInt(), item.protocol.major, formaterror);
convert(b_BOARDVERSIONS_PROTOCOL_minor.get(i+1).asInt(), item.protocol.minor, formaterror);
convert(b_BOARDVERSIONS_FIRMWARE_major.get(i+1).asInt(), item.firmware.major, formaterror);
convert(b_BOARDVERSIONS_FIRMWARE_minor.get(i+1).asInt(), item.firmware.minor, formaterror);
convert(b_BOARDVERSIONS_FIRMWARE_build.get(i+1).asInt(), item.firmware.build, formaterror);
boards.push_back(item);
}
// in here we could decide to return false if any previous conversion function has returned error
// bool fromStringToBoolean(string str, bool &anyerror); // inside: if error then .... be sure to set error = true. dont set it to false.
if(true == formaterror)
{
yError() << "ServiceParser::parseGlobalBoardVersions() has detected an illegal format for some of the params of BOARDVERSIONS some param has inconsistent length";
boards.resize(0);
return false;
}
return true;
}
bool ServiceParser::check_analog(Searchable &config, eOmn_serv_type_t type)
{
bool formaterror = false;
// so far we check for eomn_serv_AS_mais / strain / inertial only
if((eomn_serv_AS_mais != type) && (eomn_serv_AS_strain != type) && (eomn_serv_AS_inertials != type))
{
yError() << "ServiceParser::check() is called with wrong type";
return false;
}
// i parse the global board versions. if section is not found we can safely continue but we have boards.size() equal to zero.
parseGlobalBoardVersions(config);
// format is SERVICE{ type, PROPERTIES{ CANBOARDS, SENSORS }, SETTINGS }
Bottle b_SERVICE(config.findGroup("SERVICE"));
if(b_SERVICE.isNull())
{
yError() << "ServiceParser::check() cannot find SERVICE group";
return false;
}
// check whether we have the proper type
if(false == b_SERVICE.check("type"))
{
yError() << "ServiceParser::check() cannot find SERVICE.type";
return false;
}
else
{
Bottle b_type(b_SERVICE.find("type").asString());
if(false == convert(b_type.toString(), as_service.type, formaterror))
{
yError() << "ServiceParser::check() has found unknown SERVICE.type = " << b_type.toString();
return false;
}
if(type != as_service.type)
{
yError() << "ServiceParser::check() has found wrong SERVICE.type = " << as_service.type << "it must be" << "TODO: tostring() function";
return false;
}
}
// check whether we have the proper groups
Bottle b_PROPERTIES = Bottle(b_SERVICE.findGroup("PROPERTIES"));
if(b_PROPERTIES.isNull())
{
yError() << "ServiceParser::check() cannot find PROPERTIES";
return false;
}
else
{
Bottle b_PROPERTIES_CANBOARDS = Bottle(b_PROPERTIES.findGroup("CANBOARDS"));
if(b_PROPERTIES_CANBOARDS.isNull())
{
yError() << "ServiceParser::check() cannot find PROPERTIES.CANBOARDS";
return false;
}
else
{
// now get type, useGlobalParams, PROTOCOL.major/minor, FIRMWARE.major/minor/build and see their sizes. the must be all equal.
// for mais and strain and so far for intertials it must be numboards = 1.
Bottle b_PROPERTIES_CANBOARDS_type = b_PROPERTIES_CANBOARDS.findGroup("type");
if(b_PROPERTIES_CANBOARDS_type.isNull())
{
yError() << "ServiceParser::check() cannot find PROPERTIES.CANBOARDS.type";
return false;
}
Bottle b_PROPERTIES_CANBOARDS_useGlobalParams = b_PROPERTIES_CANBOARDS.findGroup("useGlobalParams");
if(b_PROPERTIES_CANBOARDS_useGlobalParams.isNull())
{
yError() << "ServiceParser::check() cannot find PROPERTIES.CANBOARDS.useGlobalParams";
return false;
}
Bottle b_PROPERTIES_CANBOARDS_PROTOCOL = Bottle(b_PROPERTIES_CANBOARDS.findGroup("PROTOCOL"));
if(b_PROPERTIES_CANBOARDS_PROTOCOL.isNull())
{
yError() << "ServiceParser::check() cannot find PROPERTIES.CANBOARDS.PROTOCOL";
return false;
}
Bottle b_PROPERTIES_CANBOARDS_PROTOCOL_major = Bottle(b_PROPERTIES_CANBOARDS_PROTOCOL.findGroup("major"));
if(b_PROPERTIES_CANBOARDS_PROTOCOL_major.isNull())
{
yError() << "ServiceParser::check() cannot find PROPERTIES.CANBOARDS.PROTOCOL.major";
return false;
}
Bottle b_PROPERTIES_CANBOARDS_PROTOCOL_minor = Bottle(b_PROPERTIES_CANBOARDS_PROTOCOL.findGroup("minor"));
if(b_PROPERTIES_CANBOARDS_PROTOCOL_minor.isNull())
{
yError() << "ServiceParser::check() cannot find PROPERTIES.CANBOARDS.PROTOCOL.minor";
return false;
}
Bottle b_PROPERTIES_CANBOARDS_FIRMWARE = Bottle(b_PROPERTIES_CANBOARDS.findGroup("FIRMWARE"));
if(b_PROPERTIES_CANBOARDS_FIRMWARE.isNull())
{
yError() << "ServiceParser::check() cannot find PROPERTIES.CANBOARDS.FIRMWARE";
return false;
}
Bottle b_PROPERTIES_CANBOARDS_FIRMWARE_major = Bottle(b_PROPERTIES_CANBOARDS_FIRMWARE.findGroup("major"));
if(b_PROPERTIES_CANBOARDS_FIRMWARE_major.isNull())
{
yError() << "ServiceParser::check() cannot find PROPERTIES.CANBOARDS.FIRMWARE.major";
return false;
}
Bottle b_PROPERTIES_CANBOARDS_FIRMWARE_minor = Bottle(b_PROPERTIES_CANBOARDS_FIRMWARE.findGroup("minor"));
if(b_PROPERTIES_CANBOARDS_FIRMWARE_minor.isNull())
{
yError() << "ServiceParser::check() cannot find PROPERTIES.CANBOARDS.FIRMWARE.minor";
return false;
}
Bottle b_PROPERTIES_CANBOARDS_FIRMWARE_build = Bottle(b_PROPERTIES_CANBOARDS_FIRMWARE.findGroup("build"));
if(b_PROPERTIES_CANBOARDS_FIRMWARE_build.isNull())
{
yError() << "ServiceParser::check() cannot find PROPERTIES.CANBOARDS.FIRMWARE.build";
return false;
}
int tmp = b_PROPERTIES_CANBOARDS_type.size();
int numboards = tmp - 1; // first position of bottle contains the tag "type"
// check if all other fields have the same size.
if( (tmp != b_PROPERTIES_CANBOARDS_useGlobalParams.size()) ||
(tmp != b_PROPERTIES_CANBOARDS_PROTOCOL_major.size()) ||
(tmp != b_PROPERTIES_CANBOARDS_PROTOCOL_minor.size()) ||
(tmp != b_PROPERTIES_CANBOARDS_FIRMWARE_major.size()) ||
(tmp != b_PROPERTIES_CANBOARDS_FIRMWARE_minor.size()) ||
(tmp != b_PROPERTIES_CANBOARDS_FIRMWARE_build.size())
)
{
yError() << "ServiceParser::check() in PROPERTIES.CANBOARDS some param has inconsistent length";
return false;
}
as_service.properties.canboards.resize(0);
formaterror = false;
for(int i=0; i<numboards; i++)
{
servCanBoard_t item;
convert(b_PROPERTIES_CANBOARDS_type.get(i+1).asString(), item.type, formaterror);
convert(b_PROPERTIES_CANBOARDS_useGlobalParams.get(i+1).asString(), item.useglobalparams, formaterror);
if(true == item.useglobalparams)
{
// must search boards an entry which matches eObrd_type_t tt;
eObrd_type_t tt = eobrd_none;
servBoard_t brd;
convert(b_PROPERTIES_CANBOARDS_type.get(i+1).asString(), tt, formaterror);
if(0 == boards.size())
{
yWarning() << "ServiceParser::check() in PROPERTIES.CANBOARDS: useGlobalParams is true for board" << eoboards_type2string(tt) << "but we dont have global params for boards. we shall use prot = (0,0) and firm = (0, 0, 0) ";
}
else
{
for(int i=0; i<boards.size(); i++)
{
servBoard_t cur = boards.at(i);
if(tt == cur.type)
{
brd = cur;
break;
}
}
if(eobrd_none == brd.type)
{
yWarning() << "ServiceParser::check() in PROPERTIES.CANBOARDS: useGlobalParams is true for board" << eoboards_type2string(tt) << "but we cannot find its global params. we shall use prot = (0,0) and firm = (0, 0, 0) ";
}
}
item.protocol.major = brd.protocol.major;
item.protocol.minor = brd.protocol.minor;
item.firmware.major = brd.firmware.major;
item.firmware.minor = brd.firmware.minor;
item.firmware.build = brd.firmware.build;
}
else
{
convert(b_PROPERTIES_CANBOARDS_PROTOCOL_major.get(i+1).asInt(), item.protocol.major, formaterror);
convert(b_PROPERTIES_CANBOARDS_PROTOCOL_minor.get(i+1).asInt(), item.protocol.minor, formaterror);
convert(b_PROPERTIES_CANBOARDS_FIRMWARE_major.get(i+1).asInt(), item.firmware.major, formaterror);
convert(b_PROPERTIES_CANBOARDS_FIRMWARE_minor.get(i+1).asInt(), item.firmware.minor, formaterror);
convert(b_PROPERTIES_CANBOARDS_FIRMWARE_build.get(i+1).asInt(), item.firmware.build, formaterror);
}
as_service.properties.canboards.push_back(item);
}
// in here we could decide to return false if any previous conversion function has returned error
// bool fromStringToBoolean(string str, bool &anyerror); // inside: if error then .... be sure to set error = true. dont set it to false.
if(true == formaterror)
{
yError() << "ServiceParser::check() has detected an illegal format for some of the params of PROPERTIES.CANBOARDS some param has inconsistent length";
return false;
}
}
Bottle b_PROPERTIES_SENSORS = Bottle(b_PROPERTIES.findGroup("SENSORS"));
if(b_PROPERTIES_SENSORS.isNull())
{
yError() << "ServiceParser::check() cannot find PROPERTIES.SENSORS";
return false;
}
else
{
Bottle b_PROPERTIES_SENSORS_id = Bottle(b_PROPERTIES_SENSORS.findGroup("id"));
if(b_PROPERTIES_SENSORS_id.isNull())
{
yError() << "ServiceParser::check() cannot find PROPERTIES.SENSORS.id";
return false;
}
Bottle b_PROPERTIES_SENSORS_type = Bottle(b_PROPERTIES_SENSORS.findGroup("type"));
if(b_PROPERTIES_SENSORS_type.isNull())
{
yError() << "ServiceParser::check() cannot find PROPERTIES.SENSORS.type";
return false;
}
Bottle b_PROPERTIES_SENSORS_location = Bottle(b_PROPERTIES_SENSORS.findGroup("location"));
if(b_PROPERTIES_SENSORS_location.isNull())
{
yError() << "ServiceParser::check() cannot find PROPERTIES.SENSORS.location";
return false;
}
int tmp = b_PROPERTIES_SENSORS_id.size();
int numsensors = tmp - 1; // first position of bottle contains the tag "id"
// check if all other fields have the same size.
if( (tmp != b_PROPERTIES_SENSORS_type.size()) ||
(tmp != b_PROPERTIES_SENSORS_location.size())
)
{
yError() << "ServiceParser::check() in PROPERTIES.SENSORS some param has inconsistent length";
return false;
}
as_service.properties.sensors.resize(0);
formaterror = false;
for(int i=0; i<numsensors; i++)
{
servAnalogSensor_t item;
item.type = eoas_none;
item.location.any.place = eobrd_place_none;
convert(b_PROPERTIES_SENSORS_id.get(i+1).asString(), item.id, formaterror);
convert(b_PROPERTIES_SENSORS_type.get(i+1).asString(), item.type, formaterror);
convert(b_PROPERTIES_SENSORS_location.get(i+1).asString(), item.location, formaterror);
as_service.properties.sensors.push_back(item);
}
// in here we could decide to return false if any previous conversion function has returned error
// bool fromStringToBoolean(string str, bool &anyerror); // inside: if error then .... be sure to set error = true. dont set it to false.
if(true == formaterror)
{
yError() << "ServiceParser::check() has detected an illegal format for some of the params of PROPERTIES.SENSORS some param has inconsistent length";
return false;
}
}
}
Bottle b_SETTINGS = Bottle(b_SERVICE.findGroup("SETTINGS"));
if(b_SETTINGS.isNull())
{
yError() << "ServiceParser::check() cannot find SETTINGS";
return false;
}
else
{
Bottle b_SETTINGS_acquisitionRate = Bottle(b_SETTINGS.findGroup("acquisitionRate"));
if(b_SETTINGS_acquisitionRate.isNull())
{
yError() << "ServiceParser::check() cannot find SETTINGS.acquisitionRate";
return false;
}
Bottle b_SETTINGS_enabledSensors = Bottle(b_SETTINGS.findGroup("enabledSensors"));
if(b_SETTINGS_enabledSensors.isNull())
{
yError() << "ServiceParser::check() cannot find SETTINGS.enabledSensors";
return false;
}
int numenabledsensors = b_SETTINGS_enabledSensors.size() - 1; // first position of bottle contains the tag "enabledSensors"
// the enabled must be <= the sensors.
if( numenabledsensors > as_service.properties.sensors.size() )
{
yError() << "ServiceParser::check() in SETTINGS.enabledSensors there are too many items with respect to supported sensors:" << numenabledsensors << "vs." << as_service.properties.sensors.size();
return false;
}
convert(b_SETTINGS_acquisitionRate.get(1).asInt(), as_service.settings.acquisitionrate, formaterror);
as_service.settings.enabledsensors.resize(0);
for(int i=0; i<numenabledsensors; i++)
{
servAnalogSensor_t founditem;
ConstString s_enabled_id = b_SETTINGS_enabledSensors.get(i+1).asString();
// const char *str = s_enabled_id.c_str();
// std::string cpp_str = str;
// we must now search inside the whole vector<> as_service.properties.sensors if we find an id which matches s_enabled_id ....
// if we dont, ... we issue a warning.
// if we find, ... we do a pushback of it inside
bool found = false;
// i decide to use a brute force search ... for now
for(int n=0; n<as_service.properties.sensors.size(); n++)
{
servAnalogSensor_t item = as_service.properties.sensors.at(n);
//if(item.id == cpp_str)
if(item.id == s_enabled_id)
{
found = true;
founditem = item;
break;
}
}
if(true == found)
{
as_service.settings.enabledsensors.push_back(founditem);
}
}
// in here we issue an error if we dont have at least one enabled sensor
if(0 == as_service.settings.enabledsensors.size())
{
yError() << "ServiceParser::check() could not find any item in SETTINGS.enabledSensors which matches what in PROPERTIES.SENSORS.id";
return false;
}
}
// now we may have one or more sections which are specific of the device ...
// only strain so far.
if(eomn_serv_AS_strain == type)
{
Bottle b_STRAIN_SETTINGS = Bottle(b_SERVICE.findGroup("STRAIN_SETTINGS"));
if(b_STRAIN_SETTINGS.isNull())
{
yError() << "ServiceParser::check() cannot find STRAIN_SETTINGS";
return false;
}
else
{
Bottle b_STRAIN_SETTINGS_useCalibration = Bottle(b_STRAIN_SETTINGS.findGroup("useCalibration"));
if(b_STRAIN_SETTINGS_useCalibration.isNull())
{
yError() << "ServiceParser::check() cannot find STRAIN_SETTINGS.useCalibration";
return false;
}
formaterror = false;
convert(b_STRAIN_SETTINGS_useCalibration.get(1).asString(), as_strain_settings.useCalibration, formaterror);
if(true == formaterror)
{
yError() << "ServiceParser::check() has detected an illegal format for paramf STRAIN_SETTINGS.useCalibration";
return false;
}
}
}
// we we are in here we have the struct filled with all variables ... some validations are still due to the calling device.
// for instance, if embObjMais
return true;
}
bool VirtualAnalogWrapper::open(Searchable& config)
{
cout << config.toString().c_str() << endl << endl;
mIsVerbose = (config.check("verbose","if present, give detailed output"));
if (mIsVerbose) cout << "running with verbose output\n";
//thus thread period is useful for output port... this input port has callback so maybe can skip it (?)
//thread_period = prop.check("threadrate", 20, "thread rate in ms. for streaming encoder data").asInt();
std::cout << "Using VirtualAnalogServer\n";
if (!config.check("networks", "list of networks merged by this wrapper"))
{
cerr << "Error: missing networks parameters" << endl;
return false;
}
Bottle *networks=config.find("networks").asList();
mNSubdevs=networks->size();
mSubdevices.resize(mNSubdevs);
mChan2Board.resize(MAX_ENTRIES);
mChan2BAddr.resize(MAX_ENTRIES);
for (int i=0; i< MAX_ENTRIES; i++)
{
mChan2Board[i]=-1;
mChan2BAddr[i]=-1;
}
int totalJ=0;
for (int k=0; k<networks->size(); ++k)
{
Bottle parameters=config.findGroup(networks->get(k).asString().c_str());
if (parameters.size()!=5) // mapping joints using the paradigm: part from - to / network from - to
{
cerr << "Error: check network parameters in part description" << endl;
cerr << "--> I was expecting " << networks->get(k).asString().c_str() << " followed by four integers" << endl;
return false;
}
int map0=parameters.get(1).asInt();
int map1=parameters.get(2).asInt();
int map2=parameters.get(3).asInt();
int map3=parameters.get(4).asInt();
if (map0 >= MAX_ENTRIES || map1 >= MAX_ENTRIES || map2>= MAX_ENTRIES || map3>= MAX_ENTRIES ||
map0 <0 || map1 <0 || map2<0 || map3<0)
{
cerr << "Error: invalid map entries in networks section, failed initial check" << endl;
return false;
}
for (int j=map0; j<=map1; ++j)
{
mChan2Board[j]=k;
mChan2BAddr[j]=j-map0+map2;
}
if (!mSubdevices[k].configure(map2,map3,networks->get(k).asString().c_str()))
{
cerr << "configure of subdevice ret false" << endl;
return false;
}
totalJ+=map1-map0+1;
}
// Verify minimum set of parameters required
if(!config.check("robotName") ) // ?? qui dentro, da dove lo pesco ??
{
cout << "VirtualAnalogServer missing robot Name, check your configuration file!! Quitting\n";
return false;
}
std::string root_name;
std::string port_name = config.check("name",Value("controlboard"),"prefix for port names").asString().c_str();
std::string robot_name = config.find("robotName").asString().c_str();
root_name+="/";
root_name+=robot_name;
root_name+= "/joint_vsens" + port_name + ":i";
if (!mPortInputTorques.open(root_name.c_str()))
{
cerr << "can't open port " << root_name.c_str() << endl;
return false;
}
return true;
}
bool VirtualAnalogWrapper::open(Searchable& config)
{
yDebug() << config.toString().c_str();
mIsVerbose = (config.check("verbose","if present, give detailed output"));
if (mIsVerbose) yDebug() << "running with verbose output\n";
//thus thread period is useful for output port... this input port has callback so maybe can skip it (?)
//thread_period = prop.check("threadrate", 20, "thread rate in ms. for streaming encoder data").asInt();
yDebug() << "Using VirtualAnalogServer\n";
if (!config.check("networks", "list of networks merged by this wrapper"))
{
yError() << "VirtualAnalogWrapper: missing networks parameters";
return false;
}
Bottle *networks=config.find("networks").asList();
mNSubdevs=networks->size();
mSubdevices.resize(mNSubdevs);
mChan2Board.resize(MAX_ENTRIES);
mChan2BAddr.resize(MAX_ENTRIES);
for (int i=0; i< MAX_ENTRIES; i++)
{
mChan2Board[i]=-1;
mChan2BAddr[i]=-1;
}
int totalJ=0;
for (int k=0; k<networks->size(); ++k)
{
Bottle parameters=config.findGroup(networks->get(k).asString().c_str());
if (parameters.size()!=5) // mapping joints using the paradigm: part from - to / network from - to
{
yError() << "VirtualAnalogWrapper: check network parameters in part description"
<< " I was expecting " << networks->get(k).asString().c_str() << " followed by four integers";
return false;
}
int map0=parameters.get(1).asInt();
int map1=parameters.get(2).asInt();
int map2=parameters.get(3).asInt();
int map3=parameters.get(4).asInt();
if (map0 >= MAX_ENTRIES || map1 >= MAX_ENTRIES || map2>= MAX_ENTRIES || map3>= MAX_ENTRIES ||
map0 <0 || map1 <0 || map2<0 || map3<0)
{
yError() << "VirtualAnalogWrapper: invalid map entries in networks section, failed initial check";
return false;
}
for (int j=map0; j<=map1; ++j)
{
mChan2Board[j]=k;
mChan2BAddr[j]=j-map0+map2;
}
if (!mSubdevices[k].configure(map2,map3,networks->get(k).asString().c_str()))
{
yError() << "VirtualAnalogWrapper: configure of subdevice ret false";
return false;
}
totalJ+=map1-map0+1;
}
// Verify minimum set of parameters required
if(!config.check("robotName") ) // ?? qui dentro, da dove lo pesco ??
{
yError() << "VirtualAnalogWrapper: missing robotName, check your configuration file!";
return false;
}
if (config.check("deviceId"))
{
yError() << "VirtualAnalogWrapper: the parameter 'deviceId' has been deprecated, please use parameter 'name' instead. \n"
<< "e.g. In the VFT wrapper configuration files of your robot, replace '<param name=""deviceId""> left_arm </param>' \n"
<< "with '/icub/joint_vsens/left_arm:i' ";
return false;
}
std::string port_name = config.check("name",Value("controlboard"),"Virtual analog wrapper port name, e.g. /icub/joint_vsens/left_arm:i").asString().c_str();
std::string robot_name = config.find("robotName").asString().c_str();
if (!mPortInputTorques.open(port_name.c_str()))
{
yError() << "VirtualAnalogWrapper: can't open port " << port_name.c_str();
return false;
}
return true;
}
GuiSalience::GuiSalience(Searchable& config, QWidget* parent, const char* name, bool modal, WFlags fl)
: GuiSalienceBase( parent, name, fl ),
frmMainLayout(NULL),
_scrlView(NULL),
_scrlFrame(NULL),
_scrlFrameLayout(NULL),
_spacer(NULL),
_qwConfConnection(NULL)
{
Property prop;
if(!config.check("name")){
prop.put("name", "/salienceGui");
}
else{
prop.put("name", config.find("name"));
}
if(config.check("REMOTE_SALIENCE")){
prop.put("remote", config.findGroup("REMOTE_SALIENCE").find("remote").asString().c_str());
}
else{
if(config.check("remote")){
prop.put("remote", config.find("remote").asString().c_str());
}
else{
prop.put("remote", "/salience/conf");
}
}
// salience controls
// *****************
frmMainLayout = new QVBoxLayout( frmMain, 3, 3, "frmMainLayout");
// scroll view
_scrlView = new QScrollView(frmMain);
frmMainLayout->addWidget(_scrlView);
_scrlView->setResizePolicy(QScrollView::AutoOneFit);
// frame for filter widgets
_scrlFrame = new QFrame(_scrlView->viewport());
_scrlView->addChild(_scrlFrame);
_scrlFrameLayout = new QVBoxLayout(_scrlFrame, 3, 3, "_scrlFrameLayout");
// setup connection widget
grbConfigurationConnection->setColumnLayout(0, Qt::Vertical );
QVBoxLayout *grbConnectionLayout = new QVBoxLayout(grbConfigurationConnection->layout());
grbConfigurationConnection->layout()->setSpacing( 3 );
grbConfigurationConnection->layout()->setMargin( 3 );
grbConnectionLayout->setAlignment(Qt::AlignTop);
_qwConfConnection = new QWidgetConnection(grbConfigurationConnection);
grbConnectionLayout->addWidget(_qwConfConnection);
yarp::os::ConstString strLocalPort = std::string(std::string(prop.find("name").asString().c_str())+ std::string("/conf")).c_str();
yarp::os::ConstString strRemotePort = prop.find("remote").asString();
_remoteSalience.open(strLocalPort);
_qwConfConnection->setTargetPortName(strRemotePort.c_str());
_qwConfConnection->setSourcePortName(strLocalPort.c_str());
this->initializeUI();
// line edit validators
lneThreshold->setValidator(new QDoubleValidator(this));
lneBlur->setValidator(new QIntValidator(this));
// set gui values to current values
QVariant thr(_remoteSalience.getSalienceThreshold());
lneThreshold->setText(thr.asString());
QVariant nb(_remoteSalience.getNumBlurPasses());
lneBlur->setText(nb.asString());
}
bool AnalogWrapper::checkROSParams(Searchable &config)
{
// check for ROS parameter group
if(!config.check("ROS") )
{
useROS = ROS_disabled;
yInfo() << "No ROS group found in config file ... skipping ROS initialization.";
return true;
}
yInfo() << "ROS group was FOUND in config file.";
Bottle &rosGroup = config.findGroup("ROS");
if(rosGroup.isNull())
{
yError() << sensorId << "ROS group params is not a valid group or empty";
useROS = ROS_config_error;
return false;
}
// check for useROS parameter
if(!rosGroup.check("useROS"))
{
yError() << sensorId << " cannot find useROS parameter, mandatory when using ROS message. \n \
Allowed values are true, false, ROS_only";
useROS = ROS_config_error;
return false;
}
yarp::os::ConstString ros_use_type = rosGroup.find("useROS").asString();
if(ros_use_type == "false")
{
yInfo() << sensorId << "useROS topic if set to 'false'";
useROS = ROS_disabled;
return true;
}
else if(ros_use_type == "true")
{
yInfo() << sensorId << "useROS topic if set to 'true'";
useROS = ROS_enabled;
}
else if(ros_use_type == "only")
{
yInfo() << sensorId << "useROS topic if set to 'only";
useROS = ROS_only;
}
else
{
yInfo() << sensorId << "useROS parameter is set to unvalid value ('" << ros_use_type << "'), supported values are 'true', 'false', 'only'";
useROS = ROS_config_error;
return false;
}
// check for ROS_nodeName parameter
if(!rosGroup.check("ROS_nodeName"))
{
yError() << sensorId << " cannot find ROS_nodeName parameter, mandatory when using ROS message";
useROS = ROS_config_error;
return false;
}
rosNodeName = rosGroup.find("ROS_nodeName").asString(); // TODO: check name is correct
yInfo() << sensorId << "rosNodeName is " << rosNodeName;
// check for ROS_topicName parameter
if(!rosGroup.check("ROS_topicName"))
{
yError() << sensorId << " cannot find ROS_topicName parameter, mandatory when using ROS message";
useROS = ROS_config_error;
return false;
}
rosTopicName = rosGroup.find("ROS_topicName").asString();
yInfo() << sensorId << "ROS_topicName is " << rosTopicName;
// check for ROS_msgType parameter
if (!rosGroup.check("ROS_msgType"))
{
yError() << sensorId << " cannot find ROS_msgType parameter, mandatory when using ROS message";
useROS = ROS_config_error;
return false;
}
rosMsgType = rosGroup.find("ROS_msgType").asString();
// check for frame_id parameter
if (rosMsgType == "geometry_msgs/WrenchedStamped")
{
yInfo() << sensorId << "ROS_msgType is " << rosMsgType;
if (!rosGroup.check("frame_id"))
{
yError() << sensorId << " cannot find frame_id parameter, mandatory when using ROS message";
useROS = ROS_config_error;
return false;
}
frame_id = rosGroup.find("frame_id").asString();
yInfo() << sensorId << "frame_id is " << frame_id;
}
else if (rosMsgType == "sensor_msgs/JointState")
{
yInfo() << sensorId << "ROS_msgType is " << rosMsgType;
if (!rosGroup.check("joint_names"))
{
yError() << sensorId << " cannot find some ros parameters";
useROS = ROS_config_error;
return false;
}
yarp::os::Bottle& jnam =rosGroup.findGroup("joint_names");
int joint_names_size = jnam.size()-1;
for (int i = 0; i < joint_names_size; i++)
{
ros_joint_names.push_back(jnam.get(i+1).asString());
}
}
else
{
yError() << sensorId << "ROS_msgType '" << rosMsgType << "' not supported ";
return false;
}
return true;
}
bool ARMarkerDetectorModule::open(Searchable& config)
{
if (config.check("help","if present, display usage message")) {
printf("Call with --file configFile.ini\n");
return false;
}
ConstString input_port = config.check("inputPort", Value("img:i"), "Camera intrinsics definition file.").asString().c_str();
ConstString output_port = config.check("outputPort", Value("data:o"), "Camera intrinsics definition file.").asString().c_str();
ConstString conf_port = config.check("confPort", Value("conf"), "Camera intrinsics definition file.").asString().c_str();
ConstString display_name = config.check("displayName", Value("ARTrackerUH"), "Camera intrinsics definition file.").asString().c_str();
ConstString camera_file = config.check("cameraparameters", Value("camera_para.dat"), "Camera intrinsics definition file.").asString().c_str();
ConstString objects_file = config.check("objects", Value("object_data.txt"), "Markers definition file.").asString().c_str();
tracker.loadCameraParameters( camera_file.c_str(), 640, 480);
//tracker.loadObjectData( objects_file.c_str() );
int numberofobjects = config.check("numberofpatterns", 0, "number of patterns").asInt();
tracker.initobjectdata( numberofobjects );
ConstString directory = config.check("directory", "/", "directory where patterns are located").asString();
for(int cnt=0;cnt<numberofobjects;cnt++)
{
char name[256];
char filename[256];
char pattern[16];
sprintf( pattern,"pattern%d",cnt+1);
Bottle& K = config.findGroup( pattern, "object");
cout << K.toString() << endl;
strcpy( name, (const char*)K.get(1).asString());
strcpy( filename, (const char*)directory);
strcat( filename, (const char*)K.get(2).asString() );
double size = K.get(3).asDouble();
printf("reading conf bootle s0%s s1%s s2%s s3%g\n",
(const char*)K.get(0).asString(),
(const char*)K.get(1).asString(),
(const char*)K.get(2).asString(),
size);
tracker.addobjectdata( cnt, name, filename, size );
}
//tracker.loadObjectData( config );
//check visualization
int visualize = config.check("Visualization", 0, "Show the tracker image").asInt();
if(visualize)
tracker.openVisualization(display_name);
//load in the object data - trained markers and associated bitmap files
_imgPort.open(getName(input_port));
_configPort.open(getName(conf_port));
_outPort.open(getName(output_port));
attach(_configPort, true);
// imageOut.open("/testOut");
return true;
}
SkinMeshThreadPort::SkinMeshThreadPort(Searchable& config,int period) : RateThread(period),mutex(1)
{
yDebug("SkinMeshThreadPort running at %g ms.",getRate());
mbSimpleDraw=config.check("light");
sensorsNum=0;
for (int t=0; t<MAX_SENSOR_NUM; ++t)
{
sensor[t]=NULL;
}
std::string part="/skinGui/";
std::string part_virtual="";
if (config.check("name"))
{
part ="/";
part+=config.find("name").asString().c_str();
part+="/";
}
part.append(config.find("robotPart").asString());
part_virtual = part;
part_virtual.append("_virtual");
part.append(":i");
part_virtual.append(":i");
skin_port.open(part.c_str());
// Ideally, we would use a --virtual flag. since this would make the skinmanager xml file unflexible,
// let's keep the code structure without incurring in any problem whatsoever
// if (config.check("virtual"))
if (true)
{
skin_port_virtual.open(part_virtual.c_str());
}
int width =config.find("width" ).asInt();
int height=config.find("height").asInt();
bool useCalibration = config.check("useCalibration");
if (useCalibration==true) yInfo("Using calibrated skin values (0-255)");
else yDebug("Using raw skin values (255-0)");
Bottle *color = config.find("color").asList();
unsigned char r=255, g=0, b=0;
if(color)
{
if(color->size()<3 || !color->get(0).isInt() || !color->get(1).isInt() || !color->get(2).isInt())
{
yError("Error while reading the parameter color: three integer values should be specified (%s).", color->toString().c_str());
}
else
{
r = color->get(0).asInt();
g = color->get(1).asInt();
b = color->get(2).asInt();
yInfo("Using specified color: %d %d %d", r, g, b);
}
}
else
{
yDebug("Using red as default color.");
}
defaultColor.push_back(r);
defaultColor.push_back(g);
defaultColor.push_back(b);
skinThreshold = config.check("skinThreshold")?config.find("skinThreshold").asDouble():SKIN_THRESHOLD;
yDebug("Skin Threshold set to %g", skinThreshold);
yarp::os::Bottle sensorSetConfig=config.findGroup("SENSORS").tail();
for (int t=0; t<sensorSetConfig.size(); ++t)
{
yarp::os::Bottle sensorConfig(sensorSetConfig.get(t).toString());
std::string type(sensorConfig.get(0).asString());
if (type=="triangle" || type=="fingertip" || type=="fingertip2L" || type=="fingertip2R" ||
type=="triangle_10pad" || type=="quad16" || type=="palmR" || type=="palmL")
{
int id=sensorConfig.get(1).asInt();
double xc=sensorConfig.get(2).asDouble();
double yc=sensorConfig.get(3).asDouble();
double th=sensorConfig.get(4).asDouble();
double gain=sensorConfig.get(5).asDouble();
int lrMirror=sensorConfig.get(6).asInt();
int layoutNum=sensorConfig.get(7).asInt();
yDebug("%s %d %f",type.c_str(),id,gain);
if (id>=0 && id<MAX_SENSOR_NUM)
{
if (sensor[id])
{
yError("Triangle %d already exists.",id);
}
else
{
if (type=="triangle")
{
sensor[id]=new Triangle(xc,yc,th,gain,layoutNum,lrMirror);
}
else if (type=="triangle_10pad")
{
sensor[id]=new Triangle_10pad(xc,yc,th,gain,layoutNum,lrMirror);
}
else if (type=="fingertip")
{
sensor[id]=new Fingertip(xc,yc,th,gain,layoutNum,lrMirror);
}
else if (type=="fingertip2L")
{
sensor[id]=new Fingertip2L(xc,yc,th,gain,layoutNum,lrMirror);
}
else if (type=="fingertip2R")
{
sensor[id]=new Fingertip2R(xc,yc,th,gain,layoutNum,lrMirror);
}
else if (type=="quad16")
{
sensor[id]=new Quad16(xc,yc,th,gain,layoutNum,lrMirror);
}
else if (type=="palmR")
{
sensor[id]=new PalmR(xc,yc,th,gain,layoutNum,lrMirror);
}
else if (type=="palmL")
{
sensor[id]=new PalmL(xc,yc,th,gain,layoutNum,lrMirror);
}
sensor[id]->setCalibrationFlag(useCalibration);
++sensorsNum;
}
}
else
{
yWarning(" %d is invalid triangle Id [0:%d].",id, MAX_SENSOR_NUM-1);
}
}
else
{
yWarning(" sensor type %s unknown, discarded.",type.c_str());
}
}
int max_tax=0;
for (int t=0; t<MAX_SENSOR_NUM; ++t)
{
if (sensor[t])
{
sensor[t]->min_tax=max_tax;
max_tax = sensor[t]->min_tax+sensor[t]->get_nTaxels();
sensor[t]->max_tax=max_tax-1;
sensor[t]->setColor(r, g, b);
}
else
{
//this deals with the fact that some traingles can be not present,
//but they anyway broadcast an array of zeros...
max_tax += 12;
}
}
resize(width,height);
}
bool teo::CanBusControlboard::open(Searchable& config) {
std::string mode = config.check("mode",Value(DEFAULT_MODE),"position/velocity mode").asString();
int16_t ptModeMs = config.check("ptModeMs",Value(DEFAULT_PT_MODE_MS),"PT mode miliseconds").asInt();
Bottle ids = config.findGroup("ids").tail(); //-- e.g. 15
Bottle trs = config.findGroup("trs").tail(); //-- e.g. 160
Bottle ks = config.findGroup("ks").tail(); //-- e.g. 0.0706
Bottle maxs = config.findGroup("maxs").tail(); //-- e.g. 360
Bottle mins = config.findGroup("mins").tail(); //-- e.g. -360
Bottle refAccelerations = config.findGroup("refAccelerations").tail(); //-- e.g. 0.575437
Bottle refSpeeds = config.findGroup("refSpeeds").tail(); //-- e.g. 737.2798
Bottle types = config.findGroup("types").tail(); //-- e.g. 15
//-- Initialize the CAN device.
Property canBusOptions;
canBusOptions.fromString(config.toString()); // canDevice, canBitrate
canBusOptions.put("device","CanBusHico");
canBusDevice.open(canBusOptions);
if( ! canBusDevice.isValid() ){
CD_ERROR("canBusDevice instantiation not worked.\n");
return false;
}
canBusDevice.view(iCanBus);
//-- Start the reading thread (required for checkMotionDoneRaw).
this->Thread::start();
//-- Populate the CAN nodes vector.
nodes.resize( ids.size() );
iControlLimitsRaw.resize( nodes.size() );
iControlModeRaw.resize( nodes.size() );
iEncodersTimedRaw.resize( nodes.size() );
iPositionControlRaw.resize( nodes.size() );
iPositionDirectRaw.resize( nodes.size() );
iTorqueControlRaw.resize( nodes.size() );
iVelocityControlRaw.resize( nodes.size() );
iCanBusSharer.resize( nodes.size() );
for(int i=0; i<nodes.size(); i++)
{
if(types.get(i).asString() == "")
CD_WARNING("Argument \"types\" empty at %d.\n",i);
//-- Create CAN node objects with a pointer to the CAN device, its id and tr (these are locally stored parameters).
Property options;
options.put("device",types.get(i).asString()); //-- "TechnosoftIpos", "LacqueyFetch"
options.put("canId",ids.get(i).asInt());
options.put("tr",trs.get(i).asDouble());
options.put("min",mins.get(i).asDouble());
options.put("max",maxs.get(i).asDouble());
options.put("k",ks.get(i).asDouble());
options.put("refAcceleration",refAccelerations.get(i).asDouble());
options.put("refSpeed",refSpeeds.get(i).asDouble());
options.put("ptModeMs",ptModeMs);
PolyDriver* driver = new PolyDriver(options);
//-- Fill a map entry ( drivers.size() if before push_back, otherwise do drivers.size()-1).
//-- Just "i" if resize already performed.
idxFromCanId[ ids.get(i).asInt() ] = i;
//-- Push the motor driver on to the vectors.
nodes[i] = driver;
driver->view( iControlLimitsRaw[i] );
driver->view( iControlModeRaw[i] );
driver->view( iEncodersTimedRaw[i] );
driver->view( iPositionControlRaw[i] );
driver->view( iPositionDirectRaw[i] );
driver->view( iTorqueControlRaw[i] );
driver->view( iVelocityControlRaw[i] );
driver->view( iCanBusSharer[i] );
//-- Associate absolute encoders to motor drivers
if( types.get(i).asString() == "CuiAbsolute" ) {
int driverCanId = ids.get(i).asInt() - 100;
iCanBusSharer[ idxFromCanId[driverCanId] ]->setIEncodersTimedRawExternal( iEncodersTimedRaw[i] );
}
//-- Aditionally sets initial parameters on physical motor drivers.
iCanBusSharer[i]->setCanBusPtr( iCanBus );
}
//-- Set all motor drivers to mode.
if( mode=="position") {
if( ! this->setPositionMode() )
return false;
} else if( mode=="velocity") {
if( ! this->setVelocityMode() )
return false;
} else if( mode=="torque") {
if( ! this->setTorqueMode() )
return false;
} else {
CD_ERROR("Not prepared for initializing in mode %s.\n",mode.c_str());
return false;
}
//-- Check the status of each driver.
std::vector<int> tmp( nodes.size() );
this->getControlModes( tmp.data() );
//-- Initialize the drivers: start (0.1) ready (0.1) on (2) enable. Wait between each step.
for(int i=0; i<nodes.size(); i++)
{
if( ! iCanBusSharer[i]->start() )
return false;
}
yarp::os::Time::delay(0.1);
for(int i=0; i<nodes.size(); i++)
{
if( ! iCanBusSharer[i]->readyToSwitchOn() )
return false;
}
yarp::os::Time::delay(0.1);
for(int i=0; i<nodes.size(); i++)
{
if( ! iCanBusSharer[i]->switchOn() )
return false;
}
yarp::os::Time::delay(2);
for(int i=0; i<nodes.size(); i++)
{
if( ! iCanBusSharer[i]->enable() )
return false;
}
if( config.check("home") ) {
CD_DEBUG("Moving motors to zero.\n");
for(int i=0; i<nodes.size(); i++)
{
if ( ! iPositionControlRaw[i]->positionMoveRaw(0,0) )
return false;
}
for(int i=0; i<nodes.size(); i++)
{
bool motionDone = false;
while( ! motionDone ) {
yarp::os::Time::delay(0.5); //-- [s]
CD_DEBUG("Moving %d to zero...\n",i);
if( ! iPositionControlRaw[i]->checkMotionDoneRaw(0,&motionDone) )
return false;
}
}
CD_DEBUG("Moved motors to zero.\n");
}
if( config.check("reset") ) {
CD_DEBUG("Forcing encoders to zero.\n");
if ( ! this->resetEncoders() )
return false;
}
return true;
}
