Homepage | Demos | Overview | Downloads | Dev. Resources | Reference | Credits |
StareAtBallBehavior.ccGo to the documentation of this file.00001 #include "StareAtBallBehavior.h" 00002 #include "Events/EventRouter.h" 00003 #include "Events/VisionObjectEvent.h" 00004 #include "Shared/WorldState.h" 00005 #include "Motion/HeadPointerMC.h" 00006 #include "Shared/ProjectInterface.h" 00007 00008 //! Converts degrees to radians 00009 inline double DtoR(double deg) { return (deg/180.0*M_PI); } 00010 00011 void StareAtBallBehavior::DoStart() { 00012 BehaviorBase::DoStart(); 00013 headpointer_id = motman->addPersistentMotion(SharedObject<HeadPointerMC>()); 00014 erouter->addListener(this,EventBase::visObjEGID,ProjectInterface::visPinkBallSID); 00015 } 00016 00017 void StareAtBallBehavior::DoStop() { 00018 erouter->removeListener(this); 00019 motman->removeMotion(headpointer_id); 00020 BehaviorBase::DoStop(); 00021 } 00022 00023 //this could be cleaned up event-wise (only use a timer when out of view) 00024 void StareAtBallBehavior::processEvent(const EventBase& event) { 00025 /*if(event.getSourceID()==ProjectInterface::visBlueBallSID) { 00026 cout << "BLUE" << endl; 00027 } else if(event.getSourceID()==ProjectInterface::visPinkBallSID) { 00028 cout << "PINK" << endl; 00029 } 00030 */ 00031 00032 static float horiz=0,vert=0; 00033 if(event.getGeneratorID()==EventBase::visObjEGID && event.getTypeID()==EventBase::statusETID) { 00034 horiz=static_cast<const VisionObjectEvent*>(&event)->getCenterX(); 00035 vert=static_cast<const VisionObjectEvent*>(&event)->getCenterY(); 00036 } 00037 00038 //cout << horiz << ' ' << vert << endl; 00039 00040 // Very simple visual servoing control -- move the head in the direction of the target 00041 double tilt=state->outputs[HeadOffset+TiltOffset]-vert*M_PI/7; 00042 double pan=state->outputs[HeadOffset+PanOffset]-horiz*M_PI/6; 00043 HeadPointerMC * headpointer= (HeadPointerMC*)motman->checkoutMotion(headpointer_id); 00044 #ifdef TGT_ERS7 00045 //on an ers-7, we want to set the nod joint to look up, since tilt can only look down 00046 headpointer->setJoints(tilt,pan,outputRanges[HeadOffset+NodOffset][MaxRange]); 00047 #else 00048 //on other models (we'll just assume ers-2xx), center the roll joint 00049 headpointer->setJoints(tilt,pan,0); 00050 #endif 00051 motman->checkinMotion(headpointer_id); 00052 } 00053 00054 /*! @file 00055 * @brief Implements StareAtBallBehavior, which points the head at the ball 00056 * @author tss (Creator) 00057 */ 00058 00059 |
Tekkotsu v2.4.1 |
Generated Tue Aug 16 16:32:49 2005 by Doxygen 1.4.4 |