Пример #1
0
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)
Пример #2
0
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
Пример #3
0
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
Пример #4
0
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