bool yarp::dev::OpenNI2DeviceDriverClient::open(yarp::os::Searchable& config){
string localPortPrefix,remotePortPrefix;
inUserSkeletonPort = outPort = NULL;
skeletonData = new OpenNI2SkeletonData();
if(config.check("localName")) localPortPrefix = config.find("localName").asString();
else {
printf("\t- Error: localName element not found in PolyDriver.\n");
return false;
}
if(config.check("remoteName")) remotePortPrefix = config.find("remoteName").asString();
else {
printf("\t- Error: remoteName element not found in PolyDriver.\n");
return false;
}
string remotePortIn = remotePortPrefix+":i";
if(!NetworkBase::exists(remotePortIn.c_str())){
printf("\t- Error: remote port not found. (%s)\n\t Check if OpenNI2DeviceDriverServer is running.\n",remotePortIn.c_str());
return false;
}
if(!connectPorts(remotePortPrefix,localPortPrefix)) {
printf("\t- Error: Could not connect or create ports.\n");
return false;
}
inUserSkeletonPort->useCallback(*this);
inDepthFramePort->useCallback(*this);
inImageFramePort->useCallback(*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");
#if GAZEBO_MAJOR_VERSION >= 7
m_camera_count = this->m_parentSensor->CameraCount();
#else
m_camera_count = this->m_parentSensor->GetCameraCount();
#endif
for (unsigned int i = 0; i < m_camera_count; ++i) {
#if GAZEBO_MAJOR_VERSION >= 7
m_camera.push_back(m_parentSensor->Camera(i));
#else
m_camera.push_back(m_parentSensor->GetCamera(i));
#endif
if(m_camera[i] == NULL) {
yError() << "GazeboYarpMultiCameraDriver: camera" << i << "pointer is not valid";
return false;
}
#if GAZEBO_MAJOR_VERSION >= 7
m_width.push_back(m_camera[i]->ImageWidth());
m_height.push_back(m_camera[i]->ImageHeight());
#else
m_width.push_back(m_camera[i]->GetImageWidth());
m_height.push_back(m_camera[i]->GetImageHeight());
#endif
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 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;
}
bool PortAudioDeviceDriver::open(yarp::os::Searchable& config)
{
driverConfig.rate = config.check("rate",Value(0),"audio sample rate (0=automatic)").asInt32();
driverConfig.samples = config.check("samples",Value(0),"number of samples per network packet (0=automatic). For chunks of 1 second of recording set samples=rate. Channels number is handled internally.").asInt32();
driverConfig.channels = config.check("channels",Value(0),"number of audio channels (0=automatic, max is 2)").asInt32();
driverConfig.wantRead = (bool)config.check("read","if present, just deal with reading audio (microphone)");
driverConfig.wantWrite = (bool)config.check("write","if present, just deal with writing audio (speaker)");
driverConfig.deviceNumber = config.check("id",Value(-1),"which portaudio index to use (-1=automatic)").asInt32();
if (!(driverConfig.wantRead||driverConfig.wantWrite))
{
driverConfig.wantRead = driverConfig.wantWrite = true;
}
if (config.check("loopback","if present, send audio read from microphone immediately back to speaker"))
{
printf ("WARN: loopback not yet implemented\n");
loopBack = true;
}
if (config.check("render_mode_append"))
{
renderMode = RENDER_APPEND;
}
if (config.check("render_mode_immediate"))
{
renderMode = RENDER_IMMEDIATE;
}
return open(driverConfig);
}
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 JoypadControlClient::open(yarp::os::Searchable& config)
{
if(config.check("help"))
{
yInfo() << "parameter:\n\n" <<
"local - prefix of the local port\n" <<
"remote - prefix of the port provided to and opened by JoypadControlServer\n";
}
if(!config.check("local"))
{
yError() << "JoypadControlClient: unable to 'local' parameter.. check configuration file";
return false;
}
m_local = config.find("local").asString();
if(!m_rpcPort.open(m_local + "/rpc:o"))
{
yError() << "JoypadControlClient: unable to open rpc port..";
return false;
}
yInfo() << "rpc port opened.. starting the handshake";
if(!config.check("remote"))
{
yError() << "JoypadControlClient: unable to find the 'remote' parameter.. check configuration file";
return false;
}
m_remote = config.find("remote").asString();
if(!yarp::os::NetworkBase::connect(m_local + "/rpc:o", m_remote + "/rpc:i"))
{
yError() << "handshake failed.. unable to connect to remote port" << m_remote + "/rpc:i";
return false;
}
yInfo() << "handshake succeded! retrieving info";
if(!getJoypadInfo())
{
yError() << "unable to get joypad info..";
return false;
}
watchdog.start();
return true;
}
bool DeviceGroup::open(yarp::os::Searchable& config) {
if (implementation==NULL) {
implementation = new DeviceGroupHelper;
}
if (implementation==NULL) {
printf("Out of memory\n");
return false;
}
if (config.check("part","a list of section names, with each section containing a device")) {
Bottle bot = config.findGroup("part").tail();
printf("Assembly of: %s\n", bot.toString().c_str());
for (int i=0; i<bot.size(); i++) {
ConstString name = bot.get(i).asString();
printf(" %s -> %s\n", name.c_str(),
config.findGroup(name).toString().c_str());
bool result = HELPER(implementation).add(name,
config.findGroup(name));
if (!result) {
HELPER(implementation).close();
return false;
}
}
return true;
}
return false;
}
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 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 JointTorqueControl::loadGains(yarp::os::Searchable& config)
{
if( !config.check("TRQ_PIDS") )
{
yError("No TRQ_PIDS group find, initialization failed");
return false;
}
yarp::os::Bottle & bot = config.findGroup("TRQ_PIDS");
bool gains_ok = true;
gains_ok = gains_ok && checkVectorExistInConfiguration(bot,"kff",this->axes);
gains_ok = gains_ok && checkVectorExistInConfiguration(bot,"kp",this->axes);
gains_ok = gains_ok && checkVectorExistInConfiguration(bot,"ki",this->axes);
gains_ok = gains_ok && checkVectorExistInConfiguration(bot,"maxPwm",this->axes);
gains_ok = gains_ok && checkVectorExistInConfiguration(bot,"maxInt",this->axes);
gains_ok = gains_ok && checkVectorExistInConfiguration(bot,"stictionUp",this->axes);
gains_ok = gains_ok && checkVectorExistInConfiguration(bot,"stictionDown",this->axes);
gains_ok = gains_ok && checkVectorExistInConfiguration(bot,"bemf",this->axes);
gains_ok = gains_ok && checkVectorExistInConfiguration(bot,"coulombVelThr",this->axes);
gains_ok = gains_ok && checkVectorExistInConfiguration(bot,"frictionCompensation",this->axes);
if( !gains_ok )
{
yError("TRQ_PIDS group is missing some information, initialization failed");
return false;
}
for(int j=0; j < this->axes; j++)
{
jointTorqueLoopGains[j].reset();
motorParameters[j].reset();
jointTorqueLoopGains[j].kp = bot.find("kp").asList()->get(j).asDouble();
jointTorqueLoopGains[j].ki = bot.find("ki").asList()->get(j).asDouble();
jointTorqueLoopGains[j].max_pwm = bot.find("maxPwm").asList()->get(j).asDouble();
jointTorqueLoopGains[j].max_int = bot.find("maxInt").asList()->get(j).asDouble();
motorParameters[j].kff = bot.find("kff").asList()->get(j).asDouble();
motorParameters[j].kcp = bot.find("stictionUp").asList()->get(j).asDouble();
motorParameters[j].kcn = bot.find("stictionDown").asList()->get(j).asDouble();
motorParameters[j].kv = bot.find("bemf").asList()->get(j).asDouble();
motorParameters[j].coulombVelThr = bot.find("coulombVelThr").asList()->get(j).asDouble();
motorParameters[j].frictionCompensation = bot.find("frictionCompensation").asList()->get(j).asDouble();
if (motorParameters[j].frictionCompensation > 1 || motorParameters[j].frictionCompensation < 0) {
motorParameters[j].frictionCompensation = 0;
yWarning("[TRQ_PIDS] frictionCompensation parameter is outside the admissible range [0, 1]. FrictionCompensation reset to 0.0");
}
}
return true;
}
bool yarp::dev::IJoypadController::parseActions(const yarp::os::Searchable& cfg, int* count)
{
int dummy;
size_t i;
int& actCount = count ? *count : dummy;
if(!cfg.check(buttActionGroupName))
{
myInfo() << "no actions found in the configuration file (no" << buttActionGroupName << "group found)";
actCount = 0;
return true;
}
Bottle& actionsGroup = cfg.findGroup(buttActionGroupName);
if(!actionsGroup.size())
{
myError() << "no action found under" << buttActionGroupName << "group";
actCount = 0;
return false;
}
for(i = 1; i < actionsGroup.size(); i++)
{
if(!actionsGroup.get(i).isList())
{
yDebug() << "error parsing cfg";
return false;
}
Bottle& keyvalue = *actionsGroup.get(i).asList();
yDebug() << keyvalue.toString();
unsigned int buttonCount;
if(!this->getButtonCount(buttonCount))
{
myError() << "unable to get button count while parsing the actions";
actCount = 0;
return false;
}
if(!keyvalue.get(0).isInt32() ||
keyvalue.get(0).asInt32() < 0 ||
(unsigned int) keyvalue.get(0).asInt32() > buttonCount-1 ||
!keyvalue.get(1).isString())
{
myError() << "Button's actions parameters must be in the format 'unsigned int string' and the button id must be in range";
actCount = 0;
return false;
}
myInfo() << "assigning actions" << keyvalue.get(1).asString() << "to button" << keyvalue.get(0).asInt32();
m_actions[keyvalue.get(0).asInt32()] = keyvalue.get(1).asString();
}
actCount = i;
myInfo() << actCount << "action parsed succesfully";
return true;
}
bool OptFlowEMD::configure (yarp::os::Searchable &config){
// configuration
_alpha = (float)config.check("alpha",
yarp::os::Value(0.28),
"EMD delay parameter (double)").asDouble();
_threshold =(float)config.check("threshold",
yarp::os::Value(80.0),
"EMD motion threshold (double)").asDouble();
_thresholdSquared = pow(_threshold,2.0f);
_constrain = config.check("constrain", yarp::os::Value(0),
"Constrain salience values to <=constrainValue? (1/0)").asInt()!=0;
_constrainValue = (float)config.check("constrainValue", yarp::os::Value(255.0),
"Constrain salience values to this value if constrain = 1 (double)").asDouble();
_constrainValueSquared = pow(_constrainValue,2.0f);
_scale = (float)config.check("scale",yarp::os::Value(1.0),
"Scale salience values (double).").asDouble();
// TODO reimplement distortion for cv version
//if (config.check("distortion", "2x2 distortion matrix in list format: topLeft, "
// "topRight, bottomLeft, bottomRight. Default = 1 0 0 1.")){
// //cout << "distortion" << endl;
// yarp::os::Bottle &bot = config.findGroup("distortion");
// if (bot != NULL){
// //cout << "bottle != NULL" << endl;
// if (bot.size() == 5){
// //cout << "bottle of size 5" << endl;
// for (int i = 1; i < 5; i++){
// //cout << "value: " << bot.get(i).asDouble() << endl;
// _matrix[i-1] = (float)(bot.get(i).asDouble());
// }
// }
// }
//}
_oldImgSize.width = -1;
_oldImgSize.height = -1;
return true;
}
bool FfmpegGrabber::openFirewire(yarp::os::Searchable & config,
AVFormatContext **ppFormatCtx) {
AVInputFormat *iformat;
ConstString devname = config.check("devname",
Value("/dev/dv1394"),
"firewire device name").asString();
iformat = av_find_input_format("dv1394");
printf("Checking for digital video in %s\n", devname.c_str());
m_uri = devname;
return avformat_open_input(ppFormatCtx, strdup(devname.c_str()), iformat, 0) == 0;
}
bool VfwGrabber::open(yarp::os::Searchable& config) {
system_resource = new CvCaptureCAM_VFW;
if (system_resource!=NULL) {
int index = config.check("index",
yarp::os::Value(0),
"VFW device index").asInt();
int result = icvOpenCAM_VFW(&HELPER(system_resource),index);
if (!result) {
printf("failed to find camera\n");
close();
}
}
return system_resource!=NULL;
}
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 checkRequiredParamIsInt(yarp::os::Searchable& config,
const std::string& paramName)
{
bool correct = config.check(paramName);
if( correct )
{
correct = config.find(paramName).isInt();
}
if( !correct )
{
yError("CanBusInertialMTB: problem loading parameter %s as int",paramName.c_str());
}
return correct;
}
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 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;
}
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;
}
/*
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;
}
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 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;
}
// 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;
}
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;
}
}
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 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 yarp::dev::Rangefinder2DClient::open(yarp::os::Searchable &config)
{
local.clear();
remote.clear();
local = config.find("local").asString().c_str();
remote = config.find("remote").asString().c_str();
if (local=="")
{
yError("Rangefinder2DClient::open() error you have to provide valid local name");
return false;
}
if (remote=="")
{
yError("Rangefinder2DClient::open() error you have to provide valid remote name");
return false;
}
if (config.check("period"))
{
_rate = config.find("period").asInt();
}
else
{
yError("Rangefinder2DClient::open() missing period parameter");
return false;
}
ConstString local_rpc = local;
local_rpc += "/rpc:o";
ConstString remote_rpc = remote;
remote_rpc += "/rpc:i";
if (!inputPort.open(local.c_str()))
{
yError("Rangefinder2DClient::open() error could not open port %s, check network\n",local.c_str());
return false;
}
inputPort.useCallback();
if (!rpcPort.open(local_rpc.c_str()))
{
yError("Rangefinder2DClient::open() error could not open rpc port %s, check network\n", local_rpc.c_str());
return false;
}
bool ok=Network::connect(remote.c_str(), local.c_str(), "udp");
if (!ok)
{
yError("Rangefinder2DClient::open() error could not connect to %s\n", remote.c_str());
return false;
}
ok=Network::connect(local_rpc.c_str(), remote_rpc.c_str());
if (!ok)
{
yError("Rangefinder2DClient::open() error could not connect to %s\n", remote_rpc.c_str());
return false;
}
//getScanLimits is used here to update the cached values of scan_angle_min, scan_angle_max
double tmp_min;
double tmp_max;
this->getScanLimits(tmp_min, tmp_max);
//get the position of the device, if it is available
device_position_x = 0;
device_position_y = 0;
device_position_theta = 0;
yarp::dev::PolyDriver* drv = new yarp::dev::PolyDriver;
Property TransformClientOptions;
TransformClientOptions.put("device", "transformClient");
TransformClientOptions.put("local", "/rangefinder2DTransformClient");
TransformClientOptions.put("remote", "/transformServer");
TransformClientOptions.put("period", "10");
bool b_canOpenTransformClient = false;
if (config.check("laser_frame_name") &&
config.check("robot_frame_name"))
{
laser_frame_name = config.find("laser_frame_name").toString();
robot_frame_name = config.find("robot_frame_name").toString();
b_canOpenTransformClient = drv->open(TransformClientOptions);
}
if (b_canOpenTransformClient)
{
yarp::dev::IFrameTransform* iTrf = nullptr;
drv->view(iTrf);
if (!iTrf)
{
yError() << "A Problem occurred while trying to view the IFrameTransform interface";
drv->close();
delete drv;
return false;
}
yarp::sig::Matrix mat;
iTrf->getTransform(laser_frame_name, robot_frame_name, mat);
yarp::sig::Vector v = yarp::math::dcm2rpy(mat);
device_position_x = mat[0][3];
device_position_y = mat[1][3];
device_position_theta = v[2];
if (fabs(v[0]) < 1e-6 && fabs(v[1]) < 1e-6)
{
yError() << "Laser device is not planar";
}
yInfo() << "Position information obtained fromtransform server";
drv->close();
}
else
{
if (config.check("device_position_x") &&
config.check("device_position_y") &&
config.check("device_position_theta"))
{
yInfo() << "Position information obtained from configuration parameters";
device_position_x = config.find("device_position_x").asDouble();
device_position_y = config.find("device_position_y").asDouble();
device_position_theta = config.find("device_position_theta").asDouble();
}
else
{
yDebug() << "No position information provided for this device";
}
}
delete drv;
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();
}