class DonkeyUnitySimHandler(IMesgHandler):
#cross track error max
CTE_MAX_ERR = 5.0
FPS = 60.0
def __init__(self, level, time_step=0.05):
self.iSceneToLoad = level
self.time_step = time_step
self.wait_time_for_obs = 0.1
self.sock = None
self.loaded = False
self.verbose = False
self.timer = FPSTimer()
# sensor size - height, width, depth
self.camera_img_size = (80, 160, 3)
self.image_array = np.zeros(self.camera_img_size)
self.last_obs = None
self.last_throttle = 0.0
self.hit = "none"
self.cte = 0.0
self.x = 0.0
self.y = 0.0
self.z = 0.0
self.fns = {
'telemetry': self.on_telemetry,
"scene_selection_ready": self.on_scene_selection_ready,
"scene_names": self.on_recv_scene_names,
"car_loaded": self.on_car_loaded
}
def on_connect(self, socketHandler):
self.sock = socketHandler
def on_disconnect(self):
self.sock = None
def on_recv_message(self, message):
if not 'msg_type' in message:
print('expected msg_type field')
return
msg_type = message['msg_type']
if msg_type in self.fns:
self.fns[msg_type](message)
else:
print('unknown message type', msg_type)
## ------- Env interface ---------- ##
def reset(self):
if self.verbose:
print("reseting")
self.image_array = np.zeros(self.camera_img_size)
self.last_obs = None
self.hit = "none"
self.cte = 0.0
self.x = 0.0
self.y = 0.0
self.z = 0.0
self.send_reset_car()
time.sleep(1.0)
self.timer.reset()
def get_sensor_size(self):
return self.camera_img_size
def take_action(self, action):
if self.verbose:
print("take_action")
# Static throttle
throttle = 0.5
self.last_throttle = throttle
self.send_control(action[0], throttle)
def observe(self):
while self.last_obs is self.image_array:
time.sleep(1.0 / 120.0)
self.last_obs = self.image_array
observation = self.image_array
done = self.is_game_over()
reward = self.calc_reward(done)
info = {}
self.timer.on_frame()
return observation, reward, done, info
def is_game_over(self):
# Workarund for big error at start.
if math.fabs(self.cte) > 2 * self.CTE_MAX_ERR:
return False
return self.hit != "none" or math.fabs(self.cte) > self.CTE_MAX_ERR
## ------ RL interface ----------- ##
# Use velocity (m/s) as reward for every step,
# except when episode done (failed).
def calc_reward(self, done):
if done:
return 0.0
velocity = self.last_throttle * (1.0 / self.FPS)
return velocity
## ------ Socket interface ----------- ##
def on_telemetry(self, data):
imgString = data["image"]
image = Image.open(BytesIO(base64.b64decode(imgString)))
# Crop to the zone of interest - remove top third.
# Crop image to size 80x160x3.
self.image_array = np.delete(np.asarray(image), np.s_[0:40:], axis=0)
#name of object we just hit. "none" if nothing.
if self.hit == "none":
self.hit = data["hit"]
self.x = data["pos_x"]
self.y = data["pos_y"]
self.z = data["pos_z"]
#Cross track error not always present.
#Will be missing if path is not setup in the given scene.
#It should be setup in the 3 scenes available now.
try:
self.cte = data["cte"]
except:
pass
def on_scene_selection_ready(self, data):
print("SceneSelectionReady ")
self.send_get_scene_names()
def on_car_loaded(self, data):
if self.verbose:
print("car loaded")
self.loaded = True
def on_recv_scene_names(self, data):
if data:
names = data['scene_names']
if self.verbose:
print("SceneNames:", names)
self.send_load_scene(names[self.iSceneToLoad])
def send_control(self, steer, throttle):
if not self.loaded:
return
msg = {
'msg_type': 'control',
'steering': steer.__str__(),
'throttle': throttle.__str__(),
'brake': '0.0'
}
self.queue_message(msg)
def send_reset_car(self):
msg = {'msg_type': 'reset_car'}
self.queue_message(msg)
def send_get_scene_names(self):
msg = {'msg_type': 'get_scene_names'}
self.queue_message(msg)
def send_load_scene(self, scene_name):
msg = {'msg_type': 'load_scene', 'scene_name': scene_name}
self.queue_message(msg)
def queue_message(self, msg):
if self.sock is None:
if self.verbose:
print('skiping:', msg)
return
if self.verbose:
print('sending', msg)
self.sock.queue_message(msg)
class DonkeyUnitySimHandler(IMesgHandler):
"""
Socket message handler.
:param level: (int) Level ID
:param max_cte_error: (float) Max cross track error before reset
"""
def __init__(self, level, max_cte_error=3.0):
self.level_idx = level
self.sock = None
self.loaded = False
self.verbose = False
self.timer = FPSTimer(verbose=0)
self.max_cte_error = max_cte_error
# sensor size - height, width, depth
self.camera_img_size = INPUT_DIM
self.image_array = np.zeros(self.camera_img_size)
self.original_image = None
self.last_obs = None
self.last_throttle = 0.0
self.hit = "none"
self.cte = 0.0
self.x = 0.0
self.y = 0.0
self.z = 0.0
self.steering_angle = 0.0
self.current_step = 0
self.speed = 0
self.steering = None
# Define which method should be called
# for each type of message
self.fns = {
'telemetry': self.on_telemetry,
"scene_selection_ready": self.on_scene_selection_ready,
"scene_names": self.on_recv_scene_names,
"car_loaded": self.on_car_loaded
}
def on_connect(self, socket_handler):
"""
:param socket_handler: (socket object)
"""
self.sock = socket_handler
def on_disconnect(self):
"""
Close socket
"""
self.sock.close()
self.sock = None
def on_recv_message(self, message):
"""
Distribute the received message
to the appropriate function.
:param message: (dict)
"""
if 'msg_type' not in message:
print('Expected msg_type field')
return
msg_type = message['msg_type']
if msg_type in self.fns:
self.fns[msg_type](message)
else:
print('Unknown message type', msg_type)
def reset(self):
"""
Global reset, notably it
resets car to initial position.
"""
if self.verbose:
print("reseting")
self.image_array = np.zeros(self.camera_img_size)
self.last_obs = None
self.hit = "none"
self.cte = 0.0
self.x = 0.0
self.y = 0.0
self.z = 0.0
self.current_step = 0
self.send_reset_car()
self.send_control(0, 0)
time.sleep(1.0)
self.timer.reset()
def get_sensor_size(self):
"""
:return: (tuple)
"""
return self.camera_img_size
def take_action(self, action):
"""
:param action: ([float]) Steering and throttle
"""
if self.verbose:
print("take_action")
throttle = action[1]
self.steering = action[0]
self.last_throttle = throttle
self.current_step += 1
self.send_control(self.steering, throttle)
def observe(self):
while self.last_obs is self.image_array:
time.sleep(1.0 / 120.0)
self.last_obs = self.image_array
observation = self.image_array
done = self.is_game_over()
reward = self.calc_reward(done)
info = {}
self.timer.on_frame()
return observation, reward, done, info
def is_game_over(self):
"""
:return: (bool)
"""
return self.hit != "none" or math.fabs(self.cte) > self.max_cte_error
def calc_reward(self, done):
"""
Compute reward:
- +1 life bonus for each step + throttle bonus
- -10 crash penalty - penalty for large throttle during a crash
:param done: (bool)
:return: (float)
"""
if done:
# penalize the agent for getting off the road fast
norm_throttle = (self.last_throttle -
MIN_THROTTLE) / (MAX_THROTTLE - MIN_THROTTLE)
return REWARD_CRASH - CRASH_SPEED_WEIGHT * norm_throttle
# 1 per timesteps + throttle
throttle_reward = THROTTLE_REWARD_WEIGHT * (self.last_throttle /
MAX_THROTTLE)
return 1 + throttle_reward
# ------ Socket interface ----------- #
def on_telemetry(self, data):
"""
Update car info when receiving telemetry message.
:param data: (dict)
"""
img_string = data["image"]
image = Image.open(BytesIO(base64.b64decode(img_string)))
# Resize and crop image
image = np.array(image)
# Save original image for render
self.original_image = np.copy(image)
# Resize if using higher resolution images
# image = cv2.resize(image, CAMERA_RESOLUTION)
# Region of interest
r = ROI
image = image[int(r[1]):int(r[1] + r[3]), int(r[0]):int(r[0] + r[2])]
# Convert RGB to BGR
# image = image[:, :, ::-1]
self.image_array = image
# Here resize is not useful for now (the image have already the right dimension)
# self.image_array = cv2.resize(image, (IMAGE_WIDTH, IMAGE_HEIGHT))
# name of object we just hit. "none" if nothing.
# NOTE: obstacle detection disabled
# if self.hit == "none":
# self.hit = data["hit"]
self.x = data["pos_x"]
self.y = data["pos_y"]
self.z = data["pos_z"]
self.steering_angle = data['steering_angle']
self.speed = data["speed"]
# Cross track error not always present.
# Will be missing if path is not setup in the given scene.
# It should be setup in the 3 scenes available now.
try:
self.cte = data["cte"]
# print(self.cte)
except KeyError:
print("No Cross Track Error in telemetry")
pass
def on_scene_selection_ready(self, _data):
"""
Get the level names when the scene selection screen is ready
"""
print("Scene Selection Ready")
self.send_get_scene_names()
def on_car_loaded(self, _data):
if self.verbose:
print("Car Loaded")
self.loaded = True
def on_recv_scene_names(self, data):
"""
Select the level.
:param data: (dict)
"""
if data is not None:
names = data['scene_names']
if self.verbose:
print("SceneNames:", names)
self.send_load_scene(names[self.level_idx])
def send_control(self, steer, throttle):
"""
Send message to the server for controlling the car.
:param steer: (float)
:param throttle: (float)
"""
if not self.loaded:
return
msg = {
'msg_type': 'control',
'steering': steer.__str__(),
'throttle': throttle.__str__(),
'brake': '0.0'
}
self.queue_message(msg)
def send_reset_car(self):
"""
Reset car to initial position.
"""
msg = {'msg_type': 'reset_car'}
self.queue_message(msg)
def send_get_scene_names(self):
"""
Get the different levels availables
"""
msg = {'msg_type': 'get_scene_names'}
self.queue_message(msg)
def send_load_scene(self, scene_name):
"""
Load a level.
:param scene_name: (str)
"""
msg = {'msg_type': 'load_scene', 'scene_name': scene_name}
self.queue_message(msg)
def send_exit_scene(self):
"""
Go back to scene selection.
"""
msg = {'msg_type': 'exit_scene'}
self.queue_message(msg)
def queue_message(self, msg):
"""
Add message to socket queue.
:param msg: (dict)
"""
if self.sock is None:
if self.verbose:
print('skipping:', msg)
return
if self.verbose:
print('sending', msg)
self.sock.queue_message(msg)
class DonkeyUnitySimHandler(IMesgHandler):
def __init__(self,
level,
time_step=0.05,
max_cte=5.0,
verbose=False,
cam_resolution=None):
self.iSceneToLoad = level
self.time_step = time_step
self.wait_time_for_obs = 0.1
self.sock = None
self.loaded = False
self.verbose = verbose
self.max_cte = max_cte
self.timer = FPSTimer()
# sensor size - height, width, depth
self.camera_img_size = cam_resolution
self.image_array = np.zeros(self.camera_img_size)
self.last_obs = None
self.hit = "none"
self.cte = 0.0
self.x = 0.0
self.y = 0.0
self.z = 0.0
self.speed = 0.0
self.over = False
self.fns = {
'telemetry': self.on_telemetry,
"scene_selection_ready": self.on_scene_selection_ready,
"scene_names": self.on_recv_scene_names,
"car_loaded": self.on_car_loaded
}
def on_connect(self, socketHandler):
self.sock = socketHandler
def on_disconnect(self):
self.sock = None
def on_recv_message(self, message):
if not 'msg_type' in message:
print('expected msg_type field')
return
msg_type = message['msg_type']
if msg_type in self.fns:
self.fns[msg_type](message)
else:
print('unknown message type', msg_type)
## ------- Env interface ---------- ##
def reset(self):
if self.verbose:
print("reseting")
self.image_array = np.zeros(self.camera_img_size)
self.last_obs = self.image_array
self.hit = "none"
self.cte = 0.0
self.x = 0.0
self.y = 0.0
self.z = 0.0
self.speed = 0.0
self.over = False
self.send_reset_car()
self.timer.reset()
time.sleep(1)
def get_sensor_size(self):
return self.camera_img_size
def take_action(self, action):
if self.verbose:
print("take_action")
self.send_control(action[0], action[1])
def observe(self):
while self.last_obs is self.image_array:
time.sleep(1.0 / 120.0)
self.last_obs = self.image_array
observation = self.image_array
done = self.is_game_over()
reward = self.calc_reward(done)
info = {
'pos': (self.x, self.y, self.z),
'cte': self.cte,
"speed": self.speed,
"hit": self.hit
}
self.timer.on_frame()
return observation, reward, done, info
def is_game_over(self):
return self.over
## ------ RL interface ----------- ##
def calc_reward(self, done):
if done:
return -1.0
if self.cte > self.max_cte:
return -1.0
if self.hit != "none":
return -2.0
#going fast close to the center of lane yeilds best reward
return 1.0 - (self.cte / self.max_cte) * self.speed
## ------ Socket interface ----------- ##
def on_telemetry(self, data):
imgString = data["image"]
image = Image.open(BytesIO(base64.b64decode(imgString)))
#always update the image_array as the observation loop will hang if not changing.
self.image_array = np.asarray(image)
self.x = data["pos_x"]
self.y = data["pos_y"]
self.z = data["pos_z"]
self.speed = data["speed"]
#Cross track error not always present.
#Will be missing if path is not setup in the given scene.
#It should be setup in the 4 scenes available now.
if "cte" in data:
self.cte = data["cte"]
#don't update hit once session over
if self.over:
return
self.hit = data["hit"]
self.determine_episode_over()
def determine_episode_over(self):
#we have a few initial frames on start that are sometimes very large CTE when it's behind
#the path just slightly. We ignore those.
if math.fabs(self.cte) > 2 * self.max_cte:
pass
elif math.fabs(self.cte) > self.max_cte:
if self.verbose:
print("game over: cte", self.cte)
self.over = True
elif self.hit != "none":
if self.verbose:
print("game over: hit", self.hit)
self.over = True
def on_scene_selection_ready(self, data):
print("SceneSelectionReady ")
self.send_get_scene_names()
def on_car_loaded(self, data):
if self.verbose:
print("car loaded")
self.loaded = True
def on_recv_scene_names(self, data):
if data:
names = data['scene_names']
if self.verbose:
print("SceneNames:", names)
self.send_load_scene(names[self.iSceneToLoad])
def send_control(self, steer, throttle):
if not self.loaded:
return
msg = {
'msg_type': 'control',
'steering': steer.__str__(),
'throttle': throttle.__str__(),
'brake': '0.0'
}
self.queue_message(msg)
def send_reset_car(self):
msg = {'msg_type': 'reset_car'}
self.queue_message(msg)
def send_get_scene_names(self):
msg = {'msg_type': 'get_scene_names'}
self.queue_message(msg)
def send_load_scene(self, scene_name):
msg = {'msg_type': 'load_scene', 'scene_name': scene_name}
self.queue_message(msg)
def queue_message(self, msg):
if self.sock is None:
if self.verbose:
print('skiping:', msg)
return
if self.verbose:
print('sending', msg)
self.sock.queue_message(msg)
class DonkeySimMsgHandler(IMesgHandler):
STEERING = 0
THROTTLE = 1
def __init__(self, model, constant_throttle, movie_handler=None):
self.model = model
self.constant_throttle = constant_throttle
self.sock = None
self.timer = FPSTimer()
self.image_folder = None
self.movie_handler = movie_handler
self.fns = {'telemetry' : self.on_telemetry}
def on_connect(self, socketHandler):
self.sock = socketHandler
self.timer.reset()
def on_recv_message(self, message):
self.timer.on_frame()
if not 'msg_type' in message:
print('expected msg_type field')
return
msg_type = message['msg_type']
if msg_type in self.fns:
self.fns[msg_type](message)
else:
print('unknown message type', msg_type)
def on_telemetry(self, data):
imgString = data["image"]
image = Image.open(BytesIO(base64.b64decode(imgString)))
image_array = np.asarray(image)
self.predict(image_array)
# maybe write movie
if self.movie_handler is not None:
self.movie_handler.add_image(image_array)
def predict(self, image_array):
outputs = self.model.predict(image_array[None, :, :, :])
self.parse_outputs(outputs)
def parse_outputs(self, outputs):
res = []
for iO, output in enumerate(outputs):
if len(output.shape) == 2:
if iO == self.STEERING:
steering_angle = linear_unbin(output)
res.append(steering_angle)
elif iO == self.THROTTLE:
throttle = linear_unbin(output, N=output.shape[1], offset=0.0, R=0.5)
res.append(throttle)
else:
res.append( np.argmax(output) )
else:
for i in range(output.shape[0]):
res.append(output[i])
self.on_parsed_outputs(res)
def on_parsed_outputs(self, outputs):
self.outputs = outputs
steering_angle = 0.0
throttle = 0.2
if len(outputs) > 0:
steering_angle = outputs[self.STEERING]
if self.constant_throttle != 0.0:
throttle = self.constant_throttle
elif len(outputs) > 1:
throttle = outputs[self.THROTTLE] * conf.throttle_out_scale
self.send_control(steering_angle, throttle)
def send_control(self, steer, throttle):
msg = { 'msg_type' : 'control', 'steering': steer.__str__(), 'throttle':throttle.__str__(), 'brake': '0.0' }
#print(steer, throttle)
self.sock.queue_message(msg)
def on_disconnect(self):
if self.movie_handler:
self.movie_handler.close()