BasePlatform.h

Go to the documentation of this file.

#ifndef   _GAMS_PLATFORM_BASE_H_
#define   _GAMS_PLATFORM_BASE_H_

#include <string>

#include "gams/variables/Self.h"
#include "gams/variables/Sensor.h"
#include "gams/variables/PlatformStatus.h"
#include "gams/utility/GPSPosition.h"
#include "gams/pose/ReferenceFrame.h"
#include "gams/pose/Position.h"
#include "gams/pose/Pose.h"
#include "madara/knowledge/KnowledgeBase.h"

namespace gams
{
  namespace controllers
  {
    class BaseController;
    class Multicontroller;
  }

  namespace platforms
  {
    enum PlatformAnalyzeStatus
    {
      UNKNOWN = 0,
      OK  = 1,
      WAITING = 2,
      DEADLOCKED = 4,
      FAILED = 8,
      MOVING = 16,
      REDUCED_SENSING_AVAILABLE = 128,
      REDUCED_MOVEMENT_AVAILABLE = 256,
      COMMUNICATION_AVAILABLE = 512,
      SENSORS_AVAILABLE = 1024,
      MOVEMENT_AVAILABLE = 2048
    };

    enum PlatformReturnValues
    {
      PLATFORM_ERROR = 0,
      PLATFORM_IN_PROGRESS = 1,
      PLATFORM_MOVING = 1,
      PLATFORM_OK = 1,
      PLATFORM_ARRIVED = 2
    };

    class GAMS_EXPORT PositionBounds {
    public:
      virtual bool check_position(
          const pose::Position &current,
          const pose::Position &target) const = 0;

      virtual ~PositionBounds() = default;
    };

    class GAMS_EXPORT OrientationBounds {
    public:
      virtual bool check_orientation(
          const pose::Orientation &current,
          const pose::Orientation &target) const = 0;

      virtual ~OrientationBounds() = default;
    };

    class GAMS_EXPORT PoseBounds :
      public PositionBounds, public OrientationBounds {};

    class GAMS_EXPORT Epsilon : public PoseBounds {
    private:
      double dist_ = 0.1, radians_ = M_PI/16;
    public:
      Epsilon() {}

      Epsilon(double dist)
        : dist_(dist) {}

      Epsilon(double dist, double radians)
        : dist_(dist), radians_(radians) {}

      //
      template<typename AngleUnits>
      Epsilon(double dist, double angle, AngleUnits u)
        : dist_(dist), radians_(u.to_radians(angle)) {}

      bool check_position(
          const pose::Position &current,
          const pose::Position &target) const override {
        return current.distance_to(target) <= dist_;
      }

      bool check_orientation(
          const pose::Orientation &current,
          const pose::Orientation &target) const override {
        return fabs(current.angle_to(target)) <= radians_;
      }
    };

    class GAMS_EXPORT BasePlatform
    {
    public:
      // allow Base controller to initialize our variables
      friend class controllers::BaseController;
      friend class controllers::Multicontroller;

      BasePlatform (
        madara::knowledge::KnowledgeBase * knowledge = 0,
        variables::Sensors * sensors = 0,
        variables::Self * self = 0);

      virtual ~BasePlatform ();

      void operator= (const BasePlatform & rhs);

      virtual int analyze (void) = 0;

      virtual double get_accuracy (void) const;

      utility::Position * get_position ();

      pose::Position get_location () const;

      pose::Orientation get_orientation () const;

      pose::Pose get_pose () const;

      virtual std::string get_name () const = 0;

      virtual std::string get_id () const = 0;

      virtual double get_min_sensor_range () const;

      virtual double get_move_speed () const;

      virtual const variables::Sensor & get_sensor (const std::string& name) const;

      virtual void get_sensor_names (variables::SensorNames & sensors) const;

      virtual int home (void);

      virtual int land (void);

      virtual int move (const pose::Position & target) {
        return move (target, Epsilon());
      }

      virtual int move (const pose::Position & target,
        const PositionBounds &bounds);

      int move (const pose::Position & target, double epsilon) {
        return move (target, Epsilon(epsilon));
      }

      virtual int orient (const pose::Orientation & target) {
        return orient (target, Epsilon());
      }

      virtual int orient (const pose::Orientation & target,
        const OrientationBounds &bounds);

      int orient (const pose::Orientation & target, double epsilon) {
        return orient(target, Epsilon (epsilon));
      }

      virtual int pose (const pose::Pose & target) {
        return pose (target, Epsilon());
      }

      virtual int pose (const pose::Pose & target, const PoseBounds &bounds);

      int pose (const pose::Pose & target,
        double loc_epsilon, double rot_epsilon = M_PI/16) {
        return pose(target, Epsilon(loc_epsilon, rot_epsilon));
      }

      virtual void pause_move (void);

      virtual int sense (void) = 0;

      void set_knowledge (madara::knowledge::KnowledgeBase * rhs);

      virtual void set_move_speed (const double& speed);

      virtual void set_sensors (variables::Sensors * sensors);

      virtual void stop_move (void);

      virtual void resume_move (void);

      virtual void resume_orientation (void);

      virtual void stop_orientation (void);

      virtual int takeoff (void);

      madara::knowledge::KnowledgeBase * get_knowledge_base (void) const;

      variables::Self * get_self (void) const;

      variables::Sensors * get_sensors (void) const;

      variables::PlatformStatus * get_platform_status (void);

      const variables::PlatformStatus * get_platform_status (void) const;

      virtual const pose::ReferenceFrame & get_frame (void) const;

    protected:
      double move_speed_;

      madara::knowledge::KnowledgeBase * knowledge_;

      variables::Self * self_;

      variables::Sensors * sensors_;

      variables::PlatformStatus status_;

      //static pose::GPSFrame frame_;
    };

    // deprecated typdef. Please use BasePlatform instead.
    typedef  BasePlatform    Base;
  }
}

#include "BasePlatform.inl"

#endif // _GAMS_PLATFORM_BASE_H_