00001
00002 #ifndef INCLUDED_Lookout_h_
00003 #define INCLUDED_Lookout_h_
00004
00005 #include <queue>
00006
00007 #include "Behaviors/BehaviorBase.h"
00008 #include "Motion/MMAccessor.h"
00009 #include "Motion/MotionManager.h"
00010 #include "Motion/HeadPointerMC.h"
00011 #include "Motion/MotionSequenceMC.h"
00012 #include "Motion/PostureMC.h"
00013 #include "Events/VisionObjectEvent.h"
00014 #include "LookoutRequests.h"
00015
00016 namespace DualCoding {
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029 class Lookout : public BehaviorBase {
00030 public:
00031
00032 Lookout();
00033
00034 virtual void DoStart();
00035 virtual void DoStop();
00036 virtual void processEvent(const EventBase& event);
00037
00038 static std::string getClassDescription() { return "Moves head and collects data according to request"; }
00039 virtual std::string getDescription() const { return getClassDescription(); }
00040
00041 virtual unsigned int executeRequest(const LookoutRequest&);
00042 bool busy() { return curReq != NULL; }
00043 void stopTrack();
00044
00045 static std::vector<DualCoding::Point> groundSearchPoints();
00046
00047 void moveHeadToPoint();
00048
00049
00050 void relax();
00051
00052 static Point findLocationFor(const float normX, const float normY);
00053 static const unsigned int invalid_LO_ID=(unsigned int)-1;
00054
00055 protected:
00056 virtual void executeRequest();
00057 virtual void requestComplete(bool result=true);
00058
00059 template<class T>
00060 void pushRequest(const LookoutRequest& req) {
00061 requests.push(new T(dynamic_cast<const T&>(req)));
00062 }
00063
00064
00065
00066
00067 void processPointAtEvent(const EventBase& event);
00068 bool findPixelModes();
00069 #ifdef TGT_HAS_IR_DISTANCE
00070 bool findDistanceMode();
00071 float getDistanceModeValue();
00072 #endif
00073
00074
00075
00076
00077 void setupScan();
00078 void triggerScanMotionSequence();
00079 void processScanEvent(const EventBase& event);
00080 void scanAlongLine(const Point& start,const Point& end);
00081 void scanAlongPolygon(const std::vector<Point>& vertices, const bool closed=false);
00082 void scanArea(const Point &topLeft, const Point &topRight,
00083 const Point &bottomLeft, const Point &bottomRight);
00084
00085 static Point findLocationFor(const VisionObjectEvent& visev) {
00086 return findLocationFor(visev.getCenterX(), visev.getCenterY());
00087 }
00088
00089 static Point findLocationFor(const CMVision::region* reg) {
00090 return findLocationFor(2.0*reg->cen_x/VRmixin::camSkS.getWidth()-1.0,
00091 2.0*reg->cen_y/VRmixin::camSkS.getHeight()-1.0);
00092 }
00093
00094 void storeVisionRegionDataTo(std::vector<Point>&, const std::set<color_index>&, int);
00095 #ifdef TGT_HAS_IR_DISTANCE
00096 void storeIRDataTo(std::vector<Point>&);
00097 #endif
00098
00099
00100
00101
00102 void setupTrack();
00103 void processTrackEvent(const EventBase& event);
00104
00105
00106
00107
00108
00109
00110
00111
00112
00113 void setupSearch();
00114 void processSearchEvent(const EventBase& event);
00115 void triggerSearchMotionSequence();
00116 static void searchAt(HeadPointerMC &hpmc_temp,
00117 std::vector<float> &jointvals,
00118 const Point &target);
00119
00120
00121
00122 private:
00123 Lookout& operator=(const Lookout&);
00124 Lookout(const Lookout&);
00125
00126 protected:
00127 std::vector<std::map<uchar,unsigned int> > pixelHistograms;
00128 std::priority_queue<float> distanceSamples;
00129
00130 MotionManager::MC_ID pointer_id;
00131 MotionManager::MC_ID posture_id;
00132 MotionManager::MC_ID sequence_id;
00133 std::queue<LookoutRequest*> requests;
00134 LookoutRequest *curReq;
00135 LookoutPointRequest *curPAR;
00136 bool successSave;
00137
00138 enum TrackerStates {
00139 inactive,
00140 moveToAcquire,
00141 tracking,
00142 searching,
00143 centering,
00144 lost
00145 } trackerState;
00146 unsigned int idCounter;
00147
00148 enum LookoutTimerSourceId_t {
00149 settle_timer = 1,
00150 sample_timer,
00151 lost_timer,
00152 scan_timer,
00153 };
00154 };
00155
00156 }
00157
00158 #endif