0
저는 Morse와 ROS-Hydro 버전을 연구하고 있습니다. 플래그가 설정되면 플래그가 1로 설정되고 (이 플래그는 opencv 부분에 설정 됨) for 루프에서 빠져 나가기를 원하면 입력을 시작하고 다른 기능과 인쇄 '플래그가 설정'되어야합니다. I 내가 for 루프를 깨뜨릴 수있는 프로그래밍에 대한 의구심을 가지십시오.for 루프를 어기는 법은 무엇입니까?
#!/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()
고마워요 - : 다음은 코드입니다! 앞으로 답을 기다리고 있습니다!