Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

DrawSkeletonBehavior.h

Go to the documentation of this file.
00001 //-*-c++-*-
00002 #ifndef INCLUDED_DrawSkeletonBehavior_h_
00003 #define INCLUDED_DrawSkeletonBehavior_h_
00004 
00005 #include "Behaviors/BehaviorBase.h"
00006 #include "Events/EventRouter.h"
00007 
00008 #include "Shared/ProjectInterface.h"
00009 #include "Events/VisionObjectEvent.h"
00010 #include "Events/FilterBankEvent.h"
00011 #include "Vision/Graphics.h"
00012 #include "Shared/Config.h"
00013 #include "Vision/RawCameraGenerator.h"
00014 #include "Behaviors/Mon/RawCamBehavior.h"
00015 #include "Motion/Kinematics.h"
00016 #include "Shared/newmat/newmatio.h"
00017 
00018 //! Draws the kinematics "skeleton" on the camera frame
00019 class DrawSkeletonBehavior : public BehaviorBase {
00020 public:
00021   //! constructor
00022   DrawSkeletonBehavior() : BehaviorBase("DrawSkeletonBehavior") {}
00023 
00024   virtual void DoStart() {
00025     BehaviorBase::DoStart(); // do this first (required)
00026     erouter->addListener(this, EventBase::visRawCameraEGID,ProjectInterface::visRawCameraSID,EventBase::statusETID);
00027   }
00028 
00029   virtual void DoStop() {
00030     erouter->removeListener(this); //generally a good idea, unsubscribe all
00031     BehaviorBase::DoStop(); // do this last (required)
00032   }
00033 
00034   virtual void processEvent(const EventBase& e) {
00035     //We're assuming e is a filter bank event and that its source is not null...
00036     //You could add error checking code for that.
00037     const FilterBankEvent& fbe=dynamic_cast<const FilterBankEvent&>(e);
00038 
00039     unsigned int chan=RawCameraGenerator::CHAN_U;
00040     unsigned int layer=RawCamBehavior::getSourceLayer(chan,fbe.getNumLayers());
00041     unsigned char color=48;
00042     float originBoxSize=.1;
00043     
00044     Graphics g(*fbe.getSource(),layer,chan);
00045     g.setColor(color);
00046 
00047     NEWMAT::ColumnVector p=Kinematics::pack(0,0,0);
00048     for(unsigned int leg=0; leg<NumLegs; leg++) {
00049       float lastx,lasty,curx,cury;
00050       bool lastFront=false;
00051       bool front=getCameraPoint(LegOffset+leg*JointsPerLeg+ElevatorOffset,p,lastx,lasty);
00052       if(front) {
00053         g.setColor(-color);
00054         g.drawRect(lastx-originBoxSize/2,lasty-originBoxSize/2,originBoxSize,originBoxSize);
00055       }
00056       for(unsigned int j=KneeOffset; j<JointsPerLeg; j++) {
00057         //actually, there's only one more joint for this loop to process, but just for generality...
00058         lastFront=front;
00059         front=getCameraPoint(LegOffset+leg*JointsPerLeg+j,p,curx,cury);
00060         if(lastFront) {
00061           g.setColor(front?color:-color);
00062           g.drawLine(lastx,lasty,curx,cury);
00063         }
00064         if(front) {
00065           g.setColor(-color);
00066           g.drawRect(curx-originBoxSize/2,cury-originBoxSize/2,originBoxSize,originBoxSize);
00067         }
00068         lastx=curx;
00069         lasty=cury;
00070       }
00071       lastFront=front;
00072       front=getCameraPoint(PawFrameOffset+leg,p,curx,cury);
00073       if(lastFront) {
00074         g.setColor(front?color:-color);
00075         g.drawLine(lastx,lasty,curx,cury);
00076       }
00077       if(front) {
00078         g.setColor(-color);
00079         g.drawRect(curx-originBoxSize/2,cury-originBoxSize/2,originBoxSize,originBoxSize);
00080       }
00081       lastx=curx;
00082       lasty=cury;
00083     }
00084   }
00085 
00086   static std::string getClassDescription() { return "Draws the kinematics \"skeleton\" on the camera frame"; }
00087   virtual std::string getDescription() const { return getClassDescription(); }
00088   
00089 
00090 protected:
00091   //! returns the @a x and @a y coordinates with the camera image corresponding to a point (@a p) in 3d space (relative to @a jointOffset)
00092   bool getCameraPoint(unsigned int jointOffset, NEWMAT::ColumnVector& p, float& x, float& y) {
00093     NEWMAT::Matrix T = kine->linkToJoint(jointOffset,CameraFrameOffset);
00094     NEWMAT::ColumnVector o=T*p; //o is now the position of point p on the link, relative to the camera
00095     bool front=o(3)>=0;
00096     config->vision.computePixel(o(1),o(2),o(3),x,y);
00097     //cout << jointOffset << ' ' << o.t() << " -> " << x << ' ' << y << endl;
00098     return front;
00099   }
00100 
00101   
00102 private:
00103   // Providing declarations for these functions will avoid a compiler warning if
00104   // you have any class members which are pointers.  However, as it is, an error
00105   // will result if you inadvertantly cause a call to either (which is probably
00106   // a good thing, unless you really intended to copy/assign a behavior, in
00107   // which case simply provide implementations for the functions)
00108   DrawSkeletonBehavior(const DrawSkeletonBehavior&); //!< don't call (copy constructor)
00109   DrawSkeletonBehavior& operator=(const DrawSkeletonBehavior&); //!< don't call (assignment operator)
00110 };
00111 
00112 /*! @file
00113  * @brief Defines DrawSkeletonBehavior, which draws the kinematics "skeleton" on the camera frame
00114  * @author ejt (Creator)
00115  *
00116  * $Author: ejt $
00117  * $Name: tekkotsu-2_4_1 $
00118  * $Revision: 1.3 $
00119  * $State: Exp $
00120  * $Date: 2005/08/07 04:11:03 $
00121  */
00122 
00123 #endif

Tekkotsu v2.4.1
Generated Tue Aug 16 16:32:46 2005 by Doxygen 1.4.4