Quadrotor

#include <ros/ros.h>

#include <stdio.h>

#include <vision/TargetDescriptor.h>

#include <image_transport/image_transport.h>

#include <geometry_msgs/Point.h>

#include <tf/transform_listener.h>

ros::Publisher pub; // publishes Point

ros::Publisher img_pub; //publishes Image with only blob showing

int hk = 216; // target hsv values

int sk = 255;

int vk = 255;

/* Given index p and image pointer img, 

 * determines whether or not the pixel 

 * in the indicated vicinity is equal to it.

 */

bool isRight(sensor_msgs::Image * img, int p){

        return (img->data[p + 1] == img->data[p]);

}

bool isLeft(sensor_msgs::Image * img, int p){

        return (img->data[p - 1] == img->data[p]);

}

bool isUp(sensor_msgs::Image * img, int p){

        return (img->data[p - img->width] == img->data[p]);

}

bool isDown(sensor_msgs::Image * img, int p){

        return (img->data[p + img->width] == img->data[p]);

}

int min(int a, int b){

        if (a <= b) return a;

        else return b;

}

int max(int a, int b){

        if (a >= b) return a;

        else return b;

}

/* Checks for any adjacent pixel, ensuring a continuous blob.

 */

bool hasAnAdjacent(sensor_msgs::Image * image, int j){

        int nl = image->height; // number of lines

        int nc = image->width; 

        int nChannels = image->step / image->width;

        int pos = j / nChannels; 

        if ((pos % nc) != 0){

                if (isLeft(image, pos)) return true;

        }

        if ((pos / nc) != 0){

                if (isUp(image, pos)) return true;

        }

        if ((pos % nc) != nc - 1){

                if (isRight(image, pos)) return true;

        }

        if ((pos / nc) != nc - 1){

                if (isDown(image, pos)) return true;

        }

        else return false;

}

/* Detects the blobs in an image within above defined constraints. Transforms image into 255

 * for pixels in the blob, 0 for not in the blob.

 */

void detectBlobs(sensor_msgs::Image * image, int * comX, int * comY, int * area){

        int nl = image->height; // number of lines

        int nc = image->step; // number of columns

        int nChannels = image->step / image->width; // effective width of each pixel

        // process image

        for (int i = 0; i < nl; i++){

                for (int j = 0; j < nc; j+= nChannels){

                    // if within bounds...        

            if (image->data[i * image->width + j] <= hk + 10 && image->data[j] >= hk - 10){ 

                                image->data[i * image->width + j] = 255; // set each to 255

                                image->data[i * image->width + j+1] = 255;

                                image->data[i * image->width + j+2] = 255;

                        }

            // otherwise set to 0

                        else {

                                image->data[i * image->width + j] = 0;

                                image->data[i * image->width + j+1] = 0;

                                image->data[i * image->width + j+2] = 0;

                        }

                }

        }

    // The following is to find the average

        int comXSum = 0; 

        int comYSum = 0; 

        int blobPixelCount = 0; 

        for (int i = 0; i < image->width * image->height; i+=nChannels){

        // if the pixel is not both the right hue and has adjacents, set to 0

                if ( !(image->data[i] == 255 && hasAnAdjacent(image, i)) ){

                        image->data[i] = 0;

                        image->data[i + 1] = 0;

                        image->data[i + 2] = 0;

                }

        else {

                    blobPixelCount++;

                    comXSum += image->data[i] / 255;

                    comYSum += image->data[i] / 255;

        }

        }

  if (blobPixelCount != 0){

        // get average

          comXSum /= blobPixelCount;

          comYSum /= blobPixelCount;

  }

    // output results

        *area = blobPixelCount;

    *comX = comXSum;

    *comY = comYSum;

    img_pub.publish(*image); // publish new image of only black and white

}

void convertToHSV(sensor_msgs::Image * orig){

  int nChannels = orig->step / orig->width; 

  printf("Converting to HSV \n");

  // get the max, set it as v

  for (int i = 0; i < orig->height * orig->width; i += nChannels){

      int h;

      int s;

      int v; 

      int low;

      // get min, max

      v = max(max(orig->data[i], orig->data[i+1]), orig->data[i+2]);

      low = min(min(orig->data[i], orig->data[i+1]), orig->data[i+2]);

      // set s

      if (v != 0){

      s = (v - low) / v; 

      }

      else s = 0;

      // set h

      if (s == 0) h = 0;

      else if (v == orig->data[i]){

        h = 60 * (orig->data[i+1] - orig->data[i+2]) / s;

      }

      else if (v == orig->data[i+1]){

        h = 120 + 60 * (orig->data[i+2] - orig->data[i]) / s;

      }

      else {

        h = 240 + 60 * (orig->data[i] - orig->data[i + 1]) / s;

      }

      // set values

      orig->data[i] = h;

      orig->data[i+1] = s;

      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;

  /*try {

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

  }

  catch (tf::TransformException ex) {

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

  }*/

  // image types

  sensor_msgs::Image copy = orig;

  sensor_msgs::Image * source = © // pointer to the image

  sensor_msgs::Image hsvImage = orig; // May be time 

  sensor_msgs::Image * hsvPtr = &hsvImage;

  // convert to HSV

  convertToHSV(source);

  // detect blob

  int comX; 

  int comY;

  int area;

  detectBlobs(hsvPtr, &comX, &comY, &area);

  int distance;

  // give distance as inversely proportional to area, for (x,y,z) 

  if (area > 1){

    distance = 100.0 / (float) area;

  }

  else {

    distance = -1;

  } 

  // fill point based on target and tf

  geometry_msgs::Point origPoint;

  // TODO: Change coordinate axes

  origPoint.x = comX;

  origPoint.y = comY; 

  origPoint.z = distance;

  // TODO transform into point

  // for now just publish that

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

  ros::spin();

  return 0;

}