kzbr93

Untitled

Apr 27th, 2017
96
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. #include <ros/ros.h>
  10. #include <stdlib.h>
  11. #include <iostream>
  12. #include <opencv2/highgui/highgui.hpp>
  13. #include <opencv2/opencv.hpp>
  14. #include <opencv2/imgproc/imgproc.hpp>
  15. #include <opencv2/core/core.hpp>
  16. #include <librealsense/rs.hpp>
  17. #include <librealsense/rscore.hpp>
  18. #include <cv_bridge/cv_bridge.h>
  19. #include <image_transport/image_transport.h>
  20. #include <sensor_msgs/image_encodings.h>
  21. #include <geometry_msgs/Twist.h>
  22. #include <geometry_msgs/Vector3.h>
  23. #include <pcl/point_types.h>
  24. #include <pcl_ros/transforms.h>
  25. #include <pcl/conversions.h>
  26. #include <pcl/PCLPointCloud2.h>
  27. #include <pcl_conversions/pcl_conversions.h>
  28. #include <boost/foreach.hpp>
  29. #include <geometry_msgs/Point.h>
  30. #include <sensor_msgs/PointCloud2.h>
  31. #include <blobdetection/Followmesrv.h>
  32. #include <blobdetection/Followmemsg.h>
  33. #include <std_msgs/String.h>
  34. #include <sstream>
  35.  
  36. namespace enc = sensor_msgs::image_encodings;
  37. using namespace std;
  38.  
  39.  
  40. //static const char WINDOW[] = "Image Processed";
  41. //static const char RESULT[] = "Tracking";
  42.  
  43. sensor_msgs::PointCloud2 my_pcl;
  44.  
  45. //Use method of ImageTransport to create image publisher
  46. image_transport::Publisher pub;
  47. bool hasNewPcl = false;
  48.  
  49. int LowerH = 170;
  50. int LowerS = 150;
  51. int LowerV = 60;
  52. int UpperH = 179;
  53. int UpperS = 255;
  54. int UpperV = 255;
  55.  
  56. int posX;
  57. int posY;
  58.  
  59. geometry_msgs::Twist speed;
  60. ros::Publisher velPub;
  61. blobdetection::Followmemsg follow_msg;
  62. ros::Publisher followmemsg_pub;
  63.  
  64. bool isStop = false;
  65. bool isStart = false;
  66. bool person = false;
  67. int countPerson = 0;
  68. int COUNT_PERSON_MAX = 1;
  69. int lastPersonSend = 0;
  70. ros::NodeHandle* n;
  71.  
  72. sensor_msgs::PointCloud2 depth;
  73. pcl::PointCloud<pcl::PointXYZ> pcl_cloud;
  74.  
  75. // typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
  76.  
  77. //service
  78.  
  79. bool followmesrv(blobdetection::Followmesrv::Request& req,
  80. blobdetection::Followmesrv::Response& res)
  81. {
  82. if (req.function == 0) {
  83. res.success = true;
  84. isStop = true;
  85. isStart = false;
  86. }else if(req.function == 1 && lastPersonSend == 1) {
  87. res.success = true;
  88. isStop = false;
  89. isStart = true;
  90. }else{
  91. res.success = false;
  92. }
  93. return true;
  94. }
  95.  
  96. void depthcallback(const sensor_msgs::PointCloud2ConstPtr& cloud_msg)
  97. {
  98. my_pcl = *cloud_msg;
  99. hasNewPcl = true;
  100. //cout<<"here"<<endl;
  101. }
  102.  
  103. void publishPersonDetected(bool isPersonDetected)
  104. {
  105. cout << "Person detected " << isPersonDetected << endl;
  106. if (isPersonDetected)
  107. {
  108. if (follow_msg.person == 1)
  109. {
  110. countPerson++;
  111. }
  112. else
  113. {
  114. countPerson = 0;
  115. cout << "Change msg person = 1" << endl;
  116. follow_msg.person = 1;
  117. }
  118. }
  119. else
  120. {
  121. if (follow_msg.person == 0)
  122. {
  123. countPerson++;
  124. }
  125. else
  126. {
  127. countPerson = 0;
  128. cout << "Change msg person = 0" << endl;
  129. follow_msg.person = 0;
  130. }
  131. }
  132. if (countPerson >= COUNT_PERSON_MAX && lastPersonSend != follow_msg.person)
  133. {
  134. followmemsg_pub.publish(follow_msg);
  135. lastPersonSend = follow_msg.person;
  136. countPerson = 0;
  137. }
  138. }
  139.  
  140. float getDepthAt(int x, int y)
  141. {
  142. int arrayPosition = y * my_pcl.row_step + x * my_pcl.point_step;
  143. int arrayPosX = arrayPosition + my_pcl.fields[0].offset; // X has an offset of 0
  144. int arrayPosY = arrayPosition + my_pcl.fields[1].offset; // Y has an offset of 4
  145. int arrayPosZ = arrayPosition + my_pcl.fields[2].offset; // Z has an offset of 8
  146.  
  147. float X;
  148. float Y;
  149. float Z;
  150.  
  151. memcpy(&X, &my_pcl.data[arrayPosX], sizeof(float));
  152. memcpy(&Y, &my_pcl.data[arrayPosY], sizeof(float));
  153. memcpy(&Z, &my_pcl.data[arrayPosZ], sizeof(float));
  154. return Z;
  155. }
  156.  
  157. void getXYZ(int x, int y)
  158. {
  159. int arrayPosition = y * my_pcl.row_step + x * my_pcl.point_step;
  160. int arrayPosX = arrayPosition + my_pcl.fields[0].offset; // X has an offset of 0
  161. int arrayPosY = arrayPosition + my_pcl.fields[1].offset; // Y has an offset of 4
  162. int arrayPosZ = arrayPosition + my_pcl.fields[2].offset; // Z has an offset of 8
  163.  
  164. float X;
  165. float Y;
  166. float Z;
  167.  
  168. memcpy(&X, &my_pcl.data[arrayPosX], sizeof(float));
  169. memcpy(&Y, &my_pcl.data[arrayPosY], sizeof(float));
  170. memcpy(&Z, &my_pcl.data[arrayPosZ], sizeof(float));
  171.  
  172. geometry_msgs::Point p;
  173.  
  174. // put data into the point p
  175. p.x = X;
  176. p.y = Y;
  177. p.z = Z;
  178.  
  179. if (!isnan(p.x) || !isnan(p.y) || !isnan(p.z))
  180.  
  181. {
  182.  
  183. std::cout << "x :" << p.x << "y :" << p.y << "z :" << p.z << std::endl;
  184.  
  185.  
  186. if (isnan(p.x) || isnan(p.z) || p.x > 0.35 || p.x < -0.35 || p.z > 3.20 || p.z < 1.0)
  187. {
  188. //ROS_INFO("No Person Detected.");
  189. speed.linear.x = 0;
  190. speed.angular.z = 0;
  191. person = false;
  192. }
  193. else
  194. //ROS_INFO("Person Detected.");
  195. {
  196. //Angular
  197. if (p.x > 0.21 && p.x <= 0.35)
  198. {
  199. speed.angular.z = 1.0;
  200. }
  201. if (p.x > 0.7 && p.x <= 0.21)
  202. {
  203. speed.angular.z = 0.7;
  204. }
  205. if (p.x > -0.7 && p.x <= 0.7)
  206. {
  207. speed.angular.z = 0.0;
  208. }
  209. if (p.x > -0.21 && p.x <= -0.7)
  210. {
  211. speed.angular.z = -0.7;
  212. }
  213. if (p.x >= -0.35 && p.x <= -0.21)
  214. {
  215. speed.angular.z = -1.5;
  216. }
  217.  
  218. //Velocity
  219. if (p.z > 2.6 && p.z <= 3.2)
  220. {
  221. speed.linear.x = 1;
  222. }
  223. if (p.z > 1.5 && p.z <= 2.6)
  224. {
  225. speed.linear.x = 0.70;
  226. }
  227. if (p.z > 0.5 && p.z <= 1.5)
  228. {
  229. speed.linear.x = 0.50;
  230. }
  231. if (p.z > 0.5 && p.z <= 1.30)
  232. {
  233. speed.linear.x = 0.40;
  234. }
  235. if (p.z > 0.5 && p.z <= 1.20)
  236. {
  237. speed.linear.x = 0.30;
  238. }
  239. if (p.z > 0.5 && p.z <= 1.10)
  240. {
  241. speed.linear.x = 0.20;
  242. }
  243.  
  244.  
  245. person = true;
  246. //follow_msg.person = 1;
  247. //followmemsg_pub.publish();
  248. }
  249.  
  250. if (isStart == false && isStop == true)
  251. {
  252. publishPersonDetected(person);
  253. }
  254.  
  255. if (isStart == true && isStop == false && lastPersonSend == 1)
  256. {
  257. ROS_INFO("MOVING.");
  258. velPub.publish(speed);
  259. if(person == false)
  260. {
  261. countPerson ++;
  262. }
  263. else
  264. {
  265. countPerson = 0;
  266. }
  267. if(countPerson > COUNT_PERSON_MAX)
  268. {
  269. isStart = false;
  270. isStop = true;
  271. }
  272. }
  273.  
  274. return;
  275. }
  276. }
  277.  
  278. void blobDetectionCallback(const sensor_msgs::ImageConstPtr& original_image)
  279. {
  280. //Convert from the ROS image message to a CvImage suitable for working with OpenCV for processing
  281. cv_bridge::CvImagePtr cv_ptr;
  282. try
  283. {
  284. cv_ptr = cv_bridge::toCvCopy(original_image, enc::BGR8);
  285. }
  286.  
  287. catch (cv_bridge::Exception& e)
  288. {
  289. //if there is an error during conversion, display it
  290. ROS_ERROR("tutorialROSOpenCV::main.cpp::cv_bridge exception: %s", e.what());
  291. return;
  292. }
  293.  
  294. cv::Mat img_mask, img_hsv;
  295. cv::cvtColor(cv_ptr->image, img_hsv, CV_BGR2HSV);
  296. cv::inRange(img_hsv, cv::Scalar(LowerH, LowerS, LowerV), cv::Scalar(UpperH, UpperS, UpperV), img_mask);
  297.  
  298. cv::erode(img_mask, img_mask, cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(5, 5)));
  299. cv::dilate(img_mask, img_mask, cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(5, 5)));
  300.  
  301. cv::erode(img_mask, img_mask, cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(5, 5)));
  302. cv::dilate(img_mask, img_mask, cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(5, 5)));
  303.  
  304. //Calculate the moments of the thresholded image
  305. cv::Moments oMoments = moments(img_mask);
  306.  
  307. double dM01 = oMoments.m01;
  308. double dM10 = oMoments.m10;
  309. double dArea = oMoments.m00;
  310.  
  311. posX = dM10 / dArea;
  312. posY = dM01 / dArea;
  313.  
  314. cv::circle(img_mask, cv::Point(posX, posY), 10, cv::Scalar(0, 255, 255), 2);
  315.  
  316. cv::Mat Points;
  317. cv::findNonZero(img_mask, Points);
  318. cv::Rect Min_Rect = boundingRect(Points);
  319.  
  320. cv::rectangle(img_mask, Min_Rect.tl(), Min_Rect.br(), cv::Scalar(255, 0, 0), 1);
  321.  
  322. //Display the image using OpenCV
  323. cv::imshow("RESULT", img_mask);
  324. cv::waitKey(3);
  325.  
  326. int npts = 0;
  327. float depth_average = 0;
  328. for(int i = -25; i < 25; i++)
  329. {
  330. for(int j = -25; j < 25; j++)
  331. {
  332. if(posY - j > 0 && posY + j < 480 && posX - i > 0 && posX + i < 640)
  333. {
  334. if(getDepthAt(posX + i, posY + j) < 5 && !isnan(getDepthAt(posX + i, posY + j)))
  335. {
  336. depth_average += getDepthAt(posX+i, posY+j);
  337. npts++;
  338. }
  339. }
  340.  
  341. }
  342. }
  343. cv::waitKey(3);
  344. //Convert the CvImage to a ROS image message and publish it on the "camera/image_processed" topic.
  345. pub.publish(cv_ptr->toImageMsg());
  346. }
  347.  
  348. int main(int argc, char** argv)
  349. {
  350.  
  351. ros::init(argc, argv, "image_processor");
  352. ros::NodeHandle nh;
  353. image_transport::ImageTransport it(nh);
  354.  
  355. cv::namedWindow("blob");
  356. cv::createTrackbar("LowerH", "blob", &LowerH, 179, NULL);
  357. cv::createTrackbar("UpperH", "blob", &UpperH, 179, NULL);
  358. cv::createTrackbar("LowerS", "blob", &LowerS, 255, NULL);
  359. cv::createTrackbar("UpperS", "blob", &UpperS, 255, NULL);
  360. cv::createTrackbar("LowerV", "blob", &LowerV, 255, NULL);
  361. cv::createTrackbar("UpperV", "blob", &UpperV, 255, NULL);
  362.  
  363. //cv::namedWindow(WINDOW, CV_WINDOW_AUTOSIZE);
  364. //cv::namedWindow(RESULT, CV_WINDOW_AUTOSIZE);
  365.  
  366. ros::Subscriber dep;
  367.  
  368. dep = nh.subscribe("/camera/depth_registered/points", 1, depthcallback);
  369. image_transport::Subscriber sub = it.subscribe("/camera/rgb/image_rect_color", 1, blobDetectionCallback);
  370.  
  371. followmemsg_pub = nh.advertise<blobdetection::Followmemsg>("/agv1/bt/followmemsg", 10);
  372. follow_msg.person == 1;
  373.  
  374.  
  375. ros::ServiceServer service = nh.advertiseService("followmeservice", followmesrv);
  376. velPub = nh.advertise<geometry_msgs::Twist>("/agv1/cmd_vel", 1);
  377. pub = it.advertise("camera/image_processed", 1);
  378.  
  379. ros::Rate rate(30.0);
  380.  
  381. int count = 1;
  382. while (nh.ok())
  383. {
  384. std_msgs::String msg;
  385. std::stringstream ss;
  386.  
  387. if (hasNewPcl && isStop == true || isStart == true)
  388. {
  389. getXYZ(posX, posY);
  390. hasNewPcl = false;
  391. }
  392. ros::spinOnce();
  393. rate.sleep();
  394. }
  395. }
RAW Paste Data