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;
}
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 fakeMotorDeviceClient::open(Searchable &config)
{
printf("Opening Fake Motor Device Client ...\n");
string remote=config.check("remote",Value("/fakeyServer")).asString().c_str();
string local=config.check("local",Value("/fakeyClient")).asString().c_str();
statePort.open((local+"/state:i").c_str());
cmdPort.open((local+"/cmd:o").c_str());
rpcPort.open((local+"/rpc").c_str());
bool ok=true;
ok&=Network::connect((remote+"/state:o").c_str(),statePort.getName().c_str(),"udp");
ok&=Network::connect(cmdPort.getName().c_str(),(remote+"/cmd:i").c_str(),"udp");
ok&=Network::connect(rpcPort.getName().c_str(),(remote+"/rpc").c_str(),"tcp");
if (ok)
{
configured=true;
printf("Fake Motor Device Client successfully open\n");
return true;
}
else
{
statePort.close();
cmdPort.close();
rpcPort.close();
printf("Fake Motor Device Client failed to open\n");
return false;
}
}
bool ServerSerial::open(Searchable& prop)
{
verb = (prop.check("verbose",Value(0),"Specifies if the device is in verbose mode (0/1).").asInt())>0;
if (verb)
printf("running with verbose output\n");
Value *name;
if (prop.check("subdevice",name,"name of specific control device to wrap")) {
printf("Subdevice %s\n", name->toString().c_str());
if (name->isString()) {
// maybe user isn't doing nested configuration
Property p;
p.setMonitor(prop.getMonitor(),
"subdevice"); // pass on any monitoring
p.fromString(prop.toString());
p.put("device",name->toString());
poly.open(p);
} else {
Bottle subdevice = prop.findGroup("subdevice").tail();
poly.open(subdevice);
}
if (!poly.isValid()) {
printf("cannot make <%s>\n", name->toString().c_str());
}
} else {
printf("\"--subdevice <name>\" not set for server_serial\n");
return false;
}
if (!poly.isValid()) {
return false;
}
ConstString rootName =
prop.check("name",Value("/serial"),
"prefix for port names").asString().c_str();
command_buffer.attach(toDevice);
reply_buffer.attach(fromDevice);
command_buffer.useCallback(callback_impl);
toDevice.open((rootName+"/in").c_str());
fromDevice.open((rootName+"/out").c_str());
if (poly.isValid())
poly.view(serial);
if(serial != NULL) {
start();
return true;
}
printf("subdevice <%s> doesn't look like a serial port (no appropriate interfaces were acquired)\n",
name->toString().c_str());
return false;
}
bool open(Searchable& p) {
bool dev = true;
if (p.check("nodevice")) {
dev = false;
}
if (dev) {
poly.open(p);
if (!poly.isValid()) {
printf("cannot open driver\n");
return false;
}
if (!p.check("mute")) {
// Make sure we can write sound
poly.view(put);
if (put==NULL) {
printf("cannot open interface\n");
return false;
}
}
}
port.setStrict(true);
if (!port.open(p.check("name",Value("/yarphear")).asString())) {
printf("Communication problem\n");
return false;
}
if (p.check("remote")) {
Network::connect(p.check("remote",Value("/remote")).asString(),
port.getName());
}
return true;
}
Contact Contact::byConfig(Searchable& config) {
Contact result;
result.port = config.check("port_number",Value(-1)).asInt();
result.hostName = config.check("ip",Value("")).asString().c_str();
result.regName = config.check("name",Value("")).asString().c_str();
result.carrier = config.check("carrier",Value("tcp")).asString().c_str();
return result;
}
Contact Contact::fromConfig(const Searchable& config)
{
Contact result;
result.mPriv->port = config.check("port_number",Value(-1)).asInt();
result.mPriv->hostname = config.check("ip",Value("")).asString().c_str();
result.mPriv->regName = config.check("name",Value("")).asString().c_str();
result.mPriv->carrier = config.check("carrier",Value("tcp")).asString().c_str();
return result;
}
bool TextureInput::open(Searchable& config) {
textureIndex = config.check("textureIndex",Value(-1),
"texture index").asInt();
string texturePort = config.check("port",Value("/texture"),"local port name").asString();
string portStr = this->moduleName + texturePort.c_str();
port.open( portStr.c_str() );
return true;
}
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"));
}
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;
}
virtual bool open(Searchable& config)
{
int i;
char portname[10];
if (config.check("help","if present, display usage message")) {
printf("Call with --name </portprefix> --file <configfile.ini>\n");
return false;
}
cols = config.check("width", 640, "Number of image columns").asInt();
lines = config.check("height", 480, "Number of image lines").asInt();
levels = config.check("levels", 4, "Number of scales in the scale space").asInt();
scales = new double[levels];
Bottle &xtmp = config.findGroup("scales");
if(xtmp.size() != levels+1)
for (i = 0; i < levels; i++)
scales[i] = pow(2.0,i+1);
for (i = 1; i < xtmp.size(); i++)
scales[i-1] = xtmp.get(i).asDouble();
ss.AllocateResources(lines, cols, levels, scales);
infloat = cvCreateImage(cvSize(cols,lines), IPL_DEPTH_32F, 1);
ingray = cvCreateImage(cvSize(cols,lines), IPL_DEPTH_8U, 1);
outgray = cvCreateImage(cvSize(cols,lines), IPL_DEPTH_8U, 1);
outfloat = cvCreateImageHeader(cvSize(cols,lines), IPL_DEPTH_32F, 1);
portIn.open(getName("in"));
portOut = new BufferedPort<ImageOf<PixelMono> >[levels];
for( i = 1; i <= levels; i++ )
{
sprintf(portname, "out:%d", i);
portOut[i-1].open(getName(portname));
}
return true;
};
bool fakeMotorDeviceServer::open(Searchable &config)
{
printf("Opening Fake Motor Device Server ...\n");
string local=config.check("local",Value("/fakeyServer")).asString().c_str();
int Ts=config.check("Ts",Value(10)).asInt();
statePort.open((local+"/state:o").c_str());
cmdPort.open((local+"/cmd:i").c_str());
rpcPort.open((local+"/rpc").c_str());
rpcPort.setReader(*this);
// the part is composed of three rotational joints
// whose bounds are given in degrees just below
Matrix lim(3,2);
lim(0,0)=-180.0; lim(0,1)=180.0; // joint 0
lim(1,0)=-90.0; lim(1,1)=90.0; // joint 1
lim(2,0)=-45.0; lim(2,1)=45.0; // joint 2
Vector q0;
for (int i=0; i<lim.rows(); i++)
q0.push_back((lim(i,0)+lim(i,1))/2.0);
// the motors themselves are represented
// by pure integrators that give back joints
// positions when fed with joints velocities
motors=new Integrator(0.001*Ts,q0,lim);
vel.resize(motors->get().length(),0.0);
setRate(Ts);
start();
configured=true;
printf("Fake Motor Device Server successfully open\n");
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 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;
}
}
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);
}
virtual bool open(Searchable& config)
{
if (config.check("help","if present, display usage message")) {
printf("Call with --name </portprefix> --file <configfile.ini>\n");
return false;
}
// Defaults will correspond to a view field of 90 deg.
_input_lines = config.check("h", 480, "Input image lines").asInt();
_input_cols = config.check("w", 640, "Input image columns").asInt();
_fx = config.check("fx", 320, "Focal distance (on horizontal pixel size units)").asDouble();
_fy = config.check("fy", 240, "Focal distance (on vertical pixel size units)").asDouble();
_cx = config.check("cx", 320, "Image center (on horizontal pixel size units)").asDouble();
_cy = config.check("cy", 240, "Image center (on vertical pixel size units)").asDouble();
_k1 = config.check("k1", 0, "Radial distortion (first parameter)").asDouble();
_k2 = config.check("k2", 0, "Radial distortion (second parameter)").asDouble();
_p1 = config.check("p1", 0, "Tangential distortion (first parameter)").asDouble();
_p2 = config.check("p2", 0, "Tangential distortion (second parameter)").asDouble();
_output_lines = config.check("oh", 480, "Output image lines").asInt();
_output_cols = config.check("ow", 640, "Output image columns").asInt();
_mapx = cvCreateImage(cvSize(_output_cols, _output_lines), IPL_DEPTH_32F, 1);
_mapy = cvCreateImage(cvSize(_output_cols, _output_lines), IPL_DEPTH_32F, 1);
if(!compute_sp_map(_input_lines, _input_cols, _output_lines, _output_cols,
_fx, _fy, _cx, _cy, _k1, _k2, _p1, _p2,
(float*)_mapx->imageData, (float*)_mapy->imageData))
return false;
portImgIn.open(getName("in"));
portImgOut.open(getName("out"));
return true;
};
bool OpenCVGrabber::open(Searchable & config) {
m_saidSize = false;
m_saidResize = false;
m_transpose = false;
m_flip_x = false;
m_flip_y = false;
// Are we capturing from a file or a camera ?
std::string file = config.check("movie", Value(""),
"if present, read from specified file rather than camera").asString();
fromFile = (file!="");
if (fromFile) {
// Try to open a capture object for the file
m_cap.open(file.c_str());
if (!m_cap.isOpened()) {
yError("Unable to open file '%s' for capture!", file.c_str());
return false;
}
// Should we loop?
m_loop = config.check("loop","if present, loop movie");
} else {
m_loop = false;
int camera_idx =
config.check("camera",
Value(CV_CAP_ANY),
"if present, read from camera identified by this index").asInt32();
// Try to open a capture object for the first camera
m_cap.open(camera_idx);
if (!m_cap.isOpened()) {
yError("Unable to open camera for capture!");
return false;
}
else
{
yInfo("Capturing from camera: %d",camera_idx);
}
if ( config.check("framerate","if present, specifies desired camera device framerate") ) {
double m_fps = config.check("framerate", Value(-1)).asFloat64();
m_cap.set(CV_CAP_PROP_FPS, m_fps);
}
if (config.check("flip_x", "if present, flip the image along the x-axis")) m_flip_x = true;
if (config.check("flip_y", "if present, flip the image along the y-axis")) m_flip_y = true;
if (config.check("transpose", "if present, rotate the image along of 90 degrees")) m_transpose = true;
}
// Extract the desired image size from the configuration if
// present, otherwise query the capture device
if (config.check("width","if present, specifies desired image width")) {
m_w = config.check("width", Value(0)).asInt32();
if (!fromFile && m_w>0) {
m_cap.set(CV_CAP_PROP_FRAME_WIDTH, m_w);
}
} else {
m_w = (size_t)m_cap.get(CV_CAP_PROP_FRAME_WIDTH);
}
if (config.check("height","if present, specifies desired image height")) {
m_h = config.check("height", Value(0)).asInt32();
if (!fromFile && m_h>0) {
m_cap.set(CV_CAP_PROP_FRAME_HEIGHT, m_h);
}
} else {
m_w = (size_t)m_cap.get(CV_CAP_PROP_FRAME_HEIGHT);
}
// Ignore capture properties - they are unreliable
yInfo("OpenCVGrabber opened");
// Success!
// save our configuration for future reference
m_config.fromString(config.toString());
return true;
}
bool ARMarkerDetectorModule::open(Searchable& config)
{
if (config.check("help","if present, display usage message")) {
printf("Call with --file configFile.ini\n");
return false;
}
ConstString input_port = config.check("inputPort", Value("img:i"), "Camera intrinsics definition file.").asString().c_str();
ConstString output_port = config.check("outputPort", Value("data:o"), "Camera intrinsics definition file.").asString().c_str();
ConstString conf_port = config.check("confPort", Value("conf"), "Camera intrinsics definition file.").asString().c_str();
ConstString display_name = config.check("displayName", Value("ARTrackerUH"), "Camera intrinsics definition file.").asString().c_str();
ConstString camera_file = config.check("cameraparameters", Value("camera_para.dat"), "Camera intrinsics definition file.").asString().c_str();
ConstString objects_file = config.check("objects", Value("object_data.txt"), "Markers definition file.").asString().c_str();
tracker.loadCameraParameters( camera_file.c_str(), 640, 480);
//tracker.loadObjectData( objects_file.c_str() );
int numberofobjects = config.check("numberofpatterns", 0, "number of patterns").asInt();
tracker.initobjectdata( numberofobjects );
ConstString directory = config.check("directory", "/", "directory where patterns are located").asString();
for(int cnt=0;cnt<numberofobjects;cnt++)
{
char name[256];
char filename[256];
char pattern[16];
sprintf( pattern,"pattern%d",cnt+1);
Bottle& K = config.findGroup( pattern, "object");
cout << K.toString() << endl;
strcpy( name, (const char*)K.get(1).asString());
strcpy( filename, (const char*)directory);
strcat( filename, (const char*)K.get(2).asString() );
double size = K.get(3).asDouble();
printf("reading conf bootle s0%s s1%s s2%s s3%g\n",
(const char*)K.get(0).asString(),
(const char*)K.get(1).asString(),
(const char*)K.get(2).asString(),
size);
tracker.addobjectdata( cnt, name, filename, size );
}
//tracker.loadObjectData( config );
//check visualization
int visualize = config.check("Visualization", 0, "Show the tracker image").asInt();
if(visualize)
tracker.openVisualization(display_name);
//load in the object data - trained markers and associated bitmap files
_imgPort.open(getName(input_port));
_configPort.open(getName(conf_port));
_outPort.open(getName(output_port));
attach(_configPort, true);
// imageOut.open("/testOut");
return true;
}
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;
}
virtual bool select(Searchable& options) override {
return options.check("type",Value("none")).asString() == "device";
}
GuiSalience::GuiSalience(Searchable& config, QWidget* parent, const char* name, bool modal, WFlags fl)
: GuiSalienceBase( parent, name, fl ),
frmMainLayout(NULL),
_scrlView(NULL),
_scrlFrame(NULL),
_scrlFrameLayout(NULL),
_spacer(NULL),
_qwConfConnection(NULL)
{
Property prop;
if(!config.check("name")){
prop.put("name", "/salienceGui");
}
else{
prop.put("name", config.find("name"));
}
if(config.check("REMOTE_SALIENCE")){
prop.put("remote", config.findGroup("REMOTE_SALIENCE").find("remote").asString().c_str());
}
else{
if(config.check("remote")){
prop.put("remote", config.find("remote").asString().c_str());
}
else{
prop.put("remote", "/salience/conf");
}
}
// salience controls
// *****************
frmMainLayout = new QVBoxLayout( frmMain, 3, 3, "frmMainLayout");
// scroll view
_scrlView = new QScrollView(frmMain);
frmMainLayout->addWidget(_scrlView);
_scrlView->setResizePolicy(QScrollView::AutoOneFit);
// frame for filter widgets
_scrlFrame = new QFrame(_scrlView->viewport());
_scrlView->addChild(_scrlFrame);
_scrlFrameLayout = new QVBoxLayout(_scrlFrame, 3, 3, "_scrlFrameLayout");
// setup connection widget
grbConfigurationConnection->setColumnLayout(0, Qt::Vertical );
QVBoxLayout *grbConnectionLayout = new QVBoxLayout(grbConfigurationConnection->layout());
grbConfigurationConnection->layout()->setSpacing( 3 );
grbConfigurationConnection->layout()->setMargin( 3 );
grbConnectionLayout->setAlignment(Qt::AlignTop);
_qwConfConnection = new QWidgetConnection(grbConfigurationConnection);
grbConnectionLayout->addWidget(_qwConfConnection);
yarp::os::ConstString strLocalPort = std::string(std::string(prop.find("name").asString().c_str())+ std::string("/conf")).c_str();
yarp::os::ConstString strRemotePort = prop.find("remote").asString();
_remoteSalience.open(strLocalPort);
_qwConfConnection->setTargetPortName(strRemotePort.c_str());
_qwConfConnection->setSourcePortName(strLocalPort.c_str());
this->initializeUI();
// line edit validators
lneThreshold->setValidator(new QDoubleValidator(this));
lneBlur->setValidator(new QIntValidator(this));
// set gui values to current values
QVariant thr(_remoteSalience.getSalienceThreshold());
lneThreshold->setText(thr.asString());
QVariant nb(_remoteSalience.getNumBlurPasses());
lneBlur->setText(nb.asString());
}
bool OpenCVGrabber::open(Searchable & config) {
// Release any previously allocated resources, just in case
close();
m_saidSize = false;
m_saidResize = false;
m_transpose = false;
m_flip_x = false;
m_flip_y = false;
// Are we capturing from a file or a camera ?
ConstString file = config.check("movie", Value(""),
"if present, read from specified file rather than camera").asString();
fromFile = (file!="");
if (fromFile) {
// Try to open a capture object for the file
m_capture = (void*)cvCaptureFromAVI(file.c_str());
if (0 == m_capture) {
yError("Unable to open file '%s' for capture!", file.c_str());
return false;
}
// Should we loop?
m_loop = config.check("loop","if present, loop movie");
} else {
m_loop = false;
int camera_idx =
config.check("camera",
Value(CV_CAP_ANY),
"if present, read from camera identified by this index").asInt();
// Try to open a capture object for the first camera
m_capture = (void*)cvCaptureFromCAM(camera_idx);
if (0 == m_capture) {
yError("Unable to open camera for capture!");
return false;
}
if ( config.check("framerate","if present, specifies desired camera device framerate") ) {
double m_fps = config.check("framerate", Value(-1)).asDouble();
cvSetCaptureProperty((CvCapture*)m_capture,
CV_CAP_PROP_FPS,m_fps);
}
if (config.check("flip_x", "if present, flip the image along the x-axis")) m_flip_x = true;
if (config.check("flip_y", "if present, flip the image along the y-axis")) m_flip_y = true;
if (config.check("transpose", "if present, rotate the image along of 90 degrees")) m_transpose = true;
}
// Extract the desired image size from the configuration if
// present, otherwise query the capture device
if (config.check("width","if present, specifies desired image width")) {
m_w = config.check("width", Value(-1)).asInt();
if (!fromFile && m_w>0) {
cvSetCaptureProperty((CvCapture*)m_capture,
CV_CAP_PROP_FRAME_WIDTH, m_w);
}
} else {
m_w = (int)cvGetCaptureProperty((CvCapture*)m_capture,
CV_CAP_PROP_FRAME_WIDTH);
}
if (config.check("height","if present, specifies desired image height")) {
m_h = config.check("height", Value(-1)).asInt();
if (!fromFile && m_h>0) {
cvSetCaptureProperty((CvCapture*)m_capture,
CV_CAP_PROP_FRAME_HEIGHT, m_h);
}
} else {
m_h = (int)cvGetCaptureProperty((CvCapture*)m_capture,
CV_CAP_PROP_FRAME_HEIGHT);
}
// Ignore capture properties - they are unreliable
// yDebug("Capture properties: %ld x %ld pixels @ %lf frames/sec.", m_w, m_h, cvGetCaptureProperty(m_capture, CV_CAP_PROP_FPS));
yInfo("OpenCVGrabber opened");
// Success!
// save our configuration for future reference
m_config.fromString(config.toString());
return true;
}
bool fromConfigFile(const ConstString& fname,Searchable& env, bool wipe=true) {
String searchPath =
env.check("CONFIG_PATH",
Value(""),
"path to search for config files").toString().c_str();
YARP_DEBUG(Logger::get(),
String("looking for ") + fname.c_str() + ", search path: " +
searchPath);
String pathPrefix("");
String txt;
bool ok = true;
if (!readFile(fname,txt,true)) {
ok = false;
SplitString ss(searchPath.c_str(),';');
for (int i=0; i<ss.size(); i++) {
String trial = ss.get(i);
trial += '/';
trial += fname;
YARP_DEBUG(Logger::get(),
String("looking for ") + fname + " as " +
trial.c_str());
txt = "";
if (readFile(trial.c_str(),txt,true)) {
ok = true;
pathPrefix = ss.get(i);
pathPrefix += '/';
break;
}
}
}
String path("");
String sfname = fname;
size_t index = sfname.rfind('/');
if (index==String::npos) {
index = sfname.rfind('\\');
}
if (index!=String::npos) {
path = sfname.substr(0,index);
}
if (!ok) {
YARP_ERROR(Logger::get(),String("cannot read from ") +
fname);
return false;
}
Property envExtended;
envExtended.fromString(env.toString());
if (path!="") {
if (searchPath.length()>0) {
searchPath += ";";
}
searchPath += pathPrefix;
searchPath += path;
envExtended.put("CONFIG_PATH",searchPath.c_str());
}
fromConfig(txt.c_str(),envExtended,wipe);
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;
}
bool SerialServoBoard::open(Searchable &config) {
if(config.check("help")==true) {
printf("SerialServoBoard Available Options:\n");
//printf(" -board NAME, where name is one of ssc32, minissc, pontech_sv203x, mondotronic_smi, parallax, pololu_usb_16servo, picopic\n");
printf(" -comport NAME, where name is COMx on Windows, and /dev/ttySx on Linux\n");
printf(" -baudrate RATE, where RATE is the Board baudrate\n");
printf("\n -help shows this help\n");
return false;
}
// char servoboard_[80];
// strcpy(servoboard_, config.check("board", yarp::os::Value("ssc32")).asString().c_str());
// if(strcmp(servoboard_, "ssc32")==0) {
// servoboard=SSC32;
// move=&movessc32;
// }else if(strcmp(servoboard_, "minissc")==0) {
// servoboard=MINISSC;
// move=&moveminissc;
// }else if(strcmp(servoboard_, "pontech_sv203x")==0) {
// servoboard=PONTECHSV203X;
// move=&movepontech;
// }else if(strcmp(servoboard_, "mondotronic_smi")==0) {
// servoboard=MONDOTRONICSMI;
// move=&movemondotronic;
// }else if(strcmp(servoboard_, "pololu_usb_16servo")==0) {
// servoboard=POLOLUUSB16;
// move=&movepololu;
// }else if(strcmp(servoboard_, "picopic")==0) {
// servoboard=PICOPIC;
// move=&movepicopic;
// }
char comport[80];
strcpy(comport, config.check("comport", yarp::os::Value("/dev/ttyACM0")).asString().c_str());
int baudrate = config.check("baudrate", yarp::os::Value(57600)).asInt();
Property conf;
// no arguments, use a default
char str[160];
sprintf(str, "(device serialport) (comport %s) (baudrate %d) (rcvenb 1) (stopbits 2) (databits 8) (paritymode none)", comport, baudrate);
conf.fromString(str);
dd.open(conf);
if (!dd.isValid()) {
printf("Failed to create and configure a device\n");
exit(1);
}
if (!dd.view(serial)) {
printf("Failed to view device through IGPUDevice interface\n");
exit(1);
}
positions=(double *)malloc(sizeof(double)*2);
speeds=(double *)malloc(sizeof(double)*2);
return true;
}
bool open(Searchable& options)
{
ConstString dbDefault = ":memory:";
ConstString subdbDefault = ":memory:";
if (options.check("memory")) {
fprintf(stderr,"The --memory option was given, but that is now a default. Continuing.\n");
}
ConstString dbFilename = options.check("portdb",
Value(dbDefault)).asString();
ConstString subdbFilename = options.check("subdb",
Value(subdbDefault)).asString();
ConstString ip = options.check("ip",Value("...")).asString();
int sock = options.check("socket",Value(Network::getDefaultPortRange())).asInt();
bool cautious = options.check("cautious");
bool verbose = options.check("verbose");
if (!silent) {
printf("Using port database: %s\n",
dbFilename.c_str());
printf("Using subscription database: %s\n",
subdbFilename.c_str());
if (dbFilename!=":memory:" || subdbFilename!=":memory:") {
printf("If you ever need to clear the name server's state, just delete those files.\n\n");
}
printf("IP address: %s\n",
(ip=="...")?"default":ip.c_str());
printf("Port number: %d\n", sock);
}
bool reset = false;
if (options.check("ip")||options.check("socket")) {
fprintf(stderr,"Database needs to be reset, IP or port number set.\n");
reset = true;
}
TripleSource *pmem = db.open(dbFilename.c_str(),cautious,reset);
if (pmem == nullptr) {
fprintf(stderr,"Aborting, ports database failed to open.\n");
return false;
}
if (verbose) {
pmem->setVerbose(1);
}
if (!subscriber.open(subdbFilename.c_str())) {
fprintf(stderr,"Aborting, subscription database failed to open.\n");
return false;
}
if (verbose) {
subscriber.setVerbose(true);
}
contact = Contact("...", "tcp", ip, sock);
if (!options.check("local")) {
if (!BootstrapServer::configFileBootstrap(contact,
options.check("read"),
options.check("write"))) {
fprintf(stderr,"Aborting.\n");
return false;
}
}
if (options.check("ros") || NetworkBase::getEnvironment("YARP_USE_ROS")!="") {
yarp::os::Bottle lst = yarp::os::Carriers::listCarriers();
std::string lstStr(lst.toString().c_str());
if (lstStr.find("rossrv") == std::string::npos ||
lstStr.find("tcpros") == std::string::npos ||
lstStr.find("xmlrpc") == std::string::npos) {
fprintf(stderr,"Missing one or more required carriers ");
fprintf(stderr,"for yarpserver --ros (rossrv, tcpros, xmlrpc).\n");
fprintf(stderr,"Run 'yarp connect --list-carriers' to see carriers on your machine\n");
fprintf(stderr,"Aborting.\n");
return false;
}
ConstString addr = NetworkBase::getEnvironment("ROS_MASTER_URI");
Contact c = Contact::fromString(addr.c_str());
if (c.isValid()) {
c.setCarrier("xmlrpc");
c.setName("/ros");
space = new RosNameSpace(c);
subscriber.setDelegate(space);
ns.setDelegate(space);
fprintf(stderr, "Using ROS with ROS_MASTER_URI=%s\n", addr.c_str());
} else {
fprintf(stderr, "Cannot find ROS, check ROS_MASTER_URI (currently '%s')\n", addr.c_str());
::exit(1);
}
}
config.minPortNumber = contact.getPort() + MIN_PORT_NUMBER_OFFSET;
config.maxPortNumber = contact.getPort() + MAX_PORT_NUMBER_OFFSET;
alloc.open(pmem,config);
ns.open(pmem,&alloc,contact);
NetworkBase::queryBypass(&ns);
subscriber.setStore(ns);
ns.setSubscriber(&subscriber);
style.configure(options);
combo1.open(subscriber,style);
open(combo1,ns);
return true;
}
bool AnalogWrapper::checkROSParams(Searchable &config)
{
// check for ROS parameter group
if(!config.check("ROS") )
{
useROS = ROS_disabled;
yInfo() << "No ROS group found in config file ... skipping ROS initialization.";
return true;
}
yInfo() << "ROS group was FOUND in config file.";
Bottle &rosGroup = config.findGroup("ROS");
if(rosGroup.isNull())
{
yError() << sensorId << "ROS group params is not a valid group or empty";
useROS = ROS_config_error;
return false;
}
// check for useROS parameter
if(!rosGroup.check("useROS"))
{
yError() << sensorId << " cannot find useROS parameter, mandatory when using ROS message. \n \
Allowed values are true, false, ROS_only";
useROS = ROS_config_error;
return false;
}
yarp::os::ConstString ros_use_type = rosGroup.find("useROS").asString();
if(ros_use_type == "false")
{
yInfo() << sensorId << "useROS topic if set to 'false'";
useROS = ROS_disabled;
return true;
}
else if(ros_use_type == "true")
{
yInfo() << sensorId << "useROS topic if set to 'true'";
useROS = ROS_enabled;
}
else if(ros_use_type == "only")
{
yInfo() << sensorId << "useROS topic if set to 'only";
useROS = ROS_only;
}
else
{
yInfo() << sensorId << "useROS parameter is set to unvalid value ('" << ros_use_type << "'), supported values are 'true', 'false', 'only'";
useROS = ROS_config_error;
return false;
}
// check for ROS_nodeName parameter
if(!rosGroup.check("ROS_nodeName"))
{
yError() << sensorId << " cannot find ROS_nodeName parameter, mandatory when using ROS message";
useROS = ROS_config_error;
return false;
}
rosNodeName = rosGroup.find("ROS_nodeName").asString(); // TODO: check name is correct
yInfo() << sensorId << "rosNodeName is " << rosNodeName;
// check for ROS_topicName parameter
if(!rosGroup.check("ROS_topicName"))
{
yError() << sensorId << " cannot find ROS_topicName parameter, mandatory when using ROS message";
useROS = ROS_config_error;
return false;
}
rosTopicName = rosGroup.find("ROS_topicName").asString();
yInfo() << sensorId << "ROS_topicName is " << rosTopicName;
// check for ROS_msgType parameter
if (!rosGroup.check("ROS_msgType"))
{
yError() << sensorId << " cannot find ROS_msgType parameter, mandatory when using ROS message";
useROS = ROS_config_error;
return false;
}
rosMsgType = rosGroup.find("ROS_msgType").asString();
// check for frame_id parameter
if (rosMsgType == "geometry_msgs/WrenchedStamped")
{
yInfo() << sensorId << "ROS_msgType is " << rosMsgType;
if (!rosGroup.check("frame_id"))
{
yError() << sensorId << " cannot find frame_id parameter, mandatory when using ROS message";
useROS = ROS_config_error;
return false;
}
frame_id = rosGroup.find("frame_id").asString();
yInfo() << sensorId << "frame_id is " << frame_id;
}
else if (rosMsgType == "sensor_msgs/JointState")
{
yInfo() << sensorId << "ROS_msgType is " << rosMsgType;
if (!rosGroup.check("joint_names"))
{
yError() << sensorId << " cannot find some ros parameters";
useROS = ROS_config_error;
return false;
}
yarp::os::Bottle& jnam =rosGroup.findGroup("joint_names");
int joint_names_size = jnam.size()-1;
for (int i = 0; i < joint_names_size; i++)
{
ros_joint_names.push_back(jnam.get(i+1).asString());
}
}
else
{
yError() << sensorId << "ROS_msgType '" << rosMsgType << "' not supported ";
return false;
}
return true;
}
bool VirtualAnalogWrapper::open(Searchable& config)
{
cout << config.toString().c_str() << endl << endl;
mIsVerbose = (config.check("verbose","if present, give detailed output"));
if (mIsVerbose) cout << "running with verbose output\n";
//thus thread period is useful for output port... this input port has callback so maybe can skip it (?)
//thread_period = prop.check("threadrate", 20, "thread rate in ms. for streaming encoder data").asInt();
std::cout << "Using VirtualAnalogServer\n";
if (!config.check("networks", "list of networks merged by this wrapper"))
{
cerr << "Error: missing networks parameters" << endl;
return false;
}
Bottle *networks=config.find("networks").asList();
mNSubdevs=networks->size();
mSubdevices.resize(mNSubdevs);
mChan2Board.resize(MAX_ENTRIES);
mChan2BAddr.resize(MAX_ENTRIES);
for (int i=0; i< MAX_ENTRIES; i++)
{
mChan2Board[i]=-1;
mChan2BAddr[i]=-1;
}
int totalJ=0;
for (int k=0; k<networks->size(); ++k)
{
Bottle parameters=config.findGroup(networks->get(k).asString().c_str());
if (parameters.size()!=5) // mapping joints using the paradigm: part from - to / network from - to
{
cerr << "Error: check network parameters in part description" << endl;
cerr << "--> I was expecting " << networks->get(k).asString().c_str() << " followed by four integers" << endl;
return false;
}
int map0=parameters.get(1).asInt();
int map1=parameters.get(2).asInt();
int map2=parameters.get(3).asInt();
int map3=parameters.get(4).asInt();
if (map0 >= MAX_ENTRIES || map1 >= MAX_ENTRIES || map2>= MAX_ENTRIES || map3>= MAX_ENTRIES ||
map0 <0 || map1 <0 || map2<0 || map3<0)
{
cerr << "Error: invalid map entries in networks section, failed initial check" << endl;
return false;
}
for (int j=map0; j<=map1; ++j)
{
mChan2Board[j]=k;
mChan2BAddr[j]=j-map0+map2;
}
if (!mSubdevices[k].configure(map2,map3,networks->get(k).asString().c_str()))
{
cerr << "configure of subdevice ret false" << endl;
return false;
}
totalJ+=map1-map0+1;
}
// Verify minimum set of parameters required
if(!config.check("robotName") ) // ?? qui dentro, da dove lo pesco ??
{
cout << "VirtualAnalogServer missing robot Name, check your configuration file!! Quitting\n";
return false;
}
std::string root_name;
std::string port_name = config.check("name",Value("controlboard"),"prefix for port names").asString().c_str();
std::string robot_name = config.find("robotName").asString().c_str();
root_name+="/";
root_name+=robot_name;
root_name+= "/joint_vsens" + port_name + ":i";
if (!mPortInputTorques.open(root_name.c_str()))
{
cerr << "can't open port " << root_name.c_str() << endl;
return false;
}
return true;
}
bool VirtualAnalogWrapper::open(Searchable& config)
{
yDebug() << config.toString().c_str();
mIsVerbose = (config.check("verbose","if present, give detailed output"));
if (mIsVerbose) yDebug() << "running with verbose output\n";
//thus thread period is useful for output port... this input port has callback so maybe can skip it (?)
//thread_period = prop.check("threadrate", 20, "thread rate in ms. for streaming encoder data").asInt();
yDebug() << "Using VirtualAnalogServer\n";
if (!config.check("networks", "list of networks merged by this wrapper"))
{
yError() << "VirtualAnalogWrapper: missing networks parameters";
return false;
}
Bottle *networks=config.find("networks").asList();
mNSubdevs=networks->size();
mSubdevices.resize(mNSubdevs);
mChan2Board.resize(MAX_ENTRIES);
mChan2BAddr.resize(MAX_ENTRIES);
for (int i=0; i< MAX_ENTRIES; i++)
{
mChan2Board[i]=-1;
mChan2BAddr[i]=-1;
}
int totalJ=0;
for (int k=0; k<networks->size(); ++k)
{
Bottle parameters=config.findGroup(networks->get(k).asString().c_str());
if (parameters.size()!=5) // mapping joints using the paradigm: part from - to / network from - to
{
yError() << "VirtualAnalogWrapper: check network parameters in part description"
<< " I was expecting " << networks->get(k).asString().c_str() << " followed by four integers";
return false;
}
int map0=parameters.get(1).asInt();
int map1=parameters.get(2).asInt();
int map2=parameters.get(3).asInt();
int map3=parameters.get(4).asInt();
if (map0 >= MAX_ENTRIES || map1 >= MAX_ENTRIES || map2>= MAX_ENTRIES || map3>= MAX_ENTRIES ||
map0 <0 || map1 <0 || map2<0 || map3<0)
{
yError() << "VirtualAnalogWrapper: invalid map entries in networks section, failed initial check";
return false;
}
for (int j=map0; j<=map1; ++j)
{
mChan2Board[j]=k;
mChan2BAddr[j]=j-map0+map2;
}
if (!mSubdevices[k].configure(map2,map3,networks->get(k).asString().c_str()))
{
yError() << "VirtualAnalogWrapper: configure of subdevice ret false";
return false;
}
totalJ+=map1-map0+1;
}
// Verify minimum set of parameters required
if(!config.check("robotName") ) // ?? qui dentro, da dove lo pesco ??
{
yError() << "VirtualAnalogWrapper: missing robotName, check your configuration file!";
return false;
}
if (config.check("deviceId"))
{
yError() << "VirtualAnalogWrapper: the parameter 'deviceId' has been deprecated, please use parameter 'name' instead. \n"
<< "e.g. In the VFT wrapper configuration files of your robot, replace '<param name=""deviceId""> left_arm </param>' \n"
<< "with '/icub/joint_vsens/left_arm:i' ";
return false;
}
std::string port_name = config.check("name",Value("controlboard"),"Virtual analog wrapper port name, e.g. /icub/joint_vsens/left_arm:i").asString().c_str();
std::string robot_name = config.find("robotName").asString().c_str();
if (!mPortInputTorques.open(port_name.c_str()))
{
yError() << "VirtualAnalogWrapper: can't open port " << port_name.c_str();
return false;
}
return true;
}
