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