Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

MotionManager.h

Go 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 "IPC/ListMemBuf.h"
00010 #include "IPC/MutexLock.h"
00011 #include "IPC/SharedObject.h"
00012 #include "MotionManagerMsg.h"
00013 
00014 #ifdef PLATFORM_APERIOS
00015 #  include <OPENR/OPENR.h>
00016 #  include <OPENR/OPENRAPI.h>
00017 #  include <OPENR/OSubject.h>
00018 #  include <OPENR/ObjcommEvent.h>
00019 #  include <OPENR/OObject.h>
00020 #else //PLATFORM_LOCAL
00021 #  include "IPC/MessageQueue.h"
00022 #endif
00023 
00024 class EventTranslator;
00025 
00026 //! The purpose of this class is to provide mutually exclusive access to the MotionCommands and simplify their sharing between memory spaces
00027 /*! Since MotionObject and MainObject run as separate processes, they
00028  *  could potentially try to access the same motion command at the
00029  *  same time, leading to unpredictable behavior.  The MotionManager
00030  *  enforces a set of locks to solve this.
00031  *
00032  *  The other problem is that we are sharing between processes.
00033  *  MotionManager will do what's necessary to distribute new
00034  *  MotionCommand's to all the processes (currently just MainObj and
00035  *  MotoObj)\n You should be able to create and add a new motion in
00036  *  one line:
00037  *
00038  *  @code
00039  *  motman->addMotion( SharedObject<YourMC>([arg1,...]) , autoprune [, priority ]);
00040  *  @endcode
00041  *  
00042  *  But if you want to do some more initializations not handled by the
00043  *  constructor (the @p arg1, @p arg2, ...  params) then you would
00044  *  want to do something like the following:
00045  *  
00046  *  @code
00047  *  SharedObject<YourMC> tmpvar([arg1,[arg2,...]]);
00048  *  tmpvar->cmd1();
00049  *  tmpvar->cmd2();
00050  *  //...
00051  *  motman->addPrunableMotion(tmpvar [, ...]); //or addPersistentMotion(...)
00052  *  @endcode
00053  *
00054  *  Notice that tmpvar is of type SharedObject, but you're calling @c
00055  *  YourMC functions on it...  SharedObject is a "smart pointer" which
00056  *  will pass your function calls on to the underlying templated type.
00057  *  Isn't C++ great? :)
00058  *
00059  *  @warning Once the MotionCommand has been added you must check it
00060  *  out to modify it or risk concurrent access problems.
00061  *
00062  *  @see MotionCommand for information on creating new motion primitives.
00063  *
00064  *  @see MMAccessor for information on accessing motions after you've
00065  *  added them to MotionManager.
00066  */
00067 class MotionManager {
00068 public:
00069   //! This is the number of processes which will be accessing the MotionManager
00070   /*! Probably just MainObject and MotionObject... This isn't really a
00071    *  hard maximum, but should be actual expected, need to know when
00072    *  they're all connected */
00073   static const unsigned int MAX_ACCESS=2;
00074 
00075   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
00076 
00077   typedef MotionManagerMsg::MC_ID MC_ID;      //!< use this type when referring to the ID numbers that MotionManager hands out
00078   static const MC_ID invalid_MC_ID=MotionManagerMsg::invalid_MC_ID; //!< for errors and undefined stuff
00079 
00080   //!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)
00081   //!@name Priority Level Constants
00082   static const float kIgnoredPriority;    //!< won't be expressed, handy if you want to temporarily pause something
00083   static const float kBackgroundPriority; //!< will only be expressed if *nothing* else is using that joint
00084   static const float kLowPriority;        //!< for stuff that's not background but lower than standard
00085   static const float kStdPriority;        //!< for every-day commands
00086   static const float kHighPriority;       //!< for stuff that should override standard stuff
00087   static const float kEmergencyPriority;  //!< for really important stuff, such as the emergency stop
00088   //@}
00089 
00090   MotionManager();                            //!< Constructor, sets all the outputs to 0
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 #else
00095   void InitAccess(MessageQueueBase& mcbufq); //!< @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
00096   void RemoveAccess(); //!< needed in order to dereference shared memory regions before shutting down
00097 #endif
00098   static void setTranslator(EventTranslator* et) {etrans=et;} //!< sets #etrans, should be called before any events can be sent
00099   void processMsg(RCRegion* region); //!< @b LOCKS @b MotionManager This gets called by receivedMsg when under Aperios, or directly if you already have an RCRegion
00100   ~MotionManager(); //!<destructor
00101 
00102   //!@name MotionCommand Safe
00103   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
00104   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
00105   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
00106   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
00107   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
00108   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
00109   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.
00110   void setPriority(MC_ID mcid, float p) { cmdlist[mcid].priority=p; }//!< sets the priority level of a MotionCommand
00111   float getPriority(MC_ID mcid) const { return cmdlist[mcid].priority; } //!< returns priority level of a MotionCommand
00112   //@}
00113 
00114   //@{
00115   inline MC_ID begin() const         { return skip_ahead(cmdlist.begin()); }   //!< returns the MC_ID of the first MotionCommand
00116   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
00117   inline MC_ID end() const           { return cmdlist.end();      } //!< returns the MC_ID of the one-past-the-end MotionCommand (like the STL)
00118   inline unsigned int size() const   { return cmdlist.size();     } //!< returns the number of MotionCommands being managed
00119   //@}
00120 
00121   //!You can have one MC check out and modify another, but make sure the other MC doesn't call setOutput()
00122   //!@name MotionCommand "Risky"
00123   MotionCommand * checkoutMotion(MC_ID mcid,bool block=true); //!< locks the command and possibly performs RTTI conversion; supports recursive calls
00124   void checkinMotion(MC_ID mcid); //!< marks a MotionCommand as unused
00125   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
00126   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
00127   bool isOwner(MC_ID mcid) { return mcid==invalid_MC_ID?false:(cmdlist[mcid].lock.owner()==_MMaccID); }
00128   //@}
00129 
00130   //!@name MotionCommand Unsafe
00131   //@{
00132   //! @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.
00133   MC_ID addPrunableMotion(const SharedObjectBase& sm, float priority=kStdPriority) { return doAddMotion(sm,true,priority); }
00134   //! @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.
00135   MC_ID addPersistentMotion(const SharedObjectBase& sm, float priority=kStdPriority) { return doAddMotion(sm,false,priority); }
00136   void removeMotion(MC_ID mcid); //!< @b LOCKS @b MotionManager removes the specified MotionCommand
00137   //@}
00138 
00139   //@{
00140   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
00141   bool trylock() { return MMlock.try_lock(_MMaccID); } //!< tries to get a lock without blocking
00142   void unlock() { MMlock.unlock(); } //!< releases a lock on the motion manager
00143   //@}
00144 
00145   //@{
00146   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...)
00147   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...)
00148 #ifdef PLATFORM_APERIOS
00149   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
00150 #endif
00151   //@}
00152 
00153   //! holds the full requested value of an output
00154   class OutputState {
00155   public:
00156     //!@name Constructors
00157     //!Constructor
00158     OutputState();
00159     OutputState(unsigned int out, float pri, MC_ID mc, const OutputCmd cmds[NumFrames]);
00160     OutputState(unsigned int out, float pri, MC_ID mc, const OutputCmd& cmd);
00161     OutputState(unsigned int out, float pri, MC_ID mc, const OutputCmd& cmd, unsigned int frame);
00162     OutputState(unsigned int out, float pri, MC_ID mc, const OutputPID& p);
00163     OutputState(unsigned int out, float pri, MC_ID mc, const OutputCmd cmds[NumFrames], const OutputPID& p);
00164     OutputState(unsigned int out, float pri, MC_ID mc, const OutputCmd& cmd, const OutputPID& p);
00165     //@}
00166     float priority;             //!< priority level
00167     MC_ID mcid;                 //!< MC_ID of requester
00168     OutputCmd frames[NumFrames]; //!< values of output planned ahead
00169     OutputPID pid;               //!< pid of output
00170   };
00171 
00172 protected:
00173   //!does the actual work of adding a motion
00174   MC_ID doAddMotion(const SharedObjectBase& sm, bool autoprune, float priority);
00175   
00176   //! used to request pids for a given joint
00177   struct PIDUpdate {
00178     //!constructor
00179     PIDUpdate() : joint((unsigned int)-1) {}
00180     //!constructor
00181     PIDUpdate(unsigned int j, const float p[3]) : joint(j) {
00182       for(unsigned int i=0; i<3; i++)
00183         pids[i]=p[i];
00184     }
00185     unsigned int joint; //!< the joint ID (see RobotInfo.h for offset values)
00186     float pids[3]; //!< the PID values to use (see ::Pid )
00187   };
00188   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)
00189   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...
00190 
00191   typedef unsigned short accID_t; //!< type to use to refer to accessors of MotionManager (or its locks)
00192 
00193   void func_begin() { MMlock.lock(_MMaccID); } //!< called at the begining of many functions to lock MotionManager
00194   void func_end() { MMlock.unlock(); } //!< called at the end of a function which called func_begin() to release it
00195   template<class T> T func_end(T val) { func_end(); return val; } //!< same as func_end(), except passes return value through
00196 
00197   MC_ID skip_ahead(MC_ID mcid) const; //!< during iteration, skips over motioncommands which are still in transit from on OObject to another
00198     
00199   //!All the information we need to maintain about a MotionCommand
00200   struct CommandEntry {
00201     //! Constructor, sets everything to basics
00202     CommandEntry() : lastAccessor((unsigned short)-1),lock(),priority(MotionManager::kStdPriority) {
00203       for(unsigned int i=0; i<MAX_ACCESS; i++) {
00204         baseaddrs[i]=NULL;
00205         rcr[i]=NULL;
00206       }
00207     }
00208     MotionCommand * baseaddrs[MAX_ACCESS]; //!< for each accessor, the base address of the motion command
00209     RCRegion * rcr[MAX_ACCESS];            //!< for each accessor the shared memory region that holds the motion command
00210     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)
00211     MutexLock<MAX_ACCESS> lock;            //!< a lock to maintain mutual exclusion
00212     float priority;                        //!< MotionCommand's priority level
00213   private:
00214     CommandEntry(const CommandEntry&); //!< this shouldn't be called...
00215     CommandEntry& operator=(const CommandEntry&); //!< this shouldn't be called...
00216   };
00217   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!
00218   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
00219 
00220 
00221   inline MC_ID pop_free() { return cmdlist.new_front(); } //!<pulls an entry from cmdlist's free section and returns its index
00222   inline void push_free(MC_ID a) { cmdlist.erase(a); }    //!<puts an entry back into cmdlist's free section
00223 
00224   MutexLock<MAX_ACCESS> MMlock;          //!< The main lock for the class
00225 
00226   typedef ListMemBuf<OutputState,MAX_MOTIONS> cmdstatelist_t; //!< shorthand for a list of OutputState's
00227   cmdstatelist_t cmdstates[NumOutputs];  //!< requested positions by each of the MC's for each of the outputs
00228   float cmdSums[NumOutputs];             //!<Holds the final values for the outputs of the last frame generated
00229   OutputCmd cmds[NumOutputs];            //!<Holds the weighted values and total weight for the outputs of the last frame
00230 
00231   accID_t numAcc;                        //!<The number of accessors who have registered with InitAccess()
00232 #ifdef PLATFORM_APERIOS
00233   OSubject* subjs[MAX_ACCESS];           //!<The OSubject for each process (accessor) on which it should be broadcast when a command is added
00234 #else //PLATFORM_LOCAL
00235   //!Storage of each process's attachment of the message queue, used to internally transmit sound buffers to SoundPlay
00236   MessageQueueBase * subjs[MAX_ACCESS];
00237 #endif
00238 
00239   static int _MMaccID;          //!<Stores the accessor for the current process
00240   static EventTranslator* etrans; //!< EventTranslator for sending events to Main -- each process will set the correct value for calls within that process.
00241 
00242 private:
00243   MotionManager(const MotionManager&); //!< this shouldn't be called...
00244   MotionManager& operator=(const MotionManager&); //!< this shouldn't be called...
00245 };
00246 
00247 //!anyone who includes MotionManager.h will be wanting to use the global motman... don't want multiple of these! created by MotoObj
00248 extern MotionManager * motman;
00249 
00250 /*! @file
00251  * @brief Describes MotionManager, simplifies sharing of MotionCommand's and provides mutual exclusion to their access
00252  * @author ejt (Creator)
00253  *
00254  * $Author: ejt $
00255  * $Name: tekkotsu-2_4_1 $
00256  * $Revision: 1.27 $
00257  * $State: Exp $
00258  * $Date: 2005/08/07 04:11:03 $
00259  */
00260 
00261 #endif

Tekkotsu v2.4.1
Generated Tue Aug 16 16:32:47 2005 by Doxygen 1.4.4