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
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
00020 make_pair( "Ice.ACM.Client", "0" ),
00021 make_pair( "Ice.ACM.Server", "0" ),
00022
00023
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
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)
00048 updatedIndices.insert(i);
00049 }
00050 #endif
00051
00052 if(updatedIndices.size()==0)
00053 return;
00054 try {
00055
00056
00057
00058
00059
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);
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;
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
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
00116
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
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)
00173 fireDataSourcesUpdated();
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
00217 if(ping) ping->destroy();
00218
00219 ping = new ObjectPingThread(peerProxy);
00220 ping->start();
00221
00222
00223 ::TeRK::QwerkPrx peer = ::TeRK::QwerkPrx::checkedCast(peerProxy);
00224 cout << "Casting to qwerkPrx: " << peer << endl;
00225
00226
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
00237
00238
00239 const unsigned int TRIES=5;
00240 for(unsigned int i=0; i<TRIES; i++) {
00241 try {
00242 qb->video->startVideoStream();
00243 break;
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
00261
00262
00263
00264 string port = properties->getProperty("TeRK.direct-connect.port");
00265 string protocol = properties->getProperty("TeRK.direct-connect.protocol");
00266
00267 string proxyString = "'" + ic->identityToString(proxyIdentity) + "':" + protocol + " -h " + host + " -p " + port;
00268 Ice::ObjectPrx objectPrx = ic->stringToProxy(proxyString);
00269
00270
00271 ::Ice::Context context;
00272
00273
00274 context["__peerProxyIdentity"] = ic->identityToString(objectPrx->ice_getIdentity());
00275 context["__peerUserId"] = uuid;
00276 context["__isDirectConnect"] = "true";
00277
00278 return objectPrx->ice_context(context);
00279 }
00280
00281
00282
00283
00284
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
00308
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
00325
00326
00327
00328
00329
00330
00331
00332
00333
00334 #endif