Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

WorldStatePool.h

Go 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