commit f3966b0da330af0011ac09817870ff8043032cdd
Author: Nick Stanley <nstanley@lithium.(none)>
Date:   Sun Feb 26 20:26:16 2012 -0500

    changed a few things, tested, and now it outputs a sensor_msgs::point so that the control group can use it. We still need to figure out a target HSV value so that we can use that to test.

diff --git a/vision/src/v2v3_converter.cpp b/vision/src/v2v3_converter.cpp
index c4aea5b..0edc1e8 100644
--- a/vision/src/v2v3_converter.cpp
+++ b/vision/src/v2v3_converter.cpp
@@ -1,4 +1,5 @@
 #include <ros/ros.h>
+#include <stdio.h>
 #include <vision/TargetDescriptor.h>
 #include <image_transport/image_transport.h>
 #include <geometry_msgs/Point.h>
@@ -7,7 +8,7 @@
 ros::Publisher pub;
 ros::Publisher img_pub;
 
-int hk = 25; 
+int hk = 15; 
 int sk = 255;
 int vk = 255;
 
@@ -30,7 +31,7 @@ bool isDown(sensor_msgs::Image * img, int p){
 bool hasAnAdjacent(sensor_msgs::Image * image, int j){
 	int nl = image->height; // number of lines
 	int nc = image->width; 
-    int nChannels = image->width / image->step;
+        int nChannels = image->step / image->width;
 	int pos = j / nChannels; 
 	if ((pos % nc) != 0){
 		if (isLeft(image, pos)) return true;
@@ -56,9 +57,9 @@ void detectBlobs(sensor_msgs::Image * image, int * comX, int * comY, int * area)
 	int nChannels = image->step / image->width; // effective width
 	
 	// process image
-	for (int i = 1; i < nl; i++){
+	for (int i = 0; i < nl; i++){
 		for (int j = 0; j < nc; j+= nChannels){
-			if (image->data[i * image->width + j] < 30 && image->data[j] > 20){
+			if (image->data[i * image->width + j] <= hk + 10 && image->data[j] >= hk - 10){
 				image->data[i * image->width + j] = 255;
 				image->data[i * image->width + j+1] = 255;
 				image->data[i * image->width + j+2] = 255;
@@ -79,13 +80,14 @@ void detectBlobs(sensor_msgs::Image * image, int * comX, int * comY, int * area)
 			image->data[i] = 0;
 			image->data[i + 1] = 0;
 			image->data[i + 2] = 0;
-      continue;
+       			continue;
 		}
 		blobPixelCount++;
 		comXSum += image->data[i] / 255;
 		comYSum += image->data[i] / 255;
 	}
   if (blobPixelCount != 0){
+	// get average
   	comXSum /= blobPixelCount;
   	comYSum /= blobPixelCount;
   }
@@ -98,7 +100,9 @@ void detectBlobs(sensor_msgs::Image * image, int * comX, int * comY, int * area)
 }
 
 void convertToHSV(sensor_msgs::Image * orig){
-  int nChannels = orig->width / orig->step;
+
+  int nChannels = orig->step / orig->width; 
+  printf("Converting to HSV \n");
 
   // get the max, set it as v
 
@@ -154,10 +158,12 @@ void convertToHSV(sensor_msgs::Image * orig){
       orig->data[i+2] = v;
 
   }
-
+  printf("Conversion complete\n");
 }
 
 void target_cb(const sensor_msgs::Image orig) {
+
+  printf("beginning callback \n");
   tf::TransformListener listener;
   geometry_msgs::Point point;
   tf::StampedTransform transform;
@@ -187,33 +193,53 @@ void target_cb(const sensor_msgs::Image orig) {
   
   detectBlobs(hsvPtr, &comX, &comY, &area);
 
+  int distance;
+
   // give distance as inversely proportional to area, for (x,y,z) 
   if (area > 10){
     
-    int distance = 100.0 / (float) area;
+    distance = 100.0 / (float) area;
+
+  }
+  else {
+    distance = -1;
+  } 
+
+  // fill point based on target and tf
 
-    // fill point based on target and tf
+  geometry_msgs::Point origPoint;
 
-    geometry_msgs::Point origPoint;
+  // TODO: Change coordinate axes
 
-    origPoint.x = comX;
-    origPoint.y = comY; 
-    origPoint.z = distance;
+  origPoint.x = comX;
+  origPoint.y = comY; 
+  origPoint.z = distance;
 
-    // TODO transform into point
+  // TODO transform into point
  
-    // for now just publish that
+  // for now just publish that
+    
+  pub.publish(origPoint);
 
-    pub.publish(origPoint);
-  }
+  printf("Ending callback");
+
+}
+
+void target_cb_test(const sensor_msgs::Image image){
+
+	geometry_msgs::Point point;
+	point.x = 1;
+	point.y = 2;
+	point.z = 3;
+	pub.publish(point);
 }
 
 int main(int argc, char **argv) {
   ros::init(argc, argv, "v2v3_converter");
   ros::NodeHandle n;
+  pub = n.advertise<geometry_msgs::Point>("vision/target_3d", 1000);
+  img_pub = n.advertise<sensor_msgs::Image>("vision/blob_image", 1000);
   ros::Subscriber sub = n.subscribe("/camera/rgb/image_color", 1, target_cb);
-  pub = n.advertise<geometry_msgs::Point>("target_3d", 1000);
-  img_pub = n.advertise<sensor_msgs::Image>("blob_image", 1000);
   ros::spin();
   return 0;
 }
