Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

TeRKDriver.cc

Go to the documentation of this file.
00001 #ifdef HAVE_ICE
00002 #include <iostream>
00003 #include <sstream>
00004 #include <unistd.h>
00005 #include "TeRKDriver.h"
00006 
00007 using namespace std; 
00008 
00009 const std::string TeRKDriver::autoRegisterTeRKDriver = DeviceDriver::getRegistry().registerType<TeRKDriver>("TeRK");
00010 
00011 //! this array defines the values for TeRKDriver::TeRKProperties::defaults
00012 pair<string,string> TeRKDriver::TeRKProperties::defaultValues[NUM_DEFAULT_VALUES] = {
00013   make_pair( "Ice.Package.peer", "edu.cmu.ri.mrpl" ),
00014   make_pair( "Ice.Package.TeRK", "edu.cmu.ri.mrpl" ),
00015   make_pair( "Ice.Default.Package", "edu.cmu.ri.mrpl" ),
00016   
00017   make_pair( "TerkClient.Client.Endpoints", "tcp" ),
00018   
00019   // Active connection management must be disabled when using bidirectional connections.
00020   make_pair( "Ice.ACM.Client", "0" ),
00021   make_pair( "Ice.ACM.Server", "0" ),
00022   
00023   // Print warning messages for certain exceptional conditions in connections
00024   make_pair( "Ice.Warn.Connections", "1" ),
00025   
00026   make_pair( "Ice.Logger.Timestamp", "1" ),
00027   
00028   make_pair( "Ice.ThreadPool.Client.Size", "5" ),
00029   make_pair( "Ice.ThreadPool.Client.SizeMax", "20" ),
00030   make_pair( "Ice.ThreadPool.Server.Size", "5" ),
00031   make_pair( "Ice.ThreadPool.Server.SizeMax", "20" ),
00032   
00033   // protocol and port to use for direct connect
00034   make_pair( "TeRK.direct-connect.protocol", "tcp" ),
00035   make_pair( "TeRK.direct-connect.port", "10101" ),
00036 };
00037 
00038 void TeRKDriver::motionUpdated(const std::vector<size_t>& changedIndices, const float outputs[][NumOutputs]) {
00039   if(qwerk==NULL)
00040     return;
00041   
00042   std::set<size_t> updatedIndices(changedIndices.begin(),changedIndices.end());
00043   
00044 #ifdef TGT_HAS_LEDS
00045   for(unsigned int i=LEDOffset; i<LEDOffset+NumLEDs; ++i) {
00046     float v = outputs[NumFrames-1][i];
00047     if(v>0 && v<1) // intermediate value, need to flicker activation
00048       updatedIndices.insert(i);
00049   }
00050 #endif
00051   
00052   if(updatedIndices.size()==0)
00053     return;
00054   try {
00055     //cout << "Got motor command. " << endl;
00056     //QwerkState qstate = qwerk->requestStateFrame();
00057     //cout << "Getting state packet with: " << qstate.motor.motorVelocities.size() << " " << qstate.motor.motorVelocities[0] << " " << qstate.motor.motorVelocities[1] << " " << qstate.motor.motorVelocities[2] << " " << qstate.motor.motorVelocities[3] << endl;
00058     //cout << "Setting motor to: " << outputs[NumFrames-1][0] << " " << outputs[NumFrames-1][1] << endl;
00059     //cout << "Setting servo to: " << outputs[NumFrames-1][2] << " " << outputs[NumFrames-1][3] << endl;
00060     
00061 #ifdef TGT_HAS_WHEELS
00062     const int MOTOR_MAX=100000;
00063     ::TeRK::MotorCommand mc;
00064     mc.motorVelocities.assign(NumWheels,0);
00065     mc.motorPositions.assign(NumWheels,0);
00066     mc.motorAccelerations.assign(NumWheels,1000000);
00067     mc.motorMask.assign(NumWheels,false); // will set to true for the motors being updated
00068     mc.motorModes.assign(NumWheels,::TeRK::MotorSpeedControl);
00069 #endif
00070     
00071 #ifdef TGT_HAS_LEDS
00072     ::TeRK::LEDCommand ledc;
00073     ledc.ledMask.assign(NumLEDs,false);
00074     ledc.ledModes.assign(NumLEDs,::TeRK::LEDOff);
00075 #endif
00076     
00077     const int SERVO_MAX=255;
00078     const int NUM_SERVOS = NumOutputs-NumWheels-NumLEDs;
00079     ::TeRK::ServoCommand sc;
00080     sc.servoPositions.assign(NUM_SERVOS,0);
00081     sc.servoSpeeds.assign(NUM_SERVOS,INT_MAX);
00082     sc.servoMask.assign(NUM_SERVOS,false);
00083     sc.servoModes.assign(NUM_SERVOS,::TeRK::ServoMotorPositionControl);
00084     
00085     for(std::set<size_t>::const_iterator it=updatedIndices.begin(); it!=updatedIndices.end(); ++it) {
00086 #ifdef TGT_HAS_WHEELS
00087       if(WheelOffset<=*it && *it<WheelOffset+NumWheels) {
00088         const unsigned int motorIdx=(*it-WheelOffset);
00089         float scale=outputRanges[*it][MaxRange]*3; // assuming symmetric range!
00090         mc.motorVelocities[motorIdx]=static_cast<int>(MOTOR_MAX*outputs[NumFrames-1][*it]/scale);
00091         mc.motorMask[motorIdx]=true;
00092         cout << "Setting " << outputNames[*it] << " (motor " << motorIdx << ") to: " << outputs[NumFrames-1][*it] << " (" << mc.motorVelocities[motorIdx] << ")" << endl;
00093       } else
00094 #endif
00095 #ifdef TGT_HAS_LEDS
00096       if(LEDOffset<=*it && *it<LEDOffset+NumLEDs){
00097         const unsigned int ledIdx = (*it-LEDOffset);
00098         ledc.ledMask[ledIdx] = true;
00099         ledc.ledModes[ledIdx] = calcLEDValue(ledIdx,outputs[NumFrames-1][*it]);
00100       } else
00101 #endif
00102       {
00103         // if it's not a wheel or LED, treat it as a servo
00104         unsigned int servoIdx=*it;
00105 #ifdef TGT_HAS_WHEELS
00106         if(servoIdx>WheelOffset)
00107           servoIdx-=NumWheels;
00108 #endif
00109 #ifdef TGT_HAS_LEDS
00110         if(servoIdx>LEDOffset)
00111           servoIdx-=NumLEDs;
00112 #endif
00113         const float range = outputRanges[*it][MaxRange]-outputRanges[*it][MinRange];
00114         sc.servoPositions[servoIdx]=(int)(SERVO_MAX*(outputs[NumFrames-1][*it]-outputRanges[*it][MinRange])/range);
00115         //int cur=(SERVO_MAX*(lastOutputs[*it]-outputRanges[*it][MinRange])/range);
00116         //sc.servoSpeeds[servoIdx]=abs(sc.servoPositions[servoIdx]-cur)*1000/(FrameTime*NumFrames);
00117         sc.servoMask[servoIdx]=true;
00118         cout << "Setting " << outputNames[*it] << " (servo " << servoIdx << ") to: " << outputs[NumFrames-1][*it] << " (" << sc.servoPositions[servoIdx] << ")" << endl;
00119       }
00120     }
00121     
00122     // push the state to the qwerk
00123 #ifdef TGT_HAS_WHEELS
00124     qwerk->motorControl->execute(mc);
00125 #endif
00126 #ifdef TGT_HAS_LEDS
00127     qwerk->ledControl->execute(ledc);
00128 #endif
00129     qwerk->servoControl->execute(sc);
00130   } catch(...) {
00131     std::cerr << "TerkMotionHook::motionCheck caught exception -- communication lost?" << std::endl;
00132     close();
00133 
00134     while(qwerk==NULL){
00135       std::cerr << "Reconnecting in 10 seconds..." << std::endl;
00136       sleep(10);
00137       this->connect();
00138     }
00139   }
00140 }
00141 
00142 
00143 void TeRKDriver::plistValueChanged(const plist::PrimitiveBase& pl) {
00144   this->connect();
00145 }
00146 
00147 void TeRKDriver::connect(){
00148   bool wasConnected=(qwerk!=NULL);
00149   close();
00150 
00151   if(host.size()!=0) {
00152     string port = properties->getProperty("TeRK.direct-connect.port");
00153     try {
00154       qwerk = connectToPeer();
00155     } catch(const Ice::Exception& e) {
00156       std::cerr << "TeRKDriver threw exception '" << e << "' when connecting to " << host << " on port " << port << std::endl;
00157     } catch(const std::string& e) {
00158       std::cerr << "TeRKDriver threw exception string '" << e << "' when connecting to " << host << " on port " << port << std::endl;
00159     } catch(const char* e) {
00160       std::cerr << "TeRKDriver threw exception cstring '" << e << "' when connecting to " << host << " on port " << port << std::endl;
00161     } catch(...) {
00162       std::cerr << "TeRKDriver threw unknown exception when connecting to " << host << " on port " << port << std::endl;
00163     }
00164     if(qwerk==NULL) {
00165       std::cerr << "TeRKDriver could not connect to " << host << " on port " << port << std::endl;
00166       close();
00167       return;
00168     }
00169   }
00170   
00171   bool isConnected=(qwerk!=NULL);
00172   if(wasConnected!=isConnected) // if we changed connection status...
00173     fireDataSourcesUpdated(); // ...either new data sources are available, or old ones are gone
00174 }
00175 
00176 QwerkBot* TeRKDriver::connectToPeer() {
00177   if(ic)
00178     ic->destroy();
00179   Ice::InitializationData iceData;
00180   iceData.properties = properties;
00181   ic = Ice::initialize(iceData);
00182 
00183 
00184   ostringstream uuidSS;
00185   uuidSS << "direct_connect_client|";
00186   uuidSS << IceUtil::generateUUID();
00187   uuidSS << "|" << idcount;
00188   uuid = uuidSS.str();
00189   idcount++;
00190 
00191   string port = properties->getProperty("TeRK.direct-connect.port");
00192   cout << "Connecting to " << host << ":" << port << endl;
00193   
00194   ::Ice::Identity peerIdentity;
00195   peerIdentity.name = "::TeRK::TerkUser";
00196   peerIdentity.category = "";
00197   
00198   ::Ice::ObjectPrx objectPrx = getPeerProxy(peerIdentity);
00199   cout << "objectPrx: " << objectPrx << endl;
00200   ::TeRK::TerkUserPrx peerProxy = ::TeRK::TerkUserPrx::checkedCast(objectPrx);
00201   
00202   ::Ice::ObjectAdapterPtr adapter = ic->createObjectAdapter("TerkClient.Client");
00203 
00204   peerProxy->ice_getConnection()->setAdapter(adapter);
00205   
00206   ::Ice::Identity callbackReceiverIdent;
00207   callbackReceiverIdent.name = "terkCallbackReceiver";
00208   callbackReceiverIdent.category = "";
00209   
00210   TerkUserI* terkUserI = new TerkUserI();
00211   ::TeRK::TerkClientPrx qwerkServantPrx = ::TeRK::TerkClientPrx::checkedCast(adapter->add(terkUserI, callbackReceiverIdent));
00212   cout << "qwerkServantPrx: " << qwerkServantPrx << endl;
00213   
00214   adapter->activate();
00215   
00216   // Clean up old ping thread if exists
00217   if(ping) ping->destroy();
00218   // Starting ping thread - to make sure connection is still alive
00219   ping = new ObjectPingThread(peerProxy);
00220   ping->start();
00221   
00222   // Validate it is a qwerk proxy 
00223   ::TeRK::QwerkPrx peer = ::TeRK::QwerkPrx::checkedCast(peerProxy);
00224   cout << "Casting to qwerkPrx: " << peer << endl;
00225   
00226   // now tell the bot that we're connected.
00227   peerProxy->peerConnected(uuid, ::peer::AccessLevelOwner, qwerkServantPrx);
00228   
00229   cout << "Told bot I exist..." << endl;
00230   QwerkBot *qb = new QwerkBot(this, peerProxy, peer);
00231   cout << "Created QwerkBot" << endl;
00232   terkUserI->setImageCache(qb->imageCache);
00233   cout << "Set image cache to bot" << endl;
00234   if(qb->video){
00235     printf("Starting Video...\n");
00236     // first time we connect to "fake qwerk" throws an internal Ice exception on startVideoStream()
00237     // this is apparently supposed to indicate you should try again, so we do.
00238     // (this whole issue may be an Ice bug that we're even seeing the exception)
00239     const unsigned int TRIES=5;
00240     for(unsigned int i=0; i<TRIES; i++) {
00241       try {
00242         qb->video->startVideoStream();
00243         break; // no exception? leave retry loop...
00244       } catch(const Ice::Exception& e) {
00245         std::cerr << "TeRKDriver threw exception '" << e << "' connecting to video on " << host << " port " << port << " (attempt " << i+1 << " of " << TRIES << ")" << std::endl;
00246       } catch(const std::string& e) {
00247         std::cerr << "TeRKDriver threw exception string '" << e << "' connecting to video on " << host << " port " << port << " (attempt " << i+1 << " of " << TRIES << ")" << std::endl;
00248       } catch(const char* e) {
00249         std::cerr << "TeRKDriver threw exception cstring '" << e << "' connecting to video on " << host << " port " << port << " (attempt " << i+1 << " of " << TRIES << ")" << std::endl;
00250       } catch(...) {
00251         std::cerr << "TeRKDriver threw unknown exception connecting to video on " << host << " port " << port << " (attempt " << i+1 << " of " << TRIES << ")" << std::endl;
00252       }
00253       usleep(10000);
00254     }
00255   }
00256   return qb;
00257 }
00258 
00259 Ice::ObjectPrx TeRKDriver::getPeerProxy(Ice::Identity proxyIdentity) const {
00260   // doesn't work... ?
00261   //Ice::ObjectPrx objectPrx = peerProxy->ice_getConnection()->getAdapter()->createProxy(proxyIdentity);
00262   
00263   // instead:
00264   string port = properties->getProperty("TeRK.direct-connect.port");
00265   string protocol = properties->getProperty("TeRK.direct-connect.protocol");
00266   // I can't find a way to create proxies (attempt above) without using this "build a stringified command line" interface.  Sigh.
00267   string proxyString = "'" + ic->identityToString(proxyIdentity) + "':" + protocol + " -h " + host + " -p " + port;
00268   Ice::ObjectPrx objectPrx = ic->stringToProxy(proxyString);
00269 
00270   // now define a custom context for interacting with this proxy
00271   ::Ice::Context context;
00272   // not sure this first property is really necessary -- for now it's not being used.
00273   // if it could be taken out, we could set the other two as the "defaultContext" and not have to make each context unique.
00274   context["__peerProxyIdentity"] = ic->identityToString(objectPrx->ice_getIdentity()); // e.g. "::TeRK::TerkUser"
00275   context["__peerUserId"] = uuid;
00276   context["__isDirectConnect"] = "true";
00277     
00278   return objectPrx->ice_context(context); // get a new proxy with the specified context
00279 }
00280 
00281 
00282 
00283 // CONFIGURATION STUFF
00284 // TeRKDriver::TeRKProperties gives an Ice interface to the tekkotsu configuration system (plist)
00285 
00286 ::std::string TeRKDriver::TeRKProperties::getPropertyWithDefault(const ::std::string& key, const ::std::string& def) {
00287   const_iterator dit = findEntry(key);
00288   if(dit!=end())
00289     return dit->second->toString();
00290   ::Ice::PropertyDict::const_iterator it = defaults.find(key);
00291   return it==defaults.end() ? def : it->second;
00292 }
00293 
00294 ::Ice::Int TeRKDriver::TeRKProperties::getPropertyAsIntWithDefault(const ::std::string& key, ::Ice::Int def) {
00295   const_iterator dit = findEntry(key);
00296   if(dit!=end())
00297     return dit->second->toLong();
00298   ::Ice::PropertyDict::const_iterator it = defaults.find(key);
00299   return it==defaults.end() ? def : atoi(it->second.c_str());
00300 }
00301 
00302 ::Ice::PropertyDict TeRKDriver::TeRKProperties::getPropertiesForPrefix(const ::std::string& prefix) {
00303   ::Ice::PropertyDict ans;
00304   for(const_iterator it=begin(); it!=end(); ++it)
00305     if(it->first.compare(0, prefix.size(), prefix) == 0)
00306       ans.insert(make_pair(it->first,it->second->toString()));
00307   // remember that map::insert ignores the call if it already exists...
00308   // so these defaults below won't override explicitly set values which have just been added
00309   for(::Ice::PropertyDict::const_iterator it=defaults.begin(); it!=defaults.end(); ++it)
00310     if(it->first.compare(0, prefix.size(), prefix) == 0)
00311       ans.insert(*it);
00312   return ans;
00313 }
00314 
00315 void TeRKDriver::TeRKProperties::load(const ::std::string& filename) {
00316   if(!loadFile(filename.c_str())) {
00317     ::Ice::FileException ex(__FILE__, __LINE__);
00318     ex.path = filename;
00319     ex.error = getSystemErrno();
00320     throw ex;
00321   }
00322 }
00323 
00324 /*! @file
00325  * @brief 
00326  * @author Ethan Tira-Thompson (ejt) (Creator)
00327  *
00328  * $Author: ejt $
00329  * $Name: tekkotsu-4_0 $
00330  * $Revision: 1.12 $
00331  * $State: Exp $
00332  * $Date: 2007/11/11 23:57:29 $
00333  */
00334 #endif

Tekkotsu Hardware Abstraction Layer 4.0
Generated Thu Nov 22 01:00:53 2007 by Doxygen 1.5.4