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