test-SeaBeeInterface.C

00001 /*!@file Demo/test-tracking.C Test tracking  */
00002 
00003 // //////////////////////////////////////////////////////////////////// //
00004 // The iLab Neuromorphic Vision C++ Toolkit - Copyright (C) 2001 by the //
00005 // University of Southern California (USC) and the iLab at USC.         //
00006 // See http://iLab.usc.edu for information about this project.          //
00007 // //////////////////////////////////////////////////////////////////// //
00008 // Major portions of the iLab Neuromorphic Vision Toolkit are protected //
00009 // under the U.S. patent ``Computation of Intrinsic Perceptual Saliency //
00010 // in Visual Environments, and Applications'' by Christof Koch and      //
00011 // Laurent Itti, California Institute of Technology, 2001 (patent       //
00012 // pending; application number 09/912,225 filed July 23, 2001; see      //
00013 // http://pair.uspto.gov/cgi-bin/final/home.pl for current status).     //
00014 // //////////////////////////////////////////////////////////////////// //
00015 // This file is part of the iLab Neuromorphic Vision C++ Toolkit.       //
00016 //                                                                      //
00017 // The iLab Neuromorphic Vision C++ Toolkit is free software; you can   //
00018 // redistribute it and/or modify it under the terms of the GNU General  //
00019 // Public License as published by the Free Software Foundation; either  //
00020 // version 2 of the License, or (at your option) any later version.     //
00021 //                                                                      //
00022 // The iLab Neuromorphic Vision C++ Toolkit is distributed in the hope  //
00023 // that it will be useful, but WITHOUT ANY WARRANTY; without even the   //
00024 // implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR      //
00025 // PURPOSE.  See the GNU General Public License for more details.       //
00026 //                                                                      //
00027 // You should have received a copy of the GNU General Public License    //
00028 // along with the iLab Neuromorphic Vision C++ Toolkit; if not, write   //
00029 // to the Free Software Foundation, Inc., 59 Temple Place, Suite 330,   //
00030 // Boston, MA 02111-1307 USA.                                           //
00031 // //////////////////////////////////////////////////////////////////// //
00032 //
00033 // Primary maintainer for this file: Lior Elazary <elazary@usc.edu>
00034 // $HeadURL: svn://isvn.usc.edu/software/invt/trunk/saliency/src/SeaBee/test-SeaBeeInterface.C $
00035 // $Id: test-SeaBeeInterface.C 14376 2011-01-11 02:44:34Z pez $
00036 //
00037 
00038 #include "Image/OpenCVUtil.H"  // must be first to avoid conflicting defs of int64, uint64
00039 
00040 #include "Component/ModelManager.H"
00041 #include "Devices/DeviceOpts.H"
00042 #include "GUI/XWinManaged.H"
00043 #include "Raster/GenericFrame.H"
00044 #include "Raster/Raster.H"
00045 #include "Component/ModelManager.H"
00046 #include "Devices/DeviceOpts.H"
00047 #include "GUI/XWindow.H"
00048 #include "Image/DrawOps.H"
00049 #include "Image/CutPaste.H"
00050 #include "Image/Image.H"
00051 #include "Image/Pixels.H"
00052 #include "Image/MathOps.H"
00053 #include "Neuro/EnvVisualCortex.H"
00054 #include "Media/FrameSeries.H"
00055 #include "Media/MediaOpts.H"
00056 #include "Transport/FrameInfo.H"
00057 #include "Raster/GenericFrame.H"
00058 #include "Raster/Raster.H"
00059 #include "Util/Timer.H"
00060 #include "Util/log.H"
00061 #include "Util/MathFunctions.H"
00062 #include "Learn/Bayes.H"
00063 #include "Learn/BackpropNetwork.H"
00064 #include "Envision/env_image_ops.h"
00065 #include "Neuro/BeoHeadBrain.H"
00066 #include "Image/ShapeOps.H"
00067 #include "Devices/BeeSTEM.H"
00068 #include "rutz/shared_ptr.h"
00069 
00070 
00071 #include <ctype.h>
00072 #include <deque>
00073 #include <iterator>
00074 #include <stdlib.h> // for atoi(), malloc(), free()
00075 #include <string>
00076 #include <vector>
00077 #include <map>
00078 
00079 #define UP 98
00080 #define DOWN 104
00081 #define RIGHT 102
00082 #define LEFT 100
00083 
00084 
00085 #define THRUSTER_UP_LEFT 1
00086 #define THRUSTER_UP_RIGHT 3
00087 #define THRUSTER_UP_BACK 2
00088 #define THRUSTER_FWD_RIGHT 4
00089 #define THRUSTER_FWD_LEFT 0
00090 
00091 //ModelManager *mgr;
00092 XWinManaged *xwin;
00093 Timer timer;
00094 Image<PixRGB<byte> > disp;
00095 byte SmaxVal = 0;
00096 int smap_level = -1;
00097 
00098 bool debug = 0;
00099 bool init_points = true;
00100 
00101 
00102 //! Our own little BeeSTEMListener
00103 class BeeSTEM_PID_Listener : public BeeSTEMListener
00104 {
00105         public:
00106                 BeeSTEM_PID_Listener(nub::soft_ref<BeeSTEM> &b) :
00107                         itsBeeStem(b)
00108         {
00109                 pidHeading.reset(new PID<float>(0.1, 0, 0, -100.0, 100.0));
00110                 pidPitch.reset(new PID<float>(0.1, 0, 0, -100.0, 100.0));
00111                 pidRoll.reset(new PID<float>(0.1, 0, 0, -100.0, 100.0));
00112 
00113                 thrust_h_left = 0;
00114                 thrust_h_right = 0;
00115                 thrust_v_left = 0;
00116                 thrust_v_right = 0;
00117                 thrust_v_back = 0;
00118 
00119                 target_heading = 0;
00120         }
00121 
00122                 virtual ~BeeSTEM_PID_Listener()
00123                 { }
00124 
00125                 //These set and turn off the PID values.
00126                 void setTargetHeading(int _h) {
00127                         target_heading = _h;
00128                         heading_enable = true;
00129 
00130                         //Now turn off all the motors so we can restart the PID
00131                         thrust_h_left = 0;
00132                         thrust_h_right = 0;
00133                         thrust_v_left = 0;
00134                         thrust_v_right = 0;
00135                         thrust_v_back = 0;
00136                 }
00137 
00138                 void setTargetPitch(int _h) {
00139                         target_pitch = _h;
00140                         pitch_enable = true;
00141 
00142                         //Now turn off all the motors so we can restart the PID
00143                         thrust_h_left = 0;
00144                         thrust_h_right = 0;
00145                         thrust_v_left = 0;
00146                         thrust_v_right = 0;
00147                         thrust_v_back = 0;
00148                 }
00149 
00150                 void disableHeadingTarget() {
00151                         heading_enable = false;
00152                 }
00153 
00154                 void disablePitchTarget() {
00155                         pitch_enable = false;
00156                 }
00157 
00158                 void setPrintOnly(bool b) {
00159                         print_only = b;
00160                 }
00161 
00162                 int getPos()
00163                 {
00164                         return itsCurrentPos;
00165 
00166                 }
00167 
00168 
00169 
00170                 virtual void event(const BeeSTEMEventType t, const unsigned char dat1,
00171                                 const unsigned char dat2)
00172                 {
00173                         LDEBUG("Event: %d dat1 = %d, dat2 = %d", int(t), dat1, dat2);
00174 
00175                         switch(t) {
00176 
00177                                 case COMPASS_HEADING_EVENT:
00178                                         {
00179                                                 int heading = (unsigned int)dat1*2;
00180                                                 itsCurrentPos = heading;
00181                                                 if (pidHeading.is_valid())
00182                                                 {
00183                                                         float rot = 100*pidHeading->update(target_heading, (float)heading);
00184                                                         thrust_h_left+=(int)rot;
00185                                                         thrust_h_right-=(int)rot;
00186                                                         //LINFO("%f %i %i %i", rot, heading, thrust_h_left, thrust_h_right);
00187                                                         if(!print_only) {
00188                                                                 //itsBeeStem->setThrust(THRUSTER_FWD_RIGHT, thrust_h_right);
00189                                                                 //itsBeeStem->setThrust(THRUSTER_FWD_LEFT, thrust_h_left);
00190                                                         }
00191                                                 }
00192 
00193                                         }
00194                                         break;  //Careful here! The heading is sent /2 because of byte limits
00195                                 case COMPASS_PITCH_EVENT:
00196                                         {
00197                                                 if(pitch_enable)
00198                                                 {
00199                                                         int pitch = (signed char)dat1;
00200                                                         if (pidPitch.is_valid())
00201                                                         {
00202                                                                 float pitchCorr = 100*pidPitch->update(target_pitch, (float)pitch);
00203                                                                 thrust_v_left += (int)pitchCorr;
00204                                                                 thrust_v_right -= (int)pitchCorr;
00205                                                                 itsBeeStem->setMotor(THRUSTER_UP_RIGHT, (int)thrust_v_right);
00206                                                                 itsBeeStem->setMotor(THRUSTER_UP_LEFT, (int)thrust_v_left);
00207                                                         }
00208                                                 }
00209                                         }
00210                                         break;
00211                         case COMPASS_ROLL_EVENT:
00212                         {
00213                                 int roll = (signed char)dat1;
00214                                 roll++;
00215                         }
00216                         break;
00217                         case ACCEL_X_EVENT:           break;
00218                         case ACCEL_Y_EVENT:           break;
00219                         case INT_PRESS_EVENT:        /* LINFO("INTERNAL PRESSURE: %d", (int)dat1); */break;
00220                         case EXT_PRESS_EVENT:
00221                                                                                                                                                                                                                                                                                                                                          //        LINFO("%i", (unsigned char)dat1);
00222                                                                                                                                                                                                                                                                                                                                          break;
00223                         case TEMP1_EVENT:             break;
00224                         case TEMP2_EVENT:             break;
00225                         case DIG_IN_EVENT:            break;
00226                         case ADC_IN_EVENT:            break;
00227                         case MOTOR_A_CURR_EVENT:      break;
00228                         case MOTOR_B_CURR_EVENT:      break;
00229                         case MOTOR_C_CURR_EVENT:      break;
00230                         case MOTOR_D_CURR_EVENT:      break;
00231                         case MOTOR_E_CURR_EVENT:      break;
00232                         case ECHO_REPLY_EVENT:        LINFO("BeeSTEM Echo Reply Recieved."); break;
00233                         case RESET_EVENT:             LERROR("BeeSTEM RESET occurred!"); break;
00234                         case SW_OVERFLOW_EVENT:       LERROR("BeeSTEM Software Overflow!"); break;
00235                         case FRAMING_ERR_EVENT:       LERROR("BeeSTEM Framing Error!"); break;
00236                         case OVR_ERR_EVENT:           LERROR("BeeSTEM Hardware Overflow!"); break;
00237                         case HMR3300_LOST_EVENT:      break;
00238                         case ACCEL_LOST_EVENT:        break;
00239                         case TEMP1_LOST_EVENT:        break;
00240                         case TEMP2_LOST_EVENT:        break;
00241                                                                                                                                                 //case TEMP3_LOST_EVENT:        break;
00242                         case ESTOP_EVENT:             break;
00243                         case UNRECOGNIZED_EVENT:      break;
00244                         case BAD_OUT_CMD_SEQ_EVENT:   LERROR("BeeSTEM Reports a Bad Command Sequence!"); break;
00245                         case BAD_IN_CMD_SEQ_EVENT:    break;
00246                         case RESET_ACK_EVENT:         LINFO("BeeSTEM Acknowledges Reset Request"); break;
00247                         case  NO_EVENT:               break;
00248                         default:                      LERROR("Unknown event %d received!", int(t)); break;
00249 
00250                 }
00251 
00252 
00253 
00254 }
00255 private:
00256 nub::soft_ref<BeeSTEM> itsBeeStem;
00257 int heading;
00258 bool heading_enable, print_only;
00259 bool pitch_enable;
00260 int target_pitch;
00261 
00262 rutz::shared_ptr< PID<float> > pidHeading;
00263 rutz::shared_ptr< PID<float> > pidPitch;
00264 rutz::shared_ptr< PID<float> > pidRoll;
00265 rutz::shared_ptr< PID<float> > pidDepth;
00266 
00267 signed int thrust_h_left, thrust_h_right, thrust_v_left, thrust_v_right, thrust_v_back;
00268 
00269 int target_heading;
00270 int itsCurrentPos;
00271 };
00272 
00273 
00274 void display(Image<PixRGB<byte> > &img);
00275 
00276 int main(int argc, const char **argv)
00277 {
00278   // Instantiate a ModelManager:
00279   ModelManager *mgr = new ModelManager("SeaBee Interface");
00280 
00281   nub::ref<InputFrameSeries> ifs(new InputFrameSeries(*mgr));
00282   mgr->addSubComponent(ifs);
00283 
00284   nub::ref<OutputFrameSeries> ofs(new OutputFrameSeries(*mgr));
00285   mgr->addSubComponent(ofs);
00286 
00287   nub::soft_ref<BeeSTEM> b(new BeeSTEM(*mgr,"BeeSTEM", "BeeSTEM", "/dev/ttyS1"));
00288   mgr->addSubComponent(b);
00289 
00290   //Make the PID Listener
00291   //TODO: fixme: obsolete with the new BeeSTEM
00292  // rutz::shared_ptr<BeeSTEM_PID_Listener> lis(new BeeSTEM_PID_Listener(b));
00293  // rutz::shared_ptr<BeeSTEMListener> lis2; lis2.dynCastFrom(lis); // cast down
00294  // b->setListener(lis2);
00295 
00296   mgr->exportOptions(MC_RECURSE);
00297 
00298   mgr->setOptionValString(&OPT_InputFrameSource, "V4L2");
00299   mgr->setOptionValString(&OPT_FrameGrabberMode, "YUYV");
00300   mgr->setOptionValString(&OPT_FrameGrabberDims, "1024x576");
00301  // mgr->setOptionValString(&OPT_FrameGrabberBswap, "no");
00302   mgr->setOptionValString(&OPT_FrameGrabberFPS, "30");
00303 
00304   // Parse command-line:
00305   if (mgr->parseCommandLine(argc, argv, "", 0, 0) == false) return(1);
00306 
00307   // do post-command-line configs:
00308   Dims imageDims(320,240);
00309 
00310   xwin = new XWinManaged(Dims(imageDims.w()*2,imageDims.h()*2+20),
00311       -1, -1, "SeaBee interface");
00312   disp = Image<PixRGB<byte> >(imageDims.w(),imageDims.h()+20, ZEROS);
00313 
00314   // let's get all our ModelComponent instances started:
00315   mgr->start();
00316 
00317   //start streaming
00318   ifs->startStream();
00319 
00320   timer.reset();
00321 
00322 
00323   while(1) {
00324 
00325     const FrameState is = ifs->updateNext();
00326     if (is == FRAME_COMPLETE)
00327       break;
00328 
00329     //grab the images
00330     GenericFrame input = ifs->readFrame();
00331     if (!input.initialized())
00332       break;
00333     Image<PixRGB<byte> > frontImg = rescale(input.asRgb(), 320, 240);
00334 
00335     ofs->writeRGB(frontImg, "input", FrameInfo("Copy of input", SRC_POS));
00336 
00337 
00338     int key = xwin->getLastKeyPress();
00339 
00340     int speed = 100;
00341     switch(key)
00342     {
00343             case 38: //a
00344                     b->setMotor(2,-1 * speed);
00345                     b->setMotor(1,-1 * speed);
00346                     b->setMotor(3,-1 * speed);
00347                     break;
00348             case 52: //z
00349                     b->setMotor(2,speed );
00350                     b->setMotor(1,speed );
00351                     b->setMotor(3,speed );
00352                     break;
00353             case LEFT:
00354                     b->setMotor(0, speed);
00355                     b->setMotor(4, -1 * speed);
00356                     break;
00357             case RIGHT:
00358                     b->setMotor(0, -1 * speed);
00359                     b->setMotor(4, speed);
00360                     break;
00361             case UP:
00362                     b->setMotor(0, -1 * speed);
00363                     b->setMotor(4, -1 * speed);
00364                     break;
00365             case DOWN:
00366                     b->setMotor(0, speed);
00367                     b->setMotor(4, speed);
00368                     break;
00369             case 65:  //space
00370                     b->setMotor(0,0);
00371                     usleep(1000);
00372                     b->setMotor(1,0);
00373                     usleep(1000);
00374                     b->setMotor(2,0);
00375                     usleep(1000);
00376                     b->setMotor(3,0);
00377                     usleep(1000);
00378                     b->setMotor(4,0);
00379                     usleep(1000);
00380                     break;
00381 
00382             default:
00383                     break;
00384     }
00385     display(frontImg);
00386 
00387   }
00388 
00389 
00390   // stop all our ModelComponents
00391   mgr->stop();
00392 
00393   // all done!
00394   return 0;
00395 }
00396 
00397 void display(Image<PixRGB<byte> > &img)
00398 {
00399   static int avgn = 0;
00400   static uint64 avgtime = 0;
00401   //char msg[255];
00402 
00403   //Left Image
00404   xwin->drawImage(img, 0, 0);
00405 
00406   //calculate fps
00407   avgn++;
00408   avgtime += timer.getReset();
00409   if (avgn == 20)
00410   {
00411     avgtime = 0;
00412     avgn = 0;
00413   }
00414 
00415 
00416 //  Image<PixRGB<byte> > infoImg(leftImg.getWidth()*2, 20, NO_INIT);
00417 //  writeText(infoImg, Point2D<int>(0,0), msg,
00418 //        PixRGB<byte>(255), PixRGB<byte>(127));
00419 //  //xwin->drawImage(infoImg, 0, leftImg.getHeight()*2);
00420 
00421 }
00422 
Generated on Sun May 8 08:42:15 2011 for iLab Neuromorphic Vision Toolkit by  doxygen 1.6.3