Quadrotor

#include <ros/ros.h>

#include <vision/TargetDescriptor.h>

#include <image_transport/image_transport.h>

#include <geometry_msgs/Point.h>

#include <tf/transform_listener.h>

ros::Publisher pub;

int hk = 25; 

int sk = 255;

int vk = 255;

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

}

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 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 1 

 * 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 = 1; i < nl; i++){

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

                        if (image->data[i * image->width + j] < 30 && image->data[j] > 20){

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

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

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

                        }

                        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] == 1 && hasAnAdjacent(image, i)) ){

                        image->data[i] = 0;

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

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

                }

                blobPixelCount++;

                comXSum += image->data[i] % nc + 1;

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

        }

        comXSum /= blobPixelCount;

        comYSum /= blobPixelCount;

        *area = blobPixelCount;

        *comX = comXSum;

        *comY = comYSum;

}

void convertToHSV(sensor_msgs::Image * orig, sensor_msgs::Image * result){

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

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

      }

      result->data[i] = h;

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

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

  }

}

void target_cb(const sensor_msgs::Image orig) {

  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 copy = orig;

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

  sensor_msgs::Image hsvImage = sensor_msgs::Image();

  sensor_msgs::Image * hsvPtr = &hsvImage;

  // convert to HSV

  convertToHSV(source, hsvPtr);

  // detect blob

  int comX; 

  int comY;

  int area;

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

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

  int distance = 100.0 / (float) area;

  // fill point based on target and tf

  geometry_msgs::Point origPoint;

  origPoint.x = comX;

  origPoint.y = comY; 

  origPoint.z = distance;

  // TODO transform into point

  // for now just publish that

  pub.publish(origPoint);

}

int main(int argc, char **argv) {

  ros::init(argc, argv, "v2v3_converter");

  ros::NodeHandle n;

  ros::Subscriber sub = n.subscribe("openni/image", 1, target_cb);

  pub = n.advertise<geometry_msgs::Point>("target_3d", 1000);

  ros::spin();

  return 0;

}