def main(): turtle = Turtlebot(pc=True) cv2.namedWindow(WINDOW) while not turtle.is_shutting_down(): # get point cloud pc = turtle.get_point_cloud() if pc is None: continue # mask out floor points mask = pc[:, :, 1] > x_range[0] # mask point too far and close mask = np.logical_and(mask, pc[:, :, 2] > z_range[0]) mask = np.logical_and(mask, pc[:, :, 2] < z_range[1]) if np.count_nonzero(mask) <= 0: continue # empty image image = np.zeros(mask.shape) # assign depth i.e. distance to image image[mask] = np.int8(pc[:, :, 2][mask] / 3.0 * 255) im_color = cv2.applyColorMap(255 - image.astype(np.uint8), cv2.COLORMAP_JET) # show image cv2.imshow(WINDOW, im_color) cv2.waitKey(1)
def main(): turtle = Turtlebot(pc=True) direction = None cv2.namedWindow(WINDOW) cv2.setMouseCallback(WINDOW, click) while not turtle.is_shutting_down(): # get point cloud pc = turtle.get_point_cloud() if pc is None: continue # 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 # empty image image = np.zeros(mask.shape) # assign depth i.e. distance to image image[mask] = np.int8(pc[:, :, 2][mask] / 3.0 * 255) im_color = cv2.applyColorMap(255 - image.astype(np.uint8), cv2.COLORMAP_JET) # show image cv2.imshow(WINDOW, im_color) cv2.waitKey(1) # check obstacle mask = np.logical_and(mask, pc[:, :, 1] > -0.2) data = np.sort(pc[:, :, 2][mask]) state = MOVE if data.size > 50: dist = np.percentile(data, 10) if dist < 0.6: state = ROTATE # command velocity if active and state == MOVE: turtle.cmd_velocity(linear=linear_vel) direction = None elif active and state == ROTATE: if direction is None: direction = np.sign(np.random.rand() - 0.5) turtle.cmd_velocity(angular=direction * angular_vel)
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(): 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