コード例 #1
0
ファイル: donkey_sim.py プロジェクト: r7vme/donkey_gym
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)
コード例 #2
0
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)
コード例 #3
0
ファイル: donkey_sim.py プロジェクト: simeneide/donkey_gym
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)
コード例 #4
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, 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()