StareAtPawBehavior2.ccGo 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
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
00022 using namespace std;
00023
00024 void StareAtPawBehavior2::DoStart() {
00025 BehaviorBase::DoStart();
00026
00027 pointID=motman->addPersistentMotion(SharedObject<HeadPointerMC>());
00028
00029
00030 erouter->addListener(this,EventBase::sensorEGID);
00031
00032 erouter->addListener(this,EventBase::buttonEGID);
00033
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();
00048 }
00049
00050 void StareAtPawBehavior2::processEvent(const EventBase& e) {
00051 if(e.getGeneratorID()==EventBase::buttonEGID) {
00052
00053
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
00076
00077
00078
00079 const char * ipname= (lastLeg==LFrLegOrder) ? "ToeLFrPaw" : "ToeRFrPaw";
00080
00081
00082 NEWMAT::ColumnVector Pobj=kine->getJointInterestPoint(BaseFrameOffset,ipname);
00083
00084
00085 MMAccessor<HeadPointerMC>(pointID)->lookAtPoint(Pobj(1),Pobj(2),Pobj(3));
00086
00087
00088
00089
00090 } else if(e.getGeneratorID()==EventBase::visRawCameraEGID) {
00091
00092
00093
00094
00095
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
00104
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
00114
00115
00116
00117
00118
00119
00120
00121
00122
00123 } else {
00124 serr->printf("StareAtPawBehavior2: Unhandled event %s\n",e.getName().c_str());
00125 }
00126 }
00127
00128
00129
00130
00131
00132
00133
00134
00135
00136
00137
|