00001
00002 #ifndef _VRmixin_h_
00003 #define _VRmixin_h_
00004
00005 #include <string>
00006 #include <iostream>
00007
00008 #include "Behaviors/BehaviorBase.h"
00009 #include "Shared/debuget.h"
00010 #include "Vision/RawCameraGenerator.h"
00011 #include "Vision/SegmentedColorGenerator.h"
00012 #include "Vision/cmv_types.h"
00013 #include "Wireless/Wireless.h"
00014 #include "Shared/Config.h"
00015 #include "Shared/get_time.h"
00016
00017 #include "ShapeAgent.h"
00018 #include "BlobData.h"
00019 #include "SketchRoot.h"
00020
00021 namespace DualCoding {
00022
00023 class Lookout;
00024 class Pilot;
00025 class SketchDataRoot;
00026 class SketchSpace;
00027 class MapBuilder;
00028 class ParticleFilter;
00029
00030 typedef unsigned char cmap_t;
00031
00032
00033 class VRmixin {
00034 protected:
00035 static unsigned int instanceCount;
00036 static unsigned int crewCount;
00037
00038 public:
00039
00040 static SketchSpace& getCamSkS();
00041 static SketchSpace& getLocalSkS();
00042 static SketchSpace& getWorldSkS();
00043 static ShapeSpace& getGroundShS();
00044
00045
00046 static MapBuilder& getMapBuilder();
00047
00048 static SketchSpace& camSkS;
00049 static ShapeSpace& camShS;
00050
00051 static ShapeSpace& groundShS;
00052
00053 static SketchSpace& localSkS;
00054 static ShapeSpace& localShS;
00055
00056 static SketchSpace& worldSkS;
00057 static ShapeSpace& worldShS;
00058 static Shape<AgentData> theAgent;
00059
00060 static MapBuilder& mapBuilder;
00061 static Pilot pilot;
00062 static Lookout lookout;
00063 static ParticleFilter filter;
00064
00065 private:
00066 static Socket *camDialogSock;
00067 static Socket *camRleSock;
00068 static Socket *localDialogSock;
00069 static Socket *localRleSock;
00070 static Socket *worldDialogSock;
00071 static Socket *worldRleSock;
00072
00073 public:
00074
00075 VRmixin();
00076
00077
00078 virtual ~VRmixin(void);
00079
00080 static void startCrew();
00081 static void stopCrew();
00082
00083
00084 static bool rleEncodeSketch(const SketchDataRoot& image);
00085
00086
00087 static Sketch<uchar> sketchFromSeg();
00088
00089
00090 static Sketch<uchar> sketchFromChannel(RawCameraGenerator::channel_id_t chan);
00091
00092
00093 static Sketch<uchar> sketchFromRawY();
00094
00095
00096 static std::vector<Shape<BlobData> >
00097 getBlobsFromRegionGenerator(int color, int minarea=0,
00098 BlobData::BlobOrientation_t orient=BlobData::groundplane,
00099 int maxblobs=50);
00100
00101
00102 static void processSketchRequest(const std::string &line,
00103 SketchSpace &sketches,
00104 ShapeSpace &shapes);
00105
00106
00107 static void projectToGround(const NEWMAT::Matrix& camToBase = kine->jointToBase(CameraFrameOffset),
00108 const NEWMAT::ColumnVector& ground_plane = kine->calculateGroundPlane());
00109
00110 private:
00111
00112 static VRmixin* theOne;
00113
00114
00115 VRmixin (const VRmixin&);
00116 VRmixin& operator=(const VRmixin&);
00117
00118
00119 static int camDialogSockCallback(char *buf, int bytes);
00120
00121
00122 static int localDialogSockCallback(char *buf, int bytes);
00123
00124
00125 static int worldDialogSockCallback(char *buf, int bytes);
00126
00127 static void dialogCallback(char* buf, int bytes, std::string& incomplete,
00128 SketchSpace &SkS, ShapeSpace &ShS);
00129 };
00130
00131 }
00132
00133 #endif