00001 #include "StartupBehavior.h"
00002
00003 #include "Behaviors/Controller.h"
00004 #include "Shared/SharedObject.h"
00005
00006 #include "Behaviors/Controls/BatteryCheckControl.h"
00007 #include "Behaviors/Controls/BehaviorActivatorControl.h"
00008 #include "Behaviors/Controls/BehaviorSwitchControl.h"
00009 #include "Behaviors/Controls/ControlBase.h"
00010 #include "Behaviors/Controls/DumpFileControl.h"
00011 #include "Behaviors/Controls/FileBrowserControl.h"
00012 #include "Behaviors/Controls/FreeMemReportControl.h"
00013 #include "Behaviors/Controls/HelpControl.h"
00014 #include "Behaviors/Controls/LoadPostureControl.h"
00015 #include "Behaviors/Controls/LoadWalkControl.h"
00016 #include "Behaviors/Controls/MCValueEditControl.h"
00017 #include "Behaviors/Controls/PlaySoundControl.h"
00018 #include "Behaviors/Controls/ProfilerCheckControl.h"
00019 #include "Behaviors/Controls/RebootControl.h"
00020 #include "Behaviors/Controls/RunSequenceControl.h"
00021 #include "Behaviors/Controls/SavePostureControl.h"
00022 #include "Behaviors/Controls/SaveWalkControl.h"
00023 #include "Behaviors/Controls/ShutdownControl.h"
00024 #include "Behaviors/Controls/ValueEditControl.h"
00025 #include "Behaviors/Controls/ValueSetControl.h"
00026
00027 #include "Behaviors/Controls/EventLogger.h"
00028
00029 #include "Behaviors/Demos/AutoGetupBehavior.h"
00030 #include "Behaviors/Demos/BatteryMonitorBehavior.h"
00031 #include "Behaviors/Demos/ChaseBallBehavior.h"
00032 #include "Behaviors/Demos/SimpleChaseBallBehavior.h"
00033 #include "Behaviors/Demos/StareAtBallBehavior.h"
00034 #include "Behaviors/Demos/FollowHeadBehavior.h"
00035 #include "Behaviors/Demos/HeadLevelBehavior.h"
00036 #include "Behaviors/Demos/EvtRptBehavior.h"
00037 #include "Behaviors/Demos/WalkToTargetMachine.h"
00038 #include "Behaviors/Demos/BanditMachine.h"
00039 #include "Behaviors/Demos/WorldModel2Behavior.h"
00040 #include "Behaviors/Demos/DumbWM2Behavior.h"
00041 #include "Behaviors/Demos/SoundTestBehavior.h"
00042 #include "Behaviors/Demos/ToggleHeadLightBehavior.h"
00043 #include "Behaviors/Demos/WalkControllerBehavior.h"
00044 #include "Behaviors/Demos/HeadPointControllerBehavior.h"
00045 #include "Behaviors/Demos/Aibo3DControllerBehavior.h"
00046 #include "Behaviors/Demos/Aibo3DMonitorBehavior.h"
00047 #include "Behaviors/Demos/EStopControllerBehavior.h"
00048
00049 #include "Motion/MotionCommand.h"
00050 #include "Motion/PostureMC.h"
00051 #include "Motion/EmergencyStopMC.h"
00052 #include "Motion/PIDMC.h"
00053 #include "Motion/MotionSequenceMC.h"
00054 #include "Motion/MMAccessor.h"
00055 #include "Shared/ERS210Info.h"
00056
00057 #include "SoundPlay/SoundManager.h"
00058
00059 StartupBehavior gStartup;
00060 BehaviorBase& startupBehavior=gStartup;
00061
00062 StartupBehavior::StartupBehavior()
00063 : BehaviorBase(), spawned(),
00064 stop_id(MotionManager::invalid_MC_ID),
00065 pid_id(MotionManager::invalid_MC_ID)
00066 {
00067 AddReference();
00068 }
00069
00070 StartupBehavior::~StartupBehavior() {cout << "Goodbye" << endl;}
00071
00072 void StartupBehavior::DoStart() {
00073 BehaviorBase::DoStart();
00074
00075
00076 pid_id=motman->addMotion(SharedObject<PIDMC>(0),MotionManager::kEmergencyPriority+1,false);
00077
00078 erouter->addTimer(this,0,4*FrameTime*NumFrames,true);
00079
00080
00081 const SharedObject<EmergencyStopMC> stop;
00082 stop->LoadFile("/ms/data/motion/liedown.pos");
00083 stop->setStopped(true,false);
00084
00085 stop_id=motman->addMotion(stop,MotionManager::kEmergencyPriority);
00086
00087
00088 BatteryCheckControl batchk;
00089 batchk.activate(MotionManager::invalid_MC_ID,NULL);
00090
00091
00092 batchk.deactivate();
00093
00094
00095 Controller * controller=new Controller;
00096 controller->DoStart();
00097 controller->setEStopID(stop_id);
00098 controller->setRoot(SetupMenus());
00099 wireless->setReceiver(sout, Controller::console_callback);
00100 spawned.push_back(controller);
00101
00102 sndman->PlayFile("roar.wav");
00103
00104
00105
00106
00107
00108
00109
00110 }
00111
00112 void StartupBehavior::DoStop() {
00113 for(std::vector<BehaviorBase*>::iterator it=spawned.begin(); it!=spawned.end(); it++)
00114 (*it)->DoStop();
00115 motman->removeMotion(stop_id);
00116 BehaviorBase::DoStop();
00117 }
00118
00119
00120 void StartupBehavior::processEvent(const EventBase&) {
00121 static unsigned int start_time=-1U;
00122 const unsigned int tot_time=2047;
00123 if(start_time==-1U) {
00124 start_time=get_time();
00125 MMAccessor<EmergencyStopMC>(stop_id)->takeSnapshot();
00126 } else {
00127 float power=(get_time()-start_time)/(float)tot_time;
00128 if(power>1)
00129 power=1;
00130 { MMAccessor<PIDMC>(pid_id)->setAllPowerLevel(power); }
00131 if(state->robotDesign & WorldState::ERS210Mask)
00132 { MMAccessor<EmergencyStopMC>(stop_id)->setOutputCmd(ERS210Info::MouthOffset,outputRanges[ERS210Info::MouthOffset][MaxRange]); }
00133 }
00134 if((get_time()-start_time)>=tot_time) {
00135 erouter->removeTimer(this);
00136 motman->removeMotion(pid_id);
00137 pid_id=MotionManager::invalid_MC_ID;
00138 }
00139 }
00140
00141
00142
00143
00144
00145
00146
00147 ControlBase*
00148 StartupBehavior::SetupMenus() {
00149 std::cout << "CONTROLLER-INIT..." << std::flush;
00150 std::stack< ControlBase* > mcsetup;
00151 {
00152 ControlBase* root=new ControlBase("Root Control");
00153 mcsetup.push(root);
00154 mcsetup.top()->pushSlot(new ControlBase("Mode Switch","Contains the \"major\" applications - mutually exclusive selection"));
00155 {
00156 mcsetup.push(mcsetup.top()->getSlots().back());
00157 BehaviorSwitchControlBase::BehaviorGroup * bg = new BehaviorSwitchControlBase::BehaviorGroup();
00158
00159 mcsetup.top()->pushSlot(new BehaviorSwitchControl<FollowHeadBehavior>("FollowHeadBehavior",bg,false));
00160
00161 mcsetup.top()->pushSlot(new BehaviorSwitchControl<SoundTestBehavior>("SoundTestBehavior",bg,false));
00162 mcsetup.top()->pushSlot(new BehaviorSwitchControl<ChaseBallBehavior>("ChaseBallBehavior",bg,false));
00163 mcsetup.top()->pushSlot(new BehaviorSwitchControl<SimpleChaseBallBehavior>("SimpleChaseBallBehavior",bg,false));
00164 mcsetup.top()->pushSlot(new BehaviorSwitchControl<StareAtBallBehavior>("StareAtBallBehavior",bg,false));
00165 mcsetup.top()->pushSlot(new BehaviorSwitchControl<WalkToTargetMachine,Factory1Arg<WalkToTargetMachine,VisionEventNS::VisionSourceID_t,VisionEventNS::PinkBallSID> >("WalkToPinkBall",bg,false));
00166 mcsetup.top()->pushSlot(new BehaviorSwitchControl<BanditMachine>("BanditMachine",bg,false));
00167 mcsetup.top()->pushSlot(new BehaviorSwitchControl<WorldModel2Behavior>("WorldModel2Behavior",bg,false));
00168 mcsetup.pop();
00169 }
00170 mcsetup.top()->pushSlot(new ControlBase("Background Behaviors","Background daemons and monitors"));
00171 {
00172 mcsetup.push(mcsetup.top()->getSlots().back());
00173 mcsetup.top()->pushSlot((new BehaviorSwitchControl<AutoGetupBehavior>("AutoGetupBehavior",false))->start());
00174 mcsetup.top()->pushSlot((new BehaviorSwitchControl<BatteryMonitorBehavior>("BatteryMonitorBehavior",false))->start());
00175 mcsetup.top()->pushSlot(new BehaviorSwitchControl<EvtRptBehavior>("EvtRptBehavior",false));
00176 mcsetup.top()->pushSlot(new BehaviorSwitchControl<DumbWM2Behavior>("DumbWM2Behavior",false));
00177 BehaviorSwitchControlBase::BehaviorGroup * aibo3D_bg = new BehaviorSwitchControlBase::BehaviorGroup();
00178 mcsetup.top()->pushSlot((new BehaviorSwitchControl<Aibo3DControllerBehavior>("Aibo3DController",aibo3D_bg,false)));
00179 mcsetup.top()->pushSlot((new BehaviorSwitchControl<Aibo3DMonitorBehavior>("Aibo3DMonitor",aibo3D_bg,false)));
00180 mcsetup.top()->pushSlot(new BehaviorSwitchControl<HeadLevelBehavior>("HeadLevelBehavior",false));
00181 if(state->robotDesign & WorldState::ERS220Mask)
00182 mcsetup.top()->pushSlot(new BehaviorSwitchControl<ToggleHeadLightBehavior>("ToggleHeadLightBehavior",false));
00183 mcsetup.pop();
00184 }
00185 mcsetup.top()->pushSlot(new ControlBase("TekkotsuMon","Servers for GUIs"));
00186 {
00187 mcsetup.push(mcsetup.top()->getSlots().back());
00188 mcsetup.top()->pushSlot((new BehaviorSwitchControl<HeadPointControllerBehavior>("Head Remote Control",false)));
00189 mcsetup.top()->pushSlot((new BehaviorSwitchControl<WalkControllerBehavior>("Walk Remote Control",false)));
00190 BehaviorSwitchControlBase::BehaviorGroup * aibo3D_bg = new BehaviorSwitchControlBase::BehaviorGroup();
00191 mcsetup.top()->pushSlot((new BehaviorSwitchControl<Aibo3DControllerBehavior>("Aibo3D Controller",aibo3D_bg,false)));
00192 mcsetup.top()->pushSlot((new BehaviorSwitchControl<Aibo3DMonitorBehavior>("Aibo3D Monitor",aibo3D_bg,false)));
00193 EStopControllerBehavior * ESCB=new EStopControllerBehavior(stop_id);
00194 mcsetup.top()->pushSlot((new BehaviorSwitchControl<EStopControllerBehavior,Factory1Arg<EStopControllerBehavior,MotionManager::MC_ID,MotionManager::invalid_MC_ID> >("EStop Remote Control",ESCB))->start());
00195 mcsetup.pop();
00196 }
00197 mcsetup.top()->pushSlot(new ControlBase("Status Reports","Displays information about the runtime environment on the console"));
00198 {
00199 mcsetup.push(mcsetup.top()->getSlots().back());
00200 mcsetup.top()->pushSlot(new BatteryCheckControl());
00201 mcsetup.top()->pushSlot(new ProfilerCheckControl());
00202 mcsetup.top()->pushSlot(new EventLogger());
00203 FreeMemReportControl * tmp=new FreeMemReportControl();
00204 tmp->DoStart();
00205 mcsetup.top()->pushSlot(tmp);
00206 mcsetup.pop();
00207 }
00208 mcsetup.top()->pushSlot(new ControlBase("File Access","Access/load files on the memory stick"));
00209 {
00210 mcsetup.push(mcsetup.top()->getSlots().back());
00211 mcsetup.top()->pushSlot(new LoadPostureControl("Load Posture",stop_id));
00212 mcsetup.top()->pushSlot(new SavePostureControl("Save Posture"));
00213 mcsetup.top()->pushSlot(new RunSequenceControl<MotionSequence::SizeXLarge>("Run Motion Sequence",stop_id));
00214 mcsetup.top()->pushSlot(new PlaySoundControl("Play Sound"));
00215 mcsetup.top()->pushSlot(new DumpFileControl("Files","/ms"));
00216 mcsetup.pop();
00217 }
00218 mcsetup.top()->pushSlot(new ControlBase("Shutdown?"));
00219 {
00220 mcsetup.push(mcsetup.top()->getSlots().back());
00221 mcsetup.top()->pushSlot(new ShutdownControl());
00222 mcsetup.top()->pushSlot(new RebootControl());
00223 mcsetup.pop();
00224 }
00225 mcsetup.top()->pushSlot(new HelpControl(root));
00226 }
00227 if(mcsetup.size()!=1)
00228 cout << "*** WARNING *** menu setup stack more than one or empty" << endl;
00229 cout << "DONE" << endl;
00230 return mcsetup.top();
00231 }
00232
00233
00234
00235
00236
00237
00238
00239
00240
00241
00242
00243
00244