def test_fps():
"""
it should increment iter on_frame
it should reset t and iter
"""
timer = FPSTimer()
assert timer.iter == 0
timer.on_frame()
assert timer.iter == 1
timer.reset()
assert timer.iter == 0
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, client):
self.client = client
self.timer.reset()
def on_recv_message(self, message):
self.timer.on_frame()
if not 'msg_type' in message:
print('expected msg_type field')
print('got:', message)
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.client.queue_message(msg)
def on_disconnect(self):
if self.movie_handler:
self.movie_handler.close()
class DonkeyUnitySimHandler(IMesgHandler):
def __init__(self, conf):
self.conf = conf
self.SceneToLoad = conf["level"]
self.loaded = False
self.max_cte = conf["max_cte"]
self.timer = FPSTimer()
# sensor size - height, width, depth
self.camera_img_size = conf["cam_resolution"]
self.image_array = np.zeros(self.camera_img_size)
self.image_array_b = None
self.last_obs = self.image_array
self.conf["cam_config"] = {}
self.conf["cam_config"]["img_h"] = self.camera_img_size[0]
self.conf["cam_config"]["img_w"] = self.camera_img_size[1]
self.time_received = time.time()
self.last_received = self.time_received
self.hit = "none"
self.cte = 0.0
self.x = 0.0
self.y = 0.0
self.z = 0.0
self.speed = 0.0
self.missed_checkpoint = False
self.starting_line_num = 0
self.lap_times = []
self.dq = False
self.over = False
self.client = None
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,
"cross_start": self.on_cross_start,
"race_start": self.on_race_start,
"race_stop": self.on_race_stop,
"DQ": self.on_DQ,
"ping": self.on_ping,
"aborted": self.on_abort,
"collision_with_starting_line":
self.on_collision_with_starting_line,
"missed_checkpoint": self.on_missed_checkpoint,
"need_car_config": self.on_need_car_config,
}
self.gyro_x = 0.0
self.gyro_y = 0.0
self.gyro_z = 0.0
self.accel_x = 0.0
self.accel_y = 0.0
self.accel_z = 0.0
self.vel_x = 0.0
self.vel_y = 0.0
self.vel_z = 0.0
self.lidar = []
# car in Unity lefthand coordinate system: roll is Z, pitch is X and yaw is Y
self.roll = 0.0
self.pitch = 0.0
self.yaw = 0.0
# variables required for lidar points decoding into array format
self.lidar_deg_per_sweep_inc = 1
self.lidar_num_sweep_levels = 1
self.lidar_deg_ang_delta = 1
def on_connect(self, client):
logger.debug("socket connected")
self.client = client
def on_disconnect(self):
logger.debug("socket disconnected")
self.client = None
def on_abort(self, message):
self.client.stop()
def on_need_car_config(self, message):
logger.info("on need car config")
self.loaded = True
self.send_config(self.conf)
@staticmethod
def extract_keys(dct, lst):
ret_dct = {}
for key in lst:
if key in dct:
ret_dct[key] = dct[key]
return ret_dct
def send_config(self, conf):
# both ways work, car_config shouldn't interfere with other config, so keeping the two alternative
self.set_car_config(conf)
if "car_config" in conf.keys():
self.set_car_config(conf["car_config"])
logger.info("done sending car config.")
if "random_start_config" in conf.keys():
flag = conf["random_start_config"]["flag"]
self.send_random_start_config(flag)
logger.info(f"done sending random start config. {flag}")
if "cam_config" in conf.keys():
cam_config = self.extract_keys(
conf["cam_config"],
[
"img_w",
"img_h",
"img_d",
"img_enc",
"fov",
"fish_eye_x",
"fish_eye_y",
"offset_x",
"offset_y",
"offset_z",
"rot_x",
"rot_y",
"rot_z",
],
)
self.send_cam_config(**cam_config)
logger.info(f"done sending cam config. {cam_config}")
if "cam_config_b" in conf.keys():
cam_config_b = self.extract_keys(
conf["cam_config_b"],
[
"img_w",
"img_h",
"img_d",
"img_enc",
"fov",
"fish_eye_x",
"fish_eye_y",
"offset_x",
"offset_y",
"offset_z",
"rot_x",
"rot_y",
"rot_z",
],
)
self.send_cam_config(**cam_config_b, msg_type="cam_config_b")
logger.info(f"done sending cam config B. {cam_config_b}")
self.image_array_b = np.zeros(self.camera_img_size)
if "lidar_config" in conf.keys():
lidar_config = self.extract_keys(
conf["lidar_config"],
[
"degPerSweepInc",
"degAngDown",
"degAngDelta",
"numSweepsLevels",
"maxRange",
"noise",
"offset_x",
"offset_y",
"offset_z",
"rot_x",
],
)
self.send_lidar_config(**lidar_config)
logger.info(f"done sending lidar config., {lidar_config}")
# what follows is needed in order not to break older conf
cam_config = self.extract_keys(
conf,
[
"img_w",
"img_h",
"img_d",
"img_enc",
"fov",
"fish_eye_x",
"fish_eye_y",
"offset_x",
"offset_y",
"offset_z",
"rot_x",
"rot_y",
"rot_z",
],
)
if cam_config != {}:
self.send_cam_config(**cam_config)
logger.info(f"done sending cam config. {cam_config}")
logger.warning("""This way of passing cam_config is deprecated,
please wrap the parameters in a sub-dictionary with the key 'lidar_config'.
Example: GYM_CONF = {'cam_config':""" + str(cam_config) + "}")
lidar_config = self.extract_keys(
conf,
[
"degPerSweepInc",
"degAngDown",
"degAngDelta",
"numSweepsLevels",
"maxRange",
"noise",
"offset_x",
"offset_y",
"offset_z",
"rot_x",
],
)
if lidar_config != {}:
self.send_lidar_config(**lidar_config)
logger.info(f"done sending lidar config., {lidar_config}")
logger.warning("""This way of passing lidar_config is deprecated,
please wrap the parameters in a sub-dictionary with the key 'lidar_config'.
Example: GYM_CONF = {'lidar_config':""" + str(lidar_config) +
"}")
def set_car_config(self, conf):
if "body_style" in conf:
self.send_car_config(
conf["body_style"],
conf["body_rgb"],
conf["car_name"],
conf["font_size"],
)
def set_racer_bio(self, conf):
self.conf = conf
if "bio" in conf:
self.send_racer_bio(
conf["racer_name"],
conf["car_name"],
conf["bio"],
conf["country"],
conf["guid"],
)
def on_recv_message(self, message):
if "msg_type" not in message:
logger.warn("expected msg_type field")
return
msg_type = message["msg_type"]
logger.debug("got message :" + msg_type)
if msg_type in self.fns:
self.fns[msg_type](message)
else:
logger.warning(f"unknown message type {msg_type}")
# ------- Env interface ---------- #
def reset(self):
logger.debug("reseting")
self.send_reset_car()
self.timer.reset()
time.sleep(1)
self.image_array = np.zeros(self.camera_img_size)
self.image_array_b = None
self.last_obs = self.image_array
self.time_received = time.time()
self.last_received = self.time_received
self.hit = "none"
self.cte = 0.0
self.x = 0.0
self.y = 0.0
self.z = 0.0
self.speed = 0.0
self.throttle = 0.0
self.over = False
self.starting_line_num = -1
self.time_stamps = []
self.lap_times = []
self.missed_checkpoint = False
self.dq = False
self.gyro_x = 0.0
self.gyro_y = 0.0
self.gyro_z = 0.0
self.accel_x = 0.0
self.accel_y = 0.0
self.accel_z = 0.0
self.vel_x = 0.0
self.vel_y = 0.0
self.vel_z = 0.0
self.lidar = []
# car
self.roll = 0.0
self.pitch = 0.0
self.yaw = 0.0
def get_sensor_size(self):
return self.camera_img_size
def take_action(self, action):
self.send_control(action[0], action[1])
def observe(self):
while self.last_received == self.time_received:
time.sleep(0.001)
self.last_received = self.time_received
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,
"gyro": (self.gyro_x, self.gyro_y, self.gyro_z),
"accel": (self.accel_x, self.accel_y, self.accel_z),
"vel": (self.vel_x, self.vel_y, self.vel_z),
"lidar": (self.lidar),
"car": (self.roll, self.pitch, self.yaw),
"starting_line_num": self.starting_line_num,
"lap_times": self.lap_times
}
# Add the second image to the dict
if self.image_array_b is not None:
info["image_b"] = self.image_array_b
self.timer.on_frame()
return observation, reward, done, info
def is_game_over(self):
return self.over
# ------ RL interface ----------- #
def set_reward_fn(self, reward_fn):
"""
allow users to set their own reward function
"""
self.calc_reward = types.MethodType(reward_fn, self)
logger.debug("custom reward fn set.")
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 - (math.fabs(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.time_received = time.time()
if "image_b" in data:
imgString_b = data["image_b"]
image_b = Image.open(BytesIO(base64.b64decode(imgString_b)))
self.image_array_b = np.asarray(image_b)
if "pos_x" in data:
self.x = data["pos_x"]
self.y = data["pos_y"]
self.z = data["pos_z"]
if "speed" in data:
self.speed = data["speed"]
if "throttle" in data:
self.throttle = data["throttle"]
if "gyro_x" in data:
self.gyro_x = data["gyro_x"]
self.gyro_y = data["gyro_y"]
self.gyro_z = data["gyro_z"]
if "accel_x" in data:
self.accel_x = data["accel_x"]
self.accel_y = data["accel_y"]
self.accel_z = data["accel_z"]
if "vel_x" in data:
self.vel_x = data["vel_x"]
self.vel_y = data["vel_y"]
self.vel_z = data["vel_z"]
if "roll" in data:
self.roll = data["roll"]
self.pitch = data["pitch"]
self.yaw = data["yaw"]
# 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"]
if "lidar" in data:
self.lidar = self.process_lidar_packet(data["lidar"])
# don't update hit once session over
if self.over:
return
if "hit" in data:
self.hit = data["hit"]
if self.hit != "none":
logger.info(f"hit with: {self.hit}")
self.determine_episode_over()
def on_cross_start(self, data):
logger.info(f"crossed start line: lap_time {data['lap_time']}")
def on_race_start(self, data):
logger.debug("race started")
def on_race_stop(self, data):
logger.debug("race stoped")
def on_collision_with_starting_line(self, data):
self.time_stamps.append(data['timeStamp'])
if len(self.time_stamps) > 1:
self.lap_times.append(self.time_stamps[-1] - self.time_stamps[-2])
self.starting_line_num += 1
logger.info(f"Collision with starting line {self.starting_line_num}")
def on_missed_checkpoint(self, data):
logger.info(
f"racer missed checkpoint: expected is {data['expectedIndex']} but {data['observedIndex']} is observed"
)
self.missed_checkpoint = True
def on_DQ(self, data):
logger.info("racer DQ")
self.dq = True
def on_ping(self, message):
"""
no reply needed at this point. Server sends these as a keep alive to make sure clients haven't gone away.
"""
pass
def set_episode_over_fn(self, ep_over_fn):
"""
allow userd to define their own episode over function
"""
self.determine_episode_over = types.MethodType(ep_over_fn, self)
logger.debug("custom ep_over fn set.")
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:
logger.debug(f"game over: cte {self.cte}")
self.over = True
elif self.hit != "none":
logger.debug(f"game over: hit {self.hit}")
self.over = True
elif self.missed_checkpoint:
logger.debug("missed checkpoint")
self.over = True
elif self.dq:
logger.debug("disqualified")
self.over = True
def on_scene_selection_ready(self, data):
logger.debug("SceneSelectionReady ")
self.send_get_scene_names()
def on_car_loaded(self, data):
logger.debug("car loaded")
self.loaded = True
self.on_need_car_config("")
def on_recv_scene_names(self, data):
if data:
names = data["scene_names"]
logger.debug(f"SceneNames: {names}")
print("loading scene", self.SceneToLoad)
if self.SceneToLoad in names:
self.send_load_scene(self.SceneToLoad)
else:
raise ValueError(
f"Scene name {self.SceneToLoad} not in scene list {names}")
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 send_exit_scene(self):
msg = {"msg_type": "exit_scene"}
self.queue_message(msg)
def send_car_config(
self,
body_style="donkey",
body_rgb=[255, 255, 255],
car_name="car",
font_size=100,
):
"""
# body_style = "donkey" | "bare" | "car01" choice of string
# body_rgb = (128, 128, 128) tuple of ints
# car_name = "string less than 64 char"
"""
assert isinstance(body_style, str)
assert isinstance(body_rgb, list) or isinstance(body_rgb, tuple)
assert len(body_rgb) == 3
assert isinstance(car_name, str)
assert isinstance(font_size, int) or isinstance(font_size, str)
msg = {
"msg_type": "car_config",
"body_style": body_style,
"body_r": str(body_rgb[0]),
"body_g": str(body_rgb[1]),
"body_b": str(body_rgb[2]),
"car_name": car_name,
"font_size": str(font_size),
}
self.blocking_send(msg)
time.sleep(0.1)
def send_random_start_config(self, flag):
msg = {"msg_type": "random_start_config", "flag": flag}
self.blocking_send(msg)
time.sleep(0.1)
def send_racer_bio(self, racer_name, car_name, bio, country, guid):
# body_style = "donkey" | "bare" | "car01" choice of string
# body_rgb = (128, 128, 128) tuple of ints
# car_name = "string less than 64 char"
# guid = "some random string"
msg = {
"msg_type": "racer_info",
"racer_name": racer_name,
"car_name": car_name,
"bio": bio,
"country": country,
"guid": guid,
}
self.blocking_send(msg)
time.sleep(0.1)
def send_cam_config(
self,
msg_type="cam_config",
img_w=0,
img_h=0,
img_d=0,
img_enc=0,
fov=0,
fish_eye_x=0,
fish_eye_y=0,
offset_x=0,
offset_y=0,
offset_z=0,
rot_x=0,
rot_y=0,
rot_z=0,
):
"""Camera config
set any field to Zero to get the default camera setting.
offset_x moves camera left/right
offset_y moves camera up/down
offset_z moves camera forward/back
rot_x will rotate the camera
with fish_eye_x/y == 0.0 then you get no distortion
img_enc can be one of JPG|PNG|TGA
"""
msg = {
"msg_type": msg_type,
"fov": str(fov),
"fish_eye_x": str(fish_eye_x),
"fish_eye_y": str(fish_eye_y),
"img_w": str(img_w),
"img_h": str(img_h),
"img_d": str(img_d),
"img_enc": str(img_enc),
"offset_x": str(offset_x),
"offset_y": str(offset_y),
"offset_z": str(offset_z),
"rot_x": str(rot_x),
"rot_y": str(rot_y),
"rot_z": str(rot_z),
}
self.blocking_send(msg)
time.sleep(0.1)
def send_lidar_config(
self,
degPerSweepInc=2.0,
degAngDown=0.0,
degAngDelta=-1.0,
numSweepsLevels=1,
maxRange=50.0,
noise=0.5,
offset_x=0.0,
offset_y=0.5,
offset_z=0.5,
rot_x=0.0,
):
"""Lidar config
the offset_x moves lidar left/right
the offset_y moves lidar up/down
the offset_z moves lidar forward/back
degPerSweepInc : as the ray sweeps around, how many degrees does it advance per sample (int)
degAngDown : what is the starting angle for the initial sweep compared to the forward vector
degAngDelta : what angle change between sweeps
numSweepsLevels : how many complete 360 sweeps (int)
maxRange : what it max distance we will register a hit
noise : what is the scalar on the perlin noise applied to point position
Here's some sample settings that similate a more sophisticated lidar:
msg = '{ "msg_type" : "lidar_config",
"degPerSweepInc" : "2.0", "degAngDown" : "25", "degAngDelta" : "-1.0",
"numSweepsLevels" : "25", "maxRange" : "50.0", "noise" : "0.2",
"offset_x" : "0.0", "offset_y" : "1.0", "offset_z" : "1.0", "rot_x" : "0.0" }'
And here's some sample settings that similate a simple RpLidar A2 one level horizontal scan.
msg = '{ "msg_type" : "lidar_config", "degPerSweepInc" : "2.0",
"degAngDown" : "0.0", "degAngDelta" : "-1.0", "numSweepsLevels" : "1",
"maxRange" : "50.0", "noise" : "0.4",
"offset_x" : "0.0", "offset_y" : "0.5", "offset_z" : "0.5", "rot_x" : "0.0" }'
"""
msg = {
"msg_type": "lidar_config",
"degPerSweepInc": str(degPerSweepInc),
"degAngDown": str(degAngDown),
"degAngDelta": str(degAngDelta),
"numSweepsLevels": str(numSweepsLevels),
"maxRange": str(maxRange),
"noise": str(noise),
"offset_x": str(offset_x),
"offset_y": str(offset_y),
"offset_z": str(offset_z),
"rot_x": str(rot_x),
}
self.blocking_send(msg)
time.sleep(0.1)
self.lidar_deg_per_sweep_inc = float(degPerSweepInc)
self.lidar_num_sweep_levels = int(numSweepsLevels)
self.lidar_deg_ang_delta = float(degAngDelta)
def process_lidar_packet(self, lidar_info):
point_per_sweep = int(360 / self.lidar_deg_per_sweep_inc)
points_num = round(abs(self.lidar_num_sweep_levels * point_per_sweep))
reconstructed_lidar_info = [-1 for _ in range(points_num)
] # we chose -1 to be the "None" value
if lidar_info is not None:
for point in lidar_info:
rx = point["rx"]
ry = point["ry"]
d = point["d"]
x_index = round(abs(rx / self.lidar_deg_per_sweep_inc))
y_index = round(abs(ry / self.lidar_deg_ang_delta))
reconstructed_lidar_info[point_per_sweep * y_index +
x_index] = d
return np.array(reconstructed_lidar_info)
def blocking_send(self, msg):
if self.client is None:
logger.debug(f"skiping: \n {msg}")
return
logger.debug(f"blocking send \n {msg}")
self.client.send_now(msg)
def queue_message(self, msg):
if self.client is None:
logger.debug(f"skiping: \n {msg}")
return
logger.debug(f"sending \n {msg}")
self.client.queue_message(msg)
class DonkeyUnitySimHandler(IMesgHandler):
def __init__(self, level, time_step=0.05, max_cte=5.0, 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.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 'msg_type' not in message:
logger.error('expected msg_type field')
return
msg_type = message['msg_type']
if msg_type in self.fns:
self.fns[msg_type](message)
else:
logger.warning(f'unknown message type {msg_type}')
## ------- Env interface ---------- ##
def reset(self):
logger.debug("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):
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 - (math.fabs(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:
logger.debug(f"game over: cte {self.cte}")
self.over = True
elif self.hit != "none":
logger.debug(f"game over: hit {self.hit}")
self.over = True
def on_scene_selection_ready(self, data):
logger.debug("SceneSelectionReady ")
self.send_get_scene_names()
def on_car_loaded(self, data):
logger.debug("car loaded")
self.loaded = True
def on_recv_scene_names(self, data):
if data:
names = data['scene_names']
logger.debug(f"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:
logger.debug(f'skiping: \n {msg}')
return
logger.debug(f'sending \n {msg}')
self.sock.queue_message(msg)
class DonkeyUnitySimHandler(IMesgHandler):
def __init__(self, level, max_cte=1.0, cam_resolution=None):
self.iSceneToLoad = level
self.loaded = False
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.now = time.time()
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,
"aborted": self.on_abort
}
def on_connect(self, client):
self.client = client
def on_disconnect(self):
self.client = None
def on_abort(self, message):
self.client.stop()
def on_recv_message(self, message):
if 'msg_type' not in message:
logger.error('expected msg_type field')
return
msg_type = message['msg_type']
if msg_type in self.fns:
self.fns[msg_type](message)
else:
logger.warning("unknown message type {}".format(str(msg_type)))
## ------- Env interface ---------- ##
def reset(self):
logger.debug("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):
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
# print(self.cte)
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
x = self.cte * 6
depress = 1 + (1 / (1 + x**4))
reward = (1.0 - ((math.fabs(self.cte * 3) / self.max_cte))**
2) * self.speed * depress
if reward < -0.9:
reward = -0.9
return reward
## ------ Socket interface ----------- ##
def on_telemetry(self, data):
# print(data["cte"],data["pos_x"],data["pos_z"],data["hit"])
# imgString = data["image"]
# image = Image.open(BytesIO(base64.b64decode(imgString)))
# always update the image_array as the observation loop will hang if not changing.
imgString = data["image_C"]
image = Image.open(BytesIO(base64.b64decode(imgString)))
image_C = np.asarray(image)
# image_C = cv2.cvtColor(image_C,cv2.COLOR_BGR2GRAY)
# imgString = json_packet["image_L"]
# image = Image.open(BytesIO(base64.b64decode(imgString)))
# image_L = np.asarray(image)
# image_L = cv2.cvtColor(image_L,cv2.COLOR_BGR2GRAY)
# imgString = json_packet["image_R"]
# image = Image.open(BytesIO(base64.b64decode(imgString)))
# image_R = np.asarray(image)
# image_R = cv2.cvtColor(image_R,cv2.COLOR_BGR2GRAY)
size = image_C.shape[0]
top = size // 8
bottom = (size * 7) // 8
self.image_array = cv2.resize(image_C[top:bottom, :, :], (160, 120))
#python gym-donkeycar/examples/reinforcement_learning/ppo_train.py
# cv2.imshow('window',self.image_array)
# cv2.waitKey(1)
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:
logger.debug("game over: cte {}".format(str(self.cte)))
self.over = True
elif self.hit != "none":
logger.debug("game over: hit {}".format(str(self.hit)))
self.over = True
def on_scene_selection_ready(self, data):
logger.debug("SceneSelectionReady ")
self.send_get_scene_names()
def on_car_loaded(self, data):
logger.debug("car loaded")
self.loaded = True
def on_recv_scene_names(self, data):
if data:
names = data['scene_names']
logger.debug("SceneNames: {}".format(str(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 send_car_config(self, body_style, body_rgb, car_name, font_size):
# body_style = "donkey" | "bare" | "car01" choice of string
# body_rgb = (128, 128, 128) tuple of ints
# car_name = "string less than 64 char"
msg = {
'msg_type': 'car_config',
'body_style': body_style,
'body_r': body_rgb[0].__str__(),
'body_g': body_rgb[1].__str__(),
'body_b': body_rgb[2].__str__(),
'car_name': car_name,
'font_size': font_size.__str__()
}
self.queue_message(msg)
def queue_message(self, msg):
if self.client is None:
logger.debug("skiping: \n {}".format(str(msg)))
return
logger.debug("sending \n {}".format(str(msg)))
self.client.queue_message(msg)
class DonkeyUnitySimHandler(IMesgHandler):
def __init__(self, level, max_cte=5.0, cam_resolution=None):
self.iSceneToLoad = level
self.loaded = False
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,
"ping": self.on_ping,
"aborted": self.on_abort
}
def on_connect(self, client):
self.client = client
def on_disconnect(self):
self.client = None
def on_abort(self, message):
self.client.stop()
def on_recv_message(self, message):
if 'msg_type' not in message:
logger.error('expected msg_type field')
return
msg_type = message['msg_type']
if msg_type in self.fns:
self.fns[msg_type](message)
else:
logger.warning(f'unknown message type {msg_type}')
## ------- Env interface ---------- ##
def reset(self):
logger.debug("reseting")
self.send_reset_car()
self.timer.reset()
time.sleep(1)
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
def get_sensor_size(self):
return self.camera_img_size
def take_action(self, 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 - (math.fabs(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 on_ping(self, message):
'''
no reply needed at this point. Server sends these as a keep alive to make sure clients haven't gone away.
'''
pass
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:
logger.debug(f"game over: cte {self.cte}")
self.over = True
elif self.hit != "none":
logger.debug(f"game over: hit {self.hit}")
self.over = True
def on_scene_selection_ready(self, data):
logger.debug("SceneSelectionReady ")
self.send_get_scene_names()
def on_car_loaded(self, data):
logger.debug("car loaded")
self.loaded = True
def on_recv_scene_names(self, data):
if data:
names = data['scene_names']
logger.debug(f"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 send_car_config(self, body_style, body_rgb, car_name, font_size):
# body_style = "donkey" | "bare" | "car01" choice of string
# body_rgb = (128, 128, 128) tuple of ints
# car_name = "string less than 64 char"
msg = {
'msg_type': 'car_config',
'body_style': body_style,
'body_r': body_rgb[0].__str__(),
'body_g': body_rgb[1].__str__(),
'body_b': body_rgb[2].__str__(),
'car_name': car_name,
'font_size': font_size.__str__()
}
self.queue_message(msg)
time.sleep(0.1)
def send_cam_config(self,
img_w=0,
img_h=0,
img_d=0,
img_enc=0,
fov=0,
fish_eye_x=0,
fish_eye_y=0,
offset_x=0,
offset_y=0,
offset_z=0,
rot_x=0):
""" Camera config
set any field to Zero to get the default camera setting.
offset_x moves camera left/right
offset_y moves camera up/down
offset_z moves camera forward/back
rot_x will rotate the camera
with fish_eye_x/y == 0.0 then you get no distortion
img_enc can be one of JPG|PNG|TGA
"""
msg = {
"msg_type": "cam_config",
"fov": str(fov),
"fish_eye_x": str(fish_eye_x),
"fish_eye_y": str(fish_eye_y),
"img_w": str(img_w),
"img_h": str(img_h),
"img_d": str(img_d),
"img_enc": str(img_enc),
"offset_x": str(offset_x),
"offset_y": str(offset_y),
"offset_z": str(offset_z),
"rot_x": str(rot_x)
}
self.queue_message(msg)
time.sleep(0.1)
def queue_message(self, msg):
if self.client is None:
logger.debug(f'skiping: \n {msg}')
return
logger.debug(f'sending \n {msg}')
self.client.queue_message(msg)
class DonkeySimMsgHandler(IMesgHandler):
STEERING = 0
THROTTLE = 1
def __init__(self, model, constant_throttle, image_cb=None, rand_seed=0):
self.model = model
self.constant_throttle = constant_throttle
self.client = None
self.timer = FPSTimer()
self.img_arr = None
self.image_cb = image_cb
self.steering_angle = 0.
self.throttle = 0.
self.rand_seed = rand_seed
self.fns = {'telemetry' : self.on_telemetry,\
'car_loaded' : self.on_car_created,\
'on_disconnect' : self.on_disconnect,
'aborted' : self.on_aborted}
def on_connect(self, client):
self.client = client
self.timer.reset()
def on_aborted(self, msg):
self.stop()
def on_disconnect(self):
pass
def on_recv_message(self, message):
self.timer.on_frame()
if not 'msg_type' in message:
print('expected msg_type field')
print("message:", message)
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_car_created(self, data):
if self.rand_seed != 0:
self.send_regen_road(0, self.rand_seed, 1.0)
def on_telemetry(self, data):
imgString = data["image"]
image = Image.open(BytesIO(base64.b64decode(imgString)))
img_arr = np.asarray(image, dtype=np.float32)
self.img_arr = img_arr.reshape((1, ) + img_arr.shape)
if self.image_cb is not None:
self.image_cb(img_arr, self.steering_angle)
def update(self):
if self.img_arr is not None:
self.predict(self.img_arr)
pub_img = self.img_arr.reshape((-1))
pub.publish(pub_img)
self.img_arr = None
# hello_str = "hello world %s" % rospy.get_time()
# rospy.loginfo(hello_str)
# rate.sleep()
def manual(self):
self.parse_outputs(manual_output)
def predict(self, image_array):
outputs = self.model.predict(image_array)
self.parse_outputs(outputs)
def parse_outputs(self, outputs):
res = []
# Expects the model with final Dense(2) with steering and throttle
for i in range(outputs.shape[1]):
res.append(outputs[0][i])
self.on_parsed_outputs(res)
def on_parsed_outputs(self, outputs):
self.outputs = outputs
self.steering_angle = 0.0
self.throttle = 0.2
if len(outputs) > 0:
self.steering_angle = outputs[self.STEERING]
if self.constant_throttle != 0.0:
self.throttle = self.constant_throttle
elif len(outputs) > 1:
self.throttle = outputs[self.THROTTLE] * conf.throttle_out_scale
self.send_control(self.steering_angle, self.throttle)
def send_control(self, steer, throttle):
# print("send st:", steer, "th:", throttle)
msg = {
'msg_type': 'control',
'steering': steer.__str__(),
'throttle': throttle.__str__(),
'brake': '0.0'
}
self.client.queue_message(msg)
def send_regen_road(self, road_style=0, rand_seed=0, turn_increment=0.0):
'''
Regenerate the road, where available. For now only in level 0.
In level 0 there are currently 5 road styles. This changes the texture on the road
and also the road width.
The rand_seed can be used to get some determinism in road generation.
The turn_increment defaults to 1.0 internally. Provide a non zero positive float
to affect the curviness of the road. Smaller numbers will provide more shallow curves.
'''
msg = {
'msg_type': 'regen_road',
'road_style': road_style.__str__(),
'rand_seed': rand_seed.__str__(),
'turn_increment': turn_increment.__str__()
}
self.client.queue_message(msg)
def stop(self):
self.client.stop()
def __del__(self):
self.stop()
class DonkeySimMsgHandler(IMesgHandler):
STEERING = 0
THROTTLE = 1
def __init__(self, model, constant_throttle, image_cb=None, rand_seed=0):
self.model = model
self.constant_throttle = constant_throttle
self.client = None
self.timer = FPSTimer()
self.img_arr = None
self.image_cb = image_cb
self.steering_angle = 0.
self.throttle = 0.
self.rand_seed = rand_seed
self.fns = {'telemetry' : self.on_telemetry,\
'car_loaded' : self.on_car_created,\
'on_disconnect' : self.on_disconnect,
'aborted' : self.on_aborted}
# images to record
self.img_orig = None
self.img_add_rain = None
self.img_processed = None
self.frame_count = 0
# model name
def on_connect(self, client):
self.client = client
self.timer.reset()
def on_aborted(self, msg):
self.stop()
def on_disconnect(self):
pass
def on_recv_message(self, message):
self.timer.on_frame()
if not 'msg_type' in message:
print('expected msg_type field')
print("message:", message)
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_car_created(self, data):
if self.rand_seed != 0:
self.send_regen_road(0, self.rand_seed, 1.0)
def on_telemetry(self, data):
imgString = data["image"]
image = Image.open(BytesIO(base64.b64decode(imgString)))
img_arr = np.asarray(image, dtype=np.float32)
self.frame_count += 1
self.img_orig = img_arr
# set to same image size expected from acquisition process
img_arr = ag.resize_expected(img_arr)
# check for rain
if (conf.rt != ''):
img_arr = add_rain(img_arr, conf.rt, conf.st)
self.img_add_rain = img_arr
# same preprocessing as for training
img_arr = ag.preprocess(img_arr)
self.img_processed = img_arr
#if(conf.record == True):
# text = (['Network Image', 'No Rain'])
# rv.add_image(img_arr, text)
# if we are testing the network with rain
self.img_arr = img_arr.reshape((1, ) + img_arr.shape)
if self.image_cb is not None:
self.image_cb(img_arr, self.steering_angle)
def update(self):
if self.img_arr is not None:
self.predict(self.img_arr)
self.img_arr = None
def predict(self, image_array):
outputs = self.model.predict(image_array)
# check if we are recording
if (conf.record == True):
# Add first image, with name of network and frame number
# TODO, get network name from argument
text = ([
rv.modelname, 'Intensity Multiplie: 1', 'Acquired image',
'Frame: ' + str(self.frame_count)
])
rv.add_image(self.img_orig, text)
# Add second image, preprocessed with rain or without
# text = (['Network image', 'No rain'])
# rv.add_image(self.img_processed, text)
# if rain added
# check for rain
if (conf.rt != ''):
rtype = 'Type: ' + conf.rt
s = 'Slant: -+' + str(conf.st)
text = (['Added rain', rtype, s])
rv.add_image(self.img_add_rain, text)
# add third image with prediction
steering = outputs[0][0]
steering *= conf.norm_const
st_str = "{:.2f}".format(steering)
st_str = "Predicted steering angle: " + st_str
# st_str = "Predicted steering angle: 20"
#rtype = 'Type: ' + conf.rt
#s = 'Slant: -+' + str(conf.st)
text = (["Network image", st_str])
rv.add_image(image_array[0], text)
rv.add_frame()
self.parse_outputs(outputs)
def parse_outputs(self, outputs):
res = []
# Expects the model with final Dense(2) with steering and throttle
for i in range(outputs.shape[1]):
res.append(outputs[0][i])
self.on_parsed_outputs(res)
def on_parsed_outputs(self, outputs):
self.outputs = outputs
self.steering_angle = 0.0
self.throttle = 0.2
if len(outputs) > 0:
self.steering_angle = outputs[self.STEERING]
if self.constant_throttle != 0.0:
self.throttle = self.constant_throttle
elif len(outputs) > 1:
self.throttle = outputs[self.THROTTLE] * conf.throttle_out_scale
self.send_control(self.steering_angle, self.throttle)
def send_control(self, steer, throttle):
# print("send st:", steer, "th:", throttle)
msg = {
'msg_type': 'control',
'steering': steer.__str__(),
'throttle': throttle.__str__(),
'brake': '0.0'
}
self.client.queue_message(msg)
def send_regen_road(self, road_style=0, rand_seed=0, turn_increment=0.0):
'''
Regenerate the road, where available. For now only in level 0.
In level 0 there are currently 5 road styles. This changes the texture on the road
and also the road width.
The rand_seed can be used to get some determinism in road generation.
The turn_increment defaults to 1.0 internally. Provide a non zero positive float
to affect the curviness of the road. Smaller numbers will provide more shallow curves.
'''
msg = {
'msg_type': 'regen_road',
'road_style': road_style.__str__(),
'rand_seed': rand_seed.__str__(),
'turn_increment': turn_increment.__str__()
}
self.client.queue_message(msg)
def stop(self):
self.client.stop()
def __del__(self):
self.stop()