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;
}
bool yarp::dev::KinectDeviceDriverClient::open(yarp::os::Searchable& config){
string localPortPrefix,remotePortPrefix;
_inUserSkeletonPort = _outPort = NULL;
_skeletonData = new KinectSkeletonData();
if(config.check("localPortPrefix")) localPortPrefix = config.find("localPortPrefix").asString();
else {
printf("\t- Error: localPortPrefix element not found in PolyDriver.\n");
return false;
}
if(config.check("remotePortPrefix")) remotePortPrefix = config.find("remotePortPrefix").asString();
else {
printf("\t- Error: remotePortPrefix element not found in PolyDriver.\n");
return false;
}
Network yarp;
string remotePortIn = remotePortPrefix+":i";
if(!yarp.exists(remotePortIn.c_str())){
printf("\t- Error: remote port not found. (%s)\n\t Check if KinectDeviceDriverServer is running.\n",remotePortIn.c_str());
return false;
}
if(!connectPorts(remotePortPrefix,localPortPrefix)) {
printf("\t- Error: Could not connect or create ports.\n");
return false;
}
//_portMod = new PortCtrlMod();
//_portMod->setInterfaceDriver(this);
_inUserSkeletonPort->useCallback(*this);
_inDepthMapPort->useCallback(*this);
_inImageMapPort->useCallback(*this);
return true;
}
bool Conspicuity::open(yarp::os::Searchable& config){
_sizeConsp = config.check("numCenterSurroundScales",
Value(3),
"Number of center surround scale levels (gaussian pyramide size = numScales + 2) (int).").asInt();
if (_sizeConsp < 1)
_sizeConsp = 1;
_sizePyr = _sizeConsp + 2;
//_prtRgb.open("/rgb");
/* filterName = config.check( "filterName",
yarp::os::Value("emd"),
"Name of this instance of the emd filter (string).").asString(); */
return true;
}
bool YarpJointDev::open ( yarp::os::Searchable& config ) {
yarp::os::Value partName = config.find ( "part" );
if ( partName.isNull() || !partName.isString() )
return false;
_chain = boost::shared_ptr<NaoJointChain> (
new NaoJointChain ( std::string ( partName.asString() ) ) );
/// Initializing trajectory speeds to max speed.
for ( unsigned i = 0; i < _chain->GetNumberOfJoints(); ++i )
_refSpeeds.push_back ( 1.0f );
_maxRefSpeed = 1.0f;
return true;
}
bool yarp::dev::LocationsServer::open(yarp::os::Searchable &config)
{
m_local_name.clear();
m_local_name = config.find("local").asString().c_str();
m_ros_enabled = false;
if (m_local_name == "")
{
yError("LocationsServer::open() error you have to provide valid local name");
return false;
}
if (config.check("ROS_enabled"))
{
m_ros_enabled = true;
m_rosNode = new yarp::os::Node("/LocationServer");
m_rosPublisherPort.topic("/locationServerMarkers");
}
if (config.check("locations_file"))
{
std::string location_file = config.find("locations_file").asString();
bool ret = load_locations(location_file);
if (ret) { yInfo() << "Location file" << location_file << "successfully loaded."; }
else { yError() << "Problems opening file" << location_file; }
}
if (config.check("period"))
{
m_period = config.find("period").asInt();
}
else
{
m_period = 10;
yWarning("LocationsServer: using default period of %d ms" , m_period);
}
ConstString local_rpc = m_local_name;
local_rpc += "/rpc";
if (!m_rpc_port.open(local_rpc.c_str()))
{
yError("LocationsServer::open() error could not open rpc port %s, check network", local_rpc.c_str());
return false;
}
m_rpc_port.setReader(*this);
return true;
}
bool yarp::dev::GazeboYarpMultiCameraDriver::open(yarp::os::Searchable& config)
{
//Get gazebo pointers
std::string sensorScopedName(config.find(YarpScopedName.c_str()).asString().c_str());
m_parentSensor = (gazebo::sensors::MultiCameraSensor*)GazeboYarpPlugins::Handler::getHandler()->getSensor(sensorScopedName);
if (!m_parentSensor) {
yError() << "GazeboYarpMultiCameraDriver Error: camera sensor was not found";
return false;
}
m_vertical_flip = config.check("vertical_flip");
m_horizontal_flip = config.check("horizontal_flip");
m_display_timestamp = config.check("display_timestamp");
m_display_time_box = config.check("display_time_box");
m_vertical = config.check("vertical");
m_camera_count = this->m_parentSensor->CameraCount();
for (unsigned int i = 0; i < m_camera_count; ++i) {
m_camera.push_back(m_parentSensor->Camera(i));
if(m_camera[i] == NULL) {
yError() << "GazeboYarpMultiCameraDriver: camera" << i << "pointer is not valid";
return false;
}
m_width.push_back(m_camera[i]->ImageWidth());
m_height.push_back(m_camera[i]->ImageHeight());
m_max_width = std::max(m_max_width, m_width[i]);
m_max_height = std::max(m_max_height, m_height[i]);
m_bufferSize.push_back(3 * m_width[i] * m_height[i]);
m_dataMutex.push_back(new yarp::os::Semaphore());
m_dataMutex[i]->wait();
m_imageBuffer.push_back(new unsigned char[m_bufferSize[i]]);
memset(m_imageBuffer[i], 0x00, m_bufferSize[i]);
m_dataMutex[i]->post();
m_lastTimestamp.push_back(yarp::os::Stamp());
}
// Connect all the cameras only when everything is set up
for (unsigned int i = 0; i < m_camera_count; ++i) {
this->m_updateConnection.push_back(this->m_camera[i]->ConnectNewImageFrame(boost::bind(&yarp::dev::GazeboYarpMultiCameraDriver::captureImage, this, i, _1, _2, _3, _4, _5)));
}
return true;
}
bool DimaxU2C::open(yarp::os::Searchable& config) {
printf("DimaxU2C: open\n");
numJoints = config.check("axes",
yarp::os::Value(DEFAULT_NUM_MOTORS),
"number of motors").asInt();
speeds = new double[numJoints];
accels = new double[numJoints];
int *amap = new int[numJoints];
double *enc = new double[numJoints];
double *zos = new double[numJoints];
for (int i=0;i<numJoints;i++) {
speeds[i]=10;
accels[i]=0;
// for now we are mapping one to one so you pass
// the raw motor position in range 800-2200
// TODO: convert from angle to joint value
amap[i]=i;
enc[i]=1.0;
zos[i]=0.0;
}
printf("Calling initialize: numJoints %d\n",numJoints);
initialize(numJoints, // number of joints/axes
amap, // axes map
enc, // encoder to angles conversion factors
zos // zeros of encoders
);
if (servos) {
printf("Initialise Servo object\n");
servos->init();
return true;
} else {
ACE_OS::fprintf(stderr,"DimaxU2C: No Servo object created\n");
return false;
}
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 Standardizer::configure(yarp::os::Searchable& config) {
bool success = this->IScaler::configure(config);
// set the desired output mean (double)
if(config.find("mean").isDouble() || config.find("mean").isInt()) {
this->setDesiredMean(config.find("mean").asDouble());
success = true;
}
// set the desired output standard deviation (double)
if(config.find("std").isDouble() || config.find("std").isInt()) {
this->setDesiredStd(config.find("std").asDouble());
success = true;
}
return success;
}
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 embObjMais::fromConfig(yarp::os::Searchable &_config)
{
#if defined(EMBOBJMAIS_USESERVICEPARSER)
if(false == parser->parseService(_config, serviceConfig))
{
return false;
}
return true;
#else
Bottle xtmp;
int _period = 0;
// Analog Sensor stuff
Bottle config = _config.findGroup("GENERAL");
if (!extractGroup(config, xtmp, "Period","transmitting period of the sensors", 1))
{
yError() << "embObjMais Using default value = 0 (disabled)";
_period = 0;
}
else
{
_period = xtmp.get(1).asInt();
yDebug() << "embObjMais::fromConfig() detects embObjMais Using value of" << _period;
}
serviceConfig.acquisitionrate = _period;
return true;
#endif
}
bool IFixedSizeMatrixInputLearner::configure(yarp::os::Searchable& config) {
bool success = false;
// set the domain rows (int)
if(config.find("domRows").isInt()) {
this->setDomainRows(config.find("domRows").asInt());
success = true;
}
// set the domain cols (int)
if(config.find("domCols").isInt()) {
this->setDomainCols(config.find("domCols").asInt());
success = true;
}
// set the codomain size (int)
if(config.find("cod").isInt()) {
this->setCoDomainSize(config.find("cod").asInt());
success = true;
}
return success;
}
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 SerialDeviceDriver::open(yarp::os::Searchable& config) {
SerialDeviceDriverSettings config2;
strcpy(config2.CommChannel, config.check("comport",Value("COM3"),"name of the serial channel").asString().c_str());
this->verbose = (config.check("verbose",Value(1),"Specifies if the device is in verbose mode (0/1).").asInt())>0;
config2.SerialParams.baudrate = config.check("baudrate",Value(9600),"Specifies the baudrate at which the communication port operates.").asInt();
config2.SerialParams.xonlim = config.check("xonlim",Value(0),"Specifies the minimum number of bytes in input buffer before XON char is sent. Negative value indicates that default value should be used (Win32)").asInt();
config2.SerialParams.xofflim = config.check("xofflim",Value(0),"Specifies the maximum number of bytes in input buffer before XOFF char is sent. Negative value indicates that default value should be used (Win32). ").asInt();
//RANDAZ: as far as I undesrood, the exit condition for recv() function is NOT readmincharacters || readtimeoutmsec. It is readmincharacters && readtimeoutmsec.
//On Linux. if readmincharacters params is set !=0, recv() may still block even if readtimeoutmsec is expired.
//On Win32, for unknown reason, readmincharacters seems to be ignored, so recv () returns after readtimeoutmsec. Maybe readmincharacters is used if readtimeoutmsec is set to -1?
config2.SerialParams.readmincharacters = config.check("readmincharacters",Value(1),"Specifies the minimum number of characters for non-canonical read (POSIX).").asInt();
config2.SerialParams.readtimeoutmsec = config.check("readtimeoutmsec",Value(100),"Specifies the time to wait before returning from read. Negative value means infinite timeout.").asInt();
// config2.SerialParams.parityenb = config.check("parityenb",Value(0),"Enable/disable parity checking.").asInt();
yarp::os::ConstString temp = config.check("paritymode",Value("EVEN"),"Specifies the parity mode (EVEN, ODD, NONE). POSIX supports even and odd parity. Additionally Win32 supports mark and space parity modes.").asString().c_str();
config2.SerialParams.paritymode = temp.c_str();
config2.SerialParams.ctsenb = config.check("ctsenb",Value(0),"Enable & set CTS mode. Note that RTS & CTS are enabled/disabled together on some systems (RTS/CTS is enabled if either <code>ctsenb</code> or <code>rtsenb</code> is set).").asInt();
config2.SerialParams.rtsenb = config.check("rtsenb",Value(0),"Enable & set RTS mode. Note that RTS & CTS are enabled/disabled together on some systems (RTS/CTS is enabled if either <code>ctsenb</code> or <code>rtsenb</code> is set).\n- 0 = Disable RTS.\n- 1 = Enable RTS.\n- 2 = Enable RTS flow-control handshaking (Win32).\n- 3 = Specifies that RTS line will be high if bytes are available for transmission.\nAfter transmission RTS will be low (Win32).").asInt();
config2.SerialParams.xinenb = config.check("xinenb",Value(0),"Enable/disable software flow control on input.").asInt();
config2.SerialParams.xoutenb = config.check("xoutenb",Value(0),"Enable/disable software flow control on output.").asInt();
config2.SerialParams.modem = config.check("modem",Value(0),"Specifies if device is a modem (POSIX). If not set modem status lines are ignored. ").asInt();
config2.SerialParams.rcvenb = config.check("rcvenb",Value(0),"Enable/disable receiver (POSIX).").asInt();
config2.SerialParams.dsrenb = config.check("dsrenb",Value(0),"Controls whether DSR is disabled or enabled (Win32).").asInt();
config2.SerialParams.dtrdisable = config.check("dtrdisable",Value(0),"Controls whether DTR is disabled or enabled.").asInt();
config2.SerialParams.databits = config.check("databits",Value(7),"Data bits. Valid values 5, 6, 7 and 8 data bits. Additionally Win32 supports 4 data bits.").asInt();
config2.SerialParams.stopbits = config.check("stopbits",Value(1),"Stop bits. Valid values are 1 and 2.").asInt();
if (config.check("line_terminator_char1", "line terminator character for receiveLine(), default '\r'"))
line_terminator_char1 = config.find("line_terminator_char1").asInt();
if (config.check("line_terminator_char2", "line terminator character for receiveLine(), default '\n'"))
line_terminator_char2 = config.find("line_terminator_char2").asInt();
return open(config2);
}
// device driver stuff
bool yarp::dev::OpenNI2DeviceDriverServer::open(yarp::os::Searchable& config) {
// this function is used in case of the Yarp Device being used as server
std::cout << "Starting OpenNI2 YARP Device please wait..." << endl;
string portPrefix;
double mConf;
int dMode, cMode;
bool printMode;
if(config.check("noRGB", "Use only depth sensor")) {
colorON = false;
} else {
colorON = true;
}
if(config.check("noRGBMirror", "enable RGB mirroring")) {
rgbMirrorON = false;
} else {
rgbMirrorON = true;
}
if(config.check("noDepthMirror", "enable depth mirroring")) {
depthMirrorON = false;
} else {
depthMirrorON = true;
}
if(config.check("noUserTracking", "Disable user tracking")) {
userTracking = false;
}
if(config.check("printVideoModes", "Print supported video modes")) {
printMode = true;
} else {
printMode = false;
}
if(config.check("depthVideoMode", "Depth video mode (default=0)")) {
dMode = config.find("depthVideoMode").asInt();
} else {
dMode = 0;
}
if(config.check("colorVideoMode", "Color video mode (default=0)")) {
cMode = config.find("colorVideoMode").asInt();
} else {
cMode = 0;
}
if(config.check("playback", "Play from .oni file")) {
oniPlayback = true;
fileDevice = config.find("playback").asString();
} else {
oniPlayback = false;
}
if(config.check("record", "Record to .oni file")) {
oniRecord = true;
oniOutputFile = config.find("record").asString();
} else {
oniRecord = false;
}
if(config.check("name", "Name for the port prefix (default=/OpenNI2)")) {
portPrefix = config.find("name").asString();
withOpenPorts = true;
openPorts(portPrefix, userTracking, colorON);
} else {
portPrefix = "/OpenNI2";
withOpenPorts = true;
openPorts(portPrefix, userTracking, colorON);
}
if (config.check("minConfidence", "Set minimum confidence (default=0.6)")) {
mConf = config.find("minConfidence").asDouble();
} else {
mConf = MINIMUM_CONFIDENCE;
}
if (config.check("loop", "Set playback to loop")) {
loop = true;
} else {
loop = false;
}
if (config.check("syncFrames", "Synchronize frames")) {
frameSync = true;
} else {
frameSync = false;
}
if (config.check("imageRegistration", "Register Images")) {
imageRegistration = true;
} else {
imageRegistration = false;
}
skeleton = new OpenNI2SkeletonTracker(userTracking, colorON, rgbMirrorON, depthMirrorON, mConf, oniPlayback, fileDevice, oniRecord, oniOutputFile, loop, frameSync, imageRegistration, printMode, dMode, cMode);
if (skeleton->getDeviceStatus() == 0) {
cout << "OpenNI2 Yarp Device started." << endl;
return true;
}
else if (skeleton->getDeviceStatus()!= 0) {
cout << "***ERROR*** Device could not be initialized." << endl;
close();
return false;
}
return true;
}
bool embObjVirtualAnalogSensor::open(yarp::os::Searchable &config)
{
std::string str;
if(config.findGroup("GENERAL").find("Verbose").asInt())
_verbose = true;
if(_verbose)
str=config.toString().c_str();
else
str="\n";
yTrace() << str;
// Read stuff from config file
if(!fromConfig(config))
{
yError() << "embObjAnalogSensor missing some configuration parameter. Check logs and your config file.";
return false;
}
// Tmp variables
Bottle groupEth;
ACE_TCHAR address[64];
ACE_UINT16 port;
// Get both PC104 and EMS ip addresses and port from config file
groupEth = Bottle(config.findGroup("ETH"));
Bottle parameter1( groupEth.find("PC104IpAddress").asString() ); // .findGroup("IpAddress");
port = groupEth.find("CmdPort").asInt(); // .get(1).asInt();
snprintf(_fId.PC104ipAddr.string, sizeof(_fId.PC104ipAddr.string), "%s", parameter1.toString().c_str());
_fId.PC104ipAddr.port = port;
Bottle parameter2( groupEth.find("IpAddress").asString() ); // .findGroup("IpAddress");
snprintf(_fId.EMSipAddr.string, sizeof(_fId.EMSipAddr.string), "%s", parameter2.toString().c_str());
_fId.EMSipAddr.port = port;
sscanf(_fId.EMSipAddr.string,"\"%d.%d.%d.%d", &_fId.EMSipAddr.ip1, &_fId.EMSipAddr.ip2, &_fId.EMSipAddr.ip3, &_fId.EMSipAddr.ip4);
sscanf(_fId.PC104ipAddr.string,"\"%d.%d.%d.%d", &_fId.PC104ipAddr.ip1, &_fId.PC104ipAddr.ip2, &_fId.PC104ipAddr.ip3, &_fId.PC104ipAddr.ip4);
snprintf(_fId.EMSipAddr.string, sizeof(_fId.EMSipAddr.string), "%u.%u.%u.%u:%u", _fId.EMSipAddr.ip1, _fId.EMSipAddr.ip2, _fId.EMSipAddr.ip3, _fId.EMSipAddr.ip4, _fId.EMSipAddr.port);
snprintf(_fId.PC104ipAddr.string, sizeof(_fId.PC104ipAddr.string), "%u.%u.%u.%u:%u", _fId.PC104ipAddr.ip1, _fId.PC104ipAddr.ip2, _fId.PC104ipAddr.ip3, _fId.PC104ipAddr.ip4, _fId.PC104ipAddr.port);
// Debug info
snprintf(_fId.name, sizeof(_fId.name), "embObjAnalogSensor: referred to EMS: %d at address %s\n", _fId.boardNum, address); // Saving User Friendly Id
// Set dummy values
_fId.boardNum = FEAT_boardnumber_dummy;
_fId.ep = eoprot_endpoint_none;
Value val =config.findGroup("ETH").check("Ems",Value(1), "Board number");
if(val.isInt())
_fId.boardNum =val.asInt();
else
{
yError () << "embObjAnalogSensor: EMS Board number identifier not found for IP" << _fId.PC104ipAddr.string;
return false;
}
ethManager = TheEthManager::instance();
if(NULL == ethManager)
{
yFatal() << "Unable to instantiate ethManager";
return false;
}
//N.B.: use a dynamic_cast to extract correct interface when using this pointer
_fId.handle = (this);
/* Once I'm ok, ask for resources, through the _fId struct I'll give the ip addr, port and
* and boradNum to the ethManagerin order to create the ethResource requested.
* I'll Get back the very same sturct filled with other data useful for future handling
* like the EPvector and EPhash_function */
res = ethManager->requestResource(&_fId);
if(NULL == res)
{
yError() << "EMS device not instantiated... unable to continue";
return false;
}
/*IMPORTANT: implement isEpManagedByBoard like every embObj obj when virtaulAnalogSensor will be exist in eo proto!!!!*/
// if(!isEpManagedByBoard())
// {
// yError() << "EMS "<< _fId.boardNum << "is not connected to virtual analog sensor";
// return false;
// }
yTrace() << "EmbObj Virtual Analog Sensor for board "<< _fId.boardNum << "instantiated correctly";
return true;
}
bool 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 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;
}
void setOptions(yarp::os::Searchable& options) {
// switch to subsections if available
yarp::os::Value *val;
if (options.check("PortName",val)||options.check("name",val)) {
ACE_OS::sprintf(_options.portName, "%s", val->asString().c_str());
fprintf(stderr, "testing name: %s\n", val->asString().c_str());
}
if (options.check("NetName",val)||options.check("n",val)) {
ACE_OS::sprintf(_options.networkName, "%s", val->asString().c_str());
}
if (options.check("OutPortName",val)||options.check("out",val)) {
ACE_OS::sprintf(_options.outPortName, "%s", val->asString().c_str());
}
if (options.check("OutNetName",val)||options.check("neto",val)) {
ACE_OS::sprintf(_options.outNetworkName, "%s", val->asString().c_str());
}
if (options.check("RefreshTime",val)||options.check("p",val)) {
_options.refreshTime = val->asInt();
}
if (options.check("PosX",val)||options.check("x",val)) {
_options.posX = val->asInt();
}
if (options.check("PosY",val)||options.check("y",val)) {
_options.posY = val->asInt();
}
if (options.check("Width",val)||options.check("w",val)) {
_options.windWidth = val->asInt();
}
if (options.check("Height",val)||options.check("h",val)) {
_options.windHeight = val->asInt();
}
if (options.check("OutputEnabled",val)) {
_options.outputEnabled = val->asInt();
}
if (options.check("out",val)) {
_options.outputEnabled = true;
}
if (options.check("SaveOptions",val)||options.check("saveoptions",val)) {
_options.outputEnabled = val->asInt();
}
if (options.check("synch"))
{
_options.synch=true;
}
}
// IGenericSensor interface.
bool imu3DM_GX3::open(yarp::os::Searchable &config)
{
bool ret = true;
Value *baudrate, *serial;
if(!config.check("serial", serial))
{
std::cout << "Can't find 'serial' name in config file";
return false;
}
comPortName = serial->toString();
int errNum = 0;
/* open serial */
printf("\n\nSerial opening %s\n\n\n", comPortName.c_str());
fd_ser = ::open(comPortName.c_str(), O_RDWR | O_NOCTTY );
if (fd_ser < 0) {
printf("can't open %s, %s\n", comPortName.c_str(), strerror(errno));
return false;
}
//Get the current options for the port...
struct termios options;
tcgetattr(fd_ser, &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 close_port(int
options.c_oflag = 0; // raw output
options.c_lflag = 0; // raw input
//Time-Outs -- won't work with NDELAY option in the call to open
options.c_cc[VMIN] = 0; // block reading until RX x characers. If x = 0, it is non-blocking.
options.c_cc[VTIME] = 100; // 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)
{ //For error message
printf("Configuring comport failed\n");
return false;
}
this->start();
return true;
}
bool embObjAnalogSensor::fromConfig(yarp::os::Searchable &_config)
{
Bottle xtmp;
int format=0;
// embObj parameters, in ETH group
Value val =_config.findGroup("ETH").check("Ems",Value(1), "Board number");
if(val.isInt())
_fId.boardNumber = val.asInt();
else
{
_as_type=AS_NONE;
yError () << "embObjAnalogSensor: EMS Board number identifier not found\n";
return false;
}
// Analog Sensor stuff
Bottle config = _config.findGroup("GENERAL");
if (!extractGroup(config, xtmp, "Period","transmetting period of the sensor", 1))
{
yError() << "embObjAnalogSensor Using default value = 0 (disabled)";
_period = 0;
_as_type=AS_NONE;
}
else
{
_period = xtmp.get(1).asInt();
yDebug() << "embObjAnalogSensor::fromConfig() detects embObjAnalogSensor Using value of" << _period;
}
if (!extractGroup(config, xtmp, "Channels","Number of channels of the Analog Sensor", 1))
{
yWarning("embObjAnalogSensor: Using default value = 0 (disabled)\n");
_channels = 0;
_period = 0;
_as_type=AS_NONE;
}
else
{
_channels = xtmp.get(1).asInt();
}
if (!extractGroup(config, xtmp, "Format","data format of the Analog Sensor", 1))
{
yWarning("embObjAnalogSensor: Using default value = 0 (disabled)");
_channels = 0;
_period = 0;
_as_type=AS_NONE;
}
else
{
format = xtmp.get(1).asInt();
}
if((_channels==NUMCHANNEL_STRAIN) && (format==FORMATDATA_STRAIN))
{
_as_type=AS_STRAIN;
}
else if((_channels==NUMCHANNEL_MAIS) && (format==FORMATDATA_MAIS))
{
_as_type=AS_MAIS;
}
else
{
_as_type=AS_NONE;
yError() << "embObjAnalogSensor incorrect config!channels="<< _channels <<" format="<< format;
return false;
}
if(AS_STRAIN == _as_type)
{
if (!extractGroup(config, xtmp, "UseCalibration","Calibration parameters are needed", 1))
{
return false;
}
else
{
_useCalibration = xtmp.get(1).asInt();
}
}
return true;
}
bool CanBusInertialMTB::open(yarp::os::Searchable& config)
{
std::vector<int> canAddresses;
bool correct = this->validateConf(config,canAddresses);
if (!correct)
{
yError("CanBusInertialMTB: Insufficient parameters to CanBusInertialMTB\n");
return false;
}
//Read sensor period for all sensors
int sensorPeriod = CANBUS_INERTIAL_MTB_DEFAULT_SENSOR_PERIOD;
if (config.check("sensorPeriod"))
{
int int_sensorPeriod = config.find("sensorPeriod").asInt();
if( int_sensorPeriod < 1 || int_sensorPeriod > 255 )
{
yError("CanBusInertialMTB: sensorPeriod is lower than 1 or bigger then 255\n");
return false;
}
sensorPeriod = int_sensorPeriod;
}
//Parse sensor type and address of all readed sensors
this->boards.resize(canAddresses.size());
this->nrOfTotalChannels = 0;
for(size_t board=0; board < this->boards.size(); board++ )
{
this->boards[board].boardId = canAddresses[board];
if( config.find("sensorType").asString() == "acc" )
{
this->boards[board].enabledSensors = CANBUS_INERTIAL_MTB_INTERNAL_ACC_BIT;
this->boards[board].enabledGyro = false;
this->boards[board].nrOfChannels = 3;
this->boards[board].vectorOffset = this->nrOfTotalChannels;
this->nrOfTotalChannels += this->boards[board].nrOfChannels;
}
else if( config.find("sensorType").asString() == "extAccAndGyro" )
{
this->boards[board].enabledSensors = CANBUS_INERTIAL_MTB_EXTERNAL_GYRO_BIT |
CANBUS_INERTIAL_MTB_EXTERNAL_ACC_BIT;
this->boards[board].enabledGyro = true;
this->boards[board].nrOfChannels = 6;
this->boards[board].vectorOffset = this->nrOfTotalChannels;
this->nrOfTotalChannels += this->boards[board].nrOfChannels;
}
else
{
yError("CanBusInertialMTB: unknown sensorType %s",config.find("sensorType").asString().c_str());
return false;
}
}
if (config.check("period")==true)
{
int period=10;
period=config.find("period").asInt();
setPeriod((double)period/1000.0);
}
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", CANBUS_INERTIAL_MTB_CAN_DRIVER_BUFFER_SIZE);
prop.put("canRxQueueSize", CANBUS_INERTIAL_MTB_CAN_DRIVER_BUFFER_SIZE);
pCanBus=0;
pCanBufferFactory=0;
//open the can driver
driver.open(prop);
if (!driver.isValid())
{
yError("Unable to open CanBusInertialMTB check parameters\n");
return false;
}
driver.view(pCanBus);
if (!pCanBus)
{
yError("Unable to open CAN device not available\n");
return false;
}
driver.view(pCanBufferFactory);
outBuffer=pCanBufferFactory->createBuffer(CANBUS_INERTIAL_MTB_CAN_DRIVER_BUFFER_SIZE);
inBuffer=pCanBufferFactory->createBuffer(CANBUS_INERTIAL_MTB_CAN_DRIVER_BUFFER_SIZE);
//select the communication speed
pCanBus->canSetBaudRate(0); //default 1MB/s
// open the can mask for the desired canDeviceId
// messages class is 0x500
for(size_t board=0; board < this->boards.size(); board++ )
{
unsigned short boardId = this->boards[board].boardId;
if( this->boards[board].enabledGyro )
{
pCanBus->canIdAdd((CAN_MSG_CLASS_ACC_GYRO)+(boardId<<4)+MSG_TYPE_GYRO);
}
pCanBus->canIdAdd((CAN_MSG_CLASS_ACC_GYRO)+(boardId<<4)+MSG_TYPE_ACC);
}
data.resize(this->nrOfTotalChannels);
data.zero();
privateData.resize(this->nrOfTotalChannels);
privateData.zero();
sharedStatus.resize(this->nrOfTotalChannels,IAnalogSensor::AS_OK);
privateStatus.resize(this->nrOfTotalChannels,IAnalogSensor::AS_OK);
PeriodicThread::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 CanBusVirtualAnalogSensor::sensor_start(yarp::os::Searchable& analogConfig)
{
fprintf(stderr, "--> Initializing analog device %s\n", analogConfig.find("deviceId").toString().c_str());
/*
unsigned int canMessages=0;
unsigned id = 0x200 + boardId;
if (analogConfig.check("period"))
{
int period=analogConfig.find("period").asInt();
CanMessage &msg=outBuffer[0];
msg.setId(id);
msg.getData()[0]=0x08;
msg.getData()[1]=period;
msg.setLen(2);
canMessages=0;
pCanBus->canWrite(outBuffer, 1, &canMessages);
}
//init message for mais board
if (channelsNum==16 && dataFormat==ANALOG_FORMAT_8_BIT)
{
CanMessage &msg=outBuffer[0];
msg.setId(id);
msg.getData()[0]=0x07;
msg.getData()[1]=0x00;
msg.setLen(2);
canMessages=0;
pCanBus->canWrite(outBuffer, 1, &canMessages);
}
//init message for strain board
else if (channelsNum==6 && dataFormat==ANALOG_FORMAT_16_BIT)
{
//calibrated astrain board
if (useCalibration)
{
//get the full scale values from the strain board
for (int ch=0; ch<6; ch++)
{
bool b=false;
int attempts = 0;
while(attempts<15)
{
b = readFullScaleAnalog(ch);
if (b==true)
{
if (attempts>0) fprintf(stderr, "*** WARNING: Trying to get fullscale data from sensor 0x%X: channel recovered (ch:%d)\n", boardId, ch);
break;
}
attempts++;
yarp::os::Time::delay(0.020);
}
if (attempts>=15)
{
fprintf(stderr, "*** ERROR: Trying to get fullscale data from sensor 0x%X: all attempts failed (ch:%d)\n", boardId, ch);
}
}
// debug messages
#if 1
fprintf(stderr, "Sensor Fullscale Id %#4X: ",boardId);
fprintf(stderr, " %f ", scaleFactor[0]);
fprintf(stderr, " %f ", scaleFactor[1]);
fprintf(stderr, " %f ", scaleFactor[2]);
fprintf(stderr, " %f ", scaleFactor[3]);
fprintf(stderr, " %f ", scaleFactor[4]);
fprintf(stderr, " %f ", scaleFactor[5]);
fprintf(stderr, " \n ");
#endif
// start the board
CanMessage &msg=outBuffer[0];
msg.setId(id);
msg.getData()[0]=0x07;
msg.getData()[1]=0x00;
msg.setLen(2);
canMessages=0;
pCanBus->canWrite(outBuffer, 1, &canMessages);
}
//not calibrated strain board
else
{
CanMessage &msg=outBuffer[0];
msg.setId(id);
msg.getData()[0]=0x07;
msg.getData()[1]=0x03;
msg.setLen(2);
canMessages=0;
pCanBus->canWrite(outBuffer, 1, &canMessages);
}
}*/
return true;
}
bool FakeLaser::open(yarp::os::Searchable& config)
{
info = "Fake Laser device for test/debugging";
device_status = DEVICE_OK_STANBY;
#ifdef LASER_DEBUG
yDebug("%s\n", config.toString().c_str());
#endif
if (config.check("help"))
{
yInfo("Some examples:");
yInfo("yarpdev --device fakeLaser --help");
yInfo("yarpdev --device Rangefinder2DWrapper --subdevice fakeLaser --period 10 --name /ikart/laser:o --test no_obstacles");
yInfo("yarpdev --device Rangefinder2DWrapper --subdevice fakeLaser --period 10 --name /ikart/laser:o --test use_pattern");
yInfo("yarpdev --device Rangefinder2DWrapper --subdevice fakeLaser --period 10 --name /ikart/laser:o --test use_mapfile --map_file mymap.map");
yInfo("yarpdev --device Rangefinder2DWrapper --subdevice fakeLaser --period 10 --name /ikart/laser:o --test use_mapfile --map_file mymap.map --localization_port /fakeLaser/location:i");
yInfo("yarpdev --device Rangefinder2DWrapper --subdevice fakeLaser --period 10 --name /ikart/laser:o --test use_mapfile --map_file mymap.map --localization_client /fakeLaser/localizationClient");
return false;
}
bool br = config.check("GENERAL");
if (br != false)
{
yarp::os::Searchable& general_config = config.findGroup("GENERAL");
period = general_config.check("Period", Value(50), "Period of the sampling thread").asInt32() / 1000.0;
}
string string_test_mode = config.check("test", Value(string("use_pattern")), "string to select test mode").asString();
if (string_test_mode == "no_obstacles") { m_test_mode = NO_OBSTACLES; }
else if (string_test_mode == "use_pattern") { m_test_mode = USE_PATTERN; }
else if (string_test_mode == "use_mapfile") { m_test_mode = USE_MAPFILE; }
else { yError() << "invalid/unknown value for param 'test'"; return false; }
min_distance = 0.1; //m
max_distance = 3.5; //m
min_angle = 0; //degrees
max_angle = 360; //degrees
resolution = 1.0; //degrees
sensorsNum = (int)((max_angle-min_angle)/resolution);
laser_data.resize(sensorsNum);
if (m_test_mode == USE_MAPFILE)
{
string map_file;
if (config.check("map_file"))
{
map_file = config.check("map_file",Value(string("map.yaml")),"map filename").asString();
}
else
{
yError() << "Missing map_file";
return false;
}
bool ret = m_map.loadFromFile(map_file);
if (ret == false)
{
yError() << "A problem occurred while opening:" << map_file;
return false;
}
if (config.check("localization_port"))
{
string localization_port_name = config.check("localization_port", Value(string("/fakeLaser/location:i")), "name of localization input port").asString();
m_loc_port = new yarp::os::BufferedPort<yarp::os::Bottle>;
m_loc_port->open(localization_port_name.c_str());
yInfo() << "Robot localization will be obtained from port" << localization_port_name;
m_loc_mode = LOC_FROM_PORT;
}
else if (config.check("localization_client"))
{
Property loc_options;
string localization_device_name = config.check("localization_client", Value(string("/fakeLaser/localizationClient")), "local name of localization client device").asString();
loc_options.put("device", "localization2DClient");
loc_options.put("local", localization_device_name.c_str());
loc_options.put("remote", "/localizationServer");
loc_options.put("period", 10);
m_pLoc = new PolyDriver;
if (m_pLoc->open(loc_options) == false)
{
yError() << "Unable to open localization driver";
return false;
}
m_pLoc->view(m_iLoc);
if (m_iLoc == nullptr)
{
yError() << "Unable to open localization interface";
return false;
}
yInfo() << "Robot localization will be obtained from localization_client" << localization_device_name;
m_loc_mode = LOC_FROM_CLIENT;
}
else
{
yInfo() << "No localization method selected. Robot location set to 0,0,0";
m_loc_mode = LOC_NOT_SET;
}
m_loc_x=0;
m_loc_y=0;
m_loc_t=0;
max_distance = 8; //m
}
yInfo("Starting debug mode");
yInfo("max_dist %f, min_dist %f", max_distance, min_distance);
yInfo("max_angle %f, min_angle %f", max_angle, min_angle);
yInfo("resolution %f", resolution);
yInfo("sensors %d", sensorsNum);
yInfo("test mode: %d", m_test_mode);
return PeriodicThread::start();
}
bool embObjVirtualAnalogSensor::fromConfig(yarp::os::Searchable &_config)
{
Bottle xtmp;
// embObj parameters, in ETH group
Value val =_config.findGroup("ETH").check("Ems",Value(1), "Board number");
if(val.isInt())
{
_fId.boardNum =val.asInt();
}
else
{
yError () << "embObjVirtualAnalogSensor: EMS Board number identifier not found\n";
return false;
}
// Analog Sensor stuff
Bottle config = _config.findGroup("GENERAL");
if (!extractGroup(config, xtmp, "Channels","Number of channels of the Analog Sensor", 1))
{
yError() << "embObjVirtualAnalogSensor: Missing channel number... aborting";
_channels = 0;
return false;
}
else
{
_channels = xtmp.get(1).asInt();
}
// alloc vector for keeping conversion values
_fullscale = allocAndCheck<double>(_channels);
_resolution = allocAndCheck<double>(_channels);
if (!extractGroup(config, xtmp, "UseCalibration","Calibration parameters are needed", 1))
{
fprintf(stderr, "embObjVirtualAnalogSensor: Using default value = 0 (Don't use calibration)\n");
_useCalibration = 0;
}
else
{
_useCalibration = xtmp.get(1).asInt();
}
if (!extractGroup(config, xtmp, "MaxValue","full scale value for this measure", _channels))
{
yError() << "Missing conversion factor!! Aborting...";
return false;
}
else
{
if(_verbose)
yDebug() << "embObjVirtualAnalogSensor, fullscales from config file: ";
for (int ch = 1; ch < xtmp.size(); ch++)
{
_fullscale[ch-1] = xtmp.get(ch).asDouble();
if(_verbose)
yDebug() << "ch " << ch << ": " << _fullscale[ch-1];
}
}
if (!extractGroup(config, xtmp, "Resolution","Number of bytes used for this measure", _channels))
{
yError() << "Missing resolution!! Aborting...";
return false;
}
else
{
if(_verbose)
yDebug() << "embObjVirtualAnalogSensor, resolutions from config file: ";
for (int ch = 1; ch < xtmp.size(); ch++)
{
_resolution[ch-1] = (double) (1 << (xtmp.get(ch).asInt()-1) );
if(_verbose)
yDebug() << "ch " << ch << ": " << _resolution[ch-1];
}
}
return true;
};
/*
EBaudUnknown = -1, // Unknown
EBaud110 = CBR_110, // 110 bits/sec
EBaud300 = CBR_300, // 300 bits/sec
EBaud600 = CBR_600, // 600 bits/sec
EBaud1200 = CBR_1200, // 1200 bits/sec
EBaud2400 = CBR_2400, // 2400 bits/sec
EBaud4800 = CBR_4800, // 4800 bits/sec
EBaud9600 = CBR_9600, // 9600 bits/sec
EBaud14400 = CBR_14400, // 14400 bits/sec
EBaud19200 = CBR_19200, // 19200 bits/sec (default)
EBaud38400 = CBR_38400, // 38400 bits/sec
EBaud56000 = CBR_56000, // 56000 bits/sec
EBaud57600 = CBR_57600, // 57600 bits/sec
EBaud115200 = CBR_115200, // 115200 bits/sec
EBaud128000 = CBR_128000, // 128000 bits/sec
EBaud256000 = CBR_256000, // 256000 bits/sec
*/
bool DynamixelAX12::open(yarp::os::Searchable& config)
{
char comport[100];
strcpy(comport, config.check("comport",Value("COM10"),"name of the serial channel").asString().c_str());
int _baudrate = config.check("baudrate", Value(57600), "Specifies the baudrate at which the communication port operates.").asInt();
/*
FtdiDeviceSettings ftdiSetting;
strcpy(ftdiSetting.description, config.check("FTDI_DESCRIPTION", Value("FT232R USB UART"), "Ftdi device description").asString().c_str());
strcpy(ftdiSetting.serial, config.check("FTDI_SERIAL", Value("A7003MhG"), "Ftdi device serial").asString().c_str());
strcpy(ftdiSetting.manufacturer, config.check("FTDI_MANUFACTURER", Value("FTDI"), "Ftdi device manufacturer").asString().c_str());
ftdiSetting.baudrate = config.check("baudrate", Value(57600), "Specifies the baudrate at which the communication port operates.").asInt();
ftdiSetting.vendor = config.check("ftdivendor", Value(0x0403), "USB device vendor. 0x0403 normally. Can be found by lsusb on linux").asInt();
ftdiSetting.product = config.check("ftdiproduct", Value(0x6001), "USB device product number. 0x6001 normally. Can be found by lsusb on linux").asInt();
ftdiSetting.flowctrol = config.check("flowctrl", Value(SIO_DISABLE_FLOW_CTRL), "flow control to use. Should be SIO_DISABLE_FLOW_CTRL = 0x0, SIO_RTS_CTS_HS = 0x1 << 8, SIO_DTR_DSR_HS = 0x2 << 8, or SIO_XON_XOFF_HS = 0x4 << 8").asInt();
ftdiSetting.write_chunksize = 3;
ftdiSetting.read_chunksize = 256;
*/
BaudRateType baudrate;
/*
BAUD50, //POSIX ONLY
BAUD75, //POSIX ONLY
BAUD110,
BAUD134, //POSIX ONLY
BAUD150, //POSIX ONLY
BAUD200, //POSIX ONLY
BAUD300,
BAUD600,
BAUD1200,
BAUD1800, //POSIX ONLY
BAUD2400,
BAUD4800,
BAUD9600,
BAUD14400, //WINDOWS ONLY
BAUD19200,
BAUD38400,
BAUD56000, //WINDOWS ONLY
BAUD57600,
BAUD76800, //POSIX ONLY
BAUD115200,
BAUD128000, //WINDOWS ONLY
BAUD256000 //WINDOWS ONLY
*/
switch(_baudrate)
{
case 50:
baudrate = BAUD50;
break;
case 75:
baudrate = BAUD75;
break;
case 110:
baudrate = BAUD110;
break;
case 134:
baudrate = BAUD134;
break;
case 150:
baudrate = BAUD150;
break;
case 300:
baudrate = BAUD300;
break;
case 600:
baudrate = BAUD600;
break;
case 1200:
baudrate = BAUD1200;
break;
case 1800:
baudrate = BAUD1800;
break;
case 2400:
baudrate = BAUD2400;
break;
case 4800:
baudrate = BAUD4800;
break;
case 9600:
baudrate = BAUD9600;
break;
case 14400:
baudrate = BAUD14400;
break;
case 19200:
baudrate = BAUD19200;
break;
case 38400:
baudrate = BAUD38400;
break;
case 56000:
baudrate = BAUD56000;
break;
case 57600:
baudrate = BAUD57600;
break;
case 76800:
baudrate = BAUD76800;
break;
case 115200:
baudrate = BAUD115200;
break;
case 128000:
baudrate = BAUD128000;
break;
case 256000:
baudrate = BAUD256000;
break;
default:
return false;
}
ACE_TRACE("DynamixelAX12::initialize");
ACE_OS::printf("Opening DynamixelAX12 Device\n");
port->setPortName(QString(comport));
port->setBaudRate(baudrate);
port->setFlowControl(FLOW_OFF);
port->setParity(PAR_NONE);
port->setDataBits(DATA_8);
port->setStopBits(STOP_1);
//set timeouts to 500 ms
port->setTimeout(1000);
if(!port->open(QIODevice::ReadWrite| QIODevice::Unbuffered))
{
return false;
}
int retCode;
if(port->isOpen())
{
ACE_OS::printf("OK. Port is open\n");
}
deviceOpen = true; // Only set to be able to do close()
// Everything ready to rumble
Time::delay(3);
return true;
}
