Tekkotsu Homepage | Demos | Overview | Downloads | Dev. Resources | Reference | Credits |
WorldState.hGo 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 |