kzbr93

blobdetection

Apr 20th, 2017
2,182
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1.  
  2. /*
  3.  * blobdetection.cpp
  4.  *
  5.  *  Created on: 23/03/2017
  6.  *      Author: zubair khan
  7.  */
  8.  
  9.  
  10. #include <ros/ros.h>
  11. #include <stdlib.h>
  12. #include <iostream>
  13. #include <opencv2/highgui/highgui.hpp>
  14. #include <opencv2/opencv.hpp>
  15. #include <opencv2/imgproc/imgproc.hpp>
  16. #include <opencv2/core/core.hpp>
  17. #include <librealsense/rs.hpp>
  18. #include <librealsense/rscore.hpp>
  19. #include <cv_bridge/cv_bridge.h>
  20. #include <image_transport/image_transport.h>
  21. #include <sensor_msgs/image_encodings.h>
  22. #include <geometry_msgs/Twist.h>
  23. #include <geometry_msgs/Vector3.h>
  24. #include <pcl/point_types.h>
  25. #include <pcl_ros/transforms.h>
  26. #include <pcl/conversions.h>
  27. #include <pcl/PCLPointCloud2.h>
  28. #include <pcl_conversions/pcl_conversions.h>
  29. #include <boost/foreach.hpp>
  30.  
  31.  
  32. namespace enc = sensor_msgs::image_encodings;
  33.  using namespace std;
  34.  
  35. static const char WINDOW[] = "Image Processed";
  36. static const char RESULT[] = "Tracking";
  37.  
  38. sensor_msgs::PointCloud2 my_pcl;
  39.  
  40. //Use method of ImageTransport to create image publisher
  41. image_transport::Publisher pub;
  42. bool hasNewPcl = false;
  43.  
  44. int LowerH = 170;
  45. int LowerS = 150;
  46. int LowerV = 60;
  47. int UpperH = 179;
  48. int UpperS = 255;
  49. int UpperV = 255;
  50.  
  51. int posX;
  52. int posY;
  53.  
  54.  
  55.  
  56.     ros::NodeHandle *n;
  57.    
  58.    sensor_msgs::PointCloud2 depth;
  59.     pcl::PointCloud < pcl::PointXYZ > pcl_cloud;
  60.  
  61.    //  typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
  62.      
  63.  
  64.  
  65. void depthcallback (const sensor_msgs::PointCloud2ConstPtr& cloud_msg)
  66. {
  67.     my_pcl = *cloud_msg;
  68.     hasNewPcl = true;
  69.     cout<<"here"<<endl;
  70. }
  71.  
  72. void getXYZ(int x, int y)
  73. {
  74.     int arrayPosition = y*my_pcl.row_step + x*my_pcl.point_step;
  75.     int arrayPosX = arrayPosition + my_pcl.fields[0].offset; // X has an offset of 0
  76.     int arrayPosY = arrayPosition + my_pcl.fields[1].offset; // Y has an offset of 4
  77.     int arrayPosZ = arrayPosition + my_pcl.fields[2].offset; // Z has an offset of 8
  78.  
  79.     float X ;
  80.    float Y ;
  81.     float Z ;
  82.  
  83.     memcpy(&X, &my_pcl.data[arrayPosX], sizeof(float));
  84.     memcpy(&Y, &my_pcl.data[arrayPosY], sizeof(float));
  85.     memcpy(&Z, &my_pcl.data[arrayPosZ], sizeof(float));
  86.  
  87. geometry_msgs::Point p;
  88.      // put data into the point p
  89.     p.x = X;
  90.     p.y = Y;
  91.     p.z = Z;
  92.     cout<<"Pt "<<p.z<<endl;
  93. }
  94.  
  95. void blobDetectionCallback(const sensor_msgs::ImageConstPtr& original_image)
  96. {
  97.     //Convert from the ROS image message to a CvImage suitable for working with OpenCV for processing
  98.     cv_bridge::CvImagePtr cv_ptr;
  99.     try
  100.     {
  101.         //Always copy, returning a mutable CvImage
  102.         //OpenCV expects color images to use BGR channel order.
  103.         cv_ptr = cv_bridge::toCvCopy(original_image, enc::BGR8);
  104.     }
  105.     catch (cv_bridge::Exception& e)
  106.     {
  107.         //if there is an error during conversion, display it
  108.         ROS_ERROR("tutorialROSOpenCV::main.cpp::cv_bridge exception: %s", e.what());
  109.         return;
  110.     }
  111.     cv::Mat img_mask,img_hsv;
  112.     cv::cvtColor(cv_ptr->image,img_hsv,CV_BGR2HSV);
  113.     cv::inRange(img_hsv,cv::Scalar(LowerH,LowerS,LowerV),cv::Scalar(UpperH,UpperS,UpperV),img_mask);
  114.    
  115.    
  116.     cv::erode(img_mask, img_mask, cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(5, 5)) );
  117.     cv::erode(img_mask,img_mask, cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(5, 5)) );
  118.     cv::dilate(img_mask, img_mask, cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(5, 5)) );
  119.    
  120.     //Calculate the moments of the thresholded image
  121.     cv:: Moments oMoments = moments(img_mask);
  122.              
  123.     double dM01 = oMoments.m01;
  124.     double dM10 = oMoments.m10;
  125.     double dArea = oMoments.m00;
  126.              
  127.     posX = dM10 / dArea;
  128.     posY = dM01 / dArea;
  129.                  
  130.     std::cout<<"posx:"<<posX<<"posy:"<<posY<<std::endl;  
  131.  
  132.     cv::circle(img_mask, cv::Point(posX, posY), 10, cv::Scalar(0,255,255), 2);
  133.  
  134.     cv::Mat Points;
  135.     cv::findNonZero(img_mask,Points);
  136.     cv::Rect Min_Rect=boundingRect(Points);
  137.  
  138.     cv::rectangle(img_mask,Min_Rect.tl(),Min_Rect.br(),cv::Scalar(255,0,0),1);
  139.    
  140.     //Display the image using OpenCV
  141.     cv::imshow(RESULT, img_mask);
  142.    
  143.    
  144.  
  145.     //Convert the CvImage to a ROS image message and publish it on the "camera/image_processed" topic.
  146.     pub.publish(cv_ptr->toImageMsg());
  147. }
  148.  
  149. int main(int argc, char **argv)
  150. {
  151.  
  152.     ros::init(argc, argv, "image_processor");
  153.  
  154.     ros::NodeHandle nh;
  155.     //Create an ImageTransport instance, initializing it with our NodeHandle.
  156.     image_transport::ImageTransport it(nh);
  157.  
  158.    
  159.     cv::namedWindow("blob");
  160.     cv::createTrackbar("LowerH","blob",&LowerH,179,NULL);
  161.     cv::createTrackbar("UpperH","blob",&UpperH,179,NULL);
  162.     cv::createTrackbar("LowerS","blob",&LowerS,255,NULL);
  163.     cv::createTrackbar("UpperS","blob",&UpperS,255,NULL);
  164.     cv::createTrackbar("LowerV","blob",&LowerV,255,NULL);
  165.     cv::createTrackbar("UpperV","blob",&UpperV,255,NULL);
  166.    
  167.   //OpenCV HighGUI call to create a display window on start-up.
  168.     cv::namedWindow(WINDOW, CV_WINDOW_AUTOSIZE);
  169.     cv::namedWindow(RESULT, CV_WINDOW_AUTOSIZE);
  170.    
  171.     ros::Subscriber dep;
  172.      dep = nh.subscribe ("/camera/depth_registered/points", 1, depthcallback);
  173.    
  174.    //image_transport::Subscriber depth = it.subscribe("/camera/depth/points",1,depthcallback);
  175.     image_transport::Subscriber sub = it.subscribe("/camera/rgb/image_rect_color", 1, blobDetectionCallback);
  176.    
  177.    
  178.     //OpenCV HighGUI call to destroy a display window on shut-down.
  179.     //cv::destroyWindow(WINDOW);
  180.     //cv::destroyWindow(RESULT);
  181.    
  182.     pub = it.advertise("camera/image_processed", 1);
  183.     ros::Rate rate(10.0);
  184.     while(nh.ok())
  185.     {
  186.         if(hasNewPcl)
  187.         {
  188.             getXYZ(posX, posY);
  189.             hasNewPcl = false;
  190.         }
  191.         ros::spinOnce();
  192.         rate.sleep();
  193.     }
  194.    
  195. }
RAW Paste Data Copied