marxbotsensors.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 "marxbotsensors.h"
#include "configurationhelper.h"
#include "logger.h"
#include "graphicalwobject.h"

namespace farsa {

MarXbotSensor::MarXbotSensor(ConfigurationParameters& params, QString prefix) :
    Sensor(params, prefix),
    m_marxbotResource("robot"),
    m_neuronsIteratorResource("neuronsIterator")
{
    // Reading parameters
    m_marxbotResource = ConfigurationHelper::getString(params, prefix + "marxbot", m_marxbotResource);
    m_neuronsIteratorResource = ConfigurationHelper::getString(params, prefix + "neuronsIterator", m_neuronsIteratorResource);

    // Declaring the resources that are needed here
    usableResources(QStringList() << m_marxbotResource << m_neuronsIteratorResource);
}

MarXbotSensor::~MarXbotSensor()
{
    // Nothing to do here
}

void MarXbotSensor::save(ConfigurationParameters& params, QString prefix)
{
    // Calling parent function
    Sensor::save(params, prefix);

    // Saving parameters
    params.startObjectParameters(prefix, "MarXbotSensor", this);
    params.createParameter(prefix, "marxbot", m_marxbotResource);
    params.createParameter(prefix, "neuronsIterator", m_neuronsIteratorResource);
}

void MarXbotSensor::describe(QString type)
{
    // Calling parent function
    Sensor::describe(type);

    // Describing our parameters
    Descriptor d = addTypeDescription(type, "The base class for MarXbot sensors");
    d.describeString("marxbot").def("robot").help("the name of the resource associated with the MarXbot robot to use (default is \"robot\")");
    d.describeString("neuronsIterator").def("neuronsIterator").help("the name of the resource associated with the neural network iterator (default is \"neuronsIterator\")");
}

void MarXbotSensor::resourceChanged(QString resourceName, ResourceChangeType changeType)
{
    // Calling parent function
    Sensor::resourceChanged(resourceName, changeType);

    // Here we only check whether the resource has been deleted and reset the check flag, the
    // actual work is done in subclasses
    if (changeType == Deleted) {
        resetNeededResourcesCheck();
        return;
    }
}

MarXbotProximityIRSensor::MarXbotProximityIRSensor(ConfigurationParameters& params, QString prefix) :
    MarXbotSensor(params, prefix),
    m_robot(NULL),
    m_neuronsIterator(NULL)
{
}

MarXbotProximityIRSensor::~MarXbotProximityIRSensor()
{
    // Nothing to do here
}

void MarXbotProximityIRSensor::save(ConfigurationParameters& params, QString prefix)
{
    // Calling parent function
    MarXbotSensor::save(params, prefix);

    // Saving parameters
    params.startObjectParameters(prefix, "MarXbotProximityIRSensor", this);
}

void MarXbotProximityIRSensor::describe(QString type)
{
    // Calling parent function
    MarXbotSensor::describe(type);

    // Describing our parameters
    Descriptor d = addTypeDescription(type, "The infrared proximity sensors of the MarXbot robot", "The infrared proximity sensors of the MarXbot robot. These are the very short range IR sensors all around the base");
}

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

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

    m_neuronsIterator->setCurrentBlock(name());
    for (int i = 0; i < size(); i++, m_neuronsIterator->nextNeuron()) {
        m_neuronsIterator->setInput(m_robot->proximityIRSensorController()->activation(i));
    }
}

int MarXbotProximityIRSensor::size()
{
    return 24;
}

void MarXbotProximityIRSensor::resourceChanged(QString resourceName, ResourceChangeType changeType)
{
    // Calling parent function
    MarXbotSensor::resourceChanged(resourceName, changeType);

    if (changeType == Deleted) {
        return;
    }

    if (resourceName == m_marxbotResource) {
        m_robot = getResource<PhyMarXbot>();

        // Eanbling sensors
        m_robot->proximityIRSensorController()->setEnabled(true);
    } else if (resourceName == m_neuronsIteratorResource) {
        m_neuronsIterator = getResource<NeuronsIterator>();
        m_neuronsIterator->setCurrentBlock(name());
        for (int i = 0; i < size(); i++, m_neuronsIterator->nextNeuron()) {
            m_neuronsIterator->setGraphicProperties("ir" + QString::number(i), 0.0, 1.0, Qt::red);
        }
    } else {
        Logger::info("Unknown resource " + resourceName + " for " + name());
    }
}

MarXbotGroundBottomIRSensor::MarXbotGroundBottomIRSensor(ConfigurationParameters& params, QString prefix) :
    MarXbotSensor(params, prefix),
    m_robot(NULL),
    m_neuronsIterator(NULL)
{
}

MarXbotGroundBottomIRSensor::~MarXbotGroundBottomIRSensor()
{
    // Nothing to do here
}

void MarXbotGroundBottomIRSensor::save(ConfigurationParameters& params, QString prefix)
{
    // Calling parent function
    MarXbotSensor::save(params, prefix);

    // Saving parameters
    params.startObjectParameters(prefix, "MarXbotGroundBottomIRSensor", this);
}

void MarXbotGroundBottomIRSensor::describe(QString type)
{
    // Calling parent function
    MarXbotSensor::describe(type);

    // Describing our parameters
    Descriptor d = addTypeDescription(type, "The infrared ground bottom sensors of the MarXbot robot", "The infrared ground bottom sensors of the MarXbot robot. These are the four ground sensors below the robot battery pack.");
}

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

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

    m_neuronsIterator->setCurrentBlock(name());
    for (int i = 0; i < size(); i++, m_neuronsIterator->nextNeuron()) {
        m_neuronsIterator->setInput(m_robot->groundBottomIRSensorController()->activation(i));
    }
}

int MarXbotGroundBottomIRSensor::size()
{
    return 4;
}

void MarXbotGroundBottomIRSensor::resourceChanged(QString resourceName, ResourceChangeType changeType)
{
    // Calling parent function
    MarXbotSensor::resourceChanged(resourceName, changeType);

    if (changeType == Deleted) {
        return;
    }

    if (resourceName == m_marxbotResource) {
        m_robot = getResource<PhyMarXbot>();

        // Eanbling sensors
        m_robot->groundBottomIRSensorController()->setEnabled(true);
    } else if (resourceName == m_neuronsIteratorResource) {
        m_neuronsIterator = getResource<NeuronsIterator>();
        m_neuronsIterator->setCurrentBlock(name());
        for (int i = 0; i < size(); i++, m_neuronsIterator->nextNeuron()) {
            m_neuronsIterator->setGraphicProperties("gb" + QString::number(i), 0.0, 1.0, Qt::red);
        }
    } else {
        Logger::info("Unknown resource " + resourceName + " for " + name());
    }
}

MarXbotGroundAroundIRSensor::MarXbotGroundAroundIRSensor(ConfigurationParameters& params, QString prefix) :
    MarXbotSensor(params, prefix),
    m_robot(NULL),
    m_neuronsIterator(NULL)
{
}

MarXbotGroundAroundIRSensor::~MarXbotGroundAroundIRSensor()
{
    // Nothing to do here
}

void MarXbotGroundAroundIRSensor::save(ConfigurationParameters& params, QString prefix)
{
    // Calling parent function
    MarXbotSensor::save(params, prefix);

    // Saving parameters
    params.startObjectParameters(prefix, "MarXbotGroundAroundIRSensor", this);
}

void MarXbotGroundAroundIRSensor::describe(QString type)
{
    // Calling parent function
    MarXbotSensor::describe(type);

    // Describing our parameters
    Descriptor d = addTypeDescription(type, "The infrared ground around sensors of the MarXbot robot", "The infrared ground around sensors of the MarXbot robot. These are the eight ground sensors below the base of the robot (just above the wheels).");
}

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

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

    m_neuronsIterator->setCurrentBlock(name());
    for (int i = 0; i < size(); i++, m_neuronsIterator->nextNeuron()) {
        m_neuronsIterator->setInput(m_robot->groundAroundIRSensorController()->activation(i));
    }
}

int MarXbotGroundAroundIRSensor::size()
{
    return 8;
}

void MarXbotGroundAroundIRSensor::resourceChanged(QString resourceName, ResourceChangeType changeType)
{
    // Calling parent function
    MarXbotSensor::resourceChanged(resourceName, changeType);

    if (changeType == Deleted) {
        return;
    }

    if (resourceName == m_marxbotResource) {
        m_robot = getResource<PhyMarXbot>();

        // Eanbling sensors
        m_robot->groundAroundIRSensorController()->setEnabled(true);
    } else if (resourceName == m_neuronsIteratorResource) {
        m_neuronsIterator = getResource<NeuronsIterator>();
        m_neuronsIterator->setCurrentBlock(name());
        for (int i = 0; i < size(); i++, m_neuronsIterator->nextNeuron()) {
            m_neuronsIterator->setGraphicProperties("ga" + QString::number(i), 0.0, 1.0, Qt::red);
        }
    } else {
        Logger::info("Unknown resource " + resourceName + " for " + name());
    }
}

MarXbotLinearCameraSensor::MarXbotLinearCameraSensor(ConfigurationParameters& params, QString prefix) :
    MarXbotSensor(params, prefix),
    m_robot(NULL),
    m_neuronsIterator(NULL),
    m_numReceptors(ConfigurationHelper::getInt(params, prefix + "numReceptors", 8)),
    m_aperture(ConfigurationHelper::getDouble(params, prefix + "aperture", 360.0f)),
    m_camera(NULL),
    m_drawCamera(ConfigurationHelper::getBool(params, prefix + "drawCamera", true))
{
}

MarXbotLinearCameraSensor::~MarXbotLinearCameraSensor()
{
    // Deleting the camera
    delete m_camera;
}

void MarXbotLinearCameraSensor::save(ConfigurationParameters& params, QString prefix)
{
    // Calling parent function
    MarXbotSensor::save(params, prefix);

    // Saving parameters
    params.startObjectParameters(prefix, "MarXbotLinearCameraSensor", this);
    params.createParameter(prefix, "numReceptors", QString::number(m_numReceptors));
    params.createParameter(prefix, "aperture", QString::number(m_aperture));
    params.createParameter(prefix, "drawCamera", (m_drawCamera ? "true" : "false"));
}

void MarXbotLinearCameraSensor::describe(QString type)
{
    // Calling parent function
    MarXbotSensor::describe(type);

    // Describing our parameters
    Descriptor d = addTypeDescription(type, "The linear camera sensor of the MarXbot robot", "This is a 360° linear camera");
    d.describeInt("numReceptors").def(8).limits(1, MaxInteger).help("The number of receptors of the sensor", "Each receptor returns three values, one for each of the three colors (red, green, blue). This means that the size returned by this sensor is 3 * numReceptors (default is 8)");
    d.describeReal("aperture").def(360.0f).limits(0.0f, 360.0f).help("The aperture of the camera in degrees", "The real MarXbot has an omnidirectional camera, so you can use here any value up to 360 degrees (default is 360)");
    d.describeBool("drawCamera").def(true).help("Whether to draw the camera or not");
}

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

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

    // Updating the camera
    m_camera->update();

    // Reading activations: first the red one, then the green one and finally the blue one
    m_neuronsIterator->setCurrentBlock(name());
    for (int i = 0; i < m_numReceptors; i++, m_neuronsIterator->nextNeuron()) {
        m_neuronsIterator->setInput(m_camera->colorForReceptor(i).redF());
    }
    for (int i = 0; i < m_numReceptors; i++, m_neuronsIterator->nextNeuron()) {
        m_neuronsIterator->setInput(m_camera->colorForReceptor(i).greenF());
    }
    for (int i = 0; i < m_numReceptors; i++, m_neuronsIterator->nextNeuron()) {
        m_neuronsIterator->setInput(m_camera->colorForReceptor(i).blueF());
    }
}

int MarXbotLinearCameraSensor::size()
{
    return m_numReceptors * 3;
}

void MarXbotLinearCameraSensor::resourceChanged(QString resourceName, ResourceChangeType changeType)
{
    // Calling parent function
    MarXbotSensor::resourceChanged(resourceName, changeType);

    if (changeType == Deleted) {
        // Deleting the camera if the robot was deleted
        if (resourceName == m_marxbotResource) {
            delete m_camera;
            m_camera = NULL;
        }

        return;
    }

    if (resourceName == m_marxbotResource) {
        m_robot = getResource<PhyMarXbot>();

        // Now we can also create the camera
        wMatrix mtr = wMatrix::roll(-PI_GRECO / 2.0);
#ifdef __GNUC__
    #warning QUI INVECE DI UNA COSTANTE, CALCOLARSI UNA POSIZIONE DALLE DIMENSIONI DEL ROBOT
#endif
        mtr.w_pos.z = 0.13;
        m_camera = new LinearCamera(m_robot, mtr, toRad(m_aperture), m_numReceptors, Qt::black);

        // Sharing resources with the camera
        m_camera->shareResourcesWith(this);
        m_camera->drawCamera(m_drawCamera);
    } else if (resourceName == m_neuronsIteratorResource) {
        m_neuronsIterator = getResource<NeuronsIterator>();
        m_neuronsIterator->setCurrentBlock(name());
        int i = 0;
        for (; i < m_numReceptors; i++, m_neuronsIterator->nextNeuron()) {
            m_neuronsIterator->setGraphicProperties("lr" + QString::number(i % m_numReceptors), 0.0, 1.0, Qt::red);
        }
        for (; i < 2 * m_numReceptors; i++, m_neuronsIterator->nextNeuron()) {
            m_neuronsIterator->setGraphicProperties("lg" + QString::number(i % m_numReceptors), 0.0, 1.0, Qt::red);
        }
        for (; i < 3 * m_numReceptors; i++, m_neuronsIterator->nextNeuron()) {
            m_neuronsIterator->setGraphicProperties("lb" + QString::number(i % m_numReceptors), 0.0, 1.0, Qt::red);
        }
    } else {
        Logger::info("Unknown resource " + resourceName + " for " + name());
    }
}

#ifdef __GNUC__
    #warning QUI VEDERE COME CODIFICARE IL SENSORE DI TRAZIONE
#endif
MarXbotTractionSensor::MarXbotTractionSensor(ConfigurationParameters& params, QString prefix) :
    MarXbotSensor(params, prefix),
    m_robot(NULL),
    m_neuronsIterator(NULL)
{
}

MarXbotTractionSensor::~MarXbotTractionSensor()
{
    // Nothing to do here
}

void MarXbotTractionSensor::save(ConfigurationParameters& params, QString prefix)
{
    // Calling parent function
    MarXbotSensor::save(params, prefix);

    // Saving parameters
    params.startObjectParameters(prefix, "MarXbotTractionSensor", this);
}

void MarXbotTractionSensor::describe(QString type)
{
    // Calling parent function
    MarXbotSensor::describe(type);

    // Describing our parameters
    Descriptor d = addTypeDescription(type, "The traction sensors of the MarXbot robot", "The traction sensors of the MarXbot robot. It is placed between the base and the turret. Note that this sensor only works when the robot is in dynamic mode");
}

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

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

    m_neuronsIterator->setCurrentBlock(name());
    const wVector& t = m_robot->tractionSensorController()->traction();
    m_neuronsIterator->setInput(t.x);
    m_neuronsIterator->nextNeuron();
    m_neuronsIterator->setInput(t.y);
    m_neuronsIterator->nextNeuron();
    m_neuronsIterator->setInput(t.z);
}

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

void MarXbotTractionSensor::resourceChanged(QString resourceName, ResourceChangeType changeType)
{
    // Calling parent function
    MarXbotSensor::resourceChanged(resourceName, changeType);

    if (changeType == Deleted) {
        return;
    }

    if (resourceName == m_marxbotResource) {
        m_robot = getResource<PhyMarXbot>();

        // Eanbling sensors
        m_robot->proximityIRSensorController()->setEnabled(true);
    } else if (resourceName == m_neuronsIteratorResource) {
        m_neuronsIterator = getResource<NeuronsIterator>();
        m_neuronsIterator->setCurrentBlock(name());
        m_neuronsIterator->setGraphicProperties("tx", -1.0, 1.0, Qt::red);
        m_neuronsIterator->nextNeuron();
        m_neuronsIterator->setGraphicProperties("ty", -1.0, 1.0, Qt::red);
        m_neuronsIterator->nextNeuron();
        m_neuronsIterator->setGraphicProperties("tz", -1.0, 1.0, Qt::red);
    } else {
        Logger::info("Unknown resource " + resourceName + " for " + name());
    }
}

}