bool BmsBattery::open(yarp::os::Searchable& config)
{
bool correct=true;
//debug
yDebug("%s\n", config.toString().c_str());
Bottle& group_general = config.findGroup("GENERAL");
Bottle& group_serial = config.findGroup("SERIAL_PORT");
if (group_general.isNull())
{
yError() << "Insufficient parameters to BmsBattery, section GENERAL missing";
return false;
}
if (group_serial.isNull())
{
yError() << "Insufficient parameters to BmsBattery, section SERIAL_PORT missing";
return false;
}
int period=config.find("thread_period").asInt();
setRate(period);
Property prop;
std::string ps = group_serial.toString();
prop.fromString(ps);
prop.put("device", "serialport");
//open the driver
driver.open(prop);
if (!driver.isValid())
{
yError() << "Error opening PolyDriver check parameters";
#ifndef DEBUG_TEST
return false;
#endif
}
//open the serial interface
driver.view(pSerial);
if (!pSerial)
{
yError("Error opening serial driver. Device not available");
#ifndef DEBUG_TEST
return false;
#endif
}
// Other options
this->logEnable = group_general.check("logToFile", Value(0), "enable / disable the log to file").asBool();
this->verboseEnable = group_general.check("verbose", Value(0), "enable/disable the verbose mode").asBool();
this->screenEnable = group_general.check("screen", Value(0), "enable/disable the screen output").asBool();
this->debugEnable = group_general.check("debug", Value(0), "enable/disable the debug mode").asBool();
this->shutdownEnable = group_general.check("shutdown", Value(0), "enable/disable the automatic shutdown").asBool();
RateThread::start();
return true;
}
bool DeviceGroup::open(yarp::os::Searchable& config) {
if (implementation==NULL) {
implementation = new DeviceGroupHelper;
}
if (implementation==NULL) {
printf("Out of memory\n");
return false;
}
if (config.check("part","a list of section names, with each section containing a device")) {
Bottle bot = config.findGroup("part").tail();
printf("Assembly of: %s\n", bot.toString().c_str());
for (int i=0; i<bot.size(); i++) {
ConstString name = bot.get(i).asString();
printf(" %s -> %s\n", name.c_str(),
config.findGroup(name).toString().c_str());
bool result = HELPER(implementation).add(name,
config.findGroup(name));
if (!result) {
HELPER(implementation).close();
return false;
}
}
return true;
}
return false;
}
/*Device Driver*/
bool FakeCan::open(yarp::os::Searchable &par)
{
cerr<<"Opening FakeCan network" << endl;
//fprintf(stderr, "%s", par.toString().c_str());
int njoints=par.findGroup("GENERAL").find("Joints").asInt();
Bottle &can = par.findGroup("CAN");
Bottle ids=can.findGroup("CanAddresses");
if (ids.size()<njoints/2)
{
fprintf(stderr, "Check ini file, wrong number of board ids or joints\n");
return false;
}
for(int i=1;i<=njoints/2;i++)
{
FakeBoard *tmp=new FakeBoard;
int id=ids.get(i).asInt();
tmp->setId(id); //just as a test
tmp->setReplyFifo(&replies);
tmp->start();
boardList.push_back(tmp);
}
return true;
}
// \todo TODO bug ?
bool checkRequiredParamIsVectorOfString(yarp::os::Searchable& config,
const std::string& paramName,
std::vector<std::string> & output_vector)
{
bool correct = !(config.findGroup(paramName).isNull());
if( correct )
correct = true;
{
Bottle ids = config.findGroup(paramName).tail();
std::cout << "ids : " << ids.toString() << std::endl;
std::cout << "ids : " << config.find(paramName).toString() << std::endl;
output_vector.resize(ids.size());
for(int i = 0; i < ids.size(); i++ )
{
output_vector[i] = ids.get(i).asString().c_str();
}
}
if( !correct )
{
yError("CanBusInertialMTB: problem loading parameter %s as vector of string",paramName.c_str());
}
return correct;
}
bool ethResources::open(yarp::os::Searchable &config, FEAT_ID request)
{
// ACE_TCHAR remoteIp_string[64], localIp_string[64];
//uint16_t loc_port, rem_port;
// uint8_t loc_ip1,loc_ip2,loc_ip3,loc_ip4;
// uint8_t rem_ip1,rem_ip2,rem_ip3,rem_ip4;
// Get the pointer to the actual Singleton ethManager
ethManager = TheEthManager::instance();
transMutex.wait();
// Fill 'info' field with human friendly string
sprintf(info, "ethResources - referred to EMS: %s:%d",request.EMSipAddr.string, request.EMSipAddr.port);
yTrace() << "Ems ip address " << info;
if(config.findGroup("GENERAL").find("verbose").asBool())
{
infoPkts->_verbose = true;
}
else
{
infoPkts->_verbose = false;
}
//
// EMBOBJ INIT
//
bool ret;
eOipv4addr_t eo_locIp = eo_common_ipv4addr(request.PC104ipAddr.ip1, request.PC104ipAddr.ip2, request.PC104ipAddr.ip3, request.PC104ipAddr.ip4);
eOipv4addr_t eo_remIp = eo_common_ipv4addr(request.EMSipAddr.ip1, request.EMSipAddr.ip2, request.EMSipAddr.ip3, request.EMSipAddr.ip4);
Bottle groupProtocol = Bottle(config.findGroup("PROTOCOL"));
if(!init(groupProtocol, eo_locIp, eo_remIp, request.EMSipAddr.port, rxBUFFERsize, request.boardNum))
{
ret = false;
yError() << "cannot init transceiver... maybe wrong board number... check log and config file.";
}
else
{
ret = true;
// yDebug() << "Transceiver succesfully initted.";
}
boardNum = request.boardNum;
ACE_UINT32 hostip = (request.EMSipAddr.ip1 << 24) | (request.EMSipAddr.ip2 << 16) | (request.EMSipAddr.ip3 << 8) | (request.EMSipAddr.ip4);
ACE_INET_Addr myIP((u_short)request.EMSipAddr.port, hostip);
remote_dev = myIP;
transMutex.post();
infoPkts->setBoardNum(boardNum);
return ret;
}
bool FakeLaser::open(yarp::os::Searchable& config)
{
bool correct=true;
info = "Fake Laser device for test/debugging";
device_status = DEVICE_OK_STANBY;
#if LASER_DEBUG
yDebug("%s\n", config.toString().c_str());
#endif
bool br = config.check("GENERAL");
if (br != false)
{
yarp::os::Searchable& general_config = config.findGroup("GENERAL");
period = general_config.check("Period", Value(50), "Period of the sampling thread").asInt();
}
min_distance = 0.1; //m
max_distance = 2.5; //m
min_angle = 0; //degrees
max_angle = 359; //degrees
resolution = 1.0; //degrees
sensorsNum = (int)((max_angle-min_angle)/resolution);
laser_data.resize(sensorsNum);
yInfo("Starting debug mode");
yInfo("max_dist %f, min_dist %f", max_distance, min_distance);
yInfo("max_angle %f, min_angle %f", max_angle, min_angle);
yInfo("resolution %f", resolution);
yInfo("sensors %d", sensorsNum);
Time::turboBoost();
RateThread::start();
return true;
}
bool DeviceGroup::open(const char *key, PolyDriver& poly,
yarp::os::Searchable& config, const char *comment) {
Value *name;
if (config.check(key,name,comment)) {
if (name->isString()) {
// maybe user isn't doing nested configuration
yarp::os::Property p;
p.setMonitor(config.getMonitor(),
name->toString().c_str()); // pass on any monitoring
p.fromString(config.toString());
p.put("device",name->toString());
p.unput("subdevice");
p.unput("wrapped");
poly.open(p);
} else {
Bottle subdevice = config.findGroup(key).tail();
poly.open(subdevice);
}
if (!poly.isValid()) {
printf("cannot make <%s>\n", name->toString().c_str());
return false;
}
} else {
printf("\"--%s <name>\" not set\n", key);
return false;
}
return true;
}
bool JointTorqueControl::loadGains(yarp::os::Searchable& config)
{
if( !config.check("TRQ_PIDS") )
{
yError("No TRQ_PIDS group find, initialization failed");
return false;
}
yarp::os::Bottle & bot = config.findGroup("TRQ_PIDS");
bool gains_ok = true;
gains_ok = gains_ok && checkVectorExistInConfiguration(bot,"kff",this->axes);
gains_ok = gains_ok && checkVectorExistInConfiguration(bot,"kp",this->axes);
gains_ok = gains_ok && checkVectorExistInConfiguration(bot,"ki",this->axes);
gains_ok = gains_ok && checkVectorExistInConfiguration(bot,"maxPwm",this->axes);
gains_ok = gains_ok && checkVectorExistInConfiguration(bot,"maxInt",this->axes);
gains_ok = gains_ok && checkVectorExistInConfiguration(bot,"stictionUp",this->axes);
gains_ok = gains_ok && checkVectorExistInConfiguration(bot,"stictionDown",this->axes);
gains_ok = gains_ok && checkVectorExistInConfiguration(bot,"bemf",this->axes);
gains_ok = gains_ok && checkVectorExistInConfiguration(bot,"coulombVelThr",this->axes);
gains_ok = gains_ok && checkVectorExistInConfiguration(bot,"frictionCompensation",this->axes);
if( !gains_ok )
{
yError("TRQ_PIDS group is missing some information, initialization failed");
return false;
}
for(int j=0; j < this->axes; j++)
{
jointTorqueLoopGains[j].reset();
motorParameters[j].reset();
jointTorqueLoopGains[j].kp = bot.find("kp").asList()->get(j).asDouble();
jointTorqueLoopGains[j].ki = bot.find("ki").asList()->get(j).asDouble();
jointTorqueLoopGains[j].max_pwm = bot.find("maxPwm").asList()->get(j).asDouble();
jointTorqueLoopGains[j].max_int = bot.find("maxInt").asList()->get(j).asDouble();
motorParameters[j].kff = bot.find("kff").asList()->get(j).asDouble();
motorParameters[j].kcp = bot.find("stictionUp").asList()->get(j).asDouble();
motorParameters[j].kcn = bot.find("stictionDown").asList()->get(j).asDouble();
motorParameters[j].kv = bot.find("bemf").asList()->get(j).asDouble();
motorParameters[j].coulombVelThr = bot.find("coulombVelThr").asList()->get(j).asDouble();
motorParameters[j].frictionCompensation = bot.find("frictionCompensation").asList()->get(j).asDouble();
if (motorParameters[j].frictionCompensation > 1 || motorParameters[j].frictionCompensation < 0) {
motorParameters[j].frictionCompensation = 0;
yWarning("[TRQ_PIDS] frictionCompensation parameter is outside the admissible range [0, 1]. FrictionCompensation reset to 0.0");
}
}
return true;
}
bool yarp::dev::IJoypadController::parseActions(const yarp::os::Searchable& cfg, int* count)
{
int dummy;
size_t i;
int& actCount = count ? *count : dummy;
if(!cfg.check(buttActionGroupName))
{
myInfo() << "no actions found in the configuration file (no" << buttActionGroupName << "group found)";
actCount = 0;
return true;
}
Bottle& actionsGroup = cfg.findGroup(buttActionGroupName);
if(!actionsGroup.size())
{
myError() << "no action found under" << buttActionGroupName << "group";
actCount = 0;
return false;
}
for(i = 1; i < actionsGroup.size(); i++)
{
if(!actionsGroup.get(i).isList())
{
yDebug() << "error parsing cfg";
return false;
}
Bottle& keyvalue = *actionsGroup.get(i).asList();
yDebug() << keyvalue.toString();
unsigned int buttonCount;
if(!this->getButtonCount(buttonCount))
{
myError() << "unable to get button count while parsing the actions";
actCount = 0;
return false;
}
if(!keyvalue.get(0).isInt32() ||
keyvalue.get(0).asInt32() < 0 ||
(unsigned int) keyvalue.get(0).asInt32() > buttonCount-1 ||
!keyvalue.get(1).isString())
{
myError() << "Button's actions parameters must be in the format 'unsigned int string' and the button id must be in range";
actCount = 0;
return false;
}
myInfo() << "assigning actions" << keyvalue.get(1).asString() << "to button" << keyvalue.get(0).asInt32();
m_actions[keyvalue.get(0).asInt32()] = keyvalue.get(1).asString();
}
actCount = i;
myInfo() << actCount << "action parsed succesfully";
return true;
}
bool checkRequiredParamIsVectorOfInt(yarp::os::Searchable& config,
const std::string& paramName,
std::vector<int> & output_vector)
{
bool correct = !(config.findGroup(paramName).isNull());
if( correct )
{
Bottle ids = config.findGroup(paramName).tail();
output_vector.resize(ids.size());
for(int i = 0; i < ids.size(); i++ )
{
output_vector[i] = ids.get(i).asInt();
}
}
if( !correct )
{
yError("CanBusInertialMTB: problem loading parameter %s as vector of int",paramName.c_str());
}
return correct;
}
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;
}
bool embObjMais::fromConfig(yarp::os::Searchable &_config)
{
#if defined(EMBOBJMAIS_USESERVICEPARSER)
if(false == parser->parseService(_config, serviceConfig))
{
return false;
}
return true;
#else
Bottle xtmp;
int _period = 0;
// Analog Sensor stuff
Bottle config = _config.findGroup("GENERAL");
if (!extractGroup(config, xtmp, "Period","transmitting period of the sensors", 1))
{
yError() << "embObjMais Using default value = 0 (disabled)";
_period = 0;
}
else
{
_period = xtmp.get(1).asInt();
yDebug() << "embObjMais::fromConfig() detects embObjMais Using value of" << _period;
}
serviceConfig.acquisitionrate = _period;
return true;
#endif
}
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 parametricCalibrator::open(yarp::os::Searchable& config)
{
yTrace();
Property p;
p.fromString(config.toString());
if (p.check("GENERAL") == false)
{
yError() << "Parametric calibrator: missing [GENERAL] section";
return false;
}
if (p.findGroup("GENERAL").check("deviceName"))
{
deviceName = p.findGroup("GENERAL").find("deviceName").asString();
}
else
{
yError() << "Parametric calibrator: missing deviceName parameter";
return false;
}
std::string str;
if (config.findGroup("GENERAL").find("verbose").asInt())
{
str = config.toString().c_str();
yTrace() << deviceName.c_str() << str;
}
// Check clearHwFaultBeforeCalibration
Value val_clearHwFault = config.findGroup("GENERAL").find("clearHwFaultBeforeCalibration");
if (val_clearHwFault.isNull())
{
clearHwFault = false;
}
else
{
if (!val_clearHwFault.isBool())
{
yError() << deviceName.c_str() << ": clearHwFaultBeforeCalibration bool param is different from accepted values (true / false). Assuming false";
clearHwFault = false;
}
else
{
clearHwFault = val_clearHwFault.asBool();
if (clearHwFault)
yInfo() << deviceName.c_str() << ": clearHwFaultBeforeCalibration option enabled\n";
}
}
// Check Vanilla = do not use calibration!
skipCalibration = config.findGroup("GENERAL").find("skipCalibration").asBool();// .check("Vanilla",Value(1), "Vanilla config");
skipCalibration = !!skipCalibration;
if (skipCalibration)
{
yWarning() << deviceName << ": skipping calibration!! This option was set in general.xml file.";
yWarning() << deviceName << ": BE CAREFUL USING THE ROBOT IN THIS CONFIGURATION! See 'skipCalibration' param in config file";
}
int nj = 0;
if (p.findGroup("GENERAL").check("joints"))
{
nj = p.findGroup("GENERAL").find("joints").asInt();
}
else if (p.findGroup("GENERAL").check("Joints"))
{
// This is needed to be backward compatibile with old iCubInterface
nj = p.findGroup("GENERAL").find("Joints").asInt();
}
else
{
yError() << deviceName.c_str() << ": missing joints parameter";
return false;
}
type = new unsigned char[nj];
param1 = new double[nj];
param2 = new double[nj];
param3 = new double[nj];
maxPWM = new int[nj];
zeroPos = new double[nj];
zeroVel = new double[nj];
currPos = new double[nj];
currVel = new double[nj];
homePos = new double[nj];
homeVel = new double[nj];
zeroPosThreshold = new double[nj];
int i = 0;
Bottle& xtmp = p.findGroup("CALIBRATION").findGroup("calibration1");
if (xtmp.size() - 1 != nj) { yError() << deviceName << ": invalid number of Calibration1 params " << xtmp.size() << " " << nj; return false; }
for (i = 1; i < xtmp.size(); i++) param1[i - 1] = xtmp.get(i).asDouble();
xtmp = p.findGroup("CALIBRATION").findGroup("calibration2");
if (xtmp.size() - 1 != nj) { yError() << deviceName << ": invalid number of Calibration2 params"; return false; }
for (i = 1; i < xtmp.size(); i++) param2[i - 1] = xtmp.get(i).asDouble();
xtmp = p.findGroup("CALIBRATION").findGroup("calibration3");
if (xtmp.size() - 1 != nj) { yError() << deviceName << ": invalid number of Calibration3 params"; return false; }
for (i = 1; i < xtmp.size(); i++) param3[i - 1] = xtmp.get(i).asDouble();
xtmp = p.findGroup("CALIBRATION").findGroup("calibrationType");
if (xtmp.size() - 1 != nj) { yError() << deviceName << ": invalid number of Calibration3 params"; return false; }
for (i = 1; i < xtmp.size(); i++) type[i - 1] = (unsigned char)xtmp.get(i).asDouble();
xtmp = p.findGroup("CALIBRATION").findGroup("startupPosition");
if (xtmp.size() - 1 != nj) { yError() << deviceName << ": invalid number of startupPosition params"; return false; }
for (i = 1; i < xtmp.size(); i++) zeroPos[i - 1] = xtmp.get(i).asDouble();
xtmp = p.findGroup("CALIBRATION").findGroup("startupVelocity");
if (xtmp.size() - 1 != nj) { yError() << deviceName << ": invalid number of startupVelocity params"; return false; }
for (i = 1; i < xtmp.size(); i++) zeroVel[i - 1] = xtmp.get(i).asDouble();
xtmp = p.findGroup("HOME").findGroup("positionHome");
if (xtmp.size() - 1 != nj) { yError() << deviceName << ": invalid number of PositionHome params"; return false; }
for (i = 1; i < xtmp.size(); i++) homePos[i - 1] = xtmp.get(i).asDouble();
xtmp = p.findGroup("HOME").findGroup("velocityHome");
if (xtmp.size() - 1 != nj) { yError() << deviceName << ": invalid number of VelocityHome params"; return false; }
for (i = 1; i < xtmp.size(); i++) homeVel[i - 1] = xtmp.get(i).asDouble();
xtmp = p.findGroup("CALIBRATION").findGroup("startupMaxPwm");
if (xtmp.size() - 1 != nj) { yError() << deviceName << ": invalid number of startupMaxPwm params"; return false; }
for (i = 1; i < xtmp.size(); i++) maxPWM[i - 1] = xtmp.get(i).asInt();
xtmp = p.findGroup("CALIBRATION").findGroup("startupPosThreshold");
if (xtmp.size()-1!=nj) {yError() << deviceName << ": invalid number of startupPosThreshold params"; return false;}
for (i = 1; i < xtmp.size(); i++) zeroPosThreshold[i-1] = xtmp.get(i).asDouble();
xtmp = p.findGroup("CALIB_ORDER");
int calib_order_size = xtmp.size();
if (calib_order_size <= 1) {yError() << deviceName << ": invalid number CALIB_ORDER params"; return false;}
//yDebug() << "CALIB_ORDER: group size: " << xtmp.size() << " values: " << xtmp.toString().c_str();
std::list<int> tmp;
for(int i=1; i<xtmp.size(); i++)
{
tmp.clear();
Bottle *set;
set= xtmp.get(i).asList();
for(int j=0; j<set->size(); j++)
{
tmp.push_back(set->get(j).asInt() );
}
joints.push_back(tmp);
}
return true;
}
bool laserHokuyo::open(yarp::os::Searchable& config)
{
bool correct=true;
internal_status = HOKUYO_STATUS_NOT_READY;
info = "Hokuyo Laser";
device_status = DEVICE_OK_STANBY;
#if LASER_DEBUG
yDebug("%s\n", config.toString().c_str());
#endif
bool br = config.check("GENERAL");
if (br == false)
{
yError("cannot read 'GENERAL' section");
return false;
}
yarp::os::Searchable& general_config = config.findGroup("GENERAL");
//list of mandatory options
//TODO change comments
period = general_config.check("Period", Value(50), "Period of the sampling thread").asInt();
start_position = general_config.check("Start_Position", Value(0), "Start position").asInt();
end_position = general_config.check("End_Position", Value(1080), "End Position").asInt();
error_codes = general_config.check("Convert_Error_Codes", Value(0), "Substitute error codes with legal measurments").asInt();
yarp::os::ConstString s = general_config.check("Laser_Mode", Value("GD"), "Laser Mode (GD/MD").asString();
if (general_config.check("Measurement_Units"))
{
yError() << "Deprecated parameter 'Measurement_Units'. Please Remove it from the configuration file.";
}
if (error_codes==1)
{
yInfo("'error_codes' option enabled. Invalid samples will be substituded with out-of-range measurements.");
}
if (s=="GD")
{
laser_mode = GD_MODE;
yInfo("Using GD mode (single acquisition)");
}
else if (s=="MD")
{
laser_mode = MD_MODE;
yInfo("Using MD mode (continuous acquisition)");
}
else
{
laser_mode = GD_MODE;
yError("Laser_mode not found. Using GD mode (single acquisition)");
}
bool ok = general_config.check("Serial_Configuration");
if (!ok)
{
yError("Cannot find configuration file for serial port communication!");
return false;
}
yarp::os::ConstString serial_filename = general_config.find("Serial_Configuration").asString();
//string st = config.toString();
setRate(period);
Property prop;
prop.put("device", "serialport");
ok = prop.fromConfigFile(serial_filename.c_str(),config,false);
if (!ok)
{
yError("Unable to read from serial port configuration file");
return false;
}
pSerial=0;
driver.open(prop);
if (!driver.isValid())
{
yError("Error opening PolyDriver check parameters");
return false;
}
driver.view(pSerial);
if (!pSerial)
{
yError("Error opening serial driver. Device not available");
return false;
}
Bottle b;
Bottle b_ans;
string ans;
// *** Check if the URG device is present ***
b.addString("SCIP2.0\n");
pSerial->send(b);
yarp::os::Time::delay(0.040);
pSerial->receive(b_ans);
if (b_ans.size()>0)
{
yInfo("URG device successfully initialized.\n");
yDebug("%s\n", b_ans.get(0).asString().c_str());
}
else
{
yError("Error: URG device not found.\n");
//return false;
}
b.clear();
b_ans.clear();
// *** Change the baud rate to 115200 ***
/*b.addString("SS01152001\n");
pSerial->send(b);
yarp::os::Time::delay(0.040);
pSerial->receive(b_ans);
ans = b_ans.get(0).asString();
yDebug("%s\n",ans.c_str());
b.clear();
b_ans.clear();*/
// *** Read the firmware version parameters ***
b.addString("VV\n");
pSerial->send(b);
yarp::os::Time::delay(0.040);
pSerial->receive(b_ans);
ans = b_ans.get(0).asString();
yDebug("%s\n",ans.c_str());
b.clear();
b_ans.clear();
// *** Read the sensor specifications ***
b.addString("II\n");
pSerial->send(b);
yarp::os::Time::delay(0.040);
pSerial->receive(b_ans);
ans = b_ans.get(0).asString();
yDebug("%s\n", ans.c_str());
b.clear();
b_ans.clear();
// *** Read the URG configuration parameters ***
b.addString("PP\n");
pSerial->send(b);
yarp::os::Time::delay(0.040);
pSerial->receive(b_ans);
/*
syntax of the answer:
MODL ... Model information of the sensor.
DMIN ... Minimum measurable distance [mm]
DMAX ... Maximum measurable distance [mm]
ARES ... Angular resolution(Number of splits in 360 degree)
AMIN ... First Step of the Measurement Range
AMAX ... Last Step of the Measurement Range
AFRT ... Step number on the sensor's front axis
SCAN ... Standard angular velocity
*/
ans = b_ans.get(0).asString();
yDebug( "%s\n", ans.c_str());
//parsing the answer
size_t found;
found = ans.find("MODL");
if (found!=string::npos) sensor_properties.MODL = string(ans.c_str()+found+5);
found = ans.find("DMIN");
if (found!=string::npos) sensor_properties.DMIN = atoi(ans.c_str()+found+5);
found = ans.find("DMAX");
if (found!=string::npos) sensor_properties.DMAX = atoi(ans.c_str()+found+5);
found = ans.find("ARES");
if (found!=string::npos) sensor_properties.ARES = atoi(ans.c_str()+found+5);
found = ans.find("AMIN");
if (found!=string::npos) sensor_properties.AMIN = atoi(ans.c_str()+found+5);
found = ans.find("AMAX");
if (found!=string::npos) sensor_properties.AMAX = atoi(ans.c_str()+found+5);
found = ans.find("AFRT");
if (found!=string::npos) sensor_properties.AFRT = atoi(ans.c_str()+found+5);
found = ans.find("SCAN");
if (found!=string::npos) sensor_properties.SCAN = atoi(ans.c_str()+found+5);
b.clear();
b_ans.clear();
// *** Turns on the Laser ***
b.addString("BM\n");
pSerial->send(b);
yarp::os::Time::delay(0.040);
pSerial->receive(b_ans);
// @@@TODO: Check the answer
b.clear();
b_ans.clear();
//elements are:
sensorsNum=16*12;
laser_data.resize(sensorsNum);
if (laser_mode==MD_MODE)
{
// *** Starts endless acquisition mode***
char message [255];
sprintf (message,"MD%04d%04d%02d%01d%02d\n",start_position,end_position,1,1,0);
b.addString(message);
pSerial->send(b);
b.clear();
b_ans.clear();
}
else if (laser_mode==GD_MODE)
{
// *** Starts one single acquisition ***
char message [255];
sprintf (message,"GD%04d%04d%02d\n",start_position,end_position,1);
b.addString(message);
pSerial->send(b);
b.clear();
b_ans.clear();
}
Time::turboBoost();
RateThread::start();
return true;
}
bool parametricCalibrator::open(yarp::os::Searchable& config)
{
yTrace();
Property p;
p.fromString(config.toString());
if (p.check("GENERAL")==false)
{
yError() << "missing [GENERAL] section";
return false;
}
if(p.findGroup("GENERAL").check("deviceName"))
{
deviceName = p.findGroup("GENERAL").find("deviceName").asString();
}
else
{
yError() << "missing deviceName parameter";
return false;
}
std::string str;
if(config.findGroup("GENERAL").find("verbose").asInt())
{
str=config.toString().c_str();
yTrace() << deviceName.c_str() << str;
}
// Check Vanilla = do not use calibration!
skipCalibration =config.findGroup("GENERAL").find("skipCalibration").asBool() ;// .check("Vanilla",Value(1), "Vanilla config");
skipCalibration = !!skipCalibration;
if(skipCalibration )
yWarning() << "parametric calibrator: skipping calibration!! This option was set in general.xml file.";
int nj = 0;
if(p.findGroup("GENERAL").check("joints"))
{
nj = p.findGroup("GENERAL").find("joints").asInt();
}
else
{
yError() << deviceName.c_str() << ": missing joints parameter" ;
return false;
}
type = new unsigned char[nj];
param1 = new double[nj];
param2 = new double[nj];
param3 = new double[nj];
maxPWM = new int[nj];
zeroPos = new double[nj];
zeroVel = new double[nj];
currPos = new double[nj];
currVel = new double[nj];
homePos = new double[nj];
homeVel = new double[nj];
zeroPosThreshold = new double[nj];
int i=0;
Bottle& xtmp = p.findGroup("CALIBRATION").findGroup("calibration1");
if (xtmp.size()-1!=nj) {yError() << deviceName << ": invalid number of Calibration1 params " << xtmp.size()<< " " << nj; return false;}
for (i = 1; i < xtmp.size(); i++) param1[i-1] = xtmp.get(i).asDouble();
xtmp = p.findGroup("CALIBRATION").findGroup("calibration2");
if (xtmp.size()-1!=nj) {yError() << "invalid number of Calibration2 params"; return false;}
for (i = 1; i < xtmp.size(); i++) param2[i-1] = xtmp.get(i).asDouble();
xtmp = p.findGroup("CALIBRATION").findGroup("calibration3");
if (xtmp.size()-1!=nj) {yError() << "invalid number of Calibration3 params"; return false;}
for (i = 1; i < xtmp.size(); i++) param3[i-1] = xtmp.get(i).asDouble();
xtmp = p.findGroup("CALIBRATION").findGroup("calibrationType");
if (xtmp.size()-1!=nj) {yError() << "invalid number of Calibration3 params"; return false;}
for (i = 1; i < xtmp.size(); i++) type[i-1] = (unsigned char) xtmp.get(i).asDouble();
xtmp = p.findGroup("CALIBRATION").findGroup("positionZero");
if (xtmp.size()-1!=nj) {yError() << "invalid number of PositionZero params"; return false;}
for (i = 1; i < xtmp.size(); i++) zeroPos[i-1] = xtmp.get(i).asDouble();
xtmp = p.findGroup("CALIBRATION").findGroup("velocityZero");
if (xtmp.size()-1!=nj) {yError() << "invalid number of VelocityZero params"; return false;}
for (i = 1; i < xtmp.size(); i++) zeroVel[i-1] = xtmp.get(i).asDouble();
xtmp = p.findGroup("HOME").findGroup("positionHome");
if (xtmp.size()-1!=nj) {yError() << "invalid number of PositionHome params"; return false;}
for (i = 1; i < xtmp.size(); i++) homePos[i-1] = xtmp.get(i).asDouble();
xtmp = p.findGroup("HOME").findGroup("velocityHome");
if (xtmp.size()-1!=nj) {yError() << "invalid number of VelocityHome params"; return false;}
for (i = 1; i < xtmp.size(); i++) homeVel[i-1] = xtmp.get(i).asDouble();
xtmp = p.findGroup("CALIBRATION").findGroup("maxPwm");
if (xtmp.size()-1!=nj) {yError() << "invalid number of MaxPwm params"; return false;}
for (i = 1; i < xtmp.size(); i++) maxPWM[i-1] = xtmp.get(i).asInt();
xtmp = p.findGroup("CALIBRATION").findGroup("posZeroThreshold");
if (xtmp.size()-1!=nj) {yError() << "invalid number of PosZeroThreshold params"; return false;}
for (i = 1; i < xtmp.size(); i++) zeroPosThreshold[i-1] = xtmp.get(i).asDouble();
xtmp = p.findGroup("CALIB_ORDER");
// yDebug() << "Group size " << xtmp.size() << "\nvalues: " << xtmp.toString().c_str();
std::list<int> tmp;
for(int i=1; i<xtmp.size(); i++)
{
tmp.clear();
Bottle *set;
set= xtmp.get(i).asList();
for(int j=0; j<set->size(); j++)
{
tmp.push_back(set->get(j).asInt() );
}
joints.push_back(tmp);
}
return true;
}
bool FakeLaser::open(yarp::os::Searchable& config)
{
info = "Fake Laser device for test/debugging";
device_status = DEVICE_OK_STANBY;
#ifdef LASER_DEBUG
yDebug("%s\n", config.toString().c_str());
#endif
if (config.check("help"))
{
yInfo("Some examples:");
yInfo("yarpdev --device fakeLaser --help");
yInfo("yarpdev --device Rangefinder2DWrapper --subdevice fakeLaser --period 10 --name /ikart/laser:o --test no_obstacles");
yInfo("yarpdev --device Rangefinder2DWrapper --subdevice fakeLaser --period 10 --name /ikart/laser:o --test use_pattern");
yInfo("yarpdev --device Rangefinder2DWrapper --subdevice fakeLaser --period 10 --name /ikart/laser:o --test use_mapfile --map_file mymap.map");
yInfo("yarpdev --device Rangefinder2DWrapper --subdevice fakeLaser --period 10 --name /ikart/laser:o --test use_mapfile --map_file mymap.map --localization_port /fakeLaser/location:i");
yInfo("yarpdev --device Rangefinder2DWrapper --subdevice fakeLaser --period 10 --name /ikart/laser:o --test use_mapfile --map_file mymap.map --localization_client /fakeLaser/localizationClient");
return false;
}
bool br = config.check("GENERAL");
if (br != false)
{
yarp::os::Searchable& general_config = config.findGroup("GENERAL");
period = general_config.check("Period", Value(50), "Period of the sampling thread").asInt32() / 1000.0;
}
string string_test_mode = config.check("test", Value(string("use_pattern")), "string to select test mode").asString();
if (string_test_mode == "no_obstacles") { m_test_mode = NO_OBSTACLES; }
else if (string_test_mode == "use_pattern") { m_test_mode = USE_PATTERN; }
else if (string_test_mode == "use_mapfile") { m_test_mode = USE_MAPFILE; }
else { yError() << "invalid/unknown value for param 'test'"; return false; }
min_distance = 0.1; //m
max_distance = 3.5; //m
min_angle = 0; //degrees
max_angle = 360; //degrees
resolution = 1.0; //degrees
sensorsNum = (int)((max_angle-min_angle)/resolution);
laser_data.resize(sensorsNum);
if (m_test_mode == USE_MAPFILE)
{
string map_file;
if (config.check("map_file"))
{
map_file = config.check("map_file",Value(string("map.yaml")),"map filename").asString();
}
else
{
yError() << "Missing map_file";
return false;
}
bool ret = m_map.loadFromFile(map_file);
if (ret == false)
{
yError() << "A problem occurred while opening:" << map_file;
return false;
}
if (config.check("localization_port"))
{
string localization_port_name = config.check("localization_port", Value(string("/fakeLaser/location:i")), "name of localization input port").asString();
m_loc_port = new yarp::os::BufferedPort<yarp::os::Bottle>;
m_loc_port->open(localization_port_name.c_str());
yInfo() << "Robot localization will be obtained from port" << localization_port_name;
m_loc_mode = LOC_FROM_PORT;
}
else if (config.check("localization_client"))
{
Property loc_options;
string localization_device_name = config.check("localization_client", Value(string("/fakeLaser/localizationClient")), "local name of localization client device").asString();
loc_options.put("device", "localization2DClient");
loc_options.put("local", localization_device_name.c_str());
loc_options.put("remote", "/localizationServer");
loc_options.put("period", 10);
m_pLoc = new PolyDriver;
if (m_pLoc->open(loc_options) == false)
{
yError() << "Unable to open localization driver";
return false;
}
m_pLoc->view(m_iLoc);
if (m_iLoc == nullptr)
{
yError() << "Unable to open localization interface";
return false;
}
yInfo() << "Robot localization will be obtained from localization_client" << localization_device_name;
m_loc_mode = LOC_FROM_CLIENT;
}
else
{
yInfo() << "No localization method selected. Robot location set to 0,0,0";
m_loc_mode = LOC_NOT_SET;
}
m_loc_x=0;
m_loc_y=0;
m_loc_t=0;
max_distance = 8; //m
}
yInfo("Starting debug mode");
yInfo("max_dist %f, min_dist %f", max_distance, min_distance);
yInfo("max_angle %f, min_angle %f", max_angle, min_angle);
yInfo("resolution %f", resolution);
yInfo("sensors %d", sensorsNum);
yInfo("test mode: %d", m_test_mode);
return PeriodicThread::start();
}
bool embObjAnalogSensor::open(yarp::os::Searchable &config)
{
std::string str;
if(config.findGroup("GENERAL").find("verbose").asBool())
str=config.toString().c_str();
else
str="\n";
yTrace() << str;
// Read stuff from config file
if(!fromConfig(config))
{
yError() << "embObjAnalogSensor missing some configuration parameter. Check logs and your config file.";
return false;
}
// Tmp variables
Bottle groupEth;
ACE_TCHAR address[64];
ACE_UINT16 port;
bool ret;
Bottle groupTransceiver = Bottle(config.findGroup("TRANSCEIVER"));
if(groupTransceiver.isNull())
{
yError() << "embObjAnalogSensor::open(): Can't find TRANSCEIVER group in xml config files";
return false;
}
Bottle groupProtocol = Bottle(config.findGroup("PROTOCOL"));
if(groupProtocol.isNull())
{
yError() << "embObjAnalogSensor::open(): Can't find PROTOCOL group in xml config files";
return false;
}
// Get both PC104 and EMS ip addresses and port from config file
groupEth = Bottle(config.findGroup("ETH"));
Bottle parameter1( groupEth.find("PC104IpAddress").asString() ); // .findGroup("IpAddress");
port = groupEth.find("CmdPort").asInt(); // .get(1).asInt();
snprintf(_fId.pc104IPaddr.string, sizeof(_fId.pc104IPaddr.string), "%s", parameter1.toString().c_str());
_fId.pc104IPaddr.port = port;
Bottle parameter2( groupEth.find("IpAddress").asString() ); // .findGroup("IpAddress");
snprintf(_fId.boardIPaddr.string, sizeof(_fId.boardIPaddr.string), "%s", parameter2.toString().c_str());
_fId.boardIPaddr.port = port;
sscanf(_fId.boardIPaddr.string,"\"%d.%d.%d.%d", &_fId.boardIPaddr.ip1, &_fId.boardIPaddr.ip2, &_fId.boardIPaddr.ip3, &_fId.boardIPaddr.ip4);
sscanf(_fId.pc104IPaddr.string,"\"%d.%d.%d.%d", &_fId.pc104IPaddr.ip1, &_fId.pc104IPaddr.ip2, &_fId.pc104IPaddr.ip3, &_fId.pc104IPaddr.ip4);
snprintf(_fId.boardIPaddr.string, sizeof(_fId.boardIPaddr.string), "%u.%u.%u.%u:%u", _fId.boardIPaddr.ip1, _fId.boardIPaddr.ip2, _fId.boardIPaddr.ip3, _fId.boardIPaddr.ip4, _fId.boardIPaddr.port);
snprintf(_fId.pc104IPaddr.string, sizeof(_fId.pc104IPaddr.string), "%u.%u.%u.%u:%u", _fId.pc104IPaddr.ip1, _fId.pc104IPaddr.ip2, _fId.pc104IPaddr.ip3, _fId.pc104IPaddr.ip4, _fId.pc104IPaddr.port);
// debug info
memset(info, 0x00, sizeof(info));
snprintf(info, sizeof(info), "embObjAnalogSensor: referred to EMS: %d at address %s\n", _fId.boardNumber, address);
snprintf(_fId.name, sizeof(_fId.name), "%s", info); // Saving User Friendly Id
// Set dummy values
_fId.boardNumber = FEAT_boardnumber_dummy;
Value val = config.findGroup("ETH").check("Ems", Value(1), "Board number");
if(val.isInt())
_fId.boardNumber = val.asInt();
else
{
yError () << "embObjAnalogSensor: EMS Board number identifier not found for IP" << _fId.pc104IPaddr.string;
return false;
}
ethManager = TheEthManager::instance();
if(NULL == ethManager)
{
yFatal() << "embObjAnalogSensor::open() cannot instantiate ethManager";
return false;
}
// N.B.: use a dynamic_cast<> to extract correct interface when using this pointer
_fId.interface = this;
_fId.endpoint = eoprot_endpoint_analogsensors;
if(AS_STRAIN == _as_type)
_fId.type = ethFeatType_AnalogStrain;
else if(AS_MAIS == _as_type)
_fId.type = ethFeatType_AnalogMais;
/* Once I'm ok, ask for resources, through the _fId struct I'll give the ip addr, port and
* and boardNum to the ethManager in order to create the ethResource requested.
*/
res = ethManager->requestResource(config, groupTransceiver, groupProtocol, _fId);
if(NULL == res)
{
yError() << "embObjAnalogSensor::open() fails because could not instantiate the ethResource board" << _fId.boardNumber << " ... unable to continue";
return false;
}
if(false == res->isEPmanaged(eoprot_endpoint_analogsensors))
{
yError() << "embObjAnalogSensor::open() detected that EMS "<< _fId.boardNumber << " does not support analog sensors";
cleanup();
return false;
}
if(false == res->verifyBoard(groupProtocol))
{
yError() << "embObjAnalogSensor::open() fails in function verifyBoard() for board " << _fId.boardNumber << ": CANNOT PROCEED ANY FURTHER";
cleanup();
return false;
}
else
{
if(verbosewhenok)
{
yDebug() << "embObjAnalogSensor::open() has succesfully verified that board "<< _fId.boardNumber << " is communicating correctly";
}
}
if(!res->verifyEPprotocol(groupProtocol, eoprot_endpoint_analogsensors))
{
yError() << "embObjAnalogSensor::open() fails in function verifyEPprotocol() for board "<< _fId.boardNumber << ": either the board cannot be reached or it does not have the same eoprot_endpoint_management and/or eoprot_endpoint_analogsensors protocol version: DO A FW UPGRADE";
cleanup();
return false;
}
else
{
if(verbosewhenok)
{
yDebug() << "embObjAnalogSensor::open() has succesfully verified that board "<< _fId.boardNumber << " has same protocol version for analogsensors as robotInterface";
}
}
// // marco.accame on 04 sept 2014: we could add a verification about the entities of analog ... MAYBE in the future
//
// uint8_t numberofmaisboards = eoprot_entity_numberof_get(featIdBoardNum2nvBoardNum(_fId.boardNumber), eoprot_endpoint_analogsensors, eoprot_entity_as_mais);
// if(false == res->verifyENTITYnumber(groupProtocol, eoprot_endpoint_analogsensors, eoprot_entity_as_mais, numberofmaisboards))
// { // JUST AN EXAMPLE OF HOW TO DO IT FOR THE MAIS.
// yError() << "embObjAnalogSensor::init() fails in function verifyENTITYnumber() for board "<< _fId.boardNumber << " and entity eoprot_entity_as_mais: VERIFY their number in board, and in XML files";
// return false;
// }
// uint8_t numberofstrainboards = eoprot_entity_numberof_get(featIdBoardNum2nvBoardNum(_fId.boardNumber), eoprot_endpoint_analogsensors, eoprot_entity_as_strain);
// if(false == res->verifyENTITYnumber(groupProtocol, eoprot_endpoint_analogsensors, eoprot_entity_as_strain, numberofstrainboards))
// { // JUST AN EXAMPLE OF HOW TO DO IT FOR THE STRAIN.
// yError() << "embObjAnalogSensor::init() fails in function verifyENTITYnumber() for board "<< _fId.boardNumber << " and entity eoprot_entity_as_strain: VERIFY their number in board, and in XML files";
// return false;
// }
data = new AnalogData(_channels, _channels+1);
scaleFactor = new double[_channels];
int i=0;
for (i=0; i<_channels; i++) scaleFactor[i]=1;
// Real values will be read from the sensor itself during its initalization hereafter
for(int i=0; i<_channels; i++)
{
scaleFactor[i]=1;
}
//#warning --> marco.accame on 04sept14: both embObjAnalogSensors and embObjMotionControl initialises the mais board. but potentially w/ different params (mc uses: datarate = 10 and mode = eoas_maismode_txdatacontinuously).
switch(_as_type)
{
case AS_MAIS:
{
ret = sendConfig2Mais();
} break;
case AS_STRAIN:
{
ret = sendConfig2Strain();
} break;
default:
{
//i should not be here. if AS_NONE then i should get error in fromConfig function
ret = false;
}
}
if(!ret)
{
cleanup();
return false;
}
// Set variable to be signalled
if(! init())
{
cleanup();
return false;
}
if(false == res->goToRun())
{
yError() << "embObjAnalogSensor::open() fails to start control loop of board" << _fId.boardNumber << ": cannot continue";
cleanup();
return false;
}
else
{
if(verbosewhenok)
{
yDebug() << "embObjAnalogSensor::open() correctly activated control loop of BOARD" << _fId.boardNumber;
}
}
opened = true;
return true;
}
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","transmetting period of the sensor", 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 Analog Sensor", 1))
{
yWarning("embObjAnalogSensor: Using default value = 0 (disabled)\n");
_channels = 0;
_period = 0;
_as_type=AS_NONE;
}
else
{
_channels = xtmp.get(1).asInt();
}
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;
return false;
}
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 JointTorqueControl::loadCouplingMatrix(yarp::os::Searchable& config,
CouplingMatrices & coupling_matrix,
std::string group_name)
{
if( !config.check(group_name) )
{
coupling_matrix.reset(this->axes);
return true;
}
else
{
yarp::os::Bottle couplings_bot = config.findGroup(group_name);
if(!checkVectorExistInConfiguration(couplings_bot,"axesNames",this->axes) )
{
std::cerr << "JointTorqueControl: error in loading axesNames parameter" << std::endl;
return false;
}
if(!checkVectorExistInConfiguration(couplings_bot,"motorNames",this->axes) )
{
std::cerr << "JointTorqueControl: error in loading motorNames parameter" << std::endl;
return false;
}
std::vector<std::string> motorNames(this->axes);
std::vector<std::string> axesNames(this->axes);
for(int j=0; j < this->axes; j++)
{
motorNames[j] = couplings_bot.find("motorNames").asList()->get(j).asString();
axesNames[j] = couplings_bot.find("axesNames").asList()->get(j).asString();
}
for(int axis_id = 0; axis_id < (int)this->axes; axis_id++)
{
//search if coupling information is provided for each axis, if not return false
std::string axis_name = axesNames[axis_id];
if( !couplings_bot.check(axis_name) ||
!(couplings_bot.find(axis_name).isList()) )
{
std::cerr << "[ERR] " << group_name << " group found, but no coupling found for joint "
<< axis_name << ", exiting" << std::endl;
return false;
}
//Check coupling configuration is well formed
yarp::os::Bottle * axis_coupling_bot = couplings_bot.find(axis_name).asList();
for(int coupled_motor=0; coupled_motor < axis_coupling_bot->size(); coupled_motor++ )
{
if( !(axis_coupling_bot->get(coupled_motor).isList()) ||
!(axis_coupling_bot->get(coupled_motor).asList()->size() == 2) ||
!(axis_coupling_bot->get(coupled_motor).asList()->get(0).isDouble()) ||
!(axis_coupling_bot->get(coupled_motor).asList()->get(1).isString()) )
{
std::cerr << "[ERR] " << group_name << " group found, but coupling for axis "
<< axis_name << " is malformed" << std::endl;
return false;
}
std::string motorName = axis_coupling_bot->get(coupled_motor).asList()->get(1).asString().c_str();
if( !contains(motorNames,motorName) )
{
std::cerr << "[ERR] " << group_name << "group found, but motor name "
<< motorName << " is not part of the motor list" << std::endl;
return false;
}
}
// Zero the row of the selected axis in velocity coupling matrix
coupling_matrix.fromJointVelocitiesToMotorVelocities.row(axis_id).setZero();
// Get non-zero coefficient of the coupling matrices
for(int coupled_motor=0; coupled_motor < axis_coupling_bot->size(); coupled_motor++ )
{
double coeff = axis_coupling_bot->get(coupled_motor).asList()->get(0).asDouble();
std::string motorName = axis_coupling_bot->get(coupled_motor).asList()->get(1).asString();
int motorIndex = findAndReturnIndex(motorNames,motorName);
if( motorIndex == -1 )
{
return false;
}
coupling_matrix.fromJointVelocitiesToMotorVelocities(axis_id,motorIndex) = coeff;
}
}
std::cerr << "loadCouplingMatrix DEBUG: " << std::endl;
std::cerr << "loaded kinematic coupling matrix from group " << group_name << std::endl;
std::cerr << coupling_matrix.fromJointVelocitiesToMotorVelocities << std::endl;
// std::cerr << "loaded torque coupling matrix from group " << group_name << std::endl;
// std::cerr << coupling_matrix.fromJointTorquesToMotorTorques << std::endl;
coupling_matrix.fromJointTorquesToMotorTorques = coupling_matrix.fromJointVelocitiesToMotorVelocities.transpose();
coupling_matrix.fromMotorTorquesToJointTorques = coupling_matrix.fromJointTorquesToMotorTorques.inverse();
coupling_matrix.fromJointVelocitiesToMotorVelocities = coupling_matrix.fromJointVelocitiesToMotorVelocities.inverse();
// Compute the torque coupling matrix
return true;
}
}
bool embObjVirtualAnalogSensor::fromConfig(yarp::os::Searchable &_config)
{
Bottle xtmp;
// Analog Sensor stuff
Bottle config = _config.findGroup("GENERAL");
if (!extractGroup(config, xtmp, "Channels","Number of channels of the Analog Sensor", 1))
{
yError() << "embObjVirtualAnalogSensor: Missing channel number... aborting";
_channels = 0;
return false;
}
else
{
_channels = xtmp.get(1).asInt();
}
// alloc vector for keeping conversion values
_fullscale = allocAndCheck<double>(_channels);
_resolution = allocAndCheck<double>(_channels);
if (!extractGroup(config, xtmp, "UseCalibration","Calibration parameters are needed", 1))
{
return false;
}
else
{
_useCalibration = xtmp.get(1).asInt();
}
if (!extractGroup(config, xtmp, "MaxValue","full scale value for this measure", _channels))
{
yError() << "Missing conversion factor!! Aborting...";
return false;
}
else
{
if(_verbose)
yDebug() << "embObjVirtualAnalogSensor, fullscales from config file: ";
for (int ch = 1; ch < xtmp.size(); ch++)
{
_fullscale[ch-1] = xtmp.get(ch).asDouble();
if(_verbose)
yDebug() << "ch " << ch << ": " << _fullscale[ch-1];
}
}
if (!extractGroup(config, xtmp, "Resolution","Number of bytes used for this measure", _channels))
{
yError() << "Missing resolution!! Aborting...";
return false;
}
else
{
if(_verbose)
yDebug() << "embObjVirtualAnalogSensor, resolutions from config file: ";
for (int ch = 1; ch < xtmp.size(); ch++)
{
_resolution[ch-1] = (double) (1 << (xtmp.get(ch).asInt()-1) );
if(_verbose)
yDebug() << "ch " << ch << ": " << _resolution[ch-1];
}
}
return true;
}
bool embObjVirtualAnalogSensor::open(yarp::os::Searchable &config)
{
std::string str;
if(config.findGroup("GENERAL").find("Verbose").asInt())
_verbose = true;
if(_verbose)
str=config.toString().c_str();
else
str="\n";
yTrace() << str;
// Read stuff from config file
if(!fromConfig(config))
{
yError() << "embObjAnalogSensor missing some configuration parameter. Check logs and your config file.";
return false;
}
// Tmp variables
Bottle groupEth;
ACE_TCHAR address[64];
ACE_UINT16 port;
// Get both PC104 and EMS ip addresses and port from config file
groupEth = Bottle(config.findGroup("ETH"));
Bottle parameter1( groupEth.find("PC104IpAddress").asString() ); // .findGroup("IpAddress");
port = groupEth.find("CmdPort").asInt(); // .get(1).asInt();
snprintf(_fId.PC104ipAddr.string, sizeof(_fId.PC104ipAddr.string), "%s", parameter1.toString().c_str());
_fId.PC104ipAddr.port = port;
Bottle parameter2( groupEth.find("IpAddress").asString() ); // .findGroup("IpAddress");
snprintf(_fId.EMSipAddr.string, sizeof(_fId.EMSipAddr.string), "%s", parameter2.toString().c_str());
_fId.EMSipAddr.port = port;
sscanf(_fId.EMSipAddr.string,"\"%d.%d.%d.%d", &_fId.EMSipAddr.ip1, &_fId.EMSipAddr.ip2, &_fId.EMSipAddr.ip3, &_fId.EMSipAddr.ip4);
sscanf(_fId.PC104ipAddr.string,"\"%d.%d.%d.%d", &_fId.PC104ipAddr.ip1, &_fId.PC104ipAddr.ip2, &_fId.PC104ipAddr.ip3, &_fId.PC104ipAddr.ip4);
snprintf(_fId.EMSipAddr.string, sizeof(_fId.EMSipAddr.string), "%u.%u.%u.%u:%u", _fId.EMSipAddr.ip1, _fId.EMSipAddr.ip2, _fId.EMSipAddr.ip3, _fId.EMSipAddr.ip4, _fId.EMSipAddr.port);
snprintf(_fId.PC104ipAddr.string, sizeof(_fId.PC104ipAddr.string), "%u.%u.%u.%u:%u", _fId.PC104ipAddr.ip1, _fId.PC104ipAddr.ip2, _fId.PC104ipAddr.ip3, _fId.PC104ipAddr.ip4, _fId.PC104ipAddr.port);
// Debug info
snprintf(_fId.name, sizeof(_fId.name), "embObjAnalogSensor: referred to EMS: %d at address %s\n", _fId.boardNum, address); // Saving User Friendly Id
// Set dummy values
_fId.boardNum = FEAT_boardnumber_dummy;
_fId.ep = eoprot_endpoint_none;
Value val =config.findGroup("ETH").check("Ems",Value(1), "Board number");
if(val.isInt())
_fId.boardNum =val.asInt();
else
{
yError () << "embObjAnalogSensor: EMS Board number identifier not found for IP" << _fId.PC104ipAddr.string;
return false;
}
ethManager = TheEthManager::instance();
if(NULL == ethManager)
{
yFatal() << "Unable to instantiate ethManager";
return false;
}
//N.B.: use a dynamic_cast to extract correct interface when using this pointer
_fId.handle = (this);
/* Once I'm ok, ask for resources, through the _fId struct I'll give the ip addr, port and
* and boradNum to the ethManagerin order to create the ethResource requested.
* I'll Get back the very same sturct filled with other data useful for future handling
* like the EPvector and EPhash_function */
res = ethManager->requestResource(&_fId);
if(NULL == res)
{
yError() << "EMS device not instantiated... unable to continue";
return false;
}
/*IMPORTANT: implement isEpManagedByBoard like every embObj obj when virtaulAnalogSensor will be exist in eo proto!!!!*/
// if(!isEpManagedByBoard())
// {
// yError() << "EMS "<< _fId.boardNum << "is not connected to virtual analog sensor";
// return false;
// }
yTrace() << "EmbObj Virtual Analog Sensor for board "<< _fId.boardNum << "instantiated correctly";
return true;
}
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 ServerFrameGrabber::open(yarp::os::Searchable& config) {
if (active) {
printf("Did you just try to open the same ServerFrameGrabber twice?\n");
return false;
}
// for AV, control whether output goes on a single port or multiple
bool separatePorts = false;
p.setReader(*this);
yarp::os::Value *name;
if (config.check("subdevice",name,"name (or nested configuration) of device to wrap")) {
if (name->isString()) {
// maybe user isn't doing nested configuration
yarp::os::Property p;
p.setMonitor(config.getMonitor(),
name->toString().c_str()); // pass on any monitoring
p.fromString(config.toString());
p.put("device",name->toString());
p.unput("subdevice");
poly.open(p);
} else {
Bottle subdevice = config.findGroup("subdevice").tail();
poly.open(subdevice);
}
if (!poly.isValid()) {
//printf("cannot make <%s>\n", name->toString().c_str());
return false;
}
} else {
printf("\"--subdevice <name>\" not set for server_framegrabber\n");
return false;
}
if (poly.isValid()) {
IAudioVisualStream *str;
poly.view(str);
bool a = true;
bool v = true;
bool vraw = true;
if (str!=NULL) {
a = str->hasAudio();
v = str->hasVideo();
vraw = str->hasRawVideo();
}
if (v) {
poly.view(fgImage);
}
if (vraw) {
poly.view(fgImageRaw);
}
if (a) {
poly.view(fgSound);
}
if (a&&v) {
poly.view(fgAv);
}
poly.view(fgCtrl);
poly.view(fgTimed);
}
canDrop = !config.check("no_drop","if present, use strict policy for sending data");
addStamp = config.check("stamp","if present, add timestamps to data");
p.promiseType(Type::byName("yarp/image")); // TODO: reflect audio options
p.setWriteOnly();
p.open(config.check("name",Value("/grabber"),
"name of port to send data on").asString());
/*
double framerate=0;
if (config.check("framerate", name,
"maximum rate in Hz to read from subdevice")) {
framerate=name->asDouble();
}
*/
if (fgAv&&
!config.check("shared-ports",
"If present, send audio and images on same port")) {
separatePorts = true;
yAssert(p2==NULL);
p2 = new Port;
yAssert(p2!=NULL);
p2->open(config.check("name2",Value("/grabber2"),
"Name of second port to send data on, when audio and images sent separately").asString());
}
if (fgAv!=NULL) {
if (separatePorts) {
yAssert(p2!=NULL);
thread.attach(new DataWriter2<yarp::sig::ImageOf<yarp::sig::PixelRgb>, yarp::sig::Sound>(p,*p2,*this,canDrop,addStamp));
} else {
thread.attach(new DataWriter<ImageRgbSound>(p,*this,canDrop,
addStamp));
}
} else if (fgImage!=NULL) {
thread.attach(new DataWriter<yarp::sig::ImageOf<yarp::sig::PixelRgb> >(p,*this,canDrop,addStamp,fgTimed));
} else if (fgImageRaw!=NULL) {
thread.attach(new DataWriter<yarp::sig::ImageOf<yarp::sig::PixelMono> >(p,*this,canDrop,addStamp,fgTimed));
} else if (fgSound!=NULL) {
thread.attach(new DataWriter<yarp::sig::Sound>(p,*this,canDrop));
} else {
printf("subdevice <%s> doesn't look like a framegrabber\n",
name->toString().c_str());
return false;
}
singleThreaded =
config.check("single_threaded",
"if present, operate in single threaded mode")!=0;
thread.open(config.check("framerate",Value("0"),
"maximum rate in Hz to read from subdevice").asDouble(),
singleThreaded);
active = true;
/*
#define VOCAB_BRIGHTNESS VOCAB3('b','r','i')
#define VOCAB_EXPOSURE VOCAB4('e','x','p','o')
#define VOCAB_SHARPNESS VOCAB4('s','h','a','r')
#define VOCAB_WHITE VOCAB4('w','h','i','t')
#define VOCAB_HUE VOCAB3('h','u','e')
#define VOCAB_SATURATION VOCAB4('s','a','t','u')
#define VOCAB_GAMMA VOCAB4('g','a','m','m')
#define VOCAB_SHUTTER VOCAB4('s','h','u','t')
#define VOCAB_GAIN VOCAB4('g','a','i','n')
#define VOCAB_IRIS VOCAB4('i','r','i','s')
*/
DeviceResponder::makeUsage();
addUsage("[set] [bri] $fBrightness", "set brightness");
addUsage("[set] [expo] $fExposure", "set exposure");
addUsage("[set] [shar] $fSharpness", "set sharpness");
addUsage("[set] [whit] $fBlue $fRed", "set white balance");
addUsage("[set] [hue] $fHue", "set hue");
addUsage("[set] [satu] $fSaturation", "set saturation");
addUsage("[set] [gamm] $fGamma", "set gamma");
addUsage("[set] [shut] $fShutter", "set shutter");
addUsage("[set] [gain] $fGain", "set gain");
addUsage("[set] [iris] $fIris", "set iris");
addUsage("[get] [bri]", "get brightness");
addUsage("[get] [expo]", "get exposure");
addUsage("[get] [shar]", "get sharpness");
addUsage("[get] [whit]", "get white balance");
addUsage("[get] [hue]", "get hue");
addUsage("[get] [satu]", "get saturation");
addUsage("[get] [gamm]", "get gamma");
addUsage("[get] [shut]", "get shutter");
addUsage("[get] [gain]", "get gain");
addUsage("[get] [iris]", "get iris");
addUsage("[get] [w]", "get width of image");
addUsage("[get] [h]", "get height of image");
return true;
}
bool TorqueBalancingReferencesGenerator::loadReferences(yarp::os::Searchable& config,
std::vector<Postures> & postures, std::vector<ComTimeAndSetPoints> & comTimeAndSetPoints, double actuatedDOFs,
bool & changePostural, bool & changeComWithSetPoints,
double & amplitudeOfOscillation, double & frequencyOfOscillation,yarp::sig::Vector & directionOfOscillation )
{
std::string group_name = "REFERENCES";
if( !config.check(group_name) )
{
std::cerr << "[ERR] no REFERENCES group found" << std::endl;
return true;
}
else
{
yarp::os::Bottle group_bot = config.findGroup(group_name);
if( !group_bot.check("postures") )
{
std::cerr << "[INFO] postures found in config file, qDes kept at initial condition" << std::endl;
changePostural = false;
}
else
{
if( group_bot.check("posturesInRadians") &&
group_bot.check("posturesInDegrees"))
{
std::cerr << "[ERR] Both posturesInRadians and posturesInDegrees found in the configuration file. Incapable of interpreting postures' unit of measurement" << std::endl;
return false;
}
if( !(group_bot.check("posturesInRadians") ||
group_bot.check("posturesInDegrees")))
{
std::cerr << "[ERR] Neither posturesInRadians nor posturesInDegrees found in the configuration file. Incapable of interpreting postures' unit of measurement" << std::endl;
return false;
}
double conversion = 1;
bool posturesInRadians = true;
if(group_bot.check("posturesInDegrees"))
{
conversion = M_PI/180;
posturesInRadians = false;
}
changePostural = true;
if( !(group_bot.find("postures").isList()) )
{
std::cerr << "[ERR] postures is not a list of lists, each of which must be a NDOF+1 element vector, where the first is the time at which the ith qDes must be streamed"
<< std::endl;
return false;
}
yarp::os::Bottle * postures_bot = group_bot.find("postures").asList();
int numberOfPostures = postures_bot->size();
postures.resize(numberOfPostures);
Postures post;
post.reset(actuatedDOFs);
for(int i=0; i < numberOfPostures; i++ )
{
if( !(postures_bot->get(i).isList()) ||
!(postures_bot->get(i).asList()->size() == actuatedDOFs + 1) )
{
std::cerr << "[ERR] The list " << i << " of postures is of wrong dimension. Recall that if you are controlling NDOFs, the variable postures must contain NDOFs+1 element "
<< std::endl;
return false;
}
yarp::os::Bottle * postures_bot_i = postures_bot->get(i).asList();
post.time = postures_bot_i->get(0).asDouble();
if (post.time <= 0)
{
std::cerr << "[ERR] Time " << i << " of postures is negative" << std::endl;
return false;
}
if (i > 0)
{
if (postures_bot_i->get(0).asDouble() <= postures[i-1].time )
{
std::cerr << "[ERR] Time series of postures is not strictly increasing" << std::endl;
return false;
}
}
for (int j=0; j < actuatedDOFs; j++)
{
post.qDes[j] = conversion*(postures_bot_i->get(j+1).asDouble());
if (posturesInRadians)
{
if ( (post.qDes[j] > 2*M_PI) || (post.qDes[j] < -2*M_PI) )
{
std::cerr << "[ERR] postures are expected to be in radians, but they exceed the limits [-2*pi,2*pi]"<< std::endl;
return false;
}
}
}
postures[i] = post;
}
}
if(group_bot.check("directionOfOscillation") || group_bot.check("amplitudeInMeters") || group_bot.check("frequencyInHerz"))
{
if( !(group_bot.check("directionOfOscillation")) ||
!(group_bot.check("amplitudeInMeters")) ||
!(group_bot.check("frequencyInHerz")))
{
std::cerr << "[ERR] directionOfOscillation or amplitudeInMeters or frequencyInHerz is missing" << std::endl;
return false;
}
if (group_bot.check("comTimeAndSetPoints"))
{
std::cerr << "[ERR] It is impossible to stream both sinusoidal references and set points for the center of mass." << std::endl;
return false;
}
if( !(group_bot.find("directionOfOscillation").isList()) ||
!(group_bot.find("directionOfOscillation").asList()->size() == 3))
{
std::cerr << "[ERR] directionOfOscillation is malformed" << std::endl;
return false;
}
//Check coupling configuration is well formed
double normOfDirectionOfOscillation = 0;
for(int i=0; i < 3; i++ )
{
directionOfOscillation[i] = group_bot.find("directionOfOscillation").asList()->get(i).asDouble();
normOfDirectionOfOscillation += directionOfOscillation[i]*directionOfOscillation[i];
}
if( (normOfDirectionOfOscillation < 0.99) || (normOfDirectionOfOscillation > 1.01))
{
std::cerr << "[ERR] directionOfOscillation is not of unit norm" << std::endl;
return false;
}
amplitudeOfOscillation = group_bot.check("amplitudeInMeters", yarp::os::Value(0), "Looking for amplitudeOfOscillation").asDouble();
frequencyOfOscillation = group_bot.check("frequencyInHerz", yarp::os::Value(0), "Looking for frequencyOfOscillation").asDouble();
changeComWithSetPoints = false;
}
if (group_bot.check("comTimeAndSetPoints"))
{
changeComWithSetPoints = true;
if( !(group_bot.find("comTimeAndSetPoints").isList()) )
{
std::cerr << "[ERR] comTimeAndSetPoints is not a list of lists, each of which must be a four element vector, where the first is the time at which the ith comDes must be streamed"
<< std::endl;
return false;
}
yarp::os::Bottle * com_bot = group_bot.find("comTimeAndSetPoints").asList();
int numberOfCom = com_bot->size();
comTimeAndSetPoints.resize(numberOfCom);
ComTimeAndSetPoints comAndTime;
comAndTime.reset();
for(int i=0; i < numberOfCom; i++ )
{
if( !(com_bot->get(i).isList()) ||
!(com_bot->get(i).asList()->size() == 4) )
{
std::cerr << "[ERR] The list "<< i << " of comTimeAndSetPoints is of wrong dimension. It must be a four element vector, where the first is the time at which the ith comDes must be streamed"
<< std::endl;
return false;
}
yarp::os::Bottle * com_bot_i = com_bot->get(i).asList();
comAndTime.time = com_bot_i->get(0).asDouble();
if (comAndTime.time <= 0)
{
std::cerr << "[ERR] Time " << i << " of comTimeAndSetPoints is negative" << std::endl;
return false;
}
if (i > 0)
{
if (com_bot_i->get(0).asDouble() <= comTimeAndSetPoints[i-1].time )
{
std::cerr << "[ERR] Time series of comTimeAndSetPoints is not strictly increasing" << std::endl;
return false;
}
}
for (int j=0; j < 3; j++)
{
comAndTime.comDes[j] = com_bot_i->get(j+1).asDouble();
}
comTimeAndSetPoints[i] = comAndTime;
}
}
return true;
}
}
bool comanFTsensor::fromConfig(yarp::os::Searchable &_config)
{
Bottle xtmp;
if(_config.check("device") )
{
setDeviceId(_config.find("device").asString() );
}
else
{
printf("No device name found.");
std::cout << "Params are: " << _config.toString();
}
// Analog Sensor stuff
Bottle config = _config.findGroup("ANALOG");
if (!validate(config, xtmp, "numberOfSensors","Number of FT boards", 1))
{
yError() << "comanFTsensor: Boards number not found or not valid, quitting\n";
_channels = 0;
numberOfBoards = 0;
return false;
}
else
{ // get starts from 1 because the parameter name is counted as index 0
numberOfBoards = xtmp.get(1).asInt();
}
FTmap = new int[numberOfBoards];
scaleFactor=new double[numberOfBoards];
std::cout << "\n\n num of boards is " << numberOfBoards << "\n\n";
if (!validate(config, xtmp, "newtonsToSensor","Scaling factor for FT sensors", numberOfBoards))
{
yError() << "FTSensor " << getDeviceId() << ": newtonsToSensor param was not found";
return false;
}
else
{
for (int i=0; i<numberOfBoards; i++)
scaleFactor[i]=xtmp.get(1+i).asDouble();
}
if (!validate(config, xtmp, "FTmap","mapping of FTsensor unique Ids and a sequence number", numberOfBoards))
{
yWarning() << "comanFTsensor: FT sensor map not found or not correct, check your config file. Quitting";
for(int i=0; i < numberOfBoards; i++)
{
FTmap[i] = 0;
std::cout << "Ftmap[" << i << "] is " << FTmap[i];
}
return false;
}
else
{ // get starts from 1 because the parameter name is counted as index 0
for(int i=0; i<numberOfBoards; i++)
{
FTmap[i] = xtmp.get(1+i).asInt();
std::cout << "Ftmap[" << i << "] is " << FTmap[i];
}
}
if (!validate(config, xtmp, "channels","number of channels per FT sensor", 1))
{
yWarning() << "comanFTsensor: Using default value = 6 (3 forces plus 3 torques)";
_channels = 6;
}
else
{ // get starts from 1 because the parameter name is counted as index 0
_channels = xtmp.get(1).asInt();
}
if (!validate(config, xtmp, "UseCalibration","Calibration parameters if needed", 1))
{
yWarning() << "comanFTsensor: Using default value = 0 (Don't use calibration)";
_useCalibration = 0;
}
else
{ // get starts from 1 because the parameter name is counted as index 0
_useCalibration = xtmp.get(1).asInt();
}
return true;
};
bool CanBusVirtualAnalogSensor::open(yarp::os::Searchable& config)
{
bool correct=true;
//debug
fprintf(stderr, "%s\n", config.toString().c_str());
correct &= config.check("canbusDevice");
correct &= config.check("canDeviceNum");
correct &= config.check("canAddress");
correct &= config.check("format");
correct &= config.check("period");
correct &= config.check("channels");
correct &= config.check("fullScale");
if (!correct)
{
std::cerr<<"Error: insufficient parameters to CanBusVirtualAnalogSensor\n";
return false;
}
int period=config.find("period").asInt();
setRate(period);
Property prop;
prop.put("device", config.find("canbusDevice").asString().c_str());
prop.put("physDevice", config.find("physDevice").asString().c_str());
prop.put("canTxTimeout", 500);
prop.put("canRxTimeout", 500);
prop.put("canDeviceNum", config.find("canDeviceNum").asInt());
prop.put("canMyAddress", 0);
prop.put("canTxQueueSize", CAN_DRIVER_BUFFER_SIZE);
prop.put("canRxQueueSize", CAN_DRIVER_BUFFER_SIZE);
pCanBus=0;
pCanBufferFactory=0;
//open the can driver
driver.open(prop);
if (!driver.isValid())
{
fprintf(stderr, "Error opening PolyDriver check parameters\n");
return false;
}
driver.view(pCanBus);
if (!pCanBus)
{
fprintf(stderr, "Error opening can device not available\n");
return false;
}
driver.view(pCanBufferFactory);
outBuffer=pCanBufferFactory->createBuffer(CAN_DRIVER_BUFFER_SIZE);
inBuffer=pCanBufferFactory->createBuffer(CAN_DRIVER_BUFFER_SIZE);
//select the communication speed
pCanBus->canSetBaudRate(0); //default 1MB/s
//set the internal configuration
//this->isVirtualSensor = false;
this->boardId = config.find("canAddress").asInt();
this->channelsNum = config.find("channels").asInt();
this->useCalibration = (config.find("useCalibration").asInt()==1);
unsigned int tmpFormat = config.find("format").asInt();
if (tmpFormat == 8)
this->dataFormat = ANALOG_FORMAT_8_BIT;
else if (tmpFormat == 16)
this->dataFormat = ANALOG_FORMAT_16_BIT;
else
this->dataFormat = ANALOG_FORMAT_ERR;
//open the can mask for the specific canDeviceId
for (int id=0; id<16; ++id)
{
pCanBus->canIdAdd(0x300+(boardId<<4)+id);
}
pCanBus->canIdAdd(0x200+boardId);
pCanBus->canIdAdd(0x200+(boardId<<4));
//create the data vector:
int chan=config.find("channels").asInt();
data.resize(channelsNum);
Bottle fullScaleTmp = config.findGroup("fullScale");
this->scaleFactor.resize(channelsNum);
for (unsigned int i=0; i<channelsNum; i++)
{
double tmp = fullScaleTmp.get(i+1).asDouble();
this->scaleFactor[i] = tmp;
}
//start the sensor broadcast
sensor_start(config);
RateThread::start();
return true;
}
bool Rangefinder2DWrapper::checkROSParams(yarp::os::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;
}
else
{
yInfo() << "ROS group was FOUND in config file.";
Bottle &rosGroup = config.findGroup("ROS");
if (rosGroup.isNull())
{
yError() << partName << "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() << partName << " 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() << partName << "useROS topic if set to 'false'";
useROS = ROS_disabled;
return true;
}
else if (ros_use_type == "true")
{
yInfo() << partName << "useROS topic if set to 'true'";
useROS = ROS_enabled;
}
else if (ros_use_type == "only")
{
yInfo() << partName << "useROS topic if set to 'only";
useROS = ROS_only;
}
else
{
yInfo() << partName << "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() << partName << " 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() << partName << "rosNodeName is " << rosNodeName;
// check for ROS_topicName parameter
if (!rosGroup.check("ROS_topicName"))
{
yError() << partName << " cannot find rosTopicName parameter, mandatory when using ROS message";
useROS = ROS_config_error;
return false;
}
rosTopicName = rosGroup.find("ROS_topicName").asString();
yInfo() << partName << "rosTopicName is " << rosTopicName;
// check for frame_id parameter
if (!rosGroup.check("frame_id"))
{
yError() << partName << " cannot find rosTopicName parameter, mandatory when using ROS message";
useROS = ROS_config_error;
return false;
}
frame_id = rosGroup.find("frame_id").asString();
yInfo() << partName << "frame_id is " << frame_id;
return true;
}
yError() << partName << "should never get here!" << __LINE__;
return false; // should never get here
}
bool embObjVirtualAnalogSensor::fromConfig(yarp::os::Searchable &_config)
{
Bottle xtmp;
// embObj parameters, in ETH group
Value val =_config.findGroup("ETH").check("Ems",Value(1), "Board number");
if(val.isInt())
{
_fId.boardNum =val.asInt();
}
else
{
yError () << "embObjVirtualAnalogSensor: EMS Board number identifier not found\n";
return false;
}
// Analog Sensor stuff
Bottle config = _config.findGroup("GENERAL");
if (!extractGroup(config, xtmp, "Channels","Number of channels of the Analog Sensor", 1))
{
yError() << "embObjVirtualAnalogSensor: Missing channel number... aborting";
_channels = 0;
return false;
}
else
{
_channels = xtmp.get(1).asInt();
}
// alloc vector for keeping conversion values
_fullscale = allocAndCheck<double>(_channels);
_resolution = allocAndCheck<double>(_channels);
if (!extractGroup(config, xtmp, "UseCalibration","Calibration parameters are needed", 1))
{
fprintf(stderr, "embObjVirtualAnalogSensor: Using default value = 0 (Don't use calibration)\n");
_useCalibration = 0;
}
else
{
_useCalibration = xtmp.get(1).asInt();
}
if (!extractGroup(config, xtmp, "MaxValue","full scale value for this measure", _channels))
{
yError() << "Missing conversion factor!! Aborting...";
return false;
}
else
{
if(_verbose)
yDebug() << "embObjVirtualAnalogSensor, fullscales from config file: ";
for (int ch = 1; ch < xtmp.size(); ch++)
{
_fullscale[ch-1] = xtmp.get(ch).asDouble();
if(_verbose)
yDebug() << "ch " << ch << ": " << _fullscale[ch-1];
}
}
if (!extractGroup(config, xtmp, "Resolution","Number of bytes used for this measure", _channels))
{
yError() << "Missing resolution!! Aborting...";
return false;
}
else
{
if(_verbose)
yDebug() << "embObjVirtualAnalogSensor, resolutions from config file: ";
for (int ch = 1; ch < xtmp.size(); ch++)
{
_resolution[ch-1] = (double) (1 << (xtmp.get(ch).asInt()-1) );
if(_verbose)
yDebug() << "ch " << ch << ": " << _resolution[ch-1];
}
}
return true;
};
bool LaserFromDepth::open(yarp::os::Searchable& config)
{
Property subConfig;
m_info = "LaserFromDepth device";
m_device_status = DEVICE_OK_STANBY;
#ifdef LASER_DEBUG
yDebug("%s\n", config.toString().c_str());
#endif
m_min_distance = 0.1; //m
m_max_distance = 2.5; //m
bool br = config.check("SUBDEVICE");
if (br != false)
{
yarp::os::Searchable& general_config = config.findGroup("SUBDEVICE");
m_clip_max_enable = general_config.check("clip_max");
m_clip_min_enable = general_config.check("clip_min");
if (m_clip_max_enable) { m_max_distance = general_config.find("clip_max").asDouble(); }
if (m_clip_min_enable) { m_min_distance = general_config.find("clip_min").asDouble(); }
m_do_not_clip_infinity_enable = (general_config.find("allow_infinity").asInt()!=0);
}
else
{
yError() << "Missing SUBDEVICE section";
return false;
}
bool bs = config.check("SKIP");
if (bs != false)
{
yarp::os::Searchable& skip_config = config.findGroup("SKIP");
Bottle mins = skip_config.findGroup("min");
Bottle maxs = skip_config.findGroup("max");
size_t s_mins = mins.size();
size_t s_maxs = mins.size();
if (s_mins == s_maxs && s_maxs > 1 )
{
for (size_t s = 1; s < s_maxs; s++)
{
Range_t range;
range.max = maxs.get(s).asDouble();
range.min = mins.get(s).asDouble();
if (range.max >= 0 && range.max <= 360 &&
range.min >= 0 && range.min <= 360 &&
range.max > range.min)
{
m_range_skip_vector.push_back(range);
}
else
{
yError() << "Invalid range in SKIP section";
return false;
}
}
}
}
Time::turboBoost();
Property prop;
if(!config.check("RGBD_SENSOR_CLIENT"))
{
yError() << "missing RGBD_SENSOR_CLIENT section in configuration file!";
return false;
}
prop.fromString(config.findGroup("RGBD_SENSOR_CLIENT").toString());
prop.put("device", "RGBDSensorClient");
driver.open(prop);
if (!driver.isValid())
{
yError("Error opening PolyDriver check parameters");
return false;
}
driver.view(iRGBD);
if (!iRGBD)
{
yError("Error opening iRGBD interface. Device not available");
return false;
}
m_depth_width = iRGBD->getDepthWidth();
m_depth_height = iRGBD->getDepthHeight();
double hfov = 0;
double vfov = 0;
iRGBD->getDepthFOV(hfov, vfov);
m_sensorsNum = m_depth_width;
m_resolution = hfov / m_depth_width;
m_laser_data.resize(m_sensorsNum, 0.0);
m_max_angle = +hfov / 2;
m_min_angle = -hfov / 2;
RateThread::start();
yInfo("Sensor ready");
return true;
}