main.cpp

Go to the documentation of this file.

/*
* Copyright (C) 2010 RobotCub Consortium, European Commission FP6 Project IST-004370
* Author: Carlo Ciliberto, Vadim Tikhanoff
* email:   carlo.ciliberto@iit.it vadim.tikhanoff@iit.it
* website: www.robotcub.org
* Permission is granted to copy, distribute, and/or modify this program
* under the terms of the GNU General Public License, version 2 or any
* later version published by the Free Software Foundation.
*
* A copy of the license can be found at
* http://www.robotcub.org/icub/license/gpl.txt
*
* 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
*/
 
#include <yarp/os/all.h>
#include <yarp/sig/Vector.h>
 
#include <string>
 
#include <iCub/utils.h>
#include <iCub/MotorThread.h>
#include <iCub/VisuoThread.h>
 
 
#define ACK                         yarp::os::createVocab32('a','c','k')
#define NACK                        yarp::os::createVocab32('n','a','c','k')
 
#define RPC_HELP                    yarp::os::createVocab32('h','e','l','p')
#define RPC_GET                     yarp::os::createVocab32('g','e','t')
#define RPC_GET_STATUS              yarp::os::createVocab32('s','t','a','t')
#define RPC_IMPEDANCE               yarp::os::createVocab32('i','m','p','e')
#define RPC_S2C_MODE                yarp::os::createVocab32('m','o','d','e')
#define RPC_INTERRUPT               yarp::os::createVocab32('i','n','t','e')
#define RPC_REINSTATE               yarp::os::createVocab32('r','e','i','n')
#define RPC_WAVEING                 yarp::os::createVocab32('w','a','v','e')
#define RPC_ELBOW                   yarp::os::createVocab32('e','l','b','o')
#define RPC_EXECTIME                yarp::os::createVocab32('t','i','m','e')
 
#define CMD_IDLE                    yarp::os::createVocab32('i','d','l','e')
#define CMD_HOME                    yarp::os::createVocab32('h','o','m','e')
#define CMD_CALIBRATE               yarp::os::createVocab32('c','a','l','i')
#define CMD_EXPLORE                 yarp::os::createVocab32('e','x','p','l')
 
#define CMD_OBSERVE                 yarp::os::createVocab32('o','b','s','e')
#define CMD_DROP                    yarp::os::createVocab32('d','r','o','p')
 
#define CMD_HOLD                    yarp::os::createVocab32('h','o','l','d')
 
#define CMD_LEARN_MIL               yarp::os::createVocab32('l','e','a','r')
 
#define CMD_GET                     yarp::os::createVocab32('g','e','t')
#define CMD_TAKE                    yarp::os::createVocab32('t','a','k','e')
#define CMD_GRASP                   yarp::os::createVocab32('g','r','a','s')
#define CMD_TOUCH                   yarp::os::createVocab32('t','o','u','c')
#define CMD_PICK                    yarp::os::createVocab32('p','i','c','k')
#define CMD_PUSH                    yarp::os::createVocab32('p','u','s','h')
#define CMD_POINT                   yarp::os::createVocab32('p','o','i','n')
#define CMD_POINT_FAR               yarp::os::createVocab32('p','f','a','r')
#define CMD_LOOK                    yarp::os::createVocab32('l','o','o','k')
#define CMD_TRACK                   yarp::os::createVocab32('t','r','a','c')
#define CMD_EXPECT                  yarp::os::createVocab32('e','x','p','e')
#define CMD_GIVE                    yarp::os::createVocab32('g','i','v','e')
#define CMD_HAND                    yarp::os::createVocab32('h','a','n','d')
#define CMD_GAZE                    yarp::os::createVocab32('r','e','l','e')
 
//commands for tool
#define CMD_TAKE_TOOL               yarp::os::createVocab32('t','a','t','o')
 
#define CMD_ACTION_TEACH            yarp::os::createVocab32('t','e','a','c')
#define CMD_ACTION_IMITATE          yarp::os::createVocab32('i','m','i','t')
 
//sub commands: get
#define GET_S2C                     yarp::os::createVocab32('s','2','c')
#define GET_TABLE                   yarp::os::createVocab32('t','a','b','l')
#define GET_HOLDING                 yarp::os::createVocab32('h','o','l','d')
#define GET_HAND                    yarp::os::createVocab32('h','a','n','d')
#define GET_IMAGE                   yarp::os::createVocab32('i','m','a','g')
#define GET_IDLE                    yarp::os::createVocab32('i','d','l','e')
 
//sub commands: calib
#define CALIB_TABLE                 yarp::os::createVocab32('t','a','b','l')
#define CALIB_FINGERS               yarp::os::createVocab32('f','i','n','g')
#define CALIB_KIN_OFFSET            yarp::os::createVocab32('k','i','n','e')
 
//sub commands: explore
#define EXPLORE_TORSO               yarp::os::createVocab32('t','o','r','s')
#define EXPLORE_HAND                yarp::os::createVocab32('h','a','n','d')
 
 
#define PORT_TAG_CMD                0
#define PORT_TAG_GET                1
 
#ifdef WIN32
    #pragma warning(disable:4996)
#endif
 
using namespace std;
using namespace yarp::os;
using namespace yarp::sig;
using namespace yarp::dev;
using namespace yarp::math;
using namespace iCub::ctrl;
using namespace iCub::action;
 
class ActionsRenderingEngine
{
protected:
    MotorThread                 *motorThr;
    VisuoThread                 *visuoThr;
 
    Initializer                 *initializer;
 
    bool                        interrupted;
    volatile bool               closing;
    bool                        idle;
 
 
    bool check(Bottle &bot, const string &name)
    {
        bool found=false;
        for(int i=0; i<bot.size(); i++)
        {
            if(bot.get(i).asString()==name)
            {
                found=true;
                break;
            }
        }
 
        return found;
    }
 
 
 
public:
    ActionsRenderingEngine()
    {}
 
    bool initialize(ResourceFinder &rf)
    {
        string name=rf.check("name",Value("actionsRenderingEngine")).asString();
 
        initializer=new Initializer(rf);
 
        motorThr=new MotorThread(rf,initializer);
        visuoThr=new VisuoThread(rf,initializer);
 
        if(!motorThr->start() || !visuoThr->start())
        {
            close();
            return false;
        }
 
        interrupted=false;
        closing=false;
        idle=true;
 
        return true;
    }
 
 
    bool close()
    {
        if(motorThr!=NULL)
        {
            motorThr->reinstate();
            motorThr->stop();
            delete motorThr;
        }
 
        if(visuoThr!=NULL)
        {
            visuoThr->reinstate();
            visuoThr->stop();
            delete visuoThr;
        }
 
        initializer->close();
 
        delete initializer;
 
        return true;
    }
 
    bool interrupt()
    {
        closing=true;
 
        initializer->interrupt();
 
        if(visuoThr!=NULL)
            visuoThr->interrupt();
 
         if(motorThr!=NULL)
            motorThr->interrupt();
 
         return true;
    }
 
    bool respond(const Bottle &command, Bottle &reply)
    {
        if(command.size()==0)
        {
            reply.addString("No command received.");
            return true;
        }
 
        switch(command.get(0).asVocab32())
        {
            case RPC_HELP:
            {
                string rep="Commands:\n";
                rep+="\"interrupt\"\t\t\tinterrupts all the actions untill a 'reinstate' command is issued\n";
                rep+="\"reinstate\"\t\t\treinstates the system (usually called after an 'interrupt' command\n";
                rep+="\"impedance on/off\"\\off\t\t\tset the impedance control on or off\n";
                rep+="\"mode homography\"\\disparity\\network\t\t\tset the 'stereo to cartesian' mode";
                rep+="\"status\"\t\t\tget the module current status\n";
                reply.addString(rep);
                break;
            }
 
            case RPC_GET:
            {
                if(command.size()>1)
                {
                    switch(command.get(1).asVocab32())
                    {
                        case RPC_GET_STATUS:
                        {
                            motorThr->getStatus(reply);
                            break;
                        }
                    }
                }
                break;
            }
 
            //set impedance on or off
            case RPC_IMPEDANCE:
            {
                if(command.get(1).asString()=="on")
                {
                    bool done=motorThr->setImpedance(true);
                    done?reply.addString("Impedance turned on"):reply.addString("Unable to set impedance on");
                }
                else if(command.get(1).asString()=="off")
                {
                    motorThr->setImpedance(false);
                    reply.addString("Impedance turned off");
                }
                else
                {
                    reply.addString("What do you want? Impedance on or off?");
                }
 
                break;
            }
 
            case RPC_WAVEING:
            {
                if(command.get(1).asString()=="on")
                {
                    motorThr->setWaveing(true);
                    reply.addString("Waveing turned on");
                }
                else if(command.get(1).asString()=="off")
                {
                    motorThr->setWaveing(false);
                    reply.addString("Waveing turned off");
                }
                else
                {
                    reply.addString("What do you want? Waveing on or off?");
                }
 
                break;
            }
 
            //set the stereo2cartesian mode
            case RPC_S2C_MODE:
            {
                motorThr->setStereoToCartesianMode(command.get(1).asVocab32(),reply);
                break;
            }
 
            case RPC_INTERRUPT:
            {
                interrupted=true;
                motorThr->interrupt();
                visuoThr->interrupt();
                reply.addString("interrupted");
                break;
            }
 
            case RPC_REINSTATE:
            {
                interrupted=false;
                motorThr->reinstate();
                visuoThr->reinstate();
                reply.addString("reinstated");
                break;
            }
 
            case RPC_ELBOW:
            {
                if (command.size()>=4)
                {
                    string arm=command.get(1).asString();
                    double height=command.get(2).asFloat64();
                    double weight=command.get(3).asFloat64();
 
                    if ((arm=="both") || (arm=="left"))
                        motorThr->changeElbowHeight(LEFT,height,weight);
 
                    if ((arm=="both") || (arm=="right"))
                        motorThr->changeElbowHeight(RIGHT,height,weight);
 
                    reply.addString("elbow parameters updated");
                }
                else
                    reply.addString("missing elbow parameters");
 
                break;
            }
 
            case RPC_EXECTIME:
            {
                if (command.size()>1)
                {
                    double execTime=command.get(1).asFloat64();
                    motorThr->changeExecTime(LEFT,execTime);
                    motorThr->changeExecTime(RIGHT,execTime);
                    reply.addString("execution time updated");
                }
                else
                    reply.addString("missing execution time");
 
                break;
            }
 
            default:
            {
                reply.addString("command not recognized");
                break;
            }
        }
        return true;
    }
 
    bool process(int &port_tag, Bottle &command, Bottle &reply)
    {
        if(closing)
        {
            reply.addVocab32(NACK);
            reply.addString("Sorry. Module closing.");
            return true;
        }
 
        if(command.size()==0)
        {
            reply.addVocab32(NACK);
            reply.addString("No command received.");
        }
        else if(interrupted)
        {
            reply.addVocab32(NACK);
            reply.addString("Module currently interrupted. Reinstate for action.");
        }
        else
        {
            if(port_tag==PORT_TAG_GET)
            {
                if(command.get(0).asVocab32()!=CMD_GET)
                {
                    reply.addVocab32(NACK);
                    reply.addString("This port can only process [get] commands");
                }
                else if(command.size()>1)
                {
                    switch(command.get(1).asVocab32())
                    {
                        case GET_S2C:
                        {
                            if(command.size()>2)
                            {
                                for(int target_idx=2; target_idx<command.size(); target_idx++)
                                {
                                    Vector xd;
                                    Bottle tmp_command;
                                    visuoThr->getTarget(command.get(target_idx),tmp_command);
 
                                    Bottle &tmp_reply=reply.addList();
                                    if(motorThr->targetToCartesian(tmp_command.find("target").asList(),xd))
                                    {
                                        for(size_t i=0; i<xd.size(); i++)
                                            tmp_reply.addFloat64(xd[i]);
                                    }
                                    else
                                        tmp_reply.addVocab32(NACK);
                                }
                            }
 
                            break;
                        }
                        case GET_IDLE:
                        {
                            reply.clear();
                            if(idle)
                                reply.addVocab32(ACK);
                            else
                                reply.addVocab32(NACK);
 
                            break;
                        }
 
                        case GET_TABLE:
                        {
                            reply.clear();
                            double table_height;
                            if(motorThr->getTableHeight(&table_height))
                            {
                                Bottle &tmp_reply=reply.addList();
                                tmp_reply.addString("table_height");
                                tmp_reply.addFloat64(table_height);
                            }
                            else
                                reply.addVocab32(NACK);
 
                            break;
                        }
 
                        case GET_HOLDING:
                        {
                            reply.clear();
                            if(motorThr->isHolding(command))
                                reply.addVocab32(ACK);
                            else
                                reply.addVocab32(NACK);
 
                            break;
                        }
 
                        case GET_HAND:
                        {
                            if(command.get(2).asVocab32()==GET_IMAGE)
                            {
                                reply.clear();
                                if(!motorThr->getHandImagePosition(reply))
                                    reply.addVocab32(NACK);
                            }
                            else
                                reply.addVocab32(NACK);
 
                            break;
                        }
 
 
                        default:
                        {
                            reply.addVocab32(NACK);
                            break;
                        }
                    }
                }
                else
                    reply.addVocab32(NACK);
            }
            else if(port_tag==PORT_TAG_CMD)
            {
                switch(command.get(0).asVocab32())
                {
                    case CMD_IDLE:
                    {
                        motorThr->setGazeIdle();
                        reply.addVocab32(ACK);
                        break;
                    }
 
                    case CMD_HOME:
                    {
                        motorThr->goHome(command);
                        reply.addVocab32(ACK);
 
                        break;
                    }
                    case CMD_HAND:
                    {
                        bool holding;
                        if (motorThr->hand(command,"",&holding))
                        {
                            reply.addVocab32(ACK);
                            reply.addInt32(holding?1:0);
                        }
                        else
                            reply.addVocab32(NACK);
 
                        break;
                    }
                    case CMD_GAZE:
                    {
                        motorThr->clearIt(command);
                        reply.addVocab32(ACK);
 
                        break;
                    }
 
                    //----------- for retro_compatibility -----------------
                    case CMD_GET:
                    {
                        if(command.size()>1)
                        {
                            switch(command.get(1).asVocab32())
                            {
                                case GET_S2C:
                                {
                                    if(command.size()>2)
                                    {
                                        Vector xd;
                                        visuoThr->getTarget(command.get(2),command);
                                        if(motorThr->targetToCartesian(command.find("target").asList(),xd))
                                        {
                                            reply.clear();
                                            for(size_t i=0; i<xd.size(); i++)
                                                reply.addFloat64(xd[i]);
                                        }
                                        else
                                            reply.addVocab32(NACK);
                                    }
 
                                    break;
                                }
                            }
 
                            break;
                        }
 
                        break;
                    }
                    //-----------------------------------------------------
 
                    case CMD_EXPLORE:
                    {
                        if(command.size()>1)
                        {
                            switch(command.get(1).asVocab32())
                            {
                                case EXPLORE_TORSO:
                                {
                                    if(motorThr->exploreTorso(command))
                                        reply.addVocab32(ACK);
                                    else
                                        reply.addVocab32(NACK);
 
                                    break;
                                }
 
                                case EXPLORE_HAND:
                                {
                                    if(motorThr->exploreHand(command))
                                        reply.addVocab32(ACK);
                                    else
                                        reply.addVocab32(NACK);
 
                                    break;
                                }
 
                                default:
                                {
                                    string rep=command.get(1).asString();
                                    reply.addVocab32(NACK);
                                    reply.addString("parameter '"+rep+"' not supported by 'explore' command.");
                                    break;
                                }
                            }
                        }
                        else
                        {
                            reply.addVocab32(NACK);
                            reply.addString("explore command needs further parameter (e.g. 'torso')");
                        }
                        break;
                    }
 
                    case CMD_CALIBRATE:
                    {
                        switch(command.get(1).asVocab32())
                        {
                            case CALIB_TABLE:
                            {
                                idle = false;
                                if(motorThr->calibTable(command))
                                    reply.addVocab32(ACK);
                                else
                                    reply.addVocab32(NACK);
                                idle = true;
                                break;
                            }
 
                             case CALIB_FINGERS:
                            {
                                if(motorThr->calibFingers(command))
                                    reply.addVocab32(ACK);
                                else
                                    reply.addVocab32(NACK);
 
                                break;
                            }
 
                             case CALIB_KIN_OFFSET:
                            {
                                if(command.get(2).asString()=="start")
                                {
                                    if (command.size()>=4)
                                        if (command.get(3).isList())
                                            visuoThr->getTarget(command.get(3),command);
                                    if (motorThr->startLearningModeKinOffset(command))
                                    {
                                        reply.addVocab32(ACK);
                                        reply.addString("learn kinematic offset mode: on");
                                    }
                                    else
                                        reply.addVocab32(NACK);
                                }
                                else if(command.get(2).asString()=="stop")
                                {
                                    if(motorThr->suspendLearningModeKinOffset(command))
                                    {
                                        reply.addVocab32(ACK);
                                        reply.addString("learn kinematic offset mode: off");
                                    }
                                    else
                                        reply.addVocab32(NACK);
                                }
 
                                break;
                            }
                        }
 
                        break;
                    }
 
                    //------ action "learning" ---------//
                    case CMD_ACTION_TEACH:
                    {
                        if(check(command,"start"))
                        {
                            string action_name=command.get(1).asString();
 
                            Bottle &action=command.addList();
                            action.addString("action_name");
                            action.addString(action_name);
 
                            if(!motorThr->startLearningModeAction(command))
                            {
                                reply.addVocab32(NACK);
                                reply.addString("action "+action_name+" already known");
                            }
                            else
                            {
                                motorThr->setGazeIdle();
                                motorThr->lookAtHand(command);
                                reply.addVocab32(ACK);
                                reply.addString("start teaching");
                            }
                        }
 
                        if(check(command,"stop"))
                        {
                            if(motorThr->suspendLearningModeAction(command))
                            {
                                motorThr->setGazeIdle();
                                reply.addVocab32(ACK);
                                reply.addString("stop teaching");
                            }
                            else
                                reply.addVocab32(NACK);
                        }
 
                        break;
                    }
 
                    case CMD_ACTION_IMITATE:
                    {
                        string action_name=command.get(1).asString();
 
                        Bottle &action=command.addList();
                        action.addString("action_name");
                        action.addString(action_name);
 
                        motorThr->lookAtHand(command);
 
                        if(!motorThr->imitateAction(command))
                        {
                            reply.addVocab32(NACK);
                            reply.addString("action "+action_name+" unknown");
                        }
                        else
                        {
                            reply.addVocab32(ACK);
                            reply.addString("action "+action_name+" done");
                        }
                        motorThr->setGazeIdle();
 
                        break;
                    }
                    //-----------------------------------//
 
 
                    case CMD_LEARN_MIL:
                    {
                        string obj_name=command.get(1).asString();
                        if(motorThr->isHolding(command)) //
                            motorThr->deploy(command);
 
                        //if it is not currently tracking anything, start learning what it has in fixation
                        Vector stereo;
                        if(!visuoThr->isTracking())
                        {
                            Value v("fix");
                            visuoThr->getTarget(v,command);
                        }
 
                        visuoThr->startLearningMIL(obj_name);
                        motorThr->exploreTorso(command);
                        visuoThr->trainMIL();
 
                        reply.addString(obj_name + " learned");
                        break;
                    }
 
                    case CMD_OBSERVE:
                    {
                        if(!motorThr->isHolding(command))
                        {
                            reply.addVocab32(NACK);
                            reply.addString("Nothing to observe. Not holding anything");
                            motorThr->release(command);
                            motorThr->goHome(command);
                            break;
                        }
 
                        motorThr->lookAtHand(command);
                        motorThr->drawNear(command);
 
                        reply.addVocab32(ACK);
 
                        break;
                    }
 
                    case CMD_EXPECT:
                    {
                        if(!motorThr->expect(command))
                        {
                            motorThr->setGazeIdle();
                            motorThr->release(command);
                            motorThr->goHome(command);
                            reply.addVocab32(NACK);
                            break;
                        }
 
                        motorThr->grasp(command);
 
                        if(motorThr->isHolding(command))
                        {
                            if (check(command,"near"))
                                motorThr->drawNear(command);
                            else
                            {
                                motorThr->setGazeIdle();
                                Bottle b;
                                b.addString("head");
                                b.addString("arms");
                                motorThr->goHome(b);
                            }
 
                            reply.addVocab32(ACK);
                        }
                        else
                        {
                           motorThr->setGazeIdle();
                           motorThr->release(command);
                           motorThr->goHome(command);
                           reply.addVocab32(NACK);
                        }
 
                        break;
                    }
 
                    case CMD_GIVE:
                    {
                        if(!motorThr->isHolding(command))
                        {
                            reply.addVocab32(NACK);
                            reply.addString("Nothing to give. Not holding anything");
                            motorThr->release(command);
                            motorThr->goHome(command);
                            break;
                        }
 
                        motorThr->give(command);
 
                       motorThr->setGazeIdle();
                       motorThr->release(command);
                       motorThr->goHome(command);
                       reply.addVocab32(NACK);
 
                        break;
                    }
 
                    case CMD_DROP:
                    {
                        idle = false;
                        if(check(command,"over") && command.size()>2)
                            visuoThr->getTarget(command.get(2),command);
 
                        motorThr->setGazeIdle();
 
                        motorThr->deploy(command);
                        motorThr->keepFixation(command);
                        motorThr->goUp(command);
 
                        motorThr->goHome(command);
                        motorThr->setGazeIdle();
 
                        reply.addVocab32(ACK);
                        idle = true;
                        break;
                    }
 
                    case CMD_TAKE:
                    {
                        idle = false;
                        if(command.size()<2)
                        {
                            reply.addVocab32(NACK);
                            break;
                        }
 
                        visuoThr->getTarget(command.get(1),command);
 
                        command.addString("pretake_hand");
                        if(!motorThr->reach(command))
                        {
                            reply.addVocab32(NACK);
                            break;
                        }
 
                        motorThr->lookAtHand(command);
                        motorThr->shiftAndGrasp(command);
 
                        if (motorThr->isHolding(command))
                        {
                            if (check(command,"near"))
                                motorThr->drawNear(command);
                            else
                            {
                                motorThr->goUp(command);
                                motorThr->setGazeIdle();
 
                                if (!check(command,"still"))
                                {
                                    Bottle b;
                                    b.addString("head");
                                    b.addString("arms");
                                    motorThr->goHome(b);
                                }
                            }
 
                            reply.addVocab32(ACK);
                        }
                        else
                        {
                           motorThr->setGazeIdle();
                           motorThr->release(command);
 
                           if (!check(command,"still"))
                           {
                               motorThr->goUp(command);
                               motorThr->goHome(command);
                           }
 
                           reply.addVocab32(NACK);
                        }
                        idle = true;
 
                        break;
                    }
 
                    case CMD_GRASP:
                    {
                        idle = false;
                        if (command.size()<2)
                        {
                            reply.addVocab32(NACK);
                            break;
                        }
 
                        visuoThr->getTarget(command.get(1),command);
                        if (motorThr->powerGrasp(command))
                        {
                            motorThr->goUp(command);
                            motorThr->setGazeIdle();
 
                            if (!check(command,"still"))
                            {
                                Bottle b;
                                b.addString("head");
                                b.addString("arms");
                                motorThr->goHome(b);
                            }
 
                            reply.addVocab32(ACK);
                        }
                        else
                        {
                            motorThr->setGazeIdle();
                            motorThr->release(command);
                            if (!check(command,"still"))
                                motorThr->goHome(command);
 
                            reply.addVocab32(NACK);
                        }
 
                        idle = true;
                        break;
                    }
 
                    case CMD_TOUCH:
                    {
                         if(command.size()<2)
                        {
                            reply.addVocab32(NACK);
                            break;
                        }
 
                        visuoThr->getTarget(command.get(1),command);
 
                        command.addString("touch");
                        if(!motorThr->reach(command))
                        {
                            reply.addVocab32(NACK);
                            break;
                        }
 
                        if(!check(command,"still"))
                        {
                            Time::delay(2.0);
                            motorThr->goUp(command);
                            motorThr->goHome(command);
                        }
 
                        reply.addVocab32(ACK);
                        break;
                    }
 
                    case CMD_PUSH:
                    {
                        if(command.size()<2)
                        {
                            reply.addVocab32(NACK);
                            break;
                        }
 
                        visuoThr->getTarget(command.get(1),command);
 
                        if (!motorThr->push(command))
                        {
                            reply.addVocab32(NACK);
                            break;
                        }
 
                        if (!check(command,"still"))
                        {
                            motorThr->setGazeIdle();
                            motorThr->goUp(command);
                            motorThr->goHome(command);
                        }
 
                        reply.addVocab32(ACK);
                        break;
                    }
 
                    case CMD_POINT:
                    {
                        if(command.size()<2)
                        {
                            reply.addVocab32(NACK);
                            break;
                        }
 
                        visuoThr->getTarget(command.get(1),command);
 
                        if(!motorThr->point(command))
                        {
                            reply.addVocab32(NACK);
                            break;
                        }
 
                        if(!check(command,"still"))
                        {
                            Time::delay(2.0);
                            motorThr->goHome(command);
                        }
 
                        reply.addVocab32(ACK);
                        break;
                    }
                    case CMD_POINT_FAR:
                    {
                        if(command.size()<2)
                        {
                            reply.addVocab32(NACK);
                            break;
                        }
 
                        visuoThr->getTarget(command.get(1),command);
 
                        if(!motorThr->point_far(command))
                        {
                            reply.addVocab32(NACK);
                            break;
                        }
 
                        if(!check(command,"still"))
                        {
                            Time::delay(2.0);
                            motorThr->goHome(command);
                        }
 
                        reply.addVocab32(ACK);
                        break;
                    }
                    case CMD_LOOK:
                    {
                        if(command.size()<2)
                        {
                            reply.addVocab32(NACK);
                            break;
                        }
 
                        if(!check(command,"hand"))
                            visuoThr->getTarget(command.get(1),command);
 
                        if(!motorThr->look(command))
                        {
                            reply.addVocab32(NACK);
                            break;
                        }
 
                        reply.addVocab32(ACK);
                        break;
                    }
 
                    case CMD_TRACK:
                    {
                        if (command.get(1).asVocab32()!=Vocab32::encode("motion"))
                            visuoThr->getTarget(command.get(1),command);
                        else
                            visuoThr->trackMotion();
 
                        motorThr->track(command);
                        reply.addVocab32(ACK);
                        break;
                    }
                    case CMD_TAKE_TOOL:
                    {
                        if (command.size()<2)
                        {
                            reply.addVocab32(NACK);
                            break;
                        }
 
                        motorThr->takeTool(command);
                        reply.addVocab32(ACK);
 
                        break;
                    }
                    default:
                    {
                        reply.addVocab32(NACK);
                        break;
                    }
                }
            }
 
        }
 
        if(reply.isNull() || reply.size()==0)
        {
            reply.addVocab32(NACK);
            reply.addString("Random Error");
        }
 
        return true;
    }
 
 
};
 
 
 
 
 
class ARE_PortReader: public PortReader
{
protected:
    ActionsRenderingEngine                      *are;
 
    int                                         port_tag;
 
 
    bool read(ConnectionReader &connection)
    {
        Bottle command, reply;
        if (!command.read(connection))
            return false;
 
        //do stuff with the ARE here
        are->process(port_tag,command,reply);
        //-------------
 
        if (ConnectionWriter *writer=connection.getWriter())
            reply.write(*writer);
 
        return true;
    }
 
public:
    ARE_PortReader(ActionsRenderingEngine *_are, int _port_tag)
        :are(_are),port_tag(_port_tag)
    {}
 
};
 
 
 
class ActionsRenderingEngineModule: public RFModule
{
protected:
    ActionsRenderingEngine      *are;
 
    ARE_PortReader              *port_reader_cmd;
    ARE_PortReader              *port_reader_get;
 
    RpcServer                   port_cmd;
    RpcServer                   port_get;
    RpcServer                   port_rpc;
 
 
public:
    ActionsRenderingEngineModule()
    {}
 
    virtual bool configure(ResourceFinder &rf)
    {
        string name=rf.check("name",Value("actionsRenderingEngine")).asString();
        setName(name.c_str());
 
        are=new ActionsRenderingEngine();
 
        if(!are->initialize(rf))
        {
            yError("ARE could not be initialized!");
            return false;
        }
 
        port_reader_cmd=new ARE_PortReader(are,PORT_TAG_CMD);
        port_reader_get=new ARE_PortReader(are,PORT_TAG_GET);
 
        port_cmd.setReader(*port_reader_cmd);
        port_get.setReader(*port_reader_get);
 
        port_cmd.open("/"+name+"/cmd:io");
        port_get.open("/"+name+"/get:io");
        port_rpc.open("/"+name+"/rpc");
 
        attach(port_rpc);
 
        return true;
    }
 
    virtual bool interruptModule()
    {
        port_cmd.interrupt();
        port_get.interrupt();
        port_rpc.interrupt();
 
        are->interrupt();
 
        return true;
    }
 
    virtual bool close()
    {
        port_cmd.close();
        port_get.close();
        port_rpc.close();
 
        are->close();
 
        delete port_reader_cmd;
        delete port_reader_get;
 
        delete are;
 
        return true;
    }
 
    virtual double getPeriod()
    {
        return 0.1;
    }
 
    virtual bool updateModule()
    {
        return true;
    }
 
    virtual bool respond(const Bottle &command, Bottle &reply)
    {
        if(are->respond(command,reply))
            return true;
 
        return RFModule::respond(command,reply);
    }
};
 
 
int main(int argc, char *argv[])
{
    Network yarp;
 
    if (!yarp.checkNetwork())
        return 1;
 
    ResourceFinder rf;
    rf.setDefaultContext("actionsRenderingEngine");
    rf.setDefaultConfigFile("config.ini");
    rf.setDefault("name","actionsRenderingEngine");
    rf.configure(argc,argv);
 
    ActionsRenderingEngineModule mod;
 
    return mod.runModule(rf);
}