00001 #include <iostream>
00002 #include <GameController.H>
00003 #include <CommsManager.H>
00004 #include <Egomotion.H>
00005 #include <ObstacleAvoider.H>
00006 #include <SoundController.H>
00007 #include <PTUnit.H>
00008 #include <ImageDisplay.H>
00009 #include <VectorMover.H>
00010
00011 void configureJoystick(std::string config, GameController &joystick);
00012
00013 using namespace WURDE;
00014 using namespace std;
00015
00016 int main(int argc, char *argv[]) {
00017 GameController joystick;
00018 CommsManager myManager("BlindTeleop");
00019 myManager.setRealName("BlindTeleop");
00020 myManager.setHelpString("\t-c [config] Specifies alternate config file to use. Default is\n\t\tjoystick-config.xml.\
00021 \n\nThis module provides a simple tele-operation interface using a joystick.\n");
00022 myManager.parseOptions(argc,argv,"c:");
00023
00024 Egomotion myEgo(STRAT_ASSIGNED,"EgoClient");
00025 ObstacleAvoider myAvoider(STRAT_ASSIGNED,"AvoiderClient");
00026
00027 SoundController mySound(STRAT_ASSIGNED,"SoundClient");
00028 PTUnit myPTUnit(STRAT_ASSIGNED, "PTUClient");
00029 ImageDisplay myImDisplay("RobotDisplay");
00030
00031 myManager.registerConsumer(&myEgo);
00032 myManager.registerConsumer(&myAvoider);
00033 myManager.registerConsumer(&mySound);
00034 myManager.registerConsumer(&myPTUnit);
00035 myManager.registerConsumer(&myImDisplay);
00036
00037 bool running=true,brakeon=true;
00038 string confFile;
00039 if(g_globalConfiguration.haveOption("c")){
00040 confFile=g_globalConfiguration.getOption("c");
00041 if(confFile[0]!='/'){
00042 confFile=g_globalConfiguration.getConfigDirectory()+"/"+confFile;
00043 }
00044 }else{
00045 confFile=g_globalConfiguration.getConfigDirectory()+"/"+"joystick-config.xml";
00046 }
00047 g_logdebug << "Reading configuration for " << myManager.getName() << " from " << confFile<< endl;
00048 configureJoystick(confFile,joystick);
00049
00050 Pose curr;
00051 Point goal;
00052 string lastSound="none";
00053 bool sound1go, sound2go, sound3go, sound4go, sound5go, sound6go,imagego;
00054 int imageIndex=0;
00055
00056 while (running&&(myManager.runUpdate()==STATE_RUN||myManager.getState()==STATE_IDLE)) {
00057
00058
00059 if(myEgo.newData()){
00060 myEgo.getNextData();
00061 curr=myEgo.data.location;
00062 }
00063
00064 if(myAvoider.newInfo()){
00065 myAvoider.getNextInfo();
00066 if(myAvoider.info.brake.getValue()){
00067 brakeon=true;
00068 }else{
00069 brakeon=false;
00070 }
00071 }
00072
00073 while(mySound.newInfo()){
00074 mySound.getNextInfo();
00075 }
00076
00077 while(mySound.newData()){
00078 mySound.getNextData();
00079 }
00080
00081 while(myImDisplay.newInfo()){
00082 myImDisplay.getNextInfo();
00083 }
00084
00085 while(myImDisplay.newData()){
00086 myImDisplay.getNextData();
00087 }
00088
00089 goal.x(0);
00090 goal.y(0);
00091
00092 if(joystick.axis("translate")/32767.0 < -0.15){
00093 goal.x(joystick.axis("translate")/32767.0*2.0*-1);
00094
00095 if(fabs(joystick.axis("rotate")/32767.0) > 0.1){
00096 goal.y(joystick.axis("rotate")/32767.0 * 1.5*-1);
00097 }
00098 myAvoider.requests.brake.setValue(false);
00099 myAvoider.moveToRelativePoint(goal);
00100 }else{
00101 if(fabs(joystick.axis("rotate")/32767.0) > 0.5){
00102
00103 myAvoider.requests.brake.setValue(false);
00104 if(joystick.axis("rotate") > 0){
00105 myAvoider.rotateToRelativeAngle(-M_PI/2);
00106 goal.y(-3.14);
00107 }else{
00108 myAvoider.rotateToRelativeAngle(M_PI/2);
00109 goal.y(3.14);
00110 }
00111 }else{
00112 myAvoider.stop();
00113 }
00114 }
00115
00116 if(fabs(joystick.axis("quicktilt"))/32767 > 0.05){
00117 myPTUnit.requests.tilt.setValue(joystick.axis("quicktilt")/32767.0*0.2);
00118 }else{
00119 myPTUnit.requests.tilt.setValue(0);
00120 }
00121
00122 if(fabs(joystick.axis("quickpan"))/32767 > 0.05){
00123
00124 int segment=joystick.axis("quickpan")/32767.0 * 6.0;
00125 myPTUnit.requests.pan.setValue(segment*(32767.0/6.0)/32767*M_PI/4.0*-1);
00126 }else{
00127 myPTUnit.requests.pan.setValue(0);
00128 }
00129
00130 myPTUnit.publishRequest();
00131
00132 if(joystick.button("resetPTU")){
00133 myPTUnit.requests.pan.setValue(0);
00134 myPTUnit.requests.tilt.setValue(0);
00135 myPTUnit.publishRequest();
00136 }
00137
00138 if(joystick.button("stop")){
00139 myAvoider.stop();
00140 }
00141
00142 if(joystick.button("quit")){
00143 myAvoider.stop();
00144 running=false;
00145 }
00146
00147
00148 if(joystick.button("sound1")&&sound1go){
00149 mySound.requests.playSound.setValue(true);
00150 mySound.requests.currentSound.setValue("sound1");
00151 mySound.requests.repeat.setValue(1);
00152 mySound.publishRequest();
00153 lastSound="sound1";
00154 sound1go=false;
00155 }else if(!joystick.button("sound1")&&!sound1go){
00156 sound1go=true;
00157 }
00158
00159 if(joystick.button("sound2")&&sound2go){
00160 mySound.requests.playSound.setValue(true);
00161 mySound.requests.currentSound.setValue("sound2");
00162 mySound.requests.repeat.setValue(1);
00163 mySound.publishRequest();
00164 lastSound="sound2";
00165 sound2go=false;
00166 }else if(!joystick.button("sound2")&&!sound2go){
00167 sound2go=true;
00168 }
00169
00170 if(joystick.button("sound3")&&sound3go){
00171 mySound.requests.playSound.setValue(true);
00172 mySound.requests.currentSound.setValue("sound3");
00173 mySound.requests.repeat.setValue(1);
00174 mySound.publishRequest();
00175 lastSound="sound3";
00176 sound3go=false;
00177 }else if(!joystick.button("sound3")&&!sound3go){
00178 sound3go=true;
00179 }
00180
00181 if(joystick.button("sound4")&&sound4go){
00182 mySound.requests.playSound.setValue(true);
00183 mySound.requests.currentSound.setValue("sound4");
00184 mySound.requests.repeat.setValue(1);
00185 mySound.publishRequest();
00186 lastSound="sound4";
00187 sound4go=false;
00188 }else if(!joystick.button("sound1")&&!sound4go){
00189 sound4go=true;
00190 }
00191
00192 if(joystick.button("sound5")&&sound5go){
00193 mySound.requests.playSound.setValue(true);
00194 mySound.requests.currentSound.setValue("sound5");
00195 mySound.requests.repeat.setValue(1);
00196 mySound.publishRequest();
00197 lastSound="sound5";
00198 sound5go=false;
00199 }else if(!joystick.button("sound5")&&!sound5go){
00200 sound5go=true;
00201 }
00202
00203 if(joystick.button("sound6")&&sound6go){
00204 mySound.requests.playSound.setValue(true);
00205 mySound.requests.currentSound.setValue("sound6");
00206 mySound.requests.repeat.setValue(1);
00207 mySound.publishRequest();
00208 lastSound="sound6";
00209 sound6go=false;
00210 }else if(!joystick.button("sound6")&&!sound6go){
00211 sound6go=true;
00212 }
00213
00214 if(joystick.button("cycleImage")){
00215 imageIndex++;
00216 if(imageIndex>3){
00217 imageIndex=0;
00218 }
00219 }
00220
00221 if(joystick.button("displayImage")&&imagego){
00222 string tmp="image";
00223 switch(imageIndex){
00224 case 0:
00225 tmp=tmp+"0";
00226 break;
00227 case 1:
00228 tmp=tmp+"1";
00229 break;
00230 case 2:
00231 tmp=tmp+"2";
00232 break;
00233 case 3:
00234 tmp=tmp+"3";
00235 break;
00236 }
00237
00238 myImDisplay.requests.currentImage.setValue(tmp);
00239 myImDisplay.requests.displayImage.setValue(true);
00240 myImDisplay.publishRequest();
00241 imagego=false;
00242 }else if(!joystick.button("displayImage")){
00243 imagego=true;
00244 }
00245
00246
00247 printf("\015");
00248 printf("Loc: (%lf,%lf,%lf)\t Goal: (%lf,%lf)\t Sound: %s\timage: %d\t",curr.x(),curr.y(),curr.theta(),
00249 goal.x(),goal.y(),lastSound.c_str(),imageIndex);
00250
00251 fflush(stdout);
00252 }
00253
00254 myManager.cleanUp();
00255
00256 return 0;
00257 }
00258
