def load_datum(datum, cam): refer = "\n".join([" ".join(refexp) for refexp in datum['refexp']]) # pprint(datum) n_steps = len(datum['act_deg_list']) image_path = datum['img_src'] waldo_position_lng = datum['gt_longitude'] waldo_position_lat = datum['gt_latitude'] pred_lng = datum['pred_xlng_deg'] pred_lat = datum['pred_ylat_deg'] gt_x, gt_y = get_coordinates(waldo_position_lng, waldo_position_lat) pred_x, pred_y = get_coordinates(pred_lng, pred_lat) print("image_path", image_path) print(refer) print("gt:", waldo_position_lat, waldo_position_lng) print("gt xy:", gt_x, gt_y) print("pred:", pred_lat, pred_lng) print("pred xy:", pred_x, pred_y) print('n_steps:', n_steps) panels = {} panels['original'] = cv2.imread(image_path, cv2.IMREAD_COLOR) panels['grid'] = cv2.imread(grid_path, cv2.IMREAD_COLOR) panels['mask'] = np.zeros((full_h, full_w, 3), dtype='uint8') #add_square(panels['original'], gt_x, gt_y, color=color_green) #add_square(panels['original'], pred_x, pred_y, color=color_red) #add_square(panels['grid'], gt_x, gt_y, color=color_green) #add_square(panels['grid'], pred_x, pred_y, color=color_red) start_loc = datum['act_deg_list'][0][0] start_x, start_y = get_coordinates(start_loc[0], start_loc[1], full_w=full_w, full_h=full_h) start_fov, _ = get_nearest(nodes, start_x, start_y) gt_path = [start_fov] for kk, act_list in enumerate(datum['act_deg_list']): act = act_list[-1] lng, lat = act x, y = get_coordinates(lng, lat, full_w=full_w, full_h=full_h) min_n, _ = get_nearest(nodes, x, y) if gt_path[-1] != min_n: gt_path.append(min_n) if len(gt_path) >= 2: start = gt_path[-2] end = gt_path[-1] intermediate_path = cam.paths[start][end] panels['grid'] = visualize_path(intermediate_path, cam.nodes, cam.edges, panels['grid']) print('intermediate_path:', intermediate_path) print('gt_path:', gt_path) return image_path, gt_path, panels, waldo_position_lng, waldo_position_lat, pred_lng, pred_lat
def process_plan(plan, convbelt): convbelt.env.SetViewer('qtcoin') convbelt.reset_to_init_state_stripstream() for step_idx, step in enumerate(plan): # todo finish this visualization script if step[0].find('pickup')!=-1: obj_name = step[1][0] grasp = step[1][1] pick_base_pose = step[1][2] convbelt.apply_two_arm_pick_action_stripstream((pick_base_pose, grasp), convbelt.env.GetKinBody(obj_name)) else: place_obj_name = step[1][0] place_base_pose = step[1][4] path = step[1][5] visualize_path(convbelt.robot, path) action = {'base_pose': place_base_pose} obj = convbelt.env.GetKinBody(place_obj_name) two_arm_place_object(obj, convbelt.robot, action)
def process_plan(plan, namo): namo.env.SetViewer('qtcoin') namo.reset_to_init_state_stripstream() for step_idx, step in enumerate(plan): # todo finish this visualization script import pdb;pdb.set_trace() print "Executing operator ", step[0] if step[0] == 'pickup': obj_name = step[1][0] grasp = step[1][1] pick_base_pose = step[1][2] g_config = step[1][3] namo.apply_two_arm_pick_action_stripstream((pick_base_pose, grasp, g_config), namo.env.GetKinBody(obj_name)) elif step[0] == 'movebase': q_init = step[1][0] q_goal = step[1][1] path = step[1][2] visualize_path(namo.robot, path) set_robot_config(q_goal, namo.robot) elif step[0] == 'movebase_with_object': q_init = step[1][4] q_goal = step[1][5] path = step[1][6] set_robot_config(q_init, namo.robot) visualize_path(namo.robot, path) set_robot_config(q_goal, namo.robot) else: place_obj_name = step[1][0] place_base_pose = step[1][4] path = step[1][-1] visualize_path(namo.robot, path) action = {'base_pose': place_base_pose} obj = namo.env.GetKinBody(place_obj_name) two_arm_place_object(obj, namo.robot, action)
starting_points_in_squares_list = utils.find_starting_point_for_square( bucket_points_list) square_results_list = algorithms.square_fix_for_first_path( starting_points_in_squares_list, bucket_points_list) point_index_dict_all = utils.load_file_to_dict(INPUT_FILE_PATH) indices_list_all, tabu_list_all = algorithms_utils.calculate_square_fix_result_first( bucket_points_list, square_results_list, point_index_dict_all) points_list_all = utils.load_file_to_list(INPUT_FILE_PATH) path_len, edges_len_list = algorithms_utils.calculate_path_length_based_on_indices_path_list( indices_list_all, points_list_all) ### second path ### square_results_list_second = algorithms.square_fix_for_second_path( starting_points_in_squares_list, bucket_points_list, tabu_list_all) indices_list_all_second = algorithms_utils.calculate_square_fix_result_second( bucket_points_list, square_results_list_second, point_index_dict_all) path_len_second, edges_len_list_second = algorithms_utils.calculate_path_length_based_on_indices_path_list( indices_list_all_second, points_list_all) end_time = time.time() utils.save_paths_to_file(indices_list_all, indices_list_all_second, OUTPUT_FILE_PATH_SQUARES) utils.save_edges_len_to_file(edges_len_list, edges_len_list_second, EDGES_OUTPUT_FILE_PATH_SQUARES) print("Squares first path length: ", path_len) print("Squares second path length: ", path_len_second) print("Time: ", end_time - start_time) points_list = utils.load_file_to_list(INPUT_FILE_PATH) utils.visualize_path(indices_list_all, points_list, grid=True) utils.visualize_path(indices_list_all_second, points_list, grid=True)
import algorithms import utils import time from config import * points_list = utils.load_file_to_list(INPUT_FILE_PATH) point_index_dict = utils.load_file_to_dict(INPUT_FILE_PATH) starting_point1 = utils.draw_starting_point(points_list) starting_point2 = utils.draw_starting_point(points_list) start_time = time.time() (res1, tabu_list, edges_len_list) = algorithms.nearest_neighbour_alg_for_first_path( starting_point1, points_list, point_index_dict) (res2, edges_len_list_second) = algorithms.nearest_neighbour_alg_for_second_path( starting_point2, points_list, point_index_dict, tabu_list) end_time = time.time() utils.save_paths_to_file(res1[1], res2[1], OUTPUT_FILE_PATH) utils.save_edges_len_to_file(edges_len_list, edges_len_list_second, EDGES_OUTPUT_FILE_PATH) print("Nearest neighbour first path length: ", res1[0]) print("Nearest neighbour second path length: ", res2[0]) print("Time: ", end_time - start_time) points_list = utils.load_file_to_list(INPUT_FILE_PATH) utils.visualize_path(res1[1], points_list) utils.visualize_path(res2[1], points_list)