Example #1
0
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;
}
Example #3
0
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;
}
Example #4
0
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;
}
Example #11
0
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;
}
Example #12
0
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;
}
Example #15
0
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;
}
Example #17
0
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;
}
Example #18
0
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;

}
Example #25
0
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;
}
Example #26
0
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
        {
Example #27
0
// 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;
}
Example #29
0
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;
};