def advance_course(): if mode != "pose": return global course_index, course, course_rot course_index += 1 if course_index >= len(course): course_index = 0 print(f"Moving to position: {course_index} at: {course[course_index]}, angle: {course_rot[course_index]}") target = Target() target.world_bearing = course_rot[course_index] target.world_point = course[course_index] send_target(target)
def update_terminal_target(HLC): global active, coords, updated, terminal_gain_x, terminal_gain_y, max_dist, target_point, delay, timestart if not updated: return updated = False if active <= 0: return active -= 1 print(f"Target updated: {time.time()}") updated_coords = (coords[0] - target_point[0], coords[1] - target_point[1]) #Get most recent camera coords, apply gain scaled_coords = (updated_coords[0] * terminal_gain_y, updated_coords[1] * terminal_gain_x) #Get position current_pose = HLC.localisation.current_pose #Add some amount to pos based on camera coords targ_x = current_pose.world_x_position + scaled_coords[1] targ_y = current_pose.world_y_position + scaled_coords[0] #Make sure is still in allowed area if targ_x > max_dist: targ_x = max_dist elif targ_x < -max_dist: targ_x = -max_dist if targ_y > max_dist: targ_y = max_dist elif targ_y < -max_dist: targ_y = -max_dist #Move to target = Target() target.world_point = (targ_x, targ_y) HLC.set_target(target) print(f"Time taken: {time.perf_counter() - timestart}s") timestart = time.perf_counter() #Artificial delay? #no return
def repeatability_test(): global pause, server log_setup() server = RP1Server("192.168.137.1") time.sleep(3) gamepad = GamepadInput(server) gamepad.variable_func = pause gamepad.emergency_brake_func = emergency_brake rs = RSLocalisation() time.sleep(1) rs_pose = get_realsense_estimate(rs) rs.update_robot_origin() time.sleep(5) planner_updated = False while not planner_updated: planner_updated = server.command_set_planner(WorldPoseControl, expect_response=True, log=True) time.sleep(1) speed_updated = False while not speed_updated: speed_updated = server.command_set_speed_limit(speed_max, expect_response=True, log=True) time.sleep(1) for acceleration in accelerations: acceleration_updated = False while not acceleration_updated: acceleration_updated = server.command_set_acceleration_limit( acceleration, expect_response=True, log=True) time.sleep(1) for repeat in range(repeats): print() print(f"Acceleration: {acceleration}, Repeat: {repeat}") print() while pause: time.sleep(1) rs.update_robot_origin() odom_reset = False while not odom_reset: odom_reset = server.command_reset_localisation( expect_response=True, log=True) time.sleep(1) for position in positions: print(f"moving to pos: {position}") target = Target() target.world_bearing = 0 target.world_point = position target_sent = False while not target_sent: target_sent = server.command_set_target( target, expect_response=True, log=True) time.sleep(0.5) pose = server.command_get_location() while not check_at_pos(position, pose): #if pose!= False: #print(f"Velocity X{pose.local_x_velocity}, Y{pose.local_y_velocity}") time.sleep(1) pose = server.command_get_location() time.sleep(1) localisation_pose = server.command_get_location() while localisation_pose == False: localisation_pose = server.command_get_location() time.sleep(0.5) rs_pose = get_realsense_estimate(rs) #log_result_test(f"A: {acceleration}, RP1: {[localisation_pose.world_x_position, localisation_pose.world_y_position]}, RS: {rs_pose}") log_result(mass, repeat, acceleration, localisation_pose.world_x_position, localisation_pose.world_y_position, rs_pose[0], rs_pose[1]) actual_pos = get_realsense_estimate(rs, true_position=True) if displacement(actual_pos) > 0.3: pause = True print("Please Recentre Robot!") # print(f"Attempting to re centre, displacement: {displacement(actual_pos):.2f}m, pos: ({actual_pos[0]:.2f},{actual_pos[1]:.2f})") # odom_reset = False # while not odom_reset: # odom_reset = server.command_reset_localisation(expect_response=True, log=True) # time.sleep(1) # centre_pos = (actual_pos[1]/2, actual_pos[0]/2) #TODO Swapped these to test # print(f"Target pos: {centre_pos[0]:.2f},{centre_pos[1]:.2f}") # target = Target() # target.world_bearing = 0 # target.world_point = centre_pos # target_sent = False # while not target_sent: # target_sent = server.command_set_target(target, expect_response=True) # time.sleep(1) # time.sleep(3) # pose = server.command_get_location() # while not check_at_pos(centre_pos, pose): # time.sleep(1) # pose = server.command_get_location() return
from rp1controller import RP1Controller, Target from rp1controller.trajectory_planners import WorldPoseControl robot_platform = RP1Controller() #Initialise the robot platform robot_platform.set_trajectory_planner( WorldPoseControl) #Set the desired planner target_position = Target() #Create a target and set the target position target_position.world_point = (1, 0) robot_platform.set_target( target_position) #Command the platform to move to the target