worldsim/src/yarpobject.cpp

/********************************************************************************
 *  WorldSim -- library for robot simulations                                   *
 *  Copyright (C) 2008-2011 Gianluca Massera <emmegian@yahoo.it>                *
 *                                                                              *
 *  This program is free software; you can redistribute it and/or modify        *
 *  it under the terms of the GNU General Public License as published by        *
 *  the Free Software Foundation; either version 2 of the License, or           *
 *  (at your option) any later version.                                         *
 *                                                                              *
 *  This program is distributed in the hope that it will be useful,             *
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of              *
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the               *
 *  GNU General Public License for more details.                                *
 *                                                                              *
 *  You should have received a copy of the GNU General Public License           *
 *  along with this program; if not, write to the Free Software                 *
 *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA  *
 ********************************************************************************/

#ifdef FARSA_USE_YARP_AND_ICUB

#include "yarpobject.h"

#ifdef __GNUC__
#pragma GCC diagnostic ignored "-Wunused-parameter"
#endif
    #include <yarp/dev/all.h>
    #include <yarp/os/all.h>
#ifdef __GNUC__
#pragma GCC diagnostic warning "-Wunused-parameter"
#endif

using namespace yarp::os;
using namespace yarp::dev;

namespace farsa {

class FakeDriverCreator : public DriverCreator {
public:
    FakeDriverCreator( DeviceDriver* instance, const char *name, const char *wrap, const char *code)
        : desc(name), wrap(wrap), code(code) {
        device = instance;
    }
    virtual yarp::os::ConstString toString() {
        return desc;
    }
    virtual yarp::os::ConstString getName() {
        return desc;
    }
    virtual yarp::os::ConstString getWrapper() {
        return wrap;
    }
    virtual yarp::os::ConstString getCode() {
        return code;
    }
    virtual DeviceDriver *create() {
        return device;
    }
private:
    ConstString desc, wrap, code;
    DeviceDriver* device;
};

YarpObject::YarpObject( World* world, QString name, const wMatrix& tm )
    : WObject( world, name, tm, false ), controls() {
    //--- nothing to do here
}

YarpObject::~YarpObject() {
    // it's responsability of subclass to proper closing any PolyDriver opened
}

void YarpObject::registerServerControlBoard( yarp::dev::DeviceDriver* device, QString devicename ) {
    QString nameDev = QString( "%1-%2-%3" ).arg( world()->name() ).arg( name() ).arg( devicename );
    //--- add to the factory a FakeDriverCreator for allow creation of ServerControlBoard
    Drivers::factory().add( new FakeDriverCreator( device, nameDev.toAscii().data(), "controlboard", nameDev.toAscii().data() ) );
    //--- create the ServerControlBoard
    QString namePort = QString( "/%1/%2/%3" ).arg( world()->name() ).arg( name() ).arg( devicename );
    QString settings = QString( "(device controlboard) (subdevice %1) (name %2)" ).arg( nameDev ).arg(namePort);
    controls[devicename] = new PolyDriver( settings.toAscii().data() );
    return;
}

void YarpObject::removeServerControlBoard( QString devicename ) {
    if ( !controls.contains(devicename) ) return;
    QString nameDev = QString( "%1-%2-%3" ).arg( world()->name() ).arg( name() ).arg( devicename );
    PolyDriver* drv = controls[devicename];
    drv->close();
    controls.remove( devicename );
    delete drv;
    Drivers::factory().remove( nameDev.toAscii().data() );
}

void YarpObject::registerServerFrameGrabber( yarp::dev::DeviceDriver* device, QString devicename ) {
    QString nameDev = QString( "%1-%2-%3" ).arg( world()->name() ).arg( name() ).arg( devicename );
    if ( nameDev.endsWith("/") ) {
        nameDev.chop(1);
    }
    //--- add to the factory a FakeDriverCreator for allow creation of ServerControlBoard
    Drivers::factory().add( new FakeDriverCreator( device, nameDev.toAscii().data(), "framegrabber", nameDev.toAscii().data() ) );
    //--- create the ServerControlBoard
    QString namePort = QString( "/%1/%2/%3" ).arg( world()->name() ).arg( name() ).arg( devicename );
    if ( namePort.endsWith("/") ) {
        namePort.chop(1);
    }
    QString settings = QString( "(device grabber) (subdevice %1) (name %2)" ).arg( nameDev ).arg(namePort);
    controls[devicename] = new PolyDriver( settings.toAscii().data() );
    return;
}

} // end namespace farsa

#endif // FARSA_USE_YARP_AND_ICUB