Revision history [back]

/* * blobdetection.cpp * * Created on: 23/03/2017 * Author: zubair khan */

include <ros ros.h="">

include <stdlib.h>

include <iostream>

include <opencv2 highgui="" highgui.hpp="">

include <opencv2 opencv.hpp="">

include <opencv2 imgproc="" imgproc.hpp="">

include <opencv2 core="" core.hpp="">

include <librealsense rs.hpp="">

include <librealsense rscore.hpp="">

include <cv_bridge cv_bridge.h="">

include <image_transport image_transport.h="">

include <sensor_msgs image_encodings.h="">

include <geometry_msgs twist.h="">

include <geometry_msgs vector3.h="">

include <pcl point_types.h="">

include <pcl_ros transforms.h="">

include <pcl conversions.h="">

include <pcl pclpointcloud2.h="">

include <pcl_conversions pcl_conversions.h="">

include <boost foreach.hpp="">

include <geometry_msgs point.h="">

include <sensor_msgs pointcloud2.h="">

namespace enc = sensor_msgs::image_encodings; using namespace std;

static const char WINDOW[] = "Image Processed"; static const char RESULT[] = "Tracking";

sensor_msgs::PointCloud2 my_pcl;

//Use method of ImageTransport to create image publisher image_transport::Publisher pub; bool hasNewPcl = false;

int LowerH = 170; int LowerS = 150; int LowerV = 60; int UpperH = 179; int UpperS = 255; int UpperV = 255;

int posX; int posY;

ros::NodeHandle *n;

sensor_msgs::PointCloud2 depth; pcl::PointCloud < pcl::PointXYZ > pcl_cloud;

typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;

void depthcallback (const sensor_msgs::PointCloud2ConstPtr& cloud_msg) { my_pcl = *cloud_msg; hasNewPcl = true; cout<<"here"<<endl;< p="">

}

void getXYZ(int x, int y) { int arrayPosition = ymy_pcl.row_step + xmy_pcl.point_step; int arrayPosX = arrayPosition + my_pcl.fields[0].offset; // X has an offset of 0 int arrayPosY = arrayPosition + my_pcl.fields[1].offset; // Y has an offset of 4 int arrayPosZ = arrayPosition + my_pcl.fields[2].offset; // Z has an offset of 8

float X ;

float Y ; float Z ;

memcpy(&X, &my_pcl.data[arrayPosX], sizeof(float));
memcpy(&Y, &my_pcl.data[arrayPosY], sizeof(float));
memcpy(&Z, &my_pcl.data[arrayPosZ], sizeof(float));

geometry_msgs::Point p; // put data into the point p p.x = X; p.y = Y; p.z = Z; std::cout<<"z:"<<p.z<<std::endl; }<="" p="">

void blobDetectionCallback(const sensor_msgs::ImageConstPtr& original_image) { //Convert from the ROS image message to a CvImage suitable for working with OpenCV for processing cv_bridge::CvImagePtr cv_ptr; try { cv_ptr = cv_bridge::toCvCopy(original_image, enc::BGR8); } catch (cv_bridge::Exception& e) { //if there is an error during conversion, display it ROS_ERROR("tutorialROSOpenCV::main.cpp::cv_bridge exception: %s", e.what()); return; } cv::Mat img_mask,img_hsv; cv::cvtColor(cv_ptr->image,img_hsv,CV_BGR2HSV); cv::inRange(img_hsv,cv::Scalar(LowerH,LowerS,LowerV),cv::Scalar(UpperH,UpperS,UpperV),img_mask);

cv::erode(img_mask, img_mask, cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(5, 5)) );
cv::dilate(img_mask, img_mask, cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(5, 5)) ); 

cv::erode(img_mask, img_mask, cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(5, 5)) );
cv::dilate(img_mask, img_mask, cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(5, 5)) ); 

//Calculate the moments of the thresholded image
cv:: Moments oMoments = moments(img_mask);

double dM01 = oMoments.m01;
double dM10 = oMoments.m10;
double dArea = oMoments.m00;

posX = dM10 / dArea;
posY = dM01 / dArea;

std::cout<<"posx:"<<posX<<"posy:"<<posY<<std::endl;   

cv::circle(img_mask, cv::Point(posX, posY), 10, cv::Scalar(0,255,255), 2);

cv::Mat Points;
cv::findNonZero(img_mask,Points);
cv::Rect Min_Rect=boundingRect(Points);

cv::rectangle(img_mask,Min_Rect.tl(),Min_Rect.br(),cv::Scalar(255,0,0),1);

//Display the image using OpenCV
cv::imshow(RESULT, img_mask);



//Convert the CvImage to a ROS image message and publish it on the "camera/image_processed" topic.
pub.publish(cv_ptr->toImageMsg());

}

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

ros::init(argc, argv, "image_processor");
ros::NodeHandle nh;
image_transport::ImageTransport it(nh);


cv::namedWindow("blob");
cv::createTrackbar("LowerH","blob",&LowerH,179,NULL);
cv::createTrackbar("UpperH","blob",&UpperH,179,NULL);
cv::createTrackbar("LowerS","blob",&LowerS,255,NULL);
cv::createTrackbar("UpperS","blob",&UpperS,255,NULL);
cv::createTrackbar("LowerV","blob",&LowerV,255,NULL);
cv::createTrackbar("UpperV","blob",&UpperV,255,NULL);


cv::namedWindow(WINDOW, CV_WINDOW_AUTOSIZE);
cv::namedWindow(RESULT, CV_WINDOW_AUTOSIZE);

ros::Subscriber dep;
dep = nh.subscribe ("/camera/depth_registered/points", 1, depthcallback);

//image_transport::Subscriber depth = it.subscribe("/camera/depth_registered/points",1,depthcallback);
image_transport::Subscriber sub = it.subscribe("/camera/rgb/image_rect_color", 1, blobDetectionCallback);

pub = it.advertise("camera/image_processed", 1);
ros::Rate rate(10.0);
while(nh.ok())
{
    if(hasNewPcl)
    {
        getXYZ(posX, posY);
        hasNewPcl = false;
    }
    ros::spinOnce();
    rate.sleep();
}

}