00001
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
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
00050
00051 static void registerMotionHook(MotionHook& h) {
00052 MarkScope l(theSim ? dynamic_cast<Resource&>(theSim->simLock) : ::emptyResource);
00053 theSim->motionHooks.insert(&h);
00054 }
00055
00056
00057
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
00078 class FrameCounter : public MessageQueueBase::MessageFilter {
00079 public:
00080 virtual bool filterSendRequest(RCRegion* ) {
00081 globals->motion.frameNumber++;
00082
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&);
00098 CommandThread& operator=(const CommandThread&);
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);
00116 static bool gotCommand(RCRegion* msg);
00117 static bool gotMotion(RCRegion* msg);
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;
00169 unsigned int fullspeedSimStart;
00170 TimeET lastFrameWallStart;
00171 float avgWallTime;
00172 float avgSimTime;
00173 static const float avgSpeedupGamma;
00174
00175 ThreadNS::Lock simLock;
00176
00177 private:
00178 Simulator(const Simulator&);
00179 Simulator& operator=(const Simulator&);
00180 };
00181
00182
00183
00184
00185
00186
00187
00188
00189
00190
00191
00192
00193 #endif