00001 #include "BlobOperator.H" 00002 #include <Logger.H> 00003 #include <string> 00004 00005 using namespace std; 00006 using namespace RobotObjects; 00007 using namespace RobotVision; 00008 00009 BlobOperator::BlobOperator(std::string pName) : VisionOperator(pName), mBlobber(pName) { 00010 m_state = STATE_RUN; 00011 mVision = NULL; 00012 m_stereo=false; 00013 g_debug("BlobFinder registering a supplier"); 00014 registerSupplier(&mBlobber); 00015 } 00016 00017 void BlobOperator::initOperator() { 00018 std::string colorfile; 00019 00020 if(g_globalConfiguration.haveModuleOption(m_name,"colorfile")){ 00021 colorfile=g_globalConfiguration.getModuleOption(m_name,"colorfile"); 00022 if(colorfile[0]!='/'){ 00023 colorfile=g_globalConfiguration.getConfigDirectory()+"/"+colorfile; 00024 } 00025 }else{ 00026 colorfile=g_globalConfiguration.getConfigDirectory()+"/colors.txt"; 00027 } 00028 00029 // 00030 // Setup CMVision 00031 // 00032 if(mVision) { 00033 delete mVision; 00034 mVision = NULL; 00035 } 00036 00037 mVision = new CMVision; 00038 if(!mVision) { 00039 g_debug("Failed to create a new CMVision object"); 00040 m_state = STATE_FAIL; 00041 return; 00042 } 00043 00044 // 00045 // Initialize CMVision and load the colors <-- should probably change 00046 // 00047 if(!mVision->initialize(320,240) || !mVision->loadOptions(colorfile.c_str())) { 00048 g_debug("Failed to init CMVision"); 00049 m_state = STATE_FAIL; 00050 return; 00051 } 00052 00053 00054 m_state = STATE_RUN; 00055 m_format=FORMAT_CHAR_111_YUV24; 00056 } 00057 00058 void BlobOperator::runOperator(RoleImage mImage) { 00059 mVision->processFrame((image_pixel *)mImage.second->imageData); 00060 int colorID = 0; 00061 mBlobber.data.foundBlobs.clear(); 00062 while(mVision->getColorName(colorID)) { 00063 g_logdebug << "Found " << mVision->numRegions(colorID) << "blobs of color " << mVision->getColorName(colorID) << endl; 00064 00065 if(mVision->numRegions(colorID)) { 00066 00067 CMVision::region *r = mVision->getRegions(colorID); 00068 for(int i = 0; i < mVision->numRegions(colorID); i++) { 00069 Blob b; 00070 b.colorName = mVision->getColorName(colorID); 00071 b.width = r->x2 - r->x1; 00072 b.height = r->y2 - r->y1; 00073 b.origin.x(r->x1); 00074 b.origin.y(r->y1); 00075 b.pixelsCovered = r->area; 00076 b.centroid.x(r->cen_x); 00077 b.centroid.y(r->cen_y); 00078 mBlobber.data.foundBlobs.push_back(b); 00079 r++; 00080 } 00081 } 00082 colorID++; 00083 } 00084 00085 mBlobber.publishData(); 00086 } 00087 00088 void BlobOperator::quitOperator() { 00089 if(mVision) { 00090 delete mVision; 00091 mVision = NULL; 00092 } 00093 00094 mBlobber.disconnect(); 00095 00096 } 00097 00098 extern "C" { 00099 VisionOperator *BlobOperator_factory(std::string name) { 00100 return new BlobOperator(name); 00101 } 00102 00103 class BlobOperatorProxy { 00104 public: 00105 BlobOperatorProxy(){ 00106 operatorFactory["BlobOperator"] = BlobOperator_factory; 00107 } 00108 }; 00109 00110 BlobOperatorProxy p_bloboperator; 00111 }