Homepage Demos Overview Downloads Tutorials Reference
Credits

Aibo3DControllerBehavior.h

Go to the documentation of this file.
00001 //-*-c++-*-
00002 #ifndef INCLUDED_Aibo3DControllerBehavior_h_
00003 #define INCLUDED_Aibo3DControllerBehavior_h_
00004 
00005 #include <iostream>
00006 #include "Wireless/Wireless.h"
00007 #include "Behaviors/BehaviorBase.h"
00008 #include "Motion/MotionManager.h"
00009 #include "Motion/RemoteControllerMC.h"
00010 #include "Events/EventRouter.h"
00011 #include "Events/EventBase.h"
00012 #include "Shared/RobotInfo.h"
00013 #include "Behaviors/Controller.h"
00014 #include "Shared/WorldState.h"
00015 
00016 //! gets input from the GUI
00017 int aibo3dcontrollercmd_callback(char *buf, int bytes);
00018 class Aibo3DControllerBehavior;
00019 
00020 //! so aibo3dcontrollercmd_callback knows where to send the input from the GUI
00021 Aibo3DControllerBehavior *aibo3dControllerBehavior = NULL;
00022 
00023   
00024 //! Listens to aibo3d control commands coming in from the command port.
00025 class Aibo3DControllerBehavior : public BehaviorBase {
00026  protected:
00027   MotionManager::MC_ID rcontrol_id; //!< remote controller motion command's id
00028  
00029   //! The input command stream socket
00030   Socket *cmdsock;
00031 
00032   float val[NumPIDJoints]; //!< the value to use for each of the PID joints
00033   char *fbuf;  //!< alias to val
00034   unsigned int pos; //!< a counter to know when we've gotten 4 frames
00035 
00036  private:
00037   Aibo3DControllerBehavior(const Aibo3DControllerBehavior&); //!< don't call
00038   Aibo3DControllerBehavior operator=(const Aibo3DControllerBehavior&); //!< don't call
00039 
00040  public:
00041   //! constructor
00042   Aibo3DControllerBehavior() :
00043     BehaviorBase("Aibo3DControllerBehavior"),
00044     rcontrol_id(MotionManager::invalid_MC_ID),
00045     cmdsock(NULL),
00046     fbuf((char*)val), pos(0)
00047   { aibo3dControllerBehavior = this; }
00048   //! destructor
00049   virtual ~Aibo3DControllerBehavior() { aibo3dControllerBehavior = NULL; }
00050 
00051   //! processes input from the GUI
00052   int registerData(char *buf, int bytes) {
00053     int read=0;
00054     while (read<bytes) {
00055       fbuf[pos]=buf[read];
00056       pos++;
00057       read++;
00058       if (pos==NumPIDJoints*sizeof(float)) {
00059         updateRC();
00060         pos=0;
00061       }
00062     }
00063     return read;
00064   }
00065 
00066   //! sends the new joint commands to the motion command
00067   void updateRC() {
00068     RemoteControllerMC *rcontrol = (RemoteControllerMC*)motman->checkoutMotion(rcontrol_id);
00069     for (unsigned int i=0; i<NumPIDJoints; i++)
00070       rcontrol->cmds[i]=val[i];
00071     rcontrol->setDirty();
00072     motman->checkinMotion(rcontrol_id);
00073   }
00074   
00075   virtual void DoStart() {
00076     // Behavior startup
00077     BehaviorBase::DoStart();
00078     for(unsigned int i=0; i<NumPIDJoints; i++)
00079       val[i]=state->outputs[i];
00080     // Enable remote control stream
00081     rcontrol_id = motman->addPersistentMotion(SharedObject<RemoteControllerMC>());
00082     updateRC();
00083     // Turn on wireless
00084     cmdsock=wireless->socket(SocketNS::SOCK_STREAM, 2048, 2048);
00085     wireless->setReceiver(cmdsock->sock, aibo3dcontrollercmd_callback);
00086     wireless->setDaemon(cmdsock,true);
00087     wireless->listen(cmdsock->sock, config->main.aibo3d_port);
00088     // open gui
00089     /*    std::vector<std::string> tmp;
00090           tmp.push_back("Aibo3D Load Instructions");
00091           tmp.push_back("To load Aibo3D, you will need to install java3d\nand then run Tekkotsu/tools/aibo3d/");
00092           tmp.back()+=getGUIType();
00093           Controller::loadGUI("ControllerMsg","LoadAibo3d",getPort(),tmp);*/
00094     Controller::loadGUI(getGUIType(),getGUIType(),getPort());
00095   }
00096 
00097   virtual void DoStop() {
00098     Controller::closeGUI(getGUIType());
00099     // Close socket; turn wireless off
00100     wireless->setDaemon(cmdsock,false);
00101     wireless->close(cmdsock);
00102     // Disable remote control
00103     motman->removeMotion(rcontrol_id);
00104     // Total behavior stop
00105     BehaviorBase::DoStop();
00106   }
00107 
00108   //! returns string corresponding to the Java GUI which should be launched
00109   virtual std::string getGUIType() const { return "org.tekkotsu.aibo3d.Aibo3DPick"; }
00110   //! returns port number the Java GUI should connect to
00111   virtual unsigned int getPort() const { return config->main.aibo3d_port; }
00112 
00113   static std::string getClassDescription() { 
00114     char tmp[20];
00115     sprintf(tmp,"%d",config->main.aibo3d_port);
00116     return std::string("Listens to aibo3d control commands coming in from port ")+tmp;
00117   }
00118   virtual std::string getDescription() const { return getClassDescription(); }
00119 
00120 };
00121 
00122 
00123 int aibo3dcontrollercmd_callback(char *buf, int bytes) {
00124   if (aibo3dControllerBehavior!=NULL)
00125     return aibo3dControllerBehavior->registerData(buf, bytes);
00126   return 0;
00127 }
00128 
00129 /*! @file
00130  * @brief Defines Aibo3DControllerBehavior, which listens to commands from the Aibo3D gui and shows current state
00131  * @author alokl (Creator)
00132  *
00133  * $Author: ejt $
00134  * $Name: tekkotsu-2_2_1 $
00135  * $Revision: 1.6 $
00136  * $State: Exp $
00137  * $Date: 2004/11/11 01:45:36 $
00138  */
00139 
00140 #endif 

Tekkotsu v2.2.1
Generated Tue Nov 23 16:36:36 2004 by Doxygen 1.3.9.1