target recognition and towards feeder navigation

c/digits/mainDigits.cpp

@@ -268,6 +268,28 @@ int clasifyDigit( CvSeq *contour, IplImage *debugImg=NULL )
   return -1;
 }
 
+bool isItTarget( CvSeq *contour, IplImage *debugImg=NULL )
+{
+  if ( contour->v_next )
+  {
+    double area = fabs( cvContourArea( contour ));
+    double subarea = 0.0;
+    // it has hole(s)
+    CvSeq* p = contour->v_next;
+    int i;
+    for( i = 0; p->h_next; i++ )
+    {
+      subarea += fabs( cvContourArea( p ));
+      p = p->h_next;
+    }
+    if( i >= 10 && i <= 12 )
+    {
+      return area < 2.5*subarea;
+    }
+  }
+  return false;
+}
+
 int findDigit( const char *filename, char *outFilename = 0 )
 {
   IplImage* img = cvLoadImage(filename);
@@ -319,6 +341,16 @@ int findDigit( const char *filename, char *outFilename = 0 )
 
     while( ptr )
     {
+      if( isItTarget( ptr ) )
+      {
+        CvRect rect = cvBoundingRect( ptr ); // Centroid would be better
+        fprintf( stdout, "('X', (%d,%d,%d,%d)),", rect.x+roi.x, rect.y+roi.y, rect.width, rect.height );
+        if( outFilename )
+        {
+          cvDrawContours( img, ptr, CV_RGB(0,0,255), CV_RGB(0,255,0), -1, 1, CV_AA, cvPoint(roi.x,roi.y) );
+        }
+      }
+
       int digit = clasifyDigit( ptr );
       if( digit != -1 )
       {

sickday2014.py

@@ -405,15 +405,28 @@ def wait( self, duration ):
   def approachFeeder( self, timeout=60 ):
     "robot should be within 1m of the feeder"
     print "Approaching Feeder"
-    desiredDist = 0.4 #0.2
+    desiredDist = 0.30 #0.4 #0.2
     countOK = 0
     startTime = self.robot.time
+    angularSpeed = 0
     while startTime + timeout > self.robot.time:
+      if self.robot.cameraData is not None and len(self.robot.cameraData)> 0 and self.robot.cameraData[0] is not None:
+        arr = eval(self.robot.cameraData[0])
+        angularSpeed = None
+        for a in arr:
+          for digit, (x,y,dx,dy) in a:
+            if digit == 'X':
+              angularSpeed = (320-(x+dx/2))/100.0
+              break
+        if angularSpeed is None:
+          angularSpeed = 0
+
       if self.robot.laserData == None or len(self.robot.laserData) != 541:
         self.robot.setSpeedPxPa( 0, 0 )
       else:
         minDist = min(self.robot.laserData[180:-180])/1000.
-        self.robot.setSpeedPxPa( min(self.driver.maxSpeed, minDist - desiredDist), 0 )
+        self.robot.setSpeedPxPa( min(self.driver.maxSpeed, minDist - desiredDist), angularSpeed )
+#        self.robot.setSpeedPxPa( 0, angularSpeed )
         if abs(minDist - desiredDist) < 0.01:
           countOK += 1
         else:
@@ -426,7 +439,7 @@ def approachFeeder( self, timeout=60 ):
     print "done."
     self.driver.turn( math.radians(180) )
     # turn 180 degrees ( or other angles if we allow non-dirrect approaches
-
+    self.wait(3)
 
 
   def goVfh( self, timeout ):