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; };