Homepage Demos Overview Downloads Tutorials Reference
Credits

GroundPlaneBehavior.h

Go to the documentation of this file.
00001 //-*-c++-*-
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 //! Reports the location of the center of the camera image on the ground plane
00015 class GroundPlaneBehavior : public BehaviorBase {
00016 public:
00017   //! constructor
00018   GroundPlaneBehavior()
00019     : BehaviorBase(),
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(); // do this first
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(); // do this last
00035   }
00036 
00037   virtual void processEvent(const EventBase& e) {
00038     if(e==clock) {
00039       //This is the direction gravity is pulling - probably a good way to find out
00040       //the attitude of the robot, assuming it is not moving.
00041       //** Note that the LAccel sensor needs to be negated to match the coordinate system **//
00042       NEWMAT::ColumnVector down=Kinematics::pack(state->sensors[BAccelOffset],
00043                                                  -state->sensors[LAccelOffset],
00044                                                  state->sensors[DAccelOffset]);
00045       
00046       //Just for kicks, lets report which leg is off the ground
00047       cout << "I think leg " << kine->findUnusedLeg(down) << " is least used" << endl;
00048 
00049       //First we determine the ground plane
00050       NEWMAT::ColumnVector p=kine->calculateGroundPlane(down);
00051       cout << "Ground plane: " << p(1)<<"x + " << p(2)<<"y + " << p(3)<<"z = 1" << endl;
00052 
00053       //Project to ground plane - we do it twice here, once for camera frame and once for base frame
00054       NEWMAT::ColumnVector ray(4); ray(1)=ray(2)=0; ray(3)=ray(4)=1;
00055       NEWMAT::ColumnVector hit;
00056       //cout <<"Current head:\n"<<state->outputs[HeadOffset] <<' '<< state->outputs[HeadOffset+1] <<' '<< state->outputs[HeadOffset+2] << endl <<kine->getTransform(CameraFrameOffset);
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   virtual std::string getName() const { return "GroundPlaneBehavior"; }
00075 
00076   static std::string getClassDescription() { return "Reports the location of the center of the camera image on the ground plane"; }
00077   
00078 protected:
00079   EventBase head_release, head_lock, clock;
00080 };
00081 
00082 /*! @file
00083  * @brief Defines GroundPlaneBehavior, which reports the location of the center of the camera image on the ground plane
00084  * @author ejt (Creator)
00085  *
00086  * $Author: ejt $
00087  * $Name: tekkotsu-2_2 $
00088  * $Revision: 1.6 $
00089  * $State: Exp $
00090  * $Date: 2004/10/17 01:16:10 $
00091  */
00092 
00093 #endif

Tekkotsu v2.2
Generated Tue Oct 19 14:19:14 2004 by Doxygen 1.3.9.1