From c44b4a9ac6f986be7fe90508f837540fea14c5cb Mon Sep 17 00:00:00 2001 From: ZwX1616 Date: Sat, 29 Feb 2020 02:18:17 -0800 Subject: [PATCH] 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 old-commit-hash: 78ba94a97703f4dda55f315e728fdbfa81b5d13a --- tools/sim/bridge.py | 117 ++++++++++++++++---- tools/sim/lib/can.py | 15 +-- tools/sim/lib/helpers.py | 36 +++++++ tools/sim/lib/manual_ctrl.py | 200 +++++++++++++++++++++++++++++++++++ 4 files changed, 343 insertions(+), 25 deletions(-) create mode 100644 tools/sim/lib/helpers.py create mode 100755 tools/sim/lib/manual_ctrl.py diff --git a/tools/sim/bridge.py b/tools/sim/bridge.py index 05a81e200d..0a11294d78 100755 --- a/tools/sim/bridge.py +++ b/tools/sim/bridge.py @@ -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) - - 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) + speed = math.sqrt(vel.x**2 + vel.y**2 + vel.z**2) * 3.6 + + 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() diff --git a/tools/sim/lib/can.py b/tools/sim/lib/can.py index be806ff276..42e0fb87b5 100755 --- a/tools/sim/lib/can.py +++ b/tools/sim/lib/can.py @@ -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)) diff --git a/tools/sim/lib/helpers.py b/tools/sim/lib/helpers.py new file mode 100644 index 0000000000..50a23202cf --- /dev/null +++ b/tools/sim/lib/helpers.py @@ -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. \ No newline at end of file diff --git a/tools/sim/lib/manual_ctrl.py b/tools/sim/lib/manual_ctrl.py new file mode 100755 index 0000000000..669a9a1997 --- /dev/null +++ b/tools/sim/lib/manual_ctrl.py @@ -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()