# ax3 = fig3.add_subplot(111) try: while RUN_ROBOT: print("\n") ## GET VICON DATA vicon.get_state() print('vicon start position: ', vicon.x_v[0:2]) print('vicon start angle: ', vicon.th_r) t_vicon = time.time() vicon_data.append([vicon.x_v[0], vicon.x_v[1], vicon.th_r, t_vicon]) ## GET KINECT DATA AND REPLAN PATH if time.time() - t_plan >= PLAN_TIME or FIRST_LOOP == True: print("Planning...") raw_depth = kinect.get_raw_depth() pcl = kinect.get_point_cloud(raw_depth) # list of points that has to go into path planner depth_map = coordTransform(pcl) world = World(depth_map, local_start, world_size=world_size) if world.bounds[1, 0] > -1: global_start = np.asarray(vicon.x_v[0:2]).flatten() local_dest = getLocalGoal(global_start, x_g, vicon.th_r, world, local_start) x_coords, y_coords, angles, path, path_cost, world, grid_coords = plan(local_start, local_dest,depth_map, world) x_traj, y_traj, th_traj = local2global(x_coords, y_coords, angles,vicon.x_v[0:2], vicon.th_r, world) print('Local Trajectory:') print('Index, X-Coord, Y-Coord, Angle') for i in range(len(x_traj)): print(i, x_coords[i], y_coords[i], angles[i]) print('Global Trajectory') for i in range(len(x_traj)): print(i, x_traj[i], y_traj[i], th_traj[i]) # time.sleep(5)
global_dest = [0, 0] global_angle = 3.0 * math.pi / 4.0 # -math.pi/2 world_size = [30, 15] local_start = [int(world_size[0] / 2.0), 0] for i in range(5): raw_depth = kinect.get_raw_depth() time.sleep(0.1) # for i in range(1): while 1: raw_depth = kinect.get_raw_depth() # raw_depth = np.random.rand(kinect.height,kinect.width)*2047 # raw_depth = cv2.imread('test_depth6_640x480.png',0)*(2047.0/255.0) print(np.max(raw_depth), np.min(raw_depth)) pcl = kinect.get_point_cloud(raw_depth) #plt.imshow(raw_depth) print(pcl.shape) depth_map = coordTransform(pcl) world = World(depth_map, world_size=world_size) if world.bounds[1, 0] > -1: local_dest = getLocalGoal(global_start, global_dest, global_angle, world, local_start) x_coords, y_coords, angles, path, path_cost, world, grid_coords = plan( local_start, local_dest, depth_map, world) global_x_coords, global_y_coords, global_angles = local2global( x_coords, y_coords, angles, global_start, global_angle, world) ax1.cla() ax2.cla()