def main(): # Open TCP connection to robot client = TCPClient() # Startup realsense pipeline pipeline = rs.pipeline() #Create a config and configure the pipeline to stream # different resolutions of color and depth streams config = rs.config() config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 60) config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 60) # Start streaming profile = pipeline.start(config) # Getting the depth sensor's depth scale (see rs-align example for explanation) depth_sensor = profile.get_device().first_depth_sensor() depth_scale = depth_sensor.get_depth_scale() print("Depth Scale is: " , depth_scale) # Create an align object # rs.align allows us to perform alignment of depth frames to others frames # The "align_to" is the stream type to which we plan to align depth frames. align_to = rs.stream.color align = rs.align(align_to) # Declare depth filters dec_filter = rs.decimation_filter() # Decimation - reduces depth frame density spat_filter = rs.spatial_filter() # Spatial - edge-preserving spatial smoothing hole_filling = rs.hole_filling_filter() temp_filter = rs.temporal_filter() # Temporal - reduces temporal noise depth_to_disparity = rs.disparity_transform(True) disparity_to_depth = rs.disparity_transform(False) # initialize variables for trajectory calculation buffer_len = 30 #number of points that will be taken into account for trajectory calculation pts = deque(maxlen=buffer_len) camera_pts = deque(maxlen=buffer_len) time_vec = deque(maxlen=buffer_len) tic = time.time() tic_frame = None none_count = 0 preditcion_store = [] points_store = [] catch_point = [] # loop for video while True: # Wait for frames from realsense frames = pipeline.wait_for_frames() # Align the depth frame to color frame aligned_frames = align.process(frames) # Get aligned frames aligned_depth_frame = aligned_frames.get_depth_frame() # aligned_depth_frame is a 640x480 depth image color_frame = aligned_frames.get_color_frame() # Filter aligned depth frame #aligned_depth_frame = dec_filter.process(aligned_depth_frame) aligned_depth_frame = depth_to_disparity.process(aligned_depth_frame) aligned_depth_frame = spat_filter.process(aligned_depth_frame) aligned_depth_frame = temp_filter.process(aligned_depth_frame) aligned_depth_frame = disparity_to_depth.process(aligned_depth_frame) aligned_depth_frame = hole_filling.process(aligned_depth_frame) # Get images to work on depth_image = np.asanyarray(aligned_depth_frame.get_data()) color_image = np.asanyarray(color_frame.get_data()) # Validate that both frames are valid if not aligned_depth_frame or not color_frame: print('Frame is none') continue depth_image = np.asanyarray(aligned_depth_frame.get_data()) color_image = np.asanyarray(color_frame.get_data()) # Detect the orange ball and return image with results center,radius = bd.detect_ball(color_image) # update the points queue if center is not None : #get depth from depth_image and append to center, append the current center to the points list depth = depth_image[center[1],center[0]] if not tic_frame: tic_frame = time.time() center.append(depth) camera_pts.append(center) # Transform point from camera coordinates to robot coordinate frame center_world = bte.transform_to_world(center) pts.append(center_world) points_store.append(center_world) #append current time to time vector toc = time.time() time_vec.append(toc-tic) else: none_count = none_count+1 #if no points were detected for some time (10 frames), reset the point vector and polynomial calculation if none_count >10: pts.clear() camera_pts.clear() time_vec.clear() none_count = 0 # if more then x ball positions were detected, calculate the trajectory estimation if(len(pts) > 7): toce = time.time() params_x,params_y,params_z = bte.estimate_trajectory(np.asarray(pts), np.asarray(time_vec)) catch_point = cpc.get_catching_point(params_x,params_y,params_z) #Send catching point to robot if catch_point is not None: client.send_message(np.round(catch_point,2)) print("Processing time:",(time.time()-toce)) print("Sent point: ",np.round(catch_point,2)) catch_point_camera = bte.transform_to_camera(catch_point) cv2.drawMarker(color_image, tuple(catch_point_camera.astype(int)[:2]), (0, 255, 0) ,cv2.MARKER_CROSS,10) # calculate future points for ball from the estimated polynomial parameters and draw them print("Tic frame: ", tic_frame) print("Time now: ", time.time) future_points = bte.get_future_points_3D(params_x,params_y,params_z,tic,time.time(),2) for point in future_points.transpose(): preditcion_store.append(point) camera_point = bte.transform_to_camera(point) cv2.drawMarker(color_image, tuple(camera_point.astype(int)[:2]), (255, 0, 0) ,cv2.MARKER_CROSS,5) # loop over the set of tracked points to draw the balls past movement print("cam points: ", camera_pts) for i in range(1, len(camera_pts)): # compute the thickness of the line and # draw the connecting lines thickness = int(np.sqrt(buffer_len / float(i + 1)) * 2.5) cv2.drawMarker(color_image, (camera_pts[i][0],camera_pts[i][1]), (0, 0, 255), cv2.MARKER_CROSS,10) break # Display results cv2.imshow("Result image", color_image) out.write(color_image) # uncomment to save video key = cv2.waitKey(1) & 0xFF # if the 'q' key is pressed, stop the loop if key == ord("q"): break cv2.imshow("Result image", color_image) del points_store[0] points_store = np.asarray(points_store) preditcion_store = np.asarray(preditcion_store) print("Catching point: ", catch_point) print("points: ", points_store) print('first: ', points_store[:,0]) print('prediction: ',preditcion_store) visualization(points_store[:,0],points_store[:,1],points_store[:,2],preditcion_store[:,0],preditcion_store[:,1] , preditcion_store[:,2],catch_point) # close all windows cv2.destroyAllWindows() client.close()
import Constants def read_cmd_code(): try: return int(input('Execute: ')) except ValueError: print("ERROR: Not a number") print("Example: 1") return read_cmd_code() if __name__ == '__main__': client = TCPClient("localhost", 5001) if client.fail: print("Contolla il server") exit(1) cmd = "" while True: print("Available actions") cn = 1 for cmd in Constants.actions: print(cn.__str__() + " - " + cmd) cn += 1 cmd = read_cmd_code() - 1 client.send_message(Constants.actions[cmd]) if Constants.actions[cmd] == "exit": break client.close_socket() exit(0)