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>

#include <algorithm>

ros::Publisher pub;

ros::Publisher img_pub;

int hk = 216; 

int sk = 255;

int vk = 255;

int threshold = 25;

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;

}

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

        // process image

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

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

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

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

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

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

                        }

                        else {

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

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

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

                        }

                }

        }

        int comXSum = 0;

        int comYSum = 0;

        int blobPixelCount = 0;

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

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

                        image->data[i] = 0;

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

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

                               continue;

                }

                blobPixelCount++;

                comXSum += i / nChannels % image->width;

                comYSum += i / nChannels / image->width;

        }

  if (blobPixelCount != 0){

        // get average

          comXSum /= blobPixelCount;

          comYSum /= blobPixelCount;

  }

        *area = blobPixelCount;

        *comX = comXSum;

        *comY = comYSum;

  img_pub.publish(*image);

}

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

      // get min, max

      if (orig->data[i] >= orig->data[i+1] && orig->data[i] >= orig->data[i+2]){

        v = orig->data[i];

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

            min = orig->data[i+2];

        }

        else min = orig->data[i+1];

      }

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

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

        if (orig->data[i] >= orig->data[i+2]){

          min = orig->data[i+2];

        }

        else min = orig->data[i];

      }

      else {

        v = orig->data[i+2];

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

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

        }

        else min = orig->data[i];

      }

      // set s

      if (v != 0){

      s = (v - min) / 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;

      }

      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;

  int width = orig.width; 

  int height = orig.height;

  /*try {

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

  }

  catch (tf::TransformException ex) {

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

  }*/

  // convert to OpenCV

  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 > 5){

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

}

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;

}