@ -1,19 +1,25 @@
#!/usr/bin/env python3
import argparse
import carla # pylint: disable=import-error
import math
import numpy as np
import time
import threading
from cereal import log
import time
import os
from multiprocessing import Process , Queue
from typing import Any
import carla # pylint: disable=import-error
import numpy as np
import pyopencl as cl
import pyopencl . array as cl_array
from lib . can import can_function
import cereal . messaging as messaging
from common . params import Params
from cereal import log
from cereal . visionipc . visionipc_pyx import VisionIpcServer , VisionStreamType # pylint: disable=no-name-in-module, import-error
from common . basedir import BASEDIR
from common . numpy_fast import clip
from common . realtime import Ratekeeper , DT_DMON
from lib . can import can_function
from common . params import Params
from common . realtime import DT_DMON , Ratekeeper
from selfdrive . car . honda . values import CruiseButtons
from selfdrive . test . helpers import set_params_enabled
@ -21,18 +27,18 @@ parser = argparse.ArgumentParser(description='Bridge between CARLA and openpilot
parser . add_argument ( ' --joystick ' , action = ' store_true ' )
parser . add_argument ( ' --low_quality ' , action = ' store_true ' )
parser . add_argument ( ' --town ' , type = str , default = ' Town04_Opt ' )
parser . add_argument ( ' --spawn_point ' , dest = ' num_selected_spawn_point ' ,
type = int , default = 16 )
parser . add_argument ( ' --spawn_point ' , dest = ' num_selected_spawn_point ' , type = int , default = 16 )
args = parser . parse_args ( )
W , H = 1164 , 874
W , H = 1928 , 1208
REPEAT_COUNTER = 5
PRINT_DECIMATION = 100
STEER_RATIO = 15.
pm = messaging . PubMaster ( [ ' roadCameraState ' , ' sensorEvents ' , ' can ' , " gpsLocationExternal " ] )
sm = messaging . SubMaster ( [ ' carControl ' , ' controlsState ' ] )
sm = messaging . SubMaster ( [ ' carControl ' , ' controlsState ' ] )
class VehicleState :
def __init__ ( self ) :
@ -40,8 +46,9 @@ class VehicleState:
self . angle = 0
self . bearing_deg = 0.0
self . vel = carla . Vector3D ( )
self . cruise_button = 0
self . is_engaged = False
self . cruise_button = 0
self . is_engaged = False
def steer_rate_limit ( old , new ) :
# Rate limiting to 0.5 degrees per step
@ -53,21 +60,56 @@ def steer_rate_limit(old, new):
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 ( ' roadCameraState ' )
dat . roadCameraState = {
" frameId " : image . frame ,
" image " : img . tobytes ( ) ,
" transform " : [ 1.0 , 0.0 , 0.0 , 0.0 , 1.0 , 0.0 , 0.0 , 0.0 , 1.0 ]
}
pm . send ( ' roadCameraState ' , dat )
frame_id + = 1
class Camerad :
def __init__ ( self ) :
self . frame_id = 0
self . vipc_server = VisionIpcServer ( " camerad " )
# TODO: remove RGB buffers once the last RGB vipc subscriber is removed
self . vipc_server . create_buffers ( VisionStreamType . VISION_STREAM_RGB_BACK , 4 , True , W , H )
self . vipc_server . create_buffers ( VisionStreamType . VISION_STREAM_YUV_BACK , 40 , False , W , H )
self . vipc_server . start_listener ( )
# set up for pyopencl rgb to yuv conversion
self . ctx = cl . create_some_context ( )
self . queue = cl . CommandQueue ( self . ctx )
cl_arg = f " -DHEIGHT= { H } -DWIDTH= { W } -DRGB_STRIDE= { W * 3 } -DUV_WIDTH= { W / / 2 } -DUV_HEIGHT= { H / / 2 } -DRGB_SIZE= { W * H } -DCL_DEBUG "
# TODO: move rgb_to_yuv.cl to local dir once the frame stream camera is removed
kernel_fn = os . path . join ( BASEDIR , " selfdrive " , " camerad " , " transforms " , " rgb_to_yuv.cl " )
prg = cl . Program ( self . ctx , open ( kernel_fn ) . read ( ) ) . build ( cl_arg )
self . krnl = prg . rgb_to_yuv
self . Wdiv4 = W / / 4 if ( W % 4 == 0 ) else ( W + ( 4 - W % 4 ) ) / / 4
self . Hdiv4 = H / / 4 if ( H % 4 == 0 ) else ( H + ( 4 - H % 4 ) ) / / 4
def cam_callback ( self , image ) :
img = np . frombuffer ( image . raw_data , dtype = np . dtype ( " uint8 " ) )
img = np . reshape ( img , ( H , W , 4 ) )
img = img [ : , : , [ 0 , 1 , 2 ] ] . copy ( )
# convert RGB frame to YUV
rgb = np . reshape ( img , ( H , W * 3 ) )
rgb_cl = cl_array . to_device ( self . queue , rgb )
yuv_cl = cl_array . empty_like ( rgb_cl )
self . krnl ( self . queue , ( np . int32 ( self . Wdiv4 ) , np . int32 ( self . Hdiv4 ) ) , None , rgb_cl . data , yuv_cl . data ) . wait ( )
yuv = np . resize ( yuv_cl . get ( ) , np . int32 ( ( rgb . size / 2 ) ) )
eof = self . frame_id * 0.05
# TODO: remove RGB send once the last RGB vipc subscriber is removed
self . vipc_server . send ( VisionStreamType . VISION_STREAM_RGB_BACK , img . tobytes ( ) , self . frame_id , eof , eof )
self . vipc_server . send ( VisionStreamType . VISION_STREAM_YUV_BACK , yuv . data . tobytes ( ) , self . frame_id , eof , eof )
dat = messaging . new_message ( ' roadCameraState ' )
dat . roadCameraState = {
" frameId " : image . frame ,
" transform " : [ 1.0 , 0.0 , 0.0 ,
0.0 , 1.0 , 0.0 ,
0.0 , 0.0 , 1.0 ]
}
pm . send ( ' roadCameraState ' , dat )
self . frame_id + = 1
def imu_callback ( imu , vehicle_state ) :
vehicle_state . bearing_deg = math . degrees ( imu . compass )
@ -83,6 +125,7 @@ def imu_callback(imu, vehicle_state):
dat . sensorEvents [ 1 ] . gyroUncalibrated . v = [ imu . gyroscope . x , imu . gyroscope . y , imu . gyroscope . z ]
pm . send ( ' sensorEvents ' , dat )
def panda_state_function ( exit_event : threading . Event ) :
pm = messaging . PubMaster ( [ ' pandaStates ' ] )
while not exit_event . is_set ( ) :
@ -97,6 +140,7 @@ def panda_state_function(exit_event: threading.Event):
pm . send ( ' pandaStates ' , dat )
time . sleep ( 0.5 )
def peripheral_state_function ( exit_event : threading . Event ) :
pm = messaging . PubMaster ( [ ' peripheralState ' ] )
while not exit_event . is_set ( ) :
@ -112,20 +156,21 @@ def peripheral_state_function(exit_event: threading.Event):
pm . send ( ' peripheralState ' , dat )
time . sleep ( 0.5 )
def gps_callback ( gps , vehicle_state ) :
dat = messaging . new_message ( ' gpsLocationExternal ' )
# transform vel from carla to NED
# north is -Y in CARLA
velNED = [
- vehicle_state . vel . y , # north/south component of NED is negative when moving south
vehicle_state . vel . x , # positive when moving east, which is x in carla
- vehicle_state . vel . y , # north/south component of NED is negative when moving south
vehicle_state . vel . x , # positive when moving east, which is x in carla
vehicle_state . vel . z ,
]
dat . gpsLocationExternal = {
" timestamp " : int ( time . time ( ) * 1000 ) ,
" flags " : 1 , # valid fix
" flags " : 1 , # valid fix
" accuracy " : 1.0 ,
" verticalAccuracy " : 1.0 ,
" speedAccuracy " : 0.1 ,
@ -141,8 +186,9 @@ def gps_callback(gps, vehicle_state):
pm . send ( ' gpsLocationExternal ' , dat )
def fake_driver_monitoring ( exit_event : threading . Event ) :
pm = messaging . PubMaster ( [ ' driverState ' , ' driverMonitoringState ' ] )
pm = messaging . PubMaster ( [ ' driverState ' , ' driverMonitoringState ' ] )
while not exit_event . is_set ( ) :
# dmonitoringmodeld output
dat = messaging . new_message ( ' driverState ' )
@ -160,16 +206,16 @@ def fake_driver_monitoring(exit_event: threading.Event):
time . sleep ( DT_DMON )
def can_function_runner ( vs : VehicleState , exit_event : threading . Event ) :
i = 0
while not exit_event . is_set ( ) :
can_function ( pm , vs . speed , vs . angle , i , vs . cruise_button , vs . is_engaged )
time . sleep ( 0.01 )
i + = 1
i + = 1
def bridge ( q ) :
# setup CARLA
client = carla . Client ( " 127.0.0.1 " , 2000 )
client . set_timeout ( 10.0 )
@ -209,11 +255,12 @@ def bridge(q):
blueprint = blueprint_library . find ( ' sensor.camera.rgb ' )
blueprint . set_attribute ( ' image_size_x ' , str ( W ) )
blueprint . set_attribute ( ' image_size_y ' , str ( H ) )
blueprint . set_attribute ( ' fov ' , ' 7 0' )
blueprint . set_attribute ( ' fov ' , ' 4 0' )
blueprint . set_attribute ( ' sensor_tick ' , ' 0.05 ' )
transform = carla . Transform ( carla . Location ( x = 0.8 , z = 1.13 ) )
camera = world . spawn_actor ( blueprint , transform , attach_to = vehicle )
camera . listen ( cam_callback )
camerad = Camerad ( )
camera . listen ( camerad . cam_callback )
vehicle_state = VehicleState ( )
@ -244,7 +291,6 @@ def bridge(q):
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
@ -253,12 +299,11 @@ def bridge(q):
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_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
while 1 :
while True :
# 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
@ -282,7 +327,6 @@ def bridge(q):
brake_manual = float ( m [ 1 ] )
is_openpilot_engaged = False
elif m [ 0 ] == " reverse " :
#in_reverse = not in_reverse
cruise_button = CruiseButtons . CANCEL
is_openpilot_engaged = False
elif m [ 0 ] == " cruise " :
@ -302,19 +346,16 @@ def bridge(q):
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 )
# TODO gas and brake is deprecated
throttle_op = clip ( sm [ ' carControl ' ] . actuators . accel / 1.6 , 0.0 , 1.0 )
brake_op = clip ( - sm [ ' carControl ' ] . actuators . accel / 4.0 , 0.0 , 1.0 )
throttle_op = clip ( sm [ ' carControl ' ] . actuators . accel / 1.6 , 0.0 , 1.0 )
brake_op = clip ( - sm [ ' carControl ' ] . actuators . accel / 4.0 , 0.0 , 1.0 )
steer_op = sm [ ' carControl ' ] . actuators . steeringAngleDeg
throttle_out = throttle_op
@ -325,24 +366,24 @@ def bridge(q):
old_steer = steer_out
else :
if throttle_out == 0 and old_throttle > 0 :
if throttle_ease_out_counter > 0 :
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 :
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 :
if steer_out == 0 and old_steer != 0 :
if steer_ease_out_counter > 0 :
steer_out = old_steer
steer_ease_out_counter + = - 1
else :
@ -350,28 +391,27 @@ def bridge(q):
old_steer = 0
# --------------Step 2-------------------------------
steer_carla = steer_out / ( max_steer_angle * STEER_RATIO * - 1 )
steer_carla = np . clip ( steer_carla , - 1 , 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 . 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 ) # in m/s
speed = math . sqrt ( vel . x * * 2 + vel . y * * 2 + vel . z * * 2 ) # in m/s
vehicle_state . speed = speed
vehicle_state . vel = vel
vehicle_state . angle = steer_out
vehicle_state . cruise_button = cruise_button
vehicle_state . is_engaged = is_openpilot_engaged
if rk . frame % PRINT_DECIMATION == 0 :
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 ( )
@ -385,6 +425,7 @@ def bridge(q):
camera . destroy ( )
vehicle . destroy ( )
def bridge_keep_alive ( q : Any ) :
while 1 :
try :
@ -393,6 +434,7 @@ def bridge_keep_alive(q: Any):
except RuntimeError :
print ( " Restarting bridge... " )
if __name__ == " __main__ " :
# make sure params are in a good state
set_params_enabled ( )
jimw
4 years ago
committed by
GitHub