Tekkotsu Homepage
Dev. Resources


Go to the documentation of this file.
00001 //-*-c++-*-
00002 #ifndef INCLUDED_DataSource_h_
00003 #define INCLUDED_DataSource_h_
00005 //#include "Shared/RobotInfo.h" // only needed for some debugging output below
00006 #include "IPC/Thread.h"
00007 #include "IPC/MessageQueue.h"
00008 #include "Shared/debuget.h"
00009 #include "Shared/get_time.h"
00010 #include "Shared/RobotInfo.h"
00011 #include "Shared/plistPrimitives.h"
00012 #include <string>
00013 #include <iostream>
00014 #include <list>
00016 class RCRegion;
00017 struct SensorState;
00019 //! abstract base class for simulator data sources
00020 /*! Each subclass will implement loading data from some piece of hardware or network protocol.
00021  *
00022  *  The general flow of calls is:
00023  *  - constructor - user creates an instance of a DeviceDriver (which creates the data source)
00024  *  - registerSource() - the user selects the data source via SharedGlobals::StreamSimConfig::sources
00025  *  - enteringRealtime() - if the simulator enters real time mode
00026  *  - leavingRealtime() - if the simulator is paused, set to full-speed mode, or begins to shut down
00027  *  - deregisterSource() - simulator is shutting down or otherwise deleting associated DeviceDriver
00028  *  - destructor - deleting associated DeviceDriver
00029  *
00030  *  If your data source provides sensor data for current output values, you should call
00031  *  providingOutput() for those outputs when registerSource() is called, and ignoringOutput()
00032  *  when you are no longer active (deregisterSource() or destructor is called).
00033  *  This prevents the Motion process from clobbering your readings with its own feedback.
00034  */
00035 class DataSource {
00036 public:
00037   DataSource() : regions(), frozen(false), imageQueue(NULL) {} //!< constructor
00038   DataSource(const DataSource& ds) : regions(ds.regions), frozen(ds.frozen), imageQueue(ds.imageQueue) {} //!< copy constructor, just in case your subclass wants it
00039   DataSource& operator=(const DataSource&) { return *this; } //!< assignment operator, just in case your subclass wants it
00040   virtual ~DataSource(); //!< destructor
00042   //! Returns the simulator time of the next data segment.
00043   /*! Should be in the future if nothing new since last data segment, otherwise should be the 
00044    *  timestamp of the most recent data segment (older segments are skipped), return -1U if there is no more data */
00045   virtual unsigned int nextTimestamp()=0;
00047   //! Returns a descriptive name of the next data segment for user feedback (e.g. a filename for logged data).
00048   /*! Just use your class name if you don't have a useful name for individual samples. */
00049   virtual const std::string& nextName()=0;
00051   //! Called when the simulator is stepping while paused or advancing a frozen data source, return true if successful, or false if no more data is available
00052   virtual bool advance()=0;
00054   //! Called by simulator if the user pauses a data source; calls doFreeze() or doUnfreeze() as appropriate
00055   /*! You probably don't want to override this function -- that's what doFreeze() doUnfreeze are for! */
00056   virtual void setFrozen(bool fr) { if(fr==frozen) return; frozen=fr; if(getTimeScale()>0) { if(frozen) doFreeze(); else doUnfreeze(); } }
00057   virtual bool getFrozen() const { return frozen; } //!< returns #frozen status
00059   //! if called, indicates a request to restart/reinitialize the data source
00060   /*! For example, a FileSystemDataSource would go back to the beginning of its list,
00061    *  and a network-based source would close and reopen the connection */
00062   virtual void reset() {}
00064   //! User hook, called when the data source should claim which outputs it provides feedback (providingOuput())
00065   /*! Does not indicate the data source should start sending updates yet — wait for enteringRealtime() or advance() to be called */
00066   virtual void registerSource() {}
00068   //! User hook, called when the data source should release its claim on outputs with feedback (ignoringOuput()).
00069   /*! It would be wise to call this from your destructor as well. */
00070   virtual void deregisterSource() {}
00072   //! User hook, called when the controller is going to be running in realtime mode, which is probably the normal mode you'd expect.
00073   /*! You might be in realtime mode, but a debugger breakpoint will still pause things, or thread scheduling could hiccup, so try to be robust.\n
00074    *  The argument is a reference to SharedGlobals::timeScale, so the data source can subscribe to changes in
00075    *  simulation speed if it can use that information.  (We avoid direct dependency on the tekkotsu simulator
00076    *  so this code can be reused for other tools too.) */
00077   virtual void enteringRealtime(const plist::Primitive<double>& /*simTimeScale*/) { if(!frozen) doUnfreeze(); }
00079   //! User hook, called when leaving realtime mode, which means you have no idea when motionCheck() is going to be called in terms of wall-clock time.
00080   /*! Argument set to true if entering full speed mode, which indicates everything should run
00081    *  at full native "frame rate", and may indicate more data will be processed than normal, CPU speed permitting.
00082    *  However, if false, almost certainly indicates updates will be sparse, trigger by user 'step' commands.
00083    *  May be called multiple times if changing between full-speed mode and paused
00084    *
00085    *  A non-realtime mode might be triggered if the user wants to pause the simulator/controller to step through something...
00086    *  No guarantees though!  The debugger might catch a breakpoint and stop things, and this won't be called! */
00087   virtual void leavingRealtime(bool /*isFullSpeed*/) { if(!frozen) doFreeze();  }
00089   //! Called by simulator during initialization to tell DataSources where the array of sensor values are stored (see #sensorState)
00090   /*! This would need to point into a shared memory region if using multi-process model, hence we can't just
00091    *  use process-local static allocation. */
00092   static void setSensorState(SensorState* senState) { sensorState=senState; }
00094   //! Called by simulator during initialization to tell the DataSource where it should send image data (if this is selected as an active image data source).
00095   /*! Each image data source (if we get around to supporting such concurrent sources) will have a separate message queue */
00096   virtual void setImageQueue(MessageQueueBase* mq) { imageQueue=mq; }
00098   //! will be called by initialization code prior to first getData() if client code is going to block on getting the first sensor reading
00099   static void setNeedsSensor(bool waiting) { requiresFirstSensor=waiting; }
00101   //! will be called by initialization code to provide a pointer to the sensor synchronization framerate #sensorFramerate
00102   static void setSensorFramerate(const plist::Primitive<float>* senFR) { sensorFramerate = senFR; }
00104   //! This structure should be written into the beginning of each image buffer sent via setImage().
00105   /*! Using "placement new" avoids doing a memcpy: <code>new (buffer) ImageHeader(...);</code>\n
00106    *  If you are storing a visual image to be processed by the pipeline (vs. a laser rangefinder sweep or such), then
00107    *  values should be interleaved YUV samples. */
00108   struct ImageHeader {
00109     //! full constructor, see corresponding members for documentation on arguments
00110     ImageHeader(unsigned int sourceID_, int layer_, unsigned int width_, unsigned int height_, unsigned int components_, unsigned int frameNumber_, unsigned int timestamp_, const std::string& name_)
00111     : sourceID(sourceID_), layer(layer_), width(width_), height(height_), components(components_), frameNumber(frameNumber_), timestamp(timestamp_) { strncpy(name,name_.c_str(),MAX_NAME_LEN); }
00113     unsigned int sourceID; //!< In order to support multiple cameras, this tracks how to identify the cameras within Tekkotsu.  You should allow users to configure the value you store here.
00114     int layer; //!< the resolution layer for this image, if 0 then automatically chooses the closest in size based on expected values in RobotInfo namespace.  Otherwise should be presented as a configuration item.
00115     unsigned int width; //!< pixels per row of image
00116     unsigned int height; //!< pixels per column of image
00117     unsigned int components; //!< number of color channels, vision pipeline expects 3 for YUV images.
00119     unsigned int frameNumber; //!< serial number per image sent, provides user notification for dropped images if the SharedGlobals::StreamSimConfig::verbose flag is set
00120     unsigned int timestamp; //!< timestamp image was recorded
00121     static const unsigned int MAX_NAME_LEN=64; //!< maximum length of #name
00122     char name[MAX_NAME_LEN]; //!< a user identifiable name for the image... see DataSource::nextName()
00123   };
00125   //! returns the pending output value, which may not have been processed by the framework yet
00126   static float getOutputValue(unsigned int i);
00128   //! returns the pending sensor value, which may not have been processed by the framework yet
00129   static float getSensorValue(unsigned int i);
00131   //! returns the pending button value, which may not have been processed by the framework yet
00132   static float getButtonValue(unsigned int i);
00134   //! returns the pending duty cycle value, which may not have been processed by the framework yet
00135   static float getPIDDutyValue(unsigned int i);
00137 protected:
00138   //! subclasses should pass this to a MarkScope instance before they begin calling the static setXXXValue functions to prevent concurrent or partial updates
00139   static Resource& getSensorWriteLock();
00141   //! subclasses should call this if they provide sensor updates which will contain a measurement of the current position of output @a i.
00142   /* A DataSource should consider itself providing an output if it will be sending some kind of measurement of
00143    *  the current value of an output, and has been assigned to a LoadDataThread
00144    *  (i.e. setDataSourceThread() has been called and #thread is non-NULL).\n
00145    *  This prevents the motion process from clobbering your readings with its own feedback.  */
00146   static void providingOutput(unsigned int i);
00148   //! subclasses should call this if they provided sensor updates containing the current position of output @a i, but are not going to do so any longer.
00149   /*! You don't need to call this if you didn't previously call providingOutput(). */
00150   static void ignoringOutput(unsigned int i);
00152   //! sets the current value for an output, be sure to register with providingOutput() (and later ignoringOutput()) in order to avoid being clobbered by "blind" feedback from the Motion process
00153   static void setOutputValue(unsigned int i, float v);
00155   //! sets the current value for a sensor
00156   static void setSensorValue(unsigned int i, float v);
00158   //! sets the current value for a button
00159   static void setButtonValue(unsigned int i, float v);
00161   //! sets the current duty cycle value for a servo joint, @a i should be relative to PIDJointOffset
00162   static void setPIDDutyValue(unsigned int i, float v);
00164   //! Returns a counter which is incremented each time the sensor data is copied into the framework
00165   /*! You could use this to determine if user code has gotten a chance to see a detection flag of some sort before clearing it again */
00166   static unsigned int getSensorFrameNumber();
00168   //! Sets the current image, for bulk data sources like cameras.
00169   /*! You should not modify data in @a region until you pass a new @a region here.
00170    *  However even then, the previous region may still be in processing.  Check
00171    *  region reference counts to determine when an old region is available for
00172    *  recycling, or otherwise dereference them after calling this so they will
00173    *  be deallocated after use.
00174    *
00175    *  Image data should be in 8-bit per channel interleaved format and the data
00176    *  should be prepended by an ImageHeader structure so the receiver knows how to
00177    *  interpret the data.
00178    *
00179    *  Recommended to just use getUnusedRegion() to handle region recycling for you. */
00180   void setImage(RCRegion* region) {
00181     ASSERTRET(imageQueue!=NULL,"DataSource::setImage called without imageQueue");
00182     imageQueue->sendMessage(region);
00183   }
00185   //! sets the current image, for bulk data sources like cameras
00186   /*! This version copies the data into an RCRegion and calls setImage(RCRegion*).  For efficiency with
00187    *  large blocks of data, if possible you should use getUnusedRegion and write your results directly
00188    *   there to call setImage(RCRegion*) instead and save a copy if possible.
00189    *
00190    *  Image @a data should be in 8-bit per channel interleaved format. */
00191   void setImage(const ImageHeader& header, const void * data);
00193   //! Finds a one-reference entry in #regions with capacity at least @a minSize, creating a new one of size @a minSize + @a padding if necessary.
00194   /*! If the first one-reference entry is too small, it will be freed and a new one created instead. */
00195   virtual RCRegion* getUnusedRegion(size_t minSize, size_t padding);
00197   std::list<RCRegion*> regions; //!< for efficiency, reuse old buffers via getUnusedRegion() -- oldest at front, most recently used at back
00199   virtual void doFreeze() {} //!< user hook for when #frozen is set to true; advance() will be called by simulator at user discretion.
00200   virtual void doUnfreeze() {} //!< user hook for when #frozen is set to false; if enteringRealtime() has been called then you should resume sending data.
00201   bool frozen;  //!< indicates that data is going to be requested "sparsely"; advance() will be called by simulator at user discretion
00203   //! if true, indicates that client code is going to wait for sensor readings returned by getData before executing
00204   static bool requiresFirstSensor;
00206   //! Assigned by setSensorFramerate() during initialization, points to the synchronization framerate for sensors.
00207   /*! It is pointless to exceed this framerate for sensor updates, so if you have control over the polling rate, set it to this value. */
00208   static const plist::Primitive<float>* sensorFramerate;
00210 private:
00211   static SensorState * sensorState;
00213   //! assigned by setImageQueue(), calls to setImage() will send messages into this queue
00214   MessageQueueBase* imageQueue;
00215 };
00218 //! A global instance of this structure is registered via DataSource::setSensorState() during launch, so that DataSources know where to put their results
00219 /*! Subclasses of DataSource don't access this structure directly, they should call the static DataSource functions to write its fields.
00220  *  Image based data sources like cameras will send raw buffers of data via DataSource::setImage, ignoring this structure. */
00221 struct SensorState : public Resource {
00222   //! constructor
00223   SensorState();
00225   //! returns true if any of #providedOutputs is greater than zero
00226   bool hasProvidedOutput() const { for(unsigned int i=0; i<NumOutputs; i++) if(providedOutputs[i]>0) return true; return false; }
00227   //! returns true if any of #providedOutputs is zero
00228   bool hasUnprovidedOutput() const { for(unsigned int i=0; i<NumOutputs; i++) if(providedOutputs[i]==0) return true; return false; }
00230   //! Counts the number of sensor data sources which are providing readings for each output
00231   /*! This isn't a configuration setting per se, but needed so motion process can tell if it
00232    *  should provide feedback for each output.  If an output doesn't have any sensor feedback
00233    *  (or #override is true), then motion should provide feedback.  If more than one
00234    *  sensor is providing the same output, that could be a problem, but not dealt with here.
00235    *
00236    *  The simulator's initialization routines will pass this to DataSource::setOutputTracker(). */
00237   unsigned int providedOutputs[NumOutputs];
00239   float outputs[NumOutputs];     //!< maps to WorldState::outputs, assign via DataSource::setOutputValue
00240   float buttons[NumButtons];     //!< maps to WorldState::buttons, assign via DataSource::setButtonValue
00241   float sensors[NumSensors];     //!< maps to WorldState::sensors, assign via DataSource::setSensorValue
00242   float pids[NumPIDJoints][3];   //!< maps to WorldState::pids, only assigned via MotionExecThread
00243   float pidduties[NumPIDJoints]; //!< maps to WorldState::pidduties, assign via DataSource::setPIDDutyValue
00245   unsigned int timestamp; //!< simulator time of last update to one of the value arrays
00247   //! Serial number for the update, incremented each time Simulator sends an update notification.
00248   /*! This is not incremented for each DataSource modification because there might be multiple data sources,
00249    *  or a data source might make multiple partial updates within each Simulator frame. */
00250   unsigned int frameNumber;
00252   //! this flag should be set when any of the fields are updated, cleared when releaseResource is called (when timestamp is set instead)
00253   bool dirty;
00255   //! This may be set to point to the Motion.OverrideSensors configuration setting, causing sensor updates for outputs to be ignored (forcing open loop)
00256   /*! This is useful if loading sensor data from log, where we want to compute new output positions in simulation, but using the "pure" sensors for input */
00257   plist::Primitive<bool>* motionOverride;
00259   // ! signals when a sensor field is updated so things like full speed mode and waiting for the first sensor update can work
00260   /*Thread::Condition updateSignal;*/
00262   //! a function to be called whenever releaseResource() is called and dirty flag is set; called at the end once the resource is immediately available
00263   void (*resourceSync)();
00265 protected:
00266   virtual void useResource(Data& d) {
00267     static_cast<Resource&>(lock).useResource(d);
00268   }
00269   virtual void releaseResource(Data& d);
00271   //! lock to prevent concurrent access to the structure, but should use the SensorState with MarkScope to set the dirty flag on unlock
00272   Thread::Lock lock;
00274 private:
00275   SensorState(const SensorState&); //!< don't call
00276   SensorState& operator=(const SensorState&); //!< don't call
00277 };
00279 inline Resource& DataSource::getSensorWriteLock() { return *sensorState; }
00280 inline void DataSource::setOutputValue(unsigned int i, float v) {
00281   if(sensorState->motionOverride!=NULL && *sensorState->motionOverride)
00282     return;
00283   sensorState->outputs[i]=v;
00284   sensorState->dirty=true;
00285 }
00286 inline float DataSource::getOutputValue(unsigned int i) { return sensorState->outputs[i]; }
00288 inline void DataSource::setSensorValue(unsigned int i, float v) { sensorState->sensors[i]=v; sensorState->dirty=true; }
00289 inline float DataSource::getSensorValue(unsigned int i) { return sensorState->sensors[i]; }
00291 inline void DataSource::setButtonValue(unsigned int i, float v) { sensorState->buttons[i]=v; sensorState->dirty=true; }
00292 inline float DataSource::getButtonValue(unsigned int i) { return sensorState->buttons[i]; }
00294 inline void DataSource::setPIDDutyValue(unsigned int i, float v) { sensorState->pidduties[i]=v; sensorState->dirty=true; }
00295 inline float DataSource::getPIDDutyValue(unsigned int i) { return sensorState->pidduties[i]; }
00297 inline unsigned int DataSource::getSensorFrameNumber() { return sensorState->frameNumber; }
00299 /*! @file
00300  * @brief Defines DataSource, an abstract base class for simulator data sources
00301  * @author Ethan Tira-Thompson (ejt) (Creator)
00302  */
00304 #endif

Tekkotsu Hardware Abstraction Layer 5.1CVS
Generated Mon May 9 05:01:38 2016 by Doxygen 1.6.3