Tekkotsu 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 "Shared/RobotInfo.h"
00009 #include "Shared/ERS7Info.h"
00010 #ifdef TGT_HAS_IR_DISTANCE
00011 #  include "Behaviors/Transitions/VisualTargetCloseTrans.h"
00012 #endif
00013 
00014 void WalkToTargetNode::DoStart() {
00015   StateNode::DoStart();
00016 
00017   headpointer_id = motman->addPersistentMotion(SharedObject<HeadPointerMC>());
00018   walker_id = motman->addPersistentMotion(SharedObject<WalkMC>());
00019 
00020   erouter->addListener(this,EventBase::visObjEGID,tracking);
00021 }
00022 
00023 void WalkToTargetNode::DoStop() {
00024   erouter->removeListener(this);
00025 
00026   motman->removeMotion(headpointer_id);
00027   headpointer_id=MotionManager::invalid_MC_ID;
00028   motman->removeMotion(walker_id);
00029   walker_id=MotionManager::invalid_MC_ID;
00030 
00031   StateNode::DoStop();
00032 }
00033 
00034 //this could be cleaned up event-wise (only use a timer when out of view)
00035 void WalkToTargetNode::processEvent(const EventBase& event) {
00036   static float horiz=0,vert=0;
00037   const VisionObjectEvent *ve = dynamic_cast<const VisionObjectEvent*>(&event);
00038   if(ve!=NULL && event.getTypeID()==EventBase::statusETID) {
00039     horiz=ve->getCenterX();
00040     vert=ve->getCenterY();
00041   } else
00042     return;
00043 
00044   // for portability, look to see if the host hardware has a head pan & tilt joints
00045   const unsigned int panIdx = capabilities.findOutputOffset(ERS7Info::outputNames[ERS7Info::HeadOffset+ERS7Info::PanOffset]);
00046   const unsigned int tiltIdx = capabilities.findOutputOffset(ERS7Info::outputNames[ERS7Info::HeadOffset+ERS7Info::TiltOffset]);
00047   if(panIdx==-1U || tiltIdx==-1U)
00048     return; // guess not...
00049   
00050   //cout << "Pos: " << horiz << ' ' << vert << endl;
00051 
00052   double tilt=state->outputs[tiltIdx]-vert*M_PI/6;
00053   double pan=state->outputs[panIdx]-horiz*M_PI/7.5;
00054   if(tilt>outputRanges[tiltIdx][MaxRange])
00055     tilt=outputRanges[tiltIdx][MaxRange];
00056   if(tilt<outputRanges[tiltIdx][MinRange]*3/4)
00057     tilt=outputRanges[tiltIdx][MinRange]*3/4;
00058   if(pan>outputRanges[panIdx][MaxRange]*2/3)
00059     pan=outputRanges[panIdx][MaxRange]*2/3;
00060   if(pan<outputRanges[panIdx][MinRange]*2/3)
00061     pan=outputRanges[panIdx][MinRange]*2/3;
00062   MMAccessor<HeadPointerMC>(headpointer_id)->setJoints(tilt,pan,0); // note no variable name, one-line scope
00063   
00064   {
00065     MMAccessor<WalkMC> walker(walker_id);
00066     if(pan<-.05 || pan>.05)
00067       walker->setTargetVelocity(100,0,pan);
00068     else
00069       walker->setTargetVelocity(160,0,0);
00070   }
00071 }
00072 
00073 Transition* WalkToTargetNode::newDefaultLostTrans(StateNode* dest) {
00074   return new TimeOutTrans(dest,1500,EventBase::visObjEGID,tracking);
00075 }
00076 
00077 Transition* WalkToTargetNode::newDefaultCloseTrans(StateNode* dest) {
00078 #ifdef TGT_HAS_IR_DISTANCE
00079   return new VisualTargetCloseTrans(dest,tracking);
00080 #else
00081   (void)dest;
00082   return NULL;
00083 #endif
00084 }
00085 
00086 
00087 /*! @file
00088  * @brief Implements WalkToTargetNode, a state node for walking towards a visual target
00089  * @author ejt (Creator)
00090  *
00091  * $Author: ejt $
00092  * $Name: tekkotsu-4_0 $
00093  * $Revision: 1.4 $
00094  * $State: Exp $
00095  * $Date: 2007/08/28 18:05:19 $
00096  */

Tekkotsu v4.0
Generated Thu Nov 22 00:54:56 2007 by Doxygen 1.5.4