bool RGBDSensorWrapper::open(yarp::os::Searchable &config)
{
// DeviceResponder::makeUsage();
// addUsage("[set] [bri] $fBrightness", "set brightness");
// addUsage("[set] [expo] $fExposure", "set exposure");
//
m_conf.fromString(config.toString());
if(verbose >= 5)
yTrace() << "\nParameters are: \n" << config.toString();
if(!fromConfig(config))
{
yError() << "Device RGBDSensorWrapper failed to open, check previous log for error messsages.\n";
return false;
}
setId("RGBDSensorWrapper for " + depthFrame_StreamingPort_Name);
if(!initialize_YARP(config) )
{
yError() << sensorId << "Error initializing YARP ports";
return false;
}
if(!initialize_ROS(config) )
{
yError() << sensorId << "Error initializing ROS topic";
return false;
}
return true;
}
bool USBCameraDriver::open(yarp::os::Searchable& config)
{
// open OS dependant device
yTrace() << "input params are " << config.toString();
#if defined(_MSC_VER)
os_device = (DeviceDriver*) new WIN_camera;
#elif defined __unix
os_device = (DeviceDriver*) new V4L_camera;
#endif
yarp::os::Property prop;
prop.fromString(config.toString().c_str());
if(!prop.check("pixelType")){
switch(pixelType)
{
case VOCAB_PIXEL_MONO:
prop.put("pixelType", VOCAB_PIXEL_MONO);
break;
case VOCAB_PIXEL_RGB:
default:
prop.put("pixelType", VOCAB_PIXEL_RGB);
break;
}
}
if(!os_device->open(prop) )
return false;
os_device->view(deviceRgb);
os_device->view(deviceRaw);
os_device->view(deviceControls);
os_device->view(deviceControls2);
os_device->view(deviceTimed);
os_device->view(deviceRgbVisualParam);
if(deviceRaw)
{
_width = deviceRaw->width();
_height = deviceRaw->height();
}
if(deviceRgb)
{
_width = deviceRgb->width();
_height = deviceRgb->height();
}
return true;
}
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 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(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 BatteryWrapper::open(yarp::os::Searchable &config)
{
Property params;
params.fromString(config.toString().c_str());
if (!config.check("period"))
{
yError() << "BatteryWrapper: missing 'period' parameter. Check you configuration file\n";
return false;
}
else
_rate = config.find("period").asInt();
if (!config.check("name"))
{
yError() << "BatteryWrapper: missing 'name' parameter. Check you configuration file; it must be like:";
yError() << " name: full name of the port, like /robotName/deviceId/sensorType:o";
return false;
}
else
{
streamingPortName = config.find("name").asString().c_str();
rpcPortName = streamingPortName + "/rpc:i";
setId("batteryWrapper");
}
if(!initialize_YARP(config) )
{
yError() << sensorId << "Error initializing YARP ports";
return false;
}
return true;
}
virtual bool reconfigure(yarp::os::Searchable& config) {
options.fromString(config.toString());
if (options.check("overlay")) {
workOverlay = options.check("overlay",Value("")).toString().c_str();
}
needRead = true;
return true;
}
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 CanBusInertialMTB::validateConf(yarp::os::Searchable& config,
std::vector<int> & canAddresses)
{
std::cout << "CanBusInertialMTB::validateConf : " << config.toString() << std::endl;
bool correct=true;
correct = correct && checkRequiredParamIsString(config,"canbusDevice");
correct = correct && checkRequiredParamIsInt(config,"canDeviceNum");
correct = correct && checkRequiredParamIsVectorOfInt(config,"canAddress",canAddresses);
correct = correct && checkRequiredParamIsString(config,"physDevice");
correct = correct && checkRequiredParamIsString(config,"sensorType");
return correct;
}
//DEVICE DRIVER
bool GazeboYarpDepthCameraDriver::open(yarp::os::Searchable &config)
{
string sensorScopedName((config.find(YarpScopedName.c_str()).asString().c_str()));
//Get gazebo pointers
m_conf.fromString(config.toString());
m_depthCameraSensorPtr = dynamic_cast<gazebo::sensors::DepthCameraSensor*> (GazeboYarpPlugins::Handler::getHandler()->getSensor(sensorScopedName));
if (!m_depthCameraSensorPtr)
{
yError("camera sensor was not found (sensor's scoped name %s!)", sensorScopedName.c_str());
return false;
}
m_depthCameraPtr = this->m_depthCameraSensorPtr->DepthCamera();
m_width = m_depthCameraPtr->ImageWidth();
m_height = m_depthCameraPtr->ImageHeight();
m_imageFrame_BufferSize = m_depthCameraPtr->ImageDepth() * m_width * m_height;
m_depthFrame_BufferSize = m_width * m_height * sizeof(float);
m_depthFrameMutex.wait();
m_depthFrame_Buffer = new float[m_width * m_height];
memset(m_depthFrame_Buffer, 0x00, m_depthFrame_BufferSize);
m_depthFrameMutex.post();
m_colorFrameMutex.wait();
m_imageFrame_Buffer = new unsigned char[m_imageFrame_BufferSize];
memset(m_imageFrame_Buffer, 0x00, m_imageFrame_BufferSize);
m_colorFrameMutex.post();
//Connect the driver to the gazebo simulation
auto imageConnectionBind = boost::bind(&GazeboYarpDepthCameraDriver::OnNewImageFrame, this, _1, _2, _3, _4, _5);
auto depthConnectionBind = boost::bind(&GazeboYarpDepthCameraDriver::OnNewDepthFrame, this, _1, _2, _3, _4, _5);
this->m_updateImageFrame_Connection = m_depthCameraPtr->ConnectNewImageFrame(imageConnectionBind);
this->m_updateDepthFrame_Connection = m_depthCameraPtr->ConnectNewDepthFrame(depthConnectionBind);
return true;
}
//DEVICE DRIVER
bool GazeboYarpJointSensorsDriver::open(yarp::os::Searchable& config)
{
std::cout << "GazeboYarpJointSensorsDriver::open() called" << std::endl;
yarp::os::Property pluginParameters;
pluginParameters.fromString(config.toString().c_str());
std::string robotName (pluginParameters.find("robotScopedName").asString().c_str());
std::cout << "DeviceDriver is looking for robot " << robotName << "...\n";
_robot = GazeboYarpPlugins::Handler::getHandler()->getRobot(robotName);
if(NULL == _robot)
{
std::cout << "GazeboYarpJointSensorsDriver error: robot was not found\n";
return false;
}
bool ok = setJointPointers(pluginParameters);
assert(joint_ptrs.size() == jointsensors_nr_of_channels);
if( !ok )
{
return false;
}
ok = setJointSensorsType(pluginParameters);
if( !ok )
{
return false;
}
data_mutex.wait();
jointsensors_data.resize(jointsensors_nr_of_channels,0.0);
data_mutex.post();
//Connect the driver to the gazebo simulation
this->updateConnection = gazebo::event::Events::ConnectWorldUpdateBegin (
boost::bind ( &GazeboYarpJointSensorsDriver::onUpdate, this, _1 ) );
std::cout << "GazeboYarpJointSensorsDriver::open() returning true" << std::endl;
return true;
}
bool GazeboYarpContactLoadCellArrayDriver::open(yarp::os::Searchable& config)
{
yarp::os::Property pluginParams;
pluginParams.fromString(config.toString().c_str());
std::string robotName(pluginParams.find("robotName").asString().c_str());
this->m_robot = GazeboYarpPlugins::Handler::getHandler()->getRobot(robotName);
if (this->m_robot == NULL)
{
yError() << "GazeboYarpContactLoadCellArrayDriver: Robot Model was not found";
return false;
}
if (!this->initLinkAssociatedToContactSensor(pluginParams))
{
return false;
}
if (!this->initContactSensor())
{
return false;
}
if (!this->configure(pluginParams))
{
return false;
}
if (!this->prepareLinkInformation())
{
return false;
}
yarp::os::LockGuard guard(m_dataMutex);
this->m_updateConnection = gazebo::event::Events::ConnectWorldUpdateBegin(boost::bind(&GazeboYarpContactLoadCellArrayDriver::onUpdate, this, _1));
this->m_sensor->SetActive(true);
return true;
}
bool FakeAnalogSensor::open(yarp::os::Searchable& config)
{
yTrace();
bool correct=true;
//debug
fprintf(stderr, "%s\n", config.toString().c_str());
// Check parameters first
// if(!config.check("channels"))
// {
// correct = false;
// yError() << "Parameter 'channels' missing";
// }
if(!config.check("period"))
{
correct = false;
yError() << "Parameter 'period' missing";
}
if (!correct)
{
yError() << "Insufficient parameters to FakeAnalogSensor\n";
return false;
}
int period=config.find("period").asInt();
setRate(period);
//create the data vector:
this->channelsNum = 1;
data.resize(channelsNum);
data.zero();
RateThread::start();
return true;
}
bool comanFTsensor::open(yarp::os::Searchable &config)
{
_comanHandler = comanDevicesHandler::instance();
if(_comanHandler == NULL)
{
yError() << "unable to create a new Coman Device Handler class!";
return false;
}
if(!_comanHandler->open(config))
{
yError() << "Unable to initialize Coman Device Handler class... probably np boards were found. Check log.";
return false;
}
_boards_ctrl = _comanHandler->getBoard_ctrl_p();
if(_boards_ctrl == NULL)
{
yError() << "unable to create a new Boards_ctrl class!";
return false;
}
Property prop;
std::string str=config.toString().c_str();
yTrace() << str;
if(!fromConfig(config))
return false;
prop.fromString(str.c_str());
// TODO fix this!
#warning "<><> TODO: This is a copy of the mcs map. Verify that things will never change after this copy or use a pointer (better) <><>"
_fts = _boards_ctrl->get_fts_map();
return true;
}
bool PolyDriver::open(yarp::os::Searchable& config) {
if (isValid()) {
// already open - should close first
return false;
}
if (system_resource==NULL) {
system_resource = new YarpDevMonitor;
}
yAssert(system_resource!=NULL);
bool removeMonitorAfterwards = false;
if (config.getMonitor()==NULL) {
config.setMonitor(&HELPER(system_resource));
removeMonitorAfterwards = true;
}
//dd = Drivers::factory().open(config);
coreOpen(config);
HELPER(system_resource).info.fromString(config.toString());
if (removeMonitorAfterwards) {
config.setMonitor(NULL);
}
return isValid();
}
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 BoschIMU::open(yarp::os::Searchable& config)
{
//debug
yTrace("Parameters are:\n\t%s", config.toString().c_str());
if(!config.check("comport"))
{
yError() << "Param 'comport' not found";
return false;
}
int period = config.check("period",Value(10),"Thread period in ms").asInt();
setRate(period);
nChannels = config.check("channels", Value(12)).asInt();
fd_ser = ::open(config.find("comport").toString().c_str(), O_RDWR | O_NOCTTY );
if (fd_ser < 0) {
yError("can't open %s, %s", config.find("comport").toString().c_str(), strerror(errno));
return false;
}
//Get the current options for the port...
struct termios options;
tcgetattr(fd_ser, &options);
cfmakeraw(&options);
//set the baud rate to 115200
int baudRate = B115200;
cfsetospeed(&options, baudRate);
cfsetispeed(&options, baudRate);
//set the number of data bits.
options.c_cflag &= ~CSIZE; // Mask the character size bits
options.c_cflag |= CS8;
//set the number of stop bits to 1
options.c_cflag &= ~CSTOPB;
//Set parity to None
options.c_cflag &=~PARENB;
//set for non-canonical (raw processing, no echo, etc.)
// options.c_iflag = IGNPAR; // ignore parity check
options.c_oflag = 0; // raw output
options.c_lflag = 0; // raw input
// SET NOT BLOCKING READ
options.c_cc[VMIN] = 0; // block reading until RX x characters. If x = 0, it is non-blocking.
options.c_cc[VTIME] = 2; // Inter-Character Timer -- i.e. timeout= x*.1 s
//Set local mode and enable the receiver
options.c_cflag |= (CLOCAL | CREAD);
tcflush(fd_ser, TCIOFLUSH);
//Set the new options for the port...
if ( tcsetattr(fd_ser, TCSANOW, &options) != 0)
{
yError("Configuring comport failed");
return false;
}
if(!RateThread::start())
return false;
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 iCubHandCalibrator::open (yarp::os::Searchable& config)
{
Property p;
p.fromString(config.toString());
if (!p.check("GENERAL")) {
fprintf(stderr, "HANDCALIB::Cannot understand configuration parameters\n");
return false;
}
int nj = p.findGroup("GENERAL").find("Joints").asInt();
if (nj!=numberOfJoints)
{
fprintf(stderr, "HANDCALIB::calibrator is for %d joints but device has %d\n", numberOfJoints, nj);
return false;
}
type = new unsigned char[nj];
param1 = new double[nj];
param2 = new double[nj];
param3 = new double[nj];
pos = new double[nj];
vel = new double[nj];
homePos = new double[nj];
homeVel = new double[nj];
Bottle& xtmp = p.findGroup("CALIBRATION").findGroup("Calibration1");
int i;
for (i = 1; i < xtmp.size(); i++)
param1[i-1] = xtmp.get(i).asDouble();
xtmp = p.findGroup("CALIBRATION").findGroup("Calibration2");
for (i = 1; i < xtmp.size(); i++)
param2[i-1] = xtmp.get(i).asDouble();
xtmp = p.findGroup("CALIBRATION").findGroup("Calibration3");
for (i = 1; i < xtmp.size(); i++)
param3[i-1] = xtmp.get(i).asDouble();
xtmp = p.findGroup("CALIBRATION").findGroup("CalibrationType");
for (i = 1; i < xtmp.size(); i++)
type[i-1] = (unsigned char) xtmp.get(i).asDouble();
xtmp = p.findGroup("CALIBRATION").findGroup("PositionZero");
for (i = 1; i < xtmp.size(); i++)
pos[i-1] = xtmp.get(i).asDouble();
xtmp = p.findGroup("CALIBRATION").findGroup("VelocityZero");
for (i = 1; i < xtmp.size(); i++)
vel[i-1] = xtmp.get(i).asDouble();
xtmp = p.findGroup("HOME").findGroup("PositionHome");
for (i = 1; i < xtmp.size(); i++)
homePos[i-1] = xtmp.get(i).asDouble();
xtmp = p.findGroup("HOME").findGroup("VelocityHome");
for (i = 1; i < xtmp.size(); i++)
homeVel[i-1] = xtmp.get(i).asDouble();
return true;
}
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 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;
}
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 AnalogServer::open(yarp::os::Searchable &config)
{
yTrace() << "AnalogServer param = " << config.toString().c_str();
Property params;
params.fromString(config.toString().c_str());
bool correct=true;
// if(params.check("ports"))
// {
// Bottle *ports=params.find("ports").asList();
// setId(ports->get(0).asString().c_str());
// }
// Verify minimum set of parameters required
if(!params.check("robotName") ) // ?? qui dentro, da dove lo pesco ??
{
correct=false;
yError() << "AnalogServer missing robot Name, check your configuration file!! Quitting\n";
return false;
}
if(params.check("deviceId"))
{
string tmp(params.find("deviceId").asString());// .asList();
// string tmp(deviceId->get(0).asString());
cout << tmp;
setId(tmp);
}
if (params.check("period"))
{
_rate=params.find("period").asInt();
}
else
{
_rate=20;
std::cout<<"Warning: part "<< id <<" using default period ("<<_rate<<")\n";
}
// Read the list of ports
std::string robotName=params.find("robotName").asString().c_str();
std::string root_name;
root_name+="/";
root_name+=robotName;
root_name+= "/" + this->id + "/analog";
rpcPortName = root_name + "/rpc:i";
// port names are optional, do not check for correctness.
if(!params.check("ports"))
{
// if there is no "ports" section take the name of the "skin" group as the only port name
createPort((root_name+":o" ).c_str(), _rate );
// tmpPorts.resize( (size_t) 1);
// tmpPorts[0].offset = 0;
// tmpPorts[0].length = -1;
// tmpPorts[0].port_name = root_name + this->id;
}
else
{
Bottle *ports=params.find("ports").asList();
if (!params.check("total_taxels", "number of taxels of the part"))
return false;
int total_taxels=params.find("total_taxels").asInt();
int nports=ports->size();
int totalT = 0;
std::vector<AnalogPortEntry> tmpPorts;
tmpPorts.resize(nports);
for(int k=0;k<ports->size();k++)
{
Bottle parameters=params.findGroup(ports->get(k).asString().c_str());
if (parameters.size()!=5)
{
yError () <<"check skin port parameters in part description";
yError() << "--> I was expecting "<<ports->get(k).asString().c_str() << " followed by four integers";
return false;
}
int wBase=parameters.get(1).asInt();
int wTop=parameters.get(2).asInt();
int base=parameters.get(3).asInt();
int top=parameters.get(4).asInt();
cout<<"--> "<<wBase<<" "<<wTop<<" "<<base<<" "<<top<<endl;
//check consistenty
if(wTop-wBase != top-base){
cerr<<"Error: check skin port parameters in part description"<<endl;
cerr<<"Numbers of mapped taxels do not match.\n";
return false;
}
int taxels=top-base+1;
tmpPorts[k].length = taxels;
tmpPorts[k].offset = wBase;
tmpPorts[k].port_name = root_name+":o"+string(ports->get(k).asString().c_str());
createPorts(tmpPorts, _rate);
totalT+=taxels;
}
if (totalT!=total_taxels)
{
yError() << "Error total number of mapped taxels does not correspond to total taxels";
return false;
}
}
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 embObjVirtualAnalogSensor::open(yarp::os::Searchable &config)
{
// - first thing to do is verify if the eth manager is available. then i parse info about the eth board.
ethManager = TheEthManager::instance();
if(NULL == ethManager)
{
yFatal() << "embObjVirtualAnalogSensor::open() fails to instantiate ethManager";
return false;
}
if(false == ethManager->verifyEthBoardInfo(config, NULL, boardIPstring, sizeof(boardIPstring)))
{
yError() << "embObjVirtualAnalogSensor::open(): object TheEthManager fails in parsing ETH propertiex from xml file";
return false;
}
// add specific info about this device ...
// - now all other things
std::string str;
if(config.findGroup("GENERAL").find("verbose").asBool())
{
str=config.toString().c_str();
_verbose = true;
}
else
str=" ";
yTrace() << str;
// Read stuff from config file
if(!fromConfig(config))
{
yError() << "embObjAnalogSensor missing some configuration parameter. Check logs and your config file.";
return false;
}
// -- instantiate EthResource etc.
res = ethManager->requestResource2(this, config);
if(NULL == res)
{
yError() << "embObjVirtualAnalogSensor::open() fails because could not instantiate the ethResource for BOARD w/ IP = " << boardIPstring << " ... unable to continue";
return false;
}
// i verify the motion-control because .... in here we use messages of this endpoint ... and we need to verfy in order to send messages.
if(!res->verifyEPprotocol(eoprot_endpoint_motioncontrol))
{
cleanup();
return false;
}
yTrace() << "embObjVirtualAnalogSensor::open(); succefully called for BOARD" << res->getName() << "IP" << res->getIPv4string() << "instantiated correctly";
opened = true;
return true;
}
bool JointTorqueControl::open(yarp::os::Searchable& config)
{
//Workaround: writeStrict option is not documented but
// is necessary for getting correctly working
// single joint streaming, hardcoding it to on
yarp::os::Property pass_through_controlboard_config;
pass_through_controlboard_config.fromString(config.toString());
pass_through_controlboard_config.put("writeStrict","on");
PassThroughControlBoard::open(pass_through_controlboard_config);
this->getAxes(&axes);
hijackingTorqueControl.assign(axes,false);
controlModesBuffer.resize(axes);
motorParameters.resize(axes);
jointTorqueLoopGains.resize(axes);
measuredJointPositions.resize(axes,0.0);
measuredJointVelocities.resize(axes,0.0);
measuredMotorVelocities.resize(axes,0.0);
desiredJointTorques.resize(axes,0.0);
measuredJointTorques.resize(axes,0.0);
measuredJointPositionsTimestamps.resize(axes,0.0);
jointControlOutput.resize(axes,0.0);
jointTorquesError.resize(axes,0.0);
oldJointTorquesError.resize(axes,0.0);
derivativeJointTorquesError.resize(axes,0.0);
integralJointTorquesError.resize(axes,0.0);
integralState.resize(axes,0.0);
jointControlOutputBuffer.resize(axes,0.0);
//Start control thread
this->setRate(config.check("controlPeriod",10,"update period of the torque control thread (ms)").asInt());
//Load Gains configurations
bool ret = this->loadGains(config);
//Load coupling matrices
couplingMatrices.reset(this->axes);
ret = ret && this->loadCouplingMatrix(config,couplingMatrices,"FROM_MOTORS_TO_JOINTS_KINEMATIC_COUPLINGS");
couplingMatricesFirmware.reset(this->axes);
ret = ret && this->loadCouplingMatrix(config,couplingMatricesFirmware,"FROM_MOTORS_TO_JOINTS_KINEMATIC_COUPLINGS_FIRMWARE");
std::cerr << "fromJointTorquesToMotorTorques matrix" << std::endl;
std::cerr << couplingMatrices.fromJointTorquesToMotorTorques << std::endl;
std::cerr << "fromJointVelocitiesToMotorVelocities matrix " << std::endl;
std::cerr << couplingMatrices.fromJointVelocitiesToMotorVelocities << std::endl;
std::cerr << "fromJointTorquesToMotorTorques matrix firmware" << std::endl;
std::cerr << couplingMatricesFirmware.fromJointTorquesToMotorTorques << std::endl;
std::cerr << "fromJointVelocitiesToMotorVelocities matrix firmware" << std::endl;
std::cerr << couplingMatricesFirmware.fromJointVelocitiesToMotorVelocities << std::endl;
std::cerr << "fromMotorTorquesToJointTorques matrix firmware" << std::endl;
std::cerr << couplingMatricesFirmware.fromMotorTorquesToJointTorques << std::endl;
streamingOutput = config.check("streamingOutput");
std::cerr << "streamingOutput = " << streamingOutput << std::endl;
if (streamingOutput)
{
ret = ret && config.check("name");
ret = ret && config.find("name").isString();
partName = config.find("name").asString();
portForStreamingPWM.open(partName + "/output_pwms");
portForReadingRefTorques.open(partName +"/input_torques");
}
if( ret )
{
ret = ret && this->start();
}
return ret;
}
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 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();
}