StareAtPawBehavior2.hGo to the documentation of this file.00001
00002 #ifndef INCLUDED_StareAtPawBehavior2_h_
00003 #define INCLUDED_StareAtPawBehavior2_h_
00004
00005 #include "Behaviors/BehaviorBase.h"
00006 #include "Motion/PIDMC.h"
00007 #include "Motion/HeadPointerMC.h"
00008 #include "Motion/PostureMC.h"
00009 #include "Motion/MotionManager.h"
00010 #include "Shared/SharedObject.h"
00011 #include "Motion/roboop/robot.h"
00012 #include "Shared/Config.h"
00013 #include "Motion/Kinematics.h"
00014
00015
00016
00017 class StareAtPawBehavior2 : public BehaviorBase {
00018 public:
00019
00020 StareAtPawBehavior2()
00021 : BehaviorBase("StareAtPawBehavior2"), lastLeg(LFrLegOrder), pointID(MotionManager::invalid_MC_ID)
00022 { }
00023
00024 virtual void DoStart() {
00025 BehaviorBase::DoStart();
00026 pointID=motman->addPersistentMotion(SharedObject<HeadPointerMC>());
00027 erouter->addListener(this,EventBase::sensorEGID);
00028 erouter->addListener(this,EventBase::buttonEGID);
00029 if(state->robotDesign == WorldState::ERS7Mask)
00030 sout->printf("NOTICE: The ERS-7 has a rather \"sticky\" nod joint\n"
00031 "(the upper tilt joint). This can cause it to hesitate\n"
00032 "or altogether fail to precisely center the target position\n"
00033 "vertically in the center of the image...\n");
00034 }
00035
00036 virtual void DoStop() {
00037 motman->removeMotion(pointID);
00038 pointID=MotionManager::invalid_MC_ID;
00039 erouter->removeListener(this);
00040 BehaviorBase::DoStop();
00041 }
00042
00043 virtual void processEvent(const EventBase& e) {
00044 if(e.getGeneratorID()==EventBase::buttonEGID) {
00045
00046 if(e.getSourceID()==LFrPawOffset) {
00047 lastLeg=LFrLegOrder;
00048 } else if(e.getSourceID()==RFrPawOffset) {
00049 lastLeg=RFrLegOrder;
00050 } else
00051 return;
00052 if(e.getTypeID()==EventBase::activateETID) {
00053 unsigned int lastlegoff=LegOffset+lastLeg*JointsPerLeg;
00054 SharedObject<PIDMC> relaxLeg(lastlegoff,lastlegoff+JointsPerLeg,0);
00055 motman->addPrunableMotion(relaxLeg);
00056 } else if(e.getTypeID()==EventBase::deactivateETID) {
00057 unsigned int lastlegoff=LegOffset+lastLeg*JointsPerLeg;
00058 SharedObject<PIDMC> tightLeg(lastlegoff,lastlegoff+JointsPerLeg,1);
00059 motman->addPrunableMotion(tightLeg);
00060 }
00061
00062 } else if(e.getGeneratorID()==EventBase::sensorEGID) {
00063
00064
00065 const char * ipname= (lastLeg==LFrLegOrder) ? "ToeLFrPaw" : "ToeRFrPaw";
00066
00067
00068 NEWMAT::ColumnVector Pobj=kine->getJointInterestPoint(BaseFrameOffset,ipname);
00069
00070
00071 MMAccessor<HeadPointerMC>(pointID)->lookAtPoint(Pobj(1),Pobj(2),Pobj(3));
00072
00073
00074
00075 } else {
00076 serr->printf("StareAtPawBehavior2: Unhandled event %s\n",e.getName().c_str());
00077 }
00078 }
00079
00080 static std::string getClassDescription() { return "Uses kinematics to track the paw which last received a button press with the camera"; }
00081 virtual std::string getDescription() const { return getClassDescription(); }
00082
00083 protected:
00084 LegOrder_t lastLeg;
00085 MotionManager::MC_ID pointID;
00086 };
00087
00088
00089
00090
00091
00092
00093
00094
00095
00096
00097
00098
00099 #endif
|