00001
00002
00003
00004
00005
00006
00007 #include "WorldModel2Behavior.h"
00008
00009 #include "StateNode.h"
00010 #include "Transitions/TimeOutTrans.h"
00011 #include "Motion/MotionManager.h"
00012 #include "Motion/WalkMC.h"
00013 #include "Motion/HeadPointerMC.h"
00014 #include "Motion/MotionSequenceMC.h"
00015 #include "Motion/OutputCmd.h"
00016 #include "Shared/SharedObject.h"
00017 #include "Events/EventRouter.h"
00018 #include "Shared/WorldState.h"
00019 #include "Shared/RobotInfo.h"
00020 #include "Shared/get_time.h"
00021
00022
00023 #include "WorldModel2/FastSLAM/afsParticle.h"
00024
00025 #include "WorldModel2/FastSLAM/Configuration.h"
00026
00027 #include "WorldModel2/Poses.h"
00028
00029 #include <math.h>
00030
00031 #ifdef DEBUG_WM2B
00032 #include <iostream>
00033 using namespace std;
00034 #endif
00035
00036 WorldModel2Behavior::WorldModel2Behavior()
00037 : StateNode("WorldModel2Behavior", NULL), start(NULL), WM2(),
00038 standUp(), standUp_id(MotionManager::invalid_MC_ID)
00039 {
00040 #ifdef WANT_GLOBAL_MAP
00041
00042 WM2.enableKludge(WM2Kludge::LazyFastSLAM);
00043
00044
00045 WM2.setLandmark(0, 120, 800, 50);
00046 WM2.setLandmark(1, 470, 685, 50);
00047 WM2.setLandmark(2, 730, 410, 50);
00048 WM2.setLandmark(3, 800, 60, 50);
00049 WM2.setLandmark(4, 660, -285, 50);
00050 WM2.setLandmark(5, 375, -505, 50);
00051 WM2.setLandmark(6, 20, -550, 50);
00052 WM2.setLandmark(7, -320, -415, 50);
00053 WM2.setLandmark(8, -560, -140, 50);
00054 WM2.setLandmark(9, -630, 215, 50);
00055 WM2.setLandmark(10, -500, 550, 50);
00056 WM2.setLandmark(11, -195, 770, 50);
00057
00058
00059 WM2.fsDistribute(-630, 800, 800, -550);
00060 #endif
00061
00062
00063 standUp->setPlayTime(700);
00064 standUp->setOutputCmd(LFrLegOffset+RotatorOffset, OutputCmd(SP_LFR_JOINT, 1));
00065 standUp->setOutputCmd(LFrLegOffset+ElevatorOffset, OutputCmd(SP_LFR_SHLDR, 1));
00066 standUp->setOutputCmd(LFrLegOffset+KneeOffset, OutputCmd(SP_LFR_KNEE, 1));
00067 standUp->setOutputCmd(RFrLegOffset+RotatorOffset, OutputCmd(SP_RFR_JOINT, 1));
00068 standUp->setOutputCmd(RFrLegOffset+ElevatorOffset, OutputCmd(SP_RFR_SHLDR, 1));
00069 standUp->setOutputCmd(RFrLegOffset+KneeOffset, OutputCmd(SP_RFR_KNEE, 1));
00070 standUp->setOutputCmd(LBkLegOffset+RotatorOffset, OutputCmd(SP_LBK_JOINT, 1));
00071 standUp->setOutputCmd(LBkLegOffset+ElevatorOffset, OutputCmd(SP_LBK_SHLDR, 1));
00072 standUp->setOutputCmd(LBkLegOffset+KneeOffset, OutputCmd(SP_LBK_KNEE, 1));
00073 standUp->setOutputCmd(RBkLegOffset+RotatorOffset, OutputCmd(SP_RBK_JOINT, 1));
00074 standUp->setOutputCmd(RBkLegOffset+ElevatorOffset, OutputCmd(SP_RBK_SHLDR, 1));
00075 standUp->setOutputCmd(RBkLegOffset+KneeOffset, OutputCmd(SP_RBK_KNEE, 1));
00076
00077
00078 standUp_id = motman->addMotion(standUp, false);
00079 }
00080
00081 WorldModel2Behavior::~WorldModel2Behavior()
00082 {
00083
00084 }
00085
00086 void WorldModel2Behavior::setup()
00087 {
00088
00089 StateNode *Halt = addNode(new WaitNode("WM2B/Wait", this, standUp_id));
00090 StateNode *Gawk = addNode(new GawkNode("WM2B/Gawk", this, &WM2, standUp_id));
00091 StateNode *Walk = addNode(new WalkNode("WM2B/Walk", this, standUp_id));
00092
00093
00094 Halt->addTransition(new TimeOutTrans(Halt, Gawk, 1000));
00095 Gawk->addTransition(new TimeOutTrans(Gawk, Walk, 5000));
00096 Walk->addTransition(new TimeOutTrans(Walk, Halt, 4000));
00097
00098
00099 start = Halt;
00100
00101
00102 StateNode::setup();
00103 }
00104
00105 void WorldModel2Behavior::DoStart()
00106 {
00107 StateNode::DoStart();
00108 #ifdef WANT_GLOBAL_MAP
00109 WM2.enableMarkers();
00110 #endif
00111 start->DoStart();
00112 }
00113
00114 void WorldModel2Behavior::DoStop()
00115 {
00116
00117 StateNode::DoStop();
00118 #ifdef WANT_GLOBAL_MAP
00119 WM2.disableMarkers();
00120 #endif
00121 }
00122
00123
00124
00125
00126
00127
00128 WorldModel2Behavior::WalkNode::WalkNode(const char *title, StateNode *poppa,
00129 MotionManager::MC_ID su)
00130 : StateNode(title, poppa),
00131 walker_id(MotionManager::invalid_MC_ID),
00132 head_id(MotionManager::invalid_MC_ID),
00133 stand_id(su), walkstate(AHEAD)
00134 {
00135
00136 head_id = motman->addMotion(SharedObject<HeadPointerMC>());
00137
00138 walker_id = motman->addMotion(SharedObject<WalkMC>());
00139 }
00140
00141 WorldModel2Behavior::WalkNode::~WalkNode()
00142 {
00143
00144 motman->removeMotion(walker_id);
00145
00146 motman->removeMotion(head_id);
00147 }
00148
00149
00150 void WorldModel2Behavior::WalkNode::processEvent(const EventBase&)
00151 {
00152 WalkMC *walker;
00153
00154
00155 if(walkstate == TURN) {
00156 if(state->sensors[IRDistOffset] < THR_AHEAD) return;
00157
00158 walker = (WalkMC*)motman->checkoutMotion(walker_id);
00159
00160 walker->setTargetVelocity(180, 0, 0);
00161 motman->checkinMotion(walker_id);
00162 walkstate = AHEAD;
00163 }
00164
00165 else {
00166 if(state->sensors[IRDistOffset] > THR_TURN) return;
00167
00168 walker = (WalkMC*)motman->checkoutMotion(walker_id);
00169
00170 walker->setTargetVelocity(0, 0, 1);
00171 motman->checkinMotion(walker_id);
00172 walkstate = TURN;
00173 }
00174 }
00175
00176 void WorldModel2Behavior::WalkNode::DoStart()
00177 {
00178 #ifdef DEBUG_WM2B
00179 cout << "Walk: start at " << get_time() / 1000 << endl;
00180 #endif
00181
00182 StateNode::DoStart();
00183
00184 HeadPointerMC *headptr = (HeadPointerMC*)motman->checkoutMotion(head_id);
00185 headptr->setJoints(0.0, 0.0, 0.0);
00186 motman->checkinMotion(head_id);
00187
00188 walkstate = AHEAD;
00189 WalkMC *walker = (WalkMC*)motman->checkoutMotion(walker_id);
00190 walker->setTargetVelocity(180, 0, 0);
00191 motman->checkinMotion(walker_id);
00192
00193 erouter->addListener(this, EventBase::sensorEGID, SensorSourceID::UpdatedSID);
00194 }
00195
00196 void WorldModel2Behavior::WalkNode::DoStop()
00197 {
00198 #ifdef DEBUG_WM2B
00199 cout << "Walk: stop at " << get_time() / 1000 << endl;
00200 #endif
00201
00202
00203 erouter->removeListener(this, EventBase::sensorEGID,
00204 SensorSourceID::UpdatedSID);
00205
00206
00207 WalkMC *walker = (WalkMC*)motman->checkoutMotion(walker_id);
00208 walker->setTargetVelocity(0, 0, 0);
00209 motman->checkinMotion(walker_id);
00210 StateNode::DoStop();
00211 }
00212
00213
00214
00215
00216
00217 WorldModel2Behavior::GawkNode::GawkNode(const char *title, StateNode *poppa,
00218 WorldModel2 *WM,
00219 MotionManager::MC_ID su)
00220 : StateNode(title, poppa),
00221 WM2ref(WM),
00222 head_id(MotionManager::invalid_MC_ID),
00223 stand_id(su)
00224 {
00225
00226 }
00227
00228
00229 void WorldModel2Behavior::GawkNode::processEvent(const EventBase&)
00230 {
00231
00232
00233
00234 double now = ((double) get_time()) / 1000.0;
00235 double pan = sin(now) * (80.0*M_PI/180.0);
00236
00237 HeadPointerMC *headptr = (HeadPointerMC*)motman->checkoutMotion(head_id);
00238 headptr->setJoints(20*M_PI/180, pan, 0);
00239 motman->checkinMotion(head_id);
00240 }
00241
00242 void WorldModel2Behavior::GawkNode::DoStart()
00243 {
00244 #ifdef DEBUG_WM2B
00245 cout << "Gawk: start" << endl;
00246 #endif
00247
00248 StateNode::DoStart();
00249
00250
00251
00252
00253
00254
00255 head_id = motman->addMotion(SharedObject<HeadPointerMC>());
00256
00257 erouter->addTimer(this, 0, 5, true);
00258
00259 }
00260
00261 void WorldModel2Behavior::GawkNode::DoStop()
00262 {
00263 #ifdef DEBUG_WM2B
00264 cout << "Gawk: stop" << endl;
00265 #endif
00266
00267
00268 erouter->removeTimer(this);
00269
00270
00271
00272
00273
00274
00275 motman->removeMotion(head_id);
00276 StateNode::DoStop();
00277
00278 #ifdef DEBUG_WM2B
00279 #ifdef WANT_GLOBAL_MAP
00280
00281 afsParticle p;
00282 WM2ref->getFastSLAMMap(p);
00283
00284 cout << endl;
00285 cout << "plot(" << p.pose.x << ',' << p.pose.y << ",'g+')" << endl;
00286
00287 for(int i=0; i<AFS_NUM_LANDMARKS; ++i) {
00288 if(p.landmarks[i].state == PRIMING) continue;
00289
00290 cout << "plot(" << p.landmarks[i].mean.x << ','
00291 << p.landmarks[i].mean.y << ",'b*')" << endl;
00292
00293 cout << "plot_ellipse([" << p.landmarks[i].variance.x << ' '
00294 << p.landmarks[i].variance.xy << ';'
00295 << p.landmarks[i].variance.xy << ' '
00296 << p.landmarks[i].variance.y << "],"
00297 << p.landmarks[i].mean.x << ','
00298 << p.landmarks[i].mean.y << ");" << endl;
00299 }
00300 #endif
00301 #endif
00302 }
00303
00304
00305
00306
00307
00308 WorldModel2Behavior::WaitNode::WaitNode(const char *title, StateNode *poppa,
00309 MotionManager::MC_ID su)
00310 : StateNode(title, poppa),
00311 head_id(MotionManager::invalid_MC_ID),
00312 stand_id(su)
00313 {
00314
00315 }
00316
00317 void WorldModel2Behavior::WaitNode::DoStart()
00318 {
00319 #ifdef DEBUG_WM2B
00320 cout << "Wait: start" << endl;
00321 #endif
00322
00323 StateNode::DoStart();
00324
00325 head_id = motman->addMotion(SharedObject<HeadPointerMC>());
00326
00327 HeadPointerMC *headptr = (HeadPointerMC*)motman->checkoutMotion(head_id);
00328 headptr->setJoints(0, 0, 0);
00329 motman->checkinMotion(head_id);
00330 }
00331
00332 void WorldModel2Behavior::WaitNode::DoStop()
00333 {
00334 #ifdef DEBUG_WM2B
00335 cout << "Wait: stop" << endl;
00336 #endif
00337
00338
00339 motman->removeMotion(head_id);
00340 StateNode::DoStop();
00341 }