00001
00002
00003 #ifndef INCLUDED_GroundPlaneBehavior_h_
00004 #define INCLUDED_GroundPlaneBehavior_h_
00005
00006 #include "Behaviors/BehaviorBase.h"
00007 #include "Motion/Kinematics.h"
00008 #include "Motion/PIDMC.h"
00009 #include "Motion/MotionManager.h"
00010 #include "Shared/ERS7Info.h"
00011 #include "Shared/ERS2xxInfo.h"
00012 #include "Shared/WorldState.h"
00013
00014
00015 class GroundPlaneBehavior : public BehaviorBase {
00016 public:
00017
00018 GroundPlaneBehavior()
00019 : BehaviorBase("GroundPlaneBehavior"),
00020 head_release(EventBase::buttonEGID,(state->robotDesign&WorldState::ERS7Mask)?(int)ERS7Info::HeadButOffset:(int)ERS2xxInfo::HeadFrButOffset,EventBase::activateETID,0),
00021 head_lock(EventBase::buttonEGID,(state->robotDesign&WorldState::ERS7Mask)?(int)ERS7Info::HeadButOffset:(int)ERS2xxInfo::HeadFrButOffset,EventBase::deactivateETID,0),
00022 clock(EventBase::timerEGID,0,EventBase::statusETID,250)
00023 { }
00024
00025 virtual void DoStart() {
00026 BehaviorBase::DoStart();
00027 erouter->addListener(this,head_release);
00028 erouter->addListener(this,head_lock);
00029 processEvent(clock);
00030 }
00031
00032 virtual void DoStop() {
00033 erouter->removeListener(this);
00034 BehaviorBase::DoStop();
00035 }
00036
00037 virtual void processEvent(const EventBase& e) {
00038 if(e==clock) {
00039
00040
00041
00042 NEWMAT::ColumnVector down=Kinematics::pack(state->sensors[BAccelOffset],
00043 -state->sensors[LAccelOffset],
00044 state->sensors[DAccelOffset]);
00045
00046
00047 cout << "I think leg " << kine->findUnusedLeg(down) << " is least used" << endl;
00048
00049
00050 NEWMAT::ColumnVector p=kine->calculateGroundPlane(down);
00051 cout << "Ground plane: " << p(1)<<"x + " << p(2)<<"y + " << p(3)<<"z = 1" << endl;
00052
00053
00054 NEWMAT::ColumnVector ray(4); ray(1)=ray(2)=0; ray(3)=ray(4)=1;
00055 NEWMAT::ColumnVector hit;
00056
00057 hit=kine->projectToPlane(CameraFrameOffset,ray,BaseFrameOffset,p,CameraFrameOffset);
00058 cout << "Intersection_camera: (" << hit(1)<<','<<hit(2)<<','<<hit(3)<<')'<<endl;
00059 hit=kine->projectToPlane(CameraFrameOffset,ray,BaseFrameOffset,p,BaseFrameOffset);
00060 cout << "Intersection_base: (" << hit(1)<<','<<hit(2)<<','<<hit(3)<<')'<<endl;
00061
00062 } else if(e==head_release) {
00063 motman->addPrunableMotion(SharedObject<PIDMC>(HeadOffset,HeadOffset+NumHeadJoints,0));
00064 erouter->addListener(this,clock);
00065 processEvent(clock);
00066 } else if(e==head_lock) {
00067 motman->addPrunableMotion(SharedObject<PIDMC>(HeadOffset,HeadOffset+NumHeadJoints,1));
00068 erouter->removeListener(this,clock);
00069 } else {
00070 ASSERT(false,"unprocessed event " << e.getName() << endl);
00071 }
00072 }
00073
00074 static std::string getClassDescription() { return "Reports the location of the center of the camera image on the ground plane"; }
00075 virtual std::string getDescription() const { return getClassDescription(); }
00076
00077 protected:
00078 EventBase head_release;
00079 EventBase head_lock;
00080 EventBase clock;
00081 };
00082
00083
00084
00085
00086
00087
00088
00089
00090
00091
00092
00093
00094 #endif