iCubBreatherThread.h

/* 
 * Copyright (C) 2010 RobotCub Consortium, European Commission FP6 Project IST-004370
 * Author: Alessandro Roncone
 * email:  alessandro.roncone@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
 * This thread detects a touched taxel on the skin (through readings from the
 * skinContactList port), and it moves the "controlateral" limb toward
 * the affected taxel.
*/
 
#include <yarp/os/all.h>
 
#include <yarp/sig/Vector.h>
 
#include <yarp/dev/PolyDriver.h>
#include <yarp/dev/Drivers.h>
#include <yarp/dev/all.h>
 
#include <yarp/math/RandnScalar.h> 
 
#include <mutex>
#include <iostream>
#include <string>
#include <cstdarg>
#include <vector>
 
using namespace yarp::os;
using namespace yarp::sig;
using namespace yarp::dev;
 
using namespace std;
 
class iCubBreatherThread: public PeriodicThread
{
protected:
    /***************************************************************************/
    // EXTERNAL VARIABLES: change them from command line or through .ini file
    int verbosity;      // Flag that manages verbosity
    string name;        // Name of the module (to change port names accordingly)  
    string robot;       // Name of the robot (to address both icub and icubSim)
    string part;        // Arm to control (either "right_arm" or "left_arm")
    double noiseStd;
    double refSpeed;
    double rate;
 
    // Classical interfaces
    PolyDriver        dd;   // head device driver
    IPositionControl *ipos;
    IEncoders        *iencs;
    IControlMode     *imod;
    Vector           *encs;
    Vector            encs_0;
    int               jnts;
 
    bool isRunning;
    bool onStart;
 
    yarp::os::Bottle groupPartBottle;
    std::vector <double> noiseStDvtns;
    std::vector <double> refSpeeds;
 
    mutex mtx;
    yarp::math::RandnScalar randngen;
 
    bool setCtrlModes(const string &_s);
 
    bool areJointsHealthyAndSet(vector<int> &jointsToSet,const string &_s);
 
    bool goHome();
 
    bool goToTarget(const Vector &nT);
 
    Vector computeNewTarget();
 
    int printMessage(const int l, const char *f, ...) const;
 
public:
    // CONSTRUCTOR
    iCubBreatherThread(int _rate, string _name, string _robot,
                       string _part, bool _autoStart, double _noiseStd,
                       double _refSpeed, int _v, const ResourceFinder &_rf);
    // INIT
    virtual bool threadInit();
    // RUN
    virtual void run();
    // RELEASE
    virtual void threadRelease();
    // RESTART THE CYCLE
    bool startBreathing();
    bool stopBreathing();
};