BeoPilot.C

#include "Raster/Raster.H"
#include "Robots/Beobot2/Hardware/BeoPilot.H"
#include "Ice/BeobotEvents.ice.H"
#include "Component/ModelParam.H"
#include "Component/ModelOptionDef.H"
#include "Util/sformat.H"
#include "Util/MathFunctions.H"
#include "Image/ShapeOps.H" // for rescale()
#include <iostream>
#include <stdio.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <dirent.h>
#include <unistd.h>
#define LOG_FOLDER "../data/logs/IROS10_Equad"
const ModelOptionCateg MOC_BeoPilot = {
        MOC_SORTPRI_3, "Beobot Pilot Related Options" };

const ModelOptionDef OPT_MinimumSafeDistance =
{ MODOPT_ARG(float), "MinimumSafeDistance", &MOC_BeoPilot, OPTEXP_CORE,
        "The minimum distance (in meters) to objects we can allow before cutting off the motors.",
        "min-distance", '\0', "float", ".5"};

const ModelOptionDef OPT_MaxSpeed =
{ MODOPT_ARG(float), "MaxSpeed", &MOC_BeoPilot, OPTEXP_CORE,
        "The maximum speed of the robot (from [0 .. 1]).",
        "max-speed", '\0', "float", "1.0"};

const ModelOptionDef OPT_IgnoreSonar =
{ MODOPT_ARG(bool), "IgnoreSonar", &MOC_BeoPilot, OPTEXP_CORE,
        "Do we want to ignore the sonar for proximity warnings. Use this with caution, as someone could get hurt!?",
        "ignore-sonar", '\0', "true | false", "true"};

const ModelOptionDef OPT_LogFile =
{ MODOPT_ARG(std::string), "LogFile", &MOC_BeoPilot, OPTEXP_CORE,
        "There are 3 mode for input log file name \n"
  "you can enter\n"
  "1)  ./bin/app-BeoPilot --log-file=<Full Log File Path>\n"
  "    it will open the file directly\n"
  "2)  ./bin/app-BeoPilot --log-file=<date>\n"
  "    it will look for ../data/logs/<date>/Log_<date>.txt\n"
  "    The date formate can be flexible.\n"
  "    You can enter full date such as 2009_11_03__14_03_49\n"
  "    or 20091103 or 091103 or 1403 or 140349. \n"
  "    If there are more than one match, \n"
  "    it will list all options for choosen.\n"
  "3)  ./bin/app-BeoPilot --log-file=search\n"
  "    it will look for all directory under ../data/logs/\n"
  "    and list all files for choosen\n",
        "log-file", '\0', "<string>", "NULL"};


const ModelOptionDef OPT_ReplaySpeed=
{ MODOPT_ARG(int), "ReplaySpeed", &MOC_BeoPilot, OPTEXP_CORE,
        "The replay speed from the Log file,default is 1x. To speed up the speed, using --speed=2 or faster",
        "speed", '\0', "int", "1"};
// ######################################################################
BeoPilot::BeoPilot(int id, OptionManager& mgr,
                   const std::string& descrName, const std::string& tagName) :
  RobotBrainComponent(mgr, descrName, tagName),
  itsSerial(new Serial(mgr)),
        itsSerialTimer(1000000),
  itsCurrMessageID(0),
        itsCurrentTimeIndex(0),
        itsLogTimer(1000000),
        itsLoadFromLogFile(false),
        itsLogLeftMotorVelocity(0.0),
        itsLogRightMotorVelocity(0.0),
        itsLogPosition(0.0,0.0,0.0),
        itsLogDiffPosition(0.0,0.0,0.0),
        itsRCChannels(7, -1),
        itsEmergencyMode(-1),
        itsPosition(0.0,0.0,0.0),
        itsDiffPosition(0.0,0.0,0.0),
        itsMapImage(800-320,344,ZEROS),
        itsEncoderTimer(1000000),
        itsLeftMotorVelocity(0.0),
        itsRightMotorVelocity(0.0),
        itsPidLeft(0.3,0.000,0.00,0.0,0.0,0,0,0,1.0,-1.0),
        itsPidRight(0.0,0.000,0.00,0.0,0.0,0,0,0,1.0,-1.0),
        itsSonarTimeout(.5),
        itsControlImage(512,256,ZEROS),
        itsDispImage(800,600,ZEROS),
        itsDispCameraImage(320,240,ZEROS),
        itsMeterXwin(itsDispImage,"Pilot Control"),
        //itsMapXwin(itsMapImage,"Map Control"),
        itsDisplayUpdateRate(.05),
        itsMapScale(100.0),
        itsChannelMeters(7, SimpleMeter(20, 60, 1040,1921)),
        itsVelocityQue(50),
        itsVelocityTargetQue(50),
        itsMinimumSafeDistance(&OPT_MinimumSafeDistance, this, 0),
        itsMaxSpeed(&OPT_MaxSpeed, this, 0),
        itsIgnoreSonar(&OPT_IgnoreSonar, this, 0),
        itsLogFile(&OPT_LogFile, this, 0),
        itsReplaySpeed(&OPT_ReplaySpeed, this, 0)
{
        addSubComponent(itsSerial);
        //Connect to the serial motor board
        itsSerial->configureSearch ("motorboard", 115200);
}

void BeoPilot::checkLogFile()
{
        if(itsLogFile.getVal() =="NULL"){
                LINFO("Start BeoPilot with real device");
        }else if(itsLogFile.getVal() =="search"){
                LINFO("Search for all log file");
                DIR *dir_p;
                struct dirent *entry_p;
                dir_p = ::opendir(LOG_FOLDER);
                if(dir_p == NULL)
                        LFATAL("Can Not Open ../data/logs directory");
                //uint i = 1;

                std::vector<std::string> subdir;
                while((entry_p = ::readdir(dir_p)))
                {
                        std::string subdirname(entry_p->d_name);
                        if(subdirname != "." && subdirname !="..")
                        {
                                //LINFO("File[%d]:[%s]",i++,subdirname.c_str());
                                subdir.push_back(subdirname);
                        }
                }
                (void)::closedir(dir_p);

                //sort filename
                std::sort(subdir.begin(),subdir.end());
                for(int i = 0;i < (int)subdir.size();i++)
                {
                                LINFO("File[%d]:[%s]",i+1,subdir[i].c_str());
                }

                int option;
                do{
                        LINFO("Please Choose a File Number:");
                        scanf("%d",&option);//FIXX Any better idea?
                        if(option <1 ||option >int(subdir.size()))
                                LINFO("Option Invalid, please try again");
                }while(option < 1 || option > int(subdir.size()));
                        LINFO("Your Choose is [%d] filename[%s] ",option,subdir[option-1].c_str());
//                int run,segt;
//                char buf[255];
                //sscanf(subdir[option-1].c_str(),"S%d_R%d_%s",&run,&segt,buf);
                std::string logFileName(
                                sformat("%s/%s/%s.txt",
                                                LOG_FOLDER,
                                                subdir[option-1].c_str(),
                                                subdir[option-1].c_str()
                                                //buf
                                                ));
                                                                itsLogFile.setVal(logFileName);
                                                                loadFromLogFile();
        }else{
                LINFO("LogFileName:[%s]",itsLogFile.getVal().c_str());
                                                                loadFromLogFile();
        }


}

void BeoPilot::loadFromLogFile()
{


                FILE *logFile = fopen(itsLogFile.getVal().c_str(),"r");
                if(logFile == NULL)
                {
                                LFATAL("can't not open file: %s",itsLogFile.getVal().c_str());
                }else
                {
                        char line[512];
                        while(fgets(line,sizeof line,logFile)!= NULL)
                        {
                                float time,transVel,rotVel,encoderX,encoderY,encoderOri,rcTransVel,rcRotVel;
                                int rcMode;
                                int ret = sscanf (line,"[%f] MTR rcMode: %d, transVel: %f  rotVel: %f encoderX: %f encoderY: %f encoderOri: %f rcTransVel: %f rcRotVel: %f",
              &time, &rcMode, &transVel, &rotVel, &encoderX,  &encoderY, &encoderOri, &rcTransVel, &rcRotVel);
                                if(ret == 9){
                                        motorData tmp;
                                        tmp.time = time;
                                        tmp.rcMode = rcMode;
                                        tmp.transVel = transVel;
                                        tmp.rotVel = rotVel;
                                        tmp.encoderX = encoderX;
                                        tmp.encoderY = encoderY;
                                        tmp.encoderOri = encoderOri;
                                        tmp.rcTransVel = rcTransVel;
                                        tmp.rcRotVel = rcRotVel;
                                        itsLogData.push_back(tmp);
                                        LINFO("Got Motor Data %f %d",time,rcMode);

                                }

                                std::string imagePath;
                                char imgPath[256];
                                ret = sscanf(line,"[%f] CAM filename: %s", &time, imgPath);
                                if(ret == 2)
                                {
                                        imageData imgData;
                                        imgData.time = time;
                                        imgData.imagePath = imgPath;
                                        itsLogImageData.push_back(imgData);
                                        LINFO("Got Image at %f path=%s",time,imgPath);
                                }

                        }
                        LINFO("Load Log file with %d motor command lines",(int)itsLogData.size());
                        LINFO("Load Log file with %d Images",(int)itsLogImageData.size());
                        itsLoadFromLogFile = true;
                        itsLogTimer.reset();
                        //drawAllMap();
                }



}


void BeoPilot::start3()
{
        checkLogFile();
        //while(!itsSerial->isSerialOk()){
        //        LINFO("Can't not open serial port,keep try");
        //        itsSerial->enablePort(itsSerial->getDevName().c_str());///dev/ttyUSB0
        //}
        if(itsSerial->isSerialOk())
                LINFO("%s found on %s", itsSerial->getDeviceDescriptor().c_str(), itsSerial->getDevName().c_str());
        resetEncoder();
        //Check our command line options to ensure that they are sane
        if(itsMinimumSafeDistance.getVal() < 0)
                LFATAL("Invalid Minimum Safe Distance (%f). Must be > 0!", itsMinimumSafeDistance.getVal());
        if(itsMaxSpeed.getVal() > 1.0 || itsMaxSpeed.getVal() < 0)
                LFATAL("Invalid Maximum Speed Distance (%f). Must be > 0 && < 1.0!", itsMaxSpeed.getVal());
}

// ######################################################################
BeoPilot::~BeoPilot()
{
        //Kill the motors on exit
        SetMotors(0,0);
}


void BeoPilot::registerTopics()
{
        registerSubscription("MotorRequestTopic");
        registerSubscription("SonarMessageTopic");
        registerPublisher("MotorMessageTopic");
}


void BeoPilot::evolve()
{

                //Recieve telemetry from the propeller
                UpdateRCStatus();
                itsVelocityTargetQue.add(itsRcTransSpeedCapped);
                //itsVelocityQue.add(randomDouble()*2 -1.0);
                //itsVelocityTargetQue.add(randomDouble());

        //Check if it is in log file replay mode and also make sure emergency mode is off
        if(itsLoadFromLogFile){
                //No RC(Emergency = -1): getCurrentLogData and timer resume but no set Motor
                //With RC:
                // Emergency is off (Emergency = 0)
                //        1) Mode 1,2 :timer pause
                //        2) Mode 3 timer resume and run log file,set motor
                // Emergency is on (Emergency = 255)
                //         1) Timer pause
                if(itsEmergencyMode == -1||itsEmergencyMode == 255)
                {
                        itsLogTimer.resume();
                        getCurrentLogData();
                }
                else if(itsEmergencyMode == 0 && itsRemoteMode == 3)
                {
                        itsLogTimer.resume();
                        getCurrentLogData();
                        SetMotors(itsRotationalSpeed,itsForwardSpeed);
                }
                else{
                        itsLogTimer.pause();
                        LINFO("Wait for RC mode 3");
                }
                uint64 time = itsLogTimer.get()/1000.0;
                LDEBUG("Current Log Timer is %f",(double)time);
        }
        else{
                //Local copies of all shared data that we deal with so that we can
                //hold onto our locks for as short a time as possible
                float loc_rotVel;
                float loc_transVel;
                bool  loc_sonarProxAlert;

                //Check to make sure that we have heard from our sonar recently, and that it has
                //reported all sonars to be reading past the minimum safe distance. If we are in
                //a dangerous situation, then set the itsSonarProxAlert
                itsSonarMutex.lock();
                {
                        if(itsSonarReadings.size() > 0)
                        {
                                float minSonarReading = *(std::min_element(itsSonarReadings.begin(), itsSonarReadings.end()));
                                if(itsSonarTimer.getSecs() > itsSonarTimeout || minSonarReading < itsMinimumSafeDistance.getVal())
                                        itsSonarProxAlert = true;
                                else
                                        itsSonarProxAlert = false;
                        }
                        else
                                itsSonarProxAlert = true;

                        loc_sonarProxAlert = itsSonarProxAlert;
                }
                itsSonarMutex.unlock();

                //Set the current speed of the robot by either clamping it to the requested speed, or killing it
                //if we are in a dangerous situation
                itsSpeedMutex.lock();
                {
                        itsRotationalSpeed = itsRotationalSpeedReq;
                        itsForwardSpeed = itsForwardSpeedReq;
                        //If we haven't heard from the sonar or things are getting too close,
                        //then let's kill the motors
                        if((itsIgnoreSonar.getVal() == false) && loc_sonarProxAlert)
                        {
                                LINFO("SONAR ERROR!");
                                itsRotationalSpeed = 0;
                                itsForwardSpeed    = 0;
                        }

                        //Clamp the latest movement requests to the maximum speed as
                        //specified by the command line
                        if(itsRotationalSpeed > itsMaxSpeed.getVal())
                                itsRotationalSpeed = itsMaxSpeed.getVal();
                        else if(itsRotationalSpeed < -itsMaxSpeed.getVal())
                                itsRotationalSpeed = -itsMaxSpeed.getVal();

                        if(itsForwardSpeed > itsMaxSpeed.getVal())
                                itsForwardSpeed = itsMaxSpeed.getVal();
                        else if(itsForwardSpeed < -itsMaxSpeed.getVal())
                                itsForwardSpeed = -itsMaxSpeed.getVal();

                        //Set our local copies to the newly clamped requested speeds
                        loc_rotVel   = itsRotationalSpeed;
                        loc_transVel = itsForwardSpeed;
                        //LINFO("Set Local Rot/TranVel (%f, %f)", loc_rotVel, loc_transVel);
                }
                itsSpeedMutex.unlock();
                //Move motor in manual mode
                if(itsRemoteMode == 1 ||itsRemoteMode == 3)
                {
                        //Send the actual motor commands to the motor driver/propeller
                        SetMotors(loc_rotVel, loc_transVel);
                }else if(itsRemoteMode == 2)
                {
                double m1velReq = (itsRcTransSpeedCapped + itsRcRotSpeedCapped)*3.0;
                double m2velReq = (itsRcTransSpeedCapped - itsRcRotSpeedCapped)*3.0;

                float leftPWM =         itsPidLeft.update(m1velReq,itsLeftMotorVelocity);
                float rightPWM =         itsPidRight.update(itsRightMotorVelocity,m2velReq);

                double transPWM = leftPWM;// (leftPWM+rightPWM)/2.0;
                double rotPWM = 0.0;//(leftPWM-rightPWM)/2.0;

                LINFO("LEFT PWM %f, RIGT PWM %f",leftPWM,rightPWM);
                LINFO("LEFT Request %f, LEFT Cur %f",m1velReq,itsLeftMotorVelocity);
                LINFO("RIGT Request %f, RIGT Cur %f",m2velReq,itsRightMotorVelocity);
                LINFO("Rc Tran %f Rc Rot %f",itsRcTransSpeedCapped,itsRcRotSpeedCapped);
                LINFO("Pwm Tran %f Pwm Rot %f",transPWM,rotPWM);
                LINFO("=====================================================");
                        SetMotors(rotPWM,transPWM) ;
                }

                //Report the current motor speeds to IceStorm
                BeobotEvents::MotorMessagePtr msg = new BeobotEvents::MotorMessage;

                msg->RequestID = itsCurrMessageID;

                msg->transVel = loc_transVel;
                msg->rotVel   = loc_rotVel;
                msg->motor1   = Ice::Int(itsMotor1Speed);
                msg->motor2   = Ice::Int(itsMotor2Speed);

                msg->encoderX   = itsDiffPosition.x;
                msg->encoderY   = itsDiffPosition.y;
                msg->encoderOri = itsDiffPosition.z;

                msg->rcTransVel = itsRcTransSpeed;
                msg->rcRotVel   = itsRcRotSpeed;
                msg->rcMode     = itsRemoteMode;

                this->publish("MotorMessageTopic", msg);
                //FIXX        LINFO("MotorMessageTopic: %d", itsCurrMessageID);


                itsCurrMessageID++;
                usleep(10000);
        }
                //Draw the GUI to the screen
                updateGUI();


                double locSerialTime = (double) itsSerialTimer.get()/1000000.0;//sec
                int code = itsSerial->getSerialErrno();
                if(code==0||code==15){//FIXXX 15 is serialErrReadTimedOut
                        itsSerialTimer.reset();
                }else if(locSerialTime > 3.0){
                        LINFO("Try reconnect Timer:%f",locSerialTime);
                        itsSerialTimer.reset();
                        itsSerial->enablePort(itsSerial->getDevName().c_str());///dev/ttyUSB0
                        LINFO("Tried Reconnect");
                        usleep(100000);
                }else{
                        LINFO("Serial Timer:%f",locSerialTime);
                        LINFO("Serial Error Number:[%d]",itsSerial->getSerialErrno());
                }
}

void BeoPilot::UpdateRCStatus()
{
        unsigned char cmd = 107;
        itsSerial->write(cmd);
        std::vector<unsigned char> frame = itsSerial->readFrame(cmd, 255, -1, .2);//FIXXX Serial Crash Here


        if(frame.size() == 26)
        {
                itsRCChannels[0] = ((0x0FF & frame[ 0]) << 8) |
                                                           ((0x0FF & frame[ 1]) << 0);

                itsRCChannels[1] = ((0x0FF & frame[ 2]) << 8) |
                                                                        ((0x0FF & frame[ 3]) << 0);

                itsRCChannels[2] = ((0x0FF & frame[ 4]) << 8) |
                                                                        ((0x0FF & frame[ 5]) << 0);

                itsRCChannels[3] = ((0x0FF & frame[ 6]) << 8) |
                                                           ((0x0FF & frame[ 7]) << 0);

                itsRCChannels[4] = ((0x0FF & frame[ 8]) << 8) |
                                                                       ((0x0FF & frame[ 9]) << 0);

                itsRCChannels[5] = ((0x0FF & frame[10]) << 8) |
                                                                 ((0x0FF & frame[11]) << 0);

                itsRCChannels[6] = ((0x0FF & frame[12]) << 8) |
                                                           ((0x0FF & frame[13]) << 0);

                itsEmergencyMode = frame[14];
                if(itsRCChannels[2] > 1800)
                        itsEmergencyMode = 255;
                itsRemoteMode    = frame[15];
                itsMotor1Speed   = (frame[16] - 64.0) / 64.0;
                itsMotor2Speed   = (frame[17] - 64.0) / 64.0;


                itsLastLeftEncoder  = itsLeftEncoder;
                itsLeftEncoder   = ((0x0FF & frame[18]) << 24) |
                                                           ((0x0FF & frame[19]) << 16) |
                                                           ((0x0FF & frame[20]) <<  8) |
                                                           ((0x0FF & frame[21]) <<  0);

                itsLastRightEncoder = itsRightEncoder;
                itsRightEncoder  =
                                                                                          ((0x0FF & frame[22]) << 24) |
                                                           ((0x0FF & frame[23]) << 16) |
                                                           ((0x0FF & frame[24]) <<  8) |
                                                           ((0x0FF & frame[25]) <<  0);
                if(itsRCChannels[2] < 1800){
                        itsRcTransSpeed = (200 - ((itsRCChannels[1] - 1041) * 200)/(1874 - 1041)) -100;
                        //if it's mode 1
                        if( itsRCChannels[6] > 1800)
                                itsRcRotSpeed =         (200 - ((itsRCChannels[0] - 963) * 200)/(1775 -963)) -100;
                        else
                                itsRcRotSpeed =         (200 - ((itsRCChannels[0] - 1105) * 200)/(1928 -1105)) -100;
                        double speed_cap = 100 - ((itsRCChannels[2] - 1041) * 100)/(1750 -1041);
                        double rot_vel_cap = ((itsRCChannels[5] -900)* 100)/(2073 -900);
                        itsTransCap = speed_cap;
                        itsRotCap = rot_vel_cap;
                        if(speed_cap >  100)speed_cap = 100;
                        if(speed_cap <    0)speed_cap = 0;
                        if(rot_vel_cap   >  100)rot_vel_cap= 100;
                        if(rot_vel_cap   <    0)rot_vel_cap= 0;

                        if(itsRcTransSpeed >  100)itsRcTransSpeed = 100;
                        if(itsRcTransSpeed < -100)itsRcTransSpeed = -100;
                        if(itsRcRotSpeed   >  100)itsRcRotSpeed= 100;
                        if(itsRcRotSpeed   < -100)itsRcRotSpeed= -100;

                        itsRcTransSpeedCapped =itsRcTransSpeed* speed_cap/100;
                        itsRcRotSpeedCapped = itsRcRotSpeed * rot_vel_cap/-100;
                        itsRcTransSpeed /=100.0;
                        itsRcRotSpeed /= 100.0;
                        itsRcTransSpeedCapped /= 100.0;
                        itsRcRotSpeedCapped /= 100.0;
                        LDEBUG("RC cap %f Rc rot cap %f", speed_cap,rot_vel_cap);
                        LDEBUG("RC trans%f Rc rot  %f",  itsRcTransSpeed,itsRcRotSpeed);
                }else{
                        itsRcTransSpeed = 0;
                        itsRcRotSpeed = 0;
                        itsRcTransSpeedCapped = 0;
                        itsRcRotSpeedCapped = 0;
                }

                //After Get RC update,we update encoder value
                UpdatePosition();


        }
        else
        {
                //LERROR("BAD FRAME RECIEVED!");
        }

}

// We have 10 inch diameter wheel
// Motor Shaft will turn 21 times for one wheel spin
// The encoder have 4000 tick per revolution
// When the wheel have one revolution, it travels 10 * pi inch
//
// 10* pi inch  = 0.797964534 meters
// Which have 1000 * 21 ticks
//  (10 * pi)/84,000 * 0.0254 =  9.49957779 * 10^(-6) = 0.000009.49957779 meter
// Which means for every tick,
// the wheel will move  meter
//
// Width between two wheel W = 510 mm = 0.51m
// To convert odometry data to position, we need do some math again...
// Ot+1 = Ot + (Dr - Dl) / W
// Dt,t+1 = (Dr + Dl) / 2
// Xt+1 = Xt + Dt,t+1 * cos(Ot+1)
// Yt+1 = Yt + Dt,t+1 * sin(Ot+1)
void BeoPilot::UpdatePosition(void)
{
        if(itsLastRightEncoder == 0.0 && itsLastLeftEncoder == 0.0)
                itsEncoderTimer.reset();

        double time = (double) itsEncoderTimer.getReset()/1000000.0;//sec

//        double meterPerTick = 0.00000949957779 ;
        double ticksPerMeter = 105267.836;
        double dr = (itsRightEncoder - itsLastRightEncoder) / ticksPerMeter;
        double dl = (itsLeftEncoder - itsLastLeftEncoder)  / ticksPerMeter;
        double w = 0.51;
        double ot = itsPosition.z + ((dr - dl) / w );
        double dt = (dr + dl)/2;
        double xt = itsPosition.x + dt * cos(ot);
        double yt = itsPosition.y + dt * sin(ot);


        // compute motor velocity
        if(time != 0.0){
                itsLeftMotorVelocity = -(dl/time);
                if(itsRemoteMode == 1)
                        itsVelocityQue.add(itsLeftMotorVelocity/3.0);
                else
                        itsVelocityQue.add(itsLeftMotorVelocity);

                itsRightMotorVelocity = -(dr/time);
        }
        LDEBUG("Time[%f] Left V %f Right V %f,dl %f,dr %f",time,itsLeftMotorVelocity,
                        itsRightMotorVelocity,dl,dr);

        // the difference in position between the current and previous time step
        itsDiffPosition.x = dt * cos(ot);
        itsDiffPosition.y = dt * sin(ot);
        itsDiffPosition.z = ((dr - dl) / w ); //use z as orientation data

        itsPosition.x = xt;
        itsPosition.y = yt;
        itsPosition.z = ot;//use z as orientation data

        if(itsRemoteMode == 1)
                itsPosition.color = PixRGB<byte>(255,0,0);
        else
                itsPosition.color = PixRGB<byte>(0,255,0);
}

unsigned int BeoPilot::getRCChannel(int channel)
{
        if(channel < 0 || channel > 7)
                return 0;

        unsigned char cmd[2] = {104, channel};

        itsSerial->write(cmd, 2);
        std::vector<unsigned char> frame = itsSerial->readFrame(cmd[0], 255);

        if(frame.size() == 4)
        {
                unsigned int val = ((0x0FF & frame[0]) << 24) |
                                                                             ((0x0FF & frame[1]) << 16) |
                                                                             ((0x0FF & frame[2]) <<  8) |
                                                                             ((0x0FF & frame[3]) <<  0);
                return val;
        }
        else
                LINFO("Bad Frame Size! (%lu)", frame.size());

        return 0;

}

unsigned char BeoPilot::getRCStatus()
{
        unsigned char cmd = 103;

        itsSerial->write(cmd);
        std::vector<unsigned char> frame = itsSerial->readFrame(cmd, 255);

        if(frame.size() == 1)
                return frame[0];

        else
                LERROR("Bad Frame Size! (%lu)", frame.size());
        return 0;

}

unsigned char BeoPilot::getRCEnabled()
{
        unsigned char cmd = 105;

        itsSerial->write(cmd);
        std::vector<unsigned char> frame = itsSerial->readFrame(cmd, 255);

        if(frame.size() == 1)
                return frame[0];

        else
                LERROR("Bad Frame Size! (%lu)", frame.size());
        return 0;

}

unsigned int BeoPilot::getRCSpeed()
{
        unsigned char cmd = 106;

        itsSerial->write(cmd);
        std::vector<unsigned char> frame = itsSerial->readFrame(cmd, 255);

        if(frame.size() == 4)
        {
                unsigned int val = ((0x0FF & frame[0]) << 24) |
                                                                             ((0x0FF & frame[1]) << 16) |
                                                                             ((0x0FF & frame[2]) <<  8) |
                                                                             ((0x0FF & frame[3]) <<  0);
                return val;
        }
        else
                LINFO("Bad Frame Size! (%lu)", frame.size());

        return 0;

}
void BeoPilot::resetEncoder()
{

        unsigned char cmd = 108;
        itsSerial->write(cmd);

        itsPosition.x = 0;
        itsPosition.y = 0;
        itsPosition.z = 0;
        itsDiffPosition.x = 0;
        itsDiffPosition.y = 0;
        itsDiffPosition.z = 0;


}
// ######################################################################
void BeoPilot::SetMotors(float rotationalSpeed, float translationalSpeed)
{
        //Clamp the motor speeds to [-1 .. 1]
        if(rotationalSpeed > 1.0)
                rotationalSpeed = 1.0;
        if(rotationalSpeed < -1.0)
                rotationalSpeed = -1.0;
        if(translationalSpeed > 1.0)
                translationalSpeed = 1.0;
        if(translationalSpeed < -1.0)
                translationalSpeed = -1.0;

//        //Convert the motor speeds to [1 .. 127] so that 64 is full stop for a motor
//        unsigned char motor1speedbyte = ((0x00FF & int(std::floor(motor1speed * 64.0))) + 64);
//        unsigned char motor2speedbyte = ((0x00FF & int(std::floor(motor2speed * 64.0))) + 64);

        char rotSpeed  = char(rotationalSpeed    * 100.0);
        char tranSpeed = char(translationalSpeed * 100.0);

        //Send the command for the first motor
        unsigned char cmd[3];
        cmd[0] = 100;             //Command: Set Motor 1
        cmd[1] = rotSpeed;                                 //Command: Rotational Speed?
//        itsSerialMutex.lock();
        itsSerial->write(cmd, 2);
//        itsSerialMutex.unlock();
        usleep(10000);

        //Send the command for the second motor
        cmd[0] = 101;             //Command: Set Motor 2
        cmd[1] = tranSpeed;       //Command: Translational Speed?
//        itsSerialMutex.lock();
        itsSerial->write(cmd, 2);
//        itsSerialMutex.unlock();
        usleep(10000);
}



// ######################################################################
void BeoPilot::updateGUI()
{
        if(itsDisplayTimer.getSecs() > itsDisplayUpdateRate)
        {
                itsDisplayTimer.reset();
                //Grab the last key pressed by the user
                int key = itsMeterXwin.getLastKeyPress();

                float loc_forwardSpeed                 = 0;
                float loc_rotationalSpeed = 0;
                bool  loc_sonarProxAlert = true;
                float loc_sonarTime;

                itsSpeedMutex.lock();
                {
                        //Handle user input to manually drive the robot around using wasd keys
                        switch(key)
                        {
                                case -1:
                                        break;
                                case 25: //w
                                        itsForwardSpeed    += .05;
                                        break;
                                case 39: //s
                                        itsForwardSpeed    -= .05;
                                        break;
                                case 38: //a
                                        itsRotationalSpeed -= .05;
                                        break;
                                case 40: //d
                                        itsRotationalSpeed += .05;
                                        break;
                                case 65: //space
                                        itsForwardSpeed = 0;
                                        itsRotationalSpeed = 0;
                                        break;
                                case 9: //esc
                                        exit(0);
                                        break;
                                default:
                                        LINFO("Undefined Key: %d", key);
                                        break;
                        }
                        if(itsMeterXwin.pressedCloseButton()) exit(0);

                        loc_forwardSpeed          = itsForwardSpeed;
                        loc_rotationalSpeed = itsRotationalSpeed;
                }
                itsSpeedMutex.unlock();

                //Check on the sonar
                itsSonarMutex.lock();
                {
                        loc_sonarProxAlert = itsSonarProxAlert;
                        loc_sonarTime      = itsSonarTimer.getSecs();
                }
                itsSonarMutex.unlock();

                //Update the display
                itsControlImage = Image<PixRGB<byte> >(512,256,ZEROS);
                writeText(itsControlImage, Point2D<int>(0,0),  "Commands:",                         PixRGB<byte>(255,255,255), PixRGB<byte>(0,0,0),SimpleFont::FIXED(6));
                writeText(itsControlImage, Point2D<int>(0,8),  "w: Forwards",    PixRGB<byte>(180,255,255), PixRGB<byte>(0,0,0),SimpleFont::FIXED(6));
                writeText(itsControlImage, Point2D<int>(0,16), "s: Backwards",   PixRGB<byte>(180,255,255), PixRGB<byte>(0,0,0),SimpleFont::FIXED(6));
                writeText(itsControlImage, Point2D<int>(0,24), "a: Left",        PixRGB<byte>(180,255,255), PixRGB<byte>(0,0,0),SimpleFont::FIXED(6));
                writeText(itsControlImage, Point2D<int>(0,32), "d: Right",       PixRGB<byte>(180,255,255), PixRGB<byte>(0,0,0),SimpleFont::FIXED(6));
                writeText(itsControlImage, Point2D<int>(0,40), "space: STOP",    PixRGB<byte>(180,255,255), PixRGB<byte>(0,0,0),SimpleFont::FIXED(6));
                writeText(itsControlImage, Point2D<int>(0,48), "esc: quit",      PixRGB<byte>(180,255,255), PixRGB<byte>(0,0,0),SimpleFont::FIXED(6));

                char buffer[128];
                sprintf(buffer, "Forward Speed: %2.2f%% Right: %1.3f m/s Left: %1.3f m/s", loc_forwardSpeed*100.0,itsRightMotorVelocity,itsLeftMotorVelocity);
                writeText(itsControlImage, Point2D<int>(0,60), buffer, PixRGB<byte>(255,255,255), PixRGB<byte>(0,0,0),SimpleFont::FIXED(8));

                sprintf(buffer, "Rotational Speed: %0.2f -> %0.2f", loc_rotationalSpeed,
                                loc_rotationalSpeed *itsRotCap/100.0);
                writeText(itsControlImage, Point2D<int>(0,75), buffer, PixRGB<byte>(255,255,255), PixRGB<byte>(0,0,0),SimpleFont::FIXED(8));

                sprintf(buffer, "Right Encoder: %d", itsRightEncoder);
                writeText(itsControlImage, Point2D<int>(0,90), buffer, PixRGB<byte>(255,255,255), PixRGB<byte>(0,0,0),SimpleFont::FIXED(6));

                sprintf(buffer, "Left Encoder: %d", itsLeftEncoder);
                writeText(itsControlImage, Point2D<int>(0,98), buffer, PixRGB<byte>(255,255,255), PixRGB<byte>(0,0,0),SimpleFont::FIXED(6));

                sprintf(buffer, "Enc X: %fm",itsPosition.x );
                writeText(itsControlImage, Point2D<int>(0,116), buffer, PixRGB<byte>(255,255,255), PixRGB<byte>(0,0,0),SimpleFont::FIXED(6));

                sprintf(buffer, "Enc Y: %fm",itsPosition.y );
                writeText(itsControlImage, Point2D<int>(0,124), buffer, PixRGB<byte>(255,255,255), PixRGB<byte>(0,0,0),SimpleFont::FIXED(6));

                sprintf(buffer, "Enc R: %f",itsPosition.z );
                writeText(itsControlImage, Point2D<int>(0,132), buffer, PixRGB<byte>(255,255,255), PixRGB<byte>(0,0,0),SimpleFont::FIXED(6));

                if(itsLoadFromLogFile){
                        sprintf(buffer, "LEn X: %fm",itsLogPosition.x );
                        writeText(itsControlImage, Point2D<int>(0,140), buffer, PixRGB<byte>(255,255,255), PixRGB<byte>(0,0,0),SimpleFont::FIXED(6));

                        sprintf(buffer, "LEn Y: %fm",itsLogPosition.y );
                        writeText(itsControlImage, Point2D<int>(0,148), buffer, PixRGB<byte>(255,255,255), PixRGB<byte>(0,0,0),SimpleFont::FIXED(6));

                        sprintf(buffer, "LEn R: %f",itsLogPosition.z );
                        writeText(itsControlImage, Point2D<int>(0,156), buffer, PixRGB<byte>(255,255,255), PixRGB<byte>(0,0,0),SimpleFont::FIXED(6));
                }



                sprintf(buffer, "Sonar Timer: %0.2fs", loc_sonarTime);
                writeText(itsControlImage, Point2D<int>(256,0), buffer, PixRGB<byte>(255,255,255), PixRGB<byte>(0,0,0),SimpleFont::FIXED(8));

                PixRGB<byte> proxAlertColor;
                if(loc_sonarProxAlert)
                {
                        sprintf(buffer, "Proximity Alert!");
                        proxAlertColor = PixRGB<byte>(255,0,0);
                }
                else
                {
                        sprintf(buffer, "Sonar OK");
                        proxAlertColor = PixRGB<byte>(20,128,20);
                }
                writeText(itsControlImage, Point2D<int>(256,15), buffer, proxAlertColor, PixRGB<byte>(0,0,0),SimpleFont::FIXED(8));


                PixRGB<byte> serialOkColor;
                int code = itsSerial->getSerialErrno();
                if(code==0||code==15)
                {
                        sprintf(buffer, "Serial OK");
                        serialOkColor = PixRGB<byte>(20,128,20);
                }
                else
                {
                        sprintf(buffer, "Serial Disconnected!");
                        serialOkColor = PixRGB<byte>(255,0,0);
                }
                writeText(itsControlImage, Point2D<int>(256,30), buffer, serialOkColor, PixRGB<byte>(0,0,0),SimpleFont::FIXED(8));


                for(size_t i=0; i<itsChannelMeters.size(); i++)
                {

                        sprintf(buffer, "Channel%lu: %d", i+1, itsRCChannels[i]);

                        Point2D<int> pastePoint = Point2D<int>(
                                        int(itsChannelMeters[0].getWidth()*itsChannelMeters.size()*1.05+20),
                                        itsControlImage.getHeight() - itsChannelMeters[i].getHeight() - 10 + i*8
                                        );

                        writeText(itsControlImage, pastePoint, buffer, PixRGB<byte>(255,255,255), PixRGB<byte>(0,0,0),SimpleFont::FIXED(6));

                        pastePoint = Point2D<int>(
                                        int(10+itsChannelMeters[i].getWidth()*i*1.05),
                                        itsControlImage.getHeight() - itsChannelMeters[i].getHeight() - 10
                                        );
                        inplacePaste(itsControlImage, itsChannelMeters[i].render(itsRCChannels[i]), pastePoint );
                }

                sprintf(buffer, "Emergency Mode: %d", itsEmergencyMode);
                writeText(itsControlImage, Point2D<int>(256,88), buffer, PixRGB<byte>(255,255,255), PixRGB<byte>(0,0,0),SimpleFont::FIXED(6));

                sprintf(buffer, "RemoteMode: %d", itsRemoteMode);
                writeText(itsControlImage, Point2D<int>(256,96), buffer, PixRGB<byte>(255,255,255), PixRGB<byte>(0,0,0),SimpleFont::FIXED(6));

                if(itsLoadFromLogFile){
                        sprintf(buffer, "Log Mode: %d", itsLogRemoteMode);
                        writeText(itsControlImage, Point2D<int>(340,96), buffer, PixRGB<byte>(255,255,255), PixRGB<byte>(0,0,0),SimpleFont::FIXED(6));
                }
                sprintf(buffer, "Motor 1 Speed: %0.2f", itsMotor1Speed);
                writeText(itsControlImage, Point2D<int>(256,104), buffer, PixRGB<byte>(255,255,255), PixRGB<byte>(0,0,0),SimpleFont::FIXED(6));

                sprintf(buffer, "Motor 2 Speed: %0.2f", itsMotor2Speed);
                writeText(itsControlImage, Point2D<int>(256,112), buffer, PixRGB<byte>(255,255,255), PixRGB<byte>(0,0,0),SimpleFont::FIXED(6));

                sprintf(buffer, "RC Speed   : %0.2f -> %0.2f", itsRcTransSpeed,itsRcTransSpeedCapped);
                writeText(itsControlImage, Point2D<int>(256,120), buffer, PixRGB<byte>(255,255,255), PixRGB<byte>(0,0,0),SimpleFont::FIXED(6));

                sprintf(buffer, "RC Rotation: %0.2f -> %0.2f", itsRcRotSpeed,itsRcRotSpeedCapped);
                writeText(itsControlImage, Point2D<int>(256,128), buffer, PixRGB<byte>(255,255,255), PixRGB<byte>(0,0,0),SimpleFont::FIXED(6));



                //sprintf(buffer, "RCSpeed: %d", getRCSpeed());
                //writeText(itsControlImage, Point2D<int>(256,104), buffer, PixRGB<byte>(255,255,255), PixRGB<byte>(0,0,0),SimpleFont::FIXED(6));

                inplacePaste(itsDispImage,itsControlImage,Point2D<int>(0,0));


                //Draw The Map
                //float replayspeed = itsReplaySpeed.getVal();
                int w = itsMapImage.getWidth();
                int h = itsMapImage.getHeight();
                double scaleRate = 0.9;
          Point2D<int> drawPos(
                                int(itsPosition.x*itsMapScale + w/2),
                                 int(itsPosition.y*itsMapScale + h/2)
                                );
                        if(itsMapImage.coordsOk(drawPos))
                        {
                                itsPositionMap.push_back(itsPosition);
                        }else
                        {
                                //Find New Scale
                                do{
                                        itsMapScale *= scaleRate;
                                        drawPos = Point2D<int>(
                                                        int(itsPosition.x*itsMapScale + w/2),
                                                        int(itsPosition.y*itsMapScale + h/2)
                                                        );
                                }while(!itsMapImage.coordsOk(drawPos));
                                reDrawMap(itsMapScale);
                        }
                        itsMapImage.setVal(drawPos, itsPosition.color);

                Image<PixRGB<byte> > itsDrawMapImage(itsMapImage);
                drawCircle(itsDrawMapImage, drawPos, 7, PixRGB<byte>(255,255,255));
                drawLine(itsDrawMapImage, drawPos,-itsPosition.z, 7, PixRGB<byte>(255,255,255));

                if(itsLoadFromLogFile){
                        Point2D<int> drawPos2(
                                        int(itsLogPosition.x*itsMapScale + w/2),
                                        int(itsLogPosition.y*itsMapScale + h/2)
                                        );
                        if(!itsMapImage.coordsOk(drawPos2))
                        {
                                //Find New Scale
                                do{
                                        itsMapScale *= scaleRate;
                                        drawPos2 = Point2D<int>(
                                                        int(itsLogPosition.x*itsMapScale + w/2),
                                                        int(itsLogPosition.y*itsMapScale + h/2)
                                                        );
                                }while(!itsMapImage.coordsOk(drawPos2));
                                reDrawMap(itsMapScale);
                        }
                        //redraw it every time since we fastward the speed
                        if(itsReplaySpeed.getVal() != 1)
                                        reDrawMap(itsMapScale);
                        itsMapImage.setVal(drawPos2, itsLogPosition.color);
                        drawCircle(itsDrawMapImage, drawPos2, 7, PixRGB<byte>(0,255,0));
                        drawLine(itsDrawMapImage, drawPos2,-itsLogPosition.z, 7, PixRGB<byte>(255,255,255));
                }
                inplacePaste(itsDispImage,itsDrawMapImage,Point2D<int>(0,256));

                std::vector<std::vector<float> > lines;
                lines.push_back(itsVelocityQue.getVector());
                lines.push_back(itsVelocityTargetQue.getVector());

                std::vector<PixRGB<byte> > linesColor;
                linesColor.push_back(PixRGB<byte>(255,0,0));
                linesColor.push_back(PixRGB<byte>(255,165,0));

                Image<PixRGB<byte> > plotImage = multilinePlot(
                                lines,
                                288+70,256,
                                -1.0f,1.0f,
                                "velocity","m/s","",
                                linesColor,
                                PixRGB<byte>(255,255,255),
                                PixRGB<byte>(0,0,0)

                                );
                inplacePaste(itsDispImage,plotImage,Point2D<int>(512-70,0));

                //Draw Divide Line
                drawLine(itsDispImage,Point2D<int>(0,255),Point2D<int>(800,255),PixRGB<byte>(255,255,255));//HR
                drawLine(itsDispImage,Point2D<int>(512-70,0),Point2D<int>(512-70,255),PixRGB<byte>(255,255,255));//VT


                inplacePaste(itsDispImage,itsDispCameraImage,Point2D<int>(itsDispImage.getWidth()-320,itsDispImage.getHeight()-240));

                itsMeterXwin.drawImage(itsDispImage);

        }
}
void BeoPilot::reDrawMap(double scale)
{

        //Clean up the old images
        itsMapImage = Image<PixRGB<byte> >(800,344,ZEROS);

        int w = itsMapImage.getWidth();
        int h = itsMapImage.getHeight();

        //Redraw all points with new scale
        for(int i = 0;i < (int)itsLogPositionMap.size();i++)
        {
                Point2D<int> tmp(

                                int(itsLogPositionMap[i].x*scale + w/2),
                                int(itsLogPositionMap[i].y*scale + h/2)
                                );
                if(itsMapImage.coordsOk(tmp))
                        itsMapImage.setVal(tmp, itsLogPositionMap[i].color);
        }
        for(int i = 0;i < (int)itsPositionMap.size();i++)
        {
                Point2D<int> tmp(

                                int(itsPositionMap[i].x*scale + w/2),
                                int(itsPositionMap[i].y*scale + h/2)
                                );
                if(itsMapImage.coordsOk(tmp))
                        itsMapImage.setVal(tmp, itsPositionMap[i].color);
        }

}
// ######################################################################
void BeoPilot::updateMessage(const RobotSimEvents::EventMessagePtr& eMsg,
    const Ice::Current&)
{
        if(eMsg->ice_isA("::BeobotEvents::MotorRequest"))
        {
                itsSpeedMutex.lock();
                {
                        BeobotEvents::MotorRequestPtr mrMsg = BeobotEvents::MotorRequestPtr::dynamicCast(eMsg);
                        itsForwardSpeedReq    = mrMsg->transVel;
                        itsRotationalSpeedReq = mrMsg->rotVel;
                }
                itsSpeedMutex.unlock();
                LINFO("Received Motor Request Speed:[%f] Rotation:[%f]",itsForwardSpeedReq,itsRotationalSpeedReq);
        }
        else if(eMsg->ice_isA("::BeobotEvents::SonarMessage"))
        {
                itsSonarMutex.lock();
                {
                        itsSonarTimer.reset();
                        BeobotEvents::SonarMessagePtr srMsg = BeobotEvents::SonarMessagePtr::dynamicCast(eMsg);
                        itsSonarReadings = srMsg->distances;
                        itsSonarAngles   = srMsg->angles;
                }
                itsSonarMutex.unlock();
        }
}

// ######################################################################
void BeoPilot::getCurrentLogData()
{
        uint64 time = itsLogTimer.get()/1000.0;
if(itsReplaySpeed.getVal()==1)
{
        do{
                if(itsLogData[itsCurrentTimeIndex].time < time && itsCurrentTimeIndex < (int)itsLogData.size())
                        itsCurrentTimeIndex++;
        }while(itsLogData[itsCurrentTimeIndex].time < time && itsCurrentTimeIndex < (int)itsLogData.size());
}
        if((int)itsLogImageData.size() != 0){
                do{
                        if(itsLogImageData[itsCurrentTimeIndex2].time < time && itsCurrentTimeIndex2 < (int)itsLogImageData.size())
                                itsCurrentTimeIndex2++;
                }while(itsLogImageData[itsCurrentTimeIndex2].time < time && itsCurrentTimeIndex2 < (int)itsLogImageData.size());
                imageData img = itsLogImageData[itsCurrentTimeIndex2];
                if(file_exists(img.imagePath.c_str()))
                        itsDispCameraImage = Raster::ReadRGB(img.imagePath);
        LINFO("Found Index[%d] with Image[%f],Path %s",itsCurrentTimeIndex2,itsLogImageData[itsCurrentTimeIndex2].time,itsLogImageData[itsCurrentTimeIndex2].imagePath.c_str())        ;
        }
        LINFO("Current Time[%f] Found Index[%d] with LogTimeStamp[%f]",(double)time,itsCurrentTimeIndex,itsLogData[itsCurrentTimeIndex].time)        ;

for(int i = 0;i < itsReplaySpeed.getVal();i++)
{
        if(itsReplaySpeed.getVal() !=1 && itsCurrentTimeIndex < (int)itsLogData.size())
                        itsCurrentTimeIndex++;

        motorData tmp = itsLogData[itsCurrentTimeIndex];

//        if(img.time != 0.0)



        //itsForwardSpeed = tmp.transVel;
        //itsRotationalSpeed = tmp.rotVel;
        itsForwardSpeed = tmp.rcTransVel;
        itsRotationalSpeed = tmp.rcRotVel;

        itsLogDiffPosition.x = tmp.encoderX;
        itsLogDiffPosition.y = tmp.encoderY;
        itsLogDiffPosition.z = tmp.encoderOri;

        itsLogPosition.x += itsLogDiffPosition.x;
        itsLogPosition.y += itsLogDiffPosition.y;
        itsLogPosition.z += itsLogDiffPosition.z;

        itsRcTransSpeed = tmp.rcTransVel;
        itsRcRotSpeed = tmp.rcRotVel  ;
        itsLogRemoteMode = tmp.rcMode  ;

        itsMotor1Speed = itsForwardSpeed - itsRotationalSpeed;
        itsMotor2Speed = itsForwardSpeed + itsRotationalSpeed;
        if(itsLogRemoteMode == 1)
                itsLogPosition.color = PixRGB<byte>(255,0,0);
        else
                itsLogPosition.color = PixRGB<byte>(0,255,0);

        if(itsCurrentTimeIndex <(int)itsLogData.size())
                itsLogPositionMap.push_back(itsLogPosition);
}
}
void BeoPilot::drawAllMap(void)
{
        for(int i = 0;i < (int)itsLogData.size();i++){
                motorData tmp = itsLogData[i];
                itsLogDiffPosition.x = tmp.encoderX;
                itsLogDiffPosition.y = tmp.encoderY;
                itsLogDiffPosition.z = tmp.encoderOri;

                itsLogPosition.x += itsLogDiffPosition.x;
                itsLogPosition.y += itsLogDiffPosition.y;
                itsLogPosition.z += itsLogDiffPosition.z;
                itsLogPositionMap.push_back(itsLogPosition);
        }
}