Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

WorldState.h

Go to the documentation of this file.
00001 //-*-c++-*-
00002 #ifndef INCLUDED_WorldState_h
00003 #define INCLUDED_WorldState_h
00004 
00005 #ifdef PLATFORM_APERIOS
00006 #include <OPENR/core_macro.h>
00007 #include <OPENR/ObjcommTypes.h>
00008 #include <OPENR/OPENR.h>
00009 #include <OPENR/OPENRAPI.h>
00010 #include <OPENR/OPENRMessages.h>
00011 #else
00012 class PostureEngine;
00013 #endif
00014 
00015 #include "Shared/RobotInfo.h"
00016 #include "Events/EventBase.h"
00017 #include "IPC/ProcessID.h"
00018 #include <math.h>
00019 #include <vector>
00020 
00021 class EventRouter;
00022 class EventBase;
00023 
00024 //The following SourceIDs are for events created by WorldState's event generators
00025 
00026 //! holds source ID types for sensor events; see EventBase, see #SensorSourceID_t
00027 namespace SensorSrcID {
00028   //! holds source ID types for sensor events
00029   /*! May want to add a proximity alarm for IR distance?  Probably
00030    *  should do it from a separate generator to avoid screwing up
00031    *  behaviors relying on the current setup
00032    */
00033   enum SensorSourceID_t {
00034     UpdatedSID //!< sends status event as last event after processing a frame
00035   };
00036 }
00037 
00038 //! holds source ID types for power events; see EventBase, see #PowerSourceID_t
00039 namespace PowerSrcID {
00040   //! holds source ID types for power events
00041   /*! Also serve as offsets into WorldState::powerFlags[].
00042    *
00043    *  I've never seen a lot of these events thrown by the OS.  'NS'
00044    *  means never-seen, which could simply be because i haven't put it
00045    *  in that situation (don't have a station-type power charger) or
00046    *  because the OS doesn't actually support sending that flag.
00047    *
00048    *  Under normal conditions, you'll see MotorPowerSID,
00049    *  BatteryConnectSID, DischargingSID, and PowerGoodSID always
00050    *  active with occasional VibrationSID and UpdateSID.  When the
00051    *  chest button is pushed, PauseSID is activated and MotorPowerSID
00052    *  is deactivated.
00053    *
00054    *  The BatteryMonitorBehavior will give a warning once power begins
00055    *  getting low.  The OS won't boot off a battery with less than 15%
00056    *  power remaining (which is when the LowPowerWarnSID is thrown)
00057    *
00058    *  @note there's not a one-to-one correspondance of the events from
00059    *  the OPENR power system... i map several OPENR events to fewer
00060    *  Tekkotsu events, check the event's name if you want to know the
00061    *  specific source (say if low battery is low current and/or low
00062    *  voltage) Status ETIDS are only generated when one of a related
00063    *  group goes on/off but others are still active
00064    */
00065   enum PowerSourceID_t {
00066     PauseSID=0, //!< the chest button was pushed (this is not a normal button, it kills power to the motors in hardware)
00067     MotorPowerSID, //!< active while the motors have power
00068     VibrationSID, //!< triggered when the OS decides a large acceleration has occured, like falling down (or more specifically, hitting the ground afterward)
00069     BatteryEmptySID, //!< battery is dead
00070     LowPowerWarnSID, //!< triggered when sensors[PowerRemainOffset] <= 0.15 (PowerGoodSID stays on)
00071     BatteryFullSID,  //!< battery is full
00072     ExternalPowerSID, //!< receiving power from an external source (such as AC cable, may or may not include the "station", see StationConnectSID)
00073     ExternalPortSID,  //!< an external power source is plugged in (does not imply current is flowing however)
00074     BatteryConnectSID, //!< a battery is plugged in
00075     BatteryInitSID, //!< ? NS
00076     DischargingSID, //!< using power from the battery (although still stays on after hooked up to external power)
00077     ChargingSID, //!< you used to be able to charge while running, tho that has changed in more recent versions of OPEN-R.  In any case, I never saw this even when it did work.
00078     OverheatingSID, //!< in case the robot starts getting too hot NS
00079     PowerGoodSID, //!< there is power, either from external or battery
00080     ChargerStatusSID, //!< ? NS
00081     SuspendedSID, //!< ? NS
00082     OverChargedSID, //!< in case the charger screws up somehow (?) NS
00083     TermDischargeSID, //!< end of battery (?) NS
00084     TermChargeSID, //!< end of charging (?) NS
00085     ErrorSID, //!< general power error NS
00086     StationConnectSID, //!< connected to a station NS
00087     BatteryOverCurrentSID, //!< similar to OverChargedSID (?) NS
00088     DataFromStationSID, //!< ? NS
00089     RegisterUpdateSID, //!< ? NS
00090     RTCSID, //!< ? NS
00091     SpecialModeSID, //!< ? NS
00092     BMNDebugModeSID, //!< ? NS
00093     PlungerSID, //!< I think this is in reference to having a memorystick (?) NS
00094     UpdatedSID, //!< sent as last event after processing a frame
00095     NumPowerSIDs
00096   };
00097 }
00098 
00099 class WorldState;
00100 #ifdef PLATFORM_APERIOS
00101 extern WorldState * state; //!< the global state object, points into a shared memory region, created by MainObject
00102 #else
00103 //! This class masquerades as a simple WorldState pointer, but actually checks the process ID of the referencing thread to allow each thread group to have a separate WorldState*
00104 /*! This is to support WorldStatePool functionality, so if a behavior in Main is blocking, it doesn't prevent Motion threads from getting updated sensors */
00105 class WorldStateLookup {
00106 public:
00107   WorldStateLookup() { for(unsigned int i=0; i<ProcessID::NumProcesses; ++i) ws[i]=NULL; } //!< constructor
00108   WorldState*& operator->() { return ws[ProcessID::getID()]; } //!< smart pointer to the underlying class
00109   WorldState& operator*() { return *ws[ProcessID::getID()]; } //!< smart pointer to the underlying class
00110   operator WorldState*&() { return ws[ProcessID::getID()]; } //!< pretend we're a simple pointer
00111   WorldStateLookup& operator=(WorldState* p) { ws[ProcessID::getID()]=p; return *this; } //!< assign from a pointer as well
00112   
00113 protected:
00114   //! This holds a separate WorldState pointer for each process
00115   /*! Note that under a multi-process model, each process is only ever going to reference one of these,
00116    *  (so we could get away with a single global pointer), but under a uni-process model, we wind up
00117    *  using the various entries to differentiate the thread groups */
00118   WorldState* ws[ProcessID::NumProcesses];
00119 private:
00120         WorldStateLookup(const WorldStateLookup&); //!< don't call this
00121         WorldStateLookup& operator=(const WorldStateLookup&); //!< don't call this
00122 };
00123 //! the global state object, points into a shared memory region, created by MainObject
00124 extern WorldStateLookup state;
00125 #endif
00126 
00127 //! The state of the robot and its environment
00128 /*! Contains sensor readings, current joint positions, etc.  Notable members are:
00129  * - #outputs - joint positions and current LED values
00130  * - #buttons - current button values
00131  * - #sensors - values from other sensors (IR, acceleration, temperature, power levels)
00132  * - #pids - current PID settings for each joint (more specifically, each PID-controlled joint)
00133  * - #pidduties - how hard each of the PID joints is working to get to its target value
00134  *
00135  * Generally you will use enumerated values from a robot-specific namespace to
00136  * index into these arrays.  Each of those members' documentation specifies where
00137  * to find the list of indices to use with them.
00138  *
00139  * This is a shared memory region between Main, Motion, and possibly others in the future.
00140  * Be very careful about including structures that use pointers in
00141  * this class... they will only be valid from the OObject that created
00142  * them, others may cause a crash if they try to access them.
00143  *
00144  * WorldState takes power and sensor updates from the system and
00145  * maintains the last known values in its member fields.  It throws
00146  * events when some of these values change, listed in the
00147  * SensorSourceID, PowerSourceID namespaces, and the ButtonOffset_t
00148  * enumeration for your robot model's info
00149  * namespace. (e.g. ERS7Info::ButtonOffset_t)
00150  *
00151  * Status events for buttons only generated if the
00152  * WorldState::alwaysGenerateStatus flag is turned on.  Otherwise, by
00153  * default, they are only generated when a value has changed.
00154  * (i.e. when the pressure sensitive buttons get a new pressure
00155  * reading)
00156  */
00157 class WorldState {
00158 public:
00159   //! constructor - sets everything to zeros
00160   WorldState();
00161 
00162   bool alwaysGenerateStatus; //!< controls whether status events are generated for the boolean buttons
00163 
00164   float outputs[NumOutputs];     //!< last sensed positions of joints, last commanded value of LEDs; indexes (aka offsets) are defined in the target model's namespace (e.g. "Output Offsets" section of ERS7Info)
00165   float buttons[NumButtons];     //!< magnitude is pressure for some, 0/1 for others; indexes are defined in the ButtonOffset_t of the target model's namespace (e.g. ERS7Info::ButtonOffset_t)
00166   float sensors[NumSensors];     //!< IR, Accel, Thermo, Power stuff; indexes are defined in SensorOffset_t of the target model's namespace (e.g. ERS7Info::SensorOffset_t)
00167   float pids[NumPIDJoints][3];   //!< current PID settings (same ordering as the #outputs), not sensed -- updated by MotionManager whenever it sends a PID setting to the system; note this is only valid for PID joints, has NumPIDJoint entries (as opposed to NumOutputs)
00168   float pidduties[NumPIDJoints]; //!< duty cycles - -1 means the motor is trying to move full power in negative direction, 1 means full power in positive direction, in practice, these values stay rather small - 0.15 is significant force. (same ordering as the #outputs); note this is only valid for PID joints, has NumPIDJoint entries (as opposed to NumOutputs)
00169   
00170   float vel_x; //!< the current, egocentric rate of forward locomotion, mm/second
00171   float vel_y; //!< the current, egocentric rate of sideways (leftward is positive) locomotion, mm/second
00172   float vel_a; //!< the current, egocentric rate of rotational (counterclockwise is positive) locomotion, radian/second
00173   unsigned int vel_time; //!< the time at which we began moving along the current velocity vector
00174 
00175   unsigned int robotStatus;       //!< bitmask, see OPENR/OPower.h
00176   unsigned int batteryStatus;     //!< bitmask, see OPENR/OPower.h
00177   unsigned int powerFlags[PowerSrcID::NumPowerSIDs]; //!< bitmasks of similarly-grouped items from previous two masks, corresponds to the PowerSrcID::PowerSourceID_t's
00178 
00179   unsigned int button_times[NumButtons]; //!< value is time of current press, 0 if not down
00180   
00181   unsigned int lastSensorUpdateTime;     //!< primarily so calcDers can determine the time difference between updates, but others might want to know this too...
00182   unsigned int frameNumber;              //!< the serial number of the currently available frame
00183   unsigned int framesProcessed;          //!< the number of sensor updates which have been processed
00184 
00185   static const double g;                 //!< the gravitational acceleration of objects on earth
00186   static const double IROORDist;         //!< If IR returns this, we're out of range
00187 
00188 #ifdef PLATFORM_APERIOS
00189   void read(OSensorFrameVectorData& sensor, WorldState* lastState, EventRouter* er); //!< will process a sensor reading as given by OPEN-R
00190   void read(const OPowerStatus& power, EventRouter* er);      //!< will process a power status update as given by OPEN-R
00191   
00192   //! returns the current WorldState instance for the running process, needed if your code may be in a shared memory region; otherwise you can directly access ::state
00193   /*! Generally you can access ::state directly, but if your code is running as a member of an object in a shared memory region,
00194    *  this handles the shared object context switching problem on Aperios, and simply returns ::state on non-Aperios. */
00195   static WorldState* getCurrent() { return stateLookupMap==NULL ? NULL : stateLookupMap[ProcessID::getID()]; }
00196   
00197   // sets the current WorldState instance for the running process (handles the shared object context switching problem on Aperios, simply returns ::state on non-Aperios)
00198   // static void setCurrent(WorldState* st) { stateLookupMap[ProcessID::getID()]=st; }
00199   
00200   //! sets the lookup table (held in a shared region, WorldStatePool::stateMap) used by getCurrent under Aperios
00201   static void setWorldStateLookup(WorldState * curMap[ProcessID::NumProcesses]) { stateLookupMap=curMap; }
00202 #else
00203   void read(const PostureEngine& pose, WorldState* lastState, EventRouter* er); //!< will process the posture's output positions as a new sensor reading
00204   
00205   //! returns the current WorldState instance for the running process, needed if your code may be in a shared memory region; otherwise you can directly access ::state
00206   /*! Generally you can access ::state directly, but if your code is running as a member of an object in a shared memory region,
00207    *  this handles the shared object context switching problem on Aperios, and simply returns ::state on non-Aperios. */
00208   static WorldState* getCurrent() { return ::state; }
00209 #endif
00210 
00211 protected:
00212   unsigned int curtime; //!< set by read(OSensorFrameVectorData& sensor, EventRouter* er) for chkEvent() so each call doesn't have to
00213 
00214   //! Tests to see if the button status has changed and post events as needed
00215   void chkEvent(std::vector<EventBase>& evtBuf, unsigned int off, float newval, const char* name);
00216   //! Tests to see if any button statuses have changed and post events as needed, bases comparison between already filled-in WorldStates 
00217   void chkEvent(std::vector<EventBase>& evtBuf, WorldState* lastState);
00218 
00219   //! Apply calibration to the sensors and joint positions (reversing motion calibration parameters)
00220   void applyCalibration();
00221 
00222   //! sets the names of the flags that will be generating events
00223   /*! note that this function does not actually do the event posting,
00224    *  unlike chkEvent() */
00225   void chkPowerEvent(unsigned int sid, unsigned int cur, unsigned int mask, const char* name, 
00226                      std::string actname[PowerSrcID::NumPowerSIDs],
00227                      std::string dename[PowerSrcID::NumPowerSIDs],
00228                      unsigned int summask[PowerSrcID::NumPowerSIDs]) {
00229     if(cur&mask) {
00230       actname[sid]+=name;
00231       summask[sid]|=mask;
00232     } else if(powerFlags[sid]&mask)
00233       dename[sid]+=name;
00234   }
00235 
00236   //! given the next value, calculates and stores the next current, the velocity, and the acceleration
00237   /*! @param next the new value that's about to be set
00238    *  @param cur the previous value
00239    *  @param vel the previous 1st derivative
00240    *  @param acc the previous 2nd derivative
00241    *  @param invtimediff @f$1/(curtime-prevtime)@f$ in seconds*/
00242   inline void calcDers(double next, double& cur, double& vel, double& acc, double invtimediff) {
00243     double diff=next-cur;
00244     cur=next;
00245     next=diff*invtimediff;;
00246     diff=next-vel;
00247     vel=next;
00248     acc=diff*invtimediff;
00249   }
00250   
00251 #ifdef PLATFORM_APERIOS
00252   static WorldState ** stateLookupMap; //!< array used by getCurrent()/setCurrent() to store the state in use by each process -- should be initialized by setWorldStateLookup() to point into a shared memory region
00253 #endif
00254   
00255 private:
00256   WorldState(const WorldState&); //!< this shouldn't be called...
00257   WorldState& operator=(const WorldState&); //!< this shouldn't be called...
00258 };
00259 
00260 /*! @file
00261  * @brief Describes WorldState, maintains information about the robot's environment, namely sensors and power status
00262  * @author ejt (Creator)
00263  *
00264  * $Author: kcomer $
00265  * $Name: tekkotsu-4_0 $
00266  * $Revision: 1.46 $
00267  * $State: Exp $
00268  * $Date: 2007/11/15 21:33:04 $
00269  */
00270 
00271 #endif

Tekkotsu v4.0
Generated Thu Nov 22 00:54:57 2007 by Doxygen 1.5.4