FormationControl.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 __FORMATION_CONTROL_H
#define __FORMATION_CONTROL_H

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

#include <map>

#include <baselib.h>

#include "FormControlMsgs.h"
#include "FormCtrlShape.h"

#include <socketUDP.h>

using namespace MipBaselib;

enum FormationControlTypes{
 FORM_CTRL_TYPE_SPRDAM,
 FORM_CTRL_TYPE_ARTPOT,
 FORM_CTRL_TYPE_BOTTUP,
 FORM_CTRL_TYPES_NUM
};

static const char* FormationControlTypesNames[FORM_CTRL_TYPES_NUM] = {
 "Spring-Damper Form. Control",
 "Artificial-Potential Form. Control"
 "Form Ctrl Bottom up"
};


class FormCtrlOptions : public Options {
 public:

  StringOption *algorithm;
  StringOption *baseIp;
  IntOption    *basePort;
  Position3DOption *centerPos;

  DecimalOption *apAttrMaxDist;
  DecimalOption *apAttrGain;
  DecimalOption *apRepulMaxDist;
  DecimalOption *apRepulMinDist;
  DecimalOption *apRepulGain;
  BoolOption   *apUseShape;
  StringOption *apShape;
  IntOption   *apShapeVertNum;
  
  DecimalOption *sdSelfDamp;
  DecimalOption *sdNeighDist;
  DecimalOption *sdNeighStiff;
  DecimalOption *sdNeighDamp;
  
  DecimalOption *sdMinGrad;
  DecimalOption *sdMaxGrad;
  DecimalOption *sdGradD1;
  DecimalOption *sdGradD2;
  DecimalOption *sdGradD3;
  DecimalOption *sdGradSlope;
  DecimalOption *sdObsGain;
  DecimalOption *sdObsMinDist;
  DecimalOption *sdObsNullDist;
  DecimalOption *minTankLev;
  DecimalOption *maxTankLev;
  DecimalOption *initTankLev;
  
  FormCtrlOptions();

  string getObjectName() const {
   return "FormCtrlOptions";
  }
};


class SpringDampFormCtrlIn {
 private:
  void _setAll(Time currTime_, Position3D &robPos_, Velocity3D &robVel_, vector<Position3D>& obsPos_, vector<int>& neighList_,bool useExtVel_, Velocity3D extVel_);

  
 public:
  
  ~SpringDampFormCtrlIn();
  
  SpringDampFormCtrlIn(Time currTime_, Position3D &robPos_, Velocity3D &robVel_, vector<Position3D>& obsPos_, vector<int>& neighList_, Velocity3D extVel_);
  
  SpringDampFormCtrlIn(Time currTime_, Position3D &robPos_, Velocity3D &robVel_, vector<Position3D>& obsPos_, vector<int>& neighList_);
  
  SpringDampFormCtrlIn(const SpringDampFormCtrlIn& p);
  
  SpringDampFormCtrlIn& operator=(const SpringDampFormCtrlIn& p);
    
  Time currTime;
  Position3D robPos;
  Velocity3D robVel;
  vector<Position3D> obsPos;
  vector<int> neighList;
  bool useExtVel;
  Velocity3D extVel;
};

class ArtPotentialFormCtrlIn {
 private:
  
  void _setAll(Time& cT, Position3D &rP, vector<Position3D>& oP,vector<int>& nL, Velocity3D& eV,bool uEV,Decimal sS,bool uSS);
 public:
  
  ~ArtPotentialFormCtrlIn();
  
  ArtPotentialFormCtrlIn(Time currTime, Position3D& robotPosi, vector<Position3D>& obsPos, vector<int>& robDetList);
  
  ArtPotentialFormCtrlIn(Time currTime, Position3D& robotPosi, vector<Position3D>& obsPos, vector<int>& robDetList, Velocity3D& externalVel);
  
  ArtPotentialFormCtrlIn(Time currTime, Position3D &robotPos, vector<Position3D>& obsPos,vector<int>& robDetList, Velocity3D& externalVel, Decimal shapeScale_);
  
  ArtPotentialFormCtrlIn(const ArtPotentialFormCtrlIn& p);
  
  ArtPotentialFormCtrlIn& operator=(const ArtPotentialFormCtrlIn& p);
    
  Time currentTime;
  Position3D robotPosition;
  vector<Position3D> obsPositions;
  vector<int> neighList;
  Velocity3D extVelocity;
  bool useExtVel;
  Decimal shapeScale;
  bool useShapeScale;
};

class FormationControlOut {
 private:
  
 public:
  
  FormationControlOut();
  
  bool valid;
  Position3D commandedPos;
  Velocity3D commandedVel;
  Acceleration3D commandedAcc;
  
  Position3D virtPoint;
  Velocity3D virtPointVel;
  Velocity3D obsGradient;
};

class SpringDampFormCtrlPar : public MIPObject {
 private:
  
  Decimal _selfDamping;
  
  Decimal _neighNeutrDist;
  
  Decimal _neighStiff;
  Decimal _neighDamp;
  
  Time _neighDataLifeTime;
  
  stringstream _ssfatal;
  
 public:
  SpringDampFormCtrlPar(Decimal selfDamp=10.0,Decimal neighDist = 1.5,Decimal neighStiff = 5.0,Decimal neighDamping=5.0,Time neighDataLifeTime=Time(0.1));
  
  SpringDampFormCtrlPar(const SpringDampFormCtrlPar& p);
  
  SpringDampFormCtrlPar& operator=(const SpringDampFormCtrlPar& rhs);
  
  Decimal selfDamping();
  
  Decimal neighborNeutrDist();
  
  Decimal neighborStiff();
  
  Decimal neighborDamping();
  
  Time neighDataLifeTime();
  
  string getObjectName() const {
   return "SpringDampFormCtrlPar";
  }
};

class ArtPotentialFormCtrlPar : public MIPObject {
 private:
  
  Decimal _attractMaxDist;
  Decimal _attractGain;
  Decimal _repulMaxDist;
  Decimal _repulsMinDist;
  Decimal _repulsGain;
  
  Time _virtPointLifeTime;
  
  stringstream _ssfatal;
  
 public:
  ArtPotentialFormCtrlPar(Decimal attrMaxDist=3.0,Decimal attractGain = 1.0,Decimal repulMaxDist = 2.0,Decimal repulMinDist = 0.3,Decimal repulGain=1.0,
              Time virtPointLifeTime=Time(0.1));
  
  ArtPotentialFormCtrlPar(const ArtPotentialFormCtrlPar& p);
  
  ArtPotentialFormCtrlPar& operator=(const ArtPotentialFormCtrlPar& rhs);
  
  Decimal attrMaxDist();
  
  Decimal attrGain();
  
  Decimal repulMinDist();
  
  Decimal repulMaxDist();
  
  Decimal repulGain();
  
  Time virtPointLifeTime();
  
  string getObjectName() const {
   return "ArtPotentialFormCtrlPar";
  }
};

class SpringDampFormCtrlVar {
 private:
  
 public:
  
  Position3D virtPointPos;
  Velocity3D virtPointVel;
  Acceleration3D virtPointAcc;
  Time virtPointTime;
  
  Decimal tankLevel;
  
  vector<int> lastNeighList;
  
  map<int,Position3D> rcvdVirtPoints; 
  map<int,Time>    rcvdTimeStamps; 
  
  bool initialized;
  
  SpringDampFormCtrlVar(Decimal initTankLev);
  
  SpringDampFormCtrlVar(const SpringDampFormCtrlVar& p);

  SpringDampFormCtrlVar& operator=(const SpringDampFormCtrlVar& p);
};


enum ApFcState{
 APFC_STATE_TK_OFF,
 APFC_STATE_INIT,
 APFC_STATE_RUN,
 APFC_STATE_LAND,
 APFC_STATE_NUM
};

static const char* ApFcStateNames[APFC_STATE_NUM] = {
 "take Off",
 "Init",
 "run",
 "land",
};

enum ApFcTOffState{
 APFC_TOFF_STATE_CHECK_INIT_DATA,
 APFC_TOFF_STATE_SEND_CURR_POS,
 APFC_TOFF_STATE_FIND_ORDER,
 APFC_TOFF_STATE_ARRIVED,
 APFC_TOFF_STATE_NUM
};

static const char* ApFcTOffStateNames[APFC_TOFF_STATE_NUM] = {
 "check init data",
 "send curr pos",
 "find order",
 "arrived",
};


enum ApFcLandState{
 APFC_LAND_TODO,
 APFC_LAND_STATE_NUM
};

static const char* ApFcLandStateNames[APFC_LAND_STATE_NUM] = {
  "APFC_LAND_TODO",
};


class ArtPotentialFormCtrlVar {
 private:
  
 public:
  
  bool initialized;
  
  ApFcState state;
  ApFcTOffState tOffState;
  ApFcLandState landState;
  
  Position3D virtPoint;
  Velocity3D virtPointVel;
  Time virtPointTime;
  
  bool useShape;
  FormCtrlShape* shape;
  
  Decimal otherVPPotential;
  Decimal obsPotential;
  
  map<int,Position3D>       rcvdVirtPoints;  
  map<int,Position3D>::iterator itVP;       
  map<int,Time>          rcvdTimeStamps;   
  map<int,Time>::iterator     itT;       
  
  Decimal rcvdShapeScale;
  
  ArtPotentialFormCtrlVar();
};

class FormationControl : public MIPObject{
 private:
  FormationControlTypes _ctrlType;
  
  SpringDampFormCtrlVar *_sdVar;
  SpringDampFormCtrlPar *_sdPar;
  
  ArtPotentialFormCtrlVar *_apVar; 
  ArtPotentialFormCtrlPar *_apPar;
  
  FormCtrlOptions _options;
  
  ListenerUDP* _listener;
  pthread_mutex_t _listMtx;
  
  stringstream _ssfatal,_ssdebug;
  FILE* _logFile;
  bool _extLogFile;

  Decimal _sdGetGradient(Decimal distance);
  void           _sdGetNeighGradAndPot(Position3D posDiff, Acceleration3D& gradient, Decimal& potential);
  Acceleration3D _sdGetObsGrad(SpringDampFormCtrlIn* in);
  
  void _discardOldData(map<int,Position3D>& data , map<int,Time>& timeStamps, Time lifeTime);
  void _discardOutOfListData(map<int,Position3D>& data, map<int,Time>& timeStamps, vector<int>& toBeKept);
  
  void       _apSetShapeValues();
  Decimal    _apGetAttrMaxDist(int otherId);
  Velocity3D _apGetOtherVirtPointVel();
  Velocity3D _apGetObstacleVel(ArtPotentialFormCtrlIn *input);
  
  void _apTakeOff(ArtPotentialFormCtrlIn *input,FormationControlOut *output);
  void _apInit(ArtPotentialFormCtrlIn *input,FormationControlOut *output);
  void _apRun(ArtPotentialFormCtrlIn *input,FormationControlOut *output);
  void _apLand(ArtPotentialFormCtrlIn *input,FormationControlOut *output);
  
  void _pruneVirtPoints();
  void _sendCommPackets(vector<int>& recipients);
  
 public:
  
  FormationControl(SpringDampFormCtrlPar &par);
  
  FormationControl(ArtPotentialFormCtrlPar &par);
  
  FormationControl(int argc, const char** argv);
  
  ~FormationControl();
  
  void run(SpringDampFormCtrlIn *input,FormationControlOut *output);
  
  void run(ArtPotentialFormCtrlIn *input,FormationControlOut *output);
  
  void reset();
  
  Position3D getInitPosition();
  
  void setLogFile(FILE* logFile);
  
  FormationControlTypes getType(){
   return _ctrlType;
  }
  
  string getObjectName() const {
   return "FormationControl";
  }
};


#endif


/* @} */