def get_obs(current_pose): pos, orn = func_pose2posAndorn(current_pose, mapper_scene2z[scene_name]) env.robot.reset_new_pose(pos, orn) obs, _, _, _ = env.step(4) obs_rgb = obs['rgb_filled'] obs_depth = obs['depth'] return obs_rgb.copy(), obs_depth.copy()
## create diff_theta_list from -75 to +75 gap 15 degree
diff_theta_list = []
## -45, -30, -15, 0, 15, 30, 45
for i in range(7):
theta = -3/12.0*math.pi + i * math.pi/12
diff_theta_list.append(theta)
source_node_idx = random.randint(0, num_nodes-1)
x0 = path_finder.nodes_x[source_node_idx]
y0 = path_finder.nodes_y[source_node_idx]
theta0 = pose_theta_list[random.randint(0, 23)]
left_pose = [x0, y0, theta0]
file_addr = '/home/reza/Datasets/GibsonEnv/my_code/visual_servoing/sample_image_pairs'
current_pose = left_pose
pos, orn = func_pose2posAndorn(current_pose, mapper_scene2z[scene_name])
env.robot.reset_new_pose(pos, orn)
obs, _, _, _ = env.step(4)
obs_rgb = obs['rgb_filled']
cv2.imwrite('{}/left_img.png'.format(file_addr), obs_rgb[:,:,::-1])
right_pose_list = []
for i in range(len(theta_list)):
for j in range(len(dist_list)):
#temp_theta = plus_theta_fn(theta0, theta_list[random.randint(0, 6)])
#temp_dist = dist_list[random.randint(0, 6)]
location_theta = plus_theta_fn(theta0, theta_list[i])
location_dist = dist_list[j]
x1 = x0 + location_dist * math.cos(location_theta)
y1 = y0 + location_dist * math.sin(location_theta)
