main.cpp

Go to the documentation of this file.

// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
// Copyright (C) 2008 RobotCub Consortium
// Author: Lorenzo Natale
// CopyPolicy: Released under the terms of the GNU GPL v2.0.
 
#include <stdio.h>
 
#include <yarp/os/Network.h>
#include <yarp/os/Property.h>
#include <yarp/os/Time.h>
#include <yarp/dev/PolyDriver.h>
#include <yarp/os/BufferedPort.h>
#include <yarp/os/RFModule.h>
#include "velControlThread.h"
#include <yarp/os/Log.h>
#include <yarp/os/LogStream.h>
#include <string.h>
 
/*default rate for the control loop*/
const int CONTROL_RATE=20;
 
using namespace yarp::os;
using namespace yarp::dev;
 
class VelControlModule: public RFModule {
private:
    PolyDriver driver;
    velControlThread *vc;
    char partName[255];
  
    //added by ludovic
    Port input_port;
public:
  
    virtual bool respond(const yarp::os::Bottle &command, yarp::os::Bottle &reply)
    {
        reply.clear(); 
        yDebug("receiving command from port\n");
        int index = 0;
        int cmdSize = command.size();
    
        while(cmdSize>0)
            {
                switch(command.get(index).asVocab32())
                {
                    case yarp::os::createVocab32('s','u','s','p'):
                        reply.addVocab32("ack");
                        vc->halt();
                        cmdSize--;
                        index++;
                    break;
                    case yarp::os::createVocab32('r','u','n'):
                        reply.addVocab32("ack");
                        vc->go();
                        cmdSize--;
                        index++;
                    break;
                    //this set current position reference
                    case yarp::os::createVocab32('s','e','t'):
                        if (command.size()>=3)
                            {
                                int i=command.get(index+1).asInt32();
                                double pos=command.get(index+2).asFloat64();
                                vc->setRef(i, pos);
                                index +=3;
                                cmdSize-=3;
                            }
                        else
                            {
                                cmdSize--;
                                index++;
                                yError("Invalid set message, ignoring\n");
                            }
                        reply.addVocab32("ack");
                    break;
                    //this set maximum velocity (limiter)
                    case yarp::os::createVocab32('s','v','e','l'):
                        if(command.size()>=3)
                            {
                                int i=command.get(index+1).asInt32();
                                double vel = command.get(index+2).asFloat64();
                                vc->setVel(i,vel);
                                index += 3;
                                cmdSize-=3;;
                                reply.addVocab32("ack");
                            }
                        else
                            {
                                cmdSize--;
                                index++;
                                yError("Invalid set vel message, ignoring\n");
                                reply.addVocab32("fail");
                            }
                    break;
                    case yarp::os::createVocab32('g','a','i','n'):
                        if(command.size()>=3)
                            {
                                int i=command.get(index+1).asInt32();
                                double gain = command.get(index+2).asFloat64();
                                vc->setGain(i,gain);
                                index+=3;
                                cmdSize-=3;
                                reply.addVocab32("ack");
                            }
                        else
                            {
                                cmdSize--;
                                index++;
                                yError("Invalid set gain message, ignoring\n");
                                reply.addVocab32("fail");
                            }
                    break;
                    case yarp::os::createVocab32('h','e','l','p'):
                        fprintf(stdout,"VelocityControl module, valid commands are:\n");
                        fprintf(stdout,"-   [susp]         suspend the controller (command zero velocity)\n");
                        fprintf(stdout,"-   [run]          start (and resume after being suspended) the controller\n");
                        fprintf(stdout,"-   [quit]         quit the module (exit)\n");
                        fprintf(stdout,"-   [set]  <j> <p> move joint j to p (degrees)\n");
                        fprintf(stdout,"-   [svel] <j> <v> set maximum speed for joint j to v (deg/sec)\n");
                        fprintf(stdout,"-   [gain] <j> <k> set P gain for joint j to k\n");
                        fprintf(stdout,"-   [help] to get this help\n");
                        fprintf(stdout,"\n typical commands:\n gain 0 10\n svel 0 10\n run\n set 0 x\n\n");
                        
                        reply.addVocab32("many");
                        reply.addVocab32("ack");
                        reply.addString("VelocityControl module, valid commands are:");
                        reply.addString("-   [susp]         suspend the controller (command zero velocity)");
                        reply.addString("-   [run]          start (and resume after being suspended) the controller");
                        reply.addString("-   [quit]         quit the module (exit)");
                        reply.addString("-   [set]  <j> <p> move joint j to p (degrees)");
                        reply.addString("-   [svel] <j> <v> set maximum speed for joint j to v (deg/sec)");
                        reply.addString("-   [gain] <j> <k> set P gain for joint j to k");
                        reply.addString("-   [help] to get this help");
                        reply.addString("\n typical commands:\n gain 0 10\n svel 0 10\n run\n set 0 x\n\n");
                    break;
                    default:
                        yError("Invalid command, ignoring\n");
                        reply.addVocab32("fail");
                        //cmdSize--;
                        //index++;
                        //return respond(command, reply); // call default
                    break;
                }
                return true;
            }
 
        return false;
    }
  
    virtual bool configure(yarp::os::ResourceFinder &rf)
    {
        Property options;
        options.fromString(rf.toString());
        char robotName[255];
        options.put("device", "remote_controlboard");
        if(options.check("robot"))
            strncpy(robotName, options.find("robot").asString().c_str(),sizeof(robotName));
        else
            strncpy(robotName, "icub", sizeof(robotName));
    
        if(options.check("part"))
            {
                char tmp[800];
                snprintf(tmp, sizeof(tmp), "/%s/vc/%s/client",
                        robotName,
                        options.find("part").asString().c_str());
                options.put("local",tmp);
        
                snprintf(tmp, sizeof(tmp),"/%s/%s",
                        robotName,
                        options.find("part").asString().c_str());
                options.put("remote", tmp);
        
                snprintf(tmp, sizeof(tmp), "/%s/vc/%s/input",
                        robotName,
                        options.find("part").asString().c_str());
                input_port.open(tmp);
            
                options.put("carrier", "mcast");
            
                attach(input_port);
            }
        else
            {
                yError("Please specify part (e.g. --part head)\n");
                return false;
            }
    
        if (!driver.open(options))
            {
                yError("Error opening device, check parameters\n");
                return false;
            }
 
        int period = CONTROL_RATE;
        if(options.check("period"))
            period = options.find("period").asInt32();
        
        yInfo("control rate is %d ms",period);
 
        if (!options.check("part"))
            return false;
        
        snprintf(partName, sizeof(partName), "%s", options.find("part").asString().c_str());
 
        vc=new velControlThread(period);
        vc->init(&driver, partName,
                 robotName);
 
        vc->start();
        return true;
    }
 
    virtual bool close()
    {
        yInfo("Closing module [%s]\n", partName);
        vc->stop();
        vc->halt();
        delete vc;
        yInfo("Thead [%s] stopped\n", partName);
 
        driver.close();
        input_port.close();
 
        yInfo("Module [%s] closed\n", partName);
        return true;
    }
 
    bool updateModule()
    {
        return true;
    }
 
};
 
int main(int argc, char *argv[])
{
    Network yarp;
    VelControlModule mod;
    ResourceFinder rf;
    
    rf.configure(argc, argv);
    if (mod.configure(rf)==false) return 1;
    mod.runModule();
    yInfo("Main returning\n");
    return 0;
}