VREPBase.h

Go to the documentation of this file.

 
#ifndef   _GAMS_PLATFORM_VREP_BASE_H_
#define   _GAMS_PLATFORM_VREP_BASE_H_
 
#include "gams/variables/Self.h"
#include "gams/variables/Sensor.h"
#include "gams/variables/PlatformStatus.h"
#include "gams/platforms/BasePlatform.h"
#include "madara/knowledge/KnowledgeBase.h"
#include "gams/pose/CartesianFrame.h"
#include "gams/pose/Position.h"
#include "gams/pose/Orientation.h"
#include "gams/pose/GPSFrame.h"
#include "madara/threads/Threader.h"
#include "madara/threads/BaseThread.h"
#include "madara/LockType.h"
#include "madara/knowledge/containers/Integer.h"
 
#include "gams/loggers/GlobalLogger.h"
 
#ifdef __GNUC__
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wpedantic"
#pragma GCC diagnostic ignored "-Wunused-but-set-variable"
#pragma GCC diagnostic ignored "-Wunused-parameter"
#endif
 
extern "C" {
#include "extApi.h"
}
 
#ifdef __GNUC__
#pragma GCC diagnostic pop
#endif
 
#ifdef _GAMS_VREP_
 
namespace gams
{
  namespace platforms
  {
    class GAMS_EXPORT VREPBase : public BasePlatform
    {
    public:
      VREPBase(
        std::string model_file,
        simxUChar is_client_side,
        madara::knowledge::KnowledgeBase * knowledge,
        variables::Sensors * sensors,
        variables::Self * self);
 
      virtual ~VREPBase();
 
      void operator=(const VREPBase & rhs);
 
      virtual int sense(void) override;
 
      virtual int analyze(void) override;
 
      virtual double get_accuracy() const override;
 
      virtual double get_move_speed() const override;
 
      virtual int land(void) override;
 
      int move(const pose::Position & location,
        const PositionBounds &bounds) override;
 
      using BasePlatform::move;
 
      int orient(const pose::Orientation & location,
        const OrientationBounds &bounds) override;
 
      using BasePlatform::orient;
 
      virtual void set_move_speed(const double& speed) override;
 
      virtual int takeoff(void) override;
 
      virtual const pose::ReferenceFrame & get_frame(void) const override;
 
      const pose::ReferenceFrame & get_vrep_frame(void) const;
 
    protected:
      virtual void add_model_to_environment(const std::string& file, 
        const simxUChar client_side) = 0;
 
      bool get_ready(void);
 
      virtual void get_target_handle() = 0;
 
      virtual void set_initial_position();
 
      virtual double get_initial_z() const;
 
      bool sim_is_running(void);
 
      bool vrep_is_ready(void);
 
      bool agent_is_ready(void);
 
      bool airborne_;
 
      simxInt client_id_;
 
      double move_speed_;
 
      simxInt node_id_;
 
      simxInt node_target_;
 
      pose::Pose sw_pose_;
 
      pose::ReferenceFrame vrep_frame_;
 
      madara::knowledge::containers::Double thread_rate_;
 
      madara::knowledge::containers::Double thread_move_speed_;
 
      madara::knowledge::containers::Double max_delta_;
 
      madara::knowledge::containers::Double max_orient_delta_;
 
      const static std::string MOVE_THREAD_NAME;
 
      class TargetMover : public madara::threads::BaseThread
      {
        public:
          TargetMover(VREPBase &base);
 
          void run();
 
          static const std::string NAME;
        private:
          VREPBase &base_;
      };
 
      friend class TargetMover;
 
      TargetMover *mover_;
 
      madara::threads::Threader threader_;
 
      mutable MADARA_LOCK_TYPE vrep_mutex_;
 
    private:
 
      void create_ready_conditions(void);
 
      bool agent_is_ready_;
 
      bool vrep_is_ready_;
 
      bool sim_is_running_;
 
      std::string model_file_;
 
      simxUChar is_client_side_;
 
      madara::knowledge::containers::Integer begin_sim_;
 
      madara::knowledge::containers::Integer vrep_ready_;
 
      madara::knowledge::containers::Integer agent_ready_;
 
      pose::Pose get_sw_pose(const pose::ReferenceFrame &frame);
 
      int do_move(const pose::Position & target,
                   const pose::Position & current, double max_delta);
 
      int do_orient(pose::Orientation target,
                     const pose::Orientation & current, double max_delta);
    }; // class VREPBase
  } // namespace platform
} // namespace gams
 
#include "VREPBase.inl"
 
#endif // _GAMS_VREP_
 
#endif // _GAMS_PLATFORM_VREP_BASE_H_