Homepage | Demos | Overview | Downloads | Tutorials | Reference | Credits |
MotionManager.hGo to the documentation of this file.00001 //-*-c++-*- 00002 #ifndef INCLUDED_MotionManager_h 00003 #define INCLUDED_MotionManager_h 00004 00005 #include "MotionCommand.h" 00006 #include "OutputCmd.h" 00007 #include "OutputPID.h" 00008 #include "Shared/RobotInfo.h" 00009 #include "Shared/ListMemBuf.h" 00010 #include "Shared/MutexLock.h" 00011 00012 #ifdef PLATFORM_APERIOS 00013 #include "Shared/SharedObject.h" 00014 #include "MotionManagerMsg.h" 00015 #endif 00016 00017 #ifdef PLATFORM_APERIOS 00018 #include <OPENR/OPENR.h> 00019 #include <OPENR/OPENRAPI.h> 00020 #include <OPENR/OSubject.h> 00021 #include <OPENR/ObjcommEvent.h> 00022 #include <OPENR/OObject.h> 00023 #endif 00024 00025 //! The purpose of this class is to provide mutually exclusive access to the MotionCommands and simplify their sharing between memory spaces 00026 /*! Since MotionObject and MainObject run as separate processes, they 00027 * could potentially try to access the same motion command at the 00028 * same time, leading to unpredictable behavior. The MotionManager 00029 * enforces a set of locks to solve this. 00030 * 00031 * The other problem is that we are sharing between processes. 00032 * MotionManager will do what's necessary to distribute new 00033 * MotionCommand's to all the processes (currently just MainObj and 00034 * MotoObj)\n You should be able to create and add a new motion in 00035 * one line: 00036 * 00037 * @code 00038 * motman->addMotion( SharedObject<YourMC>([arg1,...]) , autoprune [, priority ]); 00039 * @endcode 00040 * 00041 * But if you want to do some more initializations not handled by the 00042 * constructor (the @p arg1, @p arg2, ... params) then you would 00043 * want to do something like the following: 00044 * 00045 * @code 00046 * SharedObject<YourMC> tmpvar([arg1,[arg2,...]]); 00047 * tmpvar->cmd1(); 00048 * tmpvar->cmd2(); 00049 * //... 00050 * motman->addPrunableMotion(tmpvar [, ...]); //or addPersistentMotion(...) 00051 * @endcode 00052 * 00053 * Notice that tmpvar is of type SharedObject, but you're calling @c 00054 * YourMC functions on it... SharedObject is a "smart pointer" which 00055 * will pass your function calls on to the underlying templated type. 00056 * Isn't C++ great? :) 00057 * 00058 * @warning Once the MotionCommand has been added you must check it 00059 * out to modify it or risk concurrent access problems. 00060 * 00061 * @see MotionCommand for information on creating new motion primitives. 00062 * 00063 * @see MMAccessor for information on accessing motions after you've 00064 * added them to MotionManager. 00065 */ 00066 class MotionManager { 00067 public: 00068 //! This is the number of processes which will be accessing the MotionManager 00069 /*! Probably just MainObject and MotionObject... This isn't really a 00070 * hard maximum, but should be actual expected, need to know when 00071 * they're all connected */ 00072 static const unsigned int MAX_ACCESS=2; 00073 00074 static const unsigned int MAX_MOTIONS=64; //!< This is the maximum number of Motions which can be managed, can probably be increased reasonably without trouble 00075 00076 typedef MotionManagerMsg::MC_ID MC_ID; //!< use this type when referring to the ID numbers that MotionManager hands out 00077 static const MC_ID invalid_MC_ID=MotionManagerMsg::invalid_MC_ID; //!< for errors and undefined stuff 00078 00079 //!Just to give you some guidelines for what values to use for different priority levels, but you can pick any value you like (that's why they are floats) 00080 //!@name Priority Level Constants 00081 static const float kIgnoredPriority; //!< won't be expressed, handy if you want to temporarily pause something 00082 static const float kBackgroundPriority; //!< will only be expressed if *nothing* else is using that joint 00083 static const float kLowPriority; //!< for stuff that's not background but lower than standard 00084 static const float kStdPriority; //!< for every-day commands 00085 static const float kHighPriority; //!< for stuff that should override standard stuff 00086 static const float kEmergencyPriority; //!< for really important stuff, such as the emergency stop 00087 //@} 00088 00089 MotionManager(); //!< Constructor, sets all the outputs to 0 00090 00091 #ifdef PLATFORM_APERIOS 00092 void InitAccess(OSubject* subj); //!< @b LOCKS @b MotionManager Everyone who is planning to use the MotionManager needs to call this before they access it or suffer a horrible fate 00093 void receivedMsg(const ONotifyEvent& event); //!< @b LOCKS @b MotionManager This gets called by an OObject when it receives a message from one of the other OObject's MotionManagerComm Subject 00094 #endif 00095 00096 //!@name MotionCommand Safe 00097 void setOutput(const MotionCommand* caller, unsigned int output, const OutputCmd& cmd); //!< @b LOCKS @b MotionManager Requests a value be set for the specified output, copies cmd across frames 00098 void setOutput(const MotionCommand* caller, unsigned int output, const OutputCmd& cmd, unsigned int frame); //!< @b LOCKS @b MotionManager Requests a value be set for the specified output in the specified frame 00099 void setOutput(const MotionCommand* caller, unsigned int output, const OutputCmd cmd[NumFrames]); //!< @b LOCKS @b MotionManager Requests a value be set for the specified output across frames 00100 void setOutput(const MotionCommand* caller, unsigned int output, const OutputPID& pid); //!< @b LOCKS @b MotionManager Requests a PID be set for the specified output, notice that this might be overruled by a higher priority motion 00101 void setOutput(const MotionCommand* caller, unsigned int output, const OutputCmd& cmd, const OutputPID& pid); //!< @b LOCKS @b MotionManager Requests a value and PID be set for the specified output 00102 void setOutput(const MotionCommand* caller, unsigned int output, const OutputCmd cmd[NumFrames], const OutputPID& pid); //!< @b LOCKS @b MotionManager Requests a value and PID be set for the specified output 00103 const OutputCmd& getOutputCmd(unsigned int output) const { return cmds[output]; } //!< Returns the value of the output last sent to the OS. Note that this will differ from the sensed value in state, even when staying still. There is no corresponding getOutputPID because this value *will* duplicate the value in state. 00104 void setPriority(MC_ID mcid, float p) { cmdlist[mcid].priority=p; }//!< sets the priority level of a MotionCommand 00105 float getPriority(MC_ID mcid) const { return cmdlist[mcid].priority; } //!< returns priority level of a MotionCommand 00106 //@} 00107 00108 //@{ 00109 inline MC_ID begin() const { return skip_ahead(cmdlist.begin()); } //!< returns the MC_ID of the first MotionCommand 00110 inline MC_ID next(MC_ID cur) const { return skip_ahead(cmdlist.next(cur)); } //!< returns the MC_ID of MotionCommand following the one that is passed 00111 inline MC_ID end() const { return cmdlist.end(); } //!< returns the MC_ID of the one-past-the-end MotionCommand (like the STL) 00112 inline unsigned int size() const { return cmdlist.size(); } //!< returns the number of MotionCommands being managed 00113 //@} 00114 00115 //!You can have one MC check out and modify another, but make sure the other MC doesn't call setOutput() 00116 //!@name MotionCommand "Risky" 00117 MotionCommand * checkoutMotion(MC_ID mcid,bool block=true); //!< locks the command and possibly performs RTTI conversion; supports recursive calls 00118 void checkinMotion(MC_ID mcid); //!< marks a MotionCommand as unused 00119 MotionCommand * peekMotion(MC_ID mcid) { return mcid==invalid_MC_ID?NULL:cmdlist[mcid].baseaddrs[_MMaccID]; } //!< allows access to a MotionCommand without checking it out; warning @b never call a function based on this, only access member fields through it 00120 unsigned int checkoutLevel(MC_ID mcid) { return mcid==invalid_MC_ID?0:cmdlist[mcid].lock.get_lock_level(); } //!< returns the number of times @a mcid has been checked out minus the times it's been checked in 00121 bool isOwner(MC_ID mcid) { return mcid==invalid_MC_ID?false:(cmdlist[mcid].lock.owner()==_MMaccID); } 00122 //@} 00123 00124 //!@name MotionCommand Unsafe 00125 //@{ 00126 #ifdef PLATFORM_APERIOS 00127 //! @b LOCKS @b MotionManager adds a new motion (wrapped in a SharedObject) and marks that it should be automatically deleted when the MotionCommand::isAlive() returns false. 00128 MC_ID addPrunableMotion(const SharedObjectBase& sm, float priority=kStdPriority) { return doAddMotion(sm,true,priority); } 00129 //! @b LOCKS @b MotionManager adds a new motion (wrapped in a SharedObject) and marks that it should @e not be deleted, until removeMotion(MC_ID mcid) is called. 00130 MC_ID addPersistentMotion(const SharedObjectBase& sm, float priority=kStdPriority) { return doAddMotion(sm,false,priority); } 00131 #endif //PLATFORM_APERIOS 00132 void removeMotion(MC_ID mcid); //!< @b LOCKS @b MotionManager removes the specified MotionCommand 00133 //@} 00134 00135 //!@name Deprecated 00136 #ifdef PLATFORM_APERIOS 00137 MC_ID addMotion(const SharedObjectBase& sm) __attribute__((deprecated)); //!< deprecated, we're recommending users call either addPrunableMotion() or addPersistentMotion() so there are no surprises 00138 MC_ID addMotion(const SharedObjectBase& sm, bool autoprune) __attribute__((deprecated)); //!< deprecated, we're recommending users call either addPrunableMotion() or addPersistentMotion() so there are no surprises 00139 MC_ID addMotion(const SharedObjectBase& sm, float priority) __attribute__((deprecated)); //!< deprecated, we're recommending users call either addPrunableMotion() or addPersistentMotion() so there are no surprises 00140 MC_ID addMotion(const SharedObjectBase& sm, float priority, bool autoprune) __attribute__((deprecated)); //!< deprecated, we're recommending users call either addPrunableMotion() or addPersistentMotion() so there are no surprises 00141 #endif //PLATFORM_APERIOS 00142 //@} 00143 00144 //@{ 00145 void lock() { MMlock.lock(_MMaccID); } //!< gets an exclusive lock on MotionManager - functions marked @b LOCKS @b MotionManager will cause (and require) this to happen automatically 00146 bool trylock() { return MMlock.try_lock(_MMaccID); } //!< tries to get a lock without blocking 00147 void release() { MMlock.release(); } //!< releases a lock on the motion manager 00148 //@} 00149 00150 //@{ 00151 void getOutputs(float outputs[NumFrames][NumOutputs]); //!< @b LOCKS @b MotionManager called by MotionObject to fill in the output values for the next ::NumFrames frames (only MotoObj should call this...) 00152 void updateWorldState(); //!< call this when you want MotionManager to set the WorldState to reflect what things should be for unsensed outputs (LEDs, ears) (only MotoObj should be calling this...) 00153 #ifdef PLATFORM_APERIOS 00154 bool updatePIDs(OPrimitiveID primIDs[NumOutputs]); //!< call this when you want MotionManager to update modified PID values, returns true if changes made (only MotoObj should be calling this...), see PIDMC for general PID documentation 00155 #endif 00156 //@} 00157 00158 //! holds the full requested value of an output 00159 class OutputState { 00160 public: 00161 //!@name Constructors 00162 //!Constructor 00163 OutputState(); 00164 OutputState(unsigned int out, float pri, MC_ID mc, const OutputCmd cmds[NumFrames]); 00165 OutputState(unsigned int out, float pri, MC_ID mc, const OutputCmd& cmd); 00166 OutputState(unsigned int out, float pri, MC_ID mc, const OutputCmd& cmd, unsigned int frame); 00167 OutputState(unsigned int out, float pri, MC_ID mc, const OutputPID& p); 00168 OutputState(unsigned int out, float pri, MC_ID mc, const OutputCmd cmds[NumFrames], const OutputPID& p); 00169 OutputState(unsigned int out, float pri, MC_ID mc, const OutputCmd& cmd, const OutputPID& p); 00170 //@} 00171 float priority; //!< priority level 00172 MC_ID mcid; //!< MC_ID of requester 00173 OutputCmd frames[NumFrames]; //!< values of output planned ahead 00174 OutputPID pid; //!< pid of output 00175 }; 00176 00177 protected: 00178 #ifdef PLATFORM_APERIOS 00179 //!does the actual work of adding a motion 00180 MC_ID doAddMotion(const SharedObjectBase& sm, bool autoprune, float priority); 00181 #endif 00182 00183 //! used to request pids for a given joint 00184 struct PIDUpdate { 00185 //!constructor 00186 PIDUpdate() : joint((unsigned int)-1) {} 00187 //!constructor 00188 PIDUpdate(unsigned int j, const float p[3]) : joint(j) { 00189 for(unsigned int i=0; i<3; i++) 00190 pids[i]=p[i]; 00191 } 00192 unsigned int joint; //!< the joint ID (see RobotInfo.h for offset values) 00193 float pids[3]; //!< the PID values to use (see ::Pid ) 00194 }; 00195 ListMemBuf<PIDUpdate,NumPIDJoints> pidchanges; //!< stores PID updates, up to one per joint (if same is set more than once, it's just overwrites previous update) 00196 void setPID(unsigned int j, const float p[3]); //!< @b LOCKS @b MotionManager, called internally to do the work of setting the PID... you probably want to call setOutput with an OutputPID argument, not this... 00197 00198 typedef unsigned short accID_t; //!< type to use to refer to accessors of MotionManager (or its locks) 00199 00200 void func_begin() { MMlock.lock(_MMaccID); } //!< called at the begining of many functions to lock MotionManager 00201 void func_end() { MMlock.release(); } //!< called at the end of a function which called func_begin() to release it 00202 template<class T> T func_end(T val) { func_end(); return val; } //!< same as func_end(), except passes return value through 00203 00204 MC_ID skip_ahead(MC_ID mcid) const; //!< during iteration, skips over motioncommands which are still in transit from on OObject to another 00205 00206 //!All the information we need to maintain about a MotionCommand 00207 struct CommandEntry { 00208 //! Constructor, sets everything to basics 00209 CommandEntry() : lastAccessor((unsigned short)-1),lock(),priority(MotionManager::kStdPriority) { 00210 for(unsigned int i=0; i<MAX_ACCESS; i++) { 00211 baseaddrs[i]=NULL; 00212 #ifdef PLATFORM_APERIOS 00213 rcr[i]=NULL; 00214 #endif 00215 } 00216 } 00217 MotionCommand * baseaddrs[MAX_ACCESS]; //!< for each accessor, the base address of the motion command 00218 #ifdef PLATFORM_APERIOS 00219 RCRegion * rcr[MAX_ACCESS]; //!< for each accessor the shared memory region that holds the motion command 00220 #endif 00221 accID_t lastAccessor; //!< the ID of the last accessor to touch the command (which implies if it wants to touch this again, we don't have to convert again) 00222 MutexLock<MAX_ACCESS> lock; //!< a lock to maintain mutual exclusion 00223 float priority; //!< MotionCommand's priority level 00224 private: 00225 CommandEntry(const CommandEntry&); //!< this shouldn't be called... 00226 CommandEntry& operator=(const CommandEntry&); //!< this shouldn't be called... 00227 }; 00228 ListMemBuf<CommandEntry,MAX_MOTIONS,MC_ID> cmdlist; //!< the list where MotionCommands are stored, remember, we're in a shared memory region with different base addresses - no pointers! 00229 MC_ID cur_cmd; //!< MC_ID of the MotionCommand currently being updated by getOutputs(), or NULL if not in getOutputs. This is used by the setOutput()'s to tell which MotionCommand is calling 00230 00231 00232 inline MC_ID pop_free() { return cmdlist.new_front(); } //!<pulls an entry from cmdlist's free section and returns its index 00233 inline void push_free(MC_ID a) { cmdlist.erase(a); } //!<puts an entry back into cmdlist's free section 00234 00235 MutexLock<MAX_ACCESS> MMlock; //!< The main lock for the class 00236 00237 typedef ListMemBuf<OutputState,MAX_MOTIONS> cmdstatelist_t; //!< shorthand for a list of OutputState's 00238 cmdstatelist_t cmdstates[NumOutputs]; //!< requested positions by each of the MC's for each of the outputs 00239 float cmdSums[NumOutputs]; //!<Holds the final values for the outputs of the last frame generated 00240 OutputCmd cmds[NumOutputs]; //!<Holds the weighted values and total weight for the outputs of the last frame 00241 00242 #ifdef PLATFORM_APERIOS 00243 accID_t numAcc; //!<The number of accessors who have registered with InitAccess() 00244 OSubject* subjs[MAX_ACCESS]; //!<The OSubject for each process (accessor) on which it should be broadcast when a command is added 00245 #endif 00246 00247 static int _MMaccID; //!<Stores the accessor for the current process 00248 00249 private: 00250 MotionManager(const MotionManager&); //!< this shouldn't be called... 00251 MotionManager& operator=(const MotionManager&); //!< this shouldn't be called... 00252 }; 00253 00254 //!anyone who #includes MotionManager.h will be wanting to use the global motman... don't want multiple of these! created by MotoObj 00255 extern MotionManager * motman; 00256 00257 /*! @file 00258 * @brief Describes MotionManager, simplifies sharing of MotionCommand's and provides mutual exclusion to their access 00259 * @author ejt (Creator) 00260 * 00261 * $Author: ejt $ 00262 * $Name: tekkotsu-2_2 $ 00263 * $Revision: 1.23 $ 00264 * $State: Exp $ 00265 * $Date: 2004/10/18 23:10:26 $ 00266 */ 00267 00268 #endif |
Tekkotsu v2.2 |
Generated Tue Oct 19 14:19:15 2004 by Doxygen 1.3.9.1 |