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

WorldModel2Behavior.cc

Go to the documentation of this file.
00001 /* Implements a stateful behavior specifically designed to feed data to the
00002  * second WorldModel.
00003  *
00004  * Started 13 February 2003, tss
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 // for afsParticle
00023 #include "WorldModel2/FastSLAM/afsParticle.h"
00024 // for AFS_NUM_LANDMARKS
00025 #include "WorldModel2/FastSLAM/Configuration.h"
00026 // for stopgap observational poses
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()    // Constructor
00037   : StateNode("WorldModel2Behavior", NULL), start(NULL), WM2(),
00038     standUp(), standUp_id(MotionManager::invalid_MC_ID)
00039 {
00040 #ifdef WANT_GLOBAL_MAP
00041   // Enable the LazyFastSLAM kludge
00042   WM2.enableKludge(WM2Kludge::LazyFastSLAM);
00043 
00044   // Initialize WM2 with landmark locations
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   // Spread FastSLAM particles around landmarks
00059   WM2.fsDistribute(-630, 800, 800, -550);
00060 #endif
00061 
00062   // Set up motion sequence for standing erect
00063   standUp->setPlayTime(700);  // a procedure it must do in 700 milliseconds
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   // Insert into motman
00078   standUp_id = motman->addMotion(standUp, false);
00079 }
00080 
00081 WorldModel2Behavior::~WorldModel2Behavior()   // Destructor
00082 {
00083   // nothing yet
00084 }
00085 
00086 void WorldModel2Behavior::setup()     // Initial setup
00087 {
00088   // Create state machine nodes
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   // Indicate transitions among states--all of these are timer driven
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   // Denote start state
00099   start = Halt;
00100 
00101   // Superclass state machine setup
00102   StateNode::setup();
00103 }
00104 
00105 void WorldModel2Behavior::DoStart()     // Behavior startup
00106 {
00107   StateNode::DoStart(); // superclass startup stuff
00108 #ifdef WANT_GLOBAL_MAP
00109   WM2.enableMarkers();  // start marker detection
00110 #endif
00111   start->DoStart(); // now start the state machine!
00112 }
00113 
00114 void WorldModel2Behavior::DoStop()      // Behavior stop
00115 {
00116   // nothing yet except...
00117   StateNode::DoStop();  // superclass stopping stuff
00118 #ifdef WANT_GLOBAL_MAP
00119   WM2.disableMarkers(); // stop marker detection
00120 #endif
00121 }
00122 
00123 
00124 
00125 
00126   // Private classes for specific behavior states
00127   // This one does a random walk
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   // get head moving controls
00136   head_id = motman->addMotion(SharedObject<HeadPointerMC>());
00137   // get walking controls
00138   walker_id = motman->addMotion(SharedObject<WalkMC>());
00139 }
00140 
00141 WorldModel2Behavior::WalkNode::~WalkNode()    // Destructor
00142 {
00143   // release walking controls
00144   motman->removeMotion(walker_id);
00145   // release head motion controls
00146   motman->removeMotion(head_id);
00147 }
00148 
00149 // IR obst. avoidance
00150 void WorldModel2Behavior::WalkNode::processEvent(const EventBase&)
00151 {
00152   WalkMC *walker;
00153 
00154   // If we're turning, see if we can stop turning
00155   if(walkstate == TURN) {
00156     if(state->sensors[IRDistOffset] < THR_AHEAD) return; // Still too close
00157     // clear water, all ahead full!
00158     walker = (WalkMC*)motman->checkoutMotion(walker_id);
00159     //walker->setTargetVelocity(0, 0, 0);
00160     walker->setTargetVelocity(180, 0, 0);
00161     motman->checkinMotion(walker_id);
00162     walkstate = AHEAD;
00163   }
00164   // If we're going forward, see if we have to turn.
00165   else {
00166     if(state->sensors[IRDistOffset] > THR_TURN) return; // no obstacles near
00167     // iceberg ahead! hard a'port!
00168     walker = (WalkMC*)motman->checkoutMotion(walker_id);
00169     //walker->setTargetVelocity(0, 0, 0);
00170     walker->setTargetVelocity(0, 0, 1);
00171     motman->checkinMotion(walker_id);
00172     walkstate = TURN;
00173   }
00174 }
00175 
00176 void WorldModel2Behavior::WalkNode::DoStart()   // Behavior startup
00177 {
00178 #ifdef DEBUG_WM2B
00179   cout << "Walk: start at " << get_time() / 1000 << endl;
00180 #endif
00181 
00182   StateNode::DoStart();
00183   // point head forward
00184   HeadPointerMC *headptr = (HeadPointerMC*)motman->checkoutMotion(head_id);
00185   headptr->setJoints(0.0, 0.0, 0.0);
00186   motman->checkinMotion(head_id);
00187   // let's start moving!
00188   walkstate = AHEAD;
00189   WalkMC *walker = (WalkMC*)motman->checkoutMotion(walker_id);
00190   walker->setTargetVelocity(180, 0, 0);
00191   motman->checkinMotion(walker_id);
00192   // turn on IR sensors
00193   erouter->addListener(this, EventBase::sensorEGID, SensorSourceID::UpdatedSID);
00194 }
00195 
00196 void WorldModel2Behavior::WalkNode::DoStop()    // Behavior stop
00197 {
00198 #ifdef DEBUG_WM2B
00199   cout << "Walk: stop at " << get_time() / 1000 << endl;
00200 #endif
00201 
00202   // turn off IR sensors
00203   erouter->removeListener(this, EventBase::sensorEGID,
00204         SensorSourceID::UpdatedSID);
00205   // stop moving
00206   // THIS CODE HAS NO EFFECT--motion is stopped before DoStop is called!!!
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 // This one wags the head around
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   // nothing yet
00226 }
00227 
00228 // Head wag timer
00229 void WorldModel2Behavior::GawkNode::processEvent(const EventBase&)
00230 {
00231   // This routine should only be called by the timer event, so we won't
00232   // bother checking who rang us...
00233   // these two lines cause sinusoidal head oscillation
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()   // Behavior startup
00243 {
00244 #ifdef DEBUG_WM2B
00245   cout << "Gawk: start" << endl;
00246 #endif
00247 
00248   StateNode::DoStart();
00249   // stand up now
00250   // STANDING IS DISABLED FOR NOW
00251   //MotionSequenceMC<MotionSequence::SizeSmall> *stand = (MotionSequenceMC<MotionSequence::SizeSmall>*)motman->checkoutMotion(stand_id);
00252   //stand->play();
00253   //motman->checkinMotion(stand_id);
00254   // get head moving controls
00255   head_id = motman->addMotion(SharedObject<HeadPointerMC>());
00256   // turn on head wag timer.
00257   erouter->addTimer(this, 0, 5, true);
00258   // Head starts wagging in processEvent
00259 }
00260 
00261 void WorldModel2Behavior::GawkNode::DoStop()    // Behavior stop
00262 {
00263 #ifdef DEBUG_WM2B
00264   cout << "Gawk: stop" << endl;
00265 #endif
00266 
00267   // turn off head timer
00268   erouter->removeTimer(this);
00269   // point head forward
00270   // THIS CODE HAS NO EFFECT--motion is stopped before DoStop is called!!!
00271   //HeadPointerMC *headptr = (HeadPointerMC*)motman->checkoutMotion(head_id);
00272   //headptr->setJoints(0.0, 0.0, 0.0);
00273   //motman->checkinMotion(head_id);
00274   // release head moving controls
00275   motman->removeMotion(head_id);
00276   StateNode::DoStop();
00277 
00278 #ifdef DEBUG_WM2B
00279 #ifdef WANT_GLOBAL_MAP
00280   // Generate MATLAB code onto the screen to see where we are.
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 // This one lets the AIBO have a reflective pause
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   // nothing yet
00315 }
00316 
00317 void WorldModel2Behavior::WaitNode::DoStart()   // Behavior startup
00318 {
00319 #ifdef DEBUG_WM2B
00320   cout << "Wait: start" << endl;
00321 #endif
00322 
00323   StateNode::DoStart();
00324   // get head moving controls
00325   head_id = motman->addMotion(SharedObject<HeadPointerMC>());
00326   // stare dead ahead (har har)
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()    // Behavior stop
00333 {
00334 #ifdef DEBUG_WM2B
00335   cout << "Wait: stop" << endl;
00336 #endif
00337 
00338   // release head moving controls
00339   motman->removeMotion(head_id);
00340   StateNode::DoStop();
00341 }

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