BeoPilot.C

00001 #include "Raster/Raster.H"
00002 #include "Robots/Beobot2/Hardware/BeoPilot.H"
00003 #include "Ice/BeobotEvents.ice.H"
00004 #include "Component/ModelParam.H"
00005 #include "Component/ModelOptionDef.H"
00006 #include "Util/sformat.H"
00007 #include "Util/MathFunctions.H"
00008 #include "Image/ShapeOps.H" // for rescale()
00009 #include <iostream>
00010 #include <stdio.h>
00011 #include <sys/types.h>
00012 #include <sys/stat.h>
00013 #include <dirent.h>
00014 #include <unistd.h>
00015 #define LOG_FOLDER "../data/logs/IROS10_Equad"
00016 const ModelOptionCateg MOC_BeoPilot = {
00017         MOC_SORTPRI_3, "Beobot Pilot Related Options" };
00018 
00019 const ModelOptionDef OPT_MinimumSafeDistance =
00020 { MODOPT_ARG(float), "MinimumSafeDistance", &MOC_BeoPilot, OPTEXP_CORE,
00021         "The minimum distance (in meters) to objects we can allow before cutting off the motors.",
00022         "min-distance", '\0', "float", ".5"};
00023 
00024 const ModelOptionDef OPT_MaxSpeed =
00025 { MODOPT_ARG(float), "MaxSpeed", &MOC_BeoPilot, OPTEXP_CORE,
00026         "The maximum speed of the robot (from [0 .. 1]).",
00027         "max-speed", '\0', "float", "1.0"};
00028 
00029 const ModelOptionDef OPT_IgnoreSonar =
00030 { MODOPT_ARG(bool), "IgnoreSonar", &MOC_BeoPilot, OPTEXP_CORE,
00031         "Do we want to ignore the sonar for proximity warnings. Use this with caution, as someone could get hurt!?",
00032         "ignore-sonar", '\0', "true | false", "true"};
00033 
00034 const ModelOptionDef OPT_LogFile =
00035 { MODOPT_ARG(std::string), "LogFile", &MOC_BeoPilot, OPTEXP_CORE,
00036         "There are 3 mode for input log file name \n"
00037   "you can enter\n"
00038   "1)  ./bin/app-BeoPilot --log-file=<Full Log File Path>\n"
00039   "    it will open the file directly\n"
00040   "2)  ./bin/app-BeoPilot --log-file=<date>\n"
00041   "    it will look for ../data/logs/<date>/Log_<date>.txt\n"
00042   "    The date formate can be flexible.\n"
00043   "    You can enter full date such as 2009_11_03__14_03_49\n"
00044   "    or 20091103 or 091103 or 1403 or 140349. \n"
00045   "    If there are more than one match, \n"
00046   "    it will list all options for choosen.\n"
00047   "3)  ./bin/app-BeoPilot --log-file=search\n"
00048   "    it will look for all directory under ../data/logs/\n"
00049   "    and list all files for choosen\n",
00050         "log-file", '\0', "<string>", "NULL"};
00051 
00052 
00053 const ModelOptionDef OPT_ReplaySpeed=
00054 { MODOPT_ARG(int), "ReplaySpeed", &MOC_BeoPilot, OPTEXP_CORE,
00055         "The replay speed from the Log file,default is 1x. To speed up the speed, using --speed=2 or faster",
00056         "speed", '\0', "int", "1"};
00057 // ######################################################################
00058 BeoPilot::BeoPilot(int id, OptionManager& mgr,
00059                    const std::string& descrName, const std::string& tagName) :
00060   RobotBrainComponent(mgr, descrName, tagName),
00061   itsSerial(new Serial(mgr)),
00062         itsSerialTimer(1000000),
00063   itsCurrMessageID(0),
00064         itsCurrentTimeIndex(0),
00065         itsLogTimer(1000000),
00066         itsLoadFromLogFile(false),
00067         itsLogLeftMotorVelocity(0.0),
00068         itsLogRightMotorVelocity(0.0),
00069         itsLogPosition(0.0,0.0,0.0),
00070         itsLogDiffPosition(0.0,0.0,0.0),
00071         itsRCChannels(7, -1),
00072         itsEmergencyMode(-1),
00073         itsPosition(0.0,0.0,0.0),
00074         itsDiffPosition(0.0,0.0,0.0),
00075         itsMapImage(800-320,344,ZEROS),
00076         itsEncoderTimer(1000000),
00077         itsLeftMotorVelocity(0.0),
00078         itsRightMotorVelocity(0.0),
00079         itsPidLeft(0.3,0.000,0.00,0.0,0.0,0,0,0,1.0,-1.0),
00080         itsPidRight(0.0,0.000,0.00,0.0,0.0,0,0,0,1.0,-1.0),
00081         itsSonarTimeout(.5),
00082         itsControlImage(512,256,ZEROS),
00083         itsDispImage(800,600,ZEROS),
00084         itsDispCameraImage(320,240,ZEROS),
00085         itsMeterXwin(itsDispImage,"Pilot Control"),
00086         //itsMapXwin(itsMapImage,"Map Control"),
00087         itsDisplayUpdateRate(.05),
00088         itsMapScale(100.0),
00089         itsChannelMeters(7, SimpleMeter(20, 60, 1040,1921)),
00090         itsVelocityQue(50),
00091         itsVelocityTargetQue(50),
00092         itsMinimumSafeDistance(&OPT_MinimumSafeDistance, this, 0),
00093         itsMaxSpeed(&OPT_MaxSpeed, this, 0),
00094         itsIgnoreSonar(&OPT_IgnoreSonar, this, 0),
00095         itsLogFile(&OPT_LogFile, this, 0),
00096         itsReplaySpeed(&OPT_ReplaySpeed, this, 0)
00097 {
00098         addSubComponent(itsSerial);
00099         //Connect to the serial motor board
00100         itsSerial->configureSearch ("motorboard", 115200);
00101 }
00102 
00103 void BeoPilot::checkLogFile()
00104 {
00105         if(itsLogFile.getVal() =="NULL"){
00106                 LINFO("Start BeoPilot with real device");
00107         }else if(itsLogFile.getVal() =="search"){
00108                 LINFO("Search for all log file");
00109                 DIR *dir_p;
00110                 struct dirent *entry_p;
00111                 dir_p = ::opendir(LOG_FOLDER);
00112                 if(dir_p == NULL)
00113                         LFATAL("Can Not Open ../data/logs directory");
00114                 //uint i = 1;
00115 
00116                 std::vector<std::string> subdir;
00117                 while((entry_p = ::readdir(dir_p)))
00118                 {
00119                         std::string subdirname(entry_p->d_name);
00120                         if(subdirname != "." && subdirname !="..")
00121                         {
00122                                 //LINFO("File[%d]:[%s]",i++,subdirname.c_str());
00123                                 subdir.push_back(subdirname);
00124                         }
00125                 }
00126                 (void)::closedir(dir_p);
00127 
00128                 //sort filename
00129                 std::sort(subdir.begin(),subdir.end());
00130                 for(int i = 0;i < (int)subdir.size();i++)
00131                 {
00132                                 LINFO("File[%d]:[%s]",i+1,subdir[i].c_str());
00133                 }
00134 
00135                 int option;
00136                 do{
00137                         LINFO("Please Choose a File Number:");
00138                         scanf("%d",&option);//FIXX Any better idea?
00139                         if(option <1 ||option >int(subdir.size()))
00140                                 LINFO("Option Invalid, please try again");
00141                 }while(option < 1 || option > int(subdir.size()));
00142                         LINFO("Your Choose is [%d] filename[%s] ",option,subdir[option-1].c_str());
00143 //                int run,segt;
00144 //                char buf[255];
00145                 //sscanf(subdir[option-1].c_str(),"S%d_R%d_%s",&run,&segt,buf);
00146                 std::string logFileName(
00147                                 sformat("%s/%s/%s.txt",
00148                                                 LOG_FOLDER,
00149                                                 subdir[option-1].c_str(),
00150                                                 subdir[option-1].c_str()
00151                                                 //buf
00152                                                 ));
00153                                                                 itsLogFile.setVal(logFileName);
00154                                                                 loadFromLogFile();
00155         }else{
00156                 LINFO("LogFileName:[%s]",itsLogFile.getVal().c_str());
00157                                                                 loadFromLogFile();
00158         }
00159 
00160 
00161 }
00162 
00163 void BeoPilot::loadFromLogFile()
00164 {
00165 
00166 
00167                 FILE *logFile = fopen(itsLogFile.getVal().c_str(),"r");
00168                 if(logFile == NULL)
00169                 {
00170                                 LFATAL("can't not open file: %s",itsLogFile.getVal().c_str());
00171                 }else
00172                 {
00173                         char line[512];
00174                         while(fgets(line,sizeof line,logFile)!= NULL)
00175                         {
00176                                 float time,transVel,rotVel,encoderX,encoderY,encoderOri,rcTransVel,rcRotVel;
00177                                 int rcMode;
00178                                 int ret = sscanf (line,"[%f] MTR rcMode: %d, transVel: %f  rotVel: %f encoderX: %f encoderY: %f encoderOri: %f rcTransVel: %f rcRotVel: %f",
00179               &time, &rcMode, &transVel, &rotVel, &encoderX,  &encoderY, &encoderOri, &rcTransVel, &rcRotVel);
00180                                 if(ret == 9){
00181                                         motorData tmp;
00182                                         tmp.time = time;
00183                                         tmp.rcMode = rcMode;
00184                                         tmp.transVel = transVel;
00185                                         tmp.rotVel = rotVel;
00186                                         tmp.encoderX = encoderX;
00187                                         tmp.encoderY = encoderY;
00188                                         tmp.encoderOri = encoderOri;
00189                                         tmp.rcTransVel = rcTransVel;
00190                                         tmp.rcRotVel = rcRotVel;
00191                                         itsLogData.push_back(tmp);
00192                                         LINFO("Got Motor Data %f %d",time,rcMode);
00193 
00194                                 }
00195 
00196                                 std::string imagePath;
00197                                 char imgPath[256];
00198                                 ret = sscanf(line,"[%f] CAM filename: %s", &time, imgPath);
00199                                 if(ret == 2)
00200                                 {
00201                                         imageData imgData;
00202                                         imgData.time = time;
00203                                         imgData.imagePath = imgPath;
00204                                         itsLogImageData.push_back(imgData);
00205                                         LINFO("Got Image at %f path=%s",time,imgPath);
00206                                 }
00207 
00208                         }
00209                         LINFO("Load Log file with %d motor command lines",(int)itsLogData.size());
00210                         LINFO("Load Log file with %d Images",(int)itsLogImageData.size());
00211                         itsLoadFromLogFile = true;
00212                         itsLogTimer.reset();
00213                         //drawAllMap();
00214                 }
00215 
00216 
00217 
00218 }
00219 
00220 
00221 void BeoPilot::start3()
00222 {
00223         checkLogFile();
00224         //while(!itsSerial->isSerialOk()){
00225         //        LINFO("Can't not open serial port,keep try");
00226         //        itsSerial->enablePort(itsSerial->getDevName().c_str());///dev/ttyUSB0
00227         //}
00228         if(itsSerial->isSerialOk())
00229                 LINFO("%s found on %s", itsSerial->getDeviceDescriptor().c_str(), itsSerial->getDevName().c_str());
00230         resetEncoder();
00231         //Check our command line options to ensure that they are sane
00232         if(itsMinimumSafeDistance.getVal() < 0)
00233                 LFATAL("Invalid Minimum Safe Distance (%f). Must be > 0!", itsMinimumSafeDistance.getVal());
00234         if(itsMaxSpeed.getVal() > 1.0 || itsMaxSpeed.getVal() < 0)
00235                 LFATAL("Invalid Maximum Speed Distance (%f). Must be > 0 && < 1.0!", itsMaxSpeed.getVal());
00236 }
00237 
00238 // ######################################################################
00239 BeoPilot::~BeoPilot()
00240 {
00241         //Kill the motors on exit
00242         SetMotors(0,0);
00243 }
00244 
00245 
00246 void BeoPilot::registerTopics()
00247 {
00248         registerSubscription("MotorRequestTopic");
00249         registerSubscription("SonarMessageTopic");
00250         registerPublisher("MotorMessageTopic");
00251 }
00252 
00253 
00254 void BeoPilot::evolve()
00255 {
00256 
00257                 //Recieve telemetry from the propeller
00258                 UpdateRCStatus();
00259                 itsVelocityTargetQue.add(itsRcTransSpeedCapped);
00260                 //itsVelocityQue.add(randomDouble()*2 -1.0);
00261                 //itsVelocityTargetQue.add(randomDouble());
00262 
00263         //Check if it is in log file replay mode and also make sure emergency mode is off
00264         if(itsLoadFromLogFile){
00265                 //No RC(Emergency = -1): getCurrentLogData and timer resume but no set Motor
00266                 //With RC:
00267                 // Emergency is off (Emergency = 0)
00268                 //        1) Mode 1,2 :timer pause
00269                 //        2) Mode 3 timer resume and run log file,set motor
00270                 // Emergency is on (Emergency = 255)
00271                 //         1) Timer pause
00272                 if(itsEmergencyMode == -1||itsEmergencyMode == 255)
00273                 {
00274                         itsLogTimer.resume();
00275                         getCurrentLogData();
00276                 }
00277                 else if(itsEmergencyMode == 0 && itsRemoteMode == 3)
00278                 {
00279                         itsLogTimer.resume();
00280                         getCurrentLogData();
00281                         SetMotors(itsRotationalSpeed,itsForwardSpeed);
00282                 }
00283                 else{
00284                         itsLogTimer.pause();
00285                         LINFO("Wait for RC mode 3");
00286                 }
00287                 uint64 time = itsLogTimer.get()/1000.0;
00288                 LDEBUG("Current Log Timer is %f",(double)time);
00289         }
00290         else{
00291                 //Local copies of all shared data that we deal with so that we can
00292                 //hold onto our locks for as short a time as possible
00293                 float loc_rotVel;
00294                 float loc_transVel;
00295                 bool  loc_sonarProxAlert;
00296 
00297                 //Check to make sure that we have heard from our sonar recently, and that it has
00298                 //reported all sonars to be reading past the minimum safe distance. If we are in
00299                 //a dangerous situation, then set the itsSonarProxAlert
00300                 itsSonarMutex.lock();
00301                 {
00302                         if(itsSonarReadings.size() > 0)
00303                         {
00304                                 float minSonarReading = *(std::min_element(itsSonarReadings.begin(), itsSonarReadings.end()));
00305                                 if(itsSonarTimer.getSecs() > itsSonarTimeout || minSonarReading < itsMinimumSafeDistance.getVal())
00306                                         itsSonarProxAlert = true;
00307                                 else
00308                                         itsSonarProxAlert = false;
00309                         }
00310                         else
00311                                 itsSonarProxAlert = true;
00312 
00313                         loc_sonarProxAlert = itsSonarProxAlert;
00314                 }
00315                 itsSonarMutex.unlock();
00316 
00317                 //Set the current speed of the robot by either clamping it to the requested speed, or killing it
00318                 //if we are in a dangerous situation
00319                 itsSpeedMutex.lock();
00320                 {
00321                         itsRotationalSpeed = itsRotationalSpeedReq;
00322                         itsForwardSpeed = itsForwardSpeedReq;
00323                         //If we haven't heard from the sonar or things are getting too close,
00324                         //then let's kill the motors
00325                         if((itsIgnoreSonar.getVal() == false) && loc_sonarProxAlert)
00326                         {
00327                                 LINFO("SONAR ERROR!");
00328                                 itsRotationalSpeed = 0;
00329                                 itsForwardSpeed    = 0;
00330                         }
00331 
00332                         //Clamp the latest movement requests to the maximum speed as
00333                         //specified by the command line
00334                         if(itsRotationalSpeed > itsMaxSpeed.getVal())
00335                                 itsRotationalSpeed = itsMaxSpeed.getVal();
00336                         else if(itsRotationalSpeed < -itsMaxSpeed.getVal())
00337                                 itsRotationalSpeed = -itsMaxSpeed.getVal();
00338 
00339                         if(itsForwardSpeed > itsMaxSpeed.getVal())
00340                                 itsForwardSpeed = itsMaxSpeed.getVal();
00341                         else if(itsForwardSpeed < -itsMaxSpeed.getVal())
00342                                 itsForwardSpeed = -itsMaxSpeed.getVal();
00343 
00344                         //Set our local copies to the newly clamped requested speeds
00345                         loc_rotVel   = itsRotationalSpeed;
00346                         loc_transVel = itsForwardSpeed;
00347                         //LINFO("Set Local Rot/TranVel (%f, %f)", loc_rotVel, loc_transVel);
00348                 }
00349                 itsSpeedMutex.unlock();
00350                 //Move motor in manual mode
00351                 if(itsRemoteMode == 1 ||itsRemoteMode == 3)
00352                 {
00353                         //Send the actual motor commands to the motor driver/propeller
00354                         SetMotors(loc_rotVel, loc_transVel);
00355                 }else if(itsRemoteMode == 2)
00356                 {
00357                 double m1velReq = (itsRcTransSpeedCapped + itsRcRotSpeedCapped)*3.0;
00358                 double m2velReq = (itsRcTransSpeedCapped - itsRcRotSpeedCapped)*3.0;
00359 
00360                 float leftPWM =         itsPidLeft.update(m1velReq,itsLeftMotorVelocity);
00361                 float rightPWM =         itsPidRight.update(itsRightMotorVelocity,m2velReq);
00362 
00363                 double transPWM = leftPWM;// (leftPWM+rightPWM)/2.0;
00364                 double rotPWM = 0.0;//(leftPWM-rightPWM)/2.0;
00365 
00366                 LINFO("LEFT PWM %f, RIGT PWM %f",leftPWM,rightPWM);
00367                 LINFO("LEFT Request %f, LEFT Cur %f",m1velReq,itsLeftMotorVelocity);
00368                 LINFO("RIGT Request %f, RIGT Cur %f",m2velReq,itsRightMotorVelocity);
00369                 LINFO("Rc Tran %f Rc Rot %f",itsRcTransSpeedCapped,itsRcRotSpeedCapped);
00370                 LINFO("Pwm Tran %f Pwm Rot %f",transPWM,rotPWM);
00371                 LINFO("=====================================================");
00372                         SetMotors(rotPWM,transPWM) ;
00373                 }
00374 
00375                 //Report the current motor speeds to IceStorm
00376                 BeobotEvents::MotorMessagePtr msg = new BeobotEvents::MotorMessage;
00377 
00378                 msg->RequestID = itsCurrMessageID;
00379 
00380                 msg->transVel = loc_transVel;
00381                 msg->rotVel   = loc_rotVel;
00382                 msg->motor1   = Ice::Int(itsMotor1Speed);
00383                 msg->motor2   = Ice::Int(itsMotor2Speed);
00384 
00385                 msg->encoderX   = itsDiffPosition.x;
00386                 msg->encoderY   = itsDiffPosition.y;
00387                 msg->encoderOri = itsDiffPosition.z;
00388 
00389                 msg->rcTransVel = itsRcTransSpeed;
00390                 msg->rcRotVel   = itsRcRotSpeed;
00391                 msg->rcMode     = itsRemoteMode;
00392 
00393                 this->publish("MotorMessageTopic", msg);
00394                 //FIXX        LINFO("MotorMessageTopic: %d", itsCurrMessageID);
00395 
00396 
00397                 itsCurrMessageID++;
00398                 usleep(10000);
00399         }
00400                 //Draw the GUI to the screen
00401                 updateGUI();
00402 
00403 
00404                 double locSerialTime = (double) itsSerialTimer.get()/1000000.0;//sec
00405                 int code = itsSerial->getSerialErrno();
00406                 if(code==0||code==15){//FIXXX 15 is serialErrReadTimedOut
00407                         itsSerialTimer.reset();
00408                 }else if(locSerialTime > 3.0){
00409                         LINFO("Try reconnect Timer:%f",locSerialTime);
00410                         itsSerialTimer.reset();
00411                         itsSerial->enablePort(itsSerial->getDevName().c_str());///dev/ttyUSB0
00412                         LINFO("Tried Reconnect");
00413                         usleep(100000);
00414                 }else{
00415                         LINFO("Serial Timer:%f",locSerialTime);
00416                         LINFO("Serial Error Number:[%d]",itsSerial->getSerialErrno());
00417                 }
00418 }
00419 
00420 void BeoPilot::UpdateRCStatus()
00421 {
00422         unsigned char cmd = 107;
00423         itsSerial->write(cmd);
00424         std::vector<unsigned char> frame = itsSerial->readFrame(cmd, 255, -1, .2);//FIXXX Serial Crash Here
00425 
00426 
00427         if(frame.size() == 26)
00428         {
00429                 itsRCChannels[0] = ((0x0FF & frame[ 0]) << 8) |
00430                                                            ((0x0FF & frame[ 1]) << 0);
00431 
00432                 itsRCChannels[1] = ((0x0FF & frame[ 2]) << 8) |
00433                                                                         ((0x0FF & frame[ 3]) << 0);
00434 
00435                 itsRCChannels[2] = ((0x0FF & frame[ 4]) << 8) |
00436                                                                         ((0x0FF & frame[ 5]) << 0);
00437 
00438                 itsRCChannels[3] = ((0x0FF & frame[ 6]) << 8) |
00439                                                            ((0x0FF & frame[ 7]) << 0);
00440 
00441                 itsRCChannels[4] = ((0x0FF & frame[ 8]) << 8) |
00442                                                                        ((0x0FF & frame[ 9]) << 0);
00443 
00444                 itsRCChannels[5] = ((0x0FF & frame[10]) << 8) |
00445                                                                  ((0x0FF & frame[11]) << 0);
00446 
00447                 itsRCChannels[6] = ((0x0FF & frame[12]) << 8) |
00448                                                            ((0x0FF & frame[13]) << 0);
00449 
00450                 itsEmergencyMode = frame[14];
00451                 if(itsRCChannels[2] > 1800)
00452                         itsEmergencyMode = 255;
00453                 itsRemoteMode    = frame[15];
00454                 itsMotor1Speed   = (frame[16] - 64.0) / 64.0;
00455                 itsMotor2Speed   = (frame[17] - 64.0) / 64.0;
00456 
00457 
00458                 itsLastLeftEncoder  = itsLeftEncoder;
00459                 itsLeftEncoder   = ((0x0FF & frame[18]) << 24) |
00460                                                            ((0x0FF & frame[19]) << 16) |
00461                                                            ((0x0FF & frame[20]) <<  8) |
00462                                                            ((0x0FF & frame[21]) <<  0);
00463 
00464                 itsLastRightEncoder = itsRightEncoder;
00465                 itsRightEncoder  =
00466                                                                                           ((0x0FF & frame[22]) << 24) |
00467                                                            ((0x0FF & frame[23]) << 16) |
00468                                                            ((0x0FF & frame[24]) <<  8) |
00469                                                            ((0x0FF & frame[25]) <<  0);
00470                 if(itsRCChannels[2] < 1800){
00471                         itsRcTransSpeed = (200 - ((itsRCChannels[1] - 1041) * 200)/(1874 - 1041)) -100;
00472                         //if it's mode 1
00473                         if( itsRCChannels[6] > 1800)
00474                                 itsRcRotSpeed =         (200 - ((itsRCChannels[0] - 963) * 200)/(1775 -963)) -100;
00475                         else
00476                                 itsRcRotSpeed =         (200 - ((itsRCChannels[0] - 1105) * 200)/(1928 -1105)) -100;
00477                         double speed_cap = 100 - ((itsRCChannels[2] - 1041) * 100)/(1750 -1041);
00478                         double rot_vel_cap = ((itsRCChannels[5] -900)* 100)/(2073 -900);
00479                         itsTransCap = speed_cap;
00480                         itsRotCap = rot_vel_cap;
00481                         if(speed_cap >  100)speed_cap = 100;
00482                         if(speed_cap <    0)speed_cap = 0;
00483                         if(rot_vel_cap   >  100)rot_vel_cap= 100;
00484                         if(rot_vel_cap   <    0)rot_vel_cap= 0;
00485 
00486                         if(itsRcTransSpeed >  100)itsRcTransSpeed = 100;
00487                         if(itsRcTransSpeed < -100)itsRcTransSpeed = -100;
00488                         if(itsRcRotSpeed   >  100)itsRcRotSpeed= 100;
00489                         if(itsRcRotSpeed   < -100)itsRcRotSpeed= -100;
00490 
00491                         itsRcTransSpeedCapped =itsRcTransSpeed* speed_cap/100;
00492                         itsRcRotSpeedCapped = itsRcRotSpeed * rot_vel_cap/-100;
00493                         itsRcTransSpeed /=100.0;
00494                         itsRcRotSpeed /= 100.0;
00495                         itsRcTransSpeedCapped /= 100.0;
00496                         itsRcRotSpeedCapped /= 100.0;
00497                         LDEBUG("RC cap %f Rc rot cap %f", speed_cap,rot_vel_cap);
00498                         LDEBUG("RC trans%f Rc rot  %f",  itsRcTransSpeed,itsRcRotSpeed);
00499                 }else{
00500                         itsRcTransSpeed = 0;
00501                         itsRcRotSpeed = 0;
00502                         itsRcTransSpeedCapped = 0;
00503                         itsRcRotSpeedCapped = 0;
00504                 }
00505 
00506                 //After Get RC update,we update encoder value
00507                 UpdatePosition();
00508 
00509 
00510         }
00511         else
00512         {
00513                 //LERROR("BAD FRAME RECIEVED!");
00514         }
00515 
00516 }
00517 
00518 // We have 10 inch diameter wheel
00519 // Motor Shaft will turn 21 times for one wheel spin
00520 // The encoder have 4000 tick per revolution
00521 // When the wheel have one revolution, it travels 10 * pi inch
00522 //
00523 // 10* pi inch  = 0.797964534 meters
00524 // Which have 1000 * 21 ticks
00525 //  (10 * pi)/84,000 * 0.0254 =  9.49957779 * 10^(-6) = 0.000009.49957779 meter
00526 // Which means for every tick,
00527 // the wheel will move  meter
00528 //
00529 // Width between two wheel W = 510 mm = 0.51m
00530 // To convert odometry data to position, we need do some math again...
00531 // Ot+1 = Ot + (Dr - Dl) / W
00532 // Dt,t+1 = (Dr + Dl) / 2
00533 // Xt+1 = Xt + Dt,t+1 * cos(Ot+1)
00534 // Yt+1 = Yt + Dt,t+1 * sin(Ot+1)
00535 void BeoPilot::UpdatePosition(void)
00536 {
00537         if(itsLastRightEncoder == 0.0 && itsLastLeftEncoder == 0.0)
00538                 itsEncoderTimer.reset();
00539 
00540         double time = (double) itsEncoderTimer.getReset()/1000000.0;//sec
00541 
00542 //        double meterPerTick = 0.00000949957779 ;
00543         double ticksPerMeter = 105267.836;
00544         double dr = (itsRightEncoder - itsLastRightEncoder) / ticksPerMeter;
00545         double dl = (itsLeftEncoder - itsLastLeftEncoder)  / ticksPerMeter;
00546         double w = 0.51;
00547         double ot = itsPosition.z + ((dr - dl) / w );
00548         double dt = (dr + dl)/2;
00549         double xt = itsPosition.x + dt * cos(ot);
00550         double yt = itsPosition.y + dt * sin(ot);
00551 
00552 
00553         // compute motor velocity
00554         if(time != 0.0){
00555                 itsLeftMotorVelocity = -(dl/time);
00556                 if(itsRemoteMode == 1)
00557                         itsVelocityQue.add(itsLeftMotorVelocity/3.0);
00558                 else
00559                         itsVelocityQue.add(itsLeftMotorVelocity);
00560 
00561                 itsRightMotorVelocity = -(dr/time);
00562         }
00563         LDEBUG("Time[%f] Left V %f Right V %f,dl %f,dr %f",time,itsLeftMotorVelocity,
00564                         itsRightMotorVelocity,dl,dr);
00565 
00566         // the difference in position between the current and previous time step
00567         itsDiffPosition.x = dt * cos(ot);
00568         itsDiffPosition.y = dt * sin(ot);
00569         itsDiffPosition.z = ((dr - dl) / w ); //use z as orientation data
00570 
00571         itsPosition.x = xt;
00572         itsPosition.y = yt;
00573         itsPosition.z = ot;//use z as orientation data
00574 
00575         if(itsRemoteMode == 1)
00576                 itsPosition.color = PixRGB<byte>(255,0,0);
00577         else
00578                 itsPosition.color = PixRGB<byte>(0,255,0);
00579 }
00580 
00581 unsigned int BeoPilot::getRCChannel(int channel)
00582 {
00583         if(channel < 0 || channel > 7)
00584                 return 0;
00585 
00586         unsigned char cmd[2] = {104, channel};
00587 
00588         itsSerial->write(cmd, 2);
00589         std::vector<unsigned char> frame = itsSerial->readFrame(cmd[0], 255);
00590 
00591         if(frame.size() == 4)
00592         {
00593                 unsigned int val = ((0x0FF & frame[0]) << 24) |
00594                                                                              ((0x0FF & frame[1]) << 16) |
00595                                                                              ((0x0FF & frame[2]) <<  8) |
00596                                                                              ((0x0FF & frame[3]) <<  0);
00597                 return val;
00598         }
00599         else
00600                 LINFO("Bad Frame Size! (%lu)", frame.size());
00601 
00602         return 0;
00603 
00604 }
00605 
00606 unsigned char BeoPilot::getRCStatus()
00607 {
00608         unsigned char cmd = 103;
00609 
00610         itsSerial->write(cmd);
00611         std::vector<unsigned char> frame = itsSerial->readFrame(cmd, 255);
00612 
00613         if(frame.size() == 1)
00614                 return frame[0];
00615 
00616         else
00617                 LERROR("Bad Frame Size! (%lu)", frame.size());
00618         return 0;
00619 
00620 }
00621 
00622 unsigned char BeoPilot::getRCEnabled()
00623 {
00624         unsigned char cmd = 105;
00625 
00626         itsSerial->write(cmd);
00627         std::vector<unsigned char> frame = itsSerial->readFrame(cmd, 255);
00628 
00629         if(frame.size() == 1)
00630                 return frame[0];
00631 
00632         else
00633                 LERROR("Bad Frame Size! (%lu)", frame.size());
00634         return 0;
00635 
00636 }
00637 
00638 unsigned int BeoPilot::getRCSpeed()
00639 {
00640         unsigned char cmd = 106;
00641 
00642         itsSerial->write(cmd);
00643         std::vector<unsigned char> frame = itsSerial->readFrame(cmd, 255);
00644 
00645         if(frame.size() == 4)
00646         {
00647                 unsigned int val = ((0x0FF & frame[0]) << 24) |
00648                                                                              ((0x0FF & frame[1]) << 16) |
00649                                                                              ((0x0FF & frame[2]) <<  8) |
00650                                                                              ((0x0FF & frame[3]) <<  0);
00651                 return val;
00652         }
00653         else
00654                 LINFO("Bad Frame Size! (%lu)", frame.size());
00655 
00656         return 0;
00657 
00658 }
00659 void BeoPilot::resetEncoder()
00660 {
00661 
00662         unsigned char cmd = 108;
00663         itsSerial->write(cmd);
00664 
00665         itsPosition.x = 0;
00666         itsPosition.y = 0;
00667         itsPosition.z = 0;
00668         itsDiffPosition.x = 0;
00669         itsDiffPosition.y = 0;
00670         itsDiffPosition.z = 0;
00671 
00672 
00673 }
00674 // ######################################################################
00675 void BeoPilot::SetMotors(float rotationalSpeed, float translationalSpeed)
00676 {
00677         //Clamp the motor speeds to [-1 .. 1]
00678         if(rotationalSpeed > 1.0)
00679                 rotationalSpeed = 1.0;
00680         if(rotationalSpeed < -1.0)
00681                 rotationalSpeed = -1.0;
00682         if(translationalSpeed > 1.0)
00683                 translationalSpeed = 1.0;
00684         if(translationalSpeed < -1.0)
00685                 translationalSpeed = -1.0;
00686 
00687 //        //Convert the motor speeds to [1 .. 127] so that 64 is full stop for a motor
00688 //        unsigned char motor1speedbyte = ((0x00FF & int(std::floor(motor1speed * 64.0))) + 64);
00689 //        unsigned char motor2speedbyte = ((0x00FF & int(std::floor(motor2speed * 64.0))) + 64);
00690 
00691         char rotSpeed  = char(rotationalSpeed    * 100.0);
00692         char tranSpeed = char(translationalSpeed * 100.0);
00693 
00694         //Send the command for the first motor
00695         unsigned char cmd[3];
00696         cmd[0] = 100;             //Command: Set Motor 1
00697         cmd[1] = rotSpeed;                                 //Command: Rotational Speed?
00698 //        itsSerialMutex.lock();
00699         itsSerial->write(cmd, 2);
00700 //        itsSerialMutex.unlock();
00701         usleep(10000);
00702 
00703         //Send the command for the second motor
00704         cmd[0] = 101;             //Command: Set Motor 2
00705         cmd[1] = tranSpeed;       //Command: Translational Speed?
00706 //        itsSerialMutex.lock();
00707         itsSerial->write(cmd, 2);
00708 //        itsSerialMutex.unlock();
00709         usleep(10000);
00710 }
00711 
00712 
00713 
00714 // ######################################################################
00715 void BeoPilot::updateGUI()
00716 {
00717         if(itsDisplayTimer.getSecs() > itsDisplayUpdateRate)
00718         {
00719                 itsDisplayTimer.reset();
00720                 //Grab the last key pressed by the user
00721                 int key = itsMeterXwin.getLastKeyPress();
00722 
00723                 float loc_forwardSpeed                 = 0;
00724                 float loc_rotationalSpeed = 0;
00725                 bool  loc_sonarProxAlert = true;
00726                 float loc_sonarTime;
00727 
00728                 itsSpeedMutex.lock();
00729                 {
00730                         //Handle user input to manually drive the robot around using wasd keys
00731                         switch(key)
00732                         {
00733                                 case -1:
00734                                         break;
00735                                 case 25: //w
00736                                         itsForwardSpeed    += .05;
00737                                         break;
00738                                 case 39: //s
00739                                         itsForwardSpeed    -= .05;
00740                                         break;
00741                                 case 38: //a
00742                                         itsRotationalSpeed -= .05;
00743                                         break;
00744                                 case 40: //d
00745                                         itsRotationalSpeed += .05;
00746                                         break;
00747                                 case 65: //space
00748                                         itsForwardSpeed = 0;
00749                                         itsRotationalSpeed = 0;
00750                                         break;
00751                                 case 9: //esc
00752                                         exit(0);
00753                                         break;
00754                                 default:
00755                                         LINFO("Undefined Key: %d", key);
00756                                         break;
00757                         }
00758                         if(itsMeterXwin.pressedCloseButton()) exit(0);
00759 
00760                         loc_forwardSpeed          = itsForwardSpeed;
00761                         loc_rotationalSpeed = itsRotationalSpeed;
00762                 }
00763                 itsSpeedMutex.unlock();
00764 
00765                 //Check on the sonar
00766                 itsSonarMutex.lock();
00767                 {
00768                         loc_sonarProxAlert = itsSonarProxAlert;
00769                         loc_sonarTime      = itsSonarTimer.getSecs();
00770                 }
00771                 itsSonarMutex.unlock();
00772 
00773                 //Update the display
00774                 itsControlImage = Image<PixRGB<byte> >(512,256,ZEROS);
00775                 writeText(itsControlImage, Point2D<int>(0,0),  "Commands:",                         PixRGB<byte>(255,255,255), PixRGB<byte>(0,0,0),SimpleFont::FIXED(6));
00776                 writeText(itsControlImage, Point2D<int>(0,8),  "w: Forwards",    PixRGB<byte>(180,255,255), PixRGB<byte>(0,0,0),SimpleFont::FIXED(6));
00777                 writeText(itsControlImage, Point2D<int>(0,16), "s: Backwards",   PixRGB<byte>(180,255,255), PixRGB<byte>(0,0,0),SimpleFont::FIXED(6));
00778                 writeText(itsControlImage, Point2D<int>(0,24), "a: Left",        PixRGB<byte>(180,255,255), PixRGB<byte>(0,0,0),SimpleFont::FIXED(6));
00779                 writeText(itsControlImage, Point2D<int>(0,32), "d: Right",       PixRGB<byte>(180,255,255), PixRGB<byte>(0,0,0),SimpleFont::FIXED(6));
00780                 writeText(itsControlImage, Point2D<int>(0,40), "space: STOP",    PixRGB<byte>(180,255,255), PixRGB<byte>(0,0,0),SimpleFont::FIXED(6));
00781                 writeText(itsControlImage, Point2D<int>(0,48), "esc: quit",      PixRGB<byte>(180,255,255), PixRGB<byte>(0,0,0),SimpleFont::FIXED(6));
00782 
00783                 char buffer[128];
00784                 sprintf(buffer, "Forward Speed: %2.2f%% Right: %1.3f m/s Left: %1.3f m/s", loc_forwardSpeed*100.0,itsRightMotorVelocity,itsLeftMotorVelocity);
00785                 writeText(itsControlImage, Point2D<int>(0,60), buffer, PixRGB<byte>(255,255,255), PixRGB<byte>(0,0,0),SimpleFont::FIXED(8));
00786 
00787                 sprintf(buffer, "Rotational Speed: %0.2f -> %0.2f", loc_rotationalSpeed,
00788                                 loc_rotationalSpeed *itsRotCap/100.0);
00789                 writeText(itsControlImage, Point2D<int>(0,75), buffer, PixRGB<byte>(255,255,255), PixRGB<byte>(0,0,0),SimpleFont::FIXED(8));
00790 
00791                 sprintf(buffer, "Right Encoder: %d", itsRightEncoder);
00792                 writeText(itsControlImage, Point2D<int>(0,90), buffer, PixRGB<byte>(255,255,255), PixRGB<byte>(0,0,0),SimpleFont::FIXED(6));
00793 
00794                 sprintf(buffer, "Left Encoder: %d", itsLeftEncoder);
00795                 writeText(itsControlImage, Point2D<int>(0,98), buffer, PixRGB<byte>(255,255,255), PixRGB<byte>(0,0,0),SimpleFont::FIXED(6));
00796 
00797                 sprintf(buffer, "Enc X: %fm",itsPosition.x );
00798                 writeText(itsControlImage, Point2D<int>(0,116), buffer, PixRGB<byte>(255,255,255), PixRGB<byte>(0,0,0),SimpleFont::FIXED(6));
00799 
00800                 sprintf(buffer, "Enc Y: %fm",itsPosition.y );
00801                 writeText(itsControlImage, Point2D<int>(0,124), buffer, PixRGB<byte>(255,255,255), PixRGB<byte>(0,0,0),SimpleFont::FIXED(6));
00802 
00803                 sprintf(buffer, "Enc R: %f",itsPosition.z );
00804                 writeText(itsControlImage, Point2D<int>(0,132), buffer, PixRGB<byte>(255,255,255), PixRGB<byte>(0,0,0),SimpleFont::FIXED(6));
00805 
00806                 if(itsLoadFromLogFile){
00807                         sprintf(buffer, "LEn X: %fm",itsLogPosition.x );
00808                         writeText(itsControlImage, Point2D<int>(0,140), buffer, PixRGB<byte>(255,255,255), PixRGB<byte>(0,0,0),SimpleFont::FIXED(6));
00809 
00810                         sprintf(buffer, "LEn Y: %fm",itsLogPosition.y );
00811                         writeText(itsControlImage, Point2D<int>(0,148), buffer, PixRGB<byte>(255,255,255), PixRGB<byte>(0,0,0),SimpleFont::FIXED(6));
00812 
00813                         sprintf(buffer, "LEn R: %f",itsLogPosition.z );
00814                         writeText(itsControlImage, Point2D<int>(0,156), buffer, PixRGB<byte>(255,255,255), PixRGB<byte>(0,0,0),SimpleFont::FIXED(6));
00815                 }
00816 
00817 
00818 
00819                 sprintf(buffer, "Sonar Timer: %0.2fs", loc_sonarTime);
00820                 writeText(itsControlImage, Point2D<int>(256,0), buffer, PixRGB<byte>(255,255,255), PixRGB<byte>(0,0,0),SimpleFont::FIXED(8));
00821 
00822                 PixRGB<byte> proxAlertColor;
00823                 if(loc_sonarProxAlert)
00824                 {
00825                         sprintf(buffer, "Proximity Alert!");
00826                         proxAlertColor = PixRGB<byte>(255,0,0);
00827                 }
00828                 else
00829                 {
00830                         sprintf(buffer, "Sonar OK");
00831                         proxAlertColor = PixRGB<byte>(20,128,20);
00832                 }
00833                 writeText(itsControlImage, Point2D<int>(256,15), buffer, proxAlertColor, PixRGB<byte>(0,0,0),SimpleFont::FIXED(8));
00834 
00835 
00836                 PixRGB<byte> serialOkColor;
00837                 int code = itsSerial->getSerialErrno();
00838                 if(code==0||code==15)
00839                 {
00840                         sprintf(buffer, "Serial OK");
00841                         serialOkColor = PixRGB<byte>(20,128,20);
00842                 }
00843                 else
00844                 {
00845                         sprintf(buffer, "Serial Disconnected!");
00846                         serialOkColor = PixRGB<byte>(255,0,0);
00847                 }
00848                 writeText(itsControlImage, Point2D<int>(256,30), buffer, serialOkColor, PixRGB<byte>(0,0,0),SimpleFont::FIXED(8));
00849 
00850 
00851                 for(size_t i=0; i<itsChannelMeters.size(); i++)
00852                 {
00853 
00854                         sprintf(buffer, "Channel%lu: %d", i+1, itsRCChannels[i]);
00855 
00856                         Point2D<int> pastePoint = Point2D<int>(
00857                                         int(itsChannelMeters[0].getWidth()*itsChannelMeters.size()*1.05+20),
00858                                         itsControlImage.getHeight() - itsChannelMeters[i].getHeight() - 10 + i*8
00859                                         );
00860 
00861                         writeText(itsControlImage, pastePoint, buffer, PixRGB<byte>(255,255,255), PixRGB<byte>(0,0,0),SimpleFont::FIXED(6));
00862 
00863                         pastePoint = Point2D<int>(
00864                                         int(10+itsChannelMeters[i].getWidth()*i*1.05),
00865                                         itsControlImage.getHeight() - itsChannelMeters[i].getHeight() - 10
00866                                         );
00867                         inplacePaste(itsControlImage, itsChannelMeters[i].render(itsRCChannels[i]), pastePoint );
00868                 }
00869 
00870                 sprintf(buffer, "Emergency Mode: %d", itsEmergencyMode);
00871                 writeText(itsControlImage, Point2D<int>(256,88), buffer, PixRGB<byte>(255,255,255), PixRGB<byte>(0,0,0),SimpleFont::FIXED(6));
00872 
00873                 sprintf(buffer, "RemoteMode: %d", itsRemoteMode);
00874                 writeText(itsControlImage, Point2D<int>(256,96), buffer, PixRGB<byte>(255,255,255), PixRGB<byte>(0,0,0),SimpleFont::FIXED(6));
00875 
00876                 if(itsLoadFromLogFile){
00877                         sprintf(buffer, "Log Mode: %d", itsLogRemoteMode);
00878                         writeText(itsControlImage, Point2D<int>(340,96), buffer, PixRGB<byte>(255,255,255), PixRGB<byte>(0,0,0),SimpleFont::FIXED(6));
00879                 }
00880                 sprintf(buffer, "Motor 1 Speed: %0.2f", itsMotor1Speed);
00881                 writeText(itsControlImage, Point2D<int>(256,104), buffer, PixRGB<byte>(255,255,255), PixRGB<byte>(0,0,0),SimpleFont::FIXED(6));
00882 
00883                 sprintf(buffer, "Motor 2 Speed: %0.2f", itsMotor2Speed);
00884                 writeText(itsControlImage, Point2D<int>(256,112), buffer, PixRGB<byte>(255,255,255), PixRGB<byte>(0,0,0),SimpleFont::FIXED(6));
00885 
00886                 sprintf(buffer, "RC Speed   : %0.2f -> %0.2f", itsRcTransSpeed,itsRcTransSpeedCapped);
00887                 writeText(itsControlImage, Point2D<int>(256,120), buffer, PixRGB<byte>(255,255,255), PixRGB<byte>(0,0,0),SimpleFont::FIXED(6));
00888 
00889                 sprintf(buffer, "RC Rotation: %0.2f -> %0.2f", itsRcRotSpeed,itsRcRotSpeedCapped);
00890                 writeText(itsControlImage, Point2D<int>(256,128), buffer, PixRGB<byte>(255,255,255), PixRGB<byte>(0,0,0),SimpleFont::FIXED(6));
00891 
00892 
00893 
00894                 //sprintf(buffer, "RCSpeed: %d", getRCSpeed());
00895                 //writeText(itsControlImage, Point2D<int>(256,104), buffer, PixRGB<byte>(255,255,255), PixRGB<byte>(0,0,0),SimpleFont::FIXED(6));
00896 
00897                 inplacePaste(itsDispImage,itsControlImage,Point2D<int>(0,0));
00898 
00899 
00900                 //Draw The Map
00901                 //float replayspeed = itsReplaySpeed.getVal();
00902                 int w = itsMapImage.getWidth();
00903                 int h = itsMapImage.getHeight();
00904                 double scaleRate = 0.9;
00905           Point2D<int> drawPos(
00906                                 int(itsPosition.x*itsMapScale + w/2),
00907                                  int(itsPosition.y*itsMapScale + h/2)
00908                                 );
00909                         if(itsMapImage.coordsOk(drawPos))
00910                         {
00911                                 itsPositionMap.push_back(itsPosition);
00912                         }else
00913                         {
00914                                 //Find New Scale
00915                                 do{
00916                                         itsMapScale *= scaleRate;
00917                                         drawPos = Point2D<int>(
00918                                                         int(itsPosition.x*itsMapScale + w/2),
00919                                                         int(itsPosition.y*itsMapScale + h/2)
00920                                                         );
00921                                 }while(!itsMapImage.coordsOk(drawPos));
00922                                 reDrawMap(itsMapScale);
00923                         }
00924                         itsMapImage.setVal(drawPos, itsPosition.color);
00925 
00926                 Image<PixRGB<byte> > itsDrawMapImage(itsMapImage);
00927                 drawCircle(itsDrawMapImage, drawPos, 7, PixRGB<byte>(255,255,255));
00928                 drawLine(itsDrawMapImage, drawPos,-itsPosition.z, 7, PixRGB<byte>(255,255,255));
00929 
00930                 if(itsLoadFromLogFile){
00931                         Point2D<int> drawPos2(
00932                                         int(itsLogPosition.x*itsMapScale + w/2),
00933                                         int(itsLogPosition.y*itsMapScale + h/2)
00934                                         );
00935                         if(!itsMapImage.coordsOk(drawPos2))
00936                         {
00937                                 //Find New Scale
00938                                 do{
00939                                         itsMapScale *= scaleRate;
00940                                         drawPos2 = Point2D<int>(
00941                                                         int(itsLogPosition.x*itsMapScale + w/2),
00942                                                         int(itsLogPosition.y*itsMapScale + h/2)
00943                                                         );
00944                                 }while(!itsMapImage.coordsOk(drawPos2));
00945                                 reDrawMap(itsMapScale);
00946                         }
00947                         //redraw it every time since we fastward the speed
00948                         if(itsReplaySpeed.getVal() != 1)
00949                                         reDrawMap(itsMapScale);
00950                         itsMapImage.setVal(drawPos2, itsLogPosition.color);
00951                         drawCircle(itsDrawMapImage, drawPos2, 7, PixRGB<byte>(0,255,0));
00952                         drawLine(itsDrawMapImage, drawPos2,-itsLogPosition.z, 7, PixRGB<byte>(255,255,255));
00953                 }
00954                 inplacePaste(itsDispImage,itsDrawMapImage,Point2D<int>(0,256));
00955 
00956                 std::vector<std::vector<float> > lines;
00957                 lines.push_back(itsVelocityQue.getVector());
00958                 lines.push_back(itsVelocityTargetQue.getVector());
00959 
00960                 std::vector<PixRGB<byte> > linesColor;
00961                 linesColor.push_back(PixRGB<byte>(255,0,0));
00962                 linesColor.push_back(PixRGB<byte>(255,165,0));
00963 
00964                 Image<PixRGB<byte> > plotImage = multilinePlot(
00965                                 lines,
00966                                 288+70,256,
00967                                 -1.0f,1.0f,
00968                                 "velocity","m/s","",
00969                                 linesColor,
00970                                 PixRGB<byte>(255,255,255),
00971                                 PixRGB<byte>(0,0,0)
00972 
00973                                 );
00974                 inplacePaste(itsDispImage,plotImage,Point2D<int>(512-70,0));
00975 
00976                 //Draw Divide Line
00977                 drawLine(itsDispImage,Point2D<int>(0,255),Point2D<int>(800,255),PixRGB<byte>(255,255,255));//HR
00978                 drawLine(itsDispImage,Point2D<int>(512-70,0),Point2D<int>(512-70,255),PixRGB<byte>(255,255,255));//VT
00979 
00980 
00981                 inplacePaste(itsDispImage,itsDispCameraImage,Point2D<int>(itsDispImage.getWidth()-320,itsDispImage.getHeight()-240));
00982 
00983                 itsMeterXwin.drawImage(itsDispImage);
00984 
00985         }
00986 }
00987 void BeoPilot::reDrawMap(double scale)
00988 {
00989 
00990         //Clean up the old images
00991         itsMapImage = Image<PixRGB<byte> >(800,344,ZEROS);
00992 
00993         int w = itsMapImage.getWidth();
00994         int h = itsMapImage.getHeight();
00995 
00996         //Redraw all points with new scale
00997         for(int i = 0;i < (int)itsLogPositionMap.size();i++)
00998         {
00999                 Point2D<int> tmp(
01000 
01001                                 int(itsLogPositionMap[i].x*scale + w/2),
01002                                 int(itsLogPositionMap[i].y*scale + h/2)
01003                                 );
01004                 if(itsMapImage.coordsOk(tmp))
01005                         itsMapImage.setVal(tmp, itsLogPositionMap[i].color);
01006         }
01007         for(int i = 0;i < (int)itsPositionMap.size();i++)
01008         {
01009                 Point2D<int> tmp(
01010 
01011                                 int(itsPositionMap[i].x*scale + w/2),
01012                                 int(itsPositionMap[i].y*scale + h/2)
01013                                 );
01014                 if(itsMapImage.coordsOk(tmp))
01015                         itsMapImage.setVal(tmp, itsPositionMap[i].color);
01016         }
01017 
01018 }
01019 // ######################################################################
01020 void BeoPilot::updateMessage(const RobotSimEvents::EventMessagePtr& eMsg,
01021     const Ice::Current&)
01022 {
01023         if(eMsg->ice_isA("::BeobotEvents::MotorRequest"))
01024         {
01025                 itsSpeedMutex.lock();
01026                 {
01027                         BeobotEvents::MotorRequestPtr mrMsg = BeobotEvents::MotorRequestPtr::dynamicCast(eMsg);
01028                         itsForwardSpeedReq    = mrMsg->transVel;
01029                         itsRotationalSpeedReq = mrMsg->rotVel;
01030                 }
01031                 itsSpeedMutex.unlock();
01032                 LINFO("Received Motor Request Speed:[%f] Rotation:[%f]",itsForwardSpeedReq,itsRotationalSpeedReq);
01033         }
01034         else if(eMsg->ice_isA("::BeobotEvents::SonarMessage"))
01035         {
01036                 itsSonarMutex.lock();
01037                 {
01038                         itsSonarTimer.reset();
01039                         BeobotEvents::SonarMessagePtr srMsg = BeobotEvents::SonarMessagePtr::dynamicCast(eMsg);
01040                         itsSonarReadings = srMsg->distances;
01041                         itsSonarAngles   = srMsg->angles;
01042                 }
01043                 itsSonarMutex.unlock();
01044         }
01045 }
01046 
01047 // ######################################################################
01048 void BeoPilot::getCurrentLogData()
01049 {
01050         uint64 time = itsLogTimer.get()/1000.0;
01051 if(itsReplaySpeed.getVal()==1)
01052 {
01053         do{
01054                 if(itsLogData[itsCurrentTimeIndex].time < time && itsCurrentTimeIndex < (int)itsLogData.size())
01055                         itsCurrentTimeIndex++;
01056         }while(itsLogData[itsCurrentTimeIndex].time < time && itsCurrentTimeIndex < (int)itsLogData.size());
01057 }
01058         if((int)itsLogImageData.size() != 0){
01059                 do{
01060                         if(itsLogImageData[itsCurrentTimeIndex2].time < time && itsCurrentTimeIndex2 < (int)itsLogImageData.size())
01061                                 itsCurrentTimeIndex2++;
01062                 }while(itsLogImageData[itsCurrentTimeIndex2].time < time && itsCurrentTimeIndex2 < (int)itsLogImageData.size());
01063                 imageData img = itsLogImageData[itsCurrentTimeIndex2];
01064                 if(file_exists(img.imagePath.c_str()))
01065                         itsDispCameraImage = Raster::ReadRGB(img.imagePath);
01066         LINFO("Found Index[%d] with Image[%f],Path %s",itsCurrentTimeIndex2,itsLogImageData[itsCurrentTimeIndex2].time,itsLogImageData[itsCurrentTimeIndex2].imagePath.c_str())        ;
01067         }
01068         LINFO("Current Time[%f] Found Index[%d] with LogTimeStamp[%f]",(double)time,itsCurrentTimeIndex,itsLogData[itsCurrentTimeIndex].time)        ;
01069 
01070 for(int i = 0;i < itsReplaySpeed.getVal();i++)
01071 {
01072         if(itsReplaySpeed.getVal() !=1 && itsCurrentTimeIndex < (int)itsLogData.size())
01073                         itsCurrentTimeIndex++;
01074 
01075         motorData tmp = itsLogData[itsCurrentTimeIndex];
01076 
01077 //        if(img.time != 0.0)
01078 
01079 
01080 
01081         //itsForwardSpeed = tmp.transVel;
01082         //itsRotationalSpeed = tmp.rotVel;
01083         itsForwardSpeed = tmp.rcTransVel;
01084         itsRotationalSpeed = tmp.rcRotVel;
01085 
01086         itsLogDiffPosition.x = tmp.encoderX;
01087         itsLogDiffPosition.y = tmp.encoderY;
01088         itsLogDiffPosition.z = tmp.encoderOri;
01089 
01090         itsLogPosition.x += itsLogDiffPosition.x;
01091         itsLogPosition.y += itsLogDiffPosition.y;
01092         itsLogPosition.z += itsLogDiffPosition.z;
01093 
01094         itsRcTransSpeed = tmp.rcTransVel;
01095         itsRcRotSpeed = tmp.rcRotVel  ;
01096         itsLogRemoteMode = tmp.rcMode  ;
01097 
01098         itsMotor1Speed = itsForwardSpeed - itsRotationalSpeed;
01099         itsMotor2Speed = itsForwardSpeed + itsRotationalSpeed;
01100         if(itsLogRemoteMode == 1)
01101                 itsLogPosition.color = PixRGB<byte>(255,0,0);
01102         else
01103                 itsLogPosition.color = PixRGB<byte>(0,255,0);
01104 
01105         if(itsCurrentTimeIndex <(int)itsLogData.size())
01106                 itsLogPositionMap.push_back(itsLogPosition);
01107 }
01108 }
01109 void BeoPilot::drawAllMap(void)
01110 {
01111         for(int i = 0;i < (int)itsLogData.size();i++){
01112                 motorData tmp = itsLogData[i];
01113                 itsLogDiffPosition.x = tmp.encoderX;
01114                 itsLogDiffPosition.y = tmp.encoderY;
01115                 itsLogDiffPosition.z = tmp.encoderOri;
01116 
01117                 itsLogPosition.x += itsLogDiffPosition.x;
01118                 itsLogPosition.y += itsLogDiffPosition.y;
01119                 itsLogPosition.z += itsLogDiffPosition.z;
01120                 itsLogPositionMap.push_back(itsLogPosition);
01121         }
01122 }
Generated on Sun May 8 08:41:17 2011 for iLab Neuromorphic Vision Toolkit by  doxygen 1.6.3