Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

WalkToTargetNode.cc

Go to the documentation of this file.
00001 #include "WalkToTargetNode.h"
00002 #include "Motion/HeadPointerMC.h"
00003 #include "Motion/WalkMC.h"
00004 #include "Motion/MMAccessor.h"
00005 #include "Events/VisionObjectEvent.h"
00006 #include "Shared/WorldState.h"
00007 #include "Behaviors/Transitions/TimeOutTrans.h"
00008 #include "Behaviors/Transitions/VisualTargetCloseTrans.h"
00009 
00010 void WalkToTargetNode::DoStart() {
00011   StateNode::DoStart();
00012 
00013   headpointer_id = motman->addPersistentMotion(SharedObject<HeadPointerMC>());
00014   walker_id = motman->addPersistentMotion(SharedObject<WalkMC>());
00015 
00016   erouter->addListener(this,EventBase::visObjEGID,tracking);
00017 }
00018 
00019 void WalkToTargetNode::DoStop() {
00020   erouter->removeListener(this);
00021 
00022   motman->removeMotion(headpointer_id);
00023   headpointer_id=MotionManager::invalid_MC_ID;
00024   motman->removeMotion(walker_id);
00025   walker_id=MotionManager::invalid_MC_ID;
00026 
00027   StateNode::DoStop();
00028 }
00029 
00030 //this could be cleaned up event-wise (only use a timer when out of view)
00031 void WalkToTargetNode::processEvent(const EventBase& event) {
00032   static float horiz=0,vert=0;
00033   const VisionObjectEvent *ve = dynamic_cast<const VisionObjectEvent*>(&event);
00034   if(ve!=NULL && event.getTypeID()==EventBase::statusETID) {
00035     horiz=ve->getCenterX();
00036     vert=ve->getCenterY();
00037   } else
00038     return;
00039 
00040   //cout << "Pos: " << horiz << ' ' << vert << endl;
00041 
00042   double tilt=state->outputs[HeadOffset+TiltOffset]-vert*M_PI/6;
00043   double pan=state->outputs[HeadOffset+PanOffset]-horiz*M_PI/7.5;
00044   if(tilt>outputRanges[HeadOffset+TiltOffset][MaxRange])
00045     tilt=outputRanges[HeadOffset+TiltOffset][MaxRange];
00046   if(tilt<outputRanges[HeadOffset+TiltOffset][MinRange]*3/4)
00047     tilt=outputRanges[HeadOffset+TiltOffset][MinRange]*3/4;
00048   if(pan>outputRanges[HeadOffset+PanOffset][MaxRange]*2/3)
00049     pan=outputRanges[HeadOffset+PanOffset][MaxRange]*2/3;
00050   if(pan<outputRanges[HeadOffset+PanOffset][MinRange]*2/3)
00051     pan=outputRanges[HeadOffset+PanOffset][MinRange]*2/3;
00052   {MMAccessor<HeadPointerMC>(headpointer_id)->setJoints(tilt,pan,0);} //note use of {}'s to limit scope
00053   
00054   {
00055     MMAccessor<WalkMC> walker(walker_id);
00056     if(pan<-.05 || pan>.05)
00057       walker->setTargetVelocity(100,0,pan);
00058     else
00059       walker->setTargetVelocity(160,0,0);
00060   }
00061 }
00062 
00063 Transition* WalkToTargetNode::newDefaultLostTrans(StateNode* dest) {
00064   return new TimeOutTrans(dest,1500,EventBase::visObjEGID,tracking);
00065 }
00066 
00067 Transition* WalkToTargetNode::newDefaultCloseTrans(StateNode* dest) {
00068   return new VisualTargetCloseTrans(dest,tracking);
00069 }
00070 
00071 
00072 /*! @file
00073  * @brief Implements WalkToTargetNode, a state node for walking towards a visual target
00074  * @author ejt (Creator)
00075  *
00076  * $Author: ejt $
00077  * $Name: tekkotsu-2_4_1 $
00078  * $Revision: 1.1 $
00079  * $State: Exp $
00080  * $Date: 2004/12/05 04:47:53 $
00081  */
00082 

Tekkotsu v2.4.1
Generated Tue Aug 16 16:32:49 2005 by Doxygen 1.4.4