faceTracker.h

Go to the documentation of this file.

// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-

/*
* Copyright (C) 2014 WYSIWYD Consortium, European Commission FP7 Project ICT-612139
* Authors: Hyung Jin Chang
* email:   hj.chang@imperial.ac.uk
* website: http://wysiwyd.upf.edu/
* 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
* icub-client/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
*/

#ifndef _FACETRACKER_MODULE_H_
#define _FACETRACKER_MODULE_H_

#include <cstdio>
#include <iostream>
#include <ctime>
#include <cmath>

#include <opencv2/opencv.hpp>

#include <yarp/os/all.h>
#include <yarp/sig/all.h>
#include <yarp/dev/all.h>

class faceTrackerModule : public yarp::os::RFModule {
    yarp::os::RpcServer handlerPort; 
    yarp::os::BufferedPort<yarp::sig::ImageOf<yarp::sig::PixelRgb> > imagePortLeft; 

    yarp::dev::IVelocityControl *vel;
    yarp::dev::IEncoders *enc;

    yarp::dev::PolyDriver *robotHead;

    yarp::sig::VectorOf<double> velocity_command;
    yarp::sig::VectorOf<double> cur_encoders;

    yarp::dev::IControlMode *ictrl;

    int getBiggestFaceIdx(const cv::Mat& cvMatImageLeft, const std::vector<cv::Rect> &faces_left);

protected:
    double x_buf;
    double y_buf;

    enum class Mode {GO_DEFAULT, FACE_SEARCHING, FACE_TRACKING, PAUSED}; 
    Mode mode;
    int counter_no_face_found; 
    int setpos_counter; 
    int panning_counter; 
    int stuck_counter;

    // random motion variables
    int tilt_target; 
    int pan_target; 
    int pan_max; 
    int tilt_max; 
    int nr_jnts; 

    cv::CascadeClassifier face_classifier_left;
    IplImage *cvIplImageLeft;

    void moveToDefaultPosition();

    void faceSearching(bool face_found);

    void faceTracking(const std::vector<cv::Rect> &faces_left, int biggest_face_left_idx);

public:

    bool configure(yarp::os::ResourceFinder &rf); 
    bool interruptModule();                       
    bool close();                                 
    bool respond(const yarp::os::Bottle& command, yarp::os::Bottle& reply);
    double getPeriod();
    bool updateModule(); 
};

#endif // __FACETRACKER_MODULE_H__