def detectCargos(data, side): frame = CvBridge().imgmsg_to_cv2(data, 'bgr8') #[90:150,120:200] hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV) out = frame.copy() auxFrame = frame.copy() out_mask = cv2.cvtColor(np.zeros(out.shape, np.uint8), cv2.COLOR_BGR2GRAY) linePoints = getDevideLine() kernal = np.ones((5,5),np.uint8) for key in ranges: # for each masks if len(ranges[key]) == 4: mask = cv2.morphologyEx(cv2.inRange(hsv, ranges[key][0], ranges[key][1]) + cv2.inRange(hsv, ranges[key][2], ranges[key][3]), cv2.MORPH_OPEN, kernal) else: mask = cv2.morphologyEx(cv2.inRange(hsv, ranges[key][0], ranges[key][1]), cv2.MORPH_OPEN, kernal) contours, hierarchy = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)[-2:] contours = [i for i in contours if cv2.contourArea(i) > OBJ_AREA] #print(str(len(contours)) + key) curContours = [] for cnt in contours: [x, y, w, h] = cv2.boundingRect(cnt) if side == 'left': if x + w//2 < linePoints[0][0]: curContours.append(cnt) else: if x + w//2 > linePoints[0][0]: curContours.append(cnt) contours = curContours #print(str(len(contours)) + key) # cargos_colors_quantity[key] += len(contours) for cnt in contours: #draw contours cv2.drawContours(out, [cnt], -1, (255, 255, 255), 2) [x, y, w, h] = cv2.boundingRect(cnt) cv2.putText(out, key, (x - 15, y + h + 10), cv2.FONT_HERSHEY_COMPLEX_SMALL, 0.7, (255,255,255), 1) cv2.putText(out_mask, key, (x - 15, y + h + 10), cv2.FONT_HERSHEY_COMPLEX_SMALL, 0.7, (255,255,255), 1) out_mask = cv2.bitwise_or(out_mask, mask) auxFrame = cv2.line(auxFrame, linePoints[0],linePoints[1], (0,0,0), 2) if side == 'left': dx = -20 else: dx = 20 for k in range(1, 3): auxFrame = cv2.arrowedLine(auxFrame , (linePoints[0][0], 80*k), (linePoints[0][0] + dx, 80*k), (0,0,0), 2) out = cv2.bitwise_and(out, out, mask=out_mask) vis = np.concatenate((out, auxFrame), axis=1) debug.publish(CvBridge().cv2_to_imgmsg(vis, 'bgr8'))
def CameraDepth_callback(self, data): try: cv_image = CvBridge().imgmsg_to_cv2(data, "16UC1") self.depth_image = cv_image.copy() except CvBridgeError as e: print(e) print('Receive depth!')
def callback(image_msg): frame = CvBridge().imgmsg_to_cv2(image_msg, desired_encoding="passthrough") frame = cv2.cvtColor(frame, cv2.COLOR_YCrCb2RGB) frame = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV) print(frame.shape) l1 = cv2.getTrackbarPos ("l1", "Colorbars") h1 = cv2.getTrackbarPos ("h1", "Colorbars") l2 = cv2.getTrackbarPos ("l2", "Colorbars") h2 = cv2.getTrackbarPos ("h2", "Colorbars") l3 = cv2.getTrackbarPos ("l3", "Colorbars") h3 = cv2.getTrackbarPos ("h3", "Colorbars") low_th = (l1, l2, l3) high_th = (h1, h2, h3) print("@") inrange_filter.set_ths (low_th, high_th) mask = inrange_filter.apply (frame) print("&") enlighted = frame.copy () enlighted [:, :, 0] = np.array (np.add (enlighted [:, :, 0], np.multiply (mask, 0.5)), dtype = np.uint8) #cv2.imshow ("enlighted", enlighted) #cv2.imshow ("mask", mask)cv2.waitKey(2) result = np.concatenate ((enlighted, to_three (mask)), axis = 1) cv2.waitKey(2) #cv2.imshow ("result", result) #os.system ('clear') print (low_th, high_th)
class ImageOverlay: def __init__(self): self.velocity = TextOverlay("Velocity", (50, 50)) self.steering = TextOverlay("Steering", (50, 100)) self.img = np.ones( (700, 1000, 3), dtype=np.uint8) * 128 # default black background print("helllo world\n\n\n\n\n\n\n\n\n") # update the image to the new image from camera feed def image_callback(self, data): self.img = CvBridge().compressed_imgmsg_to_cv2(data) print("hi\n\n\n\n\n\n\n\n\n\n\n\n") # configure and overlay stats on the image def stats_callback(self, data): new_vel = data.drive.speed new_steer = data.drive.steering_angle self.velocity.update_text(new_vel) self.steering.update_text(new_steer) def overlay(self): img_out = self.img.copy() self.velocity.text_overlay(img_out) self.steering.text_overlay(img_out) #cv2.imshow('image', self.img) #cv2.waitKey(0) #cv2.destroyAllWindows() return img_out def run(self): rospy.init_node("overlay_image") self.publisher = rospy.Publisher('overlay/compressed', CompressedImage, queue_size=1) # Read in camera feed, change first arg to read a different camera self.camera_subscriber = rospy.Subscriber( '/zed/left/image_raw/compressed', CompressedImage, self.image_callback, queue_size=1) # Read in stats, change this command to read in a different source of stats to overlay self.stats_subscriber = rospy.Subscriber( '/aamfsd_robot_control/command', AckermannDriveStamped, self.stats_callback, queue_size=1) rate = rospy.Rate(3) # default 3 Hz print("Running camera overlay in overlay/compressed") while not rospy.is_shutdown(): img = self.overlay() compressed_img = CvBridge().cv2_to_compressed_imgmsg(img) self.publisher.publish(compressed_img) rate.sleep()
def callback(self, ros_msg): start_time = time.time() # Invoke the image from camera ori_image = CvBridge().imgmsg_to_cv2(ros_msg) use_image = ori_image.copy() use_image = use_image[(use_image.shape[0] - 300) / 2:(use_image.shape[0] + 300) / 2, (use_image.shape[1] - 300) / 2:(use_image.shape[1] + 300) / 2] # Remove black points ave_inten = use_image.mean().copy() for i in range(use_image.shape[0]): for j in range(use_image.shape[1]): if use_image[i, j] < 10: use_image[i, j] = ave_inten cv_image = use_image / 1000.0 cv_image = np.clip(cv_image, 0.2, 1) cv_image = np.uint8(cv_image * 255) crop_h = int((ori_image.shape[0] - 300) / 2) crop_w = int((ori_image.shape[1] - 300) / 2) offsets = np.array([crop_w, crop_h]) # Process the image depth_img = processing_depth(cv_image) #depth_img = processing_depth(depth_read) # Predict the grasp q_img, cos_img, sin_img, width_img = predict(depth_img) cvt_img = np.uint8(q_img * 255) cvt_img = cv2.applyColorMap(cvt_img, cv2.COLORMAP_JET) # Post process the output dis_img, grasp_pts, center = process_output(cvt_img, q_img, cos_img, sin_img, width_img, grip_length=30) # Publish the grasp points center = center + offsets self.msg.depth = ori_image[290, 300] self.msg.center = list(center) self.msg.pt1 = list(grasp_pts[0] + offsets) self.msg.pt2 = list(grasp_pts[1] + offsets) self.msg.pt3 = list(grasp_pts[2] + offsets) self.msg.pt4 = list(grasp_pts[3] + offsets) try: self.grasp_pub.publish(self.msg) except KeyboardInterrupt: print("Publish error!") end_time = time.time() execute_time = end_time - start_time print("Excute time: ", int(execute_time * 1000), "ms") # Display the output cv2.imshow("Image depth", cv_image) #cv2.imshow("Image depth", depth_img) cv2.waitKey(3) cv2.imshow("Image quality", dis_img) cv2.waitKey(3)
def ImgCcallback(self, ros_img): global imgi imgi = mx.img.imdecode(ros_img.data) global img_rect global img_raw global Frame img_rect = CvBridge().compressed_imgmsg_to_cv2(ros_img) # img_raw = img_rect.copy() Frame = pv.Image(img_rect.copy())
def on_image_received(self, image): rgb_image = CvBridge().imgmsg_to_cv2(image.rgb, "bgr8") scharred_image = CvBridge().imgmsg_to_cv2(image.scharred, "mono8") bboxes = self.find_location_of_plate(rgb_image) paint_image = rgb_image.copy() if (len(bboxes) == 0): cycle_completed_msg = Bool() cycle_completed_msg.data = True self.cycle_complete_publisher.publish(cycle_completed_msg) else: plates_msg = Plates() for bbox in bboxes: plate_msg = Plate() plate_msg.top_left.x = bbox[0] plate_msg.top_left.y = bbox[1] plate_msg.down_right.x = bbox[2] plate_msg.down_right.y = bbox[3] plates_msg.plates.append(plate_msg) cv2.rectangle(paint_image, (bbox[0], bbox[1]), (bbox[2], bbox[3]), (0, 255, 0), 2) self.plates_publisher.publish(plates_msg) self.show_image_publisher.publish( self.bridge.cv2_to_imgmsg(paint_image, "bgr8"))
class view(object): def __init__(self): self.image_sub = rospy.Subscriber("/image", Image, self.imageCallback, queue_size=10) self.cluster_sub = rospy.Subscriber("/object_info", ObjectInfoArray, self.clusterCallback, queue_size=10) self.image_pub = rospy.Publisher("/object_image", Image, queue_size=10) self.flag = False self.list = ['person', 'car'] def imageCallback(self, image_msg): try: self.cv_image = CvBridge().imgmsg_to_cv2(image_msg, "bgr8") self.frame_id = image_msg.header.frame_id self.flag = True except CvBridgeerror as e: print (e) def clusterCallback(self, cluster_msg): if self.flag: print("ALL GREEN") image = self.cv_image.copy() for i in range(len(cluster_msg.object_array)): bbox = cluster_msg.object_array[i] if bbox.Class in self.list: print("class:%s score:%5.2f xmin:%d ymin:%d xmax:%d ymax:%d" % (bbox.Class, bbox.probability, bbox.xmin, bbox.ymin, bbox.xmax, bbox.ymax)) if bbox.Class == self.list[0]: cv2.rectangle(image, (bbox.xmin, bbox.ymin), (bbox.xmax, bbox.ymax), (0, 200, 0), 5) elif bbox.Class == self.list[1]: cv2.rectangle(image, (bbox.xmin, bbox.ymin), (bbox.xmax, bbox.ymax), (0, 0, 200), 5) pub_image = CvBridge().cv2_to_imgmsg(image, "bgr8") self.image_pub.publish(pub_image) def main(self): rospy.init_node("image_view") rospy.spin()
class image_converter: def __init__(self,name): self._camcon = baxter_interface.CameraController(name+'_hand_camera') self._resolution = (480,300) #可选的分辨率 1280x800,960x600,640x400,480x300,384x240 self._image_sub = rospy.Subscriber('/cameras/'+name+'_hand_camera/image', Image, self._image_callback) self._original_image = None #从相机获取的原始图像(opencv格式) self._MAX_SCALE = 100 # 找不到棋子就返回更大的值 self._pick_bias = (0.025,0.023) # the gripper is not at the center of the camera # 颜色在HSV空间中的上下阈值 蓝色:[90,130,30],[130,200,150]v 148 128 self._color_dict = {'blue':[[70,80,30],[180,255,255]],'purple':[[130,50,0],[255,200,255]]} #125 self.image_updated = False def open_cam(self,on = True): #打开相机 if on: #TODO 考虑把头上的关掉,可能有占用的问题 Baxter只允许同时开两个 self._camcon.resolution = self._resolution self._camcon.open() else: self._camcon.close() #转换为opencv图像并更新 def _image_callback(self, img_data): try: self._original_image = CvBridge().imgmsg_to_cv2(img_data, "bgr8") #从ros image转换到openCV的图像格式 self.image_updated = True except CvBridgeError as e: print e # 图像处理 # 对图像的初步处理步骤如下: # 转换到HSV图像空间(HSV空间更容易分辨颜色)--> # 提取图像中的蓝色部分--> # 腐蚀与膨胀去除噪点-->转换为灰度图像-->二值化 def _image_process(self, color='blue',best_is_nearest = True): #or biggest self.image_updated = False # wait for get new image while not self.image_updated: pass original = self._original_image.copy() # image process to get the position of chess hsv = cv2.cvtColor(original, cv2.COLOR_BGR2HSV) #转换到HSV颜色空间 lower_color = np.array(self._color_dict[color][0]) upper_color = np.array(self._color_dict[color][1]) mask = cv2.inRange(hsv, lower_color, upper_color) # get color roi kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (10, 10)) open_img = cv2.morphologyEx(mask, cv2.MORPH_OPEN, kernel) close_img = cv2.morphologyEx(open_img, cv2.MORPH_CLOSE, kernel) res = cv2.bitwise_and(original, original, mask= close_img) #open_img gray_img = cv2.cvtColor(res, cv2.COLOR_BGR2GRAY) ret, bin_img = cv2.threshold(gray_img, 10, 255, cv2.THRESH_BINARY) contours, hierarchy = cv2.findContours(bin_img, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) if len(contours)<1: # 没有找到物体 return [self._MAX_SCALE,self._MAX_SCALE,200] [bx,by,bw,bh,ba]=[0]*5 brect = None best = 1000 scale = 0.00064 # per pixel #TODO by resolution and height # cv2.imshow("Img",bin_img) # cv2.waitKey(0) biasx = self._resolution[0]/2+self._pick_bias[1]/scale biasy = self._resolution[1]/2-self._pick_bias[0]/scale for c in contours: # find bounding box coordinates # x, y, w, h = cv2.boundingRect(c) rect = cv2.minAreaRect(c) x, y = rect[0] # 中心坐标 w, h = rect[1] # 长宽,总有 width>=height angle = rect[2] # 角度:[-90,0) if best_is_nearest: now = abs(x-biasx)+abs(y-biasy) else: now = 1000-(w+h) if w*h <500: now = best box = cv2.cv.BoxPoints(rect) box =np.int0(box)#int32 / int64 cv2.drawContours(original, [box], 0, (0, 255, 0), 1) # 画出该矩形 #print (now) if now<best: [bx,by,bw,bh,ba,best]= [x,y,w,h,angle,now] brect = rect best = now box = cv2.cv.BoxPoints(brect) box =np.int0(box)#int32 / int64 cv2.drawContours(original, [box], 0, (0, 0, 255), 2) # 画出该矩形 # cv2.rectangle(original, (bx, by), (bx + bw, by + bh), (0, 255, 0), 2) cv2.rectangle(original, (np.int0(biasx), np.int0(biasy)), (np.int0(biasx) + 1, np.int0(biasy) + 1), (255, 0, 0), 2) print bx,by,bw,bh,ba cv2.imshow("Searched", original) cv2.waitKey(3) #可以减少显示时间提高速度 return [(bx-biasx)*scale,(by-biasy)*scale,abs(ba)]
def detect_char(self,req): charObjList = [] charName = [] charNum = [] angle = [] res = detectobjectionSrvResponse.NOT_DETECTED self.charname.clear() self.charnum.clear() self.poseList.clear() self.angleList.clear() imageData = rospy.wait_for_message('/image_converter/output_video', Image, timeout=5) try: cv_image = CvBridge().imgmsg_to_cv2(imageData,"bgr8") except CvBridgeError as e: print(e) global args cv2.imwrite(args.output_path+"cv_image.jpg",cv_image) if req.objectType is detectobjectionSrvRequest.ALL_OBJECT: global img, area, point1, point2 img = cv_image.copy() img2 = cv_image.copy() im = cv_image[:, :, ::-1] if area == 0: cv2.namedWindow('image') cv2.setMouseCallback('image', self.on_mouse) while True: if(area == 1): break cv2.imshow('image', im) cv2.waitKey(30) cv2.destroyAllWindows() cv2.waitKey(15) area = 1 h, w, _ = im.shape start_time = time.time() im_resized, (ratio_h, ratio_w) = self.resize_image(im) h, w, _ = im.shape timer = {'net': 0, 'restore': 0, 'nms': 0} start = time.time() score, geometry = self.sess.run([self.f_score, self.f_geometry], feed_dict={self.input_images: [im_resized]}) timer['net'] = time.time() - start boxes, timer = self.detect(score_map=score, geo_map=geometry, timer=timer) if boxes is not None: boxes = boxes[:, :8].reshape((-1, 4, 2)) boxes[:, :, 0] /= ratio_w boxes[:, :, 1] /= ratio_h duration = time.time() - start_time print('[timing] {}'.format(duration)) cv2.rectangle(img2, point1, point2, (255,0,0), 5) cv2.imwrite(args.output_path+"img2.jpg",img2) boxnum = 0 boxdic = {} if boxes is not None: i = 0 for box in boxes: if i > 9: break # to avoid submitting errors box = self.sort_poly(box.astype(np.int32)) if np.linalg.norm(box[0] - box[1]) < 5 or np.linalg.norm(box[3]-box[0]) < 5: continue ##zai gou xuan de qu yu if() x0 = box[0,0] >= point1[0] and box[0,0] <= point2[0] y0 = box[0,1] >= point1[1] and box[0,1] <= point2[1] x1 = box[1,0] >= point1[0] and box[1,0] <= point2[0] y1 = box[1,1] >= point1[1] and box[1,1] <= point2[1] x2 = box[2,0] >= point1[0] and box[2,0] <= point2[0] y2 = box[2,1] >= point1[1] and box[2,1] <= point2[1] x3 = box[3,0] >= point1[0] and box[3,0] <= point2[0] y3 = box[3,1] >= point1[1] and box[3,1] <= point2[1] if x0 and y0 and x1 and y1 and x2 and y2 and x3 and y3: print("box", box[0, 0], box[0, 1], box[1, 0], box[1, 1], box[2, 0], box[2, 1], box[3, 0], box[3, 1]) img2 = cv_image.copy() charPose = Pose() charPose.position.x = int(box[3,0]+(box[1,0]-box[3,0])/2) charPose.position.y = int(box[0,1]+(box[2,1]-box[0,1])/2) double a1, a2, a3, a4, a5, a6; double XB, YB; double XW, YW, ZW; a1 = Pixel2ChessMatrix.at<double>(0, 0) - Pixel2ChessMatrix.at<double>(2, 0) * charPose.position.x; a2 = Pixel2ChessMatrix.at<double>(0, 1) - Pixel2ChessMatrix.at<double>(2, 1) * charPose.position.x; a3 = Pixel2ChessMatrix.at<double>(0, 2) - Pixel2ChessMatrix.at<double>(2, 2) * charPose.position.x - Pixel2ChessMatrix.at<double>(2, 3) * charPose.position.x + Pixel2ChessMatrix.at<double>(0, 3); a4 = Pixel2ChessMatrix.at<double>(1, 0) - Pixel2ChessMatrix.at<double>(2, 0) * charPose.position.y; a5 = Pixel2ChessMatrix.at<double>(1, 1) - Pixel2ChessMatrix.at<double>(2, 1) * charPose.position.y; a6 = Pixel2ChessMatrix.at<double>(1, 2) - Pixel2ChessMatrix.at<double>(2, 2) * charPose.position.y - Pixel2ChessMatrix.at<double>(2, 3) * charPose.position.y + Pixel2ChessMatrix.at<double>(1, 3); YB = (a3 * a4 - a1 * a6) / (a1 * a5 - a2 * a4); XB = -1 * (a2 * YB + a3) / a1; XW = Chess2BaseMatrix.at<double>(0, 0) * XB + Chess2BaseMatrix.at<double>(0, 1) * YB + Chess2BaseMatrix.at<double>(0, 3); YW = Chess2BaseMatrix.at<double>(1, 0) * XB + Chess2BaseMatrix.at<double>(1, 1) * YB + Chess2BaseMatrix.at<double>(1, 3); ZW = Chess2BaseMatrix.at<double>(2, 0) * XB + Chess2BaseMatrix.at<double>(2, 1) * YB + Chess2BaseMatrix.at<double>(2, 3); charPose.position.x = XW + 29 charPose.position.y = XY + 16 self.poseList.append(charPose) cv2.circle(img2, tuple(box[0]), 3, (0,255,0), 2) cv2.circle(img2, tuple(box[1]), 3, (255,255,0), 2) cv2.circle(img2, tuple(box[2]), 3, (255,255,255), 2) cv2.circle(img2, (charPose.position.x,charPose.position.y), 3, (0,255,0), 2) cv2.imwrite(args.output_path+"img3.jpg",img2) ################### boxdic['box%d'%boxnum] = [[box[0, 0], box[0, 1]],box[1, 0], box[1, 1],[box[2, 0], box[2, 1]],[box[3, 0], box[3, 1]]] #################### image_happy = cv_image.copy() self.rotate(image_happy,box[1],box[2],box[3],box[0]) cv2.polylines(im[:, :, ::-1].copy(), [box.astype(np.int32).reshape((-1, 1, 2))], True, color=(255, 255, 0), thickness=1) i = i + 1 cv2.imwrite(args.output_path+"detection.jpg",im[:, :, ::-1]) charObjList = self.poseList charName = self.charname charNum = self.charnum angle = self.angleList print(charName) print('Is ok?Y/N') w = input() return detectobjectionSrvResponse(res,charObjList,charName,charNum,angle)
def callback(self, data): retstat = 0 pos = 99 frame = CvBridge().imgmsg_to_cv2(data, 'bgr8') # Our operations on the frame come here gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) pose = self.tracker.locate_marker(gray) #print(pose.quality) markerpose_msg = markerpose() markerpose_msg.header = data.header markerpose_msg.order = pose.order markerpose_msg.x = pose.x markerpose_msg.y = pose.y markerpose_msg.theta = pose.theta markerpose_msg.quality = pose.quality pos = self.determinePos(pose.x, pose.y) markerpose_msg.x = pos[0] markerpose_msg.y = pos[1] if markerpose_msg.quality > 0.5: cv2.circle(frame, (pose.x, pose.y), 20, (0, 0, 255), 2) orientation = min( abs( math.atan2(-(pose.x - self.image_width / 2), pose.y - self.image_height / 2)), abs((math.pi - math.atan2(-(pose.x - self.image_width / 2), pose.y - self.image_height / 2)))) #print math.degrees(orientation) #print orientation self.markerpose_pub.publish(markerpose_msg) #self.markerstatus_pub.publish(1) if (orientation > 0.1): retstat = 1 else: retstat = 2 #elif markerpose_msg.quality > 0.2: # self.markerstatus_pub.publish() else: retstat = 0 #self.markerstatus_pub.publish(0) #print "pose x and y", pose.x, pose.y+ #self.markerpose_pub.publish(markerpose_msg) #self.image_pub.publish(CvBridge().cv2_to_imgmsg(frame,'bgr8')) ##custom_dictionary = cv2.aruco.custom_dictionary(6, 4) #cv2.aruco.detectMarkers(frame) #markers = cv2.aruco.detectMarkers(gray, cv2.getPrede,parameters=parameters) corners, ids, rejectedImgPoints = cv2.aruco.detectMarkers( gray, self.aruco_dict, parameters=self.parameters) #print corners, ids if any(x != None for x in ids): aruco = markerpose() aruco.header = data.header aruco.order = 0 aruco.theta = 0 aruco.quality = 1 #print(corners[0][0][0][0]) X = (corners[0][0][0][0] + corners[0][0][1][0] + corners[0][0][2][0] + corners[0][0][3][0]) / 4 Y = (corners[0][0][0][1] + corners[0][0][1][1] + corners[0][0][2][1] + corners[0][0][3][1]) / 4 arucopos = self.determinePos(X, Y) aruco.x = arucopos[0] aruco.y = arucopos[1] self.arucopose_pub.publish(aruco) cv2.circle(frame, (int(X), int(Y)), 20, (0, 0, 255), 2) #self.determineHeight(corners[0]); frame_markers = cv2.aruco.drawDetectedMarkers(frame.copy(), corners, ids) self.image_pub.publish(CvBridge().cv2_to_imgmsg(frame_markers, 'bgr8')) self.markerstatus_pub.publish(retstat)
class ImageOverlay(): def __init__(self): self.scale_text = 0 self.heading = 0 self.img = np.ones((700,1000,3),dtype=np.uint8)*128 # default black background current_dir = os.path.dirname(os.path.abspath(__file__)) compass_filename = '%s/compass.png' % current_dir scale_filename = '%s/scale.png' % current_dir # Load scale image self.scale_img = load_image(scale_filename) # Load compass image self.compass_img = load_image(compass_filename) def image_callback(self, data): self.img = CvBridge().compressed_imgmsg_to_cv2(data) def heading_callback(self, data): self.heading = data.data def scale_callback(self, data): self.scale_text = data.data def overlay_telemetry(self): im_out = np.uint8(self.img.copy()) min_image_dimension = np.min([self.img.shape[1],self.img.shape[0]]) compass_img = self.compass_img compass_img = rotate_image(compass_img, self.heading) compass_img = warp_image(compass_img) # Resize compass image width_percentage = 0.6 width_px = width_percentage*min_image_dimension compass_img = resize_image(compass_img, width_px) # Resize scale image scale_img = resize_image(self.scale_img, width_px) # Overlay compass image x = im_out.shape[1] / 2 - compass_img.shape[1] / 2 # center width position y = im_out.shape[0] - int(compass_img.shape[0]) - 1 # near bottom im_out = overlay_image(compass_img, im_out, x, y) # Overlay scale image x = im_out.shape[1] / 2 - scale_img.shape[1] / 2 # center width position y = im_out.shape[0] / 2 # # center height position im_out = overlay_image(scale_img, im_out, x, y) # Set text overlay near scale_img indicating the scale value scale_factor = (min_image_dimension / 350.0) # scale font relative to img font = 2 scale = 0.5 * scale_factor thick = 1 * scale_factor line_type = 4 color = tuple(np.ones(im_out.shape[2])*255) # white text, ie. same number of 255s and number of image channels # Set text at the small end of the scale bar loc = (int(x-13*scale_factor),int(y+7*scale_factor)) cv2.putText(im_out, "0", loc, font, scale, color, int(thick), line_type) # Set text at the large end of the scale bar scale_text_pretty = "%.1f cm" % self.scale_text loc = (x+scale_img.shape[1]+5,int(y+7*scale_factor)) cv2.putText(im_out, scale_text_pretty, loc, font, scale, color, int(thick), line_type) ## Preview result # cv2.imshow("Image", im_out) # cv2.waitKey(0) # cv2.destroyAllWindo() return im_out def ros_loop(self): publisher = rospy.Publisher('overlay/compressed', CompressedImage, queue_size=1) rospy.Subscriber('camera/compressed', CompressedImage, self.image_callback, queue_size=1) if not self.heading: rospy.Subscriber('heading', Float32, self.heading_callback, queue_size=1) if not self.scale_text: rospy.Subscriber('scale', Float32, self.scale_callback, queue_size=1) rospy.loginfo('Setup complete. Image overlay process has started.') framerate = rospy.Rate(rospy.get_param('~framerate',3)) # default 3 Hz while not rospy.is_shutdown(): img = self.overlay_telemetry() compressed_image = CvBridge().cv2_to_compressed_imgmsg(img) publisher.publish(compressed_image) framerate.sleep()
def image_callback(data): global xc, yc global move_position global collecting global c_cnt global HSV_value global lower_HSV, upper_HSV # change to opencv try: cv_image1 = CvBridge().imgmsg_to_cv2(data, "bgr8") except CvBridgeError as e: print(e) cv_image_draw = cv_image1.copy() if(move_position): cv2.putText(cv_image_draw, 'please move the camera to make ', (30, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0, 255, 0), 2, cv2.LINE_AA) cv2.putText(cv_image_draw, 'the red color in the rectangle ', (30, 80), cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0, 255, 0), 2, cv2.LINE_AA) cv2.putText(cv_image_draw, 'green box!then press the \'ENTER\' ', (30, 130), cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0, 255, 0), 2, cv2.LINE_AA) cv2.putText(cv_image_draw, 'in another terminal!', (30, 180), cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0, 255, 0), 2, cv2.LINE_AA) cv2.rectangle(cv_image_draw, (355, 310), (355 + cali_w, 310 + cali_h), (0, 255, 0), 3) c_cnt = 0 cv2.imshow("win_draw", cv_image_draw) cv2.waitKey(1) return elif(collecting): c_cnt = c_cnt+1 cv2.putText(cv_image_draw, 'collecting the hsv!', (30, 30), cv2.FONT_HERSHEY_SIMPLEX, 1.2, (0, 255, 0), 2, cv2.LINE_AA) cv2.putText(cv_image_draw, ' please wait for 5s.', (30, 80), cv2.FONT_HERSHEY_SIMPLEX, 1.2, (0, 255, 0), 2, cv2.LINE_AA) cv2.rectangle(cv_image_draw, (355, 310), (355 + cali_w, 310 + cali_h), (0, 255, 0), 3) frame = cv_image1[315:310 + cali_h-5, 360:355 + cali_w-5] HSV_value = mean_hsv(frame, HSV_value) cv2.imshow("win_draw", cv_image_draw) cv2.waitKey(1) if(c_cnt >= collect_times): for i in range(len(HSV_value)): HSV_value[i] = HSV_value[i] / collect_times print(i,len(HSV_value), HSV_value[i]) lower_HSV, upper_HSV = hsv_range(HSV_value) #save_hsv(name, lower_HSV, upper_HSV) print(lower_HSV , upper_HSV) collecting = 0 cv2.destroyWindow("win_draw") return #test(lower_HSV, upper_HSV, cv_image1) cv_image_cp = cv_image1.copy() cv_image_hsv = cv2.cvtColor(cv_image_cp, cv2.COLOR_BGR2HSV) cv_image_gray = cv2.inRange(cv_image_hsv, lower_HSV, upper_HSV) # smooth and clean noise cv_image_gray = cv2.erode(cv_image_gray, None, iterations=2) cv_image_gray = cv2.dilate(cv_image_gray, None, iterations=2) cv_image_gray = cv2.GaussianBlur(cv_image_gray, (5,5), 0) # detect contour cv2.imshow("win1", cv_image1) cv2.imshow("win2", cv_image_gray) cv2.waitKey(1) contours, hier = cv2.findContours(cv_image_gray, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) size = [] size_max = 0 for i, c in enumerate(contours): rect = cv2.minAreaRect(c) box = cv2.boxPoints(rect) box = np.int0(box) x_mid = (box[0][0] + box[2][0] + box[1][0] + box[3][0]) / 4 y_mid = (box[0][1] + box[2][1] + box[1][1] + box[3][1]) / 4 w = math.sqrt((box[0][0] - box[1][0])**2 + (box[0][1] - box[1][1])**2) h = math.sqrt((box[0][0] - box[3][0])**2 + (box[0][1] - box[3][1])**2) size.append(w * h) if size[i] > size_max: size_max = size[i] index = i xc = x_mid yc = y_mid
class OffbPosCtl: curr_pose = PoseStamped() waypointIndex = 0 detections = [] tag_pt_x = 0 tag_pt_y = 0 distThreshold= 2 detection_count=0 depth = Image() depth_matrix = [] dimg = Image() KP=0.005 KD=0.0004 KI=0.00005 prev_tag_pt_x=0 prev_tag_pt_y=0 upd_tag_pt_x=0 upd_tag_pt_y=0 left_image = Image() left_height = 0 left_width = 0 right_image = Image() right_height = 0 right_width = 0 camera=PinholeCameraModel() des_pose = PoseStamped() saved_location = None isReadyToFly = False left_matrix = [] right_matrix = [] cv_image_left = [] cv_image_right = [] hover_loc = [-1,.5,10,0,0,0,0] mode="HOVER" rgb_target = [] target = [] flag_x = "allow_x" flag_y = "allow_y" target = [] count = 1 vel_pub = rospy.Publisher('/mavros/setpoint_raw/local', PositionTarget, queue_size=10) pub = rospy.Publisher('/data', String, queue_size=10) def __init__(self): rospy.init_node('offboard_test', anonymous=True) self.loc = [] rospy.Subscriber('/mavros/local_position/pose', PoseStamped, callback= self.mocap_cb) rospy.Subscriber('/mavros/state',State, callback= self.state_cb) rospy.Subscriber('/dreams/state',String,callback=self.update_state_cb) rospy.Subscriber('/darknet_ros/bounding_boxes', BoundingBoxes, callback=self.yolo) rospy.Subscriber('/darknet_ros/detection_image', Image, callback=self.detected_image) rospy.Subscriber('/camera/depth/image_raw',Image,callback=self.depth_image) self.camera.fromCameraInfo(self.rgb_info()) #self.lawnmover(10,5,5,0,2.5) #self.depth_estm(self.left_matrix,self.right_matrix) self.hover() def rgb_info(self): msg_header = Header() msg_header.frame_id = "camera_link" msg_roi = RegionOfInterest() msg_roi.x_offset = 0 msg_roi.y_offset = 0 msg_roi.height = 0 msg_roi.width = 0 msg_roi.do_rectify = 0 msg = CameraInfo() msg.header = msg_header msg.height = 480 msg.width = 640 msg.distortion_model = 'plumb_bob' msg.D = [0.0, 0.0, 0.0, 0.0, 0.0] msg.K = [1.0, 0.0, 320.5, 0.0, 1.0, 240.5, 0.0, 0.0, 1.0] msg.R = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] msg.P = [1.0, 0.0, 320.5, -0.0, 0.0, 1.0, 240.5, 0.0, 0.0, 0.0, 1.0, 0.0] msg.binning_x = 0 msg.binning_y = 0 msg.roi = msg_roi return msg def depth_image(self,Img): self.depth = Img #print(self.depth.encoding) #print(self.depth_matrix) def depth_eval(self): cx = self.rgb_target[0] cy = self.rgb_target[1] x_left_lim = int(cx) - 3 x_right_lim = int(cx) + 3 y_down_lim = int(cy) - 3 y_up_lim = int(cy) + 3 sum = 0 self.depth_matrix = CvBridge().imgmsg_to_cv2(self.depth,desired_encoding= 'passthrough') #self.depth_matrix = np.array(self.depth.data, dtype=np.uint8) cpimg = self.depth_matrix.copy().astype(float) mat = cv2.convertScaleAbs(cpimg) depth = mat[x_left_lim:x_right_lim, y_down_lim:y_up_lim] # Truncating mat #print depth for i in range(6): for j in range(6): sum = sum + depth[i][j] avg_depth = sum/49 K = np.array([[1.0, 0.0, 320.5],[0.0, 1.0, 240.5],[0.0, 0.0, 1.0]]) K_inv = np.array([[1.0, 0, -320.5],[0, 1.0, -240.5],[0, 0, 1.0]]) self.target = avg_depth *np.matmul(K_inv,self.rgb_target) #print("________________________________________________________________________________________________________________________") print(self.target) def yolo(self,data): for a in data.bounding_boxes: if a.Class == "truck" or a.Class == "bus": self.detection_count = self.detection_count + 1 self.rgb_target = np.array([[a.xmin + (a.xmax - a.xmin)/2], [a.ymin + (a.ymax - a.ymin)/2], [1]]) #print(self.rgb_target) self.depth_eval() def detected_image(self,detection): try: self.dimg = CvBridge().imgmsg_to_cv2(detection, "bgr8") except CvBridgeError as e: print(e) def copy_pose(self , pose): pt = pose.pose.position quat = pose.pose.orientation copied_pose = PoseStamped() copied_pose.header.frame_id = pose.header.frame_id copied_pose.pose.position = Point(pt.x, pt.y, pt.z) copied_pose.pose.orientation = Quaternion(quat.x , quat.y , quat.z , quat.w) return copied_pose def mocap_cb(self,msg1): self.curr_pose = msg1 #print msg1 def state_cb(self,msg): if msg.mode == 'OFFBOARD': #print msg #print("The mode is OFFBOARD") self.isReadyToFly = True else: #print("I am in state_cb") #print msg print msg.mode def update_state_cb(self,data): self.mode= data.data #print self.mode def hover(self): location = self.hover_loc loc = [location, location, location, location, location, location, location, location, location] #print loc rate = rospy.Rate(10) shape = len(loc) pose_pub = rospy.Publisher('/mavros/setpoint_position/local', PoseStamped, queue_size= 10) des_pose = self.copy_pose(self.curr_pose) waypoint_index = 0 sim_ctr = 1 #print("I am here") while self.mode=="HOVER" and self.detection_count < 5 and not rospy.is_shutdown(): print(len(self.detections)) if waypoint_index==shape: waypoint_index = 0 # changing the way point index to 0 sim_ctr = sim_ctr + 1 print "HOVER STOP COUNTER:" + str(sim_ctr) if self.isReadyToFly: des_x = loc[waypoint_index][0] des_y = loc[waypoint_index][1] des_z = loc[waypoint_index][2] des_pose.pose.position.x = des_x des_pose.pose.position.y = des_y des_pose.pose.position.z = des_z des_pose.pose.orientation.x = loc[waypoint_index][3] des_pose.pose.orientation.y = loc[waypoint_index][4] des_pose.pose.orientation.z = loc[waypoint_index][5] des_pose.pose.orientation.w = loc[waypoint_index][6] curr_x = self.curr_pose.pose.position.x curr_y = self.curr_pose.pose.position.y curr_z = self.curr_pose.pose.position.z #print('I am here') dist = math.sqrt((curr_x - des_x)*(curr_x - des_x) + (curr_y - des_y)*(curr_y - des_y) + (curr_z - des_z)*(curr_z - des_z)) if dist<self.distThreshold : waypoint_index += 1 if sim_ctr == 50: pass pose_pub.publish(des_pose) #print(des_pose) rate.sleep() def get_descent(self,x,y,z): des_vel = PositionTarget() des_vel.header.frame_id = "world" des_vel.header.stamp=rospy.Time.from_sec(time.time()) des_vel.coordinate_frame= 8 des_vel.type_mask = 3527 des_vel.velocity.x = x des_vel.velocity.y = y des_vel.velocity.z = z return des_vel
def color_callback(self, color_msg): start_time = time.time() # Invoke the image from camera ori_image = CvBridge().imgmsg_to_cv2(color_msg) use_image = ori_image.copy() # Process the image rgb_img = processing_depth(use_image) # Predict the grasp q_img, cos_img, sin_img, width_img = prediction(rgb_img) # Filter the q_img ret = filter_thresh(q_img[0], 0.5) q_img_process = local_max(q_img[0]) # Post process the output dis_img, grasp_pts, center = process_output(rgb_img[0], q_img[0], cos_img[0], sin_img[0], width_img[0]) offsets = np.array([170, 90]) dis_img2, grasp_pts_list, centers, angles = process_output2(rgb_img[0], q_img[0], cos_img[0], sin_img[0], width_img[0], offsets) # Publish the grasp points depth_point = self.depth_img[center[0]+90, center[1]+170] px, py, pz = self.coordinate.get_coord(depth_point, center[0]+90, center[1]+170) pxs, pys, pzs = self.coordinate.get_coords(self.depth_img, centers[:,0]+90, centers[:,1]+170) # print(pxs, pys, pzs) #print("Depth point: ", depth_point) grasp_pts = grasp_pts + offsets # Find the coordinate for grasping grasp_pcd, pcl2_pcd = self.coordinate.get_pcd(self.depth_img, grasp_pts, grasp_pts_list) self.msg.px = px/1000.0 self.msg.py = py/1000.0 self.msg.pz = pz/1000.0 pxs = list(np.array(pxs)/1000.0) pys = list(np.array(pys)/1000.0) pzs = list(np.array(pzs)/1000.0) costhetas = list(np.array(angles)[:, 0]) sinthetas = list(np.array(angles)[:, 1]) self.msg.pxs = pxs self.msg.pys = pys self.msg.pzs = pzs self.msg.costhetas = costhetas self.msg.sinthetas = sinthetas # Convert from depth_img to JET cv2.polylines(self.depth_img, [grasp_pts], True, (0, 255, 0)) # Publish the grasp points and pointcloud try: self.pcd_pub.publish(pcl2_pcd) self.grasp_pub.publish(self.msg) except KeyboardInterrupt: print("Publish error!") end_time = time.time() execute_time = end_time - start_time print("Excute time: ", int(execute_time*1000), "ms") # Display the output #heatmap = cv2.applyColorMap(np.uint8(q_img[0]*255), cv2.COLORMAP_JET) heatmap = cv2.applyColorMap(np.uint8(ret*255), cv2.COLORMAP_JET) q_img_process = cv2.applyColorMap(np.uint8(q_img_process*255), cv2.COLORMAP_JET) depth_dis = np.uint8(np.clip(self.depth_img, 0, 1000)/1000.0*255) cv2.imshow("Depth", depth_dis) cv2.waitKey(3) cv2.imshow("Multi-grasp", dis_img2) cv2.waitKey(3) cv2.imshow("Grasp Quality", heatmap) cv2.waitKey(3) cv2.imshow("Best Grasp", dis_img) cv2.waitKey(3)
class ImageOverlay: def __init__(self): self.i="" self.temp="" self.number=0 self.asteering_raw=0 self.max_steering = 1 self.min_steering = -1 self.epsilon_steering = math.radians(0.001) self.img = np.ones((700,1000,3),dtype=np.uint8)*128 # default black background # update the image to the new image from camera feed def image_callback(self, data): self.img = CvBridge().compressed_imgmsg_to_cv2(data) def overlay(self): img_out = self.img.copy() return img_out def call_back2(self,data): ack_cmd = AckermannDriveStamped() ack_cmd.header.stamp = rospy.Time.now() drive = AckermannDrive() drive.speed = data.linear.x drive.steering_angle = data.angular.z # impose limits on commanded angle # datetime object containing current date and time now = datetime.now() self.i = now.strftime("%H:%M:%S") my_name="youssf" # dd/mm/YY H:M:S image =self.overlay()[210:,:] if(self.temp != str(self.i)): with open("data_index.txt", "a") as myfile: cv2.imwrite("data/"+my_name+self.i+'.jpg',image) myfile.write("data/"+my_name+self.i+'.jpg,'+str(drive.steering_angle)+"\n") print("now =", str(self.number) + " with id = "+str(self.i)+" shape"+str(image.shape)) self.temp=str(self.i) self.number+=1 ack_cmd.drive = drive self.publisher.publish(ack_cmd) def run(self): rospy.init_node("get_img") #self.publisher = rospy.Publisher('overlay/compressed', CompressedImage, queue_size=1) # Read in camera feed, change first arg to read a different camera self.camera_subscriber = rospy.Subscriber('/camera/left/image_raw/compressed', CompressedImage, self.image_callback, queue_size=1) self.publisher = rospy.Publisher('/robot_control/command', AckermannDriveStamped, queue_size=10) self.s=rospy.Subscriber("/cmd_vel", Twist, self.call_back2) rate = rospy.Rate(3) # default 3 Hz #print("Running camera overlay in overlay/compressed") while(1): # init # choose ROI roi_x = 66 roi_y = 200 while True: startTime = time.time() image =self.overlay() # from 1 dim to 3 cv2.imshow('inference (q to exit)', self.overlay()) if cv2.waitKey(1) & 0xFF == ord('q'): break # send steering to serial port endTime = time.time() # calculate fps fps_q.append(endTime - startTime)
class OffbPosCtl: curr_pose = PoseStamped() des_pose = PoseStamped() cam_pose = PoseStamped() cam_img = PoseStamped() x_prev_error = 0 y_prev_error = 0 destination_x_previous = 0 destination_y_previous = 0 destination_z_previous = 0 destination_pose = PoseStamped() waypointIndex = 0 detections = [] distThreshold = 2 detection_count = 0 depth = Image() depth_matrix = [] dimg = Image() KP = .005 KD = 0.0004 des_x = 0 des_y = 0 des_z = 0 camera = PinholeCameraModel() saved_location = None isReadyToFly = False hover_loc = [-28, 16.3, 10, 0, 0, 0, 0] mode = "HOVER" rgb_target = [] ray_target = [] target = [] # a column vector that stores the x,y,z target #dronename = rospy.get_param('iris_cam') vel_pub = rospy.Publisher('/mavros/setpoint_raw/local', PositionTarget, queue_size=10) pub = rospy.Publisher('/data', String, queue_size=10) def __init__(self): rospy.init_node('offboard_test', anonymous=True) self.loc = [] rospy.Subscriber('/mavros/local_position/pose', PoseStamped, callback=self.mocap_cb) rospy.Subscriber('/mavros/state', State, callback=self.state_cb) rospy.Subscriber('/dreams/state', String, callback=self.update_state_cb) rospy.Subscriber('/darknet_ros/bounding_boxes', BoundingBoxes, callback=self.yolo) rospy.Subscriber('/darknet_ros/detection_image', Image, callback=self.detected_image) rospy.Subscriber('/camera/depth/image_raw', Image, callback=self.depth_image) #self.camera.fromCameraInfo(self.rgb_info()) # Zhiang: use this instead camera_info = rospy.wait_for_message( "/camera/depth/camera_info", CameraInfo) # Zhiang: make sure this is the correct camera #camera_info = rospy.wait_for_message("/camera/rgb/camera_info", CameraInfo) self.pinhole_camera_model = PinholeCameraModel() self.pinhole_camera_model.fromCameraInfo(camera_info) #self.listener = tf.TransformListener() # linked to tf_static self.planner() def mocap_cb(self, msg1): self.curr_pose = msg1 #br = tf.TransformBroadcaster() #br.sendTransform((msg1.pose.position.x + 0, msg1.pose.position.y + 0, msg1.pose.position.z - 0.2, msg1.pose.orientation.x + 0.3825 , msg1.pose.orientation.y + 0.0003 , msg1.pose.orientation.z + 0.924 , msg1.pose.orientation.w + 0.001),rospy.Time.now(),"world", self.dronename) def rgb_info(self): msg_header = Header() msg_header.frame_id = "camera_link" msg_roi = RegionOfInterest() msg_roi.x_offset = 0 msg_roi.y_offset = 0 msg_roi.height = 0 msg_roi.width = 0 msg_roi.do_rectify = 0 msg = CameraInfo() msg.header = msg_header msg.height = 480 msg.width = 640 msg.distortion_model = 'plumb_bob' msg.D = [0.0, 0.0, 0.0, 0.0, 0.0] msg.K = [1.0, 0.0, 320.5, 0.0, 1.0, 240.5, 0.0, 0.0, 1.0] msg.R = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] msg.P = [ 1.0, 0.0, 320.5, -0.0, 0.0, 1.0, 240.5, 0.0, 0.0, 0.0, 1.0, 0.0 ] msg.binning_x = 0 msg.binning_y = 0 msg.roi = msg_roi return msg def depth_image(self, Img): self.depth = Img #print(self.depth.encoding) #print(self.depth_matrix) def depth_eval(self): cx = self.rgb_target[0] alpha1 = 0.7 cy = self.rgb_target[1] x_left_lim = int(cx) - 3 x_right_lim = int(cx) + 3 y_down_lim = int(cy) - 3 y_up_lim = int(cy) + 3 sum = 0 #print(x_left_lim,x_right_lim,y_down_lim,y_up_lim) self.depth_matrix = CvBridge().imgmsg_to_cv2( self.depth, desired_encoding='passthrough') #self.depth_matrix = np.array(self.depth.data, dtype=np.uint8) cpimg = self.depth_matrix.copy().astype(float) #print(cpimg.shape) #mat = cv2.convertScaleAbs(cpimg, alpha = 255/cpimg.max()) depth = cpimg[x_left_lim:x_right_lim, y_down_lim:y_up_lim] # Truncating mat #print depth if depth.shape != 0: for i in range(6): for j in range(6): sum = sum + depth[i][j] avg_depth = sum / (49) # Evaluating x value from Z : #print(avg_depth) #K = np.array([[1.0, 0.0, 510],[0.0, 1.0, 384],[0.0, 0.0, 1.0]]) #K_inv = np.array([[1.0, 0, -510],[0, 1.0, -384],[0, 0, 1.0]]) #self.target = avg_depth *np.matmul(K_inv,self.rgb_target) #print(self.target) #x = ((self.rgb_target[0][0] - 510) * avg_depth)/#focal length from the pinhole camera model? # fx is the focal length in the pixel coordinate #y = ((self.rgb_target[1][0] - 384) * avg_depth)/# focal length from the pinhole camera model? # fy is the focal length in the pixel coordinate #print(self.ray_target[0]) #print(self.ray_target[1]) #print(avg_depth) #print(self.ray_target) x = self.ray_target[0] * avg_depth y = self.ray_target[1] * avg_depth z = avg_depth #print("_______________________________________________") #print x #print y #print z self.target = np.array([[x], [y], [z]]) #hom_transformation = np.array([[0, -1, 0, 0],[-0.7071, 0, 0.7071, 0],[-.7071, 0, -.7071, 0],[0, 0, 0.2, 1]]) hom_transformation = np.array([[0, -0.707, -0.707, 0], [-1, 0, 0, 0], [0, 0.707, -.707, 0], [0, 0, 0, 1]]) homogeneous_coordinates = np.array([[self.target[0][0]], [self.target[1][0]], [self.target[2][0]], [1]]) #print(homogeneous_coordinates) product = np.matmul(hom_transformation, homogeneous_coordinates) self.cam_img.pose.position.x = product[0][0] self.cam_img.pose.position.y = product[1][0] self.cam_img.pose.position.z = product[2][0] #print(product[0][0]) #print(product[1][0]) #print(product[2][0]) #self.cam_img.pose.position.z = self.target[2] self.cam_img.pose.orientation.x = 0 self.cam_img.pose.orientation.y = 0 self.cam_img.pose.orientation.z = 0 self.cam_img.pose.orientation.w = 1 self.destination_pose.pose.position.x = self.cam_img.pose.position.x + self.curr_pose.pose.position.x self.destination_pose.pose.position.y = self.cam_img.pose.position.y + self.curr_pose.pose.position.y self.destination_pose.pose.position.z = self.cam_img.pose.position.z + self.curr_pose.pose.position.z self.destination_pose.pose.orientation.x = self.curr_pose.pose.orientation.x self.destination_pose.pose.orientation.y = self.curr_pose.pose.orientation.y self.destination_pose.pose.orientation.z = self.curr_pose.pose.orientation.z self.destination_pose.pose.orientation.w = self.curr_pose.pose.orientation.w self.destination_x = ( (1 - alpha1) * self.destination_pose.pose.position.x) + ( alpha1 * self.destination_x_previous) self.destination_y = ( (1 - alpha1) * self.destination_pose.pose.position.y) + ( alpha1 * self.destination_y_previous) self.destination_z = ( (1 - alpha1) * self.destination_pose.pose.position.z) + ( alpha1 * self.destination_z_previous) self.destination_x_previous = self.destination_x self.destination_y_previous = self.destination_y self.destination_z_previous = self.destination_z #print(self.destination_x,self.destination_y,self.destination_z) def yolo(self, data): while self.curr_pose.pose.position.z > 4 and self.curr_pose.pose.position.z < 14: for a in data.bounding_boxes: if (a.Class == "stop sign" or a.Class == "kite" or a.Class == "cell phone" or a.Class == "car" or a.Class == "truck" or a.Class == "bus"): self.detection_count = self.detection_count + 1 #print(self.detection_count) self.rgb_target = np.array( [[a.xmin + (a.xmax - a.xmin) / 2], [a.ymin + (a.ymax - a.ymin) / 2], [1]]) self.ray_target = self.pinhole_camera_model.projectPixelTo3dRay( (a.xmin + (a.xmax - a.xmin) / 2, a.ymin + (a.ymax - a.ymin) / 2)) #print(self.rgb_target) #print(self.rgb_target[1]) self.depth_eval() def detected_image(self, detection): try: self.dimg = CvBridge().imgmsg_to_cv2(detection, "bgr8") except CvBridgeError as e: print(e) def copy_pose(self, pose): pt = pose.pose.position quat = pose.pose.orientation copied_pose = PoseStamped() copied_pose.header.frame_id = pose.header.frame_id copied_pose.pose.position = Point(pt.x, pt.y, pt.z) copied_pose.pose.orientation = Quaternion(quat.x, quat.y, quat.z, quat.w) return copied_pose def state_cb(self, msg): if msg.mode == 'OFFBOARD': self.isReadyToFly = True else: print msg.mode def update_state_cb(self, data): self.mode = data.data def attach(self): attach_srv = rospy.ServiceProxy('/link_attacher_node/attach', Attach) attach_srv.wait_for_service() print('Trying to attach') req = AttachRequest() req.model_name_1 = "iris" req.link_name_1 = "base_link" req.model_name_2 = "suv" # Insert the model name you want to attach to req.link_name_2 = "link" # Insert link name you want to attach to attach_srv.call(req) print('Attached') def detach(self): attach_srv = rospy.ServiceProxy('/link_attacher_node/detach', Attach) attach_srv.wait_for_service() print('Trying to detach') req = AttachRequest() req.model_name_1 = "iris" req.link_name_1 = "base_link" req.model_name_2 = "suv" # Insert the model name you want to attach to req.link_name_2 = "link" # Insert link name you want to attach to attach_srv.call(req) print('Detached') def hover(self): location = self.hover_loc loc = [ location, location, location, location, location, location, location, location, location ] rate = rospy.Rate(10) shape = len(loc) pose_pub = rospy.Publisher('/mavros/setpoint_position/local', PoseStamped, queue_size=10) des_pose = self.copy_pose(self.curr_pose) waypoint_index = 0 sim_ctr = 1 while self.mode == "HOVER" and self.detection_count < 100 and not rospy.is_shutdown( ): #print(self.detection_count) if waypoint_index == shape: waypoint_index = 0 # changing the way point index to 0 sim_ctr = sim_ctr + 1 #print "HOVER STOP COUNTER:" + str(sim_ctr) if self.isReadyToFly: des_x = loc[waypoint_index][0] des_y = loc[waypoint_index][1] des_z = loc[waypoint_index][2] des_pose.pose.position.x = des_x des_pose.pose.position.y = des_y des_pose.pose.position.z = des_z des_pose.pose.orientation.x = loc[waypoint_index][3] des_pose.pose.orientation.y = loc[waypoint_index][4] des_pose.pose.orientation.z = loc[waypoint_index][5] des_pose.pose.orientation.w = loc[waypoint_index][6] curr_x = self.curr_pose.pose.position.x curr_y = self.curr_pose.pose.position.y curr_z = self.curr_pose.pose.position.z dist = math.sqrt((curr_x - des_x) * (curr_x - des_x) + (curr_y - des_y) * (curr_y - des_y) + (curr_z - des_z) * (curr_z - des_z)) if dist < self.distThreshold: waypoint_index += 1 if sim_ctr == 50: pass #print('I am here') pose_pub.publish(des_pose) #print('I am here') rate.sleep() if self.detection_count >= 100: self.mode = "SWOOP" self.descent() #print(self.mode) def get_descent(self, x, y, z): des_vel = PositionTarget() des_vel.header.frame_id = "world" des_vel.header.stamp = rospy.Time.from_sec(time.time()) des_vel.coordinate_frame = 8 des_vel.type_mask = 3527 des_vel.velocity.x = y des_vel.velocity.y = x des_vel.velocity.z = z return des_vel def descent(self): rate = rospy.Rate(10) # 10 Hz # find tf static section under depth_eval #print self.mode while self.mode == "SWOOP" and self.curr_pose.pose.position.z > 1 and not rospy.is_shutdown( ): #print(" In while loop") err_x = self.des_x - self.curr_pose.pose.position.x err_y = self.des_y - self.curr_pose.pose.position.y err_z = self.curr_pose.pose.position.z - self.des_z # self.des_z is small - close to ground. #print(err_y) #print(err_z) # print(err_y) x_change = -(err_x * self.KP * 1 ) #-(((err_x - self.x_prev_error) * 70) * self.KD) y_change = -(err_y * self.KP * 0 ) #-(((err_y - self.y_prev_error) * 70) * self.KD) z_change = -((err_z) / (math.sqrt((err_x)**2 + (err_y)**2))) * self.KP * 200 des = self.get_descent(x_change, y_change, z_change) self.vel_pub.publish(des) self.x_prev_error = err_x self.y_prev_error = err_y self.pub.publish("PICKUP COMPLETE") rate.sleep() self.attach() # The drone gets attached to the target self.mode == "HOVER" self.hover() def planner(self): if self.mode == "HOVER": self.hover() print('transitioning')
class OffbPosCtl: curr_pose = PoseStamped() des_pose = PoseStamped() cam_pose = PoseStamped() waypointIndex = 0 detections = [] distThreshold= 2 detection_count=0 depth = Image() depth_matrix = [] dimg = Image() KP=0.005 des_x = 0 des_y = 0 des_z = 0 camera=PinholeCameraModel() saved_location = None isReadyToFly = False hover_loc = [1,-9,10,0,0,0,0] mode="HOVER" rgb_target = [] target = [] # a column vector that stores the x,y,z target #dronename = rospy.get_param('iris_cam') vel_pub = rospy.Publisher('/mavros/setpoint_raw/local', PositionTarget, queue_size=10) pub = rospy.Publisher('/data', String, queue_size=10) def __init__(self): rospy.init_node('offboard_test', anonymous=True) self.loc = [] rospy.Subscriber('/mavros/local_position/pose', PoseStamped, callback= self.mocap_cb) rospy.Subscriber('/mavros/state',State, callback= self.state_cb) rospy.Subscriber('/dreams/state',String,callback=self.update_state_cb) rospy.Subscriber('/darknet_ros/bounding_boxes', BoundingBoxes, callback=self.yolo) rospy.Subscriber('/darknet_ros/detection_image', Image, callback=self.detected_image) rospy.Subscriber('/camera/depth/image_raw',Image,callback=self.depth_image) self.camera.fromCameraInfo(self.rgb_info()) #self.listener = tf.TransformListener() # linked to tf_static self.planner() def mocap_cb(self,msg1): self.curr_pose = msg1 #br = tf.TransformBroadcaster() #br.sendTransform((msg1.pose.position.x + 0, msg1.pose.position.y + 0, msg1.pose.position.z - 0.2, msg1.pose.orientation.x + 0.3825 , msg1.pose.orientation.y + 0.0003 , msg1.pose.orientation.z + 0.924 , msg1.pose.orientation.w + 0.001),rospy.Time.now(),"world", self.dronename) def rgb_info(self): msg_header = Header() msg_header.frame_id = "camera_link" msg_roi = RegionOfInterest() msg_roi.x_offset = 0 msg_roi.y_offset = 0 msg_roi.height = 0 msg_roi.width = 0 msg_roi.do_rectify = 0 msg = CameraInfo() msg.header = msg_header msg.height = 480 msg.width = 640 msg.distortion_model = 'plumb_bob' msg.D = [0.0, 0.0, 0.0, 0.0, 0.0] msg.K = [1.0, 0.0, 320.5, 0.0, 1.0, 240.5, 0.0, 0.0, 1.0] msg.R = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] msg.P = [1.0, 0.0, 320.5, -0.0, 0.0, 1.0, 240.5, 0.0, 0.0, 0.0, 1.0, 0.0] msg.binning_x = 0 msg.binning_y = 0 msg.roi = msg_roi return msg def depth_image(self,Img): self.depth = Img #print(self.depth.encoding) #print(self.depth_matrix) def depth_eval(self): cx = self.rgb_target[0] cy = self.rgb_target[1] x_left_lim = int(cx) - 3 x_right_lim = int(cx) + 3 y_down_lim = int(cy) - 3 y_up_lim = int(cy) + 3 sum = 0 self.depth_matrix = CvBridge().imgmsg_to_cv2(self.depth,desired_encoding= 'passthrough') #self.depth_matrix = np.array(self.depth.data, dtype=np.uint8) cpimg = self.depth_matrix.copy().astype(float) mat = cv2.convertScaleAbs(cpimg, alpha = 255/cpimg.max()) depth = mat[x_left_lim:x_right_lim, y_down_lim:y_up_lim] # Truncating mat #print depth for i in range(6): for j in range(6): sum = sum + depth[i][j] avg_depth = sum/49 K = np.array([[1.0, 0.0, 320.5],[0.0, 1.0, 240.5],[0.0, 0.0, 1.0]]) K_inv = np.array([[1.0, 0, -320.5],[0, 1.0, -240.5],[0, 0, 1.0]]) self.target = .001*avg_depth *np.matmul(K_inv,self.rgb_target) hom_transformation = np.array([[-.707, 0, -.707, 0],[0, -1, 0, 0],[-.707, 0, .707, -0.2],[0, 0, 0, 1]]) homogeneous_coordinates = [[self.target[0]],[self.target[1]],[self.target[2]],[1]] product = np.matmul(hom_transformation,homogeneous_coordinates) self.cam_img.pose.position.x = product[0] self.cam_img.pose.position.y = product[1] self.cam_img.pose.position.z = product[2] self.cam_img.pose.position.z = self.target[2] self.cam_img.pose.orientation.x = 0 self.cam_img.pose.orientation.y = 0 self.cam_img.pose.orientation.z = 0 self.cam_img.pose.orientation.w = 1 self.des_pose.pose.position.x = self.cam_img.pose.position.x + self.curr_pose.pose.position.x self.des_pose.pose.position.y = self.cam_img.pose.position.y + self.curr_pose.pose.position.y self.des_pose.pose.position.z = self.cam_img.pose.position.z + self.curr_pose.pose.position.z self.des_pose.pose.orientation.x = self.curr_pose.pose.orientation.x self.des_pose.pose.orientation.y = self.curr_pose.pose.orientation.y self.des_pose.pose.orientation.z = self.curr_pose.pose.orientation.z self.des_pose.pose.orientation.w = self.curr_pose.pose.orientation.w self.des_x = self.des_pose.pose.position.x self.des_y = self.des_pose.pose.position.y self.des_z = self.des_pose.pose.position.z # Tf static # Message@Abhijith: This section has been appended under depth_eval #self.cam_img.pose.position.x = self.target[0] #self.cam_img.pose.position.y = self.target[1] #self.cam_img.pose.position.z = self.target[2] #self.cam_img.pose.orientation.x = 0 #self.cam_img.pose.orientation.y = 0 #self.cam_img.pose.orientation.z = 0 #self.cam_img.pose.orientation.w = 1 #self.cam_img.header.stamp = rospy.Time.now() #self.cam_img.header.frame_id = self.camera.tfFrame() #self.listener.waitForTransform(self.cam_img.tfFrame(), "world", rospy.Time.now(), rospy.Duration(1.0)) # listener is missing #tf_point = self.listener.transformPose("world", self.cam_img) #self.des_pose.pose.position.x = tf_point.pose.position.x #self.des_pose.pose.position.y = tf_point.pose.position.y #self.des_pose.pose.position.z = tf_point.pose.position.z #self.des_x = self.des_pose.pose.position.x #self.des_y = self.des_pose.pose.position.y #self.des_z = self.des_pose.pose.position.z print(self.des_x) print(self.des_y) print(self.des_z) print("________________________________________________________________________________________________________________________") def yolo(self,data): for a in data.bounding_boxes: if a.Class == "truck" or a.Class == "bus": self.detection_count = self.detection_count + 1 self.rgb_target = np.array([[a.xmin + (a.xmax - a.xmin)/2], [a.ymin + (a.ymax - a.ymin)/2], [1]]) self.depth_eval() def detected_image(self,detection): try: self.dimg = CvBridge().imgmsg_to_cv2(detection, "bgr8") except CvBridgeError as e: print(e) def copy_pose(self , pose): pt = pose.pose.position quat = pose.pose.orientation copied_pose = PoseStamped() copied_pose.header.frame_id = pose.header.frame_id copied_pose.pose.position = Point(pt.x, pt.y, pt.z) copied_pose.pose.orientation = Quaternion(quat.x , quat.y , quat.z , quat.w) return copied_pose def state_cb(self,msg): if msg.mode == 'OFFBOARD': self.isReadyToFly = True else: print msg.mode def update_state_cb(self,data): self.mode= data.data def attach(self): attach_srv = rospy.ServiceProxy('/link_attacher_node/attach', Attach) attach_srv.wait_for_service() print('Trying to attach') req = AttachRequest() req.model_name_1 = "iris" req.link_name_1 = "base_link" req.model_name_2 = "apriltag" # Insert the model name you want to attach to req.link_name_2 = "link" # Insert link name you want to attach to attach_srv.call(req) print('Attached') def detach(self): attach_srv = rospy.ServiceProxy('/link_attacher_node/detach', Attach) attach_srv.wait_for_service() print('Trying to detach') req = AttachRequest() req.model_name_1 = "iris" req.link_name_1 = "base_link" req.model_name_2 = "apriltag" # Insert the model name you want to attach to req.link_name_2 = "link" # Insert link name you want to attach to attach_srv.call(req) print('Detached') def hover(self): location = self.hover_loc loc = [location, location, location, location, location, location, location, location, location] rate = rospy.Rate(10) shape = len(loc) pose_pub = rospy.Publisher('/mavros/setpoint_position/local', PoseStamped, queue_size= 10) des_pose = self.copy_pose(self.curr_pose) waypoint_index = 0 sim_ctr = 1 while self.mode=="HOVER" and self.detection_count < 5 and not rospy.is_shutdown(): print(len(self.detections)) if waypoint_index==shape: waypoint_index = 0 # changing the way point index to 0 sim_ctr = sim_ctr + 1 print "HOVER STOP COUNTER:" + str(sim_ctr) if self.isReadyToFly: des_x = loc[waypoint_index][0] des_y = loc[waypoint_index][1] des_z = loc[waypoint_index][2] des_pose.pose.position.x = des_x des_pose.pose.position.y = des_y des_pose.pose.position.z = des_z des_pose.pose.orientation.x = loc[waypoint_index][3] des_pose.pose.orientation.y = loc[waypoint_index][4] des_pose.pose.orientation.z = loc[waypoint_index][5] des_pose.pose.orientation.w = loc[waypoint_index][6] curr_x = self.curr_pose.pose.position.x curr_y = self.curr_pose.pose.position.y curr_z = self.curr_pose.pose.position.z dist = math.sqrt((curr_x - des_x)*(curr_x - des_x) + (curr_y - des_y)*(curr_y - des_y) + (curr_z - des_z)*(curr_z - des_z)) if dist<self.distThreshold : waypoint_index += 1 if sim_ctr == 50: pass pose_pub.publish(des_pose) rate.sleep() if self.detection_count >= 5 : self.mode = "SWOOP" def get_descent(self,x,y,z): des_vel = PositionTarget() des_vel.header.frame_id = "world" des_vel.header.stamp=rospy.Time.from_sec(time.time()) des_vel.coordinate_frame= 8 des_vel.type_mask = 3527 des_vel.velocity.x = x des_vel.velocity.y = y des_vel.velocity.z = z return des_vel def descent(self): rate = rospy.Rate(10) # 10 Hz # find tf static section under depth_eval print self.mode while self.mode == "SWOOP" and self.curr_pose.pose.position.z > 0.02 and not rospy.is_shutdown(): print(" In while loop") err_x = self.des_x - self.curr_pose.pose.position.x err_y = self.des_y - self.curr_pose.pose.position.y err_z = self.des_z - self.curr_pose.pose.position.z #print(err_x) # print(err_y) x_change = -(err_x * self.KP * 2) y_change = -(err_y * self.KP * 2) z_change = -( (math.sqrt((err_x)^2 + (err_y)^2))/(err_z) ) * self.KP des = self.get_descent(x_change, y_change, z_change) self.vel_pub.publish(des) self.pub.publish("PICKUP COMPLETE") rate.sleep() self.attach() # The drone gets attached to the target def planner(self): if self.mode == "HOVER": self.hover() if self.mode == "SWOOP": self.descent() if self.mode=="PICKED": return self.flight()
def img_cb(msg): global sav_cnt, block_array try: cv_image = CvBridge().imgmsg_to_cv2(msg, "bgr8") except CvBridgeError as e: print("[INFO]: Error in obtaining image from CvBridge! Skipping frame!") else: # converting to grayscale gray = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY) # threshold image ret, thresh1 = cv2.threshold(gray, 127, 255, cv2.THRESH_BINARY) ret, thresh2 = cv2.threshold(gray, 127, 255, cv2.THRESH_BINARY_INV) # apply erosion to thresholded image kernel = np.ones((3, 3), np.uint8) eroded = cv2.erode(thresh2, kernel, iterations=1) # find edges edges = cv2.Canny(gray, 80, 120) """ Lane line detection """ contoured_img = find_contours(cv_image.copy(), edges.copy()) # use contoured img to get the lines in bottom half using HoughLines and not HoughLinesP contoured_img_gray = cv2.cvtColor(contoured_img, cv2.COLOR_BGR2GRAY) mask = np.zeros_like(contoured_img_gray) mask[int((2*mask.shape[1])/3):, :] = 255 masked_img = cv2.bitwise_and(edges, edges, mask=mask) lines_angled = cv2.HoughLines(masked_img, 1, m.pi/180, 75) line_factor = 500 left_theta = [] right_theta = [] left_rho = [] right_rho = [] left_theta_avg = None right_theta_avg = None left_rho_avg = None right_rho_avg = None if lines_angled.shape[0] != 0: for i in range(lines_angled.shape[0]): for rho, theta in lines_angled[i]: print(rho, theta) if (theta) < m.radians(5): continue if theta < m.pi/2: left_theta.append(theta) left_rho.append(rho) else: right_theta.append(theta) right_rho.append(rho) if len(left_theta) != 0: left_theta_avg = sum(left_theta) / len(left_theta) left_rho_avg = sum(left_rho) / len(left_rho) a = np.cos(left_theta_avg) b = np.sin(left_theta_avg) x0 = a*left_rho_avg y0 = b*left_rho_avg x1 = int(x0 + line_factor*(-b)) y1 = int(y0 + line_factor*(a)) x2 = int(x0 - line_factor*(-b)) y2 = int(y0 - line_factor*(a)) cv2.line(cv_image, (x1, y1), (x2, y2), (0, 0, 255), 2) if len(right_theta) != 0: right_theta_avg = sum(right_theta) / len(right_theta) right_rho_avg = sum(right_rho) / len(right_rho) a = np.cos(right_theta_avg) b = np.sin(right_theta_avg) x0 = a*right_rho_avg y0 = b*right_rho_avg x1 = int(x0 + line_factor*(-b)) y1 = int(y0 + line_factor*(a)) x2 = int(x0 - line_factor*(-b)) y2 = int(y0 - line_factor*(a)) cv2.line(cv_image, (x1, y1), (x2, y2), (0, 125, 125), 2) """ vertical lines detection """ # extract HoughP lines lines = cv2.HoughLinesP(thresh2, 1, m.pi/2, 2, None, 70, 1) lines_drawn = [] for i in range(lines.shape[0]): for line in lines[i]: pt1 = (line[0], line[1]) pt2 = (line[2], line[3]) # to just detect vertical lines if (line[0] != line[2]): continue # to remove the ceiling lines max_ordinate = max(line[1], line[3]) if max_ordinate < 200: continue """ to plot blue lines """ cv2.line(cv_image, pt1, pt2, (255, 0, 0), 3) lines_drawn.append(pt1) lines_drawn.append(pt2) # sort the lines_drawn pts lines_drawn.sort(key=lambda x: x[0]) # print("{} vertical lines were detected: and they are \n {}".format(len(lines_drawn), lines_drawn)) lines_drawn_new = [n for n in lines_drawn if n[1] > 200] # print("new points are : \n {}".format(lines_drawn_new)) # lets use bunching up techniques run_x_avg = lines_drawn_new[0][0] run_y = lines_drawn_new[0][1] pts_list = [] count = 1 x_thresh = 10 consider = False consider_thresh = 7 if left_theta_avg != None: slop_left = m.tan(m.pi/2 + left_theta_avg) intercept_left = -(slop_left * left_rho_avg) / \ m.cos(left_theta_avg) if right_theta_avg != None: slop_right = m.tan(m.pi/2 + right_theta_avg) intercept_right = -(slop_right * right_rho_avg) / \ m.cos(right_theta_avg) for i in range(1, len(lines_drawn_new)): if abs(run_x_avg - lines_drawn_new[i][0]) <= x_thresh: count += 1 # calculate the new x avg run_x_avg = run_x_avg + \ ((lines_drawn_new[i][0] - run_x_avg)*1.0) / count run_y = max(run_y, lines_drawn_new[i][1]) else: # we need to check if this point is close enough to the intersection point with either of the lane lines consider = False if left_theta_avg != None: y_intersect = slop_left * run_x_avg + intercept_left if abs(y_intersect - run_y) <= consider_thresh: consider = True run_y = y_intersect pts_list.append([run_x_avg, run_y, "l"]) """ To plot the magenta lines """ cv2.line(cv_image, (int(run_x_avg), 0), (int(run_x_avg), int(run_y)), (255, 0, 255), 2) run_x_avg = lines_drawn_new[i][0] run_y = lines_drawn_new[i][1] count = 1 if consider == False and right_theta_avg != None: y_intersect = slop_right * run_x_avg + intercept_right if abs(y_intersect - run_y) <= consider_thresh: consider = True run_y = y_intersect pts_list.append([run_x_avg, run_y, "r"]) """ To plot the magenta lines """ cv2.line(cv_image, (int(run_x_avg), 0), (int(run_x_avg), int(run_y)), (255, 0, 255), 2) run_x_avg = lines_drawn_new[i][0] run_y = lines_drawn_new[i][1] count = 1 consider = False if left_theta_avg != None: y_intersect = slop_left * run_x_avg + intercept_left if abs(y_intersect - run_y) <= consider_thresh: consider = True run_y = y_intersect pts_list.append([run_x_avg, run_y, "l"]) """ To plot the magenta lines """ cv2.line(cv_image, (int(run_x_avg), 0), (int(run_x_avg), int(run_y)), (255, 0, 255), 2) if consider == False and right_theta_avg != None: y_intersect = slop_right * run_x_avg + intercept_right if abs(y_intersect - run_y) <= consider_thresh: consider = True run_y = y_intersect pts_list.append([run_x_avg, run_y, "r"]) """ To plot the magenta lines """ cv2.line(cv_image, (int(run_x_avg), 0), (int(run_x_avg), int(run_y)), (255, 0, 255), 2) if len(pts_list) != 0: # sort this pts_list based on second index pts_list.sort(key=lambda x: x[1], reverse=True) prev = pts_list[0] # for i in range(1, len(pts_list)): # cur = pts_list[i] # if abs(cur[1] - prev[1]) <= 2: # # draw a line # cv2.line(cv_image, (prev[0], prev[1]), (cur[0], cur[1]), (0, 100, 100), 2) # prev = cur """ This is for plotting the yellow line connecting ends of the vertical pink lines""" for i in range(0, 1): cur = pts_list[i] prev = (0, int(cur[1])) next = (400, int(cur[1])) # for plotting line with recently acquired information cv2.line(cv_image, prev, next, (255, 0, 0), 2) cv_image = update_blocks(cv_image, cur[0], cur[1], cur[2]) # lets also put text about current block length info to debug cv_image = puttext(cv_image, str(len(block_array))) cv2.namedWindow("HoughLines", cv2.WINDOW_NORMAL) cv2.resizeWindow("HoughLines", 800, 800) cv2.imshow("HoughLines", cv_image) cv2.imshow("masked", masked_img) cv2.waitKey(1)
class LidarImage: def __init__(self): self._imageInput = message_filters.Subscriber(image_topic_name, Image) self._velodyne = message_filters.Subscriber(lidar_topic_name, PointCloud2) self._pub_img_depth = rospy.Publisher("/image_fusion", Image, queue_size=1) self._ts = message_filters.ApproximateTimeSynchronizer( [self._velodyne, self._imageInput], 1, 0.05) self._ts.registerCallback(self.lidar_camera_fusion) def lidar_camera_fusion(self, point_cloud_data, image_data): time_start_all = time.time() try: self.cvImage = CvBridge().imgmsg_to_cv2(image_data, 'bgr8') except CvBridgeError as e: print(e) image_depth = [] image_depth = self.cvImage.copy() height, width = image_depth.shape[:2] time_start_transform = time.time() pointXYZ_raw = pointcloud2_to_xyz_array(point_cloud_data, remove_nans=True) pointXYZ = pointXYZ_raw.copy() alpha = 90 - 0.5 * the_field_of_view k = math.tan(alpha * math.pi / 180.0) if the_view_number == 1: pointXYZ = pointXYZ[np.logical_and( (pointXYZ[:, 0] > k * pointXYZ[:, 1]), (pointXYZ[:, 0] > -k * pointXYZ[:, 1]))] elif the_view_number == 2: pointXYZ = pointXYZ[np.logical_and( (-pointXYZ[:, 1] > k * pointXYZ[:, 0]), (-pointXYZ[:, 1] > -k * pointXYZ[:, 0]))] elif the_view_number == 3: pointXYZ = pointXYZ[np.logical_and( (-pointXYZ[:, 0] > k * pointXYZ[:, 1]), (-pointXYZ[:, 0] > -k * pointXYZ[:, 1]))] elif the_view_number == 4: pointXYZ = pointXYZ[np.logical_and( (pointXYZ[:, 1] > k * pointXYZ[:, 0]), (pointXYZ[:, 1] > -k * pointXYZ[:, 0]))] pointXYZ = pointXYZ[np.logical_and( (pointXYZ[:, 0]**2 + pointXYZ[:, 1]**2 > the_min_distance**2), (pointXYZ[:, 0]**2 + pointXYZ[:, 1]**2 < the_max_distance**2))] pointXYZ = pointXYZ[np.logical_and( (pointXYZ[:, 2] > the_view_lower_limit - the_sensor_height), (pointXYZ[:, 2] < the_view_higher_limit - the_sensor_height))] cloud_xyz = calib.lidar_to_cam.dot(pointXYZ.T).T cloud_uv = calib.lidar_to_img.dot(pointXYZ.T).T cloud_uv = np.true_divide(cloud_uv[:, :2], cloud_uv[:, [-1]]) camera_xyz = cloud_xyz[(cloud_uv[:, 0] >= 0) & (cloud_uv[:, 0] < width) & (cloud_uv[:, 1] >= 0) & (cloud_uv[:, 1] < height)] camera_uv = cloud_uv[(cloud_uv[:, 0] >= 0) & (cloud_uv[:, 0] < width) & (cloud_uv[:, 1] >= 0) & (cloud_uv[:, 1] < height)] transform_time_cost = time.time() - time_start_transform time_start_display = time.time() jc = Jet_Color() depth = np.sqrt( np.square(camera_xyz[:, 0]) + np.square(camera_xyz[:, 1]) + np.square(camera_xyz[:, 2])) for pt in range(0, camera_uv.shape[0]): cv_color = jc.get_jet_color(depth[pt] * jet_color) cv2.circle(image_depth, (int(camera_uv[pt][0]), int(camera_uv[pt][1])), 1, cv_color, thickness=-1) display_time_cost = time.time() - time_start_display try: self._pub_img_depth.publish(CvBridge().cv2_to_imgmsg( image_depth, 'bgr8')) except CvBridgeError as e: print(e) total_time_cost = time.time() - time_start_all print(pointXYZ_raw.shape, camera_xyz.shape, camera_uv.shape) print("transform time cost:", transform_time_cost) print("display time cost:", display_time_cost) print("total time cost: ", total_time_cost) print()
def img_callback(msg): start = time.time() img = CvBridge().imgmsg_to_cv2(msg, "bgr8").copy() seq = msg.header.seq try: #cv2.imwrite("inputs/input_{}.png".format(str(msg.header.seq)), img) detector = LaneDetection([img.copy(), seq]) pre_process = PreProcessImg([img.copy(), seq]) # Pre Process pre_success, processedImg = pre_process.process_image() if pre_success == False: print("\n|| COULD NOT FOUND RED LANE FROM PREPROCESSING IMAGE ||\n") return False # Lane Detection blended_img, out_img, lane_found, path = detector.process_image(processedImg) ########################################### output contatenation out_img = cv2.copyMakeBorder( out_img, (img.shape[0] - out_img.shape[0])/2, (img.shape[0] - out_img.shape[0])/2, (img.shape[1] - out_img.shape[1])/2, (img.shape[1] - out_img.shape[1])/2, cv2.BORDER_CONSTANT, value=(128,128,128) ) comb = np.concatenate((blended_img, out_img), axis=0) ########################################### output contatenation if lane_found: pose_list = [] global past_coords past_coords.append([path[1][0], path[1][1], path[0][2]]) for i in range(len(path)): pose = PoseStamped() pose.pose.position.x = path[i][0] pose.pose.position.y = path[i][1] pose.pose.position.z = path[i][2] quat = tf.transformations.quaternion_from_euler(0,0, np.deg2rad(path[i][2])) pose.pose.orientation.x = quat[0] pose.pose.orientation.y = quat[1] pose.pose.orientation.z = quat[2] pose.pose.orientation.w = quat[3] pose_list.append(pose) path_msg = Path() path_msg.poses = pose_list path_pub.publish(path_msg) org1 = (10, comb.shape[1]+35) org2 = (10, comb.shape[1]+55) org3 = (10, comb.shape[1]+75) org4 = (10, comb.shape[1]+15) # Using cv2.putText() method cv2.putText(comb, "x:{0}, y:{1}, yaw:{2}".format(path[0][0], path[0][1], round(path[0][2], 1)), org1, cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255), 1, cv2.LINE_AA) cv2.putText(comb, "x:{0}, y:{1}, yaw:{2}".format(path[1][0], path[1][1], round(path[1][2], 1)), org2, cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 1, cv2.LINE_AA) cv2.putText(comb, "x:{0}, y:{1}, yaw:{2}".format(path[2][0], path[2][1], round(path[2][2], 1)), org3, cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 0, 0), 1, cv2.LINE_AA) cv2.putText(comb, "Lane Found: {}".format(lane_found), org4, cv2.FONT_HERSHEY_SIMPLEX, 0.5, (120, 180, 75), 1, cv2.LINE_AA) else: past_coords = [] cv2.putText(comb, "LANE IS NOT FOUND !!!".format(lane_found), (10, comb.shape[1]+15), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (120, 180, 75), 1, cv2.LINE_AA) cv2.imshow("Lane Detection", comb) cv2.waitKey(1) img_pub.publish(CvBridge().cv2_to_imgmsg(comb, "bgr8")) pr_img_pub.publish(CvBridge().cv2_to_imgmsg(processedImg)) blended_img_pub.publish(CvBridge().cv2_to_imgmsg(blended_img, "bgr8")) tf_img_pub.publish(CvBridge().cv2_to_imgmsg(out_img, "bgr8")) #cv2.imwrite("outputs/output_{}.png".format(str(msg.header.seq)), comb) except Exception as e: print("ERROR!") print(traceback.format_exc()) img_pub.publish(CvBridge().cv2_to_imgmsg(img.copy(), "bgr8")) #quit() return False t = time.time() - start f = 1/t print("time : {0}, frequency: {1}".format(t, f))
def detect2(self, req): rospy.loginfo("Try to detect objects...") HM = rospy.get_param("~hmatrix") XM = rospy.get_param("~xmatrix") CM = rospy.get_param("cameraMatrix") EM = rospy.get_param("eyebase") image_params = rospy.get_param("~image") H = HM['data'] X = XM['data'] C = CM['data'] E = EM['data'] #C = [592.988765, 0.0, 316.144026, 0.0, 589.679756, 244.158662,0.0, 0.0, 1.0] #E = [0.9999262927350819, 0.012140295651447686, 0.00014939744324133248,0.0362733301805, 0.012138358976642286, -0.9998822784140743, 0.009385603649183276, 0.289040732468, 0.0002633238591056371,-0.009383098422207622, -0.9999559430922794,0.83550686443 , 0.0, 0.0, 0.0, 1.0] giraffeObjList = [] duckObjList = [] barrotObjList = [] res = DetectObjectSrvResponse.NOT_DETECTED imageData = rospy.wait_for_message('/camera/color/image_raw', Image) try: cv_image = CvBridge().imgmsg_to_cv2(imageData, "bgr8") except CvBridgeError as e: print(e) image_np_expanded = np.expand_dims(cv_image, axis=0) image_tensor = self.graph.get_tensor_by_name('image_tensor:0') boxes = self.graph.get_tensor_by_name('detection_boxes:0') scores = self.graph.get_tensor_by_name('detection_scores:0') classes = self.graph.get_tensor_by_name('detection_classes:0') num_detections = self.graph.get_tensor_by_name('num_detections:0') (boxes, scores, classes, num_detections) = self.sess.run( [boxes, scores, classes, num_detections], feed_dict={image_tensor: image_np_expanded}) imagedeal = cv_image.copy() imagecopy = cv_image.copy() imagecopyp = cv_image.copy() vis_util.visualize_boxes_and_labels_on_image_array( imagecopy, np.squeeze(boxes), np.squeeze(classes).astype(np.int32), np.squeeze(scores), self.category_index, use_normalized_coordinates=True, line_thickness=3) box_to_color_map, str_to_display = vis_util.need_location( imagecopy, np.squeeze(boxes), np.squeeze(classes).astype(np.int32), np.squeeze(scores), self.category_index, use_normalized_coordinates=True, line_thickness=3) cv2.imwrite("jiance.jpg", imagecopy) gray_image = cv2.cvtColor(imagedeal, cv2.COLOR_BGR2GRAY) gray_image = cv2.GaussianBlur(gray_image, (5, 5), 0) ret3, th3 = cv2.threshold(gray_image, 0, 255, cv2.THRESH_BINARY + cv2.THRESH_OTSU) cv2.imwrite("beijing.jpg", th3) sp = imagecopy.shape for box, color in box_to_color_map.items(): ymin, xmin, ymax, xmax = box x = int((xmax + xmin) * sp[1] / 2.0) y = int((ymax + ymin) * sp[0] / 2.0) cv2.circle(imagecopy, (x, y), 2, (0, 255, 0), 3) label = str_to_display[box] caijian = th3[int(ymin * sp[0]):int(ymax * sp[0]), int(xmin * sp[1]):int(xmax * sp[1])] if (label[0][0] == 'p'): kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (26, 26)) kernel2 = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (10, 10)) else: kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (6, 6)) kernel2 = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (20, 20)) caijian = cv2.dilate(caijian, kernel) caijian = cv2.erode(caijian, kernel2) contours, hier = cv2.findContours(caijian, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) M = cv2.moments(contours[0]) for cnt in contours: N = cv2.moments(cnt) if M['m00'] < N['m00']: M = cv2.moments(cnt) #print(M['m00']) centroid_xD = M['m10'] / M['m00'] + xmin * sp[1] centroid_yD = M['m01'] / M['m00'] + ymin * sp[0] centroid_x = int(M['m10'] / M['m00'] + xmin * sp[1]) centroid_y = int(M['m01'] / M['m00'] + ymin * sp[0]) xc = (C[4] * centroid_xD - centroid_yD * C[1] - C[2] * C[4] + C[1] * C[5]) / (C[4] * C[0]) yc = (centroid_yD - C[5]) / C[4] zc = 1 objPose = Pose() objPose.position.x = E[0] * xc + E[1] * yc + E[2] * zc + E[3] objPose.position.y = E[4] * xc + E[5] * yc + E[6] * zc + E[7] print("objPose.position.x,", objPose.position.x) print("objPose.position.y", objPose.position.y) if label[0][0] == 'p': barrotObjList.append(objPose) elif label[0][0] == 'd': duckObjList.append(objPose) else: giraffeObjList.append(objPose) res = DetectObjectSrvResponse.SUCCESS cv2.circle(imagecopy, (centroid_x, centroid_y), 2, (0, 255, 255), 3) cv2.imwrite("i.jpg", imagecopy) return DetectObjectSrvResponse(res, giraffeObjList, duckObjList, barrotObjList)
class ImageOverlay: def __init__(self): self.i = 0.0 self.temp = "" self.number = 0 self.asteering_raw = 0 self.max_steering = 1 self.min_steering = -1 self.epsilon_steering = math.radians(0.001) self.img = np.ones( (700, 1000, 3), dtype=np.uint8) * 128 # default black background # update the image to the new image from camera feed def image_callback(self, data): self.img = CvBridge().compressed_imgmsg_to_cv2(data) def overlay(self): img_out = self.img.copy() return img_out def call_back2(self, data): ack_cmd = AckermannDriveStamped() ack_cmd.header.stamp = rospy.Time.now() drive = AckermannDrive() drive.speed = data.linear.x drive.steering_angle = data.angular.z # impose limits on commanded angle # datetime object containing current date and time now = datetime.now() print(self.i) drive.steering_angle = self.i ack_cmd.drive = drive self.publisher.publish(ack_cmd) def run(self): rospy.init_node("get_img") #self.publisher = rospy.Publisher('overlay/compressed', CompressedImage, queue_size=1) # Read in camera feed, change first arg to read a different camera self.camera_subscriber = rospy.Subscriber( '/camera/left/image_raw/compressed', CompressedImage, self.image_callback, queue_size=1) self.publisher = rospy.Publisher('/robot_control/command', AckermannDriveStamped, queue_size=10) self.s = rospy.Subscriber("/cmd_vel", Twist, self.call_back2) rate = rospy.Rate(3) # default 3 Hz #print("Running camera overlay in overlay/compressed") graph = tf.Graph() with graph.as_default(): with tf.Session(graph=graph) as sess: with tf.gfile.FastGFile('./model_tf.pb', 'rb') as model_file: graph_def = tf.GraphDef() graph_def.ParseFromString(model_file.read()) # init # choose ROI roi_x = 66 roi_y = 200 input_var = tf.placeholder("float32", [1, roi_x, roi_y, 3]) [output_image ] = tf.import_graph_def(graph_def, input_map={'input_1:0': input_var}, return_elements=['output/Sigmoid:0'], name='') while True: startTime = time.time() image = self.overlay() # from 1 dim to 3 image = (np.reshape(image, (height, width, 3))).astype(float) # resize - cv2. we don't need crop (did it before) image_BGR = cv2.resize(image, (roi_y, roi_x)) # convert to RGB image = image_BGR[..., ::-1] # normalization image /= 255.0 img = np.expand_dims(image, axis=0) # inference pred = sess.run(output_image, feed_dict={input_var: img}) steering_pred = 1 * ( (max(0, int( ((((pred[0][0]) - 0.5) * 2) + 0.5) * 255)) - 0) * (3 - -3) / (255 - 0) + -3) self.i = steering_pred