seaBeeJoyStick-master.C

00001 /*!@file src/SeaBee/seaBeeJoyStick-masater.C Joystick interface to SeaBee */
00002 
00003 // //////////////////////////////////////////////////////////////////// //
00004 // The iLab Neuromorphic Vision C++ Toolkit - Copyright (C) 2000-2002   //
00005 // by the 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: Michael Montalbo <montalbo@usc.edu>
00034 // $HeadURL: svn://isvn.usc.edu/software/invt/trunk/saliency/src/SeaBee/seaBeeJoyStick-master.C $
00035 // $Id:
00036 //
00037 
00038 #include "Beowulf/Beowulf.H"
00039 #include "Component/ModelManager.H"
00040 #include "Devices/JoyStick.H"
00041 #include "Util/Types.H"
00042 #include "Util/log.H"
00043 #include "Raster/Raster.H"
00044 #include "Media/FrameSeries.H"
00045 
00046 #include "GUI/XWindow.H"
00047 #include "Devices/DeviceOpts.H"
00048 #include "Media/FrameSeries.H"
00049 #include "Media/MediaOpts.H"
00050 #include "Image/ShapeOps.H"
00051 #include "SeaBee/SubGUI.H"
00052 
00053 #include <unistd.h>
00054 #include <signal.h>
00055 
00056 #ifdef HAVE_LINUX_JOYSTICK_H
00057 
00058 #define COM_B_NODE 0
00059 #define INIT_COMM 10000
00060 #define JS_AXIS_UPDATE   20000
00061 #define SUB_GUI_UPDATE   20001
00062 #define ABORT 90000
00063 
00064 #define X_MIN -32767
00065 #define X_MAX 32767
00066 #define Y_MIN -32767
00067 #define Y_MAX 32767
00068 #define D_MIN -32767
00069 #define D_MAX 32767
00070 
00071 volatile bool keepGoing = false;
00072 std::list<TCPmessage> outMessages;
00073 
00074 int* intPressurePtr = 0;
00075 int* headingPtr = 0;
00076 int* pitchPtr = 0;
00077 int* rollPtr = 0;
00078 int* depthPtr = 0;
00079 int* thruster_Up_Left_Ptr = 0;
00080 int* thruster_Up_Right_Ptr = 0;
00081 int* thruster_Up_Back_Ptr = 0;
00082 int* thruster_Fwd_Left_Ptr = 0;
00083 int* thruster_Fwd_Right_Ptr = 0;
00084 
00085 Image<PixRGB<byte> >* currentImgPtr;
00086 
00087 //! A simple joystick listener
00088 class TestJoyStickListener : public JoyStickListener
00089 {
00090 public:
00091   virtual ~TestJoyStickListener() { }
00092 
00093   virtual void axis(const uint num, const int16 val)
00094   {
00095     if(keepGoing)
00096       {
00097         TCPmessage smsg;     // buffer to send messages
00098 
00099         int32 sframe  = 0;
00100         int32 saction = JS_AXIS_UPDATE;
00101 
00102         smsg.reset(sframe, saction);
00103         smsg.addInt32(int32(num));
00104 
00105         //p is the percentage of thrust being applied [-100.0...100.0] for the axis
00106         float p = 0.0;
00107         float v = 0.0;
00108         v = (float)val;
00109 
00110         switch(num)
00111           {
00112           case 0:
00113             p =  (float)(v/X_MAX*100.0);
00114             LINFO("X-Axis = %f", p);
00115             break;
00116           case 1:
00117             p =  v/Y_MAX*100.0;
00118             LINFO("Y-Axis = %f", p);
00119             break;
00120           case 2:
00121             p =  v/D_MAX*100.0;
00122             LINFO("Depth = %f", p);
00123             break;
00124           default:
00125             LERROR("Unknown axis event recieved");
00126           }
00127 
00128         smsg.addFloat(p);
00129 
00130         outMessages.push_back(smsg);
00131 
00132       }
00133   }
00134 
00135   virtual void button(const uint num, const bool state)
00136   {
00137     LINFO("Button[%d] = %s", num, state?"true":"false");
00138   }
00139 };
00140 
00141 #endif
00142 
00143 // ######################################################################
00144 //! Signal handler (e.g., for control-C)
00145 void terminate(int s)
00146 { LERROR("*** INTERRUPT ***"); keepGoing = false; }
00147 
00148 
00149 // gets the messages from COM_B and relay it to the agent manager
00150 void checkInMessages
00151 ( nub::soft_ref<Beowulf> beo );
00152 
00153 // ######################################################################
00154 //! Test JoyStick code
00155 /*! Test Joystick code. */
00156 int main(const int argc, const char **argv)
00157 {
00158 #ifndef HAVE_LINUX_JOYSTICK_H
00159 
00160   LFATAL("<linux/joystick.h> must be installed to use this program");
00161 
00162 #else
00163 
00164   // get a manager going:
00165   ModelManager manager("JoyStick Manager");
00166 
00167   // instantiate our model components:
00168   nub::soft_ref<JoyStick> js(new JoyStick(manager) );
00169   manager.addSubComponent(js);
00170 
00171   nub::soft_ref<Beowulf>
00172     beo(new Beowulf(manager, "Beowulf Master", "BeowulfMaster", true));
00173   manager.addSubComponent(beo);
00174 
00175   nub::soft_ref<OutputFrameSeries> ofs(new OutputFrameSeries(manager));
00176   manager.addSubComponent(ofs);
00177 
00178   nub::soft_ref<SubGUI> subGUI(new SubGUI(manager));
00179   manager.addSubComponent(subGUI);
00180 
00181   manager.exportOptions(MC_RECURSE);
00182 
00183   // Parse command-line:
00184   if (manager.parseCommandLine(argc, argv, "[device]", 0, 1) == false)
00185     return(1);
00186 
00187   // let's configure our device:
00188   if (manager.numExtraArgs() > 0)
00189     js->setModelParamVal("JoyStickDevName",
00190                          manager.getExtraArg(0), MC_RECURSE);
00191 
00192   // register a listener:
00193   rutz::shared_ptr<TestJoyStickListener> lis(new TestJoyStickListener);
00194   rutz::shared_ptr<JoyStickListener> lis2; lis2.dynCastFrom(lis); // cast down
00195   js->setListener(lis2);
00196 
00197   TCPmessage rmsg;     // buffer to receive messages
00198   TCPmessage smsg;     // buffer to send messages
00199   int32 rframe = 0, raction = 0, rnode = 0;
00200 
00201 
00202   // catch signals and redirect them to terminate for clean exit:
00203   signal(SIGHUP, terminate); signal(SIGINT, terminate);
00204   signal(SIGQUIT, terminate); signal(SIGTERM, terminate);
00205   signal(SIGALRM, terminate);
00206 
00207   // get started:
00208   manager.start();
00209 
00210   intPressurePtr = new int;
00211   headingPtr = new int;
00212   pitchPtr = new int;
00213   rollPtr = new int;
00214   depthPtr = new int;
00215   thruster_Up_Left_Ptr = new int;
00216   thruster_Up_Right_Ptr = new int;
00217   thruster_Up_Back_Ptr = new int;
00218   thruster_Fwd_Left_Ptr = new int;
00219   thruster_Fwd_Right_Ptr = new int;
00220   currentImgPtr = new Image<PixRGB<byte> >();
00221 
00222   //start the gui thread
00223   subGUI->startThread(ofs);
00224   sleep(1);
00225   //setup gui for various objects
00226   //subGUI->setupGUI(subController.get(), true);
00227 
00228   //Main GUI Window
00229   subGUI->addMeter(intPressurePtr,
00230                    "Int Pressure", 500, PixRGB<byte>(255, 0, 0));
00231   subGUI->addMeter(headingPtr  ,
00232                    "Heading", 360, PixRGB<byte>(192, 255, 0));
00233   subGUI->addMeter(pitchPtr,
00234                    "Pitch", 256, PixRGB<byte>(192, 255, 0));
00235   subGUI->addMeter(rollPtr,
00236                    "Roll", 256, PixRGB<byte>(192, 255, 0));
00237   subGUI->addMeter(depthPtr,
00238                    "Depth", 300, PixRGB<byte>(192, 255, 0));
00239 
00240   subGUI->addMeter(thruster_Up_Left_Ptr,
00241                    "Motor_Up_Left", -100, PixRGB<byte>(0, 255, 0));
00242   subGUI->addMeter(thruster_Up_Right_Ptr,
00243                    "Motor_Up_Right", -100, PixRGB<byte>(0, 255, 0));
00244   subGUI->addMeter(thruster_Up_Back_Ptr,
00245                    "Motor_Up_Back", -100, PixRGB<byte>(0, 255, 0));
00246   subGUI->addMeter(thruster_Fwd_Left_Ptr,
00247                    "Motor_Fwd_Left", -100, PixRGB<byte>(0, 255, 0));
00248   subGUI->addMeter(thruster_Fwd_Right_Ptr,
00249                    "Motor_Fwd_Right", -100, PixRGB<byte>(0, 255, 0));
00250 
00251   subGUI->addImage(currentImgPtr);
00252 
00253   //  subGUI->update();
00254   // send params to dorsal and ventral node to initialize contact:
00255   smsg.reset(0, INIT_COMM);
00256   smsg.addInt32(int32(235));
00257 
00258   // send the same initial values message to CPU_B
00259   beo->send(COM_B_NODE, smsg);
00260 
00261   // SYNCHRONIZATION: wait until the other board is ready
00262   LINFO("waiting until COM_B is ready to go");
00263   rnode = COM_B_NODE;
00264   while(!beo->receive(rnode, rmsg, rframe, raction, 5));
00265   rmsg.reset(rframe, raction);
00266   LINFO("%d is ready", rnode);
00267   Raster::waitForKey();
00268 
00269   keepGoing = true;
00270   while(keepGoing)
00271     {
00272       if(outMessages.size() > 0)
00273         {
00274           beo->send(COM_B_NODE, outMessages.front());
00275           outMessages.pop_front();
00276         }
00277 
00278       checkInMessages(beo);
00279     }
00280 
00281   // send abort to COM_B
00282   smsg.reset(0, ABORT);
00283   beo->send(COM_B_NODE, smsg);
00284 
00285   // stop everything and exit:
00286   manager.stop();
00287   return 0;
00288 
00289 #endif // HAVE_LINUX_JOYSTICK_H
00290 
00291 }
00292 
00293 // ######################################################################
00294 void checkInMessages
00295 ( nub::soft_ref<Beowulf> beo)
00296 {
00297   int32 rframe = 0, raction = 0, rnode = 0;
00298   TCPmessage rmsg;     // buffer to receive messages
00299 
00300   // get all the messages sent in
00301   while(beo->receive(rnode, rmsg, rframe, raction, 5))
00302     {
00303       LINFO("COM_A: recieving a message: %d", raction);
00304 
00305       // check the purpose of the message
00306       switch(raction)
00307         {
00308 
00309         case SUB_GUI_UPDATE:
00310           {
00311 
00312             *(intPressurePtr) = rmsg.getElementInt32();
00313             *(headingPtr) = rmsg.getElementInt32();
00314             *(pitchPtr) = rmsg.getElementInt32();
00315             *(rollPtr) = rmsg.getElementInt32();
00316             *(depthPtr) = rmsg.getElementInt32();
00317             *(thruster_Up_Left_Ptr) = rmsg.getElementInt32();
00318             *(thruster_Up_Right_Ptr) = rmsg.getElementInt32();
00319             *(thruster_Up_Back_Ptr) = rmsg.getElementInt32();
00320             *(thruster_Fwd_Left_Ptr) = rmsg.getElementInt32();
00321             *(thruster_Fwd_Right_Ptr) = rmsg.getElementInt32();
00322 
00323             *currentImgPtr = rmsg.getElementColByteIma();
00324 
00325             break;
00326           }
00327         default:
00328           LFATAL("Unknown purpose");
00329         }
00330     }
00331 }
00332 
00333 // ######################################################################
00334 /* So things look consistent in everyone's emacs... */
00335 /* Local Variables: */
00336 /* indent-tabs-mode: nil */
00337 /* End: */
Generated on Sun May 8 08:42:14 2011 for iLab Neuromorphic Vision Toolkit by  doxygen 1.6.3