Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version

How to break and come of the for loop ?

Hello guys ! I am working on Morse and ROS- Hydro version. I want to break and come out of the for loop when my flag is set to 1 ( and this flag is set in opencv part) once the flag is set, it should start enter and another function and print 'flag is set' The following is the code:-

#!/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
flag =0
#def permanent_stop():
#   my_value = 1
#   stop(x=5)
#   return 1
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
        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 > 6000:
                print('i found the object')
                dis = timer*0.4
                #print 'dis',dis
                my_var1= a+dis
                my_var2 = b
                my_var3 = c
                flag = 1
                coorda = Float64()
                coordb = Float64()
                coordc = Float64()
                #my_value2 = False
        cv2.imshow('mask',mask)
            cv2.imshow('res',res)
        cv2.imshow("Image window", cv_image)
        cv2.waitKey(3)

def stop(x):
        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():
        now = time.time()
        global timer
        print 'left'
        global b
        cmd = rospy.Publisher("/quadrotor/rotorcraftvelocity", Twist)   
        while timer !=10:   
            motion = Twist()        
            motion.linear.y = -0.4
            cmd.publish(motion)     
            end = time.time()
            timer = round(end-now)
        b = b-4 

def right():
        now=time.time()
        global timer
        print 'right'
        global b
        cmd = rospy.Publisher("/quadrotor/rotorcraftvelocity", Twist)
        while timer !=5:                
            motion = Twist()
            motion.linear.y = +0.4
            cmd.publish(motion) 
            end = time.time()
            timer = round(end-now)
        b = b+2


def straight(x):
        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)
                print flag
            elif flag ==1:
                print 'flag',flag
                print('i was here')
                break
        a = a+16

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

def up():
        now=time.time()
        global timer
        print 'up'
        global c
        cmd = rospy.Publisher("/quadrotor/rotorcraftvelocity", Twist)
        while timer !=10:   
            motion = Twist()
            motion.linear.z = +0.4
            cmd.publish(motion)         
            end = time.time()
            timer = round(end-now)
        c=c+4
def pilot():
    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"
    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)
            else: 
                break
            if flag !=1:
                straight(x=40)
            else: 
                break
            if flag !=1:
                stop(x=20)
            else:
                break   
            if flag !=1:
                right()
            else: 
                break
            if flag !=1:
                stop(x=20)
            else: 
                break
            if flag !=1:
                back(x=40)
            else: 
                break
            if flag !=1:        
                stop(x=20)
            else: 
                break
            if flag !=1:
                right()
            else: 
                break           
        print over 
        print my_value
    if flag ==1:
            print("i am true")
            stop(x=50)

    print('hi') 
    #rospy.spin() # this will block untill you hit Ctrl+C
if __name__ == '__main__':

        pilot()

Looking forward for answers ! Thanks !

How to break and come of the for loop ?

Hello guys ! I am working on Morse and ROS- Hydro version. I want to break and come out of the for loop when my flag is set to 1 ( and this flag is set in opencv part) once the flag is set, it should start enter and an another function and print 'flag is set' The following is the code:-

#!/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
flag =0
#def permanent_stop():
#   my_value = 1
#   stop(x=5)
#   return 1
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
        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 > 6000:
                print('i found the object')
                dis = timer*0.4
                #print 'dis',dis
                my_var1= a+dis
                my_var2 = b
                my_var3 = c
                flag = 1
                coorda = Float64()
                coordb = Float64()
                coordc = Float64()
                #my_value2 = False
        cv2.imshow('mask',mask)
            cv2.imshow('res',res)
        cv2.imshow("Image window", cv_image)
        cv2.waitKey(3)

def stop(x):
        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():
        now = time.time()
        global timer
        print 'left'
        global b
        cmd = rospy.Publisher("/quadrotor/rotorcraftvelocity", Twist)   
        while timer !=10:   
            motion = Twist()        
            motion.linear.y = -0.4
            cmd.publish(motion)     
            end = time.time()
            timer = round(end-now)
        b = b-4 

def right():
        now=time.time()
        global timer
        print 'right'
        global b
        cmd = rospy.Publisher("/quadrotor/rotorcraftvelocity", Twist)
        while timer !=5:                
            motion = Twist()
            motion.linear.y = +0.4
            cmd.publish(motion) 
            end = time.time()
            timer = round(end-now)
        b = b+2


def straight(x):
        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)
                print flag
            elif flag ==1:
                print 'flag',flag
                print('i was here')
                break
        a = a+16

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

def up():
        now=time.time()
        global timer
        print 'up'
        global c
        cmd = rospy.Publisher("/quadrotor/rotorcraftvelocity", Twist)
        while timer !=10:   
            motion = Twist()
            motion.linear.z = +0.4
            cmd.publish(motion)         
            end = time.time()
            timer = round(end-now)
        c=c+4
def pilot():
    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"
    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)
            else: 
                break
            if flag !=1:
                straight(x=40)
            else: 
                break
            if flag !=1:
                stop(x=20)
            else:
                break   
            if flag !=1:
                right()
            else: 
                break
            if flag !=1:
                stop(x=20)
            else: 
                break
            if flag !=1:
                back(x=40)
            else: 
                break
            if flag !=1:        
                stop(x=20)
            else: 
                break
            if flag !=1:
                right()
            else: 
                break           
        print over 
        print my_value
    if flag ==1:
            print("i am true")
            stop(x=50)

    print('hi') 
    #rospy.spin() # this will block untill you hit Ctrl+C
if __name__ == '__main__':

        pilot()

Looking forward for answers ! Thanks !