Quadrotor

#include "/home/nstanley/OpenCV/OpenCV-2.3.1/include/opencv/cv.h"

#include "/home/nstanley/OpenCV/OpenCV-2.3.1/include/opencv/highgui.h"

#include <opencv/cxcore.h>

#include <ros/ros.h>

#include <stdio.h>

#include <cv_bridge/cv_bridge.h>

#include <sensor_msgs/image_encodings.h>

#include <vision/TargetDescriptor.h>

#include <image_transport/image_transport.h>

#include <geometry_msgs/Point.h>

#include <tf/transform_listener.h>

#include <algorithm>

#include </home/nstanley/cvblobs8.3_linux/blob.h>

#include </home/nstanley/cvblobs8.3_linux/BlobResult.h>

using namespace std;

using namespace cv;

ros::Publisher pub;

ros::Publisher img_pub;

int hk = 216; 

int sk = 255;

int vk = 255;

//int threshold = 25;

int filter(int r, int g, int b, int threshold) {

    int orange[3] = {hk,sk,vk}; // Orange in HSV

    int diff =  (orange[0]-r)*(orange[0]-r) +

                (orange[1]-g)*(orange[1]-g);

    if(diff < threshold) return abs(diff-threshold);

    return 0;

}

void target_cb(const sensor_msgs::Image orig) {

  printf("beginning callback \n");

  tf::TransformListener listener;

  geometry_msgs::Point point;

  tf::StampedTransform transform;

  /*try {

    listener.lookupTransform("/camera", "/kinect", ros::Time(0), transform);

  }

  catch (tf::TransformException ex) {

    ROS_ERROR("%s", ex.what());

  }*/

  // convert to OpenCV

  sensor_msgs::Image hsvcopy = orig;

  cv_bridge::CvImagePtr cv_ptr;

  try{

    cv_ptr = cv_bridge::toCvCopy(orig, sensor_msgs::image_encodings::BGR8);

  }

  catch (cv_bridge::Exception& e)  {

    ROS_ERROR("cv_bridge exception: %s", e.what());

    return;

  }

  // detect blob

  IplImage* input;

  input = new IplImage;

  *input = cv_ptr->image;

  IplImage* img;

  IplImage *hsv_img;

  IplImage *bw_img;

  IplImage* i1;

  CBlobResult blobs;

  CBlob blobArea;

  // Initialize images, allocate memory

  img = cvCloneImage(input);

  hsv_img = cvCloneImage(img);

  bw_img = cvCreateImage(cvSize(img->width, img->height), 8, 1);;

  i1 = cvCreateImage(cvSize(img->width, img->height), 8, 1);

  printf("Initialized!\n");

  printf("%x\n", input);

  printf("%x\n", img);

  printf("%x\n", hsv_img);

  printf("%x\n", bw_img);

  printf("%x\n", i1);

  // Smooth input image using a Gaussian filter, assign HSV, BW image

  cvSmooth(input, img, CV_GAUSSIAN, 7, 9, 0 ,0);

  printf("1");

  cvCvtColor(img, bw_img, CV_RGB2GRAY);

  printf("2");

  cvCvtColor(img, hsv_img, CV_RGB2HSV);

  printf("3");

  // Simple filter that creates a mask whose color is near orange in i1

  for(int i = 0; i < hsv_img->height; i++) {

      for(int j = 0; j < hsv_img->width; j++) {

          int r = ((uchar *)(hsv_img->imageData + i*hsv_img->widthStep))[j*hsv_img->nChannels + 2];

          int g = ((uchar *)(hsv_img->imageData + i*hsv_img->widthStep))[j*hsv_img->nChannels + 1];

          int b = ((uchar *)(hsv_img->imageData + i*hsv_img->widthStep))[j*hsv_img->nChannels + 0];

          int f = filter(r, g, b, 5000);

          if(f) {

              ((uchar *)(i1->imageData + i*i1->widthStep))[j] = 250;

          }

      }

  }

  printf("4");

  // Detect Blobs using the mask and a threshold for area. Sort blobs according to area

  blobs = CBlobResult(bw_img, i1, 100/*, true*/);

  blobs.Filter(blobs, B_INCLUDE, CBlobGetArea(), B_GREATER, 500);

  blobs.GetNthBlob(CBlobGetArea(), blobs.GetNumBlobs() - 1, blobArea); // example

  for (int i = 1; i < blobs.GetNumBlobs(); i++ )

  {

      blobArea = blobs.GetBlob(i);

      if(blobArea.Area() > 150)

      blobArea.FillBlob(img, cvScalar(255, 0, 0));

  }

  // Publish blob image

  cv_bridge::CvImage blob_image;

  //blob_image.header = img->header;

  blob_image.encoding = sensor_msgs::image_encodings::BGR8;

  blob_image.image = *(img->imageData);

  /*sensor_msgs::Image blob_image; 

  blob_image.height = img->height; 

  blob_image.width = img->width;

  blob_image.data = *(img->imageData); */

  img_pub.publish(blob_image);  

  // Clean up 

  cvReleaseImage(&i1);

  cvReleaseImage(&bw_img);

  cvReleaseImage(&hsv_img);

  cvReleaseImage(&img);

  cvReleaseImage(&input);

  //int comX; 

  //int comY;

  //int area;

  //int distance;

  // fill point based on target and tf

//  geometry_msgs::Point origPoint;

  // TODO: Change coordinate axes

//  origPoint.x = distance;

//  origPoint.y = width / 2 - comX;

//  origPoint.z = comY - height / 2;

  // TODO transform into point

  // for now just publish that

//  pub.publish(origPoint);

  printf("Ending callback\n");

}

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);

  ros::spin();

  return 0;

}