Controller.h

Go to the documentation of this file.

/* ============================================================
 *
 * This file is a part of RCI project
 *
 * Copyright (C) 2011 by Arne Nordmann <anordman at cor-lab dot uni-bielefeld dot de>
 *
 * This file may be licensed under the terms of the
 * GNU Lesser General Public License Version 3 (the ``LGPL''),
 * or (at your option) any later version.
 *
 * Software distributed under the License is distributed
 * on an ``AS IS'' basis, WITHOUT WARRANTY OF ANY KIND, either
 * express or implied. See the LGPL for the specific language
 * governing rights and limitations.
 *
 * You should have received a copy of the LGPL along with this
 * program. If not, go to http://www.gnu.org/licenses/lgpl.html
 * or write to the Free Software Foundation, Inc.,
 * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 *
 * The development of this software was supported by:
 *   CoR-Lab, Research Institute for Cognition and Robotics
 *     Bielefeld University
 *
 * ============================================================ */

#pragma once

#include <sstream>
#include <iostream>
#include <math.h>

#include <cca/Config.h>
#include <cca/Node.h>

#include <NemoMath/Vector.h>

namespace cca {
namespace rci {

//template<class T>
//class Controller;
//typedef boost::shared_ptr<Controller<T> > ControllerPtr;

template<class T>
class Controller: public CCANode {

public:

    Controller(std::string nodename) :
        CCANode(nodename), _task(), _status() {

        this->addInputPort<T> ();
        this->addInputPort<T> ();
        this->addOutputPort<T> ();

        _task = nemo::RealVector(nemo::dim(1), 0.0);
        _status = nemo::RealVector(nemo::dim(1), 0.0);
    }
    virtual ~Controller() {
    }

    virtual std::string print() const {
        std::ostringstream outstream(std::ostringstream::out);
        outstream.precision(3); // Precision when printing double values
        outstream << "<Controller \"" << this->getName() << "\">" << std::endl;
        return outstream.str();
    }

    virtual void onProcess() {
        //std::cout << "[Controller] Controlling ..." << std::endl;
        T *step;

        // Take input values
        if (!this->inputs[0]->empty() && this->inputs[0]->newItem()) {
            _task = *boost::static_pointer_cast<T>(this->inputs[0]->tryGet());
            std::cout << "[Controller] New task is\t" << _task;
        }

        if (!this->inputs[1]->empty() && this->inputs[1]->newItem()) {
            _status = *boost::static_pointer_cast<T>(this->inputs[1]->tryGet());
            std::cout << "[Controller] Status is\t\t" << _status;
        }

        // Calculate step
        double dstep = (_task.asDouble(0) - _status.asDouble(0)) / 4;
        step = new T(dstep);
        std::cout << "[Controller] Ctrl step is\t" << *step;

        // Publish task
        typename rsb::Informer<T>::DataPtr task(step);
        this->publish(0, task);
    }

private:
    Controller(Controller &node);

    void operator=(const Controller &node);

    T _task, _status;
};

}
}