# 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()
