eddiebot_line_follower.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2012, Tang Tiong Yew
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 #include <ros/ros.h>
00030 #include <stdio.h>
00031 #include <iostream>
00032 #include "std_msgs/String.h"
00033 #include <image_transport/image_transport.h>
00034 #include <cv_bridge/cv_bridge.h>
00035 #include <sensor_msgs/image_encodings.h>
00036 #include <opencv2/imgproc/imgproc.hpp>     //make sure to include the relevant headerfiles
00037 #include <opencv2/highgui/highgui.hpp>
00038 #include <opencv/cv.h>
00039 #include <opencv/highgui.h>
00040 #include <cv_bridge/CvBridge.h>
00041 #include <cvaux.h>
00042 #include <math.h>
00043 #include <cxcore.h>
00044 #include <geometry_msgs/Twist.h>
00045 #include "boost/thread/mutex.hpp"
00046 #include "boost/thread/thread.hpp"
00047 
00048 /*here is a simple program which demonstrates the use of ros and opencv to do image manipulations on video streams given out as image topics from the monocular vision
00049  of robots,here the device used is a ardrone(quad-rotor).*/
00050 using namespace std;
00051 using namespace cv;
00052 namespace enc = sensor_msgs::image_encodings;
00053 static const char WINDOW[] = "Image window";
00054 
00055 float prevVelocity_angular, prevVelocity_linear, newVelocity_angular,
00056                 newVelocity_linear;
00057 float derive_angular, derive_linear, dt = 0.5;
00058 float horizontalcount;
00059 
00060 class ImageConverter {
00061         ros::NodeHandle nh_, ph_;
00062         ros::NodeHandle n;
00063         ros::Publisher pub;
00064         ros::Publisher tog;
00065         image_transport::ImageTransport it_;
00066         image_transport::Subscriber image_sub_; //image subscriber
00067         image_transport::Publisher image_pub_; //image publisher(we subscribe to ardrone image_raw)
00068         std_msgs::String msg;
00069 
00070         double linear_, angular_;
00071         double l_scale_, a_scale_;
00072         double left_threshold, right_threshold;
00073 public:
00074         ImageConverter() :
00075                 linear_(0.05),
00076                 angular_(0.05),
00077                 l_scale_(1.0),
00078                 a_scale_(1.0),
00079                 left_threshold(150),
00080                 right_threshold(450),
00081                 it_(nh_) {
00082                         pub = n.advertise <geometry_msgs::Twist> ("cmd_vel", 1);
00083                         image_sub_ = it_.subscribe("/usb_cam/image_raw", 1,
00084                                         &ImageConverter::imageCb, this);
00085                         image_pub_ = it_.advertise("/arcv/Image", 1);
00086 
00087                         // Initialize Parameters
00088                         nh_.param("scale_angular", a_scale_, a_scale_);
00089                         nh_.param("scale_linear", l_scale_, l_scale_);
00090                         nh_.param("angular", angular_, angular_);
00091                         nh_.param("linear", linear_, linear_);
00092                         nh_.param("left_threshold", left_threshold, left_threshold);
00093                         nh_.param("right_threshold", right_threshold, right_threshold);
00094 
00095         }
00096 
00097         ~ImageConverter() {
00098                 cv::destroyWindow(WINDOW);
00099         }
00100 
00101         void imageCb(const sensor_msgs::ImageConstPtr& msg) {
00102 
00103                 sensor_msgs::CvBridge bridge; //we need this object bridge for facilitating conversion from ros-img to opencv
00104                 IplImage* img = bridge.imgMsgToCv(msg, "rgb8"); //image being converted from ros to opencv using cvbridge
00105                 geometry_msgs::Twist velMsg;
00106                 CvMemStorage* storage = cvCreateMemStorage(0);
00107                 CvSeq* lines = 0;
00108                 int i, c, d;
00109                 float c1[50];
00110                 float m, angle;
00111                 float buf;
00112                 float m1;
00113                 float dis;
00114                 int k = 0, k1 = 0;
00115                 int count = 0;
00116 
00117                 float xv;
00118                 float yv;
00119                 int vc = 0;
00120                 float xvan = 0, yvan = 0;
00121                 static float xvan1 = 0, yvan1 = 0;
00122                 float num = 0;
00123                 static float count1 = 0;
00124                 float dv;
00125                 float vxx, vyy;
00126 
00127                 cvSetImageROI(img, cvRect(0, 0, 640, 480));
00128                 IplImage* out1 = cvCreateImage(cvGetSize(img), IPL_DEPTH_8U, 3); //make sure to feed the image(img) data to the parameters necessary for canny edge output
00129                 IplImage* gray_out = cvCreateImage(cvGetSize(img), IPL_DEPTH_8U, 1);
00130                 IplImage* canny_out = cvCreateImage(cvGetSize(img), IPL_DEPTH_8U, 1);
00131                 IplImage* gray_out1 = cvCreateImage(cvGetSize(img), IPL_DEPTH_8U, 3);
00132                 IplImage* canny_out1 = cvCreateImage(cvGetSize(img), IPL_DEPTH_8U, 1);
00133                 IplImage* canny_out2 = cvCreateImage(cvGetSize(img), IPL_DEPTH_8U, 1);
00134 
00135                 IplImage* gray_out2 = cvCreateImage(cvGetSize(img), IPL_DEPTH_8U, 1);
00136 
00137                 cvSmooth(img, out1, CV_GAUSSIAN, 11, 11);
00138 
00139                 cvCvtColor(out1, gray_out, CV_RGB2GRAY);
00140                 cvNot(gray_out,gray_out2);
00141                 cvCanny(gray_out2, canny_out, 80, 125, 3);
00142                 cvCvtColor(canny_out, gray_out1, CV_GRAY2BGR);
00143 
00144 //              //////////////// Color Filtering //////////////
00145 //
00146 //              //IplImage *imgHsv,*imgResult;
00147 //              int mR_val=255,mG_val=128,mB_val=64,MAR_val=128,MAG_val=0,MAB_val=0;//default green .ctrl BLUE to find color
00148 //
00149 //
00150 //              IplImage* test=cvCreateImage(cvGetSize(img), IPL_DEPTH_8U, 3);
00151 //              IplImage* imgResult=cvCreateImage(cvGetSize(img), IPL_DEPTH_8U, 3);
00152 //              IplImage* imgHsv=cvCreateImage(cvGetSize(img), IPL_DEPTH_8U, 3);
00153 //
00154 //              cvCvtColor( test, imgHsv, CV_BGR2HSV);//convert the color space
00155 //              CvScalar min_color = CV_RGB(mR_val,mG_val,mB_val);
00156 //              CvScalar max_color = CV_RGB(MAR_val,MAG_val,MAB_val);
00157 //              cvInRangeS(imgHsv, min_color,max_color, imgResult);//search for the color in image
00158 //              //////////////// Color Filtering //////////////
00159 
00160 
00161 
00162                 lines = cvHoughLines2(canny_out, storage, CV_HOUGH_PROBABILISTIC, 1,
00163                                 CV_PI / 180, 80, 50, 10);
00164                 for (i = 0; i < lines->total; i++) {
00165 
00166                         CvPoint* line = (CvPoint*) cvGetSeqElem(lines, i);
00167                         {
00168                                 {
00169                                         cvLine(out1, line[0], line[1], CV_RGB(0, 255, 0), 1, 8);
00170                                         cvLine(gray_out1, line[0], line[1], CV_RGB(0, 255, 0), 2,
00171                                                         8);
00172                                         xv = line[0].x - line[1].x;
00173                                         yv = line[0].y - line[1].y;
00174 //                                      velMsg.linear = atan2(xv, yv) * 180 / 3.14159265;
00175 //                                      angle = velMsg.linear;
00176 //                                      printf("test");
00177 //                                      if (velMsg.linear < -90) {
00178 //                                              printf("one");
00179 //                                              velMsg.linear = velMsg.linear + 180;
00180 //                                      }
00181 //                                      buf = (line[0].x + line[1].x) / 2;
00182 //
00183 //                                      if (abs(85 - buf) <= 15) {
00184 //                                              velMsg.angular = 0;
00185 //                                              printf("two");
00186 //                                      } else {
00187 //                                              velMsg.angular = (85 - (line[0].x + line[1].x) / 2);
00188 //                                              printf("three");
00189 //                                      }
00190 
00191 //                                      if (abs(velMsg.angular) > 50) {
00192 //                                              velMsg.angular = 0;
00193 //                                              printf("four");
00194 //                                      }
00195 // Modified
00196                                         angle = atan2(xv, yv) * 180 / 3.14159265;
00197 
00198 
00199                                         buf = (line[0].x + line[1].x) / 2;
00200                                         printf("line[0].x: %f", line[0].x);
00201                                         printf("line[1].x: %f", line[1].x);
00202                                         velMsg.linear.z = buf;
00203                                         velMsg.linear.y = angle;
00204 
00205                                         if (abs(buf) <= left_threshold) { // turn left
00206                                                 velMsg.angular.z = angular_ * a_scale_;
00207                                         }
00208                                         else if (abs(buf) >= right_threshold) { // turn right
00209                                                 velMsg.angular.z = -angular_ * a_scale_;
00210                                         }
00211                                         else {
00212                                                 velMsg.linear.x = linear_ * l_scale_;
00213                                         }
00214 
00215 
00216                                         printf("\nX::Y::X1::Y1::%d:%d:%d:%d", line[0].x, line[0].y,
00217                                                         line[1].x, line[1].y);
00218 
00219                                         pub.publish(velMsg);
00220 
00221                                 }
00222 
00223                         }
00224 
00225                         cvSeqRemove(lines, i);
00226 
00227                 }
00228                 cvShowImage("OUT1", out1);   //lines projected onto the real picture
00229                 cvShowImage("GRAY_OUT1", gray_out1);
00230 
00231                 // Added Display
00232 //              cvShowImage("GRAY_OUT", gray_out);
00233 //              cvShowImage("CANNY_OUT", canny_out);
00234 //              cvShowImage("CANNY_OUT1", canny_out1);
00235 //              cvShowImage("gray_out2", gray_out2);
00236 //              cvShowImage("imgResult", imgResult);
00237                 cv::waitKey(3);
00238                 sensor_msgs::ImagePtr out = sensor_msgs::CvBridge::cvToImgMsg(img,
00239                                 "rgb8");   //image converted from opencv to ros for publishing
00240                 image_pub_.publish(out);
00241                 cvClearMemStorage(storage);
00242                 cvReleaseMemStorage(&storage);
00243                 cvReleaseImage( &out1 );
00244                 cvReleaseImage( &gray_out );
00245                 cvReleaseImage( &canny_out );
00246                 cvReleaseImage( &gray_out1 );
00247                 cvReleaseImage( &canny_out1 );
00248                 cvReleaseImage( &canny_out2 );
00249                 cvReleaseImage( &gray_out2 );
00250 
00251 
00252 
00253         }
00254 
00255 };
00256 
00257 int main(int argc, char** argv) {
00258         ros::init(argc, argv, "image_converter");
00259         ImageConverter ic;
00260         ros::spin();
00261         return 0;
00262 }


eddiebot_line_follower
Author(s): Tang Tiong Yew
autogenerated on Sun Oct 5 2014 23:39:13