diff --git a/.dockerignore b/.dockerignore index 556cc328f0..c52a810e32 100644 --- a/.dockerignore +++ b/.dockerignore @@ -38,3 +38,5 @@ xx/projects !xx/projects/map3d xx/ops xx/junk +tools/sim/carla +tools/sim/*.tar.gz diff --git a/.github/workflows/sim_tests.yaml b/.github/workflows/sim_tests.yaml new file mode 100644 index 0000000000..8ba3913e07 --- /dev/null +++ b/.github/workflows/sim_tests.yaml @@ -0,0 +1,26 @@ +name: simulator +on: + push: + + # TODO: remove the push trigger, and only run on schedule + #schedule: + #- cron: '0 * * * *' + +jobs: + docker_build: + name: build container + runs-on: ubuntu-16.04 + timeout-minutes: 50 + steps: + - uses: actions/checkout@v2 + with: + submodules: true + lfs: true + - name: Docker build + run: | + tools/sim/build_container.sh + - name: Push to dockerhub + run: | + docker login -u wmelching -p ${{ secrets.COMMA_DOCKERHUB_TOKEN }} + docker tag commaai/openpilot-sim docker.io/commaai/openpilot-sim:latest + docker push docker.io/commaai/openpilot-sim:latest diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 9f0777553f..fe582779e3 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -27,6 +27,7 @@ LDW_MIN_SPEED = 31 * CV.MPH_TO_MS LANE_DEPARTURE_THRESHOLD = 0.1 STEER_ANGLE_SATURATION_TIMEOUT = 1.0 / DT_CTRL STEER_ANGLE_SATURATION_THRESHOLD = 2.5 # Degrees +SIMULATION = "SIMULATION" in os.environ ThermalStatus = log.ThermalData.ThermalStatus State = log.ControlsState.OpenpilotState @@ -193,7 +194,7 @@ class Controls: elif self.sm['pathPlan'].laneChangeState in [LaneChangeState.laneChangeStarting, LaneChangeState.laneChangeFinishing]: self.events.add(EventName.laneChange) - + if self.can_rcv_error or (not CS.canValid and self.sm.frame > 5 / DT_CTRL): self.events.add(EventName.canError) if self.mismatch_counter >= 200: @@ -210,7 +211,9 @@ class Controls: self.events.add(EventName.sensorDataInvalid) if not self.sm['liveLocationKalman'].gpsOK and (self.distance_traveled > 1000) and os.getenv("NOSENSOR") is None: # Not show in first 1 km to allow for driving out of garage. This event shows after 5 minutes - self.events.add(EventName.noGps) + # GPS is not yet working in a simulation + if not SIMULATION: + self.events.add(EventName.noGps) if not self.sm['pathPlan'].paramsValid: self.events.add(EventName.vehicleModelInvalid) if not self.sm['liveLocationKalman'].posenetOK: @@ -229,11 +232,12 @@ class Controls: if self.sm['plan'].fcw: self.events.add(EventName.fcw) if self.sm['model'].frameDropPerc > 1: - self.events.add(EventName.modeldLagging) + if not SIMULATION: + self.events.add(EventName.modeldLagging) # Only allow engagement with brake pressed when stopped behind another stopped car if CS.brakePressed and self.sm['plan'].vTargetFuture >= STARTING_TARGET_SPEED \ - and not self.CP.radarOffCan and CS.vEgo < 0.3: + and not self.CP.radarOffCan and CS.vEgo < 0.3: self.events.add(EventName.noTarget) def data_sample(self): diff --git a/selfdrive/manager.py b/selfdrive/manager.py index bcc51ff89f..8bb1ca6a39 100755 --- a/selfdrive/manager.py +++ b/selfdrive/manager.py @@ -234,31 +234,31 @@ car_started_processes = [ 'plannerd', 'loggerd', 'radard', - 'dmonitoringd', 'calibrationd', 'paramsd', 'camerad', 'modeld', 'proclogd', - 'ubloxd', 'locationd', 'clocksd', ] driver_view_processes = [ 'camerad', - 'dmonitoringd', 'dmonitoringmodeld' ] if WEBCAM: car_started_processes += [ + 'dmonitoringd', 'dmonitoringmodeld', ] if not PC: car_started_processes += [ + 'ubloxd', 'sensord', + 'dmonitoringd', 'dmonitoringmodeld', ] diff --git a/selfdrive/modeld/modeld b/selfdrive/modeld/modeld index fb1039dbd6..3aafb88256 100755 --- a/selfdrive/modeld/modeld +++ b/selfdrive/modeld/modeld @@ -1,8 +1,7 @@ #!/bin/sh if [ -d /system ]; then - export LD_LIBRARY_PATH="/data/pythonpath/phonelibs/snpe/aarch64/:$LD_LIBRARY_PATH" + export LD_LIBRARY_PATH="/data/pythonpath/phonelibs/snpe/aarch64/:$LD_LIBRARY_PATH" else - export LD_LIBRARY_PATH="/usr/lib/aarch64-linux-gnu:/data/pythonpath/phonelibs/snpe/larch64:$HOME/openpilot/phonelibs/snpe/x86_64-linux-clang:$LD_LIBRARY_PATH" + export LD_LIBRARY_PATH="/usr/lib/aarch64-linux-gnu:/data/pythonpath/phonelibs/snpe/larch64:$HOME/openpilot/phonelibs/snpe/x86_64-linux-clang:/openpilot/phonelibs/snpe/x86_64:$HOME/openpilot/phonelibs/snpe/x86_64:$LD_LIBRARY_PATH" fi exec ./_modeld - diff --git a/tools/sim/Dockerfile.sim b/tools/sim/Dockerfile.sim new file mode 100644 index 0000000000..df316034f8 --- /dev/null +++ b/tools/sim/Dockerfile.sim @@ -0,0 +1,104 @@ +FROM commaai/openpilot-base:latest +#Carla + +COPY ./tools/sim/install_carla.sh /tmp +RUN /tmp/install_carla.sh + +#Intel openCL- run openCL on CPU +RUN apt-get update && apt-get install -y \ + apt-utils \ + unzip \ + tar \ + curl \ + xz-utils \ + + beignet-opencl-icd \ + + alien \ + clinfo \ + dbus \ + gcc-arm-none-eabi \ + tmux \ + vim \ + + # libglvnd dependencies + automake \ + libtool \ + libxext-dev \ + libx11-dev \ + x11proto-gl-dev \ + libpng16-16 \ + && rm -rf /var/lib/apt/lists/* + + +ARG INTEL_DRIVER=opencl_runtime_16.1.1_x64_ubuntu_6.4.0.25.tgz +ARG INTEL_DRIVER_URL=http://registrationcenter-download.intel.com/akdlm/irc_nas/9019 +RUN mkdir -p /tmp/opencl-driver-intel +WORKDIR /tmp/opencl-driver-intel +RUN echo INTEL_DRIVER is $INTEL_DRIVER; \ + curl -O $INTEL_DRIVER_URL/$INTEL_DRIVER; \ + if echo $INTEL_DRIVER | grep -q "[.]zip$"; then \ + unzip $INTEL_DRIVER; \ + mkdir fakeroot; \ + for f in intel-opencl-*.tar.xz; do tar -C fakeroot -Jxvf $f; done; \ + cp -R fakeroot/* /; \ + ldconfig; \ + else \ + tar -xzf $INTEL_DRIVER; \ + for i in $(basename $INTEL_DRIVER .tgz)/rpm/*.rpm; do alien --to-deb $i; done; \ + dpkg -i *.deb; \ + rm -rf $INTEL_DRIVER $(basename $INTEL_DRIVER .tgz) *.deb; \ + mkdir -p /etc/OpenCL/vendors; \ + echo /opt/intel/*/lib64/libintelocl.so > /etc/OpenCL/vendors/intel.icd; \ + fi; \ + rm -rf /tmp/opencl-driver-intel; + + +#Open[GL,CL] for gpu +ENV NVIDIA_DRIVER_CAPABILITIES ${NVIDIA_DRIVER_CAPABILITIES},display +RUN apt-get update && apt-get install -y --no-install-recommends \ + mesa-utils \ + ocl-icd-libopencl1 \ + clinfo && \ + rm -rf /var/lib/apt/lists/* +RUN mkdir -p /etc/OpenCL/vendors && \ + echo "libnvidia-opencl.so.1" > /etc/OpenCL/vendors/nvidia.icd +ENV NVIDIA_VISIBLE_DEVICES all +ENV NVIDIA_DRIVER_CAPABILITIES compute,utility,display + +#Python +ENV PYTHONPATH $HOME/openpilot:${PYTHONPATH} +RUN dbus-uuidgen > /etc/machine-id + +# we can apt-get after moving to a newer ubuntu +WORKDIR /opt/libglvnd +RUN git clone --branch="0.1.1" https://github.com/NVIDIA/libglvnd.git . && \ + ./autogen.sh && \ + ./configure --prefix=/usr/local --libdir=/usr/local/lib/x86_64-linux-gnu && \ + make -j"$(nproc)" install-strip && \ + find /usr/local/lib/x86_64-linux-gnu -type f -name 'lib*.la' -delete + +ENV LD_LIBRARY_PATH /usr/local/lib/x86_64-linux-gnu:/usr/local/lib/i386-linux-gnu${LD_LIBRARY_PATH:+:${LD_LIBRARY_PATH}} + +# get same tmux config used on NEOS for debugging +RUN cd $HOME && \ + wget https://raw.githubusercontent.com/commaai/eon-neos-builder/master/devices/eon/home/.tmux.conf + +RUN mkdir -p $HOME/openpilot + +COPY SConstruct $HOME/openpilot/ + +COPY ./phonelibs $HOME/openpilot/phonelibs +COPY ./laika $HOME/openpilot/laika +COPY ./laika_repo $HOME/openpilot/laika_repo +COPY ./rednose $HOME/openpilot/rednose +COPY ./common $HOME/openpilot/common +COPY ./models $HOME/openpilot/models +COPY ./opendbc $HOME/openpilot/opendbc +COPY ./cereal $HOME/openpilot/cereal +COPY ./panda $HOME/openpilot/panda +COPY ./selfdrive $HOME/openpilot/selfdrive +COPY ./tools $HOME/openpilot/tools + +WORKDIR $HOME/openpilot +RUN scons -j40 diff --git a/tools/sim/README.md b/tools/sim/README.md index 06cb03907a..cf4b2b3a65 100644 --- a/tools/sim/README.md +++ b/tools/sim/README.md @@ -1,32 +1,25 @@ openpilot in simulator ===================== -Needs Ubuntu 16.04 -## Checkout openpilot -``` -cd ~/ -git clone https://github.com/commaai/openpilot.git +## Setup -# Add export PYTHONPATH=$HOME/openpilot to your bashrc -# Have a working tensorflow+keras in python3.7.3 (with [packages] in openpilot/Pipfile) -``` -## Install (in terminal 1) -``` -cd ~/openpilot/tools/sim -./start_carla.sh # install CARLA 0.9.7 and start the server -``` -## openpilot (in terminal 2) +Checkout openpilot ``` -cd ~/openpilot/selfdrive/ -PASSIVE=0 NOBOARD=1 ./manager.py +cd ~/ && git clone https://github.com/commaai/openpilot.git ``` -## bridge (in terminal 3) + +## Running the simulator + +First, start the CARLA server. ``` -# links carla to openpilot, will "start the car" according to manager cd ~/openpilot/tools/sim -./bridge.py +./start_carla.sh ``` + +Then use `start_openpilot_docker.sh` to start the docker container. + ## Controls + Now put the focus on the terminal running bridge.py and you can control openpilot driving in the simulation with the following keys @@ -37,4 +30,3 @@ openpilot driving in the simulation with the following keys | 3 | Cruise cancel | | q | Exit all | - diff --git a/tools/sim/bridge.py b/tools/sim/bridge.py index 88ab3f9742..a118df0026 100755 --- a/tools/sim/bridge.py +++ b/tools/sim/bridge.py @@ -1,45 +1,65 @@ #!/usr/bin/env python3 # type: ignore +import carla # pylint: disable=import-error import time import math import atexit import numpy as np import threading -import random import cereal.messaging as messaging import argparse from common.params import Params -from common.realtime import Ratekeeper -from lib.can import can_function, sendcan_function -from lib.helpers import FakeSteeringWheel +from common.realtime import Ratekeeper, DT_DMON +from lib.can import can_function from selfdrive.car.honda.values import CruiseButtons +from selfdrive.test.helpers import set_params_enabled parser = argparse.ArgumentParser(description='Bridge between CARLA and openpilot.') -parser.add_argument('--autopilot', action='store_true') parser.add_argument('--joystick', action='store_true') -parser.add_argument('--realmonitoring', action='store_true') args = parser.parse_args() -pm = messaging.PubMaster(['frame', 'sensorEvents', 'can']) - W, H = 1164, 874 +REPEAT_COUNTER = 5 +PRINT_DECIMATION = 100 +STEER_RATIO = 15. + +pm = messaging.PubMaster(['frame', 'sensorEvents', 'can']) +sm = messaging.SubMaster(['carControl','controlsState']) + +class VehicleState: + def __init__(self): + self.speed = 0 + self.angle = 0 + self.cruise_button= 0 + self.is_engaged=False + +def steer_rate_limit(old, new): + # Rate limiting to 0.5 degrees per step + limit = 0.5 + if new > old + limit: + return old + limit + elif new < old - limit: + return old - limit + else: + return new +frame_id = 0 def cam_callback(image): + global frame_id img = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) img = np.reshape(img, (H, W, 4)) img = img[:, :, [0, 1, 2]].copy() dat = messaging.new_message('frame') dat.frame = { - "frameId": image.frame, + "frameId": frame_id, # TODO: can we get frame ID from the CARLA camera? "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) + frame_id += 1 def imu_callback(imu): - #print(imu, imu.accelerometer) - dat = messaging.new_message('sensorEvents', 2) dat.sensorEvents[0].sensor = 4 dat.sensorEvents[0].type = 0x10 @@ -54,7 +74,6 @@ def imu_callback(imu): def health_function(): pm = messaging.PubMaster(['health']) - rk = Ratekeeper(1.0) while 1: dat = messaging.new_message('health') dat.valid = True @@ -64,60 +83,79 @@ def health_function(): 'controlsAllowed': True } pm.send('health', dat) - rk.keep_time() + time.sleep(0.5) + +def fake_gps(): + # TODO: read GPS from CARLA + pm = messaging.PubMaster(['gpsLocationExternal']) + while 1: + dat = messaging.new_message('gpsLocationExternal') + pm.send('gpsLocationExternal', dat) + time.sleep(0.01) def fake_driver_monitoring(): - if args.realmonitoring: - return - pm = messaging.PubMaster(['driverState']) + pm = messaging.PubMaster(['driverState','dMonitoringState']) while 1: + + # dmonitoringmodeld output dat = messaging.new_message('driverState') dat.driverState.faceProb = 1.0 pm.send('driverState', dat) - time.sleep(0.1) + + # dmonitoringd output + dat = messaging.new_message('dMonitoringState') + dat.dMonitoringState = { + "faceDetected": True, + "isDistracted": False, + "awarenessStatus": 1., + "isRHD": False, + } + pm.send('dMonitoringState', dat) + + time.sleep(DT_DMON) + +def can_function_runner(vs): + i = 0 + while 1: + can_function(pm, vs.speed, vs.angle, i, vs.cruise_button, vs.is_engaged) + time.sleep(0.01) + i+=1 + def go(q): - threading.Thread(target=health_function).start() - threading.Thread(target=fake_driver_monitoring).start() + # setup CARLA client = carla.Client("127.0.0.1", 2000) - client.set_timeout(5.0) + client.set_timeout(10.0) world = client.load_world('Town04') - settings = world.get_settings() - settings.fixed_delta_seconds = 0.05 - world.apply_settings(settings) - - weather = carla.WeatherParameters( - cloudyness=0.1, - precipitation=0.0, - precipitation_deposits=0.0, - wind_intensity=0.0, - sun_azimuth_angle=15.0, - sun_altitude_angle=75.0) - world.set_weather(weather) + world.set_weather(carla.WeatherParameters( + cloudyness=0.1, + precipitation=0.0, + precipitation_deposits=0.0, + wind_intensity=0.0, + sun_azimuth_angle=15.0, + sun_altitude_angle=75.0 + )) blueprint_library = world.get_blueprint_library() - # for blueprint in blueprint_library.filter('sensor.*'): - # print(blueprint.id) - # exit(0) world_map = world.get_map() - vehicle_bp = random.choice(blueprint_library.filter('vehicle.tesla.*')) + + vehicle_bp = blueprint_library.filter('vehicle.tesla.*')[0] + # vehicle = world.spawn_actor(vehicle_bp, world_map.get_spawn_points()[16]) vehicle = world.spawn_actor(vehicle_bp, world_map.get_spawn_points()[16]) + max_steer_angle = vehicle.get_physics_control().wheels[0].max_steer_angle + # make tires less slippery - wheel_control = carla.WheelPhysicsControl(tire_friction=5) + # wheel_control = carla.WheelPhysicsControl(tire_friction=5) physics_control = vehicle.get_physics_control() - physics_control.mass = 1326 - physics_control.wheels = [wheel_control]*4 + physics_control.mass = 2326 + # 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: - vehicle.set_autopilot(True) - # print(vehicle.get_speed_limit()) - blueprint = blueprint_library.find('sensor.camera.rgb') blueprint.set_attribute('image_size_x', str(W)) blueprint.set_attribute('image_size_y', str(H)) @@ -140,47 +178,62 @@ def go(q): print("done") atexit.register(destroy) + + vehicle_state = VehicleState() + + # launch fake car threads + threading.Thread(target=health_function).start() + threading.Thread(target=fake_driver_monitoring).start() + threading.Thread(target=fake_gps).start() + threading.Thread(target=can_function_runner, args=(vehicle_state,)).start() + # can loop - sendcan = messaging.sub_sock('sendcan') rk = Ratekeeper(100, print_delay_threshold=0.05) # init - A_throttle = 2. - A_brake = 2. - A_steer_torque = 1. - fake_wheel = FakeSteeringWheel() + throttle_ease_out_counter = REPEAT_COUNTER + brake_ease_out_counter = REPEAT_COUNTER + steer_ease_out_counter = REPEAT_COUNTER + + + vc = carla.VehicleControl(throttle=0, steer=0, brake=0, reverse=False) + is_openpilot_engaged = False - in_reverse = False + throttle_out = steer_out = brake_out = 0 + throttle_op = steer_op = brake_op = 0 + throttle_manual = steer_manual = brake_manual = 0 + + old_steer = old_brake = old_throttle = 0 + throttle_manual_multiplier = 0.7 #keyboard signal is always 1 + brake_manual_multiplier = 0.7 #keyboard signal is always 1 + steer_manual_multiplier = 45 * STEER_RATIO #keyboard signal is always 1 - throttle_out = 0 - brake_out = 0 - steer_angle_out = 0 while 1: + # 1. Read the throttle, steer and brake from op or manual controls + # 2. Set instructions in Carla + # 3. Send current carstate to op via can + cruise_button = 0 + throttle_out = steer_out = brake_out = 0 + throttle_op = steer_op = brake_op = 0 + throttle_manual = steer_manual = brake_manual = 0 - # check for a input message, this will not block + # --------------Step 1------------------------------- if not q.empty(): - print("here") message = q.get() - 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 - # print(" === steering overriden === ") + steer_manual = float(m[1]) + is_openpilot_engaged = False if m[0] == "throttle": - throttle_out = float(m[1]) / 100. - if throttle_out > 0.3: - cruise_button = CruiseButtons.CANCEL - is_openpilot_engaged = False + throttle_manual = float(m[1]) + is_openpilot_engaged = False if m[0] == "brake": - brake_out = float(m[1]) / 100. - if brake_out > 0.3: - cruise_button = CruiseButtons.CANCEL - is_openpilot_engaged = False + brake_manual = float(m[1]) + is_openpilot_engaged = False if m[0] == "reverse": - in_reverse = not in_reverse + #in_reverse = not in_reverse cruise_button = CruiseButtons.CANCEL is_openpilot_engaged = False if m[0] == "cruise": @@ -194,41 +247,101 @@ def go(q): cruise_button = CruiseButtons.CANCEL is_openpilot_engaged = False + throttle_out = throttle_manual * throttle_manual_multiplier + steer_out = steer_manual * steer_manual_multiplier + brake_out = brake_manual * brake_manual_multiplier + + #steer_out = steer_out + # steer_out = steer_rate_limit(old_steer, steer_out) + old_steer = steer_out + old_throttle = throttle_out + old_brake = brake_out + + # print('message',old_throttle, old_steer, old_brake) + + if is_openpilot_engaged: + sm.update(0) + throttle_op = sm['carControl'].actuators.gas #[0,1] + brake_op = sm['carControl'].actuators.brake #[0,1] + steer_op = sm['controlsState'].angleSteersDes # degrees [-180,180] + + throttle_out = throttle_op + steer_out = steer_op + brake_out = brake_op + + steer_out = steer_rate_limit(old_steer, steer_out) + old_steer = steer_out + + # OP Exit conditions + # if throttle_out > 0.3: + # cruise_button = CruiseButtons.CANCEL + # is_openpilot_engaged = False + # if brake_out > 0.3: + # cruise_button = CruiseButtons.CANCEL + # is_openpilot_engaged = False + # if steer_out > 0.3: + # cruise_button = CruiseButtons.CANCEL + # is_openpilot_engaged = False + + else: + if throttle_out==0 and old_throttle>0: + if throttle_ease_out_counter>0: + throttle_out = old_throttle + throttle_ease_out_counter += -1 + else: + throttle_ease_out_counter = REPEAT_COUNTER + old_throttle = 0 + + if brake_out==0 and old_brake>0: + if brake_ease_out_counter>0: + brake_out = old_brake + brake_ease_out_counter += -1 + else: + brake_ease_out_counter = REPEAT_COUNTER + old_brake = 0 + + if steer_out==0 and old_steer!=0: + if steer_ease_out_counter>0: + steer_out = old_steer + steer_ease_out_counter += -1 + else: + steer_ease_out_counter = REPEAT_COUNTER + old_steer = 0 + + # --------------Step 2------------------------------- + + steer_carla = steer_out / (max_steer_angle * STEER_RATIO * -1) + + steer_carla = np.clip(steer_carla, -1,1) + steer_out = steer_carla * (max_steer_angle * STEER_RATIO * -1) + old_steer = steer_carla * (max_steer_angle * STEER_RATIO * -1) + + vc.throttle = throttle_out/0.6 + vc.steer = steer_carla + vc.brake = brake_out + vehicle.apply_control(vc) + + # --------------Step 3------------------------------- 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, is_engaged=is_openpilot_engaged) - - 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) + speed = math.sqrt(vel.x**2 + vel.y**2 + vel.z**2) # in m/s + vehicle_state.speed = speed + vehicle_state.angle = steer_out + vehicle_state.cruise_button = cruise_button + vehicle_state.is_engaged = is_openpilot_engaged + + if rk.frame%PRINT_DECIMATION == 0: + print("frame: ", "engaged:", is_openpilot_engaged, "; throttle: ", round(vc.throttle, 3), "; steer(c/deg): ", round(vc.steer, 3), round(steer_out, 3), "; brake: ", round(vc.brake, 3)) rk.keep_time() if __name__ == "__main__": + + # make sure params are in a good state params = Params() + params.clear_all() + set_params_enabled() params.delete("Offroad_ConnectivityNeeded") - from selfdrive.version import terms_version, training_version - params.put("HasAcceptedTerms", terms_version) - params.put("CompletedTrainingVersion", training_version) - params.put("CommunityFeaturesToggle", "1") - params.put("CalibrationParams", '{"vanishing_point": [582.06, 442.78], "valid_blocks": 20}') - - # no carla, still run - try: - import carla - except ImportError: - print("WARNING: NO CARLA") - while 1: - time.sleep(1) + params.put("CalibrationParams", '{"calib_radians": [0,0,0], "valid_blocks": 20}') from multiprocessing import Process, Queue q = Queue() diff --git a/tools/sim/build_container.sh b/tools/sim/build_container.sh new file mode 100755 index 0000000000..7fb94ba855 --- /dev/null +++ b/tools/sim/build_container.sh @@ -0,0 +1,10 @@ +#!/bin/bash + +DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" +cd $DIR/../../ + +docker pull commaai/openpilot-base:latest +docker build \ + --cache-from commaai/openpilot-sim:latest \ + -t commaai/openpilot-sim:latest \ + -f tools/sim/Dockerfile.sim . diff --git a/tools/sim/install_carla.sh b/tools/sim/install_carla.sh new file mode 100755 index 0000000000..652b1e0c3e --- /dev/null +++ b/tools/sim/install_carla.sh @@ -0,0 +1,15 @@ +#!/usr/bin/env bash + +cd /tmp +FILE=CARLA_0.9.7.tar.gz +rm -f $FILE +curl -O http://carla-assets-internal.s3.amazonaws.com/Releases/Linux/$FILE + +rm -rf carla_tmp +mkdir -p carla_tmp +cd carla_tmp +tar xvf ../$FILE +easy_install PythonAPI/carla/dist/carla-0.9.7-py3.5-linux-x86_64.egg || true +cd .. +rm -rf /tmp/$FILE +rm -rf carla_tmp diff --git a/tools/sim/launch_openpilot.sh b/tools/sim/launch_openpilot.sh index 207997c867..90706344f9 100755 --- a/tools/sim/launch_openpilot.sh +++ b/tools/sim/launch_openpilot.sh @@ -2,6 +2,7 @@ export PASSIVE="0" export NOBOARD="1" +export SIMULATION="1" DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null && pwd )" cd ../../selfdrive && ./manager.py diff --git a/tools/sim/lib/can.py b/tools/sim/lib/can.py index 7ae7f3f7e8..72d2212d1b 100755 --- a/tools/sim/lib/can.py +++ b/tools/sim/lib/can.py @@ -4,7 +4,6 @@ from opendbc.can.packer import CANPacker from selfdrive.boardd.boardd_api_impl import can_list_to_can_capnp # pylint: disable=no-name-in-module,import-error 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() @@ -12,19 +11,20 @@ cp = get_car_can_parser() packer = CANPacker("honda_civic_touring_2016_can_generated") rpacker = CANPacker("acura_ilx_2016_nidec") -SR = 7.5 +def can_function(pm, speed, angle, idx, cruise_button, is_engaged): -def angle_to_sangle(angle): - return - math.degrees(angle) * SR - -def can_function(pm, speed, angle, idx, cruise_button=0, is_engaged=False): msg = [] + + # *** powertrain bus *** + + speed = speed * 3.6 # convert m/s to kph msg.append(packer.make_can_msg("ENGINE_DATA", 0, {"XMISSION_SPEED": speed}, idx)) - msg.append(packer.make_can_msg("WHEEL_SPEEDS", 0, - {"WHEEL_SPEED_FL": speed, - "WHEEL_SPEED_FR": speed, - "WHEEL_SPEED_RL": speed, - "WHEEL_SPEED_RR": speed}, -1)) + msg.append(packer.make_can_msg("WHEEL_SPEEDS", 0, { + "WHEEL_SPEED_FL": speed, + "WHEEL_SPEED_FR": speed, + "WHEEL_SPEED_RL": speed, + "WHEEL_SPEED_RR": speed + }, -1)) msg.append(packer.make_can_msg("SCM_BUTTONS", 0, {"CRUISE_BUTTONS": cruise_button}, idx)) @@ -37,7 +37,7 @@ def can_function(pm, speed, angle, idx, cruise_button=0, is_engaged=False): 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_to_sangle(angle)}, idx)) + msg.append(packer.make_can_msg("STEERING_SENSORS", 0, {"STEER_ANGLE": angle}, idx)) msg.append(packer.make_can_msg("VSA_STATUS", 0, {}, idx)) msg.append(packer.make_can_msg("STANDSTILL", 0, {}, idx)) msg.append(packer.make_can_msg("STEER_MOTOR_TORQUE", 0, {}, idx)) @@ -47,14 +47,13 @@ def can_function(pm, speed, angle, idx, cruise_button=0, is_engaged=False): msg.append(packer.make_can_msg("CRUISE", 0, {}, idx)) msg.append(packer.make_can_msg("SCM_FEEDBACK", 0, {"MAIN_ON": 1}, idx)) msg.append(packer.make_can_msg("POWERTRAIN_DATA", 0, {"ACC_STATUS": int(is_engaged)}, idx)) - #print(msg) - # cam bus + # *** cam bus *** msg.append(packer.make_can_msg("STEERING_CONTROL", 2, {}, idx)) msg.append(packer.make_can_msg("ACC_HUD", 2, {}, idx)) msg.append(packer.make_can_msg("BRAKE_COMMAND", 2, {}, idx)) - # radar + # *** radar bus *** if idx % 5 == 0: msg.append(rpacker.make_can_msg("RADAR_DIAGNOSTIC", 1, {"RADAR_STATE": 0x79}, -1)) for i in range(16): @@ -65,6 +64,7 @@ def can_function(pm, speed, angle, idx, cruise_button=0, is_engaged=False): for k, v in FINGERPRINTS[CAR.CIVIC][0].items(): if k not in done and k not in [0xE4, 0x194]: msg.append([k, 0, b'\x00'*v, 0]) + pm.send('can', can_list_to_can_capnp(msg)) def sendcan_function(sendcan): @@ -72,18 +72,18 @@ def sendcan_function(sendcan): cp.update_strings(sc, sendcan=True) if cp.vl[0x1fa]['COMPUTER_BRAKE_REQUEST']: - brake = cp.vl[0x1fa]['COMPUTER_BRAKE'] * 0.003906248 + brake = cp.vl[0x1fa]['COMPUTER_BRAKE'] / 1024. else: brake = 0.0 if cp.vl[0x200]['GAS_COMMAND'] > 0: - gas = cp.vl[0x200]['GAS_COMMAND'] / 256.0 + gas = ( cp.vl[0x200]['GAS_COMMAND'] + 83.3 ) / (0.253984064 * 2**16) else: gas = 0.0 if cp.vl[0xe4]['STEER_TORQUE_REQUEST']: - steer_torque = cp.vl[0xe4]['STEER_TORQUE']*1.0/0x1000 + steer_torque = cp.vl[0xe4]['STEER_TORQUE']/3840 else: steer_torque = 0.0 - return (gas, brake, steer_torque) + return gas, brake, steer_torque diff --git a/tools/sim/lib/helpers.py b/tools/sim/lib/helpers.py deleted file mode 100644 index f6bb4b8173..0000000000 --- a/tools/sim/lib/helpers.py +++ /dev/null @@ -1,34 +0,0 @@ -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. diff --git a/tools/sim/lib/keyboard_ctrl.py b/tools/sim/lib/keyboard_ctrl.py index 18a491fec0..c66bf94e20 100644 --- a/tools/sim/lib/keyboard_ctrl.py +++ b/tools/sim/lib/keyboard_ctrl.py @@ -37,7 +37,7 @@ def getch(): def keyboard_poll_thread(q): while True: c = getch() - print("got %s" % c) + # print("got %s" % c) if c == '1': q.put(str("cruise_up")) if c == '2': diff --git a/tools/sim/start_carla.sh b/tools/sim/start_carla.sh index cc5e961b81..b46dcdbe05 100755 --- a/tools/sim/start_carla.sh +++ b/tools/sim/start_carla.sh @@ -1,18 +1,19 @@ -#!/bin/bash -e +#!/bin/bash -FILE=CARLA_0.9.7.tar.gz -if [ ! -f $FILE ]; then - curl -O http://carla-assets-internal.s3.amazonaws.com/Releases/Linux/$FILE -fi -if [ ! -d carla ]; then - rm -rf carla_tmp - mkdir -p carla_tmp - cd carla_tmp - tar xvf ../$FILE - easy_install PythonAPI/carla/dist/carla-0.9.7-py3.5-linux-x86_64.egg || true - cd ../ - mv carla_tmp carla +#Requires nvidia docker - https://github.com/NVIDIA/nvidia-docker +if ! $(apt list --installed | grep -q nvidia-container-toolkit); then + if [ -z "$INSTALL" ]; then + echo "Nvidia docker is required. Re-run with INSTALL=1 to automatically install." + exit 0 + else + distribution=$(. /etc/os-release;echo $ID$VERSION_ID) + echo $distribution + curl -s -L https://nvidia.github.io/nvidia-docker/gpgkey | sudo apt-key add - + curl -s -L https://nvidia.github.io/nvidia-docker/$distribution/nvidia-docker.list | sudo tee /etc/apt/sources.list.d/nvidia-docker.list + sudo apt-get update && sudo apt-get install -y nvidia-container-toolkit + sudo systemctl restart docker + fi fi -cd carla -./CarlaUE4.sh +docker pull carlasim/carla:0.9.7 +docker run --net=host --gpus all carlasim/carla:0.9.7 diff --git a/tools/sim/start_carla_docker.sh b/tools/sim/start_carla_docker.sh deleted file mode 100755 index 46619d5d6a..0000000000 --- a/tools/sim/start_carla_docker.sh +++ /dev/null @@ -1,5 +0,0 @@ -#!/bin/bash - -# Requires nvidia docker - https://github.com/NVIDIA/nvidia-docker -docker pull carlasim/carla:0.9.7 -docker run -p 2000-2002:2000-2002 --runtime=nvidia --gpus all carlasim/carla:0.9.7 diff --git a/tools/sim/start_openpilot_docker.sh b/tools/sim/start_openpilot_docker.sh new file mode 100755 index 0000000000..562984c43e --- /dev/null +++ b/tools/sim/start_openpilot_docker.sh @@ -0,0 +1,16 @@ +#!/bin/bash + +# expose X to the container +xhost +local:root +docker pull commaai/openpilot-sim:latest + +docker run --net=host\ + --rm \ + -it \ + --gpus all \ + --device=/dev/dri/ \ + -v /tmp/.X11-unix:/tmp/.X11-unix \ + --shm-size 1G \ + -e DISPLAY=$DISPLAY \ + commaai/openpilot-sim:latest \ + /bin/bash -c "cd tools && cd sim && sh tmux_script.sh" diff --git a/tools/sim/tmux_script.sh b/tools/sim/tmux_script.sh new file mode 100755 index 0000000000..cd293d41ee --- /dev/null +++ b/tools/sim/tmux_script.sh @@ -0,0 +1,6 @@ +#!/bin/bash +tmux new -d -s htop +tmux send-keys "./launch_openpilot.sh" ENTER +tmux neww +tmux send-keys "./bridge.py" ENTER +tmux a \ No newline at end of file diff --git a/tools/ubuntu_setup.sh b/tools/ubuntu_setup.sh index a77a1123bd..696c8c25cf 100755 --- a/tools/ubuntu_setup.sh +++ b/tools/ubuntu_setup.sh @@ -1,5 +1,6 @@ #!/bin/bash -e + sudo apt-get update && sudo apt-get install -y \ autoconf \ build-essential \