Fixup joystick (#21129)
* some common changes * rename to joystick * add alert and update controlsd to work with joystick * update joystickd * Update Joystick readme * assume we have inputs * only send gb or steer when engaged_toggle is true * Update instructions * fix --ip * Easier to understand at a glance * much better * -a * receive events and send msg in same loop * always import * Update selfdrive/controls/lib/events.py Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com> * Update selfdrive/controls/lib/events.py Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com> * combine logic and clean up * use argparse * outdated, and use normal class * rm * bit of a refactor * refactor part 2 / 3 * much better (3 / 3) * Simplify * bump cereal and update readme * capitalize * Update tools/joystick/joystickd.py Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com> * Update tools/joystick/joystickd.py Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com> * make joystick abstraction class clearer * use interp, clearer without comments * no need to use apply_deadzone * more explicit * define btns and axes once * split function by use_keyboard again, but simpler * we can use handle_button as a reset function * need to flip sign * remove * invert axes map for kb, easier to read the button mapping * apply changes from review * new lateral log for debug mode * bump * add saturated * static alert * joystick_mode * conditionally subscribe * Update selfdrive/controls/controlsd.py Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com> * move params instantiation * Spoof active and enabled * Only allow car to engage * no startup alert if joystick * Update controlsd.py * Should be orange not enabled, green enabled * no more button states * should work * blue * submaster conflates, so only send when we have an update * final change * remove msg * clean up a bit sort of clean up clean up a bit remove msg * this was right * Apply suggestions from code review Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com> * lowercase * suggestions from code review * forgot laptop * bump to latest * fixes Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com> Co-authored-by: vanillagorillaa <31773928+vanillagorillaa@users.noreply.github.com>pull/21243/head
ShaneSmiskol
4 years ago
committed by
GitHub
parent
9ad129fd5f
commit
ae77f693a2
13 changed files with 181 additions and 315 deletions
@ -1,26 +0,0 @@ |
|||||||
Debug car controls |
|
||||||
------------- |
|
||||||
|
|
||||||
**Hardware needed**: [panda](panda.comma.ai), [giraffe](https://comma.ai/shop/products/giraffe/), joystick |
|
||||||
|
|
||||||
Use the panda's OBD-II port to connect with your car and a usb cable to connect the panda to your pc. |
|
||||||
Also, connect a joystick to your pc. |
|
||||||
|
|
||||||
`joystickd.py` runs a deamon that reads inputs from a joystick and publishes them over zmq. |
|
||||||
`boardd` sends the CAN messages from your pc to the panda. |
|
||||||
`debug_controls` is a mocked version of `controlsd.py` and uses input from a joystick to send controls to your car. |
|
||||||
|
|
||||||
Make sure the conditions are met in the panda to allow controls (e.g. cruise control engaged). You can also make a modification to the panda code to always allow controls. |
|
||||||
|
|
||||||
Usage: |
|
||||||
``` |
|
||||||
python carcontrols/joystickd.py |
|
||||||
|
|
||||||
# In another terminal: |
|
||||||
BASEDIR=$(pwd) selfdrive/boardd/boardd |
|
||||||
|
|
||||||
# In another terminal: |
|
||||||
python carcontrols/debug_controls.py |
|
||||||
|
|
||||||
``` |
|
||||||
 |
|
||||||
@ -1,90 +0,0 @@ |
|||||||
#!/usr/bin/env python |
|
||||||
from common.numpy_fast import clip |
|
||||||
from common.params import Params |
|
||||||
from copy import copy |
|
||||||
from cereal import car, log |
|
||||||
import cereal.messaging as messaging |
|
||||||
from selfdrive.car.car_helpers import get_car, get_one_can |
|
||||||
from selfdrive.boardd.boardd import can_list_to_can_capnp |
|
||||||
|
|
||||||
PandaType = log.HealthData.PandaType |
|
||||||
|
|
||||||
|
|
||||||
def steer_thread(): |
|
||||||
poller = messaging.Poller() |
|
||||||
|
|
||||||
logcan = messaging.sub_sock('can') |
|
||||||
joystick_sock = messaging.sub_sock('testJoystick', conflate=True, poller=poller) |
|
||||||
|
|
||||||
carstate = messaging.pub_sock('carState') |
|
||||||
carcontrol = messaging.pub_sock('carControl') |
|
||||||
sendcan = messaging.pub_sock('sendcan') |
|
||||||
|
|
||||||
button_1_last = 0 |
|
||||||
enabled = False |
|
||||||
|
|
||||||
# wait for CAN packets |
|
||||||
print("Waiting for CAN messages...") |
|
||||||
get_one_can(logcan) |
|
||||||
|
|
||||||
CI, CP = get_car(logcan, sendcan) |
|
||||||
Params().put("CarParams", CP.to_bytes()) |
|
||||||
|
|
||||||
CC = car.CarControl.new_message() |
|
||||||
|
|
||||||
while True: |
|
||||||
|
|
||||||
# send |
|
||||||
joystick = messaging.recv_one(joystick_sock) |
|
||||||
can_strs = messaging.drain_sock_raw(logcan, wait_for_one=True) |
|
||||||
CS = CI.update(CC, can_strs) |
|
||||||
|
|
||||||
# Usually axis run in pairs, up/down for one, and left/right for |
|
||||||
# the other. |
|
||||||
actuators = car.CarControl.Actuators.new_message() |
|
||||||
|
|
||||||
if joystick is not None: |
|
||||||
axis_3 = clip(-joystick.testJoystick.axes[3] * 1.05, -1., 1.) # -1 to 1 |
|
||||||
actuators.steer = axis_3 |
|
||||||
actuators.steeringAngleDeg = axis_3 * 43. # deg |
|
||||||
axis_1 = clip(-joystick.testJoystick.axes[1] * 1.05, -1., 1.) # -1 to 1 |
|
||||||
actuators.gas = max(axis_1, 0.) |
|
||||||
actuators.brake = max(-axis_1, 0.) |
|
||||||
|
|
||||||
pcm_cancel_cmd = joystick.testJoystick.buttons[0] |
|
||||||
button_1 = joystick.testJoystick.buttons[1] |
|
||||||
if button_1 and not button_1_last: |
|
||||||
enabled = not enabled |
|
||||||
|
|
||||||
button_1_last = button_1 |
|
||||||
|
|
||||||
#print "enable", enabled, "steer", actuators.steer, "accel", actuators.gas - actuators.brake |
|
||||||
|
|
||||||
hud_alert = 0 |
|
||||||
if joystick.testJoystick.buttons[3]: |
|
||||||
hud_alert = "steerRequired" |
|
||||||
|
|
||||||
CC.actuators.gas = actuators.gas |
|
||||||
CC.actuators.brake = actuators.brake |
|
||||||
CC.actuators.steer = actuators.steer |
|
||||||
CC.actuators.steeringAngleDeg = actuators.steeringAngleDeg |
|
||||||
CC.hudControl.visualAlert = hud_alert |
|
||||||
CC.hudControl.setSpeed = 20 |
|
||||||
CC.cruiseControl.cancel = pcm_cancel_cmd |
|
||||||
CC.enabled = enabled |
|
||||||
can_sends = CI.apply(CC) |
|
||||||
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan')) |
|
||||||
|
|
||||||
# broadcast carState |
|
||||||
cs_send = messaging.new_message('carState') |
|
||||||
cs_send.carState = copy(CS) |
|
||||||
carstate.send(cs_send.to_bytes()) |
|
||||||
|
|
||||||
# broadcast carControl |
|
||||||
cc_send = messaging.new_message('carControl') |
|
||||||
cc_send.carControl = copy(CC) |
|
||||||
carcontrol.send(cc_send.to_bytes()) |
|
||||||
|
|
||||||
|
|
||||||
if __name__ == "__main__": |
|
||||||
steer_thread() |
|
||||||
@ -1,122 +0,0 @@ |
|||||||
#!/usr/bin/env python |
|
||||||
import pygame # pylint: disable=import-error |
|
||||||
|
|
||||||
# Define some colors |
|
||||||
BLACK = ( 0, 0, 0) |
|
||||||
WHITE = ( 255, 255, 255) |
|
||||||
|
|
||||||
# This is a simple class that will help us print to the screen |
|
||||||
# It has nothing to do with the joysticks, just outputting the |
|
||||||
# information. |
|
||||||
class TextPrint: |
|
||||||
def __init__(self): |
|
||||||
self.reset() |
|
||||||
self.font = pygame.font.Font(None, 20) |
|
||||||
|
|
||||||
def printf(self, screen, textString): |
|
||||||
textBitmap = self.font.render(textString, True, BLACK) |
|
||||||
screen.blit(textBitmap, [self.x, self.y]) |
|
||||||
self.y += self.line_height |
|
||||||
|
|
||||||
def reset(self): |
|
||||||
self.x = 10 |
|
||||||
self.y = 10 |
|
||||||
self.line_height = 15 |
|
||||||
|
|
||||||
def indent(self): |
|
||||||
self.x += 10 |
|
||||||
|
|
||||||
def unindent(self): |
|
||||||
self.x -= 10 |
|
||||||
|
|
||||||
|
|
||||||
pygame.init() |
|
||||||
|
|
||||||
# Set the width and height of the screen [width,height] |
|
||||||
size = [500, 700] |
|
||||||
screen = pygame.display.set_mode(size) |
|
||||||
|
|
||||||
pygame.display.set_caption("My Game") |
|
||||||
|
|
||||||
#Loop until the user clicks the close button. |
|
||||||
done = False |
|
||||||
|
|
||||||
# Used to manage how fast the screen updates |
|
||||||
clock = pygame.time.Clock() |
|
||||||
|
|
||||||
# Initialize the joysticks |
|
||||||
pygame.joystick.init() |
|
||||||
|
|
||||||
# Get ready to print |
|
||||||
textPrint = TextPrint() |
|
||||||
|
|
||||||
# -------- Main Program Loop ----------- |
|
||||||
while not done: |
|
||||||
# EVENT PROCESSING STEP |
|
||||||
for event in pygame.event.get(): # User did something |
|
||||||
if event.type == pygame.QUIT: # If user clicked close |
|
||||||
done = True # Flag that we are done so we exit this loop |
|
||||||
|
|
||||||
# Possible joystick actions: JOYAXISMOTION JOYBALLMOTION JOYBUTTONDOWN JOYBUTTONUP JOYHATMOTION |
|
||||||
if event.type == pygame.JOYBUTTONDOWN: |
|
||||||
print("Joystick button pressed.") |
|
||||||
if event.type == pygame.JOYBUTTONUP: |
|
||||||
print("Joystick button released.") |
|
||||||
|
|
||||||
# DRAWING STEP |
|
||||||
# First, clear the screen to white. Don't put other drawing commands |
|
||||||
# above this, or they will be erased with this command. |
|
||||||
screen.fill(WHITE) |
|
||||||
textPrint.reset() |
|
||||||
|
|
||||||
# Get count of joysticks |
|
||||||
joystick_count = pygame.joystick.get_count() |
|
||||||
|
|
||||||
textPrint.printf(screen, "Number of joysticks: {}".format(joystick_count) ) |
|
||||||
textPrint.indent() |
|
||||||
|
|
||||||
# For each joystick: |
|
||||||
joystick = pygame.joystick.Joystick(0) |
|
||||||
joystick.init() |
|
||||||
|
|
||||||
textPrint.printf(screen, "Joystick {}".format(0) ) |
|
||||||
textPrint.indent() |
|
||||||
|
|
||||||
# Get the name from the OS for the controller/joystick |
|
||||||
name = joystick.get_name() |
|
||||||
textPrint.printf(screen, "Joystick name: {}".format(name) ) |
|
||||||
|
|
||||||
# Usually axis run in pairs, up/down for one, and left/right for |
|
||||||
# the other. |
|
||||||
axes = joystick.get_numaxes() |
|
||||||
textPrint.printf(screen, "Number of axes: {}".format(axes) ) |
|
||||||
textPrint.indent() |
|
||||||
|
|
||||||
for i in range( axes ): |
|
||||||
axis = joystick.get_axis( i ) |
|
||||||
textPrint.printf(screen, "Axis {} value: {:>6.3f}".format(i, axis) ) |
|
||||||
textPrint.unindent() |
|
||||||
|
|
||||||
buttons = joystick.get_numbuttons() |
|
||||||
textPrint.printf(screen, "Number of buttons: {}".format(buttons) ) |
|
||||||
textPrint.indent() |
|
||||||
|
|
||||||
for i in range( buttons ): |
|
||||||
button = joystick.get_button( i ) |
|
||||||
textPrint.printf(screen, "Button {:>2} value: {}".format(i, button) ) |
|
||||||
textPrint.unindent() |
|
||||||
|
|
||||||
textPrint.unindent() |
|
||||||
|
|
||||||
# ALL CODE TO DRAW SHOULD GO ABOVE THIS COMMENT |
|
||||||
|
|
||||||
# Go ahead and update the screen with what we've drawn. |
|
||||||
pygame.display.flip() |
|
||||||
|
|
||||||
# Limit to 20 frames per second |
|
||||||
clock.tick(20) |
|
||||||
|
|
||||||
# Close the window and quit. |
|
||||||
# If you forget this line, the program will 'hang' |
|
||||||
# on exit if running from IDLE. |
|
||||||
pygame.quit() |
|
||||||
@ -1,65 +0,0 @@ |
|||||||
#!/usr/bin/env python |
|
||||||
|
|
||||||
# This process publishes joystick events. Such events can be suscribed by |
|
||||||
# mocked car controller scripts. |
|
||||||
|
|
||||||
|
|
||||||
### this process needs pygame and can't run on the EON ### |
|
||||||
|
|
||||||
import pygame # pylint: disable=import-error |
|
||||||
import cereal.messaging as messaging |
|
||||||
|
|
||||||
def joystick_thread(): |
|
||||||
joystick_sock = messaging.pub_sock('testJoystick') |
|
||||||
|
|
||||||
pygame.init() |
|
||||||
|
|
||||||
# Used to manage how fast the screen updates |
|
||||||
clock = pygame.time.Clock() |
|
||||||
|
|
||||||
# Initialize the joysticks |
|
||||||
pygame.joystick.init() |
|
||||||
|
|
||||||
# Get count of joysticks |
|
||||||
joystick_count = pygame.joystick.get_count() |
|
||||||
if joystick_count > 1: |
|
||||||
raise ValueError("More than one joystick attached") |
|
||||||
elif joystick_count < 1: |
|
||||||
raise ValueError("No joystick found") |
|
||||||
|
|
||||||
# -------- Main Program Loop ----------- |
|
||||||
while True: |
|
||||||
# EVENT PROCESSING STEP |
|
||||||
for event in pygame.event.get(): # User did something |
|
||||||
if event.type == pygame.QUIT: # If user clicked close |
|
||||||
pass |
|
||||||
# Available joystick events: JOYAXISMOTION JOYBALLMOTION JOYBUTTONDOWN JOYBUTTONUP JOYHATMOTION |
|
||||||
if event.type == pygame.JOYBUTTONDOWN: |
|
||||||
print("Joystick button pressed.") |
|
||||||
if event.type == pygame.JOYBUTTONUP: |
|
||||||
print("Joystick button released.") |
|
||||||
|
|
||||||
joystick = pygame.joystick.Joystick(0) |
|
||||||
joystick.init() |
|
||||||
|
|
||||||
# Usually axis run in pairs, up/down for one, and left/right for |
|
||||||
# the other. |
|
||||||
axes = [] |
|
||||||
buttons = [] |
|
||||||
|
|
||||||
for a in range(joystick.get_numaxes()): |
|
||||||
axes.append(joystick.get_axis(a)) |
|
||||||
|
|
||||||
for b in range(joystick.get_numbuttons()): |
|
||||||
buttons.append(bool(joystick.get_button(b))) |
|
||||||
|
|
||||||
dat = messaging.new_message('testJoystick') |
|
||||||
dat.testJoystick.axes = axes |
|
||||||
dat.testJoystick.buttons = buttons |
|
||||||
joystick_sock.send(dat.to_bytes()) |
|
||||||
|
|
||||||
# Limit to 100 frames per second |
|
||||||
clock.tick(100) |
|
||||||
|
|
||||||
if __name__ == "__main__": |
|
||||||
joystick_thread() |
|
||||||
@ -0,0 +1,45 @@ |
|||||||
|
# Joystick |
||||||
|
|
||||||
|
**Hardware needed**: [comma two devkit](https://comma.ai/shop/products/comma-two-devkit), laptop, joystick (optional) |
||||||
|
|
||||||
|
With joystickd, you can connect your laptop to your comma device over the network and debug controls using a joystick or keyboard, however a joystick is recommended for more precise control. |
||||||
|
|
||||||
|
Using a keyboard |
||||||
|
--- |
||||||
|
|
||||||
|
To get started, ssh into your comma device and start joystickd with the following command: |
||||||
|
|
||||||
|
```shell |
||||||
|
tools/joystick/joystickd.py --keyboard |
||||||
|
``` |
||||||
|
|
||||||
|
The available buttons and axes will print showing their key mappings. In general, the WASD keys control gas and brakes and steering torque in 5% increments. |
||||||
|
|
||||||
|
Using a joystick |
||||||
|
--- |
||||||
|
|
||||||
|
In order to use a joystick over the network, we need to run joystickd locally from your laptop and have it send `testJoystick` ZMQ packets over the network to the comma device. First connect a compatible joystick to your PC; joystickd uses [inputs](https://pypi.org/project/inputs) which supports many common gamepads and joysticks. |
||||||
|
|
||||||
|
1. Connect your laptop to your comma device's hotspot and open a new ssh shell. Since joystickd is being run on your laptop, we need to write a parameter to let controlsd know to start in joystick debug mode: |
||||||
|
```shell |
||||||
|
# on your comma device |
||||||
|
echo -n "1" > /data/params/d/JoystickDebugMode |
||||||
|
``` |
||||||
|
2. Run bridge with your laptop's IP address. This republishes the `testJoystick` packets sent from your laptop so that openpilot can receive them: |
||||||
|
```shell |
||||||
|
# on your comma device |
||||||
|
cereal/messaging/bridge {LAPTOP_IP} testJoystick |
||||||
|
``` |
||||||
|
3. Finally, start joystickd on your laptop and tell it to publish ZMQ packets over the network: |
||||||
|
```shell |
||||||
|
# on your laptop |
||||||
|
export ZMQ=1 |
||||||
|
tools/joystick/joystickd.py |
||||||
|
``` |
||||||
|
|
||||||
|
--- |
||||||
|
Now start your car and openpilot should go into debug mode with an alert on startup! The status of the axes will display on the alert, while button statuses print in the shell. |
||||||
|
|
||||||
|
Make sure the conditions are met in the panda to allow controls (e.g. cruise control engaged). You can also make a modification to the panda code to always allow controls. |
||||||
|
|
||||||
|
 |
||||||
@ -0,0 +1,86 @@ |
|||||||
|
#!/usr/bin/env python |
||||||
|
import argparse |
||||||
|
|
||||||
|
import cereal.messaging as messaging |
||||||
|
from common.numpy_fast import interp, clip |
||||||
|
from common.params import Params |
||||||
|
from inputs import get_gamepad |
||||||
|
from tools.lib.kbhit import KBHit |
||||||
|
|
||||||
|
|
||||||
|
class Keyboard: |
||||||
|
def __init__(self): |
||||||
|
self.kb = KBHit() |
||||||
|
self.axis_increment = 0.05 # 5% of full actuation each key press |
||||||
|
self.axes_map = {'w': 'gb', 's': 'gb', |
||||||
|
'a': 'steer', 'd': 'steer'} |
||||||
|
self.axes_values = {'gb': 0., 'steer': 0.} |
||||||
|
|
||||||
|
def update(self): |
||||||
|
key = self.kb.getch().lower() |
||||||
|
self.cancel = False |
||||||
|
if key == 'r': |
||||||
|
self.axes_values = {ax: 0. for ax in self.axes_values} |
||||||
|
elif key == 'c': |
||||||
|
self.cancel = True |
||||||
|
elif key in self.axes_map: |
||||||
|
axis = self.axes_map[key] |
||||||
|
incr = self.axis_increment if key in ['w', 'a'] else -self.axis_increment |
||||||
|
self.axes_values[axis] = clip(self.axes_values[axis] + incr, -1, 1) |
||||||
|
else: |
||||||
|
return False |
||||||
|
return True |
||||||
|
|
||||||
|
|
||||||
|
class Joystick: |
||||||
|
def __init__(self): |
||||||
|
self.max_axis_value = 255 # tune based on your joystick, 0 to this |
||||||
|
self.cancel_button = 'BTN_TRIGGER' |
||||||
|
self.axes_values = {'ABS_Y': 0., 'ABS_RZ': 0.} # gb, steer |
||||||
|
|
||||||
|
def update(self): |
||||||
|
joystick_event = get_gamepad()[0] |
||||||
|
event = (joystick_event.code, joystick_event.state) |
||||||
|
self.cancel = False |
||||||
|
if event[0] == self.cancel_button and event[1] == 0: # state 0 is falling edge |
||||||
|
self.cancel = True |
||||||
|
elif event[0] in self.axes_values: |
||||||
|
norm = -interp(event[1], [0, self.max_axis_value], [-1., 1.]) |
||||||
|
self.axes_values[event[0]] = norm if abs(norm) > 0.05 else 0. # center can be noisy, deadzone of 5% |
||||||
|
else: |
||||||
|
return False |
||||||
|
return True |
||||||
|
|
||||||
|
|
||||||
|
def joystick_thread(use_keyboard): |
||||||
|
Params().put_bool('JoystickDebugMode', True) |
||||||
|
joystick_sock = messaging.pub_sock('testJoystick') |
||||||
|
joystick = Keyboard() if use_keyboard else Joystick() |
||||||
|
|
||||||
|
while True: |
||||||
|
ret = joystick.update() # processes joystick/key events and handles state of axes |
||||||
|
if ret: |
||||||
|
dat = messaging.new_message('testJoystick') |
||||||
|
dat.testJoystick.axes = [joystick.axes_values[a] for a in joystick.axes_values] |
||||||
|
dat.testJoystick.buttons = [joystick.cancel] |
||||||
|
joystick_sock.send(dat.to_bytes()) |
||||||
|
print('\n' + ', '.join([f'{name}: {round(v, 3)}' for name, v in joystick.axes_values.items()])) |
||||||
|
|
||||||
|
|
||||||
|
if __name__ == '__main__': |
||||||
|
parser = argparse.ArgumentParser( |
||||||
|
description='Publishes events from your joystick to control your car', |
||||||
|
formatter_class=argparse.ArgumentDefaultsHelpFormatter) |
||||||
|
parser.add_argument('--keyboard', action='store_true', help='Use your keyboard instead of a joystick') |
||||||
|
args = parser.parse_args() |
||||||
|
|
||||||
|
if args.keyboard: |
||||||
|
print('\nGas/brake control: `W` and `S` keys\n' |
||||||
|
'Steering control: `A` and `D` keys') |
||||||
|
print('Buttons:\n' |
||||||
|
'- `R`: Resets axes\n' |
||||||
|
'- `C`: Cancel cruise control') |
||||||
|
else: |
||||||
|
print('\nUsing joystick, make sure to run bridge on your device if running over the network!') |
||||||
|
|
||||||
|
joystick_thread(args.keyboard) |
||||||
|
Before Width: | Height: | Size: 7.0 MiB After Width: | Height: | Size: 7.0 MiB |
Loading…
Reference in new issue