00001
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
00014
00015 int aibo3dcontrollercmd_callback(char *buf, int bytes);
00016 class Aibo3DControllerBehavior;
00017
00018
00019 Aibo3DControllerBehavior *aibo3dControllerBehavior = NULL;
00020
00021
00022
00023 class Aibo3DControllerBehavior : public BehaviorBase {
00024 protected:
00025 MotionManager::MC_ID rcontrol_id;
00026
00027
00028 Socket *cmdsock;
00029
00030 float val[NumPIDJoints];
00031 char *fbuf;
00032 unsigned int pos;
00033
00034 private:
00035 Aibo3DControllerBehavior(const Aibo3DControllerBehavior&);
00036 Aibo3DControllerBehavior operator=(const Aibo3DControllerBehavior&);
00037
00038 public:
00039
00040 Aibo3DControllerBehavior() :
00041 BehaviorBase(),
00042 rcontrol_id(MotionManager::invalid_MC_ID),
00043 cmdsock(wireless->socket(SocketNS::SOCK_STREAM, 2048, 2048)),
00044 fbuf((char*)val), pos(0)
00045 { aibo3dControllerBehavior = this; }
00046
00047 virtual ~Aibo3DControllerBehavior() { aibo3dControllerBehavior = NULL; }
00048
00049
00050 int registerData(char *buf, int bytes) {
00051 int read=0;
00052 while (read<bytes) {
00053 if (pos==NumPIDJoints*4) {
00054 RemoteControllerMC *rcontrol =
00055 (RemoteControllerMC*)motman->checkoutMotion(rcontrol_id);
00056 for (unsigned int i=0; i<NumPIDJoints; i++)
00057 rcontrol->cmds[i]=val[i];
00058 rcontrol->setDirty();
00059 motman->checkinMotion(rcontrol_id);
00060 pos=0;
00061 }
00062 fbuf[pos]=buf[read];
00063 pos++;
00064 read++;
00065 }
00066 return read;
00067 }
00068
00069 virtual void DoStart() {
00070
00071 BehaviorBase::DoStart();
00072
00073 rcontrol_id = motman->addMotion(SharedObject<RemoteControllerMC>());
00074
00075 wireless->setReceiver(cmdsock->sock, aibo3dcontrollercmd_callback);
00076 wireless->listen(cmdsock->sock, config->main.aibo3d_port);
00077
00078 std::vector<std::string> tmp;
00079 tmp.push_back("Aibo3D Load Instructions");
00080 tmp.push_back("To load Aibo3D, you will need to install java3d\nand then run Tekkotsu/tools/aibo3d/");
00081 tmp.back()+=getGUIType();
00082 Controller::loadGUI("ControllerMsg","LoadAibo3d",getPort(),tmp);
00083
00084 }
00085
00086 virtual void DoStop() {
00087 Controller::closeGUI(getGUIType());
00088
00089 wireless->close(cmdsock);
00090
00091 motman->removeMotion(rcontrol_id);
00092
00093 BehaviorBase::DoStop();
00094 }
00095
00096 virtual std::string getGUIType() const { return "Aibo3DPick"; }
00097 virtual unsigned int getPort() const { return config->main.aibo3d_port; }
00098
00099 virtual std::string getName() const { return "Aibo3D Controller"; }
00100 static std::string getClassDescription() {
00101 char tmp[20];
00102 sprintf(tmp,"%d",config->main.aibo3d_port);
00103 return std::string("Listens to aibo3d control commands coming in from port ")+tmp;
00104 }
00105 };
00106
00107
00108 int aibo3dcontrollercmd_callback(char *buf, int bytes) {
00109 if (aibo3dControllerBehavior!=NULL)
00110 return aibo3dControllerBehavior->registerData(buf, bytes);
00111 return 0;
00112 }
00113
00114
00115
00116
00117
00118
00119
00120
00121
00122
00123
00124
00125 #endif