Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

StareAtPawBehavior2.cc

Go to the documentation of this file.
00001 #include "StareAtPawBehavior2.h"
00002 
00003 #include "Events/EventRouter.h"
00004 #include "Shared/WorldState.h"
00005 #include "Motion/MMAccessor.h"
00006 #include "Motion/PIDMC.h"
00007 #include "Motion/HeadPointerMC.h"
00008 #include "Motion/PostureMC.h"
00009 #include "IPC/SharedObject.h"
00010 #include "Motion/roboop/robot.h"
00011 #include "Shared/Config.h"
00012 #include "Motion/Kinematics.h"
00013 
00014 // these are for drawing into the camera frame
00015 #include "Shared/ProjectInterface.h"
00016 #include "Vision/Graphics.h"
00017 #include "Events/FilterBankEvent.h"
00018 #include "Vision/RawCameraGenerator.h"
00019 #include "Behaviors/Mon/RawCamBehavior.h"
00020 
00021 //better to put this here instead of the header
00022 using namespace std; 
00023 
00024 void StareAtPawBehavior2::DoStart() {
00025   BehaviorBase::DoStart(); // do this first
00026 
00027   pointID=motman->addPersistentMotion(SharedObject<HeadPointerMC>());
00028 
00029   //sensorEGID to update head pointer whenever we get new info about joint positions
00030   erouter->addListener(this,EventBase::sensorEGID);
00031   //buttonEGID to switch the target when the LFr or RFr button is pressed (ignores the others)
00032   erouter->addListener(this,EventBase::buttonEGID);
00033   //visRawCameraEGID to draw a dot on the camera image where we think the toe is
00034   erouter->addListener(this,EventBase::visRawCameraEGID,ProjectInterface::visRawCameraSID,EventBase::statusETID);
00035     
00036   if(state->robotDesign == WorldState::ERS7Mask)
00037     sout->printf("NOTICE: The ERS-7 has a rather \"sticky\" nod joint\n"
00038                  "(the upper tilt joint).  This can cause it to hesitate\n"
00039                  "or altogether fail to precisely center the target position\n"
00040                  "vertically in the center of the image...\n");
00041 }
00042 
00043 void StareAtPawBehavior2::DoStop() {
00044   motman->removeMotion(pointID);
00045   pointID=MotionManager::invalid_MC_ID;
00046   erouter->removeListener(this);
00047   BehaviorBase::DoStop(); // do this last
00048 }
00049   
00050 void StareAtPawBehavior2::processEvent(const EventBase& e) {
00051   if(e.getGeneratorID()==EventBase::buttonEGID) {
00052     //*******************************//
00053     //*** Switch the "active" toe ***//
00054     //*******************************//
00055 
00056     if(e.getSourceID()==LFrPawOffset) {
00057       lastLeg=LFrLegOrder;
00058     } else if(e.getSourceID()==RFrPawOffset) {
00059       lastLeg=RFrLegOrder;
00060     } else
00061       return;
00062     if(e.getTypeID()==EventBase::activateETID) {
00063       unsigned int lastlegoff=LegOffset+lastLeg*JointsPerLeg;
00064       SharedObject<PIDMC> relaxLeg(lastlegoff,lastlegoff+JointsPerLeg,0);
00065       motman->addPrunableMotion(relaxLeg);
00066     } else if(e.getTypeID()==EventBase::deactivateETID) {
00067       unsigned int lastlegoff=LegOffset+lastLeg*JointsPerLeg;
00068       SharedObject<PIDMC> tightLeg(lastlegoff,lastlegoff+JointsPerLeg,1);
00069       motman->addPrunableMotion(tightLeg);
00070     }
00071 
00072 
00073   } else if(e.getGeneratorID()==EventBase::sensorEGID) {
00074     //***************************************************************************//
00075     //*** Update the position of the head based on new information on the toe ***//
00076     //***************************************************************************//
00077 
00078     //Which paw do we want?
00079     const char * ipname= (lastLeg==LFrLegOrder) ? "ToeLFrPaw" : "ToeRFrPaw";
00080 
00081     //Ask kinematics for current location of that paw (this is the "objective" aka target)
00082     NEWMAT::ColumnVector Pobj=kine->getJointInterestPoint(BaseFrameOffset,ipname);
00083 
00084     //Now point the head there
00085     MMAccessor<HeadPointerMC>(pointID)->lookAtPoint(Pobj(1),Pobj(2),Pobj(3)); //keep head as far away as possible
00086     //Alternative method:
00087     // MMAccessor<HeadPointerMC>(pointID)->lookAtPoint(Pobj(1),Pobj(2),Pobj(3),80); //keep head 80mm away
00088 
00089 
00090   } else if(e.getGeneratorID()==EventBase::visRawCameraEGID) {
00091     //**************************************************************//
00092     //*** Draw into the camera frame to put a box around the toe ***//
00093     //**************************************************************//
00094       
00095     //same as above, but get the toe position relative to the camera
00096     const char * ipname= (lastLeg==LFrLegOrder) ? "ToeLFrPaw" : "ToeRFrPaw";
00097     NEWMAT::ColumnVector Pobj=kine->getJointInterestPoint(CameraFrameOffset,ipname);
00098     float x,y;
00099     config->vision.computePixel(Pobj(1),Pobj(2),Pobj(3),x,y);
00100     y*=config->vision.aspectRatio;
00101     float r=atan2(10.f,sqrt(Pobj.SumSquare()))/CameraHorizFOV/2;
00102       
00103     //draw into the layer which will be sent to the gui
00104     //not doing any error checking on the type of the event or its source... cross your fingers ;)
00105     const FilterBankEvent& fbe=dynamic_cast<const FilterBankEvent&>(e);
00106     unsigned chan=RawCameraGenerator::CHAN_Y;
00107     unsigned int layer=RawCamBehavior::getSourceLayer(chan,fbe.getNumLayers());
00108     Graphics g(*fbe.getSource(), layer, chan);
00109     g.setColor(255);
00110     g.drawRect(x-r,y-r,r*2,r*2);
00111       
00112     /*
00113     //this method of drawing gets the pixel coordinates without applying
00114     // camera calibration via config->vision.computePixel
00115     g.setColor(192);
00116     r=atan2(5.f,sqrt(Pobj.SumSquare()))/CameraHorizFOV/2; //draws a half-size rectangle
00117     x=Pobj(1)/(Pobj(3)*tan(CameraHorizFOV/2));
00118     y=Pobj(2)/(Pobj(3)*tan(CameraVertFOV/2));
00119     y*=config->vision.aspectRatio;
00120     g.drawRect(x-r,y-r,r*2,r*2);
00121     */
00122 
00123   } else {
00124     serr->printf("StareAtPawBehavior2: Unhandled event %s\n",e.getName().c_str());
00125   }
00126 }
00127 
00128 /*! @file
00129  * @brief Implements StareAtPawBehavior2, which uses new-style ROBOOP kinematics to track the paw which last received a button press with the camera
00130  * @author ejt (Creator)
00131  *
00132  * $Author: ejt $
00133  * $Name: tekkotsu-2_4_1 $
00134  * $Revision: 1.3 $
00135  * $State: Exp $
00136  * $Date: 2005/08/04 21:09:26 $
00137  */

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