WIP: sim, need to unhack openpilot (#1174)
* comma hackathon * update readme * update readme * Update README.md * Update README.md * Update README.md * add more to readme * Update README.md * unhack * Delete README.md * Delete get_carla_095.sh * Delete run_carla_095.sh * remove NO_FLIP, and fix imu destroy * remove pipfile * make ui match * actually match ui Co-authored-by: George Hotz <geohot@gmail.com>albatross
ZwX1616
GitHub
parent
0a77684bcd
commit
78ba94a977
|
|
@ -1,6 +1,7 @@
|
|||
#!/usr/bin/env python3
|
||||
import carla
|
||||
import os
|
||||
import time
|
||||
import time, termios, tty, sys
|
||||
import math
|
||||
import atexit
|
||||
import numpy as np
|
||||
|
|
@ -8,10 +9,14 @@ 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
|
||||
import queue
|
||||
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')
|
||||
|
|
@ -31,6 +36,7 @@ def cam_callback(image):
|
|||
dat.frame = {
|
||||
"frameId": image.frame,
|
||||
"image": img.tostring(),
|
||||
"transform": [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
|
||||
}
|
||||
pm.send('frame', dat)
|
||||
|
||||
|
|
@ -78,19 +84,18 @@ def go():
|
|||
import carla
|
||||
client = carla.Client("127.0.0.1", 2000)
|
||||
client.set_timeout(5.0)
|
||||
world = client.load_world('Town03')
|
||||
|
||||
world = client.load_world('Town04')
|
||||
settings = world.get_settings()
|
||||
settings.fixed_delta_seconds = 0.05
|
||||
world.apply_settings(settings)
|
||||
|
||||
weather = carla.WeatherParameters(
|
||||
cloudyness=0.0,
|
||||
cloudyness=0.1,
|
||||
precipitation=0.0,
|
||||
precipitation_deposits=0.0,
|
||||
wind_intensity=0.0,
|
||||
sun_azimuth_angle=0.0,
|
||||
sun_altitude_angle=0.0)
|
||||
sun_azimuth_angle=15.0,
|
||||
sun_altitude_angle=75.0)
|
||||
world.set_weather(weather)
|
||||
|
||||
blueprint_library = world.get_blueprint_library()
|
||||
|
|
@ -101,12 +106,18 @@ def go():
|
|||
"""
|
||||
|
||||
world_map = world.get_map()
|
||||
vehicle_bp = random.choice(blueprint_library.filter('vehicle.tesla.*'))
|
||||
vehicle = world.spawn_actor(vehicle_bp, world_map.get_spawn_points()[16])
|
||||
|
||||
vehicle_bp = random.choice(blueprint_library.filter('vehicle.bmw.*'))
|
||||
vehicle = world.spawn_actor(vehicle_bp, random.choice(world_map.get_spawn_points()))
|
||||
# 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)
|
||||
vehicle.apply_physics_control(physics_control)
|
||||
|
||||
if args.autopilot:
|
||||
vehicle.set_autopilot(True)
|
||||
# print(vehicle.get_speed_limit())
|
||||
|
||||
blueprint = blueprint_library.find('sensor.camera.rgb')
|
||||
blueprint.set_attribute('image_size_x', str(W))
|
||||
|
|
@ -117,7 +128,7 @@ def go():
|
|||
camera = world.spawn_actor(blueprint, transform, attach_to=vehicle)
|
||||
camera.listen(cam_callback)
|
||||
|
||||
# TODO: wait for carla 0.9.7
|
||||
# reenable IMU
|
||||
imu_bp = blueprint_library.find('sensor.other.imu')
|
||||
imu = world.spawn_actor(imu_bp, transform, attach_to=vehicle)
|
||||
imu.listen(imu_callback)
|
||||
|
|
@ -133,18 +144,86 @@ def go():
|
|||
# can loop
|
||||
sendcan = messaging.sub_sock('sendcan')
|
||||
rk = Ratekeeper(100)
|
||||
steer_angle = 0
|
||||
|
||||
# init
|
||||
A_throttle = 2.
|
||||
A_brake = 2.
|
||||
A_steer_torque = 1.
|
||||
fake_wheel = FakeSteeringWheel()
|
||||
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)
|
||||
speed = math.sqrt(vel.x**2 + vel.y**2 + vel.z**2) * 3.6
|
||||
|
||||
can_function(pm, speed, steer_angle, rk.frame, rk.frame%500 == 499)
|
||||
if rk.frame%5 == 0:
|
||||
throttle, brake, steer = sendcan_function(sendcan)
|
||||
steer_angle += steer/10000.0 # torque
|
||||
vc = carla.VehicleControl(throttle=throttle, steer=steer_angle, brake=brake)
|
||||
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'))
|
||||
|
||||
m = message.decode('UTF-8').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
|
||||
# print(" === steering overriden === ")
|
||||
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)
|
||||
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)
|
||||
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)
|
||||
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)
|
||||
is_openpilot_engaged = True
|
||||
if m[1] == "up":
|
||||
can_function(pm, speed, fake_wheel.angle, rk.frame, 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)
|
||||
is_openpilot_engaged = False
|
||||
|
||||
except zmq.Again as e:
|
||||
#skip if no message
|
||||
pass
|
||||
|
||||
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, " ===")
|
||||
if is_openpilot_engaged:
|
||||
fake_wheel.response(steer_torque_op * A_steer_torque, speed)
|
||||
throttle_out = throttle_op * A_throttle
|
||||
brake_out = brake_op * A_brake
|
||||
steer_angle_out = fake_wheel.angle
|
||||
# print(steer_torque_op)
|
||||
# print(steer_angle_out)
|
||||
vc = carla.VehicleControl(throttle=throttle_out, steer=steer_angle_out / 3.14, brake=brake_out, reverse=in_reverse)
|
||||
vehicle.apply_control(vc)
|
||||
print(speed, steer_angle, vc)
|
||||
|
||||
rk.keep_time()
|
||||
|
||||
|
|
|
|||
|
|
@ -6,6 +6,7 @@ from opendbc.can.packer import CANPacker
|
|||
from selfdrive.boardd.boardd_api_impl import can_list_to_can_capnp
|
||||
from selfdrive.car.honda.values import FINGERPRINTS, CAR
|
||||
from selfdrive.car import crc8_pedal
|
||||
import math
|
||||
|
||||
from selfdrive.test.longitudinal_maneuvers.plant import get_car_can_parser
|
||||
cp = get_car_can_parser()
|
||||
|
|
@ -14,7 +15,12 @@ cp = get_car_can_parser()
|
|||
packer = CANPacker("honda_civic_touring_2016_can_generated")
|
||||
rpacker = CANPacker("acura_ilx_2016_nidec")
|
||||
|
||||
def can_function(pm, speed, angle, idx, engage):
|
||||
SR = 7.5
|
||||
|
||||
def angle_to_sangle(angle):
|
||||
return - math.degrees(angle) * SR
|
||||
|
||||
def can_function(pm, speed, angle, idx, cruise_button=0):
|
||||
msg = []
|
||||
msg.append(packer.make_can_msg("ENGINE_DATA", 0, {"XMISSION_SPEED": speed}, idx))
|
||||
msg.append(packer.make_can_msg("WHEEL_SPEEDS", 0,
|
||||
|
|
@ -23,10 +29,7 @@ def can_function(pm, speed, angle, idx, engage):
|
|||
"WHEEL_SPEED_RL": speed,
|
||||
"WHEEL_SPEED_RR": speed}, -1))
|
||||
|
||||
if engage:
|
||||
msg.append(packer.make_can_msg("SCM_BUTTONS", 0, {"CRUISE_BUTTONS": 3}, idx))
|
||||
else:
|
||||
msg.append(packer.make_can_msg("SCM_BUTTONS", 0, {"CRUISE_BUTTONS": 0}, idx))
|
||||
msg.append(packer.make_can_msg("SCM_BUTTONS", 0, {"CRUISE_BUTTONS": cruise_button}, idx))
|
||||
|
||||
values = {"COUNTER_PEDAL": idx&0xF}
|
||||
checksum = crc8_pedal(packer.make_can_msg("GAS_SENSOR", 0, {"COUNTER_PEDAL": idx&0xF}, -1)[2][:-1])
|
||||
|
|
@ -37,7 +40,7 @@ def can_function(pm, speed, angle, idx, engage):
|
|||
msg.append(packer.make_can_msg("GAS_PEDAL_2", 0, {}, idx))
|
||||
msg.append(packer.make_can_msg("SEATBELT_STATUS", 0, {"SEATBELT_DRIVER_LATCHED": 1}, idx))
|
||||
msg.append(packer.make_can_msg("STEER_STATUS", 0, {}, idx))
|
||||
msg.append(packer.make_can_msg("STEERING_SENSORS", 0, {"STEER_ANGLE": angle}, idx))
|
||||
msg.append(packer.make_can_msg("STEERING_SENSORS", 0, {"STEER_ANGLE":angle_to_sangle(angle)}, idx))
|
||||
msg.append(packer.make_can_msg("POWERTRAIN_DATA", 0, {}, idx))
|
||||
msg.append(packer.make_can_msg("VSA_STATUS", 0, {}, idx))
|
||||
msg.append(packer.make_can_msg("STANDSTILL", 0, {}, idx))
|
||||
|
|
|
|||
|
|
@ -0,0 +1,36 @@
|
|||
import numpy as np
|
||||
|
||||
class FakeSteeringWheel():
|
||||
def __init__(self, dt=0.01):
|
||||
# physical params
|
||||
self.DAC = 4. / 0.625 # convert torque value from can to Nm
|
||||
self.k = 0.035
|
||||
self.centering_k = 4.1 * 0.9
|
||||
self.b = 0.1 * 0.8
|
||||
self.I = 1 * 1.36 * (0.175**2)
|
||||
self.dt = dt
|
||||
# ...
|
||||
|
||||
self.angle = 0. # start centered
|
||||
# self.omega = 0.
|
||||
|
||||
def response(self, torque, vego=0):
|
||||
exerted_torque = torque * self.DAC
|
||||
# centering_torque = np.clip(-(self.centering_k * self.angle), -1.1, 1.1)
|
||||
# damping_torque = -(self.b * self.omega)
|
||||
# self.omega += self.dt * (exerted_torque + centering_torque + damping_torque) / self.I
|
||||
# self.omega = np.clip(self.omega, -1.1, 1.1)
|
||||
# self.angle += self.dt * self.omega
|
||||
self.angle += self.dt * self.k * exerted_torque # aristotle
|
||||
|
||||
# print(" ========== ")
|
||||
# print("angle,", self.angle)
|
||||
# print("omega,", self.omega)
|
||||
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.
|
||||
|
|
@ -0,0 +1,200 @@
|
|||
#!/usr/bin/env python3
|
||||
# set up wheel
|
||||
import os, struct, array
|
||||
from fcntl import ioctl
|
||||
|
||||
# Iterate over the joystick devices.
|
||||
print('Available devices:')
|
||||
for fn in os.listdir('/dev/input'):
|
||||
if fn.startswith('js'):
|
||||
print(' /dev/input/%s' % (fn))
|
||||
|
||||
# We'll store the states here.
|
||||
axis_states = {}
|
||||
button_states = {}
|
||||
|
||||
# These constants were borrowed from linux/input.h
|
||||
axis_names = {
|
||||
0x00 : 'x',
|
||||
0x01 : 'y',
|
||||
0x02 : 'z',
|
||||
0x03 : 'rx',
|
||||
0x04 : 'ry',
|
||||
0x05 : 'rz',
|
||||
0x06 : 'trottle',
|
||||
0x07 : 'rudder',
|
||||
0x08 : 'wheel',
|
||||
0x09 : 'gas',
|
||||
0x0a : 'brake',
|
||||
0x10 : 'hat0x',
|
||||
0x11 : 'hat0y',
|
||||
0x12 : 'hat1x',
|
||||
0x13 : 'hat1y',
|
||||
0x14 : 'hat2x',
|
||||
0x15 : 'hat2y',
|
||||
0x16 : 'hat3x',
|
||||
0x17 : 'hat3y',
|
||||
0x18 : 'pressure',
|
||||
0x19 : 'distance',
|
||||
0x1a : 'tilt_x',
|
||||
0x1b : 'tilt_y',
|
||||
0x1c : 'tool_width',
|
||||
0x20 : 'volume',
|
||||
0x28 : 'misc',
|
||||
}
|
||||
|
||||
button_names = {
|
||||
0x120 : 'trigger',
|
||||
0x121 : 'thumb',
|
||||
0x122 : 'thumb2',
|
||||
0x123 : 'top',
|
||||
0x124 : 'top2',
|
||||
0x125 : 'pinkie',
|
||||
0x126 : 'base',
|
||||
0x127 : 'base2',
|
||||
0x128 : 'base3',
|
||||
0x129 : 'base4',
|
||||
0x12a : 'base5',
|
||||
0x12b : 'base6',
|
||||
0x12f : 'dead',
|
||||
0x130 : 'a',
|
||||
0x131 : 'b',
|
||||
0x132 : 'c',
|
||||
0x133 : 'x',
|
||||
0x134 : 'y',
|
||||
0x135 : 'z',
|
||||
0x136 : 'tl',
|
||||
0x137 : 'tr',
|
||||
0x138 : 'tl2',
|
||||
0x139 : 'tr2',
|
||||
0x13a : 'select',
|
||||
0x13b : 'start',
|
||||
0x13c : 'mode',
|
||||
0x13d : 'thumbl',
|
||||
0x13e : 'thumbr',
|
||||
|
||||
0x220 : 'dpad_up',
|
||||
0x221 : 'dpad_down',
|
||||
0x222 : 'dpad_left',
|
||||
0x223 : 'dpad_right',
|
||||
|
||||
# XBox 360 controller uses these codes.
|
||||
0x2c0 : 'dpad_left',
|
||||
0x2c1 : 'dpad_right',
|
||||
0x2c2 : 'dpad_up',
|
||||
0x2c3 : 'dpad_down',
|
||||
}
|
||||
|
||||
axis_map = []
|
||||
button_map = []
|
||||
|
||||
def wheel_poll_thread():
|
||||
# Open the joystick device.
|
||||
fn = '/dev/input/js0'
|
||||
print('Opening %s...' % fn)
|
||||
jsdev = open(fn, 'rb')
|
||||
|
||||
# Get the device name.
|
||||
#buf = bytearray(63)
|
||||
buf = array.array('B', [0] * 64)
|
||||
ioctl(jsdev, 0x80006a13 + (0x10000 * len(buf)), buf) # JSIOCGNAME(len)
|
||||
js_name = buf.tobytes().rstrip(b'\x00').decode('utf-8')
|
||||
print('Device name: %s' % js_name)
|
||||
|
||||
# Get number of axes and buttons.
|
||||
buf = array.array('B', [0])
|
||||
ioctl(jsdev, 0x80016a11, buf) # JSIOCGAXES
|
||||
num_axes = buf[0]
|
||||
|
||||
buf = array.array('B', [0])
|
||||
ioctl(jsdev, 0x80016a12, buf) # JSIOCGBUTTONS
|
||||
num_buttons = buf[0]
|
||||
|
||||
# Get the axis map.
|
||||
buf = array.array('B', [0] * 0x40)
|
||||
ioctl(jsdev, 0x80406a32, buf) # JSIOCGAXMAP
|
||||
|
||||
for axis in buf[:num_axes]:
|
||||
axis_name = axis_names.get(axis, 'unknown(0x%02x)' % axis)
|
||||
axis_map.append(axis_name)
|
||||
axis_states[axis_name] = 0.0
|
||||
|
||||
# Get the button map.
|
||||
buf = array.array('H', [0] * 200)
|
||||
ioctl(jsdev, 0x80406a34, buf) # JSIOCGBTNMAP
|
||||
|
||||
for btn in buf[:num_buttons]:
|
||||
btn_name = button_names.get(btn, 'unknown(0x%03x)' % btn)
|
||||
button_map.append(btn_name)
|
||||
button_states[btn_name] = 0
|
||||
|
||||
print('%d axes found: %s' % (num_axes, ', '.join(axis_map)))
|
||||
print('%d buttons found: %s' % (num_buttons, ', '.join(button_map)))
|
||||
|
||||
# Enable FF
|
||||
import evdev
|
||||
from evdev import ecodes, InputDevice, ff
|
||||
device = evdev.list_devices()[0]
|
||||
evtdev = InputDevice(device)
|
||||
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)
|
||||
# print(mtype, number, value)
|
||||
if mtype & 0x02: # wheel & paddles
|
||||
axis = axis_map[number]
|
||||
|
||||
if axis == "z": # gas
|
||||
fvalue = value / 32767.0
|
||||
axis_states[axis] = fvalue
|
||||
normalized = (1 - fvalue) * 50
|
||||
socket.send_string(str("throttle_%f" % normalized))
|
||||
_ = socket.recv()
|
||||
|
||||
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()
|
||||
|
||||
if axis == "x": # steer angle
|
||||
fvalue = value / 32767.0
|
||||
axis_states[axis] = fvalue
|
||||
normalized = fvalue
|
||||
socket.send_string(str("steer_%f" % normalized))
|
||||
_ = socket.recv()
|
||||
|
||||
if mtype & 0x01: # buttons
|
||||
if number in [0,19]: # X
|
||||
if value == 1: # press down
|
||||
socket.send_string(str("cruise_down"))
|
||||
_ = socket.recv()
|
||||
|
||||
if number in [3,18]: # triangle
|
||||
if value == 1: # press down
|
||||
socket.send_string(str("cruise_up"))
|
||||
_ = socket.recv()
|
||||
|
||||
if number in [1,6]: # square
|
||||
if value == 1: # press down
|
||||
socket.send_string(str("cruise_cancel"))
|
||||
_ = socket.recv()
|
||||
|
||||
if number in [10,21]: # R3
|
||||
if value == 1: # press down
|
||||
socket.send_string(str("reverse_switch"))
|
||||
_ = socket.recv()
|
||||
|
||||
if __name__ == '__main__':
|
||||
from multiprocessing import Process
|
||||
p = Process(target=wheel_poll_thread)
|
||||
p.start()
|
||||
Loading…
Reference in New Issue