eddiebot_line_follower.cpp

Go to the documentation of this file.

#include <ros/ros.h>
#include <stdio.h>
#include <iostream>
#include "std_msgs/String.h"
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/imgproc/imgproc.hpp>     //make sure to include the relevant headerfiles
#include <opencv2/highgui/highgui.hpp>
#include <opencv/cv.h>
#include <opencv/highgui.h>
#include <cv_bridge/CvBridge.h>
#include <cvaux.h>
#include <math.h>
#include <cxcore.h>
#include <geometry_msgs/Twist.h>
#include "boost/thread/mutex.hpp"
#include "boost/thread/thread.hpp"

/*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
 of robots,here the device used is a ardrone(quad-rotor).*/
using namespace std;
using namespace cv;
namespace enc = sensor_msgs::image_encodings;
static const char WINDOW[] = "Image window";

float prevVelocity_angular, prevVelocity_linear, newVelocity_angular,
                newVelocity_linear;
float derive_angular, derive_linear, dt = 0.5;
float horizontalcount;

class ImageConverter {
        ros::NodeHandle nh_, ph_;
        ros::NodeHandle n;
        ros::Publisher pub;
        ros::Publisher tog;
        image_transport::ImageTransport it_;
        image_transport::Subscriber image_sub_; //image subscriber
        image_transport::Publisher image_pub_; //image publisher(we subscribe to ardrone image_raw)
        std_msgs::String msg;

        double linear_, angular_;
        double l_scale_, a_scale_;
        double left_threshold, right_threshold;
public:
        ImageConverter() :
                linear_(0.05),
                angular_(0.05),
                l_scale_(1.0),
                a_scale_(1.0),
                left_threshold(150),
                right_threshold(450),
                it_(nh_) {
                        pub = n.advertise <geometry_msgs::Twist> ("cmd_vel", 1);
                        image_sub_ = it_.subscribe("/usb_cam/image_raw", 1,
                                        &ImageConverter::imageCb, this);
                        image_pub_ = it_.advertise("/arcv/Image", 1);

                        // Initialize Parameters
                        nh_.param("scale_angular", a_scale_, a_scale_);
                        nh_.param("scale_linear", l_scale_, l_scale_);
                        nh_.param("angular", angular_, angular_);
                        nh_.param("linear", linear_, linear_);
                        nh_.param("left_threshold", left_threshold, left_threshold);
                        nh_.param("right_threshold", right_threshold, right_threshold);

        }

        ~ImageConverter() {
                cv::destroyWindow(WINDOW);
        }

        void imageCb(const sensor_msgs::ImageConstPtr& msg) {

                sensor_msgs::CvBridge bridge; //we need this object bridge for facilitating conversion from ros-img to opencv
                IplImage* img = bridge.imgMsgToCv(msg, "rgb8"); //image being converted from ros to opencv using cvbridge
                geometry_msgs::Twist velMsg;
                CvMemStorage* storage = cvCreateMemStorage(0);
                CvSeq* lines = 0;
                int i, c, d;
                float c1[50];
                float m, angle;
                float buf;
                float m1;
                float dis;
                int k = 0, k1 = 0;
                int count = 0;

                float xv;
                float yv;
                int vc = 0;
                float xvan = 0, yvan = 0;
                static float xvan1 = 0, yvan1 = 0;
                float num = 0;
                static float count1 = 0;
                float dv;
                float vxx, vyy;

                cvSetImageROI(img, cvRect(0, 0, 640, 480));
                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
                IplImage* gray_out = cvCreateImage(cvGetSize(img), IPL_DEPTH_8U, 1);
                IplImage* canny_out = cvCreateImage(cvGetSize(img), IPL_DEPTH_8U, 1);
                IplImage* gray_out1 = cvCreateImage(cvGetSize(img), IPL_DEPTH_8U, 3);
                IplImage* canny_out1 = cvCreateImage(cvGetSize(img), IPL_DEPTH_8U, 1);
                IplImage* canny_out2 = cvCreateImage(cvGetSize(img), IPL_DEPTH_8U, 1);

                IplImage* gray_out2 = cvCreateImage(cvGetSize(img), IPL_DEPTH_8U, 1);

                cvSmooth(img, out1, CV_GAUSSIAN, 11, 11);

                cvCvtColor(out1, gray_out, CV_RGB2GRAY);
                cvNot(gray_out,gray_out2);
                cvCanny(gray_out2, canny_out, 80, 125, 3);
                cvCvtColor(canny_out, gray_out1, CV_GRAY2BGR);

//              //////////////// Color Filtering //////////////
//
//              //IplImage *imgHsv,*imgResult;
//              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
//
//
//              IplImage* test=cvCreateImage(cvGetSize(img), IPL_DEPTH_8U, 3);
//              IplImage* imgResult=cvCreateImage(cvGetSize(img), IPL_DEPTH_8U, 3);
//              IplImage* imgHsv=cvCreateImage(cvGetSize(img), IPL_DEPTH_8U, 3);
//
//              cvCvtColor( test, imgHsv, CV_BGR2HSV);//convert the color space
//              CvScalar min_color = CV_RGB(mR_val,mG_val,mB_val);
//              CvScalar max_color = CV_RGB(MAR_val,MAG_val,MAB_val);
//              cvInRangeS(imgHsv, min_color,max_color, imgResult);//search for the color in image
//              //////////////// Color Filtering //////////////



                lines = cvHoughLines2(canny_out, storage, CV_HOUGH_PROBABILISTIC, 1,
                                CV_PI / 180, 80, 50, 10);
                for (i = 0; i < lines->total; i++) {

                        CvPoint* line = (CvPoint*) cvGetSeqElem(lines, i);
                        {
                                {
                                        cvLine(out1, line[0], line[1], CV_RGB(0, 255, 0), 1, 8);
                                        cvLine(gray_out1, line[0], line[1], CV_RGB(0, 255, 0), 2,
                                                        8);
                                        xv = line[0].x - line[1].x;
                                        yv = line[0].y - line[1].y;
//                                      velMsg.linear = atan2(xv, yv) * 180 / 3.14159265;
//                                      angle = velMsg.linear;
//                                      printf("test");
//                                      if (velMsg.linear < -90) {
//                                              printf("one");
//                                              velMsg.linear = velMsg.linear + 180;
//                                      }
//                                      buf = (line[0].x + line[1].x) / 2;
//
//                                      if (abs(85 - buf) <= 15) {
//                                              velMsg.angular = 0;
//                                              printf("two");
//                                      } else {
//                                              velMsg.angular = (85 - (line[0].x + line[1].x) / 2);
//                                              printf("three");
//                                      }

//                                      if (abs(velMsg.angular) > 50) {
//                                              velMsg.angular = 0;
//                                              printf("four");
//                                      }
// Modified
                                        angle = atan2(xv, yv) * 180 / 3.14159265;


                                        buf = (line[0].x + line[1].x) / 2;
                                        printf("line[0].x: %f", line[0].x);
                                        printf("line[1].x: %f", line[1].x);
                                        velMsg.linear.z = buf;
                                        velMsg.linear.y = angle;

                                        if (abs(buf) <= left_threshold) { // turn left
                                                velMsg.angular.z = angular_ * a_scale_;
                                        }
                                        else if (abs(buf) >= right_threshold) { // turn right
                                                velMsg.angular.z = -angular_ * a_scale_;
                                        }
                                        else {
                                                velMsg.linear.x = linear_ * l_scale_;
                                        }


                                        printf("\nX::Y::X1::Y1::%d:%d:%d:%d", line[0].x, line[0].y,
                                                        line[1].x, line[1].y);

                                        pub.publish(velMsg);

                                }

                        }

                        cvSeqRemove(lines, i);

                }
                cvShowImage("OUT1", out1);   //lines projected onto the real picture
                cvShowImage("GRAY_OUT1", gray_out1);

                // Added Display
//              cvShowImage("GRAY_OUT", gray_out);
//              cvShowImage("CANNY_OUT", canny_out);
//              cvShowImage("CANNY_OUT1", canny_out1);
//              cvShowImage("gray_out2", gray_out2);
//              cvShowImage("imgResult", imgResult);
                cv::waitKey(3);
                sensor_msgs::ImagePtr out = sensor_msgs::CvBridge::cvToImgMsg(img,
                                "rgb8");   //image converted from opencv to ros for publishing
                image_pub_.publish(out);
                cvClearMemStorage(storage);
                cvReleaseMemStorage(&storage);
                cvReleaseImage( &out1 );
                cvReleaseImage( &gray_out );
                cvReleaseImage( &canny_out );
                cvReleaseImage( &gray_out1 );
                cvReleaseImage( &canny_out1 );
                cvReleaseImage( &canny_out2 );
                cvReleaseImage( &gray_out2 );



        }

};

int main(int argc, char** argv) {
        ros::init(argc, argv, "image_converter");
        ImageConverter ic;
        ros::spin();
        return 0;
}