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)