Quadrotor

#include <ros/ros.h>

#include <vision/TargetDescriptor.h>

#include <geometry_msgs/Point.h>

#include <tf/transform_listener.h>

#include <opencv/cv.hpp>

//#include <cv_bridge/cv_bridge.h>

ros::Publisher pub;

int hk = 25; 

int sk = 255;

int vk = 255;

bool isRight(cv_bridge::CvImagePtr img, int p){

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

}

bool isLeft(cv_bridge::CvImagePtr img, int p){

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

}

bool isUp(cv_bridge::CvImagePtr img, int p){

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

}

bool isDown(cv_bridge::CvImagePtr img, int p){

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

}

bool hasAnAdjacent(cv_bridge::CvImagePtr image, int j){

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

        int nc = image->width; // number of columns CHECK TO MAKE SURE THIS IS RIGHT AND NOT WITH CHANNELS

        int pos = j / image->nChannels; 

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

                if isLeft() return true;

        }

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

                if isUp() return true;

        }

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

                if isRight() return true;

        }

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

                if isDown() 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(cv_bridge::CvImagePtr image, int * comX, int * comY, int * area){

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

        int nc = image->width * image->nChannels; // number of columns

        int step = image->widthStep; // effective width

        // process image

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

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

                        if (image->data[j] > 30 || image->data[j] < 20){

                                image->data[j] = 1;

                                image->data[j+1] = 1;

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

                        }

                        else {

                                image->data[j] = 0;

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

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

                        }

                }

        }

        int comXSum = 0;

        int comYSum = 0;

        int blobPixelCount = 0;

        int maxHeight = 0;

        int minHeight = 0;

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

                i+=image->nChannels){

                if ( !(data[i] == 1 && hasAnAdjacent(image, i)) ){

                        data[i] = 0;

                        data[i + 1] = 0;

                        data[i + 2] = 0;

                }

                blobPixelCount++;

                comXSum += data[i] % nc + 1;

                comYSum += data[i] / nc + 1;

        }

        comXSum /= blobPixelCount;

        comYSum /= blobPixelCount;

        *area = blobPixelCount;

        *comX = comXSum;

        *comY = comYSum;

}

void target_cb(const sensor_msgs::msg::_Image::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

  Image * source = &orig; // pointer to the image

  cv_bridge::CvImagePtr origRGBImage = toCvCopy(const sensor_msgs::Image& source, 

        const std::string& encoding = "rgb8"); // convert to OpenCV, CHECK FOR SYNTAX

  // convert to HSV

  cv_bridge::CvImagePtr HSVImage;

  cvCvtColor (origRGBImage, HSVImage, CV_RGB2HSV);

  // detect blob

  int comX; 

  int comY;

  int area;

  detectBlobs(HSVImage, &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 = new Point_(); 

  origPoint.x = comX;

  origPoint.y = comY; 

  origPoint.z = distance;

  // TODO transform into point

  geometry_msgs::Point point;

  pub.publish(point);

}

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;

}