worldsim/src/worldcontroller.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 "worldcontroller.h"
#include "phybox.h"
#include "physphere.h"
#include "phycylinder.h"
#include "wcamera.h"
#include "phyicub.h"
#include <QCoreApplication>
#include <QTimerEvent>

using namespace yarp::os;

namespace farsa {

ServerWorldController::ServerWorldController( WorldController* ctrl ) : QThread() {
    wctrl = ctrl;
    //--- list of commands
    asyn_cmds["start"] = &ServerWorldController::startCmd;
    asyn_cmds["pause"] = &ServerWorldController::pauseCmd;
    asyn_cmds["stop"] = &ServerWorldController::stopCmd;
    asyn_cmds["advance"] = &ServerWorldController::advanceCmd;
    //--- Sync Commands
    syn_cmds["mk"] = &ServerWorldController::makeCmd;
    syn_cmds["set"] = &ServerWorldController::setCmd;
    setTerminationEnabled( true );
}

ServerWorldController::~ServerWorldController() {
    //--- nothing to do
}

void ServerWorldController::applyQueuedCommands() {
    Port* port = wctrl->outPort();
    while( !queue_cmds.isEmpty() ) {
        Bottle cmd = queue_cmds.dequeue();
        Bottle res;
        QString cmd0( cmd.get(0).toString().c_str() );
        //--- we are sure that there is an entry into sync_cmds
        //--- because it was checked into run loop
        res = (this->*(syn_cmds[cmd0]))( cmd );
        port->write(res);
    }
}

void ServerWorldController::run() {
    Port* inport = wctrl->inPort();
    Port* outport = wctrl->outPort();
    while( true ) {
        Bottle cmd;
        Bottle res;
        //--- read the command... and will reply on outPort
        inport->read( cmd, false );
        //--- checking the command
        QString cmd0( cmd.get(0).asString().c_str() );
        if ( asyn_cmds.contains( cmd0 ) ) {
            res = (this->*(asyn_cmds[cmd0]))( cmd );
        } else if ( syn_cmds.contains( cmd0 ) ) {
            res.addString( "Command Queued" );
            queue_cmds.enqueue( cmd );
        } else {
            res.addString( "Unsupported Command: " );
            res.addString( cmd.get(0).toString() );
        }
        //--- reply
        outport->write(res);
    }
}

yarp::os::Bottle ServerWorldController::startCmd( const yarp::os::Bottle& ) {
    QCoreApplication::postEvent( wctrl->world(), new QEvent( (QEvent::Type)(World::E_Play) ) );
    Bottle res;
    res.addString("World Running");
    return res;
}

yarp::os::Bottle ServerWorldController::pauseCmd( const yarp::os::Bottle& ) {
    QCoreApplication::postEvent( wctrl->world(), new QEvent( (QEvent::Type)(World::E_Pause) ) );
    Bottle res;
    res.addString("World Paused");
    return res;
}

yarp::os::Bottle ServerWorldController::stopCmd( const yarp::os::Bottle& ) {
    QCoreApplication::postEvent( wctrl->world(), new QEvent( (QEvent::Type)(World::E_Stop) ) );
    Bottle res;
    res.addString("World Stopped");
    return res;
}

yarp::os::Bottle ServerWorldController::advanceCmd( const yarp::os::Bottle& ) {
    Bottle res;
    if ( wctrl->world()->status() == World::playingS ) {
        res.addString("World is currently running, pause it before use advance command");
    } else {
        QCoreApplication::postEvent( wctrl->world(), new QEvent( (QEvent::Type)(World::E_Advance) ) );
        res.addString("World Advanced");
    }
    return res;
}

yarp::os::Bottle ServerWorldController::makeCmd( const yarp::os::Bottle& cmd ) {
    Bottle res;
    // --- switch over the type of object to create
    QString objtype = cmd.get(1).toString().c_str();
    //--- The rotation will be computed on the basis of Pitch-Yaw-Roll rotations about
    //    the given euler angles (ZYX) convention
    //--- there are many variants of the "mk" command depending on the second parameter:
    //   mk box name x_dim y_dim z_dim x_pos y_pos z_pos x_rot y_rot z_rot
    //   mk sphere name radius x_pos y_pos z_pos x_rot y_rot z_rot
    //   mk cylinder name radius length x_pos y_pos z_pos x_rot y_rot z_rot
    //   mk icub name x_pos y_pos z_pos x_rot y_rot z_rot
    //   mk camera name x_pos y_pos z_pos x_rot y_rot z_rot width heigth
    // ------- following not yet implemented commands
    //   mk fixed name obj_parent obj_child
    //   mk hinge name obj_parent obj_child axis centre startAngle
    if ( objtype == "box" ) {
        // --- create a box
        real x_dim = 1.0;
        real y_dim = 1.0;
        real z_dim = 1.0;
        wMatrix tm = wMatrix::identity();
        Value v = cmd.get(2);
        if ( v.isNull() ) {
            res.addString( "You must specify a name for the object" );
            return res;
        }
        QString name = v.toString().c_str();
        x_dim = cmd.get(3).asDouble();
        y_dim = cmd.get(4).asDouble();
        z_dim = cmd.get(5).asDouble();
        if ( x_dim == 0 || y_dim == 0 || z_dim == 0 ) {
            res.addString( "Dimension of the object must be greater than zero" );
            return res;
        }
        wVector pos( cmd.get(6).asDouble(), cmd.get(7).asDouble(), cmd.get(8).asDouble() );
        tm = tm * wMatrix::pitch( toRad( cmd.get(9).asDouble() ) );
        tm = tm * wMatrix::yaw( toRad( cmd.get(10).asDouble() ) );
        tm = tm * wMatrix::roll( toRad( cmd.get(11).asDouble() ) );
        tm.w_pos = pos;
        new PhyBox( x_dim, y_dim, z_dim, wctrl->world(), name, tm );
    } else if ( objtype == "sphere" ) {
        // --- create a sphere
        real radius = 1.0;
        wMatrix tm = wMatrix::identity();
        Value v = cmd.get(2);
        if ( v.isNull() ) {
            res.addString( "You must specify a name for the object" );
            return res;
        }
        QString name = v.toString().c_str();
        radius = cmd.get(3).asDouble();
        if ( radius == 0 ) {
            res.addString( "Dimension of the object must be greater than zero" );
            return res;
        }
        wVector pos( cmd.get(4).asDouble(), cmd.get(5).asDouble(), cmd.get(6).asDouble() );
        tm = tm * wMatrix::pitch( toRad( cmd.get(7).asDouble() ) );
        tm = tm * wMatrix::yaw( toRad( cmd.get(8).asDouble() ) );
        tm = tm * wMatrix::roll( toRad( cmd.get(9).asDouble() ) );
        tm.w_pos = pos;
        new PhySphere( radius, wctrl->world(), name, tm );
    } else if ( objtype == "cylinder" ) {
        // --- create a cylinder
        real radius = 1.0;
        real len = 1.0;
        wMatrix tm = wMatrix::identity();
        Value v = cmd.get(2);
        if ( v.isNull() ) {
            res.addString( "You must specify a name for the object" );
            return res;
        }
        QString name = v.toString().c_str();
        radius = cmd.get(3).asDouble();
        len = cmd.get(4).asDouble();
        if ( radius == 0 || len == 0 ) {
            res.addString( "Dimension of the object must be greater than zero" );
            return res;
        }
        wVector pos( cmd.get(5).asDouble(), cmd.get(6).asDouble(), cmd.get(7).asDouble() );
        tm = tm * wMatrix::pitch( toRad(cmd.get(8).asDouble()) );
        tm = tm * wMatrix::yaw( toRad(cmd.get(9).asDouble()) );
        tm = tm * wMatrix::roll( toRad(cmd.get(10).asDouble()) );
        tm.w_pos = pos;
        new PhyCylinder( radius, len, wctrl->world(), name, tm );
    } else if ( objtype == "icub" ) {
        // --- create a iCub
        wMatrix tm = wMatrix::identity();
        Value v = cmd.get(2);
        if ( v.isNull() ) {
            res.addString( "You must specify a name for the object" );
            return res;
        }
        QString name = v.toString().c_str();
        wVector pos( cmd.get(3).asDouble(), cmd.get(4).asDouble(), cmd.get(5).asDouble() );
        tm = tm * wMatrix::pitch( toRad(cmd.get(6).asDouble()) );
        tm = tm * wMatrix::yaw( toRad(cmd.get(7).asDouble()) );
        tm = tm * wMatrix::roll( toRad(cmd.get(8).asDouble()) );
        tm.w_pos = pos;
        PhyiCub* icub = new PhyiCub( wctrl->world(), name, tm );
        icub->blockTorso0( true );
    } else if ( objtype == "camera" ) {
        // --- create a camera
        wMatrix tm = wMatrix::identity();
        Value v = cmd.get(2);
        if ( v.isNull() ) {
            res.addString( "You must specify a name for the object" );
            return res;
        }
        QString name = v.toString().c_str();
        wVector pos( cmd.get(3).asDouble(), cmd.get(4).asDouble(), cmd.get(5).asDouble() );
        tm = tm * wMatrix::pitch( toRad(cmd.get(6).asDouble()) );
        tm = tm * wMatrix::yaw( toRad(cmd.get(7).asDouble()) );
        tm = tm * wMatrix::roll( toRad(cmd.get(8).asDouble()) );
        tm.w_pos = pos;
        unsigned int width = cmd.get(9).asInt();
        unsigned int height = cmd.get(10).asInt();
        if ( width == 0 || height == 0 ) {
            res.addString( "Dimension of the frame must be greater than zero" );
            return res;
        }
        new WCamera( wctrl->world(), name, width, height, tm );
    }
    res.addString("Make Command executed");
    return res;
}

yarp::os::Bottle ServerWorldController::setCmd( const yarp::os::Bottle& cmd ) {
    // --- the set command is quite complex but essentialy the structure is the following
    //     set [object_name] [property_name] [parameters]
    //     if the object_name is "world" then it configure the world's properties
    //     property_name specify what you want to change and it can be one of the following:
    //  position : and three number of the new position of the object
    //  rotation : and three number specifing angle of rotation about X, Y and Z axes
    //     Only in the case of "world" there are the following property
    //  size     : dimension of the world
    //  timestep : time interval of integration step
    //  threads  : if more than one activate the multi-thread capability
    //  realtime : on or off
    QString objname = cmd.get(1).toString().c_str();
    if ( objname == "world" ) {
        return setWorldCmd( cmd );
    } else {
        WObject* obj = wctrl->world()->getObject( objname );
        if ( obj != NULL ) {
            return setObjectCmd( cmd, obj );
        }
    }
    Bottle res;
    res.addString( "Object not found" );
    return res;
}

yarp::os::Bottle ServerWorldController::setWorldCmd( const yarp::os::Bottle& cmd ) {
    //--- see setCmd comments for understand the behaviour
    Bottle res;
    QString propname = cmd.get(2).toString().c_str();
    if ( propname == "size" ) {
        real x_dim = fabs( cmd.get(3).asDouble()/2.0 );
        real y_dim = fabs( cmd.get(4).asDouble()/2.0 );
        real z_dim = fabs( cmd.get(5).asDouble() );
        if ( x_dim == 0 || y_dim == 0 || z_dim == 0 ) {
            res.addString( "Dimension of the world must be greater than zero" );
            return res;
        }
        wctrl->world()->setSize( wVector( -x_dim, -y_dim, 0 ), wVector( +x_dim, +y_dim, +z_dim ) );
        res.addString( "World Re-Sized" );
    } else if ( propname == "timestep" ) {
        real newtimestep = fabs( cmd.get(3).asDouble() );
        if ( newtimestep == 0 ) {
            res.addString( "TimeStep of the world must be greater than zero" );
            return res;
        }
        wctrl->world()->setTimeStep( newtimestep );
        res.addString( "New TimeStep setted" );
    } else if ( propname == "threads" ) {
        real numThreads = fabs( (float)(cmd.get(3).asInt()) );
        if ( numThreads == 0 ) {
            res.addString( "Threads of the world must be greater than zero" );
            return res;
        }
        wctrl->world()->setMultiThread( numThreads );
        res.addString( "Multi-Thread setted" );
    } else if ( propname == "realtime" ) {
        bool ison = ( QString(cmd.get(3).toString().c_str()) == "on" );
        wctrl->world()->setIsRealTime( ison );
        if ( ison ) {
            res.addString( "RealTime Activated" );
        } else {
            res.addString( "RealTime De-Activated" );
        }
    }
    return res;
}

yarp::os::Bottle ServerWorldController::setObjectCmd( const yarp::os::Bottle& /*cmd*/, WObject* /*object*/ ) {
    //--- see setCmd comments for understand the behaviour
    Bottle res;
    res.addString( "Not Yet Implemented" );
    return res;
}

WorldController::WorldController( World* w, QString name )
    : YarpObject( w, name ) {
    inport = new Port();
    outport = new Port();
    inport->open( QString( "/%1/%2:i" ).arg( world()->name() ).arg( name ).toAscii().data() );
    outport->open( QString( "/%1/%2:o" ).arg( world()->name() ).arg( name ).toAscii().data() );
    w->pushObject( this );
    srv = new ServerWorldController( this );
    srv->start();
}

WorldController::~WorldController() {
    srv->terminate();
    inport->close();
    outport->close();
    delete srv;
    //--- this delete blocks MAC application
    //delete inport;
    //delete outport;
}

void WorldController::update() {
    srv->applyQueuedCommands();
}

} // end namespace farsa

#endif //FARSA_USE_YARP_AND_ICUB