00001
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
00019 class DrawSkeletonBehavior : public BehaviorBase {
00020 public:
00021
00022 DrawSkeletonBehavior() : BehaviorBase("DrawSkeletonBehavior") {}
00023
00024 virtual void DoStart() {
00025 BehaviorBase::DoStart();
00026 erouter->addListener(this, EventBase::visRawCameraEGID,ProjectInterface::visRawCameraSID,EventBase::statusETID);
00027 }
00028
00029 virtual void DoStop() {
00030 erouter->removeListener(this);
00031 BehaviorBase::DoStop();
00032 }
00033
00034 virtual void processEvent(const EventBase& e) {
00035
00036
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
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
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;
00095 bool front=o(3)>=0;
00096 config->vision.computePixel(o(1),o(2),o(3),x,y);
00097
00098 return front;
00099 }
00100
00101
00102 private:
00103
00104
00105
00106
00107
00108 DrawSkeletonBehavior(const DrawSkeletonBehavior&);
00109 DrawSkeletonBehavior& operator=(const DrawSkeletonBehavior&);
00110 };
00111
00112
00113
00114
00115
00116
00117
00118
00119
00120
00121
00122
00123 #endif