Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version

I have tried this and it worked for me.

#!/usr/bin/env python
import roslib
import sys
import time
import math
import rospy
import cv2
import time
#import cv2.cv as cv
import numpy as np
from std_msgs.msg import Float64
from geometry_msgs.msg import PoseStamped
from geometry_msgs.msg import Vector3
from geometry_msgs.msg import Twist
from geometry_msgs.msg import Wrench
from geometry_msgs.msg import Pose
from geometry_msgs.msg import Point
from sensor_msgs.msg import NavSatFix
from sensor_msgs.msg import CameraInfo
from sensor_msgs.msg import Image
from sensor_msgs.msg import RegionOfInterest
from cv_bridge import CvBridge, CvBridgeError
from rospy.numpy_msg import numpy_msg
a=0
b=0
c=0
timer = 0

def bridge_opencv():
        image_pub = rospy.Publisher("quadrotor/videocamera1/camera_info",Image)
            cv2.namedWindow("Image window", 1)
        image_sub = rospy.Subscriber("quadrotor/videocamera1/image",Image, callback)


def callback(data):
        global timer
        global dis
        global my_var1
        global my_var2
        global my_var3
        global a
        global b
        global c
        global flag

        bridge = CvBridge()
        try:
            cv_image = bridge.imgmsg_to_cv2(data, "bgr8")
        except CvBridgeError, e:
            print e
        (rows,cols,channels) = cv_image.shape
        if cols > 60 and rows > 60 :
            cv2.circle(cv_image, (50,50), 10, 255)
        #converting bgr to hsv  
        hsv=cv2.cvtColor(cv_image,cv2.COLOR_BGR2HSV)
        # define range of blue color in HSV
            lower_blue = np.array([60,0,0],dtype=np.uint8)
            upper_blue = np.array([255,255,255],dtype=np.uint8)
        # Threshold the HSV image to get only blue colors
            mask = cv2.inRange(hsv, lower_blue, upper_blue)
        new_mask = mask.copy()
        # Bitwise-AND mask and original image
            res = cv2.bitwise_and(cv_image,cv_image, mask= mask)
        #removing noise 
        kernel = np.ones((12,12),np.uint8)
        new_mask = cv2.morphologyEx(new_mask, cv2.MORPH_CLOSE, kernel)
        new_mask = cv2.morphologyEx(new_mask, cv2.MORPH_OPEN, kernel)
        contours, hierarchy = cv2.findContours(new_mask,cv2.RETR_TREE,cv2.CHAIN_APPROX_SIMPLE)
        cv2.drawContours(cv_image, contours, -1, (0,0,0), 3)
        if(contours):
            cv2.drawContours(cv_image, contours, -1, (0,0,0), 3)
            cnt = contours[0]
            area = cv2.contourArea(cnt)
            #print area 
            if area > 500:
                dis = timer*0.4
                my_var1= a+dis
                my_var2 = b
                my_var3 = c
                flag = 1

        cv2.imshow('mask',mask)
            cv2.imshow('res',res)
        cv2.imshow("Image window", cv_image)
        cv2.waitKey(3)

def stop(x):
        global flag
        now=time.time()
        global timer
        print 'stop'    
        cmd = rospy.Publisher("/quadrotor/rotorcraftvelocity", Twist)   
        while timer !=x:
            motion = Twist()
            motion.linear.x = +0.0
            motion.linear.y = +0.0
            motion.linear.z = +0.0
            cmd.publish(motion) 
            end = time.time()
            timer = round(end-now)

def left():
        global flag
        now = time.time()
        global timer
        print 'left'
        global b
        cmd = rospy.Publisher("/quadrotor/rotorcraftvelocity", Twist)   
        while timer !=10:
            if flag !=1:    
                motion = Twist()        
                motion.linear.y = -0.4
                cmd.publish(motion)     
                end = time.time()
                timer = round(end-now)
            if flag ==1:
                print 'flag', flag
                print('i was here')
                break
        if flag !=1:
            b = b-4 

def right():
        global flag
        now=time.time()
        global timer
        print 'right'
        global b
        cmd = rospy.Publisher("/quadrotor/rotorcraftvelocity", Twist)
        while timer !=5:
            if flag !=1:                
                motion = Twist()
                motion.linear.y = +0.4
                cmd.publish(motion) 
                end = time.time()
                timer = round(end-now)
            if flag == 1:
                print 'flag', flag
                print('i was here')
                break
        if flag !=1:
            b = b+2
def straight(x):
        global flag
        now = time.time()
        global timer
        print 'straight'
        global a
        cmd = rospy.Publisher("/quadrotor/rotorcraftvelocity", Twist)
        while timer !=x:    
            if flag != 1:
                motion = Twist()
                motion.linear.x = -0.4
                cmd.publish(motion)         
                end = time.time()
                timer = round(end-now)
            if flag ==1:
                print 'flag',flag
                print('i was here')
                break
        if flag !=1:
            a = a+16

def back(x):
        global flag
        now = time.time()
        global timer
        print 'back'
        global a 
        cmd = rospy.Publisher("/quadrotor/rotorcraftvelocity", Twist)
        while timer !=x:
            if flag !=1:
                motion = Twist()
                motion.linear.x = +0.4
                cmd.publish(motion)         
                end = time.time()
                timer = round(end-now)
            if flag ==1:
                print 'flag', flag
                print('i was here')
                break
        if flag !=1:
            a = a-16

def up():
        global flag
        now=time.time()
        global timer
        print 'up'
        global c
        cmd = rospy.Publisher("/quadrotor/rotorcraftvelocity", Twist)
        while timer !=10:
            if flag !=1:    
                motion = Twist()
                motion.linear.z = +0.4
                cmd.publish(motion)         
                end = time.time()
                timer = round(end-now)
            if flag ==1:
                print 'flag', flag
                print('i was here')
                break
        if flag !=1:
            c=c+4
def shutdown():
        rospy.loginfo("Shutting down head tracking node...")   
def print_abc():
    global my_var1
    global my_var2
    global my_var3
    print ('a',my_var1)
    print('b',my_var2)
    print('c',my_var3)
def pilot():
    global my_var1
    global my_var2
    global my_var3
    rospy.init_node("pilot")    
    puba = rospy.Publisher("box_positiona", Float64)
    pubb = rospy.Publisher("box_positionb", Float64)
    pubc = rospy.Publisher("box_positionc", Float64)
    print "my inital position"
    global flag
    flag = 0
    bridge_opencv()
    if flag != 1:   
        print('i am finding..')
        up()
        stop(x=5)   
        left()
        for i in range(1,4):
            if flag != 1:       
                stop(x=20)
                straight(x=40)
                stop(x=20)
                right()
                stop(x=20)
                back(x=40)
                stop(x=20)
                right() 
            else:
                break
    print_abc()
    stop(20)    
    coorda = Float64()
    coordb = Float64()
    coordc = Float64()
    coorda.data = my_var1
    coordb.data = my_var2
    coordc.data = my_var3
    while not rospy.is_shutdown():
        if flag == 1:
            puba.publish(coorda)
            pubb.publish(coordb)
            pubc.publish(coordc)            
    #rospy.spin() # this will block untill you hit Ctrl+C
if __name__ == '__main__':
    try:
        pilot()
    except rospy.ROSInterruptException:
            rospy.loginfo("Head tracking node is shut down.")