Tekkotsu Homepage | Demos | Overview | Downloads | Dev. Resources | Reference | Credits |
WorldStatePool.hGo to the documentation of this file.00001 //-*-c++-*- 00002 #ifndef INCLUDED_WorldStatePool_h_ 00003 #define INCLUDED_WorldStatePool_h_ 00004 00005 #include "WorldState.h" 00006 #include "IPC/MutexLock.h" 00007 #include "IPC/ListMemBuf.h" 00008 #include "Shared/Resource.h" 00009 #include "Motion/PostureEngine.h" 00010 #include <stdexcept> 00011 00012 class RCRegion; 00013 00014 #ifndef WORLDSTATEPOOL_NUM_STATES 00015 //! provides default value for WorldStatePool::NUM_STATES, allows you to override from build settings, without touching source 00016 #define WORLDSTATEPOOL_NUM_STATES 3 00017 #endif 00018 00019 //! holds multiple instances of WorldState, allows one process to be updating while another is reading 00020 /*! 00021 Use the AutoGetReadState and AutoGetWriteState to access individual WorldState entries... their 00022 constructors and destructors allow WorldStatePool to keep track of which entries are in use. 00023 00024 When a state wants to write, it is given the oldest unused entry to write into. During writing, 00025 other accessors can read the newest (complete) entry, or concurrently write into a different 00026 entry (in case they don't want to wait for the other process to finish). 00027 00028 A global lock (#lock) is used while choosing an entry, and individual locks (#writeLocks) are 00029 used while writing into entries (to easily allow readers to block on the lock until writing is done) 00030 00031 One point of trickiness is that entries being written are moved to the end of the list when 00032 the writing begins, not when it is complete. Readers can always scan backwards in the list 00033 to find the newest entries, but writers must check the end to see if newer (or equivalent) 00034 frames are already in progress, as well as the beginning to find the oldest unreferenced. 00035 00036 When a writer tries to access an entry, and another writer is already processing that frame, 00037 if blocking is set then the writer will given that entry once the original is done with it (so 00038 check your frame index when you receive it so you can tell if it was already processed). 00039 If blocking is *not* set, then you will get a separate entry with no indication another process 00040 is also working on the same frame. 00041 */ 00042 class WorldStatePool : public Resource { 00043 public: 00044 //! common base class for ReadRequest or WriteRequest 00045 class Request : public Resource::Data { 00046 protected: 00047 //! constructor, sets the WorldState point to be assigned, whether to block, and whether is an instance of ReadRequest 00048 /*! @a wantRead is because we can't trust RTTI (i.e. dynamic_cast) to work correctly on Aperios :( */ 00049 Request(WorldState*& target, bool block, bool wantRead) : Resource::Data(), 00050 bufUsed(-1U), tgt(target), prev(NULL), bl(block), depth(0), isRead(wantRead) 00051 {} 00052 //! shallow copy constructor supported 00053 Request(const Request& r) : Resource::Data(r), bufUsed(r.bufUsed), tgt(r.tgt), prev(r.prev), bl(r.bl), depth(r.depth), isRead(r.isRead) {} 00054 //! shallow assignment supported 00055 Request& operator=(const Request& r) { bufUsed=r.bufUsed; tgt=r.tgt; prev=r.prev; bl=r.bl; depth=r.depth; Resource::Data::operator=(r); return *this; } 00056 00057 public: 00058 unsigned int bufUsed; //!< the entry index used 00059 WorldState*& tgt; //!< reference to pointer, which is set to an element of the pool when the request goes through 00060 WorldState* prev; //!< stores previous value at #tgt so it can be restored upon release (needed to support recursive usage) 00061 bool bl; //!< whether to block if a write is in progress, or use most recent "complete" entry 00062 unsigned int depth; //!< supports recursive read requests 00063 bool isRead; //!< true if instance is a read request 00064 }; 00065 //! retrieves the current WorldState entry, and through it's destructor, marks the entry available again when it goes out of scope 00066 class ReadRequest : public Request { 00067 public: 00068 //! stores the current completed WorldState into #tgt, optionally blocking if an update is in progress (otherwise it returns the previous completed entry) 00069 ReadRequest(WorldState*& target, bool block) : Request(target,block,true) {} 00070 //! shallow copy constructor supported 00071 ReadRequest(const ReadRequest& r) : Request(r) {} 00072 //! shallow assignment supported 00073 ReadRequest& operator=(const ReadRequest& r) { Request::operator=(r); return *this; } 00074 }; 00075 //! retrieves the current WorldState entry, and through it's destructor, marks the entry available again when it goes out of scope 00076 /*! By default, when the release occurs, the entry is marked "complete" unless you have called setComplete(false) before then */ 00077 class WriteRequest : public Request { 00078 public: 00079 //! stores the oldest unreferenced WorldState into #tgt, optionally blocking if an update is currently in progress for the same frame 00080 WriteRequest(WorldState*& target, bool block, unsigned int frame_number) : Request(target,block,false), 00081 src(NULL), srcRequest(src,false), frame(frame_number), statusCache(0), completeCache(false) 00082 {} 00083 //! shallow copy constructor supported 00084 WriteRequest(const WriteRequest& r) : Request(r), src(r.src), srcRequest(r.srcRequest), frame(r.frame), statusCache(r.statusCache), completeCache(r.completeCache) {} 00085 //! shallow assignment supported 00086 WriteRequest& operator=(const WriteRequest& r) { src=r.src; srcRequest=r.srcRequest; frame=r.frame; statusCache=r.statusCache; completeCache=r.completeCache; Request::operator=(r); return *this; } 00087 00088 WorldState* src; //!< will be set to the previously written element, so you can copy over unmodified values 00089 ReadRequest srcRequest; //!< used to get #src 00090 unsigned int frame; //!< should be initialized to the frame number about to be written 00091 00092 unsigned int getStatus() const { return statusCache; } //!< returns the WorldStatePool::status value for the target WorldState entry (see documentation for WorldStatePool::status) 00093 void setStatus(unsigned int status) { statusCache=status; } //!< sets the WorldStatePool::status value for the target WorldState entry (see documentation for WorldStatePool::status) 00094 bool getComplete() const { return completeCache; } //!< returns the WorldStatePool::complete value for the target WorldState entry (see documentation for WorldStatePool::complete) 00095 void setComplete(bool complete) { completeCache=complete; } //!< returns the WorldStatePool::complete value for the target WorldState entry (see documentation for WorldStatePool::complete) 00096 00097 protected: 00098 unsigned int statusCache; //!< when using resource, this field is set to the status field for that entry, and when released, this value is stored back 00099 bool completeCache; //!< when using resource, this field is set to the complete flag for that entry, and when released, this value is stored back 00100 }; 00101 00102 //! constructor 00103 WorldStatePool(); 00104 00105 #ifdef PLATFORM_APERIOS 00106 //! returned by isUnread containing information parsed from the incoming message 00107 class UpdateInfo { 00108 public: 00109 //! constructor, sets #msg to NULL, #frameNumber to -1U 00110 UpdateInfo() 00111 #ifdef DEBUG 00112 : msg(NULL), frameNumber(-1U), intendedBuf(-1U) {} 00113 #else 00114 : msg(NULL), frameNumber(-1U) {} 00115 #endif 00116 OSensorFrameVectorData* msg; //!< incoming data 00117 unsigned int frameNumber; //!< serial number of the message 00118 #ifdef DEBUG 00119 unsigned int intendedBuf; //!< the write buffer read() is expected to use; This is for debugging, if this isn't the buffer selected, display warning 00120 #endif 00121 private: 00122 UpdateInfo(const UpdateInfo&); //!< not implemented 00123 UpdateInfo operator=(const UpdateInfo&); //!< not implemented 00124 }; 00125 00126 //! returns true if the process should call WorldState::read (i.e. @a msg has new or unprocessed data (such as motion needs to supply feedback)) 00127 /*! only one call to this can be made at a time per process (not threadsafe, but is multi-process safe) 00128 * @param msg the incoming sensor data from the system -- should be const, but accessor functions from Sony aren't marked const themselves :-/ 00129 * @param[out] lastFrameNumber if the incoming frame is already complete (no need to read), then the frame's number will be assigned here 00130 * @return returns a static UpdateInfo structure (to be passed to read()) if the message is unread, otherwise returns NULL. The structure is static -- DO NOT DELETE IT */ 00131 WorldStatePool::UpdateInfo* isUnread(OSensorFrameVectorData& msg, unsigned int& lastFrameNumber); 00132 00133 #else //PLATFORM_LOCAL 00134 //! flags for the status field on each WriteRequest -- tracks partial completion when multiple writers are involved 00135 enum status_t { 00136 SENSORS_APPLIED=1<<0, //!< bit flag signals sensor data has been applied to the write target 00137 FEEDBACK_APPLIED=1<<1 //!< bit flag signals motion feedback has been applied to the write target (only required if feedback is being generated) 00138 }; 00139 00140 //! returned by isUnread containing information parsed from the incoming message 00141 class UpdateInfo { 00142 public: 00143 //! constructor 00144 UpdateInfo() 00145 #ifdef DEBUG 00146 : verbose(0), frameNumber(-1U), filename(), dataInQueue(false), payload(NULL), payloadSize(0), intendedBuf(-1U) {} 00147 #else 00148 : verbose(0), frameNumber(-1U), filename(), dataInQueue(false), payload(NULL), payloadSize(0) {} 00149 #endif 00150 unsigned int verbose; //!< status of processing the message should be displayed 00151 unsigned int frameNumber; //!< serial number of the message 00152 std::string filename; //!< source of the data in the message 00153 bool dataInQueue; //!< sender indicates data is in the queue (if this is heartbeat, treat it as data) 00154 char* payload; //!< pointer to beginning of the data (NULL if no data available, i.e. heartbeat message) 00155 unsigned int payloadSize; //!< size of data (0 if no data available, i.e. heartbeat message) 00156 #ifdef DEBUG 00157 unsigned int intendedBuf; //!< the write buffer read() is expected to use; This is for debugging, if this isn't the buffer selected, display warning 00158 #endif 00159 private: 00160 UpdateInfo(const UpdateInfo&); //!< not implemented 00161 UpdateInfo operator=(const UpdateInfo&); //!< not implemented 00162 }; 00163 00164 //! returns true if the process should call read (i.e. @a msg has new or unprocessed data (such as motion needs to supply feedback)) 00165 /*! only one call to this can be made at a time per process (not threadsafe, but is multi-process safe) 00166 * @param msg incoming message to test 00167 * @param curFrameNumber the most recent frame number sent, i.e. SharedGlobals::MotionSimConfig::frameNumber 00168 * @param[out] lastFrameNumber if the incoming frame is already complete (no need to read), then the frame's number will be assigned here 00169 * @param haveFeedback you should pass true if motion feedback <em>can be</em> provided to read() (note the feedback doesn't necessarily need to be available right now, just if the call to read is necessary) 00170 * @param motionOverride true if motion feedback overrides sensor data (i.e SharedGlobals::MotionSimConfig::override) 00171 * @return returns a static UpdateInfo structure (to be passed to read()) if the message is unread, otherwise returns NULL. The structure is static -- DO NOT DELETE IT */ 00172 WorldStatePool::UpdateInfo* isUnread(const RCRegion& msg, unsigned int curFrameNumber, unsigned int& lastFrameNumber, bool haveFeedback, bool motionOverride); 00173 //! takes a sensor update from the simulator/system, updates an entry in the pool with the new information 00174 /*! If isUnread() returns non-NULL, you should acquire a WriteRequest, and check that it succeeds, is current, and is still incomplete, before calling read() 00175 * @param info the info structure returned by isUnread 00176 * @param wsw the WriteRequest aquired before calling read (yes, you should submit a WriteRequest for @a info.frameNumber between isUnread() and read()) 00177 * @param feedbackGenerated pass true if feedback will be generated by a caller to read() -- doesn't need to be <em>this</em> process, but <em>a</em> process. 00178 * @param zeroPIDFeedback pass true if sensor data should override motion feedback for joints with 0's for PID control (i.e. SharedGlobals::MotionSimConfig::zeroPIDFeedback) 00179 * @param feedback pointer to actual motion feedback, or NULL if not available in this process (weight values of feedback are ignored, assumes all values are valid) */ 00180 bool read(const UpdateInfo& info, WriteRequest& wsw, bool feedbackGenerated, bool zeroPIDFeedback, const PostureEngine* feedback=NULL); 00181 #endif 00182 00183 //! processes a request, passed as either a ReadRequest or WriteRequest, to access an entry in the pool 00184 virtual void useResource(Data& d) { doUseResource(d); } 00185 //! completes access to an entry in the pool, you must pass the same request instance here which you originally passed to useResource() 00186 virtual void releaseResource(Data& d) { doReleaseResource(d); } 00187 00188 //! does the actual work of useResource() 00189 /*! this is split off as a non-virtual function to avoid some process 00190 * identity issues that occur with virtual functions under Aperios */ 00191 void doUseResource(Data& d); 00192 //! does the actual work of releaseResource() 00193 /*! this is split off as a non-virtual function to avoid some process 00194 * identity issues that occur with virtual functions under Aperios */ 00195 void doReleaseResource(Data& d); 00196 00197 #ifdef PLATFORM_APERIOS 00198 //! registers #stateLookupMap with WorldState::setWorldStateLookup() 00199 void InitAccess() { WorldState::setWorldStateLookup(stateLookupMap); } 00200 #endif 00201 00202 protected: 00203 //! number of buffers to set up 00204 static const unsigned int NUM_STATES=WORLDSTATEPOOL_NUM_STATES; 00205 00206 //! shorthand for the type of #order 00207 typedef ListMemBuf<unsigned int, NUM_STATES> order_t; 00208 //! indicies of entries, in the order they were written (i.e. corresponding value in #frames should be monotonically increasing) 00209 order_t order; 00210 00211 //! shorthand to test if all three P, I, and D values are 0 for the specified joint index (relative to 0, not PIDJointOffset) 00212 static bool isZeroPID(WorldState* s, unsigned int i) { return i>=PIDJointOffset && i<PIDJointOffset+NumPIDJoints && s->pids[i][0]==0 && s->pids[i][1]==0 && s->pids[i][2]==0; } 00213 00214 //! called when access to an entry for reading is requested 00215 unsigned int getCurrentReadState(WorldState*& tgt, bool block); 00216 //! called when an read access to an entry is complete 00217 void doneReadingState(unsigned int i); 00218 00219 //! returns true if the specified element of #states has been marked completed 00220 bool isComplete(unsigned int idx) const; 00221 //! returns index of buffer in #states to use for write request 00222 unsigned int selectWriteState(unsigned int frame, bool block) const { order_t::index_t idx; return selectWriteState(frame,block,idx); } 00223 //! returns index of buffer in #states to use for write request, stores index of corresponding entry of #order in @a idx 00224 unsigned int selectWriteState(unsigned int frame, bool block, order_t::index_t& idx) const; 00225 //! called when access to an entry for writing is requested 00226 unsigned int getCurrentWriteState(WorldState*& tgt, unsigned int frame, bool block); 00227 //! called when an write access to an entry is complete 00228 void doneWritingState(unsigned int i, bool completed); 00229 00230 //! entries to hand out 00231 WorldState states[NUM_STATES]; 00232 //! serial numbers of corresponding entries in #states, set when writing begins, should be monotonically increasing relative to #order (i.e. if you run through #order and look at corresponding values in #frames, should be monotonically increasing serial numbers) 00233 unsigned int frames[NUM_STATES]; 00234 //! flag set when a reader is blocking for writing to finish, until read is satisified 00235 unsigned int reading[NUM_STATES]; 00236 //! count of writers in line for access (occurs when a writer is blocking on another writer of the same frame, or no other frames are free) 00237 unsigned int writing[NUM_STATES]; 00238 //! the status is intended as a bitfield to support communication between writers if they need to cooperatively fill out an entry 00239 /*! The value is set to 0 before handing out to a writer with a new frame number */ 00240 unsigned int status[NUM_STATES]; 00241 #ifdef PLATFORM_APERIOS 00242 //! this lock indicates/controls whether the state is available for reading 00243 /*! The lock is set before handing out to a writer with a new frame number, and released 00244 * when a writer has marked the entry complete (via the WriteRequest upon releaseResource()) */ 00245 MutexLock<ProcessID::NumProcesses> complete[NUM_STATES]; 00246 #else 00247 //! this semaphore is set to positive value when writing begins, and then lowered to zero when complete 00248 /*! A semaphore is used on "normal" systems because the MutexLock also implies a 00249 * thread lock, which we actually don't want in this case because a different thread 00250 * may complete the entry than the one which started it. Since Aperios does not allow 00251 * multithreading, we don't have to worry about it there. 00252 * 00253 * As an aside, we @e could use this to store #writing, but that would only be feasible if 00254 * Aperios gave us semaphores. Bah. */ 00255 SemaphoreManager::semid_t complete[NUM_STATES]; 00256 #endif 00257 00258 //! locks to be acquired before handing out corresponding #states entry for writing 00259 MutexLock<ProcessID::NumProcesses> writeLocks[NUM_STATES]; 00260 //! lock on WorldStatePool's own members 00261 MutexLock<ProcessID::NumProcesses> lock; 00262 00263 #ifdef PLATFORM_APERIOS 00264 //! the current state in use by each process 00265 WorldState* stateLookupMap[ProcessID::NumProcesses]; 00266 #endif 00267 00268 private: 00269 WorldStatePool(const WorldStatePool&); //!< this shouldn't be called... 00270 WorldStatePool& operator=(const WorldStatePool&); //!< this shouldn't be called... 00271 }; 00272 00273 /*! @file 00274 * @brief 00275 * @author Ethan Tira-Thompson (ejt) (Creator) 00276 * 00277 * $Author: ejt $ 00278 * $Name: tekkotsu-4_0 $ 00279 * $Revision: 1.19 $ 00280 * $State: Exp $ 00281 * $Date: 2007/11/11 23:57:26 $ 00282 */ 00283 00284 #endif |
Tekkotsu v4.0 |
Generated Thu Nov 22 00:54:57 2007 by Doxygen 1.5.4 |