Beispiel #1
0
int main (int argc, char * const argv[]) {
    char code;
    bool draw = true;
    Mat image;
    image = Mat::zeros(361, 301, CV_8UC3);
    image = cv::Scalar::all(0);
	setup();

    std::vector<Point > edges;
    std::vector<Point > doors;
    std::vector<Point > docks;
    Point arcReactorLoc;
    parseFile(edges,doors,docks,arcReactorLoc);
    drawMap(image, edges, doors, docks, arcReactorLoc);
	cv::Point pos(15, 15);
    
    MapNode* nodes[6][5];
    
    //generate nodes
    for(int y = 0; y < 6; y++)
    {
      for(int x = 0; x < 5; x++)
      {
		cv::Point* nodeLoc = new cv::Point((x * 60) + 30, (y * 60) + 30);
		MapNode* mapNode = new MapNode(*nodeLoc, x, y);
		nodes[y][x] = mapNode;
	  }
    }
    
    //figure out where each node can go
    for(int y = 0; y < 6; y++)
    {
      for(int x = 0; x < 5; x++)
      {
	//MapNode mapNode(nodeLoc);
	MapNode* mapNode = nodes[y][x];
	cv::Point nodeLoc = mapNode->getPosition();
	if(y - 1 >= 0)
	  mapNode->setDown((nodes[y - 1][x]));
	if(y + 1 <= 5)
	  mapNode->setUp((MapNode*)(nodes[y + 1][x]));
	if(x - 1 >= 0)
	  mapNode->setLeft((nodes[y][x - 1]));
	if(x + 1 <= 4)
	  mapNode->setRight((nodes[y][x + 1]));
	for(int move = 0; move < 31; move++)
	{
	  //check up
	  if((mapNode->getUp() != NULL) && (image.at<Vec3b>(nodeLoc.y + move, nodeLoc.x).val[0] == 255))
	    mapNode->setUp(NULL);
	  //check down
	  if(mapNode->getDown() != NULL && (image.at<Vec3b>(nodeLoc.y - move, nodeLoc.x).val[0] == 255))
	    mapNode->setDown(NULL);
	  //check left
	  if(mapNode->getLeft() != NULL && image.at<Vec3b>(nodeLoc.y, nodeLoc.x - move).val[0] == 255)
	    mapNode->setLeft(NULL);
	  //check right
	  if(mapNode->getRight() != NULL && image.at<Vec3b>(nodeLoc.y, nodeLoc.x + move).val[0] == 255)
	    mapNode->setRight(NULL);
	}
      }
    }
    
    for(int y = 0; y < 6; y++)
    {
      for(int x = 0; x < 5; x++)
      {
	MapNode* mapNode = nodes[y][x];
	cv::Point nodeLoc = mapNode->getPosition();
	if(mapNode->getDown() != NULL)
	  line(image, mapNode->getPosition(), mapNode->getDown()->getPosition(), Scalar(0, 255, 0), 4, 6);
	if(mapNode->getUp() != NULL)
	  line(image, mapNode->getPosition(), (mapNode->getUp())->getPosition(), Scalar(0, 255, 0), 4, 6);
	if(mapNode->getLeft() != NULL)
	  line(image, mapNode->getPosition(), mapNode->getLeft()->getPosition(), Scalar(0, 255, 0), 4, 6);
	if(mapNode->getRight() != NULL)
	  line(image, mapNode->getPosition(), mapNode->getRight()->getPosition(), Scalar(0, 255, 0), 4, 6);
      }
    }
	cv :: Mat logo = cv :: imread ("ironman_icon.jpg");
	cv :: Mat imageROI;
    imageROI = image (cv :: Rect (pos.x, pos.y, logo.cols, logo.rows));
    logo.copyTo (imageROI);
    cv :: namedWindow("result");
    cv :: imshow ("result", image);
	cv::waitKey(0);
    std::vector<MapNode*> possibleNodes(30);
	//std::vector<MapNode*> possibleReverseNodes(0);
    int i = 0;
    for(int y = 0; y < 6; y++)
    {
      for(int x = 0; x < 5; x++)
      {
		possibleNodes[i++] = nodes[y][x];
      }
    }
    printf("init size of possible locactions %i\n", possibleNodes.size());
    int lastMove = 0;
    //list of all nodes made, check sonar and remove impossible nodes
	//bool reverse = false;
    while(possibleNodes.size() > 1)
    {
	  move(UP, true);
      int hasUp = checkSonar(1) / 60;
	  move(RIGHT, true);
      int hasRight = checkSonar(1) / 60;
	  move(DOWN, true);
      int hasDown = checkSonar(1) / 60;
	  move(LEFT, true);
      int hasLeft = checkSonar(1) / 60;
	  //int before = possibleReverseNodes.size();
	  printf("size %i u%i d%i l%i r%i\n", possibleNodes.size(), hasDown, hasUp, hasLeft, hasRight);
	  for(int j = 0; j < possibleNodes.size(); j++)
	  {
	  //remove nodes that dont match this
		if(possibleNodes[j]->getDownFull() != hasDown)
		{
			possibleNodes.erase(possibleNodes.begin() + j);
			j--;
			continue;
		}
		if(possibleNodes[j]->getUpFull() != hasUp)
		{
			possibleNodes.erase(possibleNodes.begin() + j);
			j--;
			continue;
		}
		if(hasRight && possibleNodes[j]->getRightFull() < hasRight)
		{
			/**if(reverse)
				continue;
			possibleReverseNodes.push_back(possibleNodes[j]);**/
			possibleNodes.erase(possibleNodes.begin() + j);
			j--;
			continue;
		}
		if(hasLeft && possibleNodes[j]->getLeftFull() < hasLeft)
		{
			/**if(reverse)
				continue;
			possibleReverseNodes.push_back(possibleNodes[j]);**/
			possibleNodes.erase(possibleNodes.begin() + j);
			j--;
			continue;
		}
      }
	  cout << possibleNodes.size() << endl;
	  if(possibleNodes.size() == 0)
	  {
		  printf("error\n");
		  printf("error\n");
		  printf("error\n");
		  printf("error\n");
		  return 0;
	  }
	  /**else if(possibleNodes.size() == 0)
	  {
		  /**for(int toAdd = 0; possibleReverseNodes.size(); toAdd++)
		  {
			  possibleNodes.push_back(possibleReverseNodes.pop_back());
		  }
		  possibleNodes.reserve(possibleNodes.size() + possibleReverseNodes.size());
		  copy(possibleReverseNodes.begin(), possibleReverseNodes.end(), inserter(possibleNodes, possibleNodes.end()));
		  possibleReverseNodes.clear();
		  reverse = true;
	  }
	  else
	  {
		  reverse = false;
	  }**/
      //do we need to move again
      if(possibleNodes.size() > 1)
      {
		//printf("ran\n");
		//favor up 1 and right 2
		repeat_this_code:
		if(hasUp && (lastMove & 4) != 4)
		{
			printf("went up\n");
			move(UP);
			lastMove |= 1;
			for(int z=0; z < possibleNodes.size(); z++)
			{
				possibleNodes[z] = possibleNodes[z]->getUp();
			}
		}
		else if(hasRight && (lastMove & 8) != 8)
		{
			printf("went right\n");
			lastMove |= 2;
			move(RIGHT);
			for(int z=0; z < possibleNodes.size(); z++)
			{
				possibleNodes[z] = possibleNodes[z]->getRight();
			}
		}
		else if(hasDown && (lastMove & 1) != 1)
		{
			lastMove |= 4;
			printf("went down\n");
			move(DOWN);
			for(int z=0; z < possibleNodes.size(); z++)
			{
				possibleNodes[z] = possibleNodes[z]->getDown();
			}
		}
		else if(hasLeft && (lastMove & 2) != 2)
		{
			lastMove |= 8;
			printf("went left\n");
			move(LEFT);
			for(int z=0; z < possibleNodes.size(); z++)
			{
				possibleNodes[z] = possibleNodes[z]->getLeft();
			}
		}
		else
		{
			
			if(hasLeft && (lastMove & 8) != 8)
			{
				lastMove &= ~2;
				lastMove |= 8;
				move(LEFT);
			}
			else if(hasDown && (lastMove & 4) != 4)
			{
				lastMove &= ~1;
				lastMove |= 4;
				move(DOWN);
			}
			else
			{
				lastMove = 0;
				goto repeat_this_code;
			}
		}
      }
    }
	cout << "found?\n";
	MapNode* node = possibleNodes[0];
    printf("found robot at %i %i\n", node->getPosX(), node->getPosY());
    //should know where we are by this point
	
	redrawMacro();
	
	//**********************
	//OPEN UP VIDEO STUFF
	//**********************
	
	VideoCapture cap(0); //capture the video from webcam

    if ( !cap.isOpened() )  // if not success, exit program
    {
         cout << "Cannot open the web cam" << endl;
         return -1;
    }

	cap.set(CV_CAP_PROP_FRAME_WIDTH, 160);
	cap.set(CV_CAP_PROP_FRAME_HEIGHT, 120);

   // namedWindow("Control", CV_WINDOW_AUTOSIZE); //create a window called "Control"

	//Capture a temporary image from the camera
	Mat imgTmp;
    imgTmp = Mat::zeros(301, 301, CV_8UC3);

	cap.read(imgTmp); 

	//Create a black image with the size as the camera output
	Mat imgLines = Mat::zeros( imgTmp.size(), CV_8UC3 );;
	
	//********************
	//end camera open
	//********************
	
	//start doing tasks
	int positionCheck = 0;
	bool visitedA = false;
	bool visitedB = false;
	bool visited2 = false;
	while(!visitedA || !visitedB)
	{
		MapNode* toVisit = NULL;
		switch(positionCheck)
		{
			case 0:
				toVisit = nodes[0][2];
				break;
			case 1:
				toVisit = nodes[0][3];
				break;
			case 2:
				toVisit = nodes[3][0];
				break;
			case 3:
				cout << "Visited all locations, missed a lab somewhere probably, error and ending\n";
				return 0;
		}
		if(positionCheck == 1)
		{
			string tempP = pathFind(node->getX(), node->getY(), toVisit->getX(), toVisit->getY(), nodes);
				toVisit = nodes[3][0];
				if(tempP.length() > pathFind(node->getX(), node->getY(), toVisit->getX(), toVisit->getY(), nodes).length())
					visited2 = true;
				else
					toVisit = nodes[0][3];
		}
		else if(positionCheck == 2)
			if(!visited2)
					toVisit = nodes[3][0];
				else
					toVisit = nodes[0][3];
		//move to node we are visiting
		node = node->traverse(toVisit, pathFind(node->getX(), node->getY(), toVisit->getX(), toVisit->getY(), nodes));
		redrawMacro();
		positionCheck++; //setup for next run if necessary
		
		//check sonar
		int toWallDist = checkSonar(1);
		if(toWallDist < 20)
		{
			while(checkSonar(1) < 20)
			{
				setSpeed(fp, ZERO - (180 - ZERO), 180);
				delay(20);
				setSpeed(fp, 0, 0);
			}
		}
		
		//camera check
		bool found = false;
		bool foundA = false;
		bool lookLeft = false;
		bool lookRight = false;
		bool lookStr = false;
		for(int tries = 0; tries < CAMERA_TRIES_MAX; tries++)
		{
			Mat imgOriginal;
			bool bSuccess = cap.read(imgOriginal); // read a new frame from video
			if (!bSuccess) //if not success, break loop
			{
				cout << "Cannot read a frame from video stream" << endl;
				return 0;
			}

			Mat imgHSV;
			cvtColor(imgOriginal, imgHSV, COLOR_BGR2HSV); //Convert the captured frame from BGR to HSV
			
			if(!visitedA)
			{
				Mat imgThresholded;
				//booklet
				int iLowH = 80;
				int iHighH = 102;

				int iLowS = 32; 
				int iHighS = 255;

				int iLowV = 91;
				int iHighV = 255;

				inRange(imgHSV, Scalar(iLowH, iLowS, iLowV), Scalar(iHighH, iHighS, iHighV), imgThresholded); //Threshold the image
      
				//morphological opening (removes small objects from the foreground)
				erode(imgThresholded, imgThresholded, getStructuringElement(MORPH_ELLIPSE, Size(5, 5)) );
				dilate( imgThresholded, imgThresholded, getStructuringElement(MORPH_ELLIPSE, Size(5, 5)) ); 

				//morphological closing (removes small holes from the foreground)
				dilate( imgThresholded, imgThresholded, getStructuringElement(MORPH_ELLIPSE, Size(5, 5)) ); 
				erode(imgThresholded, imgThresholded, getStructuringElement(MORPH_ELLIPSE, Size(5, 5)) );
	
				//Calculate the moments of the thresholded image
				Moments oMoments = moments(imgThresholded);

				double dM01 = oMoments.m01;
				double dM10 = oMoments.m10;
				double dArea = oMoments.m00;
			
				found = dArea > IMAGE_THRES_REQ;
				
				if(found)
				{
					foundA = true;
					break;
				}
			}
			if(!visitedB && !found)
			{
				Mat imgThresholded;
				
				//green lego
				int iLowH = 63;
				int iHighH = 77;

				int iLowS = 73; 
				int iHighS = 255;

				int iLowV = 60;
				int iHighV = 255;

				inRange(imgHSV, Scalar(iLowH, iLowS, iLowV), Scalar(iHighH, iHighS, iHighV), imgThresholded); //Threshold the image
      
				//morphological opening (removes small objects from the foreground)
				erode(imgThresholded, imgThresholded, getStructuringElement(MORPH_ELLIPSE, Size(5, 5)) );
				dilate( imgThresholded, imgThresholded, getStructuringElement(MORPH_ELLIPSE, Size(5, 5)) ); 

				//morphological closing (removes small holes from the foreground)
				dilate( imgThresholded, imgThresholded, getStructuringElement(MORPH_ELLIPSE, Size(5, 5)) ); 
				erode(imgThresholded, imgThresholded, getStructuringElement(MORPH_ELLIPSE, Size(5, 5)) );
	
				//Calculate the moments of the thresholded image
				Moments oMoments = moments(imgThresholded);

				double dM01 = oMoments.m01;
				double dM10 = oMoments.m10;
				double dArea = oMoments.m00;
			
				found = dArea > IMAGE_THRES_REQ;
				
				if(found)
					break;
			}
			
			if(!lookLeft)
			{
				lookLeft = true;
				setSpeed(fp, 180, 180);
				delay(200);
				setSpeed(fp, 0, 0);
			}
			else if(!lookRight)
			{
				lookRight = true;
				setSpeed(fp, ZERO - (180 - ZERO), ZERO - (180 - ZERO));
				delay(400);
				setSpeed(fp, 0, 0);
			}
			else if(!lookStr)
			{
				lookStr = true;
				setSpeed(fp, 180, 180);
				delay(200);
				setSpeed(fp, 0, 0);
			}

		}	
		if(found && foundA)
		{
			cout << "Found virus A, going to lab A\n";
			node = node->traverse(nodes[4][0], pathFind(node->getX(), node->getY(), 0, 4, nodes));
			redrawMacro();
			visitedA = true;
		}
		else if(found)
		{
			cout << "Found virus B, going to lab B\n";
			node = node->traverse(nodes[4][4], pathFind(node->getX(), node->getY(), 4, 4, nodes));
			redrawMacro();
			visitedB = true;
		}
		else
		{
			cout << "No virus found, proceeding to next location!\n";
		}
		//if we looked left without looking straight, we need to fix position
		if(lookLeft && !lookStr)
		{
			lookRight = true;
			setSpeed(fp, ZERO - (180 - ZERO), ZERO - (180 - ZERO));
			delay(100);
			setSpeed(fp, 0, 0);
		}
	}
	printf("robot should be done\n");
    return 0;
}
Beispiel #2
0
void redraw(cv::Mat logo, Point pos, cv::Mat image, std::vector<Point> &edges, std::vector<Point> &virus,
                  std::vector<Point> &labA,
                  Point &labB, MapNode* nodes[][5])
{
  static cv::Mat clear = Mat::zeros(361, 301, CV_8UC3);
  clear.copyTo(image);
  int thickness = 6;
  int lineType = 8;
  
  //Draw Wall
  for(int i = 0;i < edges.size()-1; i+=2){
    Point p1 = edges[i];
    Point p2 = edges[i+1];
    line (image, p1, p2, Scalar(255, 0, 0), thickness, lineType);
  }
  //Draw Virus Location
    for(int i = 0;i < virus.size()-1; i+=2) {
        Point p1 = virus[i];
        Point p2 = virus[i+1];
        
        Point topLeft1, topRight1, bottomLeft1, bottomRight1;
        topLeft1.x = int (p1.x - p2.x/2);
        topLeft1.y = int (p1.y - p2.y/2);
        topRight1.x = int (p1.x + p2.x/2);
        topRight1.y = int (p1.y - p2.y/2);
        bottomLeft1.x = int (p1.x - p2.x/2);
        bottomLeft1.y = int (p1.y + p2.y/2);
        bottomRight1.x = int (p1.x + p2.x/2);
        bottomRight1.y = int (p1.y + p2.y/2);
        
        line(image,topLeft1, bottomRight1, CV_RGB(0, 150, 150));
        line(image,topRight1, bottomLeft1, CV_RGB(0, 150, 150));
    }
    
    //Draw LabA Location
    for(int i = 0;i < labA.size()-1; i+=2){
        Point p1 = labA[i];
        Point p2 = labA[i+1];
        
        Point topLeft, topRight, bottomLeft, bottomRight;
        topLeft.x = int (p1.x - p2.x/2);
        topLeft.y = int (p1.y - p2.y/2);
        topRight.x = int (p1.x + p2.x/2);
        topRight.y = int (p1.y - p2.y/2);
        bottomLeft.x = int (p1.x - p2.x/2);
        bottomLeft.y = int (p1.y + p2.y/2);
        bottomRight.x = int (p1.x + p2.x/2);
        bottomRight.y = int (p1.y + p2.y/2);
        rectangle(image, topLeft, bottomRight, cvScalar(204, 0, 204, 0),1,8,0);
        
        line(image,topLeft, bottomRight, CV_RGB(204,0,204));
        line(image,topRight, bottomLeft, CV_RGB(204,0,204));
    }
    //Draw LabB Location
    circle(image, labB, 10, CV_RGB(255,0,0), 5, CV_AA, 0);
    for(int y = 0; y < 6; y++)
    {
      for(int x = 0; x < 5; x++)
      {
	MapNode* mapNode = nodes[y][x];
	cv::Point nodeLoc = mapNode->getPosition();
	if(mapNode->getDown() != NULL)
	  line(image, mapNode->getPosition(), mapNode->getDown()->getPosition(), Scalar(0, 255, 0), 4, 6);
	if(mapNode->getUp() != NULL)
	  line(image, mapNode->getPosition(), (mapNode->getUp())->getPosition(), Scalar(0, 255, 0), 4, 6);
	if(mapNode->getLeft() != NULL)
	  line(image, mapNode->getPosition(), mapNode->getLeft()->getPosition(), Scalar(0, 255, 0), 4, 6);
	if(mapNode->getRight() != NULL)
	  line(image, mapNode->getPosition(), mapNode->getRight()->getPosition(), Scalar(0, 255, 0), 4, 6);
      }
    }
  cv :: Mat imageROI;
  imageROI = image (cv :: Rect (pos.x, pos.y, logo.cols, logo.rows));
  logo.copyTo (imageROI);
 // for(int i = 0; i < p.size(); i++)
 // {
 //   drawCross(p[i].getPosition(), Scalar(0, 255, 0), 5);
 // }
  cv :: imshow ("result", image);
}