Main Page | Modules | Namespace List | Class Hierarchy | Alphabetical List | Class List | Directories | File List | Namespace Members | Class Members | File Members | Related Pages

PlayerStage.cpp

Go to the documentation of this file.
00001 /* ---------------------------------------------------------------------------
00002     Phission : 
00003         Realtime Vision Processing System
00004     
00005     Copyright (C) 2003 Philip D.S. Thoren (pthoren@cs.uml.edu)
00006     University of Massachusetts at Lowell,
00007     Laboratory for Artificial Intelligence and Robotics
00008     
00009     This file is part of Phission.
00010 
00011  ---------------------------------------------------------------------------*/
00012 #include <PlayerStage.h>
00013 
00014 #include <phPlayerClient.h>
00015 #include <phPlayerPosition.h>
00016 #include <phPlayerSonar.h>
00017 #include <phPlayerPtz.h>
00018 #include <phPlayerPower.h>
00019 #include <phPlayerWifi.h>
00020  
00021 #include <phLinuxJoystick.h>
00022 
00023 #include <phSonarHandler.h>
00024 #include <phWander.h>
00025 #include <phJoystickControl.h>
00026 
00027 #include <phission.h>
00028 
00029 const double max_translate   = 0.2;
00030 const double max_rotate      = 0.2;
00031 
00032 /* ------------------------------------------------------------------------ */
00033 void usage()
00034 {
00035     printf("\n\n\tUsage:\n");
00036     printf("\t\t\t--help\t\t\tdisplay usage\n");
00037     printf("\t\t\t--host <hostname>\tthe player server hostname\n");
00038     printf("\t\t\t--port <port>\t\tthe player server port\n");
00039 
00040     printf("\n\n");
00041     
00042     exit(1);
00043 }
00044 
00045 /* ------------------------------------------------------------------------ */
00046 int main( int argc, char *argv[] )
00047 {
00048     phFUNCTION("main")
00049     int                 i       = 0;
00050 
00051     uint16_t    port            = 0;
00052     char       *host            = NULL;
00053     
00054     int         dataFreq        = 1000;
00055     
00056     phPlayerClient      client;
00057     phPlayerSonar       sonar;
00058     phPlayerPosition    position;
00059     phPlayerPtz         ptz;
00060     phPlayerPower       power;
00061     phPlayerWifi        wifi;
00062 
00063     phPowerData         power_data;
00064     phWifiData          wifi_data;
00065     phWifiLinkData      wifi_link_data;
00066     int                 wifi_link_connected = 0;
00067     phLiveObject        *wifi_link_source   = NULL;
00068 
00069     phWander            wanderer(&position,&sonar);
00070     phSonarHandler      sonar_handler(&position,&sonar);
00071     
00072     phLinuxJoystick     joystick    ("/dev/js0");
00073     phJoystickControl   joy_control (&joystick,&position,&sonar,&ptz);
00074     
00075     phSystem            system;
00076 
00077     /* -------------------------------------------------------------------- */
00078     phArgTable          *arg_parser = new phArgTable();
00079 
00080     rc = arg_parser->add("--host",&host,phARG_CHAR);
00081     phCHECK_RC(rc,NULL,"arg_parser->add - host");
00082     
00083     rc = arg_parser->add("--port",&port,phARG_UINT16);
00084     phCHECK_RC(rc,NULL,"arg_parser->add - port");
00085 
00086     rc = arg_parser->add("--help",(void *)&usage,phARG_FUNC);
00087     phCHECK_RC(rc,NULL,"arg_parser->add");
00088  
00089     rc = arg_parser->parse(argc,argv);
00090     phCHECK_RC(rc,NULL,"arg_parser->parse");
00091 
00092     /* -------------------------------------------------------------------- */
00093     if (host == NULL)   host = "localhost";
00094     if (port == 0)      port = 6665;
00095     
00096     /* Create a client object and connect to the server */
00097     rc = client.set(host,port);
00098     phCHECK_RC(rc,NULL,"client.set");
00099 
00100     rc = client.datafreq(dataFreq);
00101     phCHECK_RC(rc,NULL,"client.datafreq(%d)",dataFreq);
00102 
00103     rc = system.add(&client);
00104     phCHECK_RC(rc,NULL,"system.add client failed");
00105 
00106     //rc = system.add(&sonar_handler);
00107     //phCHECK_RC(rc,NULL,"rc = system.add(sonar_handler)");
00108 
00109     /* THREADS */
00110 
00111     phPROGRESS("system.add(&wanderer)\n");
00112     rc = system.add(&wanderer);
00113     phCHECK_RC(rc,NULL,"rc = system.add(wanderer)");
00114 
00115     phPROGRESS("system.add(&joy_control)\n");
00116     rc = system.add(&joy_control);
00117     phCHECK_RC(rc,NULL,"system.add joy_control failed");
00118 
00119     /* DATA connections */
00120     phPROGRESS("power_data.connect(power.getSource())\n");
00121     rc = power_data.connect(power.getSource());
00122     phCHECK_RC(rc,NULL,"power_data.connect(power.getSource())");
00123 
00124     phPROGRESS("wifi_data.connect(wifi.getSource())\n");
00125     rc = wifi_data.connect(wifi.getSource());
00126     phCHECK_RC(rc,NULL,"wifi_data.connect(wifi.getSource())");
00127 
00128     /* DEVICES */
00129     /* Joystick */
00130     phPROGRESS("system.add(&joystick)\n");
00131     rc = system.add(&joystick);
00132     phCHECK_RC(rc,NULL,"system.add joystick failed");
00133 
00134     phPROGRESS("system.startup()\n");
00135     rc = system.startup();
00136     phCHECK_RC(rc,NULL,"system.startup");
00137 
00138     /* PROXIES */
00139     /* Wifi */
00140     phPROGRESS("wifi.subscribe( phMODE_READ, 0, &client )\n");
00141     rc = wifi.subscribe( phMODE_READ, 0, &client );
00142     phCHECK_RC(rc,NULL,"wifi.subscribe");
00143     
00144     /* Power */
00145     phPROGRESS("power.subscribe( phMODE_READ, 0, &client )\n");
00146     rc = power.subscribe( phMODE_READ, 0, &client );
00147     phCHECK_RC(rc,NULL,"power.subscribe");
00148     
00149     /* Sonar */
00150     phPROGRESS("sonar.subscribe( phMODE_READ, 0, &client )\n");
00151     rc = sonar.subscribe( phMODE_READ, 0, &client );
00152     phCHECK_RC(rc,NULL,"sonar.subscribe");
00153    
00154 #if 0
00155     phPROGRESS("sonar.getGeometry()\n");
00156     rc = sonar.getGeometry();
00157     phCHECK_RC(rc,NULL,"sonar.getGeometry()");
00158 #endif
00159     /* Position */
00160     phPROGRESS("position.subscribe( phMODE_ALL, 0, &client )\n");
00161     rc = position.subscribe( phMODE_ALL, 0, &client );
00162     phCHECK_RC(rc,NULL,"position.subscribe");
00163 
00164 /*    
00165     rc = position.enable();
00166     phCHECK_RC(rc,NULL,"position.enable()");
00167     rc = position.setCmdVel(0.0, 0.0, 0.0, 1);
00168     phCHECK_RC(rc,NULL,"position.setCmdVel");
00169 */
00170     /* Ptz */
00171 #if 1
00172     phPROGRESS("ptz.subscribe( phMODE_ALL, 0, &client )\n");
00173     rc = ptz.subscribe( phMODE_ALL, 0, &client );
00174     phCHECK_RC(rc,NULL,"ptz.subscribe");
00175 #endif
00176 
00177     rc = wifi_data.update();
00178     phCHECK_RC(rc,NULL,"wifi_data.update()");
00179     
00180     phPROGRESS("%d:%d\n", system.threadsActive(), system.threadCount() );
00181     while (system.threadsActive() == system.threadCount() )
00182     {
00183         if (wifi_link_connected == 0)
00184         {
00185             wifi_link_source = wifi_data.getSource(0);
00186             if (wifi_link_source != NULL)
00187             {
00188                 wifi_link_data.connect(wifi_link_source);
00189                 phCHECK_RC(rc,NULL,"wifi_link_data.connect(wifi_link_source:%p)",
00190                          wifi_link_source);
00191 
00192                 wifi_link_connected = 1;
00193                 phPROGRESS("Connected to wifi link status\n");
00194             }
00195         }
00196         /* Get and print power data */
00197         rc = power_data.update(phLiveObjectNOWAIT);
00198         if (rc == phSUCCESS)
00199         {
00200             if (power_data.getCharge() < 12.5)
00201             {
00202                 phPRINT("** WARNING: Robot Charge is low: %0.8f **\n",
00203                       power_data.getCharge());
00204             }
00205             else
00206             {
00207                 power_data.print_data();
00208             }
00209         }
00210 
00211         /* Get and print wifi data */
00212         if (wifi_link_connected == 1)
00213         {
00214             rc = wifi_link_data.update(phLiveObjectNOWAIT);
00215             if (rc == phSUCCESS)
00216             {
00217                 wifi_link_data.print_data();
00218             }
00219         }
00220         phMSleep(100);
00221     }
00222     
00223     phPRINT("Closing down...\n");
00224 
00225     phPROGRESS("ptz.unsubscribe( )\n");
00226     rc = ptz.unsubscribe( );
00227     phCHECK_RC(rc,NULL,"ptz.unsubscribe( )");
00228 
00229     phPROGRESS("sonar.unsubscribe( )\n");
00230     rc = sonar.unsubscribe( );
00231     phCHECK_RC(rc,NULL,"sonar.unsubscribe( )");
00232 
00233     phPROGRESS("position.unsubscribe( )\n");
00234     rc = position.unsubscribe( );
00235     phCHECK_RC(rc,NULL,"position.unsubscribe( )");
00236 
00237     phPROGRESS("wifi.unsubscribe( )\n");
00238     rc = wifi.unsubscribe( );
00239     phCHECK_RC(rc,NULL,"wifi.unsubscribe( )");
00240 
00241     phPROGRESS("power.unsubscribe( )\n");
00242     rc = power.unsubscribe( );
00243     phCHECK_RC(rc,NULL,"power.unsubscribe( )");
00244 
00245     phPROGRESS("system.shutdown( )\n");
00246     rc = system.shutdown( );
00247     phPRINT_RC(rc,NULL,"system.shutdown( )");
00248 
00249     exit(1);
00250 
00251     phDelete(arg_parser);
00252 
00253     return phSUCCESS;
00254 error:
00255     phDelete(arg_parser);
00256 
00257     return phFAIL;
00258 }
00259 
00260 




Copyright (C) 2002 - 2007 Philip D.S. Thoren ( pthoren@users.sourceforge.net )
University Of Massachusetts at Lowell
Robotics Lab
SourceForge.net Logo

Generated on Sat Jun 16 02:44:07 2007 for phission by  doxygen 1.4.4