import time
## you need to git clone the PS4Joystick repo and run `sudo bash install.sh`
## Configurable ##
MESSAGE_RATE = 20
PUPPER_COLOR = {"red": 0, "blue": 0, "green": 255}
joystick_pub = Publisher(8830, 65530)
joystick_subcriber = Subscriber(8840, timeout=0.01)
joystick = Joystick()
joystick.led_color(**PUPPER_COLOR)
while True:
print("running")
values = joystick.get_input()
left_y = -values["left_analog_y"]
right_y = -values["right_analog_y"]
right_x = values["right_analog_x"]
left_x = values["left_analog_x"]
L2 = values["l2_analog"]
R2 = values["r2_analog"]
R1 = values["button_r1"]
L1 = values["button_l1"]
square = values["button_square"]
x = values["button_cross"]
circle = values["button_circle"]
from PS4Joystick import Joystick
import time
## you need to git clone the PS4Joystick repo and run `sudo bash install.sh`
## Configurable ##
MESSAGE_RATE = 20
PUPPER_COLOR = {"red":255, "blue":255, "green":0}
drive_pub = Publisher(8830)
j = Joystick()
while True:
print("running")
values = j.get_input()
j.led_color(**PUPPER_COLOR)
forward_left = - values['left_analog_y']
forward_right = - values['right_analog_y']
twist_right = values['right_analog_x']
twist_left = values['left_analog_x']
L2 = values['l2_analog']
R2 = values['r2_analog']
on_right = values['button_r1']
on_left = values['button_l1']
square = values['button_square']
x = values['button_cross']
class Control(object):
STATES = ['off', 'rest', 'meander', 'goto', 'avoid']
SCREEN_MID_X = 150
def __init__(self, target='tennis_ball'):
self.timer = RepeatTimer(1 / MESSAGE_RATE, self._step)
self.control_state = ControllerState()
self.pos = PositionTracker(self.control_state)
self.obj_sensors = ObjectSensors()
self.active = False
self.walking = False
self.user_stop = False
self.cmd_pub = Publisher(CMD_PUB_PORT)
self.cmd_sub = Subscriber(CMD_SUB_PORT)
self.cv_sub = Subscriber(CV_SUB_PORT)
self.joystick = Joystick()
self.joystick.led_color(**PUPPER_COLOR)
self.state = 'off'
self.target = target
self.current_target = None
self.pusher_client = PusherClient()
self._last_sensor_data = (None, None, None)
def move_forward(self, vel=ControllerState.LEFT_ANALOG_Y_MAX):
self.control_state.left_analog_y = vel
def move_backward(self, vel=-ControllerState.LEFT_ANALOG_Y_MAX):
self.control_state.left_analog_y = vel
def move_stop(self):
self.control_state.left_analog_y = 0
def turn_right(self, rate=ControllerState.RIGHT_ANALOG_X_MAX):
self.control_state.right_analog_x = rate
def turn_left(self, rate=-ControllerState.RIGHT_ANALOG_X_MAX):
self.control_state.right_analog_x = rate
def turn_stop(self):
self.control_state.right_analog_x = 0
def start_walk(self):
if self.walking or not self.active:
return
self.reset()
self.control_state.r1 = True
self.toggle_cmd()
self.walking = True
self.control_state.walking = True
self.reset()
def activate(self):
if self.active:
return
self.reset()
self.control_state.l1 = True
self.toggle_cmd()
self.reset()
self.active = True
self.state = 'rest'
self.walking = False
self.control_state.walking = False
if not self.pos.running:
self.pos.run()
def stop_walk(self):
if not self.walking:
return
self.reset()
self.control_state.r1 = True
self.toggle_cmd()
self.walking = False
self.control_state.walking = False
self.reset()
self.state = 'rest'
def reset(self):
self.control_state.reset()
def run_loop(self):
self.timer.start()
def stop_loop(self):
self.timer.cancel()
self.reset()
self.stop_walk()
self.pos.stop()
self.active = False
self.send_cmd()
self.user_stop = True
def toggle_cmd(self):
# For toggle commands, they should be sent several times to ensure they
# are put into effect
for _ in range(3):
self.send_cmd()
time.sleep(1 / MESSAGE_RATE)
def send_cmd(self):
cmd = self.control_state.get_state()
self.cmd_pub.send(cmd)
try:
msg = self.cmd_sub.get()
self.joystick.led_color(**msg["ps4_color"])
except timeout:
pass
def get_sensor_data(self):
try:
obj = self.obj_sensors.read()
pos = self.pos.data
try:
cv = self.cv_sub.get()
except timeout:
cv = []
except:
obj, pos, cv = self._last_sensor_data
self._last_sensor_data = (obj, pos, cv)
return obj, pos, cv
def _step(self):
js_msg = self.joystick.get_input()
# Force stop moving
if js_msg['button_l2']:
self.user_stop = True
self.stop_walk()
return
# User activate
if js_msg['button_l1']:
self.user_stop = False
self.activate()
return
if self.user_stop or not self.active:
self.reset()
self.send_cmd()
self.send_pusher_message()
return
if not self.walking:
self.start_walk()
return
self.update_behavior()
self.send_cmd()
self.send_pusher_message()
def update_behavior(self):
obj, pos, cv = self.get_sensor_data()
if not any(obj.values()):
self.turn_stop()
if any(obj.values()):
# If object, dodge
self.state = 'avoid'
self.dodge(obj)
elif any([x['bbox_label'] == self.target for x in cv]):
# if target, chase
self.state = 'goto'
self.goto(cv)
else:
# if nothing, wander
self.state = 'meander'
self.meander()
def dodge(self, obj):
'''Takes the object sensor data and adjusts the command to avoid any
objects
'''
if obj['left'] and obj['center']:
self.move_stop()
self.turn_right()
elif (obj['right'] and obj['center']) or obj['center']:
self.move_stop()
self.turn_left()
elif obj['left']:
self.move_forward(vel=ControllerState.LEFT_ANALOG_Y_MAX / 2)
self.turn_right()
elif obj['right']:
self.move_forward(vel=ControllerState.LEFT_ANALOG_Y_MAX / 2)
self.turn_left()
elif not any(obj.values()):
self.turn_stop()
def meander(self):
'''moves forward and makes slight turns left and right to search for a
target -- eventually, for now just move forward
'''
self.current_target = None
self.move_forward()
def goto(self, cv):
'''takes a list of bounding boxes, picks the most likely ball and moves
toward it
'''
tmp = [x for x in cv if x['bbox_label'] == self.target]
if len(tmp) == 0:
self.meander()
conf = np.array([x['bbox_confidence'] for x in tmp])
idx = np.argmax(conf)
best = tmp[idx]
center_x = best['bbox_x'] + best['bbox_w'] / 2
if center_x < self.SCREEN_MID_X:
self.turn_left()
elif center_x > self.SCREEN_MID_X:
self.turn_right()
self.move_forward()
self.current_target = best
def send_pusher_message(self):
obj, pos, cv = self.get_sensor_data()
bbox = self.current_target
timestamp = time.time()
if bbox is None:
bbox = {
'bbox_x': None,
'bbox_y': None,
'bbox_h': None,
'bbox_w': None,
'bbox_label': None,
'bbox_confidence': None
}
message = {
'time': timestamp,
'x_pos': pos['x'],
'y_pos': pos['y'],
'x_vel': pos['x_vel'],
'y_vel': pos['y_vel'],
'x_acc': pos['x_acc'],
'y_acc': pos['y_acc'],
'yaw': pos['yaw'],
'yaw_rate': pos['yaw_rate'],
'left_sensor': obj['left'],
'center_sensor': obj['center'],
'right_sensor': obj['right'],
'state': self.state,
**bbox
}
self.pusher_client.send(message)