def main(): turtle = Turtlebot(rgb=True) cv2.namedWindow(WINDOW) while not turtle.is_shutting_down(): # get point cloud image = turtle.get_rgb_image() # wait for image to be ready if image is None: continue # detect markers in the image markers = detector.detect_markers(image) # draw markers in the image detector.draw_markers(image, markers) # show image cv2.imshow(WINDOW, image) cv2.waitKey(1)
from turtlebot import Turtlebot import numpy as np turtle = Turtlebot(pc=True, rgb=True, depth=True) while not turtle.is_shutting_down(): depth = turtle.get_depth_image() rgb = turtle.get_rgb_image() point_cloud = turtle.get_point_cloud() rgb_K = turtle.get_rgb_K() depth_K = turtle.get_depth_K() if rgb is None: continue if depth is None: continue if point_cloud is None: continue if rgb_K is None: continue if depth_K is None: continue np.save('exports/depth.npy', depth) np.save('exports/rgb.npy', rgb) np.save('exports/point_cloud.npy', point_cloud) np.save('exports/rgb_K.npy', rgb_K) np.save('exports/depth_K.npy', depth_K) exit(0)
def main(): maximum = 10 count = 0 speed = 30 s = 0.0001 prev_start = [[0] * 2 for i in range(maximum)] prev_end = [[0] * 2 for i in range(maximum)] circle = [[0] * 2 for i in range(maximum)] turtle = Turtlebot(rgb=True) cv2.namedWindow("OKNO") while not turtle.is_shutting_down(): # get point cloud image = turtle.get_rgb_image() # wait for image to be ready if image is None: continue cv2.rectangle(image, (0, 0), (image.shape[1], int(image.shape[0] / 1.6)), (0, 0, 0), -1) gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) kernel_size = 5 blur_gray = cv2.GaussianBlur(gray, (kernel_size, kernel_size), 0) ret, black = cv2.threshold(blur_gray, 215, 255, cv2.THRESH_BINARY) edges = cv2.Canny(black, 200, 255) rho = 2 # distance resolution in pixels of the Hough grid theta = np.pi / 60 # angular resolution in radians of the Hough grid threshold = 55 # minimum number of votes (intersections in Hough grid cell) min_line_length = 5 # minimum number of pixels making up a line max_line_gap = 40 # maximum gap in pixels between connectable line segments line_image = np.copy(image) * 0 # creating a blank to draw lines on lines = cv2.HoughLinesP(edges, rho, theta, threshold, np.array([]), min_line_length, max_line_gap) left_line_x = [] left_line_y = [] right_line_x = [] right_line_y = [] Rmotor = 0 Lmotor = 0 if lines is not None: for line in lines: for x1, y1, x2, y2 in line: cv2.line(line_image, (x1, y1), (x2, y2), (0, 0, 255), 1) if (x2 - x1) == 0: continue slope = (y2 - y1) / (x2 - x1) # <-- Calculating the slope. if math.fabs(slope) < 0.5: # <-- Only consider extreme slope continue if slope <= 0: # <-- If the slope is negative, left group. left_line_x.extend([x1, x2]) left_line_y.extend([y1, y2]) else: # <-- Otherwise, right group. right_line_x.extend([x1, x2]) right_line_y.extend([y1, y2]) min_y = image.shape[0] / 1.3 # <-- Just below the horizon max_y = image.shape[0] # <-- The bottom of the image left_x_start = 0 left_x_end = 0 right_x_start= image.shape[1] right_x_end = image.shape[1] if len(left_line_x) != 0: poly_left = np.poly1d(np.polyfit( left_line_y, left_line_x, deg=1 )) left_x_start = int(poly_left(max_y)) left_x_end = int(poly_left(min_y)) cv2.line(line_image, (int(left_x_start), int(max_y)), (int(left_x_end), int(min_y)), (0, 255, 0), 2) if len(right_line_x) != 0: poly_right = np.poly1d(np.polyfit( right_line_y, right_line_x, deg=1 )) right_x_start = int(poly_right(max_y)) right_x_end = int(poly_right(min_y)) cv2.line(line_image, (int(right_x_start), int(max_y)), (int(right_x_end), int(min_y)), (0, 255, 0), 2) prev_start[count][0] = right_x_start prev_start[count][1] = left_x_start prev_end[count][0] = right_x_end prev_end[count][1] = left_x_end # print(prev_start) # print(prev_end) avg_start = [0] * 2 avg_end = [0] * 2 for i in range(maximum): avg_start[0] += prev_start[i][0] avg_start[1] += prev_start[i][1] avg_end[0] += prev_end[i][0] avg_end[1] += prev_end[i][1] avg_start[0] /= maximum avg_start[1] /= maximum avg_end[0] /= maximum avg_end[1] /= maximum center_x_start = int((avg_start[0] + avg_start[1]) / 2) center_x_end = int((avg_end[0] + avg_end[1]) / 2) Rmotor = speed + int( np.arctan2((center_x_start - center_x_end), (max_y - min_y)) * (180.0 / 3.141592653589793238463)) Lmotor = speed + int( np.arctan2((center_x_end - center_x_start), (max_y - min_y)) * (180.0 / 3.141592653589793238463)) Rmotor = 100 if Rmotor > 100 else Rmotor Lmotor = 100 if Lmotor > 100 else Lmotor cv2.line(line_image, (center_x_start, int(max_y)), (center_x_end, int(min_y)), (255, 0, 0), 3) cv2.line(line_image, (int(image.shape[1] / 2), int(max_y)), (int((image.shape[1] / 2 - center_x_start) + center_x_end), int(min_y)), (255, 255, 0), 4) lines_edges = cv2.addWeighted(image, 0.8, line_image, 1, 0) cv2.putText(lines_edges, str(Lmotor), (int(image.shape[1] / 6), 200), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 4); cv2.putText(lines_edges, str(Rmotor), (int(image.shape[1] - image.shape[1] / 6), 200), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 4); circle[count][0], circle[count][1] = line_intersection( ([int(left_x_start), int(max_y)], [int(left_x_end), int(min_y)]), ([int(right_x_start), int(max_y)], [int(right_x_end), int(min_y)])) circleX = 0 circleY = 0 n = 0 for i in range(maximum): if circle[i][0] is None: n += 1 continue circleX += circle[i][0] circleY += circle[i][1] if maximum-n != 0: circleX /= (maximum - n) circleY /= (maximum - n) if circleX != 0 and circleY != 0: uhel = int(np.arctan2((image.shape[1] / 2 - circleX), (max_y - circleY)) * (180.0 / 3.141592653589793238463)) if circleX > image.shape[1] / 2 - 40 and circleX < image.shape[1] / 2 + 40: cv2.circle(lines_edges, (int(circleX), int(circleY)), 8, (0, 255, 0), -1) turtle.cmd_velocity(linear=0.05+s) s += 0.001 else: cv2.circle(lines_edges, (int(circleX), int(circleY)), 8, (0, 0, 255), -1) turtle.cmd_velocity(angular=np.sign(uhel)*0.5) s = 0.0001 cv2.line(lines_edges, (int(image.shape[1] / 2 - (circleX - center_x_start)), int(image.shape[0] - 2)), (int(image.shape[1] / 2), int(image.shape[0]) - 2), (0, 0, 255), 3) cv2.putText(lines_edges, str(uhel), (int(image.shape[1]/2), 200),cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 4); count += 1 if count == maximum: count = 0 cv2.line(lines_edges, (int(image.shape[1] / 2 - 40), 0), (int(image.shape[1] / 2 - 40), int(image.shape[0])), (0, 211, 255), 1) cv2.line(lines_edges, (int(image.shape[1] / 2 + 40), 0), (int(image.shape[1] / 2 + 40), int(image.shape[0])), (0, 211, 255), 1) cv2.imshow('lines', line_image) cv2.imshow('linesedge', lines_edges) # show image cv2.imshow("OKNO", image) cv2.imshow("OKNO1", edges) cv2.waitKey(1)
def main(): Found_first_beacon = False Found_second_beacon = False Found_third_beacon = False Start_caprure_2D = False see_map_with_rotation = False full_rotarion_1 = False full_rotarion_2 = False movment1 = False data = np.empty((2, 1500, 100)) data[:] = np.nan odom = np.empty((1, 3, 100)) odom[:] = np.nan counter = 0 sample_T_2D = 5 C = np.array([[], []]) MOVE = 1 ROTATE = 2 running = True active = True linear_vel = 0.2 angular_vel = 0.4 turtle = Turtlebot(rgb=True, depth=False, pc=True) direction = -1 scale = float(12) grid_width = 150 grid = -1 * np.ones((grid_width, grid_width)) minx_occ = 70 miny_occ = 70 plt.ion() plt.show() turtle.reset_odometry() # get odometry odometry = turtle.get_odometry() if np.array_equal(odometry, [0.0, 0.0, 0.0]): print("Odom reset correctly") else: print("Odom is not zero") return -1 rate = Rate(10) t = get_time() # plt.figure(3) # plt.title('Grid') # plt.imshow(grid.T, interpolation='none') # plt.gca().invert_yaxis() # plt.show() while not turtle.is_shutting_down(): # get RGB img_rgb = turtle.get_rgb_image() # get odometry odometry = turtle.get_odometry() odometry = [odometry[1], odometry[0], odometry[2]] odometry = np.dot(odometry, [[-1, 0, 0], [0, 1, 0], [0, 0, 1]]) # print("Odometry", odometry) # get point cloud pc = turtle.get_point_cloud() if pc is None: continue if img_rgb is not None and pc is not None: # plt.figure(3) # plt.title('Grid') # plt.imshow(grid.T, interpolation='none') # plt.gca().invert_yaxis() # plt.show() # Print 2D and update if get_time() - t > sample_T_2D: A = pcl_to_2d(pc) # get odometry odometry = turtle.get_odometry() odometry = [odometry[1], odometry[0], odometry[2]] odometry = np.dot(odometry, [[-1, 0, 0], [0, 1, 0], [0, 0, 1]]) R = R_from_ang_single(odometry[2]) T = [[odometry[0]], [odometry[1]]] map = np.dot(R, A) + T occ(map * scale, odometry, grid, scale, minx_occ, miny_occ) t = get_time() # # C = np.hstack((C, map)) # # print("shape map= ", C.shape) # # plt.scatter(C[0, :], C[1, :], marker='.') # # plt.scatter(odometry[0], odometry[1]) # # plt.axis('equal') # # plt.ylim(-5, 5) # # plt.xlim(-5, 5) # # plt.pause(0.001) # # show image # cv2.imshow("grid", grid) # cv2.waitKey(1) # Start_caprure_2D = True # Find the 2nd beacon if Found_first_beacon == True and Found_second_beacon == False: turtle.cmd_velocity(angular=0.3) if Found_first_beacon == True and Found_second_beacon == True and see_map_with_rotation == False: turtle.cmd_velocity(angular=0.7) if -1.5 <= odometry[2] <= 0: see_map_with_rotation = True # np.save("/home/ros/Desktop/aroprojectjim/grid.npy", grid) # Make full rotation if 0 < odometry[0] < 2.5 and 2.5 < odometry[1] < 3.4 and ( full_rotarion_1 == False): print("inside if for rotation") t = get_time() print("Full rotation 1 before") while (get_time() - t < 16): turtle.cmd_velocity(angular=0.5) # get RGB img_rgb = turtle.get_rgb_image() # get point cloud pc = turtle.get_point_cloud() markers = np.array(detect_markers(img_rgb)) # draw markers in the image detector.draw_markers(img_rgb, markers) # show image cv2.imshow("markers", img_rgb) cv2.waitKey(1) # Odometry # print(turtle.get_odometry()) if len(markers) != 0: id = markers[0, 1] if id not in id_array and int(id) in range(5, 17): # Print depth # plt.figure(2) # plt.title('depth') # plt.imshow(depth) # plt.show(clear) # find coordinates # get odometry odometry = turtle.get_odometry() odometry = [odometry[1], odometry[0], odometry[2]] odometry = np.dot( odometry, [[-1, 0, 0], [0, 1, 0], [0, 0, 1]]) coordinates_beacon = find_pozition_beacon( pc, markers, odometry) if coordinates_beacon != -1: id_array.append(id) cord_array.append([ coordinates_beacon[0], coordinates_beacon[1] ]) if len(id_array) == 1: print("found first beacon ") Found_first_beacon = True # play sound turtle.play_sound(sound_id=3) if len(id_array) == 2: Found_second_beacon = True # play sound turtle.play_sound(sound_id=3) if len(id_array) == 3: Found_second_beacon = True # play sound turtle.play_sound(sound_id=3) if len(id_array) == 4: Found_second_beacon = True # play sound turtle.play_sound(sound_id=4) return 1 print("Full rotation 1 after") full_rotarion_1 = True # Make full rotation if -3 < odometry[0] < 0 and 2.5 < odometry[1] < 3.4 and ( full_rotarion_2 == False): print("inside if for rotation 2") t = get_time() while (get_time() - t < 16): turtle.cmd_velocity(angular=0.5) # get RGB img_rgb = turtle.get_rgb_image() # get point cloud pc = turtle.get_point_cloud() markers = np.array(detect_markers(img_rgb)) # draw markers in the image detector.draw_markers(img_rgb, markers) # show image cv2.imshow("markers", img_rgb) cv2.waitKey(1) # Odometry # print(turtle.get_odometry()) if len(markers) != 0: id = markers[0, 1] if id not in id_array and int(id) in range(5, 17): # Print depth # plt.figure(2) # plt.title('depth') # plt.imshow(depth) # plt.show(clear) # find coordinates # get odometry odometry = turtle.get_odometry() odometry = [odometry[1], odometry[0], odometry[2]] odometry = np.dot( odometry, [[-1, 0, 0], [0, 1, 0], [0, 0, 1]]) coordinates_beacon = find_pozition_beacon( pc, markers, odometry) if coordinates_beacon != -1: id_array.append(id) cord_array.append([ coordinates_beacon[0], coordinates_beacon[1] ]) if len(id_array) == 1: print("found first beacon ") Found_first_beacon = True # play sound turtle.play_sound(sound_id=3) if len(id_array) == 2: Found_second_beacon = True # play sound turtle.play_sound(sound_id=3) if len(id_array) == 3: Found_second_beacon = True # play sound turtle.play_sound(sound_id=3) if len(id_array) == 4: Found_second_beacon = True # play sound turtle.play_sound(sound_id=4) return 1 full_rotarion_2 = True # Find the 3rd and 4rd beacon if Found_first_beacon == True and Found_second_beacon == True and see_map_with_rotation == True: # mask out floor points mask = pc[:, :, 1] < 0.2 # mask point too far mask = np.logical_and(mask, pc[:, :, 2] < 3.0) if np.count_nonzero(mask) <= 0: continue # check obstacle mask = np.logical_and(mask, pc[:, :, 1] > 0) mask = np.logical_and(mask, pc[:, :, 1] < 0.1) data = np.sort(pc[:, :, 2][mask]) state = MOVE if data.size > 50: dist = np.percentile(data, 10) # get odometry odometry = turtle.get_odometry() odometry = [odometry[1], odometry[0], odometry[2]] odometry = np.dot(odometry, [[-1, 0, 0], [0, 1, 0], [0, 0, 1]]) # print ("Odom before collision function :", odometry) # col = collision(grid, odometry[0]*scale + minx_occ, odometry[1]*scale + miny_occ) # print("Colission free ", col ) # print("dist ", dist) # print("Odom after collision function: ", odometry) if dist < 0.8: state = ROTATE # command velocity if active and state == MOVE: turtle.cmd_velocity(linear=linear_vel) direction = 1 elif active and state == ROTATE: if direction is None: direction = np.sign(np.random.rand() - 0.5) turtle.cmd_velocity(angular=direction * angular_vel) # End of the random walking # get RGB img_rgb = turtle.get_rgb_image() # get point cloud pc = turtle.get_point_cloud() # get odometry b = turtle.get_odometry() a = [b[1], b[0], b[2]] odometry = np.dot(a, [[-1, 0, 0], [0, 1, 0], [0, 0, 1]]) markers = np.array(detect_markers(img_rgb)) # draw markers in the image detector.draw_markers(img_rgb, markers) # show image cv2.imshow("markers", img_rgb) cv2.waitKey(1) # Odometry # print(turtle.get_odometry()) if len(markers) != 0: id = markers[0, 1] if id not in id_array and int(id) in range(5, 17): # Print depth # plt.figure(2) # plt.title('depth') # plt.imshow(depth) # plt.show(clear) # find coordinates # # get odometry # odometry = turtle.get_odometry() # odometry = [odometry[1], odometry[0], odometry[2]] # odometry = np.dot(odometry, [[-1, 0, 0], # [0, 1, 0], # [0, 0, 1]]) coordinates_beacon = find_pozition_beacon( pc, markers, odometry) if coordinates_beacon != -1: id_array.append(id) cord_array.append( [coordinates_beacon[0], coordinates_beacon[1]]) if len(id_array) == 1: print("found first beacon ") Found_first_beacon = True # play sound turtle.play_sound(sound_id=3) if len(id_array) == 2: Found_second_beacon = True # play sound turtle.play_sound(sound_id=3) if len(id_array) == 3: Found_second_beacon = True # play sound turtle.play_sound(sound_id=3) if len(id_array) == 4: Found_second_beacon = True # play sound turtle.play_sound(sound_id=4) return 1 for i in range(len(id_array)): print("Beacon ", id_array[i], cord_array[i][0], -cord_array[i][1])
from turtlebot import Turtlebot, sleep import numpy as np from scipy.io import savemat from datetime import datetime # initialize turlebot turtle = Turtlebot(rgb=True, depth=True, pc=True) # sleep 2 set to receive images sleep(2) # get K, images, and point cloud data = dict() data['K_rgb'] = turtle.get_rgb_K() data['K_depth'] = turtle.get_depth_K() data['image_rgb'] = turtle.get_rgb_image() data['image_depth'] = turtle.get_depth_image() data['point_cloud'] = turtle.get_point_cloud() # save data to .mat file filename = datetime.today().strftime("%Y-%m-%d-%H-%M-%S") + ".mat" savemat(filename, data) print "Data saved in " + filename