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 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