from turtlebot import Turtlebot turtle = Turtlebot() #turtle.wait_for_rgb_image() #rgb = turtle.get_rgb_image() #print str(rgb.shape) turtle.wait_for_odometry() print "Odometry: ", turtle.get_odometry()
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])
class TurtleClient(threading.Thread): """ Class representing a client which receive turtlebot commands and send turtlebot odometry. """ def __init__(self, host="", port=9000): threading.Thread.__init__(self) self.socket = None self.host = host self.port = port self.turtle = Turtlebot() self.running = True self.start() def run(self): """ Main loop. """ print "TurtleClient: Connecting.." connected = False while not connected: try: self.socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) self.socket.connect((self.host, self.port)) self.socket.settimeout(2) connected = True except socket.error: continue print "TurtleClient: Connected to host." while self.running: try: msg = self.socket.recv(1) if len(msg) == 0: print "Connection to server lost." self.close() elif msg == chr(0): print "Initialize." self.turtle.initialize() self.socket.send(str(self.turtle.getSize())) self.socket.send("\n") elif msg == chr(1): print "Shutdown" self.turtle.shutdown() self.close() elif msg == chr(2): print "Move forward." self.turtle.move(ord(msg)) self.socket.send(self.turtle.get_odometry()) elif msg == chr(3): print "Turn right." self.turtle.move(ord(msg)) self.socket.send(self.turtle.get_odometry()) elif msg == chr(4): print "Turn left." self.turtle.move(ord(msg)) self.socket.send(self.turtle.get_odometry()) elif msg == chr(5): print "Wait." self.turtle.move(ord(msg)) self.socket.send(self.turtle.get_odometry()) else: print "Error: Invalid command!" except socket.timeout: pass except socket.error, error: print error self.close() except KeyboardInterrupt: print "Interrupted." self.close()