iCubBreatherModule.cpp

/* 
 * Copyright (C) 2014 iCub Facility - Istituto Italiano di Tecnologia
 * Author: Alessandro Roncone & Ugo Pattacini
 * email:  alessandro.roncone@iit.it
 * 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/RFModule.h>
#include <yarp/os/Network.h>
#include <yarp/os/PeriodicThread.h>
#include <yarp/os/RpcClient.h>
 
#include <yarp/sig/Vector.h>
#include <yarp/sig/Matrix.h>
 
#include <yarp/math/Math.h>
 
#include <iostream>
#include <string.h> 
 
#include "iCubBreatherThread.h"
 
using namespace yarp;
using namespace yarp::os;
using namespace yarp::sig;
using namespace yarp::math;
 
using namespace std;
 
class iCubBreather: public RFModule
{
    private:
        iCubBreatherThread *iCubBreatherThrd;
        RpcServer           rpcSrvr;
 
    public:
        iCubBreather()
        {
            iCubBreatherThrd=0;
        }
 
        bool respond(const Bottle &command, Bottle &reply)
        {
            int ack =Vocab32::encode("ack");
            int nack=Vocab32::encode("nack");
 
            if (command.size()>0)
            {
                switch (command.get(0).asVocab32())
                {
                    case createVocab32('s','t','a','r'):
                    {
                        int res=Vocab32::encode("started");
                        if (iCubBreatherThrd -> startBreathing())
                        {
                            reply.addVocab32(ack);
                        }
                        else
                            reply.addVocab32(nack);
                        
                        reply.addVocab32(res);
                        return true;
                    }
                    case createVocab32('s','t','o','p'):
                    {
                        int res=Vocab32::encode("stopped");
                        if (iCubBreatherThrd -> stopBreathing())
                        {
                            reply.addVocab32(ack);
                        }
                        else
                            reply.addVocab32(nack);
                        
                        reply.addVocab32(res);
                        return true;
                    }
                    //-----------------
                    default:
                        return RFModule::respond(command,reply);
                }
            }
 
            reply.addVocab32(nack);
            return true;
        }
 
        bool configure(ResourceFinder &rf)
        {
            string name         = "iCubBreather";
            string robot        = "icub";
            string part         = "left_arm";
            int    verbosity    =      0;    // verbosity
            int    rate         =   2000;    // rate of the iCubBreatherThread
            int    numWaypoints =      1;
            bool   autoStart    =  false;
            double noiseStd     =    0.5;
            double refSpeeds    =   10.0;
 
            //******************* NAME ******************
                if (rf.check("name"))
                {
                    name = rf.find("name").asString();
                    yInfo("*** Module name set to %s",name.c_str());  
                }
                else yInfo("*** Module name set to default, i.e. %s",name.c_str());
                setName(name.c_str());
 
            //****************** rate ******************
                if (rf.check("rate"))
                {
                    rate = rf.find("rate").asInt32();
                    yInfo(("*** "+name+": thread working at %i ms").c_str(), rate);
                }
                else yInfo(("*** "+name+": could not find rate in the config file; using %i ms as default").c_str(), rate);
 
            //******************* VERBOSE ******************
                if (rf.check("verbosity"))
                {
                    verbosity = rf.find("verbosity").asInt32();
                    yInfo(("*** "+name+": verbosity set to %i").c_str(),verbosity);
                }
                else yInfo(("*** "+name+": could not find verbosity option in the config file; using %i as default").c_str(),verbosity);
 
            //******************* NOISESTD ******************
                if (rf.check("noiseStd"))
                {
                    noiseStd = rf.find("noiseStd").asFloat64();
                    yInfo(("*** "+name+": noiseStd set to %g").c_str(),noiseStd);
                }
                else yInfo(("*** "+name+": could not find noiseStd option in the config file; using %g as default").c_str(),noiseStd);
 
            //******************* REFPEEDS ******************
                if (rf.check("refSpeeds"))
                {
                    refSpeeds = rf.find("refSpeeds").asFloat64();
                    yInfo(("*** "+name+": refSpeeds set to %g").c_str(),refSpeeds);
                }
                else yInfo(("*** "+name+": could not find refSpeeds option in the config file; using %g as default").c_str(),refSpeeds);
 
            //******************* ROBOT ******************
                if (rf.check("robot"))
                {
                    robot = rf.find("robot").asString();
                    yInfo(("*** "+name+": robot is %s").c_str(),robot.c_str());
                }
                else yInfo(("*** "+name+": could not find robot option in the config file; using %s as default").c_str(),robot.c_str());
 
            //******************* PART *******************
                if (rf.check("part"))
                {
                    part = rf.find("part").asString();
                    yInfo(("*** "+name+": part is %s").c_str(),part.c_str());
                }
                else yInfo(("*** "+name+": could not find part option in the config file; using %s as default").c_str(),part.c_str());
 
            //************* AUTOSTART *******************
                if (rf.check("autoStart"))
                {
                    autoStart = true;
                }
 
            //****************** THREAD ******************
                iCubBreatherThrd = new iCubBreatherThread(rate, name, robot, part, autoStart,
                                                          noiseStd, refSpeeds, verbosity, rf);
 
                iCubBreatherThrd -> start();
                bool strt = 1;
                if (!strt)
                {
                    delete iCubBreatherThrd;
                    iCubBreatherThrd = 0;
                    yError(" iCubBreatherThread wasn't instantiated!!");
                    return false;
                }
                yInfo("ICUB BREATHER: iCubBreatherThread istantiated..");
 
            //************************ RPC ***********************
                rpcSrvr.open(("/"+name+"/rpc:i").c_str());
                attach(rpcSrvr);
 
            return true;
        }
 
        bool close()
        {
            yInfo("ICUB BREATHER: Stopping threads..");
            if (iCubBreatherThrd)
            {
                iCubBreatherThrd->stop();
                delete iCubBreatherThrd;
                iCubBreatherThrd=0;
            }
 
            return true;
        }
 
        double getPeriod()  { return 1.0; }
        bool updateModule() { return true; }
};
 
int main(int argc, char * argv[])
{
    ResourceFinder rf;
    rf.setDefaultContext("funny-things");
    rf.setDefaultConfigFile("iCubBreather.ini");
    rf.configure(argc,argv);
 
    if (rf.check("help"))
    {    
        cout << endl << "IMU IDENTIFIER module" << endl;
        cout << endl << "Options:" << endl;
        cout << "   --context      path:   where to find the called resource. Default gazeStabilization." << endl;
        cout << "   --from         from:   the name of the .ini file. Default iCubBreather.ini." << endl;
        cout << "   --name         name:   the name of the module. Default iCubBreather." << endl;
        cout << "   --robot        robot:  the name of the robot. Default icub." << endl;
        cout << "   --rate         rate:   the period used by the thread. Default 500ms." << endl;
        cout << "   --noiseStd     double: standard deviation of the noise. Default 1.0." << endl;
        cout << "   --refSpeeds    double: The reference speeds at the joints. Default 5.0." << endl;
        cout << "   --verbosity    int:    verbosity level. Default 0." << endl;
        cout << "   --autoStart    flag:   if to autostart the module or not. Default no." << endl;
        cout << endl;
        return 0;
    }
 
    Network yarp;
    if (!yarp.checkNetwork())
    {
        yInfo("No Network!!!\n");
        return 1;
    }
 
    iCubBreather icubBrthr;
    return icubBrthr.runModule(rf);
}
// empty line to make gcc happy