experiments/src/sensors.cpp

/********************************************************************************
 *  FARSA Experiments Library                                                   *
 *  Copyright (C) 2007-2012                                                     *
 *  Gianluca Massera <emmegian@yahoo.it>                                        *
 *  Stefano Nolfi <stefano.nolfi@istc.cnr.it>                                   *
 *  Tomassino Ferrauto <tomassino.ferrauto@istc.cnr.it>                         *
 *  Onofrio Gigliotta <onofrio.gigliotta@istc.cnr.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  *
 ********************************************************************************/

#include "sensors.h"
#include "configurationhelper.h"
#include "motorcontrollers.h"
#include "logger.h"
#include "graphicalwobject.h"
#include <QStringList>

namespace farsa {



//ObjectPositionSensor : begin implementation
// it returns the absolute coordinate of an object into the world
ObjectPositionSensor::ObjectPositionSensor(ConfigurationParameters &params, QString prefix) :
    Sensor(params, prefix) {
    neuronsIteratorResource = ConfigurationHelper::getString(params, prefix + "neuronsIterator", neuronsIteratorResource);
    objectName = ConfigurationHelper::getString( params, prefix+"object", "object" );
    QVector<double> vec1 = ConfigurationHelper::getVector( params, prefix+"bbMin" );
    QVector<double> vec2 = ConfigurationHelper::getVector( params, prefix+"bbMax" );
    if ( vec1.size() == 3 && vec2.size() == 3 ) {
        linearize = true;
        bbMin = wVector( vec1[0], vec1[1], vec1[2] );
        bbMax = wVector( vec2[0], vec2[1], vec2[2] );
    } else {
        linearize = false;
        if ( ! (vec1.isEmpty() && vec2.isEmpty()) ) {
            Logger::warning( QString("ObjectPositionSensor %1 - bbMin and/or bbMax parameters are not well specified; they will be ignored").arg(name()) );
        }
    }

    // Declaring the resources that are needed here
    usableResources( QStringList() << objectName << neuronsIteratorResource );
}

ObjectPositionSensor::~ObjectPositionSensor() {
    // nothing to do
}

void ObjectPositionSensor::describe( QString type ) {
    Sensor::describe( type );
    Descriptor d = addTypeDescription( type, "Sensor for reading the three absolute coordinate (position into the worlf frame) of an object" );
    d.describeString("neuronsIterator").def("neuronsIterator").help("the name of the resource associated with the neural network iterator (default is \"neuronsIterator\")");
    d.describeString( "object" ).def( "object" ).props( IsMandatory ).help( "The name of the resource associated with the object to track with this sensor" );
    d.describeReal( "bbMin" ).props( IsList ).help( "The minimum 3D point used for linearize the object position into [0,1]" );
    d.describeReal( "bbMax" ).props( IsList ).help( "The maximum 3D point used for linearize the object position into [0,1]" );
}

void ObjectPositionSensor::update() {
    // Checking all resources we need exist
    checkAllNeededResourcesExist();

    // Acquiring the lock to get resources
    ResourcesLocker locker( this );

    WObject* object = getResource<WObject>( objectName );
    wVector pos = object->matrix().w_pos;
    NeuronsIterator* evonetIt = getResource<NeuronsIterator>( neuronsIteratorResource );
    evonetIt->setCurrentBlock( name() );
    for( int i=0; i<3; i++, evonetIt->nextNeuron() ) {
        if ( linearize ) {
            // linearize into [0,1]
            evonetIt->setInput( linearMap( pos[i], bbMin[i], bbMax[i], 0, 1 ) );
        } else {
            evonetIt->setInput( pos[i] );
        }
    }
}

int ObjectPositionSensor::size() {
    return 3;
}

void ObjectPositionSensor::resourceChanged(QString resourceName, ResourceChangeType changeType) {
    if (changeType == Deleted) {
        resetNeededResourcesCheck();
        return;
    }

    if (resourceName == objectName) {
        // Nothing to do here, we get the object with getResource() in update()
    } else if (resourceName == neuronsIteratorResource) {
        NeuronsIterator* evonetIt = getResource<NeuronsIterator>();
        evonetIt->setCurrentBlock( name() );
        for( int i=0; i<3; i++, evonetIt->nextNeuron() ) {
            evonetIt->setGraphicProperties( QString("obj")+QString::number(i), -10.0, 10.0, Qt::red );
        }
    } else {
        Logger::info("Unknown resource " + resourceName + " for " + name());
    }
}

void ObjectPositionSensor::save(ConfigurationParameters &params, QString prefix)
{
    Sensor::save( params, prefix );
    params.startObjectParameters( prefix, "ObjectPositionSensor", this );
    params.createParameter(prefix, "neuronsIterator", neuronsIteratorResource);
    params.createParameter( prefix, "object", objectName );
    if ( linearize ) {
        params.createParameter( prefix, "bbMin", QString("%1 %2 %3").arg(bbMin[0]).arg(bbMin[1]).arg(bbMin[2]) );
        params.createParameter( prefix, "bbMax", QString("%1 %2 %3").arg(bbMax[0]).arg(bbMax[1]).arg(bbMax[2]) );
    }
}
//ObjectPositionSensor : end implementation

} // end namespace farsa