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);
}
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 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 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 DynamixelAX12::configure(yarp::os::Searchable &config)
{
yarp::os::Value indexValue = config.find("SENSORINDEX");
yarp::os::Bottle *indexBottle = indexValue.asList();
this->initMotorIndex(indexBottle);
return true;
}
//DEVICE DRIVER
bool GazeboYarpLaserSensorDriver::open(yarp::os::Searchable& config)
{
yarp::os::LockGuard guard(m_mutex);
//Get gazebo pointers
std::string sensorScopedName(config.find(YarpLaserSensorScopedName.c_str()).asString().c_str());
m_parentSensor = dynamic_cast<gazebo::sensors::RaySensor*>(GazeboYarpPlugins::Handler::getHandler()->getSensor(sensorScopedName));
if (!m_parentSensor)
{
yError() << "Error, sensor" << sensorScopedName << "was not found" ;
return false ;
}
//Connect the driver to the gazebo simulation
this->m_updateConnection = gazebo::event::Events::ConnectWorldUpdateBegin(boost::bind(&GazeboYarpLaserSensorDriver::onUpdate, this, _1));
m_max_angle = m_parentSensor->AngleMax().Degree();
m_min_angle = m_parentSensor->AngleMin().Degree();
m_max_range = m_parentSensor->RangeMax();
m_min_range = m_parentSensor->RangeMin();
m_samples = m_parentSensor->RangeCount();
m_rate = m_parentSensor->UpdateRate();
m_sensorData.resize(m_samples, 0.0);
m_enable_clip_range = false;
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;
}
//DEVICE DRIVER
bool GazeboYarpForceTorqueDriver::open(yarp::os::Searchable& config)
{
std::cout << "GazeboYarpForceTorqueDriver::open() called" << std::endl;
m_dataMutex.wait();
m_forceTorqueData.resize(YarpForceTorqueChannelsNumber, 0.0);
m_dataMutex.post();
//Get gazebo pointers
std::string sensorScopedName(config.find(YarpScopedName.c_str()).asString().c_str());
std::cout << "GazeboYarpForceTorqueDriver::open is looking for sensor " << sensorScopedName << "..." << std::endl;
m_parentSensor = (gazebo::sensors::ForceTorqueSensor*)GazeboYarpPlugins::Handler::getHandler()->getSensor(sensorScopedName);
if (!m_parentSensor)
{
std::cout << "Error, ForceTorque sensor was not found" << std::endl;
return AS_ERROR;
}
//Connect the driver to the gazebo simulation
this->m_updateConnection = gazebo::event::Events::ConnectWorldUpdateBegin(boost::bind(&GazeboYarpForceTorqueDriver::onUpdate, this, _1));
std::cout << "GazeboYarpForceTorqueDriver::open() returning true" << std::endl;
return true;
}
// \todo TODO bug ?
bool checkRequiredParamIsVectorOfString(yarp::os::Searchable& config,
const std::string& paramName,
std::vector<std::string> & output_vector)
{
bool correct = !(config.findGroup(paramName).isNull());
if( correct )
correct = true;
{
Bottle ids = config.findGroup(paramName).tail();
std::cout << "ids : " << ids.toString() << std::endl;
std::cout << "ids : " << config.find(paramName).toString() << std::endl;
output_vector.resize(ids.size());
for(int i = 0; i < ids.size(); i++ )
{
output_vector[i] = ids.get(i).asString().c_str();
}
}
if( !correct )
{
yError("CanBusInertialMTB: problem loading parameter %s as vector of string",paramName.c_str());
}
return correct;
}
bool BmsBattery::open(yarp::os::Searchable& config)
{
bool correct=true;
//debug
yDebug("%s\n", config.toString().c_str());
Bottle& group_general = config.findGroup("GENERAL");
Bottle& group_serial = config.findGroup("SERIAL_PORT");
if (group_general.isNull())
{
yError() << "Insufficient parameters to BmsBattery, section GENERAL missing";
return false;
}
if (group_serial.isNull())
{
yError() << "Insufficient parameters to BmsBattery, section SERIAL_PORT missing";
return false;
}
int period=config.find("thread_period").asInt();
setRate(period);
Property prop;
std::string ps = group_serial.toString();
prop.fromString(ps);
prop.put("device", "serialport");
//open the driver
driver.open(prop);
if (!driver.isValid())
{
yError() << "Error opening PolyDriver check parameters";
#ifndef DEBUG_TEST
return false;
#endif
}
//open the serial interface
driver.view(pSerial);
if (!pSerial)
{
yError("Error opening serial driver. Device not available");
#ifndef DEBUG_TEST
return false;
#endif
}
// Other options
this->logEnable = group_general.check("logToFile", Value(0), "enable / disable the log to file").asBool();
this->verboseEnable = group_general.check("verbose", Value(0), "enable/disable the verbose mode").asBool();
this->screenEnable = group_general.check("screen", Value(0), "enable/disable the screen output").asBool();
this->debugEnable = group_general.check("debug", Value(0), "enable/disable the debug mode").asBool();
this->shutdownEnable = group_general.check("shutdown", Value(0), "enable/disable the automatic shutdown").asBool();
RateThread::start();
return true;
}
bool yarp::dev::Map2DClient::open(yarp::os::Searchable &config)
{
m_local_name.clear();
m_map_server.clear();
m_local_name = config.find("local").asString().c_str();
m_map_server = config.find("remote").asString().c_str();
if (m_local_name == "")
{
yError("Map2DClient::open() error you have to provide valid local name");
return false;
}
if (m_map_server == "")
{
yError("Map2DClient::open() error you have to provide valid remote name");
return false;
}
std::string local_rpc1 = m_local_name;
local_rpc1 += "/mapClient_rpc";
std::string remote_rpc1 = m_map_server;
remote_rpc1 += "/rpc";
if (!m_rpcPort_to_Map2DServer.open(local_rpc1.c_str()))
{
yError("Map2DClient::open() error could not open rpc port %s, check network", local_rpc1.c_str());
return false;
}
bool ok=false;
ok=Network::connect(local_rpc1.c_str(), remote_rpc1.c_str());
if (!ok)
{
yError("Map2DClient::open() error could not connect to %s", remote_rpc1.c_str());
return false;
}
return true;
}
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 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;
}
//DEVICE DRIVER
bool GazeboYarpDepthCameraDriver::open(yarp::os::Searchable &config)
{
string sensorScopedName((config.find(YarpScopedName.c_str()).asString().c_str()));
//Get gazebo pointers
m_conf.fromString(config.toString());
m_depthCameraSensorPtr = dynamic_cast<gazebo::sensors::DepthCameraSensor*> (GazeboYarpPlugins::Handler::getHandler()->getSensor(sensorScopedName));
if (!m_depthCameraSensorPtr)
{
yError("camera sensor was not found (sensor's scoped name %s!)", sensorScopedName.c_str());
return false;
}
m_depthCameraPtr = this->m_depthCameraSensorPtr->DepthCamera();
m_width = m_depthCameraPtr->ImageWidth();
m_height = m_depthCameraPtr->ImageHeight();
m_imageFrame_BufferSize = m_depthCameraPtr->ImageDepth() * m_width * m_height;
m_depthFrame_BufferSize = m_width * m_height * sizeof(float);
m_depthFrameMutex.wait();
m_depthFrame_Buffer = new float[m_width * m_height];
memset(m_depthFrame_Buffer, 0x00, m_depthFrame_BufferSize);
m_depthFrameMutex.post();
m_colorFrameMutex.wait();
m_imageFrame_Buffer = new unsigned char[m_imageFrame_BufferSize];
memset(m_imageFrame_Buffer, 0x00, m_imageFrame_BufferSize);
m_colorFrameMutex.post();
//Connect the driver to the gazebo simulation
auto imageConnectionBind = boost::bind(&GazeboYarpDepthCameraDriver::OnNewImageFrame, this, _1, _2, _3, _4, _5);
auto depthConnectionBind = boost::bind(&GazeboYarpDepthCameraDriver::OnNewDepthFrame, this, _1, _2, _3, _4, _5);
this->m_updateImageFrame_Connection = m_depthCameraPtr->ConnectNewImageFrame(imageConnectionBind);
this->m_updateDepthFrame_Connection = m_depthCameraPtr->ConnectNewDepthFrame(depthConnectionBind);
return true;
}
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 FakeAnalogSensor::open(yarp::os::Searchable& config)
{
yTrace();
bool correct=true;
//debug
fprintf(stderr, "%s\n", config.toString().c_str());
// Check parameters first
// if(!config.check("channels"))
// {
// correct = false;
// yError() << "Parameter 'channels' missing";
// }
if(!config.check("period"))
{
correct = false;
yError() << "Parameter 'period' missing";
}
if (!correct)
{
yError() << "Insufficient parameters to FakeAnalogSensor\n";
return false;
}
int period=config.find("period").asInt();
setRate(period);
//create the data vector:
this->channelsNum = 1;
data.resize(channelsNum);
data.zero();
RateThread::start();
return true;
}
bool 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 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 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;
}
iCubTestMotors::iCubTestMotors(yarp::os::Searchable& configuration) : iCubTest(configuration)
{
m_aTargetVal=NULL;
m_aMaxErr=NULL;
m_aMinErr=NULL;
m_aRefVel=NULL;
m_aRefAcc=NULL;
m_aTimeout=NULL;
m_part=(iCubPart)0;
if (configuration.check("robot"))
{
m_robot = std::string (configuration.find("robot").asString());
}
m_icubDriver.open(m_robot);
if (configuration.check("device"))
{
std::string device(configuration.find("device").asString());
m_part = iCubPart(device);
}
m_NumJoints=m_icubDriver.getNumOfJoints(m_part);
///////////////////////////////////////////////////////////////
/*
if (m_nJoints<=0)
{
printf("\nERROR\n\n");
return;
}
*/
///////////////////////////////////////////////////////////////
if (configuration.check("target"))
{
yarp::os::Bottle bot=configuration.findGroup("target").tail();
int n=m_NumJoints<bot.size()?m_NumJoints:bot.size();
m_aTargetVal=new double[m_NumJoints];
for (int i=0; i<n; ++i)
{
if (bot.get(i).isDouble()) m_aTargetVal[i]=bot.get(i).asDouble();
}
}
if (configuration.check("min"))
{
yarp::os::Bottle bot=configuration.findGroup("min").tail();
int n=m_NumJoints<bot.size()?m_NumJoints:bot.size();
m_aMinErr=new double[m_NumJoints];
for (int i=0; i<n; ++i)
{
if (bot.get(i).isDouble()) m_aMinErr[i]=bot.get(i).asDouble();
}
}
if (configuration.check("max"))
{
yarp::os::Bottle bot=configuration.findGroup("max").tail();
int n=m_NumJoints<bot.size()?m_NumJoints:bot.size();
m_aMaxErr=new double[m_NumJoints];
for (int i=0; i<n; ++i)
{
if (bot.get(i).isDouble()) m_aMaxErr[i]=bot.get(i).asDouble();
}
}
if (configuration.check("refvel"))
{
yarp::os::Bottle bot=configuration.findGroup("refvel").tail();
int n=m_NumJoints<bot.size()?m_NumJoints:bot.size();
m_aRefVel=new double[m_NumJoints];
for (int i=0; i<n; ++i)
{
if (bot.get(i).isDouble()) m_aRefVel[i]=bot.get(i).asDouble();
}
}
if (configuration.check("refacc"))
{
yarp::os::Bottle bot=configuration.findGroup("refacc").tail();
int n=m_NumJoints<bot.size()?m_NumJoints:bot.size();
m_aRefAcc=new double[m_NumJoints];
for (int i=0; i<n; ++i)
{
if (bot.get(i).isDouble()) m_aRefAcc[i]=bot.get(i).asDouble();
}
}
if (configuration.check("timeout"))
{
yarp::os::Bottle bot=configuration.findGroup("timeout").tail();
int n=m_NumJoints<bot.size()?m_NumJoints:bot.size();
m_aTimeout=new double[m_NumJoints];
for (int i=0; i<n; ++i)
{
if (bot.get(i).isDouble()) m_aTimeout[i]=bot.get(i).asDouble();
}
}
}
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 JointTorqueControl::open(yarp::os::Searchable& config)
{
//Workaround: writeStrict option is not documented but
// is necessary for getting correctly working
// single joint streaming, hardcoding it to on
yarp::os::Property pass_through_controlboard_config;
pass_through_controlboard_config.fromString(config.toString());
pass_through_controlboard_config.put("writeStrict","on");
PassThroughControlBoard::open(pass_through_controlboard_config);
this->getAxes(&axes);
hijackingTorqueControl.assign(axes,false);
controlModesBuffer.resize(axes);
motorParameters.resize(axes);
jointTorqueLoopGains.resize(axes);
measuredJointPositions.resize(axes,0.0);
measuredJointVelocities.resize(axes,0.0);
measuredMotorVelocities.resize(axes,0.0);
desiredJointTorques.resize(axes,0.0);
measuredJointTorques.resize(axes,0.0);
measuredJointPositionsTimestamps.resize(axes,0.0);
jointControlOutput.resize(axes,0.0);
jointTorquesError.resize(axes,0.0);
oldJointTorquesError.resize(axes,0.0);
derivativeJointTorquesError.resize(axes,0.0);
integralJointTorquesError.resize(axes,0.0);
integralState.resize(axes,0.0);
jointControlOutputBuffer.resize(axes,0.0);
//Start control thread
this->setRate(config.check("controlPeriod",10,"update period of the torque control thread (ms)").asInt());
//Load Gains configurations
bool ret = this->loadGains(config);
//Load coupling matrices
couplingMatrices.reset(this->axes);
ret = ret && this->loadCouplingMatrix(config,couplingMatrices,"FROM_MOTORS_TO_JOINTS_KINEMATIC_COUPLINGS");
couplingMatricesFirmware.reset(this->axes);
ret = ret && this->loadCouplingMatrix(config,couplingMatricesFirmware,"FROM_MOTORS_TO_JOINTS_KINEMATIC_COUPLINGS_FIRMWARE");
std::cerr << "fromJointTorquesToMotorTorques matrix" << std::endl;
std::cerr << couplingMatrices.fromJointTorquesToMotorTorques << std::endl;
std::cerr << "fromJointVelocitiesToMotorVelocities matrix " << std::endl;
std::cerr << couplingMatrices.fromJointVelocitiesToMotorVelocities << std::endl;
std::cerr << "fromJointTorquesToMotorTorques matrix firmware" << std::endl;
std::cerr << couplingMatricesFirmware.fromJointTorquesToMotorTorques << std::endl;
std::cerr << "fromJointVelocitiesToMotorVelocities matrix firmware" << std::endl;
std::cerr << couplingMatricesFirmware.fromJointVelocitiesToMotorVelocities << std::endl;
std::cerr << "fromMotorTorquesToJointTorques matrix firmware" << std::endl;
std::cerr << couplingMatricesFirmware.fromMotorTorquesToJointTorques << std::endl;
streamingOutput = config.check("streamingOutput");
std::cerr << "streamingOutput = " << streamingOutput << std::endl;
if (streamingOutput)
{
ret = ret && config.check("name");
ret = ret && config.find("name").isString();
partName = config.find("name").asString();
portForStreamingPWM.open(partName + "/output_pwms");
portForReadingRefTorques.open(partName +"/input_torques");
}
if( ret )
{
ret = ret && this->start();
}
return ret;
}
bool 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 SDLJoypad::open(yarp::os::Searchable& rf)
{
if(rf.check("help"))
{
yInfo() << "parameters:\n\n" <<
"UseAllJoypadAsOne - set it to 1 to have all the connected joypad as one\n" <<
"DefaultJoystickNumber - select the id of the joypad to use if there are more than one joypad and UseAllJoypadAsOne is set to 0\n" <<
"stick - the number of stick to configure. a stick is simply a wrapping of 2 or more axes so for every stick\n" <<
" a group named STICK*ID* containing the stick's parameters is searched. \n" <<
"\n\n" <<
"stick groups parameters:\n\n" <<
"axes - axes count for this stick\n" <<
"axis[ID]_id - axis id for current stick related axis\n" <<
"invert_axis_[ID] - invert the current axis\n" <<
"deadZone - set the deadzone for this stick\n";
return false;
}
int joy_id;
size_t joystick_num;
int actionCount;
if (SDL_InitSubSystem( SDL_INIT_JOYSTICK ) < 0 )
{
yError ( "SDLJoypad: Unable to initialize Joystick: %s\n", SDL_GetError() );
return false;
}
joy_id = 0;
joystick_num = SDL_NumJoysticks();
actionCount = 0;
if (joystick_num == 0)
{
yError ( "SDLJoypad: No joysticks found\n");
return false;
}
else if (joystick_num == 1)
{
joy_id = 0;
yInfo ( "SDLJoypad: One joystick found \n");
yInfo ( "SDLJoypad: Using joystick: %s \n", SDL_JoystickName(joy_id));
}
else
{
yInfo ( "SDLJoypad: More than one joystick found:\n");
for (size_t i = 0; i < joystick_num; i++)
{
yInfo () << i << ":" << SDL_JoystickName(i);
}
yInfo ( "\n");
if(!rf.check("UseAllJoypadAsOne"))
{
if(rf.find("UseAllJoypadAsOne").asBool())
{
// choose between multiple joysticks
if (rf.check("DefaultJoystickNumber"))
{
joy_id = rf.find("DefaultJoystickNumber").asInt32();
yInfo ( "SDLJoypad: Multiple joysticks found, using #%d, as specified in the configuration options\n", joy_id);
}
else
{
yWarning ( "SDLJoypad: No default joystick specified in the configuration options\n");
yWarning ( "SDLJoypad: Which joystick you want to use? (choose number) \n");
cin >> joy_id;
}
m_allJoypad = false;
}
else
{
m_allJoypad = true;
}
}
else
{
// 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::AnalogSensorClient::open(yarp::os::Searchable &config)
{
ConstString carrier = config.check("carrier", Value("udp"), "default carrier for streaming robot state").asString().c_str();
local.clear();
remote.clear();
local = config.find("local").asString().c_str();
remote = config.find("remote").asString().c_str();
if (local=="")
{
fprintf(stderr,"AnalogSensorClient::open() error you have to provide valid local name\n");
return false;
}
if (remote=="")
{
fprintf(stderr,"AnalogSensorClient::open() error you have to provide valid remote name\n");
return false;
}
if (config.check("period"))
{
_rate = config.find("period").asInt();
}
else
{
_rate = DEFAULT_THREAD_PERIOD;
std::cout <<"Warning: part "<< deviceId <<" using default period ("<<_rate<<")\n";
}
ConstString local_rpc = local;
local_rpc += "/rpc:o";
ConstString remote_rpc = remote;
remote_rpc += "/rpc:i";
if (!inputPort.open(local.c_str()))
{
fprintf(stderr,"AnalogSensorClient::open() error could not open port %s, check network\n",local.c_str());
return false;
}
inputPort.useCallback();
if (!rpcPort.open(local_rpc.c_str()))
{
fprintf(stderr,"AnalogSensorClient::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(), carrier.c_str());
if (!ok)
{
fprintf(stderr,"AnalogSensorClient::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)
{
fprintf(stderr,"AnalogSensorClient::open() error could not connect to %s\n", remote_rpc.c_str());
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 comanFTsensor::fromConfig(yarp::os::Searchable &_config)
{
Bottle xtmp;
if(_config.check("device") )
{
setDeviceId(_config.find("device").asString() );
}
else
{
printf("No device name found.");
std::cout << "Params are: " << _config.toString();
}
// Analog Sensor stuff
Bottle config = _config.findGroup("ANALOG");
if (!validate(config, xtmp, "numberOfSensors","Number of FT boards", 1))
{
yError() << "comanFTsensor: Boards number not found or not valid, quitting\n";
_channels = 0;
numberOfBoards = 0;
return false;
}
else
{ // get starts from 1 because the parameter name is counted as index 0
numberOfBoards = xtmp.get(1).asInt();
}
FTmap = new int[numberOfBoards];
scaleFactor=new double[numberOfBoards];
std::cout << "\n\n num of boards is " << numberOfBoards << "\n\n";
if (!validate(config, xtmp, "newtonsToSensor","Scaling factor for FT sensors", numberOfBoards))
{
yError() << "FTSensor " << getDeviceId() << ": newtonsToSensor param was not found";
return false;
}
else
{
for (int i=0; i<numberOfBoards; i++)
scaleFactor[i]=xtmp.get(1+i).asDouble();
}
if (!validate(config, xtmp, "FTmap","mapping of FTsensor unique Ids and a sequence number", numberOfBoards))
{
yWarning() << "comanFTsensor: FT sensor map not found or not correct, check your config file. Quitting";
for(int i=0; i < numberOfBoards; i++)
{
FTmap[i] = 0;
std::cout << "Ftmap[" << i << "] is " << FTmap[i];
}
return false;
}
else
{ // get starts from 1 because the parameter name is counted as index 0
for(int i=0; i<numberOfBoards; i++)
{
FTmap[i] = xtmp.get(1+i).asInt();
std::cout << "Ftmap[" << i << "] is " << FTmap[i];
}
}
if (!validate(config, xtmp, "channels","number of channels per FT sensor", 1))
{
yWarning() << "comanFTsensor: Using default value = 6 (3 forces plus 3 torques)";
_channels = 6;
}
else
{ // get starts from 1 because the parameter name is counted as index 0
_channels = xtmp.get(1).asInt();
}
if (!validate(config, xtmp, "UseCalibration","Calibration parameters if needed", 1))
{
yWarning() << "comanFTsensor: Using default value = 0 (Don't use calibration)";
_useCalibration = 0;
}
else
{ // get starts from 1 because the parameter name is counted as index 0
_useCalibration = xtmp.get(1).asInt();
}
return true;
};