for curr_zumy_ID in zumy_ID: goal_pos_for_srv[curr_zumy_ID] = Pose2D() goal_pos_for_srv[curr_zumy_ID].x = new_final_destination[ii,0] goal_pos_for_srv[curr_zumy_ID].y = new_final_destination[ii,1] goal_pos_for_srv[curr_zumy_ID].theta = new_final_destination[ii,2] ii = ii + 1 while final_dest_counter<5: zumy_reach_num = 0 for curr_zumy_ID in zumy_ID: latest_zumy_pos_cc[curr_zumy_ID] = cc.Point2D(zumy_monitor[curr_zumy_ID].position.x, zumy_monitor[curr_zumy_ID].position.y) goal_pos_for_cc[curr_zumy_ID] = cc.Point2D(goal_pos_for_srv[curr_zumy_ID].x, goal_pos_for_srv[curr_zumy_ID].y) zumy_vector_cc[curr_zumy_ID] = cc.Vector2D(latest_zumy_pos_cc[curr_zumy_ID], goal_pos_for_cc[curr_zumy_ID]) permission_result = cc.command_n_zumys(zumy_vector_cc, zumy_ID, infl_radius) for curr_zumy_ID in zumy_ID: zumy_move_premission[curr_zumy_ID] = permission_result[1][curr_zumy_ID] # cct.plot_bounding_boxes(permission_result[0].values(), # cc.Vector2D(latest_zumy_pos_cc[zumy_ID[0]], goal_pos_for_cc[zumy_ID[0]]), # cc.Vector2D(latest_zumy_pos_cc[zumy_ID[1]], goal_pos_for_cc[zumy_ID[1]]), # cc.Vector2D(latest_zumy_pos_cc[zumy_ID[2]], goal_pos_for_cc[zumy_ID[2]]), # cc.Vector2D(latest_zumy_pos_cc[zumy_ID[3]], goal_pos_for_cc[zumy_ID[3]]), # infl_radius) cct.plot_bounding_boxes(permission_result[0].values(), zumy_vector_cc.values(), infl_radius) for curr_zumy_ID in zumy_ID: #move_permission_pub[curr_zumy_ID].publish(zumy_move_premission[curr_zumy_ID]) is_goal_reached = send_loc_req_stat(curr_zumy_ID, goal_pos_for_srv[curr_zumy_ID]) if is_goal_reached:
v4 = cc.Vector2D(cc.Point2D(3,0), cc.Point2D(1,1))
print command_tuple[1]
if(run_plot > 0):
plot_n_bounding_boxes(command_tuple[0], [v1,v2,v3,v4], 0.125)
time.sleep(1)
# Test Command command_n_zumys
print("Testing n zumy assignment code")
v1 = cc.Vector2D(cc.Point2D(0,0), cc.Point2D(0,1))
v2 = cc.Vector2D(cc.Point2D(1,0), cc.Point2D(1,1))
v3 = cc.Vector2D(cc.Point2D(2,0), cc.Point2D(2,1))
v4 = cc.Vector2D(cc.Point2D(3,0), cc.Point2D(3,1))
zumy_vect_dict= {'z1':v1, 'z2':v2, 'z3':v3, 'z4':v4}
zumy_prior_list= ['z4', 'z3', 'z2', 'z1']
command_tuple = cc.command_n_zumys(zumy_vect_dict, zumy_prior_list, 0.125)
print command_tuple[1]
if(run_plot > 0):
plot_n_bounding_boxes(command_tuple[0].values(), [v1,v2,v3,v4], 0.125)
time.sleep(1)
v1 = cc.Vector2D(cc.Point2D(0,0), cc.Point2D(2,2))
v2 = cc.Vector2D(cc.Point2D(1,0), cc.Point2D(1,1))
v3 = cc.Vector2D(cc.Point2D(2,0), cc.Point2D(2,1))
v4 = cc.Vector2D(cc.Point2D(3,0), cc.Point2D(1,1))
zumy_vect_dict= {'z1':v1, 'z2':v2, 'z3':v3, 'z4':v4}
zumy_prior_list= ['z2', 'z3', 'z4', 'z1']
command_tuple = cc.command_n_zumys(zumy_vect_dict, zumy_prior_list, 0.125)
print command_tuple[1]
if(run_plot > 0):
plot_n_bounding_boxes(command_tuple[0].values(), [v1,v2,v3,v4], 0.125)
