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("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(); // 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   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; //!< event template to match to signal the head's PID joints should be relaxed
00079   EventBase head_lock;    //!< event template to match to signal the head's PID joints should be powered up again
00080   EventBase clock;        //!< event template to match to signal a new round of calculations should be performed
00081 };
00082 
00083 /*! @file
00084  * @brief Defines GroundPlaneBehavior, which reports the location of the center of the camera image on the ground plane
00085  * @author ejt (Creator)
00086  *
00087  * $Author: ejt $
00088  * $Name: tekkotsu-2_2_2 $
00089  * $Revision: 1.8 $
00090  * $State: Exp $
00091  * $Date: 2004/12/23 01:47:06 $
00092  */
00093 
00094 #endif

Tekkotsu v2.2.2
Generated Tue Jan 4 15:43:13 2005 by Doxygen 1.4.0