00001
00002 #ifndef INCLUDED_TeRKDriver_h_
00003 #define INCLUDED_TeRKDriver_h_
00004
00005 #include "local/DeviceDriver.h"
00006 #include "local/MotionHook.h"
00007
00008 #include "local/terk/ObjectPingThread.h"
00009 #include "local/terk/QwerkBot.h"
00010
00011 #include "IceUtil/UUID.h"
00012 #include "Ice/ProxyHandle.h"
00013 #include "Ice/Identity.h"
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025 class TeRKDriver : public virtual DeviceDriver, public MotionHook, public virtual plist::PrimitiveListener {
00026 public:
00027
00028 explicit TeRKDriver(const std::string& name)
00029 : DeviceDriver(autoRegisterTeRKDriver,name), MotionHook(),
00030 host(), uuid("direct_connect_client|"+IceUtil::generateUUID()), properties(), ic(),
00031 ping(), qwerk(NULL), idcount(0)
00032 {
00033 for(unsigned int i=0; i<NumLEDs; ++i)
00034 ledActivation[i]=0;
00035 TeRKProperties* tprop = new TeRKProperties;
00036 addEntry("Host",host,"Hostname of the robot");
00037
00038 addEntry("IceConfig",*tprop,"Settings for ICE protocol");
00039 Ice::InitializationData iceData;
00040 iceData.properties = properties = tprop;
00041 ic = Ice::initialize(iceData);
00042 host.addPrimitiveListener(this);
00043 }
00044
00045
00046 virtual ~TeRKDriver() {
00047 host.removePrimitiveListener(this);
00048 close();
00049 if(ic)
00050 ic->destroy();
00051
00052 }
00053
00054 virtual std::string getClassName() const { return autoRegisterTeRKDriver; }
00055
00056 virtual MotionHook* getMotionSink() { return dynamic_cast<MotionHook*>(this); }
00057 virtual void getSensorSources(std::map<std::string,DataSource*>& sources) {
00058 sources.clear();
00059 if(qwerk!=NULL && qwerk->dataCache!=NULL)
00060 sources["Sensors"]=qwerk->dataCache;
00061 }
00062 virtual void getImageSources(std::map<std::string,DataSource*>& sources) {
00063 sources.clear();
00064 if(qwerk!=NULL && qwerk->imageCache!=NULL)
00065 sources["Camera"]=qwerk->imageCache;
00066 }
00067
00068 virtual void motionStarting() {}
00069 virtual void motionUpdated(const std::vector<size_t>& changedIndices, const float outputs[][NumOutputs]);
00070 virtual void motionStopping() {}
00071
00072 virtual void plistValueChanged(const plist::PrimitiveBase& pl);
00073
00074
00075
00076 Ice::ObjectPrx getPeerProxy(Ice::Identity proxyIdentity) const;
00077
00078
00079 plist::Primitive<std::string> host;
00080
00081 protected:
00082
00083 virtual void connect();
00084
00085
00086 QwerkBot* connectToPeer();
00087
00088
00089 virtual void close() {
00090 if(ping){
00091 ping->destroy();
00092 ping=NULL;
00093 }
00094 delete qwerk;
00095 qwerk=NULL;
00096 }
00097
00098
00099 inline ::TeRK::LEDMode calcLEDValue(unsigned int i,float x) {
00100 if(x<=0.0) {
00101 ledActivation[i]*=.9;
00102 return ::TeRK::LEDOff;
00103 } else if(x>=1.0) {
00104 return ::TeRK::LEDOn;
00105 } else {
00106 x*=x;
00107 ledActivation[i]+=x;
00108 if(ledActivation[i]>=1.0) {
00109 ledActivation[i]-=1.0;
00110 return ::TeRK::LEDOn;
00111 } else {
00112 return ::TeRK::LEDOff;
00113 }
00114 }
00115 }
00116 float ledActivation[NumLEDs];
00117
00118
00119
00120
00121
00122 std::string uuid;
00123
00124
00125
00126 Ice::PropertiesPtr properties;
00127
00128
00129
00130
00131
00132
00133 Ice::CommunicatorPtr ic;
00134
00135
00136
00137 ObjectPingThreadPtr ping;
00138
00139
00140
00141 QwerkBot* qwerk;
00142
00143 int idcount;
00144
00145
00146
00147
00148
00149
00150 class TeRKProperties : public virtual ::Ice::Properties, public virtual plist::Dictionary {
00151 public:
00152 typedef ::Ice::PropertiesPtr PointerType;
00153 typedef plist::Dictionary::iterator iterator;
00154 typedef plist::Dictionary::const_iterator const_iterator;
00155
00156
00157 TeRKProperties() : defaults(defaultValues, defaultValues+NUM_DEFAULT_VALUES) {}
00158
00159
00160
00161
00162
00163
00164
00165 ::Ice::PropertyDict defaults;
00166
00167
00168 virtual ::std::string getProperty(const ::std::string& key) { return getPropertyWithDefault(key,""); }
00169
00170 virtual ::std::string getPropertyWithDefault(const ::std::string& key, const ::std::string& def);
00171
00172 virtual ::Ice::Int getPropertyAsInt(const ::std::string& key) { return getPropertyAsIntWithDefault(key,0); }
00173
00174 virtual ::Ice::Int getPropertyAsIntWithDefault(const ::std::string& key, ::Ice::Int def);
00175
00176 virtual ::Ice::PropertyDict getPropertiesForPrefix(const ::std::string& prefix);
00177
00178 virtual void setProperty(const ::std::string& key, const ::std::string& val) { setEntry(key,new plist::Primitive<std::string>(val)); }
00179
00180 virtual ::Ice::StringSeq getCommandLineOptions() { return ::Ice::StringSeq(); }
00181
00182 virtual ::Ice::StringSeq parseCommandLineOptions(const ::std::string&, const ::Ice::StringSeq& options) { return options; }
00183
00184 virtual ::Ice::StringSeq parseIceCommandLineOptions(const ::Ice::StringSeq& options) { return options; }
00185
00186 virtual void load(const ::std::string& filename);
00187
00188 virtual ::Ice::PropertiesPtr clone() { return new TeRKProperties(*this); }
00189 virtual TeRKProperties* clone() const { return new TeRKProperties(*this); }
00190
00191 protected:
00192 static const unsigned int NUM_DEFAULT_VALUES=14;
00193 static std::pair<std::string,std::string> defaultValues[NUM_DEFAULT_VALUES];
00194 };
00195
00196 private:
00197
00198 static const std::string autoRegisterTeRKDriver;
00199
00200 TeRKDriver(const TeRKDriver&);
00201 TeRKDriver& operator=(const TeRKDriver&);
00202 };
00203
00204
00205
00206
00207
00208
00209
00210
00211
00212
00213
00214
00215 #endif