class BlockingBar_Detector(Scan_image):
def __init__(self):
Scan_image.__init__(self, 'center', 0)
self.drive = Drive_Method()
self.detect = True
self.run = False
def image_callback(self, msg):
if self.run:
Scan_image.image_callback(self, msg)
lower_red = numpy.array([0, 0, 90])
upper_red = numpy.array([5, 5, 110])
img = cv2.inRange(self.image, lower_red, upper_red)
h, w = img.shape
img[0:180, 0:w] = 0
img[240:h, 0:w] = 0
_, contours, _ = cv2.findContours(img, cv2.RETR_TREE,
cv2.CHAIN_APPROX_SIMPLE)
if len(contours) == 0:
self.drive.go_sign()
self.detect = False
else:
self.drive.stop_sign()
self.detect = True
def scan_callback(self, msg):
drive = Drive_Method()
angle_180 = len(msg.ranges) / 2
angle_90 = len(msg.ranges) / 4
angle_45 = len(msg.ranges) / 8
# msg.ranges / 2 = range_ahead
self.range_ahead = msg.ranges[len(msg.ranges) / 2]
self.range_right = max(msg.ranges[len(msg.ranges) / 2:])
print self.range_right
# 정면 물체, 측면 물체까지의 거리 출력
# print "range ahead : %0.2f" % self.range_ahead
# print "range right : %0.2f" % self.range_right
if self.range_ahead > 1.8 or self.range_right > 1.8 or \
((math.isnan(self.range_ahead)) and math.isnan(self.range_right)):
value = False
self.stop_pub.publish(value)
# self.drive_controller.drive_forward(1)
#drive.go_sign()
#print('go')
else:
value = True
self.stop_pub.publish(value)
def clock_callback(self, msg):
if self.run:
self.now_time = msg.clock.secs
if self.detect_time + 3.0 < self.now_time:
drive = Drive_Method()
drive.go_sign()
self.detect = False
else:
drive = Drive_Method()
drive.stop_sign()
def image_callback(self, msg):
Scan_image.image_callback(self, msg)
drive = Drive_Method()
err = self.cx - 45 - (640 / 2)
err = -float(err) / 100
print err
drive.setTurn(err)
drive.straight()
drive.publish()
self.view() # erase at last
def pose_callback(self, msg):
if self.run:
ori = [
msg.pose.pose.orientation.x, msg.pose.pose.orientation.y,
msg.pose.pose.orientation.z, msg.pose.pose.orientation.w
]
euler = euler_from_quaternion(ori)
self.angle = round((euler[2] * 180.0 / math.pi) + 180.0,
2) #use yaw, +-15.0
if self.set_pose:
drive = Drive_Method()
turn_angle = abs(self.target_dir - self.angle)
if self.target_dir - 2.0 < self.angle < self.target_dir + 2.0:
self.set_pose = False
drive.forceStop()
drive.publish()
return # finish set
else:
drive.forceAngle(turn_angle)
drive.publish()
def image_callback(self, msg):
if self.run:
Scan_image.image_callback(self, msg)
drive = Drive_Method()
lower_white = numpy.array([0, 0, 180])
upper_white = numpy.array([255, 30, 255])
self.mask = cv2.inRange(self.hsv, lower_white, upper_white)
h, w = self.mask.shape
self.mask[0:(h / 8) * 6, 0:w] = 0
self.mask[(h / 8) * 7:h, 0:w] = 0
ret, thr = cv2.threshold(self.mask, 0, 255, 0)
_, contours, _ = cv2.findContours(thr, cv2.RETR_TREE,
cv2.CHAIN_APPROX_SIMPLE)
if len(contours) <= 0:
return #not found
for cnt in contours:
area = cv2.contourArea(cnt)
#if 11000.0 < area < 13000.0 and not self.detect: # need to find area's max range
if 10000.0 < area and not self.detect: # need to find area's max range
if (self.position[-1][0] - 4.0 < self.pose.position_x <
self.position[-1][0] + 4.0) and (
self.position[-1][1] - 4.0 <
self.pose.position_x <
self.position[-1][1] + 4.0):
drive.go_sign()
self.detect = False
else:
if self.detect_time + 5.0 < self.now_time:
drive.stop_sign()
self.position.append(
[self.pose.position_x, self.pose.position_y])
self.detect = True
self.detect_time = self.now_time
else:
drive.go_sign()
self.detect = Falsex
def image_callback(self, msg):
Scan_image.image_callback(self, msg)
drive = Drive_Method()
'''
h, w = self.mask.shape
self.mask[0:h * 3 / 5, 0:w] = 0
self.mask[h - (h / 8):h, 0:w] = 0
self.mask[0:h, 0:w / 4] = 0
self.mask[0:h, w - (w / 4):w] = 0
'''
#mask = cv2.Canny(self.mask, 100, 200)
ret, thr = cv2.threshold(self.mask, 0, 255, 0)
_, contours, _ = cv2.findContours(thr, cv2.RETR_TREE,
cv2.CHAIN_APPROX_SIMPLE)
if len(contours) <= 0:
return #not found
'''
x,y,w,h = cv2.boundingRect(cnt)
self.mask = cv2.rectangle(self.mask,(x,y),(x+w,y+h),(0,0,255),2)
cv2.drawContours(self.mask, [cnt], 0, (255, 255, 0), 1)
'''
font = cv2.FONT_HERSHEY_SIMPLEX # normal size sans-serif font
fontScale = 1.0
for cnt in contours:
area = cv2.contourArea(cnt)
epsilon1 = 0.1 * cv2.arcLength(cnt, True)
approx1 = cv2.approxPolyDP(cnt, epsilon1, True)
cv2.drawContours(self.mask, [approx1], 0, (0, 255, 0), 3)
x, y, w, h = cv2.boundingRect(cnt)
if 11000.0 < area < 13000.0: # need to find area's max range
msg = String()
msg.data = "stop"
#print "stop"
drive.stop_sign()
self.stop_pub.publish(msg)
# rospy.sleep(5)
cv2.putText(self.mask, str(area), (x, y), font, fontScale,
(255, 0, 255), 2)
else:
drive.go_sign()
#cv2.imshow("window", self.image)
#cv2.imshow("window", hsv)
#cv2.imshow("window1", self.mask)
cv2.waitKey(3)
def pose_callback(self, msg):
ori = [
msg.pose.pose.orientation.x, msg.pose.pose.orientation.y,
msg.pose.pose.orientation.z, msg.pose.pose.orientation.w
]
euler = euler_from_quaternion(ori)
self.angle = round((euler[2] * 180.0 / math.pi) + 180.0,
2) #use yaw, +-15.0
if self.set_pose:
drive = Drive_Method()
turn_angle = abs(self.target_dir - self.angle)
if self.target_dir - 2.0 < self.angle < self.target_dir + 2.0:
self.set_pose = False
return # finish set
else:
drive.forceAngle(turn_angle)
drive.publish()
'''
self.position_x = round(msg.pose.pose.position.x, 2)
self.position_y = round(msg.pose.pose.position.y, 2)
self.position = [self.position_x, self.position_y]
'''
self.print_pose_status()
def scan_callback(self, msg):
if self.run:
drive = Drive_Method()
angle_180 = len(msg.ranges) / 2
angle_90 = len(msg.ranges) / 4
angle_45 = len(msg.ranges) / 8
# msg.ranges / 2 = range_ahead
self.range_ahead = msg.ranges[len(msg.ranges) / 2]
self.range_right = max(msg.ranges[len(msg.ranges) / 2:])
print self.range_right
# 정면 물체, 측면 물체까지의 거리 출력
# print "range ahead : %0.2f" % self.range_ahead
# print "range right : %0.2f" % self.range_right
if self.range_ahead > 1.8 or self.range_right > 1.8 or \
((math.isnan(self.range_ahead)) and math.isnan(self.range_right)):
drive.go_sign()
print('go')
else:
drive.stop_sign()
print('stop')
def __init__(self):
Scan_image.__init__(self, 'center', 0)
self.drive = Drive_Method()
self.detect = True
self.run = False
def image_callback(self, msg):
Scan_image.image_callback(self, msg)
if self.run:
drive = Drive_Method()
err = 0
if self.lane == 'center':
err = (self.left.cx + self.right.cx - 640) / 2
err = -float(err) / 43
elif self.lane == 'left':
err = self.left.cx - 45 - (640 / 2)
err = -float(err) / 120
elif self.lane == 'right':
err = self.right.cx - 45 - (640 / 2)
err = -float(err) / 120
if abs(err) > 5.0:
drive.setTurn(0)
else:
drive.setTurn(err)
if abs(err) > 0.5:
drive.setSpeed(0.6)
else:
drive.straight()
drive.publish()
if self.lane == 'right' or self.lane == 'left':
return #finish
print "left", self.left.cx
print "right", self.right.cx
# if self.left.cx - 320 < 0 and self.right.cx - 320 > 0 and not self.t_flag:
if self.left.cx - 320 > -120 and self.right.cx - 320 < 150 and not self.t_flag:
self.t_flag = True
drive.stop_sign()
return # finish
def image_callback(self, msg):
Scan_image.image_callback(self, msg)
drive = Drive_Method()
err = 0
if self.lane == 'center':
err = (self.left.cx + self.right.cx - 640) / 2
err = -float(err) / 43
elif self.lane == 'left':
err = self.left.cx - 45 - (640 / 2)
err = -float(err) / 100
elif self.lane == 'right':
err = self.right.cx - 45 - (640 / 2)
err = -float(err) / 100
if abs(err) > 5.0:
drive.setTurn(0)
else:
drive.setTurn(err)
if abs(err) > 0.5:
drive.setSpeed(0.4)
else:
drive.straight()
drive.publish()
self.view() # erase at last
def __init__(self):
Scan_image.__init__(self, 'center', 0)
self.drive = Drive_Method()
from blockingbar_detector import BlockingBar_Detector
from stop_line_detector import Stop_line_detactor
from flover import Flover
from obstacle_detecter import Obstacle_detecter
from clocker import Clocker
from drive import Drive_Method
follower_center = Lane_follower('center')
follower_right = Lane_follower('right')
follower_left = Lane_follower('left')
blocking_bar = BlockingBar_Detector()
stop_line_detector = Stop_line_detactor()
flover = Flover()
obstacle_detecter = Obstacle_detecter()
clocker = Clocker()
drive = Drive_Method()
class BlockingBarState(smach.State):
global blocking_bar, follower_center
def __init__(self):
smach.State.__init__(self, outcomes=['blockingbar', 'normalDriving'])
def execute(self, userdata):
if not blocking_bar.detect:
blocking_bar.run = False
follower_center.run = False
print('BlockingBarState -> normalDrivingState')
return 'normalDriving'
blocking_bar.run = True
follower_center = Lane_follower('center')
follower_right = Lane_follower('right')
follower_left = Lane_follower('left')
blocking_bar = BlockingBar_Detector()
stop_line_detector = Stop_line_detactor()
flover = Flover()
obstacle_detecter = Obstacle_detecter()
clocker = Clocker()
course = 11
time = 0
if __name__ == "__main__":
global course, follower_center, follower_right, follower_left, blocking_bar, stop_line_detector, flover, obstacle_detecter, clocker
drive = Drive_Method()
rospy.init_node('car_test')
while not rospy.is_shutdown():
if course == 1: #blocking bar
follower_center.run = True
follower_right.run = False
follower_left.run = False
blocking_bar.run = True
stop_line_detector.run = False
flover.run = False
obstacle_detecter.run = False
if not blocking_bar.detect:
stop_line_detector.position = [[0.0, 0.0]]
course = 2 #go next course
elif course == 2: #drive linear
follower_center.run = True