def driveStop_car_callback(self, data):
"""laser scan callback function"""
#checks if the image is valid first
while self.camera_data.cv_image is None:
time.sleep(0.5)
print("sleeping")
#applies the current filter to the image and stores the image in imagePush
self.flag_boxR, self.imagePush = cd_color_segmentation(
self.camera_data.cv_image, color="r")
self.flag_boxB, self.imagePushB = cd_color_segmentation(
self.camera_data.cv_image, color="b")
self.flag_boxY, self.imagePushY = cd_color_segmentation(
self.camera_data.cv_image, color="y")
#finds the size of the box
# self.size_calc(self.flag_boxR)
# self.size_calc(self.flag_boxB)
# self.size_calc(self.flag_boxY)
#outputs the image to the IMAGE_TOPIC
try:
self.image_pub.publish(
self.bridge.cv2_to_imgmsg(self.imagePush, "bgr8"))
except CvBridgeError as e:
print("Error bridging Zed image", e)
print(self.flag_boxR, self.flag_boxB, self.flag_boxY)
self.check_stop()
if self.stopped:
self.cmd.drive.steering_angle = 1
self.cmd.drive.speed = 0
return
if self.timer <= 0:
if self.flag_boxY != self.empty_box and self.follow_color != "y":
self.timer = 30
self.follow_color = "y"
elif self.flag_boxB != self.empty_box and self.follow_color != "b" and self.flag_boxY == self.empty_box:
self.timer = 60
self.follow_color = "b"
elif self.flag_boxR != self.empty_box and self.follow_color != "r" and self.flag_boxY == self.empty_box and self.flag_boxB == self.empty_box:
self.timer = 30
self.follow_color = "r"
else:
self.timer -= 1
rospy.loginfo("following: {}".format(self.follow_color))
if self.follow_color == "y":
self.size_calc(self.flag_boxY)
elif self.follow_color == "b":
self.size_calc(self.flag_boxB)
elif self.follow_color == "r":
self.size_calc(self.flag_boxR)
else:
self.size_calc(self.empty_box)
self.drive_on_path()
def driveStop_car_callback(self, data):
"""laser scan callback function"""
# checks if the image is valid first
while self.camera_data.cv_image is None:
time.sleep(0.5)
print("sleeping")
self.i += 1
np.save("pictures/{}.npy".format(self.i), self.camera_data.cv_image)
print("took picture")
# applies the current filter to the image and stores the image in imagePush
self.flag_box, self.imagePush = cd_color_segmentation(
self.camera_data.cv_image)
# finds the size of the box
self.size_calc()
# outputs the image to the IMAGE_TOPIC
try:
self.image_pub.publish(
self.bridge.cv2_to_imgmsg(self.imagePush, "bgr8"))
except CvBridgeError as e:
print("Error bridging Zed image", e)
# if len(self.pictures) % 100 == 0:
# self.save()
# print("saved")
if AUTONOMOUS_MODE:
self.drive()
else:
pass
def greenLightGo(self):
while self.camera_data.cv_image is None:
print('sleeping')
self.cmd.drive.speed = 0
flag_box, self.img, _ = cd_color_segmentation(self.camera_data.cv_image, GREEN_LIGHT, (0, self.camera_data.cv_image.shape[1]), (0, int(1.0/3 * self.camera_data.cv_image.shape[0])))
if (flag_box[1][0] - flag_box[0][0]) * (flag_box[1][1] - flag_box[0][1]) > THRESHOLD_AREA:
print('GREEN YEET')
else:
print('no green sad boiz')
if (flag_box[1][0] - flag_box[0][0]) * (flag_box[1][1] - flag_box[0][1]) > THRESHOLD_AREA:
self.greenFrames.append(True)
else:
self.cmd.drive.speed = 0
self.greenFrames.append(False)
if len(self.greenFrames) >= 5:
green = True
for bool in self.greenFrames:
if not bool:
green = False
return green
return False
def driveStop_car_callback(self, data):
"""laser scan callback function"""
# checks if the image is valid first
while self.camera_data.cv_image is None:
time.sleep(0.5)
print("sleeping")
red_points, self.imagePush = cd_color_segmentation(
self.camera_data.cv_image, self.red_bounds[0], self.red_bounds[1])
# self.yel_vector, red_img = cd_color_segmentation(self.camera_data.cv_image, self.yel_bounds[0], self.yel_bounds[1])
# self.blu_vector, red_img = cd_color_segmentation(self.camera_data.cv_image, self.blu_bounds[0], self.blu_bounds[1])
angle = self.calc(red_points)
speed = 0.5
# applies the current filter to the image and stores the image in imagePush
# self.vector, self.imagePush = cd_color_segmentation(self.camera_data.cv_image)
# finds the size of the box
# outputs the image to the IMAGE_TOPIC
try:
self.image_pub.publish(
self.bridge.cv2_to_imgmsg(self.imagePush, "bgr8"))
except CvBridgeError as e:
print("Error bridging Zed image", e)
# if len(self.pictures) % 100 == 0:
# self.save()
# print("saved")
if AUTONOMOUS_MODE:
self.drive(speed, angle)
else:
pass
def driveStop_car_callback(self, data):
"""laser scan callback function"""
#checks if the image is valid first
while self.camera_data.cv_image is None:
time.sleep(0.5)
print("sleeping")
#applies the current filter to the image and stores the image in imagePush
self.flag_box, self.imagePush = cd_color_segmentation(
self.camera_data.cv_image[240:500][0:25], np.array([0, 60, 147]),
np.array([12, 254, 255]))
self.yellow_box, self.imagePush = cd_color_segmentation(
self.camera_data.cv_image[220:520][0:100], np.array([27, 80, 150]),
np.array([40, 255, 255]))
self.blue_box, self.imagePush = cd_color_segmentation(
self.camera_data.cv_image[220:520][0:40], np.array([105, 90, 130]),
np.array([115, 255, 255]))
#self.white_box, self.imagePush = cd_color_segmentation(self.camera_data.cv_image[200:500][0:40],np.array([0,0,100]),np.array([255,10,255]))
'''
self.grayScale = cv2.cvtColor(self.camera_data.cv_image,cv2.COLOR_BGR2GRAY)
self.cannyEdge = cv2.Canny(self.grayScale,100,200)
lines = cv2.HoughLinesP(self.cannyEdge,1,np.pi/180,100,100,10)
for x1,y1,x2,y2 in lines[0]:
a = np.cos(element[0][1])
b = np.sin(element[0][1])
x0 = a*element[0][0]
y0 = b*element[0][0]
x1 = int(x0 + 1000*(-b))
y1 = int(y0 + 1000*(a))
x2 = int(x0 - 1000*(-b))
y2 = int(y0 - 1000*(a))
cv2.line(self.camera_data.cv_image,(x1,y1),(x2,y2),(0,255,0),2)
'''
#finds the size of the box
self.size_calc(self.flag_box)
#outputs the image to the IMAGE_TOPIC
try:
self.image_pub.publish(
self.bridge.cv2_to_imgmsg(self.imagePush, "bgr8"))
except CvBridgeError as e:
print("Error bridging Zed image", e)
if AUTONOMOUS_MODE:
self.drive()
else:
pass
def driveStop_car_callback(self, data):
"""laser scan callback function"""
#checks if the image is valid first
while self.camera_data.cv_image is None:
time.sleep(0.5)
print("sleeping")
#applies the current filter to the image and stores the image in imagePush
self.flag_box, self.imagePush, self.imageHSV = cd_color_segmentation(
self.camera_data.cv_image, BLUE)
print('blue looked for')
#print(str((self.flag_box[1][0] - self.flag_box[0][0]) * (self.flag_box[1][1] - self.flag_box[0][1])))
if self.flag_box is None:
print('no blue detected')
areaOfBox = (self.flag_box[1][0] - self.flag_box[0][0]) * (
self.flag_box[1][1] - self.flag_box[0][1])
the_boolean = self.flag_box is None or areaOfBox < THRESHOLD_AREA
if self.LOOKING_FOR == 'RED' and the_boolean:
self.flag_box, self.imagePush, self.imageHSV = cd_color_segmentation(
self.camera_data.cv_image, RED)
print('red boi')
## elif areaOfBox > SMALL_THRESHOLD:
## self.LOOKING_FOR = 'BLUE'
## print('big boi, following blue')
else:
## self.cmd.speed = 0
## print('small boi, stopping')
self.LOOKING_FOR = 'BLUE'
## if self.flag_box is None or (self.flag_box[1][0] - self.flag_box[0][0]) * (self.flag_box[1][1] - self.flag_box[0][1]) < THRESHOLD_AREA:
## self.flag_box, self.imagePush, self.imageHSV = cd_color_segmentation(self.camera_data.cv_image, BLUE)
## print('red looked for')
#outputs the image to the IMAGE_TOPIC
try:
self.image_pub.publish(
self.bridge.cv2_to_imgmsg(
cv2.cvtColor(self.imageHSV, cv2.COLOR_HSV2BGR), "bgr8"))
except CvBridgeError as e:
print("Error bridging Zed image", e)
if AUTONOMOUS_MODE:
self.drive()
else:
pass
def driveStop_car_callback(self, data):
#"""laser scan callback function"""
#checks if the image is valid first
(self.redRectangleVertices,
self.greenRectangleVertices) = orb_det("./oneway.txt",
self.camera_data.cv_image)
while self.camera_data.cv_image is None:
time.sleep(0.5)
print("sleeping")
#applies the current filter to the image and returns a bounding box
self.flag_box = cd_color_segmentation("./oneway",
self.camera_data.cv_image)
#print("coordinates1:")
#print(self.flag_box)
#print(self.flag_box)
rectPOne = (self.greenRectangleVertices[1][0],
self.greenRectangleVertices[0][1])
rectPTwo = self.greenRectangleVertices[0]
rectPThree = (self.greenRectangleVertices[0][0],
self.greenRectangleVertices[1][1])
rectPFour = self.greenRectangleVertices[1]
rectPTM = (((self.greenRectangleVertices[0][0] +
self.greenRectangleVertices[1][0]) / 2),
self.greenRectangleVertices[0][1])
rectPBM = (((self.greenRectangleVertices[0][0] +
self.greenRectangleVertices[1][0]) / 2),
self.greenRectangleVertices[1][1])
frame = self.camera_data.cv_image
trueFrame = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
rectOne = trueFrame[rectPTwo[0]:rectPBM[0], rectPTwo[1]:rectPBM[1]]
rectTwo = trueFrame[rectPTM[0]:rectPFour[0], rectPTM[1]:rectPTM[1]]
whiteOne = np.sum(rectOne == 255)
whiteTwo = np.sum(rectTwo == 255)
if whiteOne > whiteTwo: #turn left
self.cmd.drive.steering_angle = -60
else: #turn right
self.cmd.drive.steering_angle = 60
if AUTONOMOUS_MODE:
self.drive()
else:
pass
def driveStop_car_callback(self,data):
"""laser scan callback function"""
#checks if the image is valid first
while self.camera_data.cv_image is None:
time.sleep(0.5)
print("sleeping")
#applies the current filter to the image and returns a bounding box
self.flag_box = cd_color_segmentation(self.camera_data.cv_image)
#finds the size of the box
self.size_calc()
if AUTONOMOUS_MODE:
self.drive()
else:
pass
def lineFollowAngle(self):
while self.camera_data.cv_image is None:
time.sleep(0.5)
print("sleeping")
ANGLE_CONSTANT = 0.1
# applies the current filter to the image and stores the image in imagePush
self.flag_box, self.imagePush, _ = cd_color_segmentation(self.camera_data.cv_image, RED_LINE, (0, self.camera_data.cv_image.shape[1]), (int(1.0/2 * self.camera_data.cv_image.shape[0])), self.camera_data.cv_image.shape[0])
CENTER = self.imagePush.shape[1] / 2
xAvg = (self.flag_box[0][0] + self.flag_box[1][0]) / 2
#print('avg: ' + str(xAvg))
error = xAvg - CENTER
# might not be times -1, but that's what worked for us in the line following challenge
turn_angle = -1*error * ANGLE_CONSTANT * np.pi/180
#print('turn angle: ' + str(turn_angle))
return turn_angle
def autonomous(self):
#maskl=cd_color_segmentation(left)
cent=cd_color_segmentation(self.camera_data.cv_image)
print(cent)
if cent==-1:
if self.lasttime+5>time.time()
self.drive(0,0)
else:
self.lasttime=time.time()
if AUTONOMOUS_MODE:
if cent==-1:
self.drive(.3,0)
else:
angle=self.angle(cent,cent)
self.drive(1,angle)
else:
pass
def autonomous(self):
#maskl=cd_color_segmentation(left)
self.mask = cd_color_segmentation(self.camera_data.cv_image)
#centl=self.findCenter(maskl)
cent = self.findCenter(self.mask)
print(cent)
if AUTONOMOUS_MODE:
if cent == -1:
self.drive(.3, 0)
else:
angle = self.angle(cent, cent)
self.drive(1, angle)
else:
pass
try:
self.image_pub.publish(self.bridge.cv2_to_imgmsg(
self.mask, "8UC1"))
except CvBridgeError as e:
print("Error bridging Zed image", e)
def bricksTurnAngle(self):
while self.camera_data.cv_image is None:
time.sleep(0.5)
print("sleeping")
ANGLE_CONSTANT = 0.1
# applies the current filter to the image and stores the image in imagePush
self.flag_box, self.imagePush, contours = cd_color_segmentation(self.camera_data.cv_image, RED_BRICKS, (0, self.camera_data.cv_image.shape[1]), (int(1.0/2 * self.camera_data.cv_image.shape[0]), self.camera_data.cv_image.shape[0]))
CENTER = self.imagePush.shape[1] / 2
xSum = 0
for contour in contours[int(4.0/5 * len(contours)):]:
x1, y1, x2, y2 = cv2.boundingRect(contour)
xSum += x1 + x1 + x2
xAvg = xSum / int(2.0/5 * len(contours))
error = xAvg - CENTER
# it might have to be -1 * error * ANGLE_CONSTANT * np.pi/180
# even though that makes no logical sense cuz that's what I had for the cone parking thing
return error * ANGLE_CONSTANT * np.pi/180
def antigreenlight(self,img):
box=cd_color_segmentation(img,colorval=[[88, 135, 179],[144, 255, 255]])#[21, 129, 63],[83, 255, 205] [ 17, 175, 23],[ 82, 255, 148]
if box[1][1]<300:
if self.size_calc(box)>1000:
return True
return False
def greenlight(self,img):
box=cd_color_segmentation(img,colorval=[[63, 119, 75],[98, 255, 100]])#[21, 129, 63],[83, 255, 205] [ 17, 175, 23],[ 82, 255, 148]
if box[1][1]<300:
if self.size_calc(box)>200:
return True
return False
import numpy as np
import cv2
from color_segmentation import cd_color_segmentation
picture = np.load('/Users/arnavgupta/Beaverworks/wall_follower/pictures/redyellowblue.npy')
_, pic, cnt = cd_color_segmentation(picture, np.array([100, 50, 100]), np.array([150, 255, 255]))
cv2.imshow("original", picture)
cv2.imshow("htest", pic)
print(cnt)
# print(_, __)
cv2.waitKey()
def driveStop_car_callback(self, data):
"""laser scan callback function"""
# checks if the image is valid first
while self.camera_data.cv_image is None:
time.sleep(0.5)
print("sleeping")
self.flag_box, filtered_pic = cd_color_segmentation(
self.camera_data.cv_image)
cv2.rectangle(filtered_pic, self.flag_box[0], self.flag_box[1],
(0, 255, 0))
self.img_pub.publish(self.bridge.cv2_to_imgmsg(filtered_pic, "bgr8"))
self.l_data = data
my_ranges = self.l_data.ranges[self.lower_ind:self.upper_ind]
if self.map == None:
self.map = my_ranges
print "Map created"
print "starting in 10"
for i in range(10):
time.sleep(1)
print("starting in {}".format(9 - i))
print "Starting"
return
ind = self.check_map(my_ranges)
# print " ", self.map[540 - 180], " ", my_ranges[540 - 180]
if len(ind) == 0:
print("No object on lidar")
return
distance = my_ranges[min(ind)]
angle = min(ind) + self.lower_ind
# applies the current filter to the image and returns a bounding box
# finds the size of the box
((height, width), (cen_x, cen_y)) = self.size_calc()
if height * width < 60:
print("No object on camera")
return
self.input_data.append([height, width, cen_x, cen_y])
self.label_data.append([distance, angle])
assert len(self.input_data) == len(self.label_data), "Strange data"
if len(self.input_data) == 500:
self.save_data()
print("Distance: {} Angle: {}".format(distance, angle))
if AUTONOMOUS_MODE:
self.drive()
else:
pass