00001 #include "BehaviorBase.h"
00002 #include "Motion/MotionManager.h"
00003 #include "Motion/WalkMC.h"
00004 #include "Motion/MotionSequenceMC.h"
00005 #include "Motion/PostureEngine.h"
00006 #include "Shared/SharedObject.h"
00007 #include "Events/EventRouter.h"
00008 #include "Events/EventBase.h"
00009
00010 #include "DriveMeBehavior.h"
00011
00012 #include <iostream>
00013 #include <string>
00014 #include <stdio.h>
00015
00016
00017
00018
00019
00020
00021 DriveMeBehavior::DriveMeBehavior()
00022 : BehaviorBase(),
00023 walker_id(MotionManager::invalid_MC_ID),
00024 stand_id(MotionManager::invalid_MC_ID),
00025 stand(),
00026 last_dx(0), last_dy(0), last_da(0), last_time(5000)
00027 {
00028
00029 stand->setPlayTime(700);
00030 stand->setPose(PostureEngine("/ms/data/motion/stand.pos"));
00031 }
00032
00033 void DriveMeBehavior::DoStart()
00034 {
00035 BehaviorBase::DoStart();
00036
00037 walker_id = motman->addMotion(SharedObject<WalkMC>());
00038
00039 stand_id = motman->addMotion(stand, MotionManager::kStdPriority+1, false);
00040
00041 erouter->addListener(this, EventBase::timerEGID);
00042
00043 erouter->addTimer(this, 0, 0, false);
00044 }
00045
00046 void DriveMeBehavior::DoStop()
00047 {
00048 BehaviorBase::DoStop();
00049
00050 erouter->removeTimer(this);
00051
00052 erouter->forgetListener(this);
00053
00054 motman->removeMotion(walker_id);
00055 motman->removeMotion(stand_id);
00056 }
00057
00058
00059 void DriveMeBehavior::processEvent(const EventBase& event)
00060 {
00061 using namespace std;
00062
00063 WalkMC *walker;
00064 MotionSequenceMC<MotionSequence::SizeSmall> *standp;
00065
00066
00067 if(event.getGeneratorID() != EventBase::timerEGID) return;
00068
00069
00070 walker = (WalkMC*)motman->checkoutMotion(walker_id);
00071 walker->setTargetVelocity(0,0,0);
00072 motman->checkinMotion(walker_id);
00073
00074
00075 standp = (MotionSequenceMC<MotionSequence::SizeSmall>*)motman->checkoutMotion(stand_id);
00076 standp->play();
00077 motman->checkinMotion(stand_id);
00078
00079
00080 string instr;
00081 cout << "dx? [" << last_dx << "] >\t";
00082 cout.flush();
00083 getline(cin, instr);
00084 sscanf(instr.data(), "%lf", &last_dx);
00085
00086 cout << "dy? [" << last_dy << "] >\t";
00087 cout.flush();
00088 getline(cin, instr);
00089 sscanf(instr.data(), "%lf", &last_dy);
00090
00091 cout << "da? [" << last_da << "] >\t";
00092 cout.flush();
00093 getline(cin, instr);
00094 sscanf(instr.data(), "%lf", &last_da);
00095
00096 cout << "time? [" << last_time << "] >\t";
00097 cout.flush();
00098 getline(cin, instr);
00099 sscanf(instr.data(), "%u", &last_time);
00100
00101
00102 walker = (WalkMC*)motman->checkoutMotion(walker_id);
00103 walker->setTargetVelocity(last_dx, last_dy, last_da);
00104 erouter->addTimer(this, 0, last_time, false);
00105 motman->checkinMotion(walker_id);
00106 }
00107
00108
00109
00110
00111
00112
00113
00114
00115
00116
00117
00118