Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

Simulator.h

Go to the documentation of this file.
00001 //-*-c++-*-
00002 #ifndef INCLUDED_Simulator_h_
00003 #define INCLUDED_Simulator_h_
00004 
00005 #include "Process.h"
00006 #include "sim.h"
00007 #include "IPC/SharedObject.h"
00008 #include "IPC/MessageQueueStatusThread.h"
00009 #include "SharedGlobals.h"
00010 #include "Shared/plist.h"
00011 #include "Shared/RobotInfo.h"
00012 #include "Shared/debuget.h"
00013 #include "local/LoadDataThread.h"
00014 #include "local/MotionHook.h"
00015 #include <set>
00016 
00017 class MessageReceiver;
00018 
00019 class Simulator : public Process,  public plist::PrimitiveListener, public plist::CollectionListener, public MessageQueueStatusThread::StatusListener {
00020 public:
00021   //! constructor
00022   Simulator();
00023   
00024   ~Simulator();
00025 
00026   virtual void DoStart();
00027   virtual void DoStop();
00028   virtual void run();
00029   
00030   static const char * getClassName() { return "Simulator"; }
00031   static ProcessID::ProcessID_t getID() { return ProcessID::SimulatorProcess; }
00032   
00033   static const char * getCameraQueueID() { return "CameraData"; }
00034   static const char * getSensorQueueID() { return "SensorData"; }
00035   static const char * getTimerWakeupID() { return "TimerWakeup"; }
00036   static const char * getMotionWakeupID() { return "MotionWakeup"; }
00037   static const char * getStatusRequestID() { return "StatusRequest"; }
00038   static const char * getCommandQueueID() { return "CommandQueue"; }
00039   
00040   virtual void plistValueChanged(const plist::PrimitiveBase& pl);
00041   virtual void plistCollectionEntryAdded(plist::Collection& col, plist::ObjectBase& primitive);
00042   virtual void plistCollectionEntryRemoved(plist::Collection& col, plist::ObjectBase& primitive);
00043   virtual void plistCollectionEntriesChanged(plist::Collection& col);
00044     
00045   virtual void messagesRead(MessageQueueBase& mq, unsigned int n);
00046   
00047   static void sendCommand(const std::string& cmd);
00048   
00049   //! Registers the MotionHook to have its motionCheck() called following each motion update.
00050   /*! Responsibility for memory (de)allocation is NOT assumed by registration. */
00051   static void registerMotionHook(MotionHook& h) {
00052     MarkScope l(theSim ? dynamic_cast<Resource&>(theSim->simLock) : ::emptyResource);
00053     theSim->motionHooks.insert(&h);
00054   }
00055   
00056   //! Removes the MotionHook from the list, no longer receives any notifications
00057   /*! Responsibility for memory (de)allocation is NOT assumed by registration, so this doesn't affect @a h directly. */
00058   static void deregisterMotionHook(MotionHook& h) {
00059     MarkScope l(theSim ? dynamic_cast<Resource&>(theSim->simLock) : ::emptyResource);
00060     theSim->motionHooks.erase(&h);
00061   }
00062   
00063   static void clearMotionHooks() {
00064     MarkScope l(theSim ? dynamic_cast<Resource&>(theSim->simLock) : ::emptyResource);
00065     theSim->motionHooks.clear();
00066   }
00067   
00068   static void setMotionStarting();
00069   static void setMotionStopping();
00070   static void updateMotion(const float outputs[][NumOutputs]);
00071   static void setMotionLeavingRealtime(bool isFullSpeed);
00072   static void setMotionEnteringRealtime();
00073   
00074 protected:
00075   static void replaceEntry(const std::string& name, plist::Dictionary& d, const std::string& comment);
00076   
00077   //! subscribed by Simulator to message queue's send; ensures that SharedGlobals::MotionSimConfig::frameNumber stays in sync with the message serial number
00078   class FrameCounter : public MessageQueueBase::MessageFilter {
00079   public:
00080     virtual bool filterSendRequest(RCRegion* /*rcr*/) {
00081       globals->motion.frameNumber++;
00082       //ASSERT(globals->motion.frameNumber==LoadFileThread::recoverSN(rcr),"globals->motion.frameNumber does not match frame number sent! (" << globals->motion.frameNumber << " vs " << LoadDataThread::recoverSN(rcr));
00083       return true;
00084     }
00085   } frameCounter;
00086   
00087   class CommandThread : public Thread {
00088   public:
00089     CommandThread() : Thread(), initThread(NULL) {}
00090     virtual void * run();
00091     virtual void cancelled();
00092     virtual void runInitThread(Thread& th);
00093     virtual void abortInitThread() { if(initThread!=NULL) initThread->stop(); }
00094   protected:
00095     Thread* initThread;
00096   private:
00097     CommandThread(const CommandThread&); //!< do not call
00098     CommandThread& operator=(const CommandThread&); //!< do not call
00099   } cmdThread;
00100   
00101   enum step_t {
00102     STEP_NONE,
00103     STEP_CAMERA,
00104     STEP_SENSOR,
00105     STEP_TIMER,
00106     STEP_MOTION
00107   };
00108   
00109   void incrementTime();
00110   void sendTimerWakeup();
00111   unsigned int getNextFrame();
00112   void resetSpeedMode();
00113   
00114   void processRunlevel(SharedGlobals::runlevel_t curRunLevel);
00115   bool processCommand(const std::string& line, bool addToHistory); //!< process an individual command
00116   static bool gotCommand(RCRegion* msg); //!< for commands coming from other processes via #commandQueue and #commandrecv
00117   static bool gotMotion(RCRegion* msg); //!< when running in multi-process mode, receives output value updates from motion process
00118   
00119   void cmdQuit(const std::vector<std::string>& args);
00120   void cmdLoad(const std::vector<std::string>& args);
00121   void cmdSave(const std::vector<std::string>& args);
00122   void cmdRunlevel(const std::vector<std::string>& args);
00123   bool cmdSet(const std::vector<std::string>& args);
00124   void cmdRun(const std::vector<std::string>& args, bool isRelative);
00125   void cmdRun(const std::vector<std::string>& args);
00126   void cmdPause(const std::vector<std::string>& args);
00127   void cmdHelp(const std::vector<std::string>& args);
00128   void cmdStep(const std::vector<std::string>& args);
00129   void cmdStatus(const std::vector<std::string>& args);
00130   void cmdAdvance(const std::vector<std::string>& args);
00131   void cmdFreeze(bool v, const std::vector<std::string>& args);
00132   void cmdReset(const std::vector<std::string>& args);
00133   void cmdNew(const std::vector<std::string>& args);
00134   void cmdDelete(const std::vector<std::string>& args);
00135   
00136   SharedObject<sim::CameraQueue_t> cameraQueue;
00137   SharedObject<sim::SensorQueue_t> sensorQueue;
00138   SharedObject<sim::TimerWakeup_t> timerWakeup;
00139   SharedObject<sim::MotionWakeup_t> motionWakeup;
00140   SharedObject<sim::StatusRequest_t> statusRequest;
00141   SharedObject<sim::EventQueue_t> events;
00142   SharedObject<sim::MotionOutput_t> motionout;
00143   typedef MessageQueue<10> CommandQueue_t;
00144   SharedObject<CommandQueue_t> commandQueue;
00145   
00146   MessageQueueStatusThread cameraStatus;
00147   MessageQueueStatusThread sensorStatus;
00148   MessageQueueStatusThread timerStatus;
00149   MessageQueueStatusThread motionStatus;
00150   MessageQueueStatusThread eventsStatus;
00151 
00152   MessageReceiver * commandrecv;
00153   MessageReceiver * motionrecv;
00154   
00155 protected:
00156   LoadDataThread vision;
00157   LoadDataThread sensors;
00158   static Simulator* theSim;
00159   std::set<unsigned int> frameTimes;
00160   float runSpeed;
00161   float lastTimeScale;
00162   step_t step;
00163   unsigned int waitingSteps;
00164   SharedGlobals::runlevel_t curLevel;
00165   
00166   static std::set<MotionHook*> motionHooks;
00167   
00168   TimeET fullspeedWallStart; //!< "real" wall-clock time that full-speed mode was entered
00169   unsigned int fullspeedSimStart; //!< simulator time at which full-speed mode was entered
00170   TimeET lastFrameWallStart; //!< "real" wall-clock time that processing started on last frame (only valid in full-speed mode)
00171   float avgWallTime; //!< running average of frame processing time
00172   float avgSimTime; //!< running average of frame increments
00173   static const float avgSpeedupGamma; //!< gamma parameter for calculating running average in #avgWallTime and #avgSimTime
00174   
00175   ThreadNS::Lock simLock;
00176 
00177 private:
00178   Simulator(const Simulator&); //!< don't call (copy constructor)
00179   Simulator& operator=(const Simulator&); //!< don't call (assignment operator)
00180 };
00181 
00182 /*! @file
00183  * @brief Defines Simulator, which DESCRIPTION
00184  * @author ejt (Creator)
00185  *
00186  * $Author: ejt $
00187  * $Name: tekkotsu-4_0 $
00188  * $Revision: 1.7 $
00189  * $State: Exp $
00190  * $Date: 2007/11/19 21:00:42 $
00191  */
00192 
00193 #endif

Tekkotsu Hardware Abstraction Layer 4.0
Generated Thu Nov 22 01:00:53 2007 by Doxygen 1.5.4