From 641f2ef1e021a8d40cb382bce56c44ee05f03466 Mon Sep 17 00:00:00 2001 From: CoderHaoranLee <917735857@qq.com> Date: Mon, 20 Jun 2022 16:25:38 +0800 Subject: [PATCH] Update CogEnvDecoder.py --- Cogenvdecoder/Cogenvdecoder/CogEnvDecoder.py | 154 +++++++++++++++---- 1 file changed, 120 insertions(+), 34 deletions(-) diff --git a/Cogenvdecoder/Cogenvdecoder/CogEnvDecoder.py b/Cogenvdecoder/Cogenvdecoder/CogEnvDecoder.py index 92701e4..fd1caf7 100644 --- a/Cogenvdecoder/Cogenvdecoder/CogEnvDecoder.py +++ b/Cogenvdecoder/Cogenvdecoder/CogEnvDecoder.py @@ -9,36 +9,45 @@ import cv2 as cv import io from random import randint +import time +from threading import Thread + class CogEnvDecoder: - def __init__(self, worker_id=1, train=True, allow_multiple_obs=True, env_name="linux_V1/1.x86_64", mat_num=0, windows_width=600, windows_height=300, no_graphics=False, time_scale=1, seed=0): + def __init__(self, worker_id=1, train=True, allow_multiple_obs=True, env_name="linux_V1/1.x86_64", mat_num=0, + windows_width=600, windows_height=300, no_graphics=False, time_scale=1, seed=0, force_sync=True): if train: engine_configuration_channel = EngineConfigurationChannel() - engine_configuration_channel.set_configuration_parameters(width=windows_width, height=windows_height, time_scale=time_scale) + engine_configuration_channel.set_configuration_parameters(width=windows_width, height=windows_height, + time_scale=time_scale) self._engine_Environment_channel = EnvironmentParametersChannel() - unity_env = UnityEnvironment(env_name, worker_id=worker_id, no_graphics=no_graphics, side_channels=[self._engine_Environment_channel, engine_configuration_channel], seed=seed) + unity_env = UnityEnvironment(env_name, worker_id=worker_id, no_graphics=no_graphics, + side_channels=[self._engine_Environment_channel, engine_configuration_channel], + seed=seed) self._env = UnityToGymWrapper(unity_env, allow_multiple_obs=True, uint8_visual=True) else: unity_env = UnityEnvironment(env_name, worker_id=worker_id) self._env = UnityToGymWrapper(unity_env, allow_multiple_obs=True, uint8_visual=True) - self._size = (300,300) + self._size = (300, 300) self.reward_range = self._env.reward_range self.metadata = self._env.metadata self.allow_multiple_obs = allow_multiple_obs self.spec = self._env.spec - - self._load_step = False #allow action transfer to run + self._load_step = False # allow action transfer to run self._obs = None self._reward = None - self._done = None + self._done = False self._info = None self._action = [0, 0, 0, 0] - self._load = 0 #Check whether the action has been loaded - + self._load = 0 # Check whether the action has been loaded + self.last_step_time = time.time() + self._thread = None + self._end_thread = False + self._force_sync = force_sync @property def observation_space(self): @@ -46,7 +55,7 @@ def observation_space(self): color_image = gym.spaces.Box(low=0, high=255, shape=shape_image, dtype=np.uint8) num_laser = 61 laser_space = gym.spaces.Box(low=0, high=100, shape=(num_laser,), dtype=np.float32) - num_vector = 28 + num_vector = 34 space_vector = gym.spaces.Box(low=-np.Inf, high=np.Inf, shape=(num_vector,), dtype=np.float32) return gym.spaces.Dict({'color_image': color_image, 'laser': laser_space, 'vector': space_vector}) @@ -56,19 +65,39 @@ def action_space(self): return self._env.action_space def close(self): + self._end_thread = True + if not self._force_sync: + if self._thread is not None: + self._thread.join() return self._env.close() - - def reset(self): + def reset(self, fri_cor=0.1, KP=8, KI=0, KD=2, VK1=0.375, M=3.4, Wheel_I=0.0125, obs_mat=False): + + if not self._force_sync: + self._end_thread = True + if self._thread is not None: + self._thread.join() + self._end_thread = False + + self._engine_Environment_channel.set_float_parameter("Friction_coeff", fri_cor) + self._engine_Environment_channel.set_float_parameter("KP", KP) + self._engine_Environment_channel.set_float_parameter("KI", KI) + self._engine_Environment_channel.set_float_parameter("KD", KD) + self._engine_Environment_channel.set_float_parameter("VK1", VK1) + self._engine_Environment_channel.set_float_parameter("M", M) + self._engine_Environment_channel.set_float_parameter("Wheel_I", Wheel_I) + if obs_mat == False: + self._engine_Environment_channel.set_float_parameter("Obs_mat", float(0)) + else: + self._engine_Environment_channel.set_float_parameter("Obs_mat", float(1)) obs = self._env.reset() img = self._GetImg(obs) - + self_pos = self._GetSelfPos(obs) # Blue one Position self_info = self._SelfInfo(obs) # Blue one remaining HP & bullet - goal1 = self._GetGoal1Pos(obs) # Goal 1 position, whether it has been activated goal2 = self._GetGoal2Pos(obs) # Goal 2 position, whether it has been activated goal3 = self._GetGoal3Pos(obs) # Goal 3 position, whether it has been activated @@ -81,21 +110,40 @@ def reset(self): enemy_pos = self._EnemyPos(obs) # Red one position enemy_info = self._EnemyInfo(obs) # Red one remaining HP & bullets + real_speed = self._GetRealSpeed(obs) + # odom = self._GetOdom(obs) + laser = self._GetLaser(obs) - # [3, 2, 1, 3, 2, 3, 3, 3, 3, 3, 2] - vector_state = [self_pos, self_info, enemy_act, enemy_pos, enemy_info, - goal1, goal2, goal3, goal4, goal5, collision_info] - state = {"color_image":img, "laser": laser, "vector": vector_state} - return state + # [3, 2, 1, 3, 2, 3, 3, 3, 3, 3, 2, 3, 3] + vector_state = [self_pos, self_info, enemy_act, enemy_pos, enemy_info, + goal1, goal2, goal3, goal4, goal5, collision_info, real_speed] + state = {"color_image": img, "laser": laser, "vector": vector_state} + if not self._force_sync: + self._done = False + self._load_step = False + self._step_thread_begin() - def step(self, action): + return state - obs, reward, done, info = self._env.step(action) - + def step(self, action): + if not self._force_sync: + self._action = action + self.last_step_time = time.time() + + while not self._load_step: + time.sleep(0.001) + # obs, reward, done, info = self._env.step(action) + obs = self._obs + reward = self._reward + done = self._done + info = self._info + + else: + obs, reward, done, info = self._env.step(action) + self._load_step = False img = self._GetImg(obs) - self_pos = self._GetSelfPos(obs) # Blue one Position self_info = self._SelfInfo(obs) # Blue one remaining HP & bullet @@ -110,41 +158,50 @@ def step(self, action): enemy_act = self._EnemyStatus(obs) # Whether enemy has been activated enemy_pos = self._EnemyPos(obs) # Red one position enemy_info = self._EnemyInfo(obs) # Red one remaining HP & bullets - + + real_speed = self._GetRealSpeed(obs) # real speed + odom = self._GetOdom(obs) # Odom data + laser = self._GetLaser(obs) score = self._Score(obs) # Current score dmg = self._DmgCaused(obs) # blue one caused damage to red one time_taken = self._TimeTaken(obs) # Time passed in the round - judge_result = [score, time_taken, dmg, flag_ach] - vector_state = [self_pos, self_info, enemy_act, enemy_pos, enemy_info, - goal1, goal2, goal3, goal4, goal5, collision_info] + vector_state = [self_pos, self_info, enemy_act, enemy_pos, enemy_info, + goal1, goal2, goal3, goal4, goal5, collision_info, real_speed, odom] - state = {"color_image":img, "laser": laser, "vector": vector_state} + state = {"color_image": img, "laser": laser, "vector": vector_state} return state, reward, done, [info, judge_result] - + def check_state(self, state, info=None): image_data = state["color_image"] laser_data = np.array(state["laser"]) vector_data = state["vector"] print("=======================state check====================") print("image shape: {}".format(image_data.shape)) - print("laser shape: {}, max distance: {}, min distance: {}".format(laser_data.shape, np.max(laser_data), np.min(laser_data))) - print("self pose: {}, self info: {}, enemy active: {}, enemy pose: {}, enemy_info: {}".format(vector_data[0], vector_data[1], vector_data[2], vector_data[3], vector_data[4])) - print("goal 1: {}, goal 2: {}, goal 3: {}, goal 4: {}, goal 5:{}".format(vector_data[5], vector_data[6], vector_data[7], vector_data[8], vector_data[9])) + print("laser shape: {}, max distance: {}, min distance: {}".format(laser_data.shape, np.max(laser_data), + np.min(laser_data))) + print("self pose: {}, self info: {}, enemy active: {}, enemy pose: {}, enemy_info: {}".format(vector_data[0], + vector_data[1], + vector_data[2], + vector_data[3], + vector_data[4])) + print("goal 1: {}, goal 2: {}, goal 3: {}, goal 4: {}, goal 5:{}".format(vector_data[5], vector_data[6], + vector_data[7], vector_data[8], + vector_data[9])) print("total collisions: {}, total collision time: {} ".format(vector_data[10][0], vector_data[10][1])) if info is not None: print("enemy active: {}".format(info[1][3])) print("time taken: {}, attack damage: {}, score: {}".format(info[1][1], info[1][2], info[1][0])) print("-----------------------end check---------------------") + def render(self, mode): return self._env.render(mode) - def _GetImg(self, data): return cv.cvtColor(data[0], cv.COLOR_RGB2BGR) @@ -233,7 +290,36 @@ def _CollCondtion(self, data): return [coll_time, cont_coll_time] def _GetLaser(self, data): - laser_data = data[1][26:] + laser_data = data[1][32:] return laser_data + def _GetRealSpeed(self, data): + speed_x = data[1][26] + speed_y = data[1][27] + speed_yaw = data[1][28] + return [speed_x, speed_y, speed_yaw] + + def _GetOdom(self, data): + odom_x = data[1][29] + odom_y = data[1][30] + odom_yaw = data[1][31] + return [odom_x, odom_y, odom_yaw] + def _thread_step(self): + while True: + if self._end_thread: + break + delta_time = time.time() - self.last_step_time + if(delta_time > 0.5): + self._action = [0.0, 0.0, 0.0, 0.0] + self._obs, self._reward, self._done, self._info = self._env.step(self._action) + self._load_step = True + if self._done: + break + + + def _step_thread_begin(self): + self._thread = Thread(target=self._thread_step) + self._thread.setDaemon(True) + self._thread.start() +