BeoPilot.H

#include "Component/ModelComponent.H"
#include "Component/ModelParam.H"
#include "Media/FrameSeries.H"
#include "Transport/FrameInfo.H"
#include "Raster/GenericFrame.H"
#include "Image/Image.H"
#include "Image/DrawOps.H"
#include "Image/Point3D.H"
#include "GUI/XWinManaged.H"
#include "GUI/ImageDisplayStream.H"
#include "Robots/RobotBrain/RobotBrainComponent.H"
#include "Util/Timer.H"
#include "GUI/SimpleMeter.H"

#include "Ice/RobotBrainObjects.ice.H"
#include "Ice/RobotSimEvents.ice.H"
#include <IceUtil/Thread.h>

#include <vector>
#include <deque>
#include <list>

#include "Devices/Serial.H"
#include "Controllers/PID.H"


typedef struct {
  double dist,angle;
} sonarData;

typedef struct{
                float time,transVel,rotVel,encoderX,encoderY,encoderOri,rcTransVel,rcRotVel;
                int rcMode;
} motorData;
typedef struct{
                float time;
                std::string imagePath ;
} imageData;
//typedef struct{
//                float x;
//                float y;
//                float z;
//
//}Point3DwithColor;
template<typename T>
class Point3DwithColor
{
public:
        //Point3DwithColor(const Point3DwithColor&);
        //Point3DwithColor& operator=(const Point3DwithColor&);
        T x,y,z;
        PixRGB<byte> color;
        Point3DwithColor(const T xx,const T yy, const T zz)
        {
                x = xx;y=yy;z=zz;
                color = PixRGB<byte>(0,0,0);
        }

        ~Point3DwithColor(){};
};
template<typename T>
class velocityQue{
        // Prevent copy and assignment
        velocityQue(const velocityQue&);
        velocityQue& operator=(const velocityQue&);
        //Max size of data
        int m_size;
        //Current size total
        int m_total;
        std::deque<T> m_data;

public:
        velocityQue():m_size(0)
        {
        }
        velocityQue(int n):m_size(0)
        {
                m_size = n;
        }
        void add(const T& data){
                // if the queue is full yet, we pop the oldest data out
                if(m_total >= m_size)
                {
                        m_data.pop_back();
                }else{
                        m_total++;
                }
                m_data.push_front(data);
        }
        std::vector<T> getVector()
        {
                return std::vector<T>(m_data.begin(),m_data.end());
        }
        ~velocityQue(){}
};

//! The BeoPilot module is responsible for actually driving the BeoBot2.  All motor
//! requests are serviced to hardware by this module through MotorRequest messages.
class BeoPilot : public RobotBrainComponent
{
public:

  BeoPilot(int id, OptionManager& mgr,
      const std::string& descrName = "BeoPilot",
      const std::string& tagName = "BeoPilot");

  ~BeoPilot();

        //! Evolve keeps the motor driver propeller up to date on the latest motor speed requests.
        //! If the evolve loop ever hangs for more than 1/4 second, then the propeller will shut down
        //! the motors until it recieves a new message.
  virtual void evolve();

  virtual void updateMessage(const RobotSimEvents::EventMessagePtr& eMsg,
      const Ice::Current&);

  virtual void registerTopics();

        void checkLogFile();
        //! Check the serial port to ensure that we are connected to a 'motorboard' propeller,
        //! and also check the sanity of our command line options

                                void loadFromLogFile();
        void start3();

        //Draw the updated local debugging GUI
        void updateGUI();

        //Send the motor speeds ([-1 .. 1]) to the motordriver/propeller
        void SetMotors(float motor1speed, float motor2speed);

        //Get the RC transmitter status
        unsigned char getRCStatus();

        //Get the status of an RC channel
        unsigned int getRCChannel(int channel);

        //Get the Enabled status from RC
        unsigned char getRCEnabled();

        //Get the Mix speed status from MotorBoard
        unsigned int getRCSpeed();

        //Get all of the RC status data in one frame
        void UpdateRCStatus();

        //Reset the encoder value
        void resetEncoder();

  //Update position after get odometry data
        void UpdatePosition(void);

        //Get Log Data from current time
        void getCurrentLogData(void);

        //Redraw map base on the current scale
        void reDrawMap(double scale);

        //Draw all point at map
        void drawAllMap(void);
        int file_exists(const char * fileName)
        {
                struct stat buf;
                int i = stat(fileName,&buf);
                if(i == 0)
                        return 1;
                return 0;


        }
private:
  nub::soft_ref<Serial> itsSerial;
        Timer itsSerialTimer;
        IceUtil::Mutex itsSerialMutex;

  int itsCurrMessageID;

        //////////////////////////////////////////
        //Log Data Structures
        //////////////////////////////////////////
        std::vector<motorData> itsLogData;
        std::vector<imageData> itsLogImageData;
        int itsCurrentTimeIndex;
        int itsCurrentTimeIndex2;
        Timer itsLogTimer;
        bool itsLoadFromLogFile;
        int itsLogRemoteMode;
        double itsLogLeftMotorVelocity;
        double itsLogRightMotorVelocity;
        //Point3D<double> itsLogPosition;
        Point3DwithColor<double> itsLogPosition;
        Point3DwithColor<double> itsLogDiffPosition;


        //////////////////////////////////////////
        //Motor Data Structures
        //////////////////////////////////////////
        IceUtil::Mutex itsSpeedMutex;
        double itsForwardSpeedReq;
        double itsRotationalSpeedReq;
        double itsForwardSpeed;
        double itsRotationalSpeed;

        double itsRcTransSpeed;
        double itsRcRotSpeed;
        double itsRcTransSpeedCapped;
        double itsRcRotSpeedCapped;

        double itsTransCap;
        double itsRotCap;

        std::vector<int> itsRCChannels;
        int itsEmergencyMode;
        int itsRemoteMode;
        float itsMotor1Speed;
        float itsMotor2Speed;
        int itsLeftEncoder;
        int itsRightEncoder;
        int itsLastLeftEncoder;
        int itsLastRightEncoder;
        Point3DwithColor<double> itsPosition;
        Point3DwithColor<double> itsDiffPosition;
        Image<PixRGB<byte> > itsMapImage;
        Timer itsEncoderTimer;
        double itsLeftMotorVelocity;
        double itsRightMotorVelocity;

        PID<float> itsPidLeft;
        PID<float> itsPidRight;

        //////////////////////////////////////////
        //Sonar Data Structures
        //////////////////////////////////////////
        IceUtil::Mutex itsSonarMutex;
        std::vector<double> itsSonarReadings;
        std::vector<double> itsSonarAngles;
        Timer itsSonarTimer;
        bool  itsSonarProxAlert;                  //  Are we in a sonar proximity alert state?
        float itsSonarTimeout;                    //  The number of seconds allowed to elapse between
                                                                                                                                                                                //sonar readings before we kill the motors

        //////////////////////////////////////////
        //Display Data Structures
        //////////////////////////////////////////
        Image<PixRGB<byte> > itsControlImage;
        Image<PixRGB<byte> > itsDispImage;
        Image<PixRGB<byte> > itsDispCameraImage;
        XWinManaged itsMeterXwin;
//        XWinManaged itsMapXwin;
        Timer itsDisplayTimer;
        float itsDisplayUpdateRate;
        float itsMapScale;
        std::vector<SimpleMeter> itsChannelMeters;
        velocityQue<float>        itsVelocityQue;
        velocityQue<float>        itsVelocityTargetQue;
        std::vector<Point3DwithColor<double> >itsLogPositionMap;
        std::vector<Point3DwithColor<double> >itsPositionMap;
        /////////////////////////////////////////
        //System Parameters
        /////////////////////////////////////////
        OModelParam<float>       itsMinimumSafeDistance;
        OModelParam<float>       itsMaxSpeed;
        OModelParam<bool>        itsIgnoreSonar;
        OModelParam<std::string> itsLogFile;
        OModelParam<int>       itsReplaySpeed;

};