Pursuing.h

Go to the documentation of this file.

// ----------------------------------------------------------------------------
//
// $Id$
//
// Copyright 2008, 2009, 2010, 2011, 2012  Antonio Franchi and Paolo Stegagno    
//
// This file is part of MIP.
//
// MIP 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 3 of the License, or
// (at your option) any later version.
//
// MIP 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 MIP. If not, see <http://www.gnu.org/licenses/>.
//
// Contact info: antonio.franchi@tuebingen.mpg.de stegagno@diag.uniroma1.it
//
// ----------------------------------------------------------------------------





#ifndef __PURSUING_H_
#define __PURSUING_H_

#ifdef MIP_HOST_APPLE
#include <applePatch.h>
#endif

#include <fstream>

#include <R2.h>
#include <Grid.h>
#include <Scan.h>

#include <Task.h>
#include <Resource.h>
#include <MobileRob.h>
#include <SharedMem.h>
#include <Time.h>

#include <CommonOptions.h>

#include <ObsAvoiAlgoSND.h>

using namespace MipResources;

namespace MipTasks{
    
/* @{ */


class PursuingOptions : public Options {
    public:
        IntOption*  customDebugLevel;

        PursuingOptions(){
           customDebugLevel    = new IntOption("PursuingDebugLevel",   "Custom debug level of the Pursuing task.", 3);

            list.push_back(customDebugLevel);
        }
};

class PursuingSharedMem : public SharedMem{
    public:
        void initSharedMem();
        
        PoseFeatures getFormation();
        
        PoseFeatures getEstimates();
        
        bool doAutoNavigation();
        
        void setAvoidMutLocIds(vector<int> &newIds);
        
        vector<int> getAvoidMutLocIds();
        
        void clearAvoidMutLocIds();
};
    

enum PursuingState
{
    WAITING,
    INITIALIZE,
    RUN
};

class Pursuing : public Task, public MessageHandler{
    private:
        static const TaskPlate _plate = PUR_TASK;
        static const long int       _maxDurSec  = 0,                    _maxDurUsec = 30000;
        static const long int       _maxSamplPeriodSec  = 0,    _maxSamplPeriodUsec = 200000;
        static const long int       _minSamplPeriodSec  = 0,    _minSamplPeriodUsec = 100000;
        
        MobileRob* _mobileRob;
        ObsAvoiAlgoSND* _pObsAvoiAlgo;
        
        PursuingState _state;
        uint _robotID;
        PursuingSharedMem* _sharedMem;
        
        PoseFeatures _otherRobotPoses;        

        Pose _goal;
        pthread_mutex_t _goalMutex;
        bool _bSendScanFromGoal;
        string _centralIP;
        
        int _count;

        RangeSensPar* _sensorParams;
        Decimal _robotRadius;
        PursuingOptions _options;
        
        bool _getScan(Scan &scan, Time waitTime = Time(0,0));
        Pose _getPose();
        Pose _getGoal();
        void _setGoal( Pose newGoal );
        bool _atGoal();
        
        void _initialize();
        void _recordOtherRobotPoses();
        void _navigate();
        void _communicate();
        
        void _sendPoseAndScan(string ip);
        void _sendPose(string ip);
        void _sendParams(string ip);
        
        string getObjectName() const {
            stringstream ss;
            assert(getPlate()<TASK_NUM);
            
            ss << TaskNames[getPlate()] << ", Agent : "<< getID();
            return ss.str();
        }
    
    public:
        Pursuing(ResourcePointers resources,int argc, const char* argv[]);
        
        ~Pursuing();
        
        TaskPlate getPlate() const {
            return _plate;
        }
        Time getMaxDuration(){
            return Time(_maxDurSec,_maxDurUsec);
        }
        Time getMaxSamplPeriod(){
            return Time(_maxSamplPeriodSec,_maxSamplPeriodUsec);
        }
        Time getMinSamplPeriod(){
            return Time(_minSamplPeriodSec,_minSamplPeriodUsec);
        }
        TaskOutputs run();
        
        uint getID();
        
        Decimal getSensorLinRangeMax();
        Angle getSensorFieldOfView();
        int getSensorNumRays();
        Angle getSensorAngRes();
        Decimal getRobotRadius();
        
        void handleMessage(string inMessage,string ip);
};

/* @} */
    
};// end namespace MipTasks

#endif