def bck():
cont = True
current_target = Target()
current_target.local_velocity = (-speed_linear,
current_target.local_velocity[1])
data = pickle.dumps(current_target)
clientsocket.send(data)
def rgt():
cont = True
current_target = Target()
current_target.local_velocity = (current_target.local_velocity[0],
speed_linear)
data = pickle.dumps(current_target)
clientsocket.send(data)
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 __del__(self):
print("Stoping Client")
if self.HLC is not None:
self.HLC.set_target(Target())
self.loop_flag = False
self.loop_handle.join()
super().__del__()
def command_set_target(self, target, expect_response=False, log=False):
success = super().command_set_target(target,
expect_response=expect_response,
log=log)
if not success:
target = Target() #Create empty target
self.HLC.set_target(target)
if expect_response:
response = success
self.send_data(response)
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
import rp1controller from rp1controller import Target from time import sleep #Test script to find problem class hlc = rp1controller.RP1Controller() #hlc.localisation.loop_run_flag = False t0 = Target((0,0),0) t1 = Target((1,0),0) t2 = Target((0,-0.5),0) delay = 1 hlc.set_target(t1) sleep(delay) hlc.set_target(t2) sleep(delay) hlc.set_target(t1) sleep(delay) hlc.set_target(t2) sleep(delay) hlc.set_target(t1) sleep(delay) hlc.set_target(t2) sleep(delay) hlc.set_target(t1) sleep(delay) for i in range(0,1000):
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
def handle_timeout(self):
self.loop_flag = False
self.HLC.set_target(Target())
#TODO Should kill odrives and stop vehicle
print("Watchdog expired on client")
return
class GamepadInput():
axis_FB = "ABS_Y"
axis_LR = "ABS_X"
axis_rot = "ABS_RX"
axis_deadzone = 0.2
button_UP = "BTN_TR"
button_DN = "BTN_TL"
button_VAR = "BTN_EAST" #TODO replace with custom function
button_BRK = "BTN_SOUTH"
button_MODE = "BTN_WEST"
button_ADVANCE = "BTN_NORTH"
button_PRP = "ABS_HAT0Y"
state_PRP_UP = -1
state_PRP_DN = 1
button_INT = "ABS_HAT0X"
state_INT_UP = 1
state_INT_DN = -1
speed_limit_rot = 1
speed_limit_linear = 1
speed_limit_step = 0.2
gamepad_loop_flag = False
gamepad_loop_handle = None
control_loop_flag = False
control_loop_handle = None
target = Target()
target_updated = False
control_modes = [
LocalVelocityControl, WorldVelocityControl, WorldPoseControl
]
control_mode_index = 0
variable_func = lambda a: print("No variable function set")
emergency_brake_func = lambda a: print("No additional brake function set")
def __init__(self, server: RP1Server, log=False):
self.server: RP1Server = server
self.log = log
self.gamepad_loop_handle = threading.Thread(
target=self.listen_to_gamepad,
daemon=False) #TODO might need to be a daemon
self.gamepad_loop_flag = True
self.gamepad_loop_handle.start()
self.control_loop_handle = threading.Thread(target=self.control_loop,
daemon=True)
self.control_loop_flag = True
self.control_loop_handle.start()
pass
def __del__(self):
print("Stopping gamepad control")
self.control_loop_flag = False
self.gamepad_loop_flag = False
if self.control_loop_handle is not None:
self.control_loop_handle.join()
if self.gamepad_loop_handle is not None:
self.gamepad_loop_handle.join()
def listen_to_gamepad(self):
while self.gamepad_loop_flag:
try:
events = get_gamepad()
except:
print("Gamepad Disconnected")
self.gamepad_loop_flag = False
self.brake()
return
for event in events:
if event.code == self.button_BRK: #Emergency stop
self.gamepad_loop_flag = False
self.brake()
self.emergency_brake_func()
print("EMERGENCY BRAKE")
return
if event.code == self.axis_FB:
self.update_target("axis_FB", event.state)
if event.code == self.axis_LR:
self.update_target("axis_LR", event.state)
if event.code == self.axis_rot:
self.update_target("axis_rot", event.state)
if event.code == self.button_UP and event.state == 1:
self.speed_limit_linear += self.speed_limit_step
if event.code == self.button_DN and event.state == 1:
self.speed_limit_linear += self.speed_limit_step
if event.code == self.button_VAR and event.state == 1: #TODO
self.variable_func()
if event.code == self.button_MODE and event.state == 1:
self.change_mode()
if event.code == self.button_ADVANCE and event.state == 1:
self.get_location()
if event.code == self.button_PRP:
if event.state == self.state_PRP_UP:
pass #self.update_PID("proportional", True)
elif event.state == self.state_PRP_DN:
pass #self.update_PID("proportional", False)
if event.code == self.button_INT:
if event.state == self.state_INT_UP:
pass #self.update_PID("integrator", True)
elif event.state == self.state_INT_DN:
pass #self.update_PID("integrator", False)
def control_loop(
self
): #Sends the updated target 10 times per second so network is not overloaded
while self.control_loop_flag:
sleep(0.1)
self.send_target()
def curve(self, input):
input = input / 32000
if input < 0:
sign = -1
else:
sign = 1
input_unsigned = abs(input)
if input_unsigned < self.axis_deadzone:
output_unsigned = 0
else:
output_unsigned = input_unsigned * 1 / (
1 - self.axis_deadzone) - self.axis_deadzone
if output_unsigned > 1: output_unsigned = 1
output = output_unsigned * sign
return output
def update_target(self, t_axis, value, urgent=False):
speed = self.curve(value)
self.target_updated = True
if t_axis == "axis_FB":
lin_speed = speed * self.speed_limit_linear
LR_speed = self.target.local_velocity[1]
self.target.local_velocity = (lin_speed, LR_speed)
self.target.world_velocity = (lin_speed, LR_speed)
elif t_axis == "axis_LR":
lin_speed = -speed * self.speed_limit_linear
FB_speed = self.target.local_velocity[0]
self.target.local_velocity = (FB_speed, lin_speed)
self.target.world_velocity = (FB_speed, lin_speed)
elif t_axis == "axis_rot":
a_speed = -speed * self.speed_limit_rot
self.target.angular_velocity = a_speed
if urgent:
self.send_target()
def send_target(self):
if self.target_updated:
self.server.command_set_target(self.target, log=self.log)
self.target_updated = False
return
def brake(self):
self.server.command_set_planner(LocalVelocityControl, log=self.log)
self.server.command_set_target(Target(), log=self.log)
return
def change_mode(self):
self.server.command_set_planner(
self.control_modes[self.control_mode_index], log=self.log)
self.control_mode_index += 1
if self.control_mode_index == len(self.control_modes):
self.control_mode_index = 0
#go through array of TMSs, support for pose is not needed. Use for pausing test
return
def get_location(self):
location = self.server.command_get_location(expect_response=True,
log=self.log)
print(location) #TODO maybe improve print format
return
def update_target()-> Target:
target = Target((target_FB,target_LR),target_rot)
target.world_velocity = (target_FB,target_LR)
return target
def rot_rgt():
cont = True
current_target = Target()
current_target.local_angular = speed_radial
data = pickle.dumps(current_target)
clientsocket.send(data)
def reset_target():
while (True):
if ((time.time() - time_last) > 0.2):
target = Target()
data = pickle.dumps(current_target)
clientsocket.send(data)
from typing import no_type_check
from rp1controller import Target
import socket, pickle, threading
import keyboard
import time
from time import sleep
current_target = Target()
speed_linear = 0.5
speed_radial = 0.8
loop = True
cont = False
time_last = time.time()
def send_target(target: Target):
data = pickle.dumps(target)
clientsocket.send(data)
time_last = time.time()
def reset_target():
while (True):
if ((time.time() - time_last) > 0.2):
target = Target()
data = pickle.dumps(current_target)
clientsocket.send(data)
def brake(self):
self.server.command_set_planner(LocalVelocityControl, log=self.log)
self.server.command_set_target(Target(), log=self.log)
return
from rp1controller.trajectory_planners import LocalVelocityControl
from rp1controller import RP1Server
from time import sleep
ip = "192.168.137.1"
print()
print("T - Starting Server")
server = RP1Server(ip)
sleep(10) #TODO add check ready
print("T - Starting test Sequence")
test_target = Target()
test_planner = LocalVelocityControl
test_speed_limit = 1.2
test_accel = 0.3
test_speeds = [(0,0),(1,0),(1,1),(0,0),(1,0),(1,1),(0,0),(1,0),(1,1),(0,0)]
if server.command_set_speed_limit(test_speed_limit, expect_response=True, log=True):
print("T - Speed limit updated successfully")
else:
print("T - Speed limit update failed")
sleep(2)
if server.command_set_acceleration_limit(test_accel, expect_response=True, log=True):
print("T - Accel limit updated successfully")
else:
def stp():
current_target = Target()
data = pickle.dumps(current_target)
clientsocket.send(data)
def shut_down():
current_target = Target()
data = pickle.dumps(current_target)
clientsocket.send(data)
loop = False
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