spacecruft
/
nopenpilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Simulator running with keyboard (#1176) 

* sim with keyboard

* write calibration

* need recv

* use readchar

* doesn't accelerate

* queue, not zmq

* fix line wrap

* fix physics

* add BLOCK to manager
pull/1178/head
George Hotz 2020-02-29 10:51:39 -08:00 committed by GitHub
parent 78ba94a977
commit 9d8d7ade3e
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
5 changed files with 116 additions and 61 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
selfdrive/manager.py
View File

@ -407,6 +407,10 @@ def manager_thread():
if os.getenv("NOBOARD") is None:
start_managed_process("pandad")
if os.getenv("BLOCK") is not None:
for k in os.getenv("BLOCK").split(","):
del managed_processes[k]
logger_dead = False
while 1:

79
tools/sim/bridge.py
View File

@ -9,17 +9,16 @@ import threading
import random
import cereal.messaging as messaging
import argparse
import zmq
import queue
from common.params import Params
from common.realtime import Ratekeeper
from lib.can import can_function, sendcan_function
from lib.helpers import FakeSteeringWheel
from lib.manual_ctrl import wheel_poll_thread
from selfdrive.car.honda.values import CruiseButtons
parser = argparse.ArgumentParser(description='Bridge between CARLA and openpilot.')
parser.add_argument('--autopilot', action='store_true')
parser.add_argument('--joystick', action='store_true')
args = parser.parse_args()
pm = messaging.PubMaster(['frame', 'sensorEvents', 'can'])
@ -80,7 +79,10 @@ def fake_driver_monitoring():
pm.send('driverState', dat)
time.sleep(0.1)
def go():
def go(q):
threading.Thread(target=health_function).start()
threading.Thread(target=fake_driver_monitoring).start()
import carla
client = carla.Client("127.0.0.1", 2000)
client.set_timeout(5.0)
@ -111,8 +113,11 @@ def go():
# make tires less slippery
wheel_control = carla.WheelPhysicsControl(tire_friction=5)
physics_control = carla.VehiclePhysicsControl(mass=1326, wheels=[wheel_control]*4, \
torque_curve=[[20.0, 500.0], [5000.0, 500.0]], gear_switch_time=0)
physics_control = vehicle.get_physics_control()
physics_control.mass = 1326
physics_control.wheels = [wheel_control]*4
physics_control.torque_curve = [[20.0, 500.0], [5000.0, 500.0]]
physics_control.gear_switch_time = 0.0
vehicle.apply_physics_control(physics_control)
if args.autopilot:
@ -143,7 +148,7 @@ def go():
# can loop
sendcan = messaging.sub_sock('sendcan')
rk = Ratekeeper(100)
rk = Ratekeeper(100, print_delay_threshold=0.05)
# init
A_throttle = 2.
@ -153,31 +158,19 @@ def go():
is_openpilot_engaged = False
in_reverse = False
# start input poll
from multiprocessing import Process
p = Process(target=wheel_poll_thread)
p.start()
# zmq receiver for input thread
context = zmq.Context()
socket = context.socket(zmq.REP)
socket.connect("tcp://127.0.0.1:4444")
throttle_out = 0
brake_out = 0
steer_angle_out = 0
while 1:
vel = vehicle.get_velocity()
speed = math.sqrt(vel.x**2 + vel.y**2 + vel.z**2) * 3.6
cruise_button = 0
try:
#check for a input message, this will not block
message = socket.recv(flags=zmq.NOBLOCK)
socket.send(b"good")
# print(message.decode('UTF-8'))
# check for a input message, this will not block
if not q.empty():
print("here")
message = q.get()
m = message.decode('UTF-8').split('_')
m = message.split('_')
if m[0] == "steer":
steer_angle_out = float(m[1])
fake_wheel.set_angle(steer_angle_out) # touching the wheel overrides fake wheel angle
@ -185,33 +178,32 @@ def go():
if m[0] == "throttle":
throttle_out = float(m[1]) / 100.
if throttle_out > 0.3:
can_function(pm, speed, fake_wheel.angle, rk.frame, cruise_button=CruiseButtons.CANCEL)
cruise_button = CruiseButtons.CANCEL
is_openpilot_engaged = False
if m[0] == "brake":
brake_out = float(m[1]) / 100.
if brake_out > 0.3:
can_function(pm, speed, fake_wheel.angle, rk.frame, cruise_button=CruiseButtons.CANCEL)
cruise_button = CruiseButtons.CANCEL
is_openpilot_engaged = False
if m[0] == "reverse":
in_reverse = not in_reverse
can_function(pm, speed, fake_wheel.angle, rk.frame, cruise_button=CruiseButtons.CANCEL)
cruise_button = CruiseButtons.CANCEL
is_openpilot_engaged = False
if m[0] == "cruise":
if m[1] == "down":
can_function(pm, speed, fake_wheel.angle, rk.frame, cruise_button=CruiseButtons.DECEL_SET)
cruise_button = CruiseButtons.DECEL_SET
is_openpilot_engaged = True
if m[1] == "up":
can_function(pm, speed, fake_wheel.angle, rk.frame, cruise_button=CruiseButtons.RES_ACCEL)
cruise_button = CruiseButtons.RES_ACCEL
is_openpilot_engaged = True
if m[1] == "cancel":
can_function(pm, speed, fake_wheel.angle, rk.frame, cruise_button=CruiseButtons.CANCEL)
cruise_button = CruiseButtons.CANCEL
is_openpilot_engaged = False
except zmq.Again as e:
#skip if no message
pass
vel = vehicle.get_velocity()
speed = math.sqrt(vel.x**2 + vel.y**2 + vel.z**2) * 3.6
can_function(pm, speed, fake_wheel.angle, rk.frame, cruise_button=cruise_button)
can_function(pm, speed, fake_wheel.angle, rk.frame)
if rk.frame%1 == 0: # 20Hz?
throttle_op, brake_op, steer_torque_op = sendcan_function(sendcan)
# print(" === torq, ",steer_torque_op, " ===")
@ -234,9 +226,7 @@ if __name__ == "__main__":
params.put("HasAcceptedTerms", terms_version)
params.put("CompletedTrainingVersion", training_version)
params.put("CommunityFeaturesToggle", "1")
threading.Thread(target=health_function).start()
threading.Thread(target=fake_driver_monitoring).start()
params.put("CalibrationParams", '{"vanishing_point": [582.06, 442.78], "valid_blocks": 20}')
# no carla, still run
try:
@ -246,5 +236,18 @@ if __name__ == "__main__":
while 1:
time.sleep(1)
go()
from multiprocessing import Process, Queue
q = Queue()
p = Process(target=go, args=(q,))
p.daemon = True
p.start()
if args.joystick:
# start input poll for joystick
from lib.manual_ctrl import wheel_poll_thread
wheel_poll_thread(q)
else:
# start input poll for keyboard
from lib.keyboard_ctrl import keyboard_poll_thread
keyboard_poll_thread(q)

4
tools/sim/lib/helpers.py
View File

@ -26,11 +26,11 @@ class FakeSteeringWheel():
# print(" ========== ")
# print("angle,", self.angle)
# print("omega,", self.omega)
print("torque,", exerted_torque)
# print("torque,", exerted_torque)
# print("centering torque,", centering_torque)
# print("damping torque,", damping_torque)
# print(" ========== ")
def set_angle(self, target):
self.angle = target
# self.omega = 0.
# self.omega = 0.

61
tools/sim/lib/keyboard_ctrl.py 100644
View File

@ -0,0 +1,61 @@
import time
import sys
import termios
from termios import *
# Indexes for termios list.
IFLAG = 0
OFLAG = 1
CFLAG = 2
LFLAG = 3
ISPEED = 4
OSPEED = 5
CC = 6
def getch():
fd = sys.stdin.fileno()
old_settings = termios.tcgetattr(fd)
try:
# set
mode = termios.tcgetattr(fd)
mode[IFLAG] = mode[IFLAG] & ~(BRKINT | ICRNL | INPCK | ISTRIP | IXON)
#mode[OFLAG] = mode[OFLAG] & ~(OPOST)
mode[CFLAG] = mode[CFLAG] & ~(CSIZE | PARENB)
mode[CFLAG] = mode[CFLAG] | CS8
mode[LFLAG] = mode[LFLAG] & ~(ECHO | ICANON | IEXTEN | ISIG)
mode[CC][VMIN] = 1
mode[CC][VTIME] = 0
termios.tcsetattr(fd, termios.TCSAFLUSH, mode)
ch = sys.stdin.read(1)
finally:
termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
return ch
def keyboard_poll_thread(q):
while True:
c = getch()
print("got %s" % c)
if c == '1':
q.put(str("cruise_up"))
if c == '2':
q.put(str("cruise_down"))
if c == '3':
q.put(str("cruise_cancel"))
if c == 'q':
exit(0)
def test(q):
while 1:
print("hello")
time.sleep(1.0)
if __name__ == '__main__':
from multiprocessing import Process, Queue
q = Queue()
p = Process(target=test, args=(q,))
p.daemon = True
p.start()
keyboard_poll_thread(q)

29
tools/sim/lib/manual_ctrl.py
View File

@ -88,7 +88,7 @@ button_names = {
axis_map = []
button_map = []
def wheel_poll_thread():
def wheel_poll_thread(q):
# Open the joystick device.
fn = '/dev/input/js0'
print('Opening %s...' % fn)
@ -139,12 +139,6 @@ def wheel_poll_thread():
val = 24000
evtdev.write(ecodes.EV_FF, ecodes.FF_AUTOCENTER, val)
# Set up zmq sock
import zmq
context = zmq.Context()
socket = context.socket(zmq.REQ) # block on send
socket.bind('tcp://127.0.0.1:4444')
while True:
evbuf = jsdev.read(8)
time, value, mtype, number = struct.unpack('IhBB', evbuf)
@ -156,43 +150,36 @@ def wheel_poll_thread():
fvalue = value / 32767.0
axis_states[axis] = fvalue
normalized = (1 - fvalue) * 50
socket.send_string(str("throttle_%f" % normalized))
_ = socket.recv()
q.put(str("throttle_%f" % normalized))
if axis == "rz": # brake
fvalue = value / 32767.0
axis_states[axis] = fvalue
normalized = (1 - fvalue) * 50
socket.send_string(str("brake_%f" % normalized))
_ = socket.recv()
q.put(str("brake_%f" % normalized))
if axis == "x": # steer angle
fvalue = value / 32767.0
axis_states[axis] = fvalue
normalized = fvalue
socket.send_string(str("steer_%f" % normalized))
_ = socket.recv()
q.put(str("steer_%f" % normalized))
if mtype & 0x01: # buttons
if number in [0,19]: # X
if value == 1: # press down
socket.send_string(str("cruise_down"))
_ = socket.recv()
q.put(str("cruise_down"))
if number in [3,18]: # triangle
if value == 1: # press down
socket.send_string(str("cruise_up"))
_ = socket.recv()
q.put(str("cruise_up"))
if number in [1,6]: # square
if value == 1: # press down
socket.send_string(str("cruise_cancel"))
_ = socket.recv()
q.put(str("cruise_cancel"))
if number in [10,21]: # R3
if value == 1: # press down
socket.send_string(str("reverse_switch"))
_ = socket.recv()
q.put(str("reverse_switch"))
if __name__ == '__main__':
from multiprocessing import Process