[Personalrobots-commit] SF.net SVN: personalrobots: [479] pkg/trunk/drivers/cam/cv_cam/cv_cam.cpp

[<mor...@us...>]

Revision: 479
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=479&view=rev
Author:   morgan_quigley
Date:     2008-05-19 16:40:38 -0700 (Mon, 19 May 2008)

Log Message:
-----------
cv_cam is now working decently well, with some parameters available to control it.

Modified Paths:
--------------
    pkg/trunk/drivers/cam/cv_cam/cv_cam.cpp

Modified: pkg/trunk/drivers/cam/cv_cam/cv_cam.cpp
===================================================================
--- pkg/trunk/drivers/cam/cv_cam/cv_cam.cpp	2008-05-19 23:17:24 UTC (rev 478)
+++ pkg/trunk/drivers/cam/cv_cam/cv_cam.cpp	2008-05-19 23:40:38 UTC (rev 479)
@@ -41,35 +41,51 @@
   CvBridge<MsgImage> cv_bridge;
   CvCapture *capture;
   IplImage *cam_image;
-  double last_image_time;
-  int show_image;
+  double last_print_time;
+  int show_image, fps_count, delay_us;
 
-  CVCam() : node("cv_cam"), cv_bridge(&image_msg), last_image_time(0), 
-            show_image(0)
+  CVCam() : node("cv_cam"), cv_bridge(&image_msg), 
+            last_print_time(0), show_image(0), fps_count(0)
   {
     advertise<MsgImage>("image");
     capture = cvCaptureFromCAM(CV_CAP_ANY);
     if (!capture)
       log(FATAL, "woah! couldn't open a camera. is one connected?\n");
+    param("display", show_image, 0);
+    param("delay_us", delay_us, 0);
+    if (show_image)
+      cvNamedWindow("cv_cam", CV_WINDOW_AUTOSIZE);
   }
   virtual ~CVCam() 
   {
     if (capture)
       cvReleaseCapture(&capture);
+    if (show_image)
+      cvDestroyWindow("cv_cam");
   }
   bool take_and_send_picture()
   {
     if (!capture)
       return false;
     cam_image = cvQueryFrame(capture);
+    if (!cam_image)
+      return false;
     double t = clock.time();
-    double dt = t - last_image_time;
-    printf("dt = %f\t(%f fps)\n", dt, 1.0 / dt);
-    last_image_time = t;
-//    if (show_image)
-      cvShowImage("cvcam", cam_image);
+    if ((last_print_time + 1 < t) && fps_count)
+    {
+      printf("%f fps\n", fps_count / (t - last_print_time));
+      last_print_time = t;
+      fps_count = 0;
+    }
+    fps_count++;
     cv_bridge.from_cv(cam_image);
     publish("image", image_msg);
+    if (show_image)
+    {
+      cvShowImage("cv_cam", cam_image);
+      cvWaitKey(5);
+    }
+    usleep(delay_us);
     return true;
   }
 };
@@ -78,17 +94,11 @@
 {
   ros::init(argc, argv);
   CVCam cvcam;
-//  if (cvcam.show_image)
-    cvNamedWindow("cvcam", CV_WINDOW_AUTOSIZE);
   while (cvcam.ok())
   {
-//    if (cvcam.show_image)
-      cvWaitKey(5);
     if (!cvcam.take_and_send_picture())
       usleep(100000);
   }
-//  if (a.show_image)
-    cvDestroyWindow("cvcam");
   ros::fini();
   return 0;
 }


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.