bool RGBDSensorWrapper::openAndAttachSubDevice(Searchable& prop)
{
Property p;
subDeviceOwned = new PolyDriver;
p.fromString(prop.toString().c_str());
p.setMonitor(prop.getMonitor(), "subdevice"); // pass on any monitoring
p.unput("device");
p.put("device",prop.find("subdevice").asString()); // subdevice was already checked before
// if error occour during open, quit here.
yDebug("opening IRGBDSensor subdevice\n");
subDeviceOwned->open(p);
if (!subDeviceOwned->isValid())
{
yError("opening controlBoardWrapper2 subdevice... FAILED\n");
return false;
}
isSubdeviceOwned = true;
if(!attach(subDeviceOwned))
return false;
RateThread::setRate(rate);
RateThread::start();
return true;
}
bool realsense2Driver::open(Searchable& config)
{
std::vector<RGBDSensorParamParser::RGBDParam*> params;
for (auto& p:params_map)
{
params.push_back(&(p.second));
}
m_verbose = config.check("verbose");
if (config.check("stereoMode")) {
m_stereoMode = config.find("stereoMode").asBool();
}
if (!m_paramParser->parseParam(config, params))
{
yError()<<"realsense2Driver: failed to parse the parameters";
return false;
}
if (!initializeRealsenseDevice())
{
yError()<<"realsense2Driver: failed to initialize the realsense device";
return false;
}
// setting Parameters
if (!setParams())
{
return false;
}
return true;
}
bool JoypadControlServer::openAndAttachSubDevice(Searchable& prop)
{
Property p;
m_subDeviceOwned = new PolyDriver;
p.fromString(prop.toString().c_str());
p.setMonitor(prop.getMonitor(), "subdevice"); // pass on any monitoring
p.unput("device");
p.put("device",prop.find("subdevice").asString()); // subdevice was already checked before
// if error occour during open, quit here.
m_subDeviceOwned->open(p);
if (!m_subDeviceOwned->isValid())
{
yError("JoypadControlServer: opening subdevice... FAILED\n");
return false;
}
m_isSubdeviceOwned = true;
if(!attach(m_subDeviceOwned))
return false;
if(!m_parser.configure(m_device) )
{
yError() << "JoypadControlServer: error configuring interfaces for parsers";
return false;
}
openPorts();
PeriodicThread::setPeriod(m_period);
return PeriodicThread::start();
}
bool HapticDeviceWrapper::open(Searchable &config)
{
portStemName=config.check("name",
Value(HAPTICDEVICE_WRAPPER_DEFAULT_NAME)).asString().c_str();
verbosity=config.check("verbosity",Value(0)).asInt();
int period=config.check("period",
Value(HAPTICDEVICE_WRAPPER_DEFAULT_PERIOD)).asInt();
setRate(period);
if (config.check("subdevice"))
{
Property p(config.toString().c_str());
p.setMonitor(config.getMonitor(),"subdevice");
p.unput("device");
p.put("device",config.find("subdevice").asString());
if (driver.open(p))
{
IHapticDevice *d;
driver.view(d);
attach(d);
}
else
{
yError("*** Haptic Device Wrapper: failed to open the driver!");
return false;
}
}
if (verbosity>0)
yInfo("*** Haptic Device Wrapper: opened");
return true;
}
Value ReachManager::GetValueFromConfig(Searchable& config, string valueName)
{
if(!config.check(valueName.c_str()))
{
cout << "ERROR with config file : couldn't find value : \"" << valueName << "\"." << endl;
return false;
}
return config.find(valueName.c_str());
}
bool HapticDeviceClient::open(Searchable &config)
{
if (!config.check("remote"))
{
yError("*** Haptic Device Client: \"remote\" option missing, failed to open!");
return false;
}
if (!config.check("local"))
{
yError("*** Haptic Device Client: \"local\" option missing, failed to open!");
return false;
}
string remote=config.find("remote").asString().c_str();
string local=config.find("local").asString().c_str();
verbosity=config.check("verbosity",Value(0)).asInt();
statePort.open((local+"/state:i").c_str());
feedbackPort.open((local+"/feedback:o").c_str());
rpcPort.open((local+"/rpc").c_str());
statePort.setClient(this);
bool ok=true;
ok&=Network::connect((remote+"/state:o").c_str(),statePort.getName().c_str(),"udp");
ok&=Network::connect(feedbackPort.getName().c_str(),(remote+"/feedback:i").c_str(),"tcp");
ok&=Network::connect(rpcPort.getName().c_str(),(remote+"/rpc").c_str(),"tcp");
if (!ok)
{
statePort.close();
feedbackPort.close();
rpcPort.close();
yError("*** Haptic Device Client: unable to connect to Haptic Device Wrapper, failed to open!");
return false;
}
if (verbosity>0)
yInfo("*** Haptic Device Client: opened");
return true;
}
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;
}
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 RGBDSensorWrapper::openAndAttachSubDevice(Searchable& prop)
{
Property p;
subDeviceOwned = new PolyDriver;
p.fromString(prop.toString().c_str());
p.setMonitor(prop.getMonitor(), "subdevice"); // pass on any monitoring
p.unput("device");
p.put("device",prop.find("subdevice").asString()); // subdevice was already checked before
// if error occour during open, quit here.
yDebug("opening IRGBDSensor subdevice\n");
subDeviceOwned->open(p);
if (!subDeviceOwned->isValid())
{
yError("opening controlBoardWrapper2 subdevice... FAILED\n");
return false;
}
isSubdeviceOwned = true;
if(!attach(subDeviceOwned))
return false;
// Configuring parsers
IRgbVisualParams * rgbVis_p;
IDepthVisualParams * depthVis_p;
subDeviceOwned->view(rgbVis_p);
subDeviceOwned->view(depthVis_p);
if(!parser.configure(sensor_p) )
{
yError() << "RGBD wrapper: error configuring interfaces for parsers";
return false;
}
/*
bool conf = rgbParser.configure(rgbVis_p);
conf &= depthParser.configure(depthVis_p);
if(!conf)
{
yError() << "RGBD wrapper: error configuring interfaces for parsers";
return false;
}
*/
RateThread::setRate(rate);
RateThread::start();
return true;
}
bool AnalogWrapper::openAndAttachSubDevice(Searchable &prop)
{
Property p;
subDeviceOwned = new PolyDriver;
p.fromString(prop.toString().c_str());
// p.setMonitor(prop.getMonitor(), "subdevice"); // pass on any monitoring
p.unput("device");
p.put("device", prop.find("subdevice").asString()); // subdevice was already checked before
// if error occour during open, quit here.
yDebug("opening analogServer subdevice...");
subDeviceOwned->open(p);
if (!subDeviceOwned->isValid())
{
yError("opening analogServer subdevice... FAILED\n");
return false;
}
subDeviceOwned->view(analogSensor_p);
if (analogSensor_p == 0)
{
yError("Opening IAnalogSensor interface of analogServer subdevice... FAILED\n");
return false;
}
int chNum = analogSensor_p->getChannels();
if (chNum <= 0)
{
yError("Calling analog sensor has invalid channels number %d.\n", chNum);
return false;
}
attach(analogSensor_p);
RateThread::setRate(_rate);
RateThread::start();
return true;
}
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;
}
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;
}
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 ClientCartesianController::open(Searchable &config)
{
ConstString remote, local, carrier;
if (config.check("remote"))
remote=config.find("remote").asString();
else
return false;
if (config.check("local"))
local=config.find("local").asString();
else
return false;
carrier=config.check("carrier",Value("udp")).asString();
if (config.check("timeout"))
timeout=config.find("timeout").asDouble();
portCmd.open((local+"/command:o").c_str());
portState.open((local+"/state:i").c_str());
portEvents.open((local+"/events:i").c_str());
portRpc.open((local+"/rpc:o").c_str());
bool ok=true;
ok&=Network::connect(portRpc.getName().c_str(),(remote+"/rpc:i").c_str());
if (ok)
{
Bottle info;
getInfoHelper(info);
if (info.check("server_version"))
{
double server_version=info.find("server_version").asDouble();
if (server_version!=CARTCTRL_CLIENT_VER)
{
yError("version mismatch => server(%g) != client(%g); please update accordingly",
server_version,CARTCTRL_CLIENT_VER);
return false;
}
}
else
yWarning("unable to retrieve server version; please update the server");
}
else
{
yError("unable to connect to the server rpc port!");
return false;
}
ok&=Network::connect(portCmd.getName().c_str(),(remote+"/command:i").c_str(),carrier.c_str());
ok&=Network::connect((remote+"/state:o").c_str(),portState.getName().c_str(),carrier.c_str());
ok&=Network::connect((remote+"/events:o").c_str(),portEvents.getName().c_str(),carrier.c_str());
// check whether the solver is alive and connected
if (ok)
{
Bottle command, reply;
command.addVocab(IKINCARTCTRL_VOCAB_CMD_GET);
command.addVocab(IKINCARTCTRL_VOCAB_OPT_ISSOLVERON);
if (!portRpc.write(command,reply))
{
yError("unable to get reply from server!");
close();
return false;
}
if (reply.get(0).asVocab()==IKINCARTCTRL_VOCAB_REP_ACK)
if (reply.size()>1)
if (reply.get(1).asVocab()==IKINCARTCTRL_VOCAB_VAL_TRUE)
return connected=true;
yError("unable to connect to solver!");
close();
return false;
}
else
{
yError("unable to connect to server!");
close();
return false;
}
}
bool PinholeCalibTool::configure (Searchable &config){
_calibImgSize.width = config.check("w",
Value(320),
"Image width for which calibration parameters were calculated (int)").asInt();
_calibImgSize.height = config.check("h",
Value(240),
"Image height for which calibration parameters were calculated (int)").asInt();
_drawCenterCross = config.check("drawCenterCross",
Value(0),
"Draw a cross at calibration center (int [0|1]).").asInt()!=0;
CV_MAT_ELEM( *_intrinsic_matrix , float, 0, 0) = (float)config.check("fx",
Value(320.0),
"Focal length x (double)").asDouble();
CV_MAT_ELEM( *_intrinsic_matrix, float, 0, 1) = 0.0f;
CV_MAT_ELEM( *_intrinsic_matrix, float, 0, 2) = (float)config.check("cx",
Value(160.0),
"Principal point x (double)").asDouble();
CV_MAT_ELEM( *_intrinsic_matrix, float, 1, 0) = 0.0f;
CV_MAT_ELEM( *_intrinsic_matrix, float, 1, 1) = (float)config.check("fy",
Value(320.0),
"Focal length y (double)").asDouble();
CV_MAT_ELEM( *_intrinsic_matrix, float, 1, 2) = (float)config.check("cy",
Value(120.0),
"Principal point y (double)").asDouble();
CV_MAT_ELEM( *_intrinsic_matrix, float, 2, 0) = 0.0f;
CV_MAT_ELEM( *_intrinsic_matrix, float, 2, 1) = 0.0f;
CV_MAT_ELEM( *_intrinsic_matrix, float, 2, 2) = 1.0f;
//check to see if the value is read correctly without caring about the default values.
if ( !config.check("drawCenterCross") ) { stopConfig("drawCenterCross"); return false; }
if ( !config.check("w") ) { stopConfig("w"); return false;}
if ( !config.check("h") ) { stopConfig("h"); return false;}
if ( !config.check("fx") ) { stopConfig("fx"); return false;}
if ( !config.check("fy") ) { stopConfig("fy"); return false;}
if ( !config.check("cx") ) { stopConfig("cx"); return false;}
if ( !config.check("cy") ) { stopConfig("cy"); return false;}
if ( !config.check("k1") ) { stopConfig("k1"); return false;}
if ( !config.check("k2") ) { stopConfig("k2"); return false;}
if ( !config.check("p1") ) { stopConfig("p1"); return false;}
if ( !config.check("p2") ) { stopConfig("p2"); return false;}
fprintf(stdout,"fx=%g\n",config.find("fx").asDouble());
fprintf(stdout,"fy=%g\n",config.find("fy").asDouble());
fprintf(stdout,"cx=%g\n",config.find("cx").asDouble());
fprintf(stdout,"cy=%g\n",config.find("cy").asDouble());
// copy to scaled matrix ;)
CV_MAT_ELEM( *_intrinsic_matrix_scaled , float, 0, 0) = CV_MAT_ELEM( *_intrinsic_matrix , float, 0, 0);
CV_MAT_ELEM( *_intrinsic_matrix_scaled , float, 0, 1) = CV_MAT_ELEM( *_intrinsic_matrix , float, 0, 1);
CV_MAT_ELEM( *_intrinsic_matrix_scaled , float, 0, 2) = CV_MAT_ELEM( *_intrinsic_matrix , float, 0, 2);
CV_MAT_ELEM( *_intrinsic_matrix_scaled , float, 1, 0) = CV_MAT_ELEM( *_intrinsic_matrix , float, 1, 0);
CV_MAT_ELEM( *_intrinsic_matrix_scaled , float, 1, 1) = CV_MAT_ELEM( *_intrinsic_matrix , float, 1, 1);
CV_MAT_ELEM( *_intrinsic_matrix_scaled , float, 1, 2) = CV_MAT_ELEM( *_intrinsic_matrix , float, 1, 2);
CV_MAT_ELEM( *_intrinsic_matrix_scaled , float, 2, 0) = CV_MAT_ELEM( *_intrinsic_matrix , float, 2, 0);
CV_MAT_ELEM( *_intrinsic_matrix_scaled , float, 2, 1) = CV_MAT_ELEM( *_intrinsic_matrix , float, 2, 1);
CV_MAT_ELEM( *_intrinsic_matrix_scaled , float, 2, 2) = CV_MAT_ELEM( *_intrinsic_matrix , float, 2, 2);
/* init the distortion coeffs */
CV_MAT_ELEM( *_distortion_coeffs, float, 0, 0) = (float)config.check("k1",
Value(0.0),
"Radial distortion 1(double)").asDouble();
CV_MAT_ELEM( *_distortion_coeffs, float, 0, 1) = (float)config.check("k2",
Value(0.0),
"Radial distortion 2(double)").asDouble();
CV_MAT_ELEM( *_distortion_coeffs, float, 0, 2) = (float)config.check("p1",
Value(0.0),
"Tangential distortion 1(double)").asDouble();
CV_MAT_ELEM( *_distortion_coeffs, float, 0, 3) = (float)config.check("p2",
Value(0.0),
"Tangential distortion 2(double)").asDouble();
_needInit = true;
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());
}
// expand any environment variables found
ConstString expand(const char *txt, Searchable& env, Searchable& env2) {
//printf("expanding %s\n", txt);
String input = txt;
if (input.find("$")==String::npos) {
// no variables present for sure
return txt;
}
// check if variables present
String output = "";
String var = "";
bool inVar = false;
bool varHasParen = false;
bool quoted = false;
for (int i=0; i<=(int)input.length(); i++) {
char ch = 0;
if (i<(int)input.length()) {
ch = input[i];
}
if (quoted) {
if (!inVar) {
output += '\\';
if (ch!=0) {
output += ch;
}
} else {
if (ch!=0) {
var += ch;
}
}
quoted = false;
continue;
} else {
if (ch=='\\') {
quoted = true;
continue;
}
}
if (inVar) {
if (ACE_OS::ace_isalnum(ch)||(ch=='_')) {
var += ch;
continue;
} else {
if (ch=='('||ch=='{') {
if (var.length()==0) {
// ok, just ignore
varHasParen = true;
continue;
}
}
inVar = false;
//printf("VARIABLE %s\n", var.c_str());
String add = NetworkBase::getEnvironment(var.c_str()).c_str();
if (add=="") {
add = env.find(var.c_str()).toString().c_str();
}
if (add=="") {
add = env2.find(var.c_str()).toString().c_str();
}
if (add=="") {
if (var=="__YARP__") {
add = "1";
}
}
if (add.find("\\")!=String::npos) {
// Specifically when reading from the command
// line, we will allow windows-style paths.
// Hence we have to break the "\" character
String buf = "";
for (unsigned int i=0; i<add.length(); i++) {
buf += add[i];
if (add[i]=='\\') {
buf += add[i];
}
}
add = buf;
}
output += add;
var = "";
if (varHasParen && (ch=='}'||ch==')')) {
continue;
// don't need current char
}
}
}
if (!inVar) {
if (ch=='$') {
inVar = true;
varHasParen = false;
continue;
} else {
if (ch!=0) {
output += ch;
}
}
}
}
return output.c_str();
}
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 teo::FakeControlboard::open(Searchable& config) {
axes = config.check("axes",DEFAULT_AXES,"number of axes to control").asInt();
double genInitPos = config.check("genInitPos",DEFAULT_GEN_INIT_POS,"general initialization positions").asDouble();
double genJointTol = config.check("genJointTol",DEFAULT_GEN_JOINT_TOL,"general joint tolerances").asDouble();
double genMaxLimit = config.check("genMaxLimit",DEFAULT_GEN_MAX_LIMIT,"general max limits").asDouble();
double genMinLimit = config.check("genMinLimit",DEFAULT_GEN_MIN_LIMIT,"general min limits").asDouble();
double genRefSpeed = config.check("genRefSpeed",DEFAULT_GEN_REF_SPEED,"general ref speed").asDouble();
double genEncRawExposed = config.check("genEncRawExposed",DEFAULT_GEN_ENC_RAW_EXPOSED,"general EncRawExposed").asDouble();
double genVelRawExposed = config.check("genVelRawExposed",DEFAULT_GEN_VEL_RAW_EXPOSED,"general VelRawExposed").asDouble();
modePosVel = config.check("modePosVel",DEFAULT_MODE_POS_VEL,"0:pos, 1:vel").asInt();
Bottle* initPoss;
if (config.check("initPoss")) {
initPoss = config.find("initPoss").asList();
printf("FakeControlboard using individual initPoss: %s\n",initPoss->toString().c_str());
if(initPoss->size() != axes) printf("[warning] initPoss->size() != axes\n");
} else {
initPoss = 0;
printf("FakeControlboard not using individual initPoss, defaulting to genInitPos.\n");
}
Bottle* jointTols;
if (config.check("jointTols")) {
jointTols = config.find("jointTols").asList();
printf("FakeControlboard using individual jointTols: %s\n",jointTols->toString().c_str());
if(jointTols->size() != axes) printf("[warning] jointTols->size() != axes\n");
} else {
jointTols = 0;
printf("FakeControlboard not using individual jointTols, defaulting to genJointTol.\n");
}
Bottle* maxLimits;
if (config.check("maxLimits")) {
maxLimits = config.find("maxLimits").asList();
printf("FakeControlboard using individual maxLimits: %s\n",maxLimits->toString().c_str());
if(maxLimits->size() != axes) printf("[warning] maxLimits->size() != axes\n");
} else {
maxLimits = 0;
printf("FakeControlboard not using individual maxLimits, defaulting to genMaxLimit.\n");
}
Bottle* minLimits;
if (config.check("minLimits")) {
minLimits = config.find("minLimits").asList();
printf("FakeControlboard using individual minLimits: %s\n",minLimits->toString().c_str());
if(minLimits->size() != axes) printf("[warning] minLimits->size() != axes\n");
} else {
minLimits = 0;
printf("FakeControlboard not using individual minLimits, defaulting to genMinLimit.\n");
}
Bottle* refSpeeds;
if (config.check("refSpeeds")) {
refSpeeds = config.find("refSpeeds").asList();
printf("FakeControlboard using individual refSpeeds: %s\n",refSpeeds->toString().c_str());
if(refSpeeds->size() != axes) printf("[warning] refSpeeds->size() != axes\n");
} else {
refSpeeds = 0;
printf("FakeControlboard not using individual refSpeeds, defaulting to genRefSpeed.\n");
}
Bottle* encRawExposeds;
if (config.check("encRawExposeds")) {
encRawExposeds = config.find("encRawExposeds").asList();
printf("FakeControlboard using individual encRawExposeds: %s\n",encRawExposeds->toString().c_str());
if(encRawExposeds->size() != axes) printf("[warning] encRawExposeds->size() != axes\n");
} else {
encRawExposeds = 0;
printf("FakeControlboard not using individual encRawExposeds, defaulting to genEncRawExposed.\n");
}
Bottle* velRawExposeds;
if (config.check("velRawExposeds")) {
velRawExposeds = config.find("velRawExposeds").asList();
printf("FakeControlboard using individual velRawExposeds: %s\n",velRawExposeds->toString().c_str());
if(velRawExposeds->size() != axes) printf("[warning] velRawExposeds->size() != axes\n");
} else {
velRawExposeds = 0;
printf("FakeControlboard not using individual velRawExposeds, defaulting to genVelRawExposed.\n");
}
encRawExposed.resize(axes);
jointStatus.resize(axes);
initPos.resize(axes);
jointTol.resize(axes);
maxLimit.resize(axes);
minLimit.resize(axes);
refSpeed.resize(axes);
velRawExposed.resize(axes);
for (unsigned int i=0; i<axes; i++) {
jointStatus[i]=0;
if(!refSpeeds) refSpeed[i]=genRefSpeed;
else refSpeed[i]=refSpeeds->get(i).asDouble();
if(!minLimits) minLimit[i]=genMinLimit;
else minLimit[i]=minLimits->get(i).asDouble();
if(!maxLimits) maxLimit[i]=genMaxLimit;
else maxLimit[i]=maxLimits->get(i).asDouble();
if(!initPoss) initPos[i]=genInitPos;
else initPos[i]=initPoss->get(i).asDouble();
if(!jointTols) jointTol[i]=genJointTol;
else jointTol[i]=jointTols->get(i).asDouble();
if(!encRawExposeds) encRawExposed[i]=genEncRawExposed;
else encRawExposed[i]=encRawExposeds->get(i).asDouble();
if(!velRawExposeds) velRawExposed[i]=genVelRawExposed;
else velRawExposed[i]=velRawExposeds->get(i).asDouble();
}
encRaw.resize(axes, 0.0);
refAcc.resize(axes, 1.0);
targetExposed.resize(axes, 0.0);
velRaw.resize(axes, 0.0);
for (unsigned int i=0; i<axes; i++) {
setEncoder(i,initPos[i]);
}
// Start the RateThread
this->setRate(jmcMs);
this->start();
return true;
}
