FormationFlying.h

Go to the documentation of this file.

#ifndef   _GAMS_ALGORITHMS_FORMATION_FLYING_H_
#define   _GAMS_ALGORITHMS_FORMATION_FLYING_H_

#include "gams/variables/Sensor.h"
#include "gams/platforms/BasePlatform.h"
#include "gams/variables/AlgorithmStatus.h"
#include "gams/variables/Self.h"
#include "gams/algorithms/BaseAlgorithm.h"
#include "gams/pose/Position.h"
#include "gams/algorithms/AlgorithmFactory.h"
#include "gams/groups/GroupFactoryRepository.h"

namespace gams
{
  namespace algorithms
  {
    class GAMS_EXPORT FormationFlying : public BaseAlgorithm
    {
    public:
      FormationFlying (
        const std::string & head_id,
        const std::vector<double> & offset,
        const std::vector<double> & destination,
        const std::string & group_name,
        const std::string & modifier,
        madara::knowledge::KnowledgeBase * knowledge = 0,
        platforms::BasePlatform * platform = 0,
        variables::Sensors * sensors = 0,
        variables::Self * self = 0);

      ~FormationFlying ();

      void operator= (const FormationFlying & rhs);
      
      virtual int analyze (void);
      
      virtual int execute (void);

      virtual int plan (void);

      bool is_head () const;

      bool is_ready () const;
      
    protected:
      pose::Position get_destination() const;

      struct Compiled
      {
        madara::knowledge::CompiledExpression ref;
        std::string agent;
      };
      std::vector<Compiled> compiled_formation_;

      madara::knowledge::containers::Integer formation_ready_;

      bool head_;

      int head_id_;

      madara::knowledge::containers::NativeDoubleArray head_location_;

      madara::knowledge::containers::NativeDoubleArray head_destination_;

      pose::Position destination_;

      madara::knowledge::containers::Integer in_formation_;

      enum
      {
        NONE,
        ROTATE
      } modifier_;

      bool need_to_move_;

      pose::Position next_position_;

      unsigned int num_agents_;

      double phi_;

      double phi_dir_;

      double rho_;

      variables::SensorNames sensor_names_;

      double z_;
    };
    
    class GAMS_EXPORT FormationFlyingFactory : public AlgorithmFactory
    {
    public:

      virtual BaseAlgorithm * create (
        const madara::knowledge::KnowledgeMap & args,
        madara::knowledge::KnowledgeBase * knowledge,
        platforms::BasePlatform * platform,
        variables::Sensors * sensors,
        variables::Self * self,
        variables::Agents * agents);
    };
  }
}

#endif // _GAMS_ALGORITHMS_FORMATION_FLYING_H_