Homepage Demos Overview Downloads Tutorials Reference
Credits
Main Page | Namespace List | Class Hierarchy | Alphabetical List | Compound List | File List | Namespace Members | Compound Members | File Members | Related Pages | Search

StartupBehavior.cc

Go to the documentation of this file.
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; //!< used to initialize the global ::startupBehavior, used by MMCombo
00060 BehaviorBase& startupBehavior=gStartup; //!< used by MMCombo as the init behavior
00061 
00062 StartupBehavior::StartupBehavior()
00063   : BehaviorBase(), spawned(),
00064     stop_id(MotionManager::invalid_MC_ID),
00065     pid_id(MotionManager::invalid_MC_ID)
00066 {
00067   AddReference(); // this is a global, so there's a global reference
00068 }
00069 
00070 StartupBehavior::~StartupBehavior() {cout << "Goodbye" << endl;}
00071 
00072 void StartupBehavior::DoStart() {
00073   BehaviorBase::DoStart();
00074   
00075   //This will "fade" in the PIDs so the joints don't jerk to full power, also looks cooler
00076   pid_id=motman->addMotion(SharedObject<PIDMC>(0),MotionManager::kEmergencyPriority+1,false);
00077   //also, pause before we start fading in, PIDs take effect right away, before the emergencystop is picked up
00078   erouter->addTimer(this,0,4*FrameTime*NumFrames,true);
00079 
00080   //This is the default emergency stop
00081   const SharedObject<EmergencyStopMC> stop;
00082   stop->LoadFile("/ms/data/motion/liedown.pos"); //This *should* be replaced by the current position, but just in case, better than setting everything to 0's
00083   stop->setStopped(true,false); //if you want to start off paused
00084   //sndman->StopPlay(); //so it doesn't play the halt sound the first time (but the false in previous line does that now)
00085   stop_id=motman->addMotion(stop,MotionManager::kEmergencyPriority);
00086   
00087   //This displays the current battery conditions
00088   BatteryCheckControl batchk;
00089   batchk.activate(MotionManager::invalid_MC_ID,NULL);
00090   //  const SharedObject<LedMC> led; //! @todo LedMC's don't support autopruning yet, it should for uses like this, or could the one in Controller
00091   //  batchk.activate(motman->addMotion(led,true));
00092   batchk.deactivate();
00093 
00094   //This is for the menu system
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   //This will close the mouth so it doesn't look stupid
00105   //Now done by setting the emergency stop directly in processEvent, but left as demo code:
00106   /*  const SharedObject<PostureMC> closemouth;
00107       closemouth->setJointCmd(MouthOffset,outputRanges[MouthOffset][MaxRange],1);
00108       motman->addMotion(closemouth,MotionCommand::kEmergencyPriority+2,true);
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 /*!Uses a few timer events at the beginning to fade in the PID values, and closes the mouth too*/
00120 void StartupBehavior::processEvent(const EventBase&) {
00121   static unsigned int start_time=-1U;
00122   const unsigned int tot_time=2047; //if this is set to 2048, i sometimes get funny errors: [oid:80000034,prio:1] AGRMSDriver::SetGain() : 0x0A IS USED FOR GAIN SHIFT VALUE.
00123   if(start_time==-1U) { //first time
00124     start_time=get_time();
00125     MMAccessor<EmergencyStopMC>(stop_id)->takeSnapshot(); //take new snapshot with hopefully valid data
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 /*class WalkToPinkBallFactory : public Factory<WalkToTargetMachine> {
00142   public:
00143   static WalkToTargetMachine* construct() { return new WalkToTargetMachine(VisionEventNS::PinkBallSID); }
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       //put behaviors here
00159       mcsetup.top()->pushSlot(new BehaviorSwitchControl<FollowHeadBehavior>("FollowHeadBehavior",bg,false));
00160       //      mcsetup.top()->pushSlot(new BehaviorSwitchControl<CameraBehavior>("Camera",bg,false));
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       /*      XORBehavior* xorbeh=new XORBehavior(); //i make other controls which reference xorbeh below
00234       mcsetup.top()->pushSlot(new BehaviorSwitchControl<XORBehavior>("AI Control",xorbeh,bg));
00235       { // I don't necessarily like this method of adding sub menus to control behavior parameters, but you can do it quickly. (cleaner to subclass the behavior, have it add its own)
00236         mcsetup.push(mcsetup.top()->getSlots().back());
00237         mcsetup.top()->pushSlot(new BehaviorActivatorControl("Toggle",(BehaviorSwitchControlBase*)mcsetup.top()));
00238         mcsetup.top()->pushSlot(new ValueSetControl<float>("Enable learning (0.035)",&xorbeh->getModel()->learn_rate,0.035));
00239         mcsetup.top()->pushSlot(new ValueSetControl<float>("Disable learning",&xorbeh->getModel()->learn_rate,0));
00240         mcsetup.top()->pushSlot(new LoadAIControl("Load AI",xorbeh->getModel()));
00241         mcsetup.top()->pushSlot(new SaveAIControl("Save AI",xorbeh->getModel()));
00242         mcsetup.pop();
00243       }
00244       */

Tekkotsu v1.4
Generated Sat Jul 19 00:06:31 2003 by Doxygen 1.3.2