def __init__(self, aimRotation,actiondim=1):
print("init InferenceModel")
dtype = torch.float64
torch.set_default_dtype(dtype)
# device = torch.device('cuda', index=0) # if torch.cuda.is_available() else
device = torch.device('cpu')
# if torch.cuda.is_available():
# torch.cuda.set_device(0)
parser = argparse.ArgumentParser(description='PyTorch PPO example')
parser.add_argument('--env-name', default="continueRealEnvPpo", metavar='G',
help='name of the environment to run')
parser.add_argument('--version', default="4.3.1.8.1", metavar='G',
help='version')
args = parser.parse_args()
path = os.path.join(assets_dir(), args.version)
print(path)
randomSeed = 2
render = False
state_dim = 64 + 12 +15 # env.observation_space.shape[0]#[0]
running_state = ZFilter((state_dim,), clip=5)
"""define actor and critic"""
policy_net =Policy(76+15, 3)#DiscretePolicy(75,5) #Policy(75, 4)
value_net = Value(76+15)
policy_net.load_state_dict(torch.load(os.path.join(path, 'policy_net_{}_ppo.pth'.format(args.env_name)), 'cpu'))
value_net.load_state_dict(torch.load(os.path.join(path, 'value_net_{}_ppo.pth'.format(args.env_name)), 'cpu'))
# policy_net = torch.load(os.path.join(path, 'policy_net_{}_ppo.pth'.format(args.env_name)), 'cpu')
# value_net = torch.load(os.path.join(path, 'value_net_{}_ppo.pth'.format(args.env_name)), 'cpu')
running_state, saveavgreward = pickle.load(
open(os.path.join(path, 'running_state_{}_ppo.p'.format(args.env_name)), "rb"))
print("get reward {}".format(saveavgreward))
policy_net.to(device)
value_net.to(device)
self.persistence = Persistence("real_0515_" + args.version)
"""create agent"""
self.agent = DroneAgent(policy_net, value_net, device, running_state=running_state, render=render,
num_threads=1)
self.lastRotation = aimRotation
self.lastLeftRightFeel = [8, 8]
self.lastaction = [0, -0.5]
self.lasttime = datetime.datetime.now()
self.forceWallMenory =0
self.aimRotaion = aimRotation
self.finalAimRotaion =aimRotation
self.stoptime = 0
self.stoplong = 5
self.lastalphadirect = 0
self.lastalphacos = 0.5
print("init succ")
def __init__(self, methodName):
self.persistence = Persistence("SecureProblem" + methodName)
self.state = {
'statePitch': 0,
'stateRoll': 0,
'stateYaw': 0,
'velocityX': 0,
'velocityY': 0,
'curTimes': 0
}
self.move = {
"mPitch": 0,
"mRoll": 0,
"mYaw": 0,
"mThrottle": 0,
"gPitch": 0,
"gRoll": 0,
"gYaw": 0,
"handleImageName": "",
"chTimes": 1
}
self.sendStopAction = False
model = args.DE_model#mono+stereo_640x192"
loadConfig()
rotation =aimRotation
# 路径
model_name = model
model_path = os.path.join(f"../static/DEmodels/", model_name)
indirectpath = f"../static/images/"
#(mod)indirectpath = os.path.join(indirectpath, nameImagesDir)
#if not os.path.exists(indirectpath) :
# os.mkdir(indirectpath)
# 简易分离日志
persistence = Persistence("serverInfo" + nameImagesDir)
# 安全解决
checkSecure = CheckSecure(nameImagesDir)
global time_count
time_count = 0
global detect_path
detect_path = 0
# ------具体服务------
# 连接测试
@app.route('/')
def hello_world():
print("初始化连接成功")
global move
global aimRotation
move = {"mPitch": 0, "mRoll": 0, "mYaw": aimRotation, "mThrottle": 0, "gPitch": 0, "gRoll": 0,
class CheckSecure:
def __init__(self, methodName):
self.persistence = Persistence("SecureProblem" + methodName)
self.state = {
'statePitch': 0,
'stateRoll': 0,
'stateYaw': 0,
'velocityX': 0,
'velocityY': 0,
'curTimes': 0
}
self.move = {
"mPitch": 0,
"mRoll": 0,
"mYaw": 0,
"mThrottle": 0,
"gPitch": 0,
"gRoll": 0,
"gYaw": 0,
"handleImageName": "",
"chTimes": 1
}
self.sendStopAction = False
def check(self, input1, input2):
self.checkSingle(input1)
return self.sendStopAction, self.state
def checkState(self, droneState):
if self.sendStopAction:
print("曾经存在异常")
if abs(droneState['statePitch']) > 20 or abs(
droneState['stateRoll']
) > 20 or droneState['velocityY'] > 5 or droneState['velocityX'] > 5:
print(f"飞行状态告警 {droneState}")
self.dangerAction(f"状态异常 {droneState}")
return self.sendStopAction, self.move
def checkControl(self, droneControlState):
dangerMove = 3.5
if self.sendStopAction:
print("曾经存在异常")
if droneControlState['mPitch'] > dangerMove or droneControlState[
'mRoll'] > dangerMove or abs(droneControlState['mYaw']) > 360:
print(f"控制信号飞行告警 {str(droneControlState)}")
self.dangerAction(f"控制信号异常 {str(droneControlState)}")
return self.sendStopAction, self.move
def checkSingle(self, input):
danger = False
dangerInfo = ""
# 检查代码
time = datetime.datetime.now()
# 记录问题
if danger:
self.dangerAction(dangerInfo)
def reset(self):
self.sendStopAction = False
def dangerAction(self, dangerInfo):
time = datetime.datetime.now()
self.persistence.saveTerminalRecord("_methodStartInfo",
f"dangertime {str(time)}")
print(f"{time} 危险告警！ {dangerInfo}")
self.sendStopAction = True
class InferenceModel():
def __init__(self, actiondim=1):
print("init InferenceModel")
dtype = torch.float64
torch.set_default_dtype(dtype)
# device = torch.device('cuda', index=0) # if torch.cuda.is_available() else
device = torch.device('cpu')
# if torch.cuda.is_available():
# torch.cuda.set_device(0)
parser = argparse.ArgumentParser(description='PyTorch PPO example')
parser.add_argument('--env-name', default="continueRealEnvPpo", metavar='G',
help='name of the environment to run')
parser.add_argument('--version', default="4.2.4.1", metavar='G',
help='version')
args = parser.parse_args()
path = os.path.join(assets_dir(), args.version)
print(path)
randomSeed = 2
render = False
state_dim = 64 + 11 # env.observation_space.shape[0]#[0]
running_state = ZFilter((state_dim,), clip=5)
"""define actor and critic"""
policy_net =Policy(75, 2)#DiscretePolicy(75,5) #Policy(75, 4)
value_net = Value(75)
policy_net.load_state_dict(torch.load(os.path.join(path, 'policy_net_{}_ppo.pth'.format(args.env_name)), 'cpu'))
value_net.load_state_dict(torch.load(os.path.join(path, 'value_net_{}_ppo.pth'.format(args.env_name)), 'cpu'))
# policy_net = torch.load(os.path.join(path, 'policy_net_{}_ppo.pth'.format(args.env_name)), 'cpu')
# value_net = torch.load(os.path.join(path, 'value_net_{}_ppo.pth'.format(args.env_name)), 'cpu')
running_state, saveavgreward = pickle.load(
open(os.path.join(path, 'running_state_{}_ppo.p'.format(args.env_name)), "rb"))
print("get reward {}".format(saveavgreward))
policy_net.to(device)
value_net.to(device)
self.persistence = Persistence("real_0515_" + args.version)
"""create agent"""
self.agent = DroneAgent(policy_net, value_net, device, running_state=running_state, render=render,
num_threads=1)
self.lastRotation = 0
self.lastLeftRightFeel = [8, 8]
self.lastaction = [0, -0.5]
self.lasttime = datetime.datetime.now()
print("init succ")
def inference(self, imgstate, rotation, aimRotation, time):
deepfeel = self.caculateObs(imgstate)
state = self.getState(deepfeel, rotation, aimRotation)
action, value = self.agent.predictTakeValue(state)
info = f"time {time} action {action} critic {value} state {state[64:]} deepfeel avg {np.mean(deepfeel)} value {deepfeel} "
info2 = f"action {action} critic {value} state {state[64:]}"
print(info2)
self.persistence.saveTerminalRecord("stateaction", info)
self.lastaction = action
return action
def inference4(self, imgstate, rotation, aimRotation, time):#仅往前走或者仅拐弯
deepfeel = self.caculateObs(imgstate)
state = self.getState(deepfeel, rotation, aimRotation)
action, value = self.agent.predictTakeValue(state)
info = f"time {time} action {action} critic {value} state {state[64:]} deepfeel avg {np.mean(deepfeel)} value {deepfeel} "
info2 = f"action {action} critic {value} state {state[64:]}"
print(info2)
self.persistence.saveTerminalRecord("stateaction", info)
self.lastaction = action
if action[1] > 0:
action[0] = 0
return action
def inference2(self, imgstate, rotation, aimRotation, time):#分开左右
deepfeel = self.caculateObs(imgstate)
state = self.getState(deepfeel, rotation, aimRotation)
action, value = self.agent.predictTakeValue(state)
info = f"time {time} action {action} critic {value} state {state[64:]} deepfeel avg {np.mean(deepfeel)} value {deepfeel} "
info2 = f"action {action} critic {value} state {state[64:]}"
print(info2)
self.persistence.saveTerminalRecord("stateaction", info)
self.lastaction = action
if action[1] <= 0:
if action[2] > action[3]:
action[0] = 1
else:
action[0] = -1
else:
action[0] /= 3
return action
def inference3(self, imgstate, rotation, aimRotation, time):# 完全离散化
deepfeel = self.caculateObs(imgstate)
state = self.getState(deepfeel, rotation, aimRotation)
action, value = self.agent.predictTakeValue(state)
info = f"time {time} action {action} critic {value} state {state[64:]} deepfeel avg {np.mean(deepfeel)} value {deepfeel} "
info2 = f"action {action} critic {value} state {state[64:]}"
print(info2)
self.persistence.saveTerminalRecord("stateaction", info)
c = -0.5
if action >= 1 and action < 4:
c = 0.5
actionsingle = action
action = [action, c]
self.lastaction = action
if actionsingle < 1:
action[0] = -1
action[1] = -0.5
elif actionsingle >= 4:
action[0] = 1
action[1] = -0.5
elif actionsingle >= 1 and actionsingle < 2:
action[0] = -0.5
action[1] = 0.5
elif actionsingle >= 3 and actionsingle < 4:
action[0] = 0.5
action[1] = 0.5
else:
action[0]= 0
action[1] =0.5
return action
def getState(self, deepfeel, rotation, aimRotation): # 极端诡异和空格于制表符显示问题
rotation = math.radians(rotation)
aimRotation = math.radians(aimRotation)
xDirect = round(math.cos(rotation), 6)
yDirect = round(math.sin(rotation), 6)
aimDirectX = round(math.cos(aimRotation), 6)
aimDirectY = round(math.sin(aimRotation), 6)
alphadirect = aimDirectX * yDirect - aimDirectY * xDirect
alphacos = aimDirectX * xDirect + aimDirectY * yDirect # 直接用目标与行进方向夹角sin cos作为状态
if alphacos < 0 and alphadirect > 0:
alphadirect = 1
if alphacos < 0 and alphadirect < 0:
alphadirect = -1
timenow = datetime.datetime.now()
internaltime = (timenow - self.lasttime).total_seconds()
other = [rotation, xDirect, yDirect, self.lastaction[0], self.lastaction[1], alphadirect, alphacos,
self.lastRotation, self.lastLeftRightFeel[0], self.lastLeftRightFeel[1], internaltime]
nextstate = []
# print(f"other {other}")
for i in deepfeel:
nextstate.append(i)
for i in other:
nextstate.append(i)
self.lastRotation = alphadirect
# self.lastRotation = rotation
self.lasttime = timenow
self.lastLeftRightFeel = [nextstate[0], nextstate[63]]
return nextstate
def caculateObs(self, state, uprange=24, downrange=38): # 压缩转成线
imageCompact = []
for i in range(uprange, downrange):
imageCompact.append(state[i][:])
imageCompact = np.array(imageCompact)
power = np.min(imageCompact, axis=0)
# print(power)
for i in range(len(power)):
if power[i] > 8:
power[i] = 8
elif power[i] < 0.2 and power[i] > 0.000001:
power[i] = 0.2
elif power[i] == 0:
power[i] = 7
# print(power)
return power
def __init__(self, aimRotation, actiondim=1):
self.aimRotation = aimRotation
print("init InferenceModel")
dtype = torch.float64
torch.set_default_dtype(dtype)
# device = torch.device('cuda', index=0) # if torch.cuda.is_available() else
device = torch.device('cpu')
# if torch.cuda.is_available():
# torch.cuda.set_device(0)
parser = argparse.ArgumentParser()
parser.add_argument(
"--policy",
default="TD3onPointMap") # Policy name (TD3, DDPG or OurDDPG)
parser.add_argument(
"--env", default="continuousEnv") # OpenAI gym environment name
parser.add_argument("--seed", default=0,
type=int) # Sets Gym, PyTorch and Numpy seeds
parser.add_argument(
"--load_model", default="TD3onPointMap_simuForReal_1.0.5"
) # Model load file name, "" doesn't load, "default" uses file_name
parser.add_argument('--version', default="0.1.5", metavar='G')
args = parser.parse_args()
path = os.path.join(assets_dir(), args.version)
print(path)
explain = "simuForReal"
file_name = f"{args.policy}_{explain}_{args.version}"
print("---------------------------------------")
print(f"Policy: {args.policy}, Env: {args.env}, Seed: {args.seed}")
print("---------------------------------------")
self.persistence = Persistence("real_0515_" + args.policy +
args.load_model)
state_image_dim = 64
single_state_dim = 12 #
action_dim = 3
max_action = float(1) # 超参量
kwargs = {
"see_dim": state_image_dim,
"state_dim": single_state_dim,
"action_dim": action_dim,
"max_action": max_action,
"discount": 0.99,
"tau": 0.005,
}
# Target policy smoothing is scaled wrt the action scale
kwargs["policy_noise"] = 0.15 * max_action
kwargs["noise_clip"] = 0.1 * max_action
kwargs["policy_freq"] = 2
self.policy = TD3(**kwargs)
if args.load_model != "":
print("load model")
policy_file = file_name if args.load_model == "default" else args.load_model
self.policy.load(f"../static/td3models/{policy_file}")
self.lastRotation = aimRotation
self.lastLeftRightFeel = [8, 8]
self.lastaction = [0, -0.5]
self.lasttime = datetime.datetime.now()
self.forceWallMenory = 0
self.aimRotaion = aimRotation
self.finalAimRotaion = aimRotation
self.stoptime = 0
self.stoplong = 5
self.lastalphadirect = 0
self.lastalphacos = 0.5
print("init succ")
class InferenceModel():
def __init__(self, aimRotation, actiondim=1):
self.aimRotation = aimRotation
print("init InferenceModel")
dtype = torch.float64
torch.set_default_dtype(dtype)
# device = torch.device('cuda', index=0) # if torch.cuda.is_available() else
device = torch.device('cpu')
# if torch.cuda.is_available():
# torch.cuda.set_device(0)
parser = argparse.ArgumentParser()
parser.add_argument(
"--policy",
default="TD3onPointMap") # Policy name (TD3, DDPG or OurDDPG)
parser.add_argument(
"--env", default="continuousEnv") # OpenAI gym environment name
parser.add_argument("--seed", default=0,
type=int) # Sets Gym, PyTorch and Numpy seeds
parser.add_argument(
"--load_model", default="TD3onPointMap_simuForReal_1.0.5"
) # Model load file name, "" doesn't load, "default" uses file_name
parser.add_argument('--version', default="0.1.5", metavar='G')
args = parser.parse_args()
path = os.path.join(assets_dir(), args.version)
print(path)
explain = "simuForReal"
file_name = f"{args.policy}_{explain}_{args.version}"
print("---------------------------------------")
print(f"Policy: {args.policy}, Env: {args.env}, Seed: {args.seed}")
print("---------------------------------------")
self.persistence = Persistence("real_0515_" + args.policy +
args.load_model)
state_image_dim = 64
single_state_dim = 12 #
action_dim = 3
max_action = float(1) # 超参量
kwargs = {
"see_dim": state_image_dim,
"state_dim": single_state_dim,
"action_dim": action_dim,
"max_action": max_action,
"discount": 0.99,
"tau": 0.005,
}
# Target policy smoothing is scaled wrt the action scale
kwargs["policy_noise"] = 0.15 * max_action
kwargs["noise_clip"] = 0.1 * max_action
kwargs["policy_freq"] = 2
self.policy = TD3(**kwargs)
if args.load_model != "":
print("load model")
policy_file = file_name if args.load_model == "default" else args.load_model
self.policy.load(f"../static/td3models/{policy_file}")
self.lastRotation = aimRotation
self.lastLeftRightFeel = [8, 8]
self.lastaction = [0, -0.5]
self.lasttime = datetime.datetime.now()
self.forceWallMenory = 0
self.aimRotaion = aimRotation
self.finalAimRotaion = aimRotation
self.stoptime = 0
self.stoplong = 5
self.lastalphadirect = 0
self.lastalphacos = 0.5
print("init succ")
def inference(self, imgstate, rotation, aimRotation, time):
max_action = 1
deepfeel = self.caculateObs(imgstate)
state = self.getState(deepfeel, rotation)
action = (self.policy.select_action(state[0:64], state[64:])).clip(
-max_action, max_action)
info = f"time {time} action {action} state {state} deepfeel avg {np.mean(deepfeel)} value {deepfeel} "
info2 = f"action {action} state {state[0:64]}"
if action[1] > 0:
self.stoptime = 0
else:
self.stoptime += 1
print(info2)
self.persistence.saveTerminalRecord("stateaction", info)
self.lastaction = action.copy()
return action
def sliceWindow(self, sliceSize=8, proValue=2, threshold=1.8):
deepfeel = self.drone.getDeep()
go = int(sliceSize / 2)
sliceRes = []
temp = 0
for i in range(0, len(deepfeel) - sliceSize + 1, go):
temp = 0
for j in range(0, sliceSize):
if deepfeel[i + j] < threshold:
if temp == 0:
temp = 10
temp = (threshold - deepfeel[i + j]) * proValue
sliceRes.append(temp)
return sliceRes
def judgeForceWall(self,
deepfeel,
alphacos,
max=6.5,
avgmax=3.2,
twoavgmax=1.8,
smallthreshold=0.55): # 待修改
#看局部最优
tempbest = False
pos = np.array([self.drone.center.x, self.drone.center.y])
propos = self.tempPoCache.get()
#if dist(pos,propos) < 1.25:
# tempbest = True
self.tempPoCache.put(pos)
if alphacos < 0.7:
return False
totalLength = 0
maxLength = 0
smallnum = 0
for i in deepfeel:
if i > maxLength:
maxLength = i
if i < smallthreshold:
smallnum += 1
totalLength += i
avgLength = totalLength / len(deepfeel)
if avgLength < avgmax and maxLength < max or avgLength < twoavgmax or smallnum > 5 or tempbest:
self.forceWallMenory = random.randint(15, 20)
return True
if self.stoptime > self.stoplong:
self.forceWallMenory = random.randint(15, 20)
return True
return False
def getState(self, deepfeel, rotation): # 极端诡异和空格于制表符显示问题
rotation = math.radians(rotation)
aimRotation = math.radians(self.aimRotation)
xDirect = round(math.cos(rotation), 6)
yDirect = round(math.sin(rotation), 6)
aimDirectX = round(math.cos(aimRotation), 6)
aimDirectY = round(math.sin(aimRotation), 6)
alphadirect = aimDirectX * yDirect - aimDirectY * xDirect
alphacos = aimDirectX * xDirect + aimDirectY * yDirect # 直接用目标与行进方向夹角sin cos作为状态
if alphacos < 0 and alphadirect > 0:
alphadirect = 1
if alphacos < 0 and alphadirect < 0:
alphadirect = -1
#judgeState = self.judgeForceWall(deepfeel, alphacos)
#if judgeState == True: # 暂未改
# pass
free = 0
if self.forceWallMenory > 0:
free = 5 + self.forceWallMenory
stop = 0
if self.stoptime >= self.stoplong:
stop = 5
timenow = datetime.datetime.now()
internaltime = (timenow - self.lasttime).total_seconds()
other = [
xDirect, yDirect, self.lastaction[0], self.lastaction[1],
alphadirect, alphacos, self.lastLeftRightFeel[0],
self.lastLeftRightFeel[1], free, stop, self.lastalphadirect,
self.lastalphacos
]
nextstate = []
# print(f"other {other}")
for i in deepfeel:
nextstate.append(i)
for i in other:
nextstate.append(i)
self.lasttime = timenow
self.lastLeftRightFeel = [nextstate[0], nextstate[63]]
if self.forceWallMenory > 0:
self.forceWallMenory -= 1
if self.forceWallMenory <= 0:
self.aimRotaion = self.finalAimRotaion.copy()
print("曾经感受到墙")
self.lastalphadirect = alphadirect
self.lastalphacos = alphacos
return nextstate
def caculateObs(self, state, uprange=25, downrange=36): # 压缩转成线
imageCompact = []
for i in range(uprange, downrange):
imageCompact.append(state[i][:])
imageCompact = np.array(imageCompact)
power = np.min(imageCompact, axis=0)
# print(power)
for i in range(len(power)):
if power[i] > 8:
power[i] = 8
elif power[i] < 0.2 and power[i] > 0.000001:
power[i] = 0.2
elif power[i] == 0:
power[i] = 7
# print(power)
return power
class InferenceModel():
def __init__(self, aimRotation, actiondim=1):
print("init InferenceModel")
dtype = torch.float64
torch.set_default_dtype(dtype)
# device = torch.device('cuda', index=0) # if torch.cuda.is_available() else
device = torch.device('cpu')
# if torch.cuda.is_available():
# torch.cuda.set_device(0)
parser = argparse.ArgumentParser(description='PyTorch PPO example')
parser.add_argument('--env-name',
default="continueRealEnvPpo",
metavar='G',
help='name of the environment to run')
parser.add_argument('--version',
default="4.3.1.8.9",
metavar='G',
help='version')
args = parser.parse_args()
path = os.path.join(assets_dir(), args.version)
print(path)
randomSeed = 2
render = False
state_dim = 64 + 12 + 15 + (
64 + 2 + 2) * 3 # env.observation_space.shape[0]#[0]
running_state = ZFilter((state_dim, ), clip=5)
"""define actor and critic"""
policy_net = Policy(state_dim, 3) #DiscretePolicy(75,5) #Policy(75, 4)
value_net = Value(state_dim)
policy_net.load_state_dict(
torch.load(
os.path.join(path,
'policy_net_{}_ppo.pth'.format(args.env_name)),
'cpu'))
value_net.load_state_dict(
torch.load(
os.path.join(path,
'value_net_{}_ppo.pth'.format(args.env_name)),
'cpu'))
# policy_net = torch.load(os.path.join(path, 'policy_net_{}_ppo.pth'.format(args.env_name)), 'cpu')
# value_net = torch.load(os.path.join(path, 'value_net_{}_ppo.pth'.format(args.env_name)), 'cpu')
running_state, saveavgreward = pickle.load(
open(
os.path.join(path,
'running_state_{}_ppo.p'.format(args.env_name)),
"rb"))
print("get reward {}".format(saveavgreward))
policy_net.to(device)
value_net.to(device)
self.persistence = Persistence("real_0515_" + args.version)
"""create agent"""
self.agent = DroneAgent(policy_net,
value_net,
device,
running_state=running_state,
render=render,
num_threads=1)
self.lastRotation = aimRotation
self.lastLeftRightFeel = [8, 8]
self.lastaction = [0, -0.5, 0]
self.lasttime = datetime.datetime.now()
self.forceWallMenory = 0
self.aimRotaion = aimRotation
self.aimRotation = aimRotation
self.finalAimRotaion = aimRotation
self.stoptime = 0
self.stoplong = 5
self.lastalphadirect = 0
self.lastalphacos = 0.5
self.stopfrequency = 0
self.stopfrequencylong = 5
self.saveTimes = 3
self.redirectNum = 0
# 历史内容暂存
self.saveDeepFeels = queue.Queue()
self.savelastActions = queue.Queue()
self.savelastAngles = queue.Queue()
for i in range(3):
temp = [8] * 64 #初始观感长预设为8
self.saveDeepFeels.put(temp)
self.savelastActions.put([0, 0])
self.savelastAngles.put([0, 1])
self.tempPoCache = queue.Queue()
for i in range(20):
temp = [-1000, -1000]
self.tempPoCache.put(temp)
print("init succ")
def inference(self, imgstate, rotation, aimRotation, time):
deepfeel = self.caculateObs(imgstate)
state = self.getState(deepfeel, rotation, self.lastaction[2])
action, value = self.agent.predictTakeValue(state)
info = f"time {time} action {action} critic {value} state {state} deepfeel avg {np.mean(deepfeel)} value {deepfeel} "
info2 = f"action {action} critic {value} state {state[0:64]}"
if action[1] > 0:
self.stoptime = 0
else:
self.stoptime += 1
self.stopfrequency += 1
print(info2)
self.persistence.saveTerminalRecord("stateaction", info)
self.lastaction = action.copy()
if self.stopfrequency > 0:
self.stopfrequency -= 0.5
if self.forceWallMenory > 0:
pass
return action
def sliceWindow(self, deepfeel, sliceSize=8, proValue=2, threshold=1.8):
go = int(sliceSize / 2)
sliceRes = []
temp = 0
for i in range(0, len(deepfeel) - sliceSize + 1, go):
temp = 0
for j in range(0, sliceSize):
if deepfeel[i + j] < threshold:
if temp == 0:
temp = 10
temp = (threshold - deepfeel[i + j]) * proValue
sliceRes.append(temp)
return sliceRes
def judgeForceWall(self,
deepfeel,
alphacos,
max=6.5,
avgmax=3.2,
twoavgmax=1.8,
smallthreshold=0.55): # 待修改
#看局部最优
tempbest = False
if self.stopfrequency > self.stopfrequencylong:
tempbest = True
if alphacos < 0.7:
return False
totalLength = 0
maxLength = 0
smallnum = 0
for i in deepfeel:
if i > maxLength:
maxLength = i
if i < smallthreshold:
smallnum += 1
totalLength += i
avgLength = totalLength / len(deepfeel)
if avgLength < avgmax and maxLength < max or avgLength < twoavgmax or smallnum > 5 or tempbest:
self.forceWallMenory = random.randint(15, 20)
return True
if self.stoptime > self.stoplong:
self.forceWallMenory = random.randint(15, 20)
return True
return False
def chTarget(self, direct):
print("更改临时目标")
pos = np.array([self.drone.center.x, self.drone.center.y])
'''
aimDirectX = self.aim[0] - pos[0]
aimDirectY = self.aim[1] - pos[1]
if direct >= 0:
self.aim[0] = aimDirectY + pos[0]
self.aim[1] = (-1) * aimDirectX + pos[1]
else:
self.aim[0] = (-1) * aimDirectY + pos[0]
self.aim[1] = aimDirectX + pos[1]
c = round(math.sin(math.radians(i)),6)
d = round(math.cos(math.radians(i)),6)
'''
aimDirectX = self.aim[0] - pos[0]
aimDirectY = self.aim[1] - pos[1]
if direct > 1:
direct = 1
elif direct < -1:
direct = -1
angle = direct * 80
sinthlta = 1
costhlta = 0 #round(math.cos(math.radians(abs(angle))),6)
if direct >= 0:
self.aimRotation -= 90
else:
self.aimRotation -= 90
if self.aimRotation < -180:
self.aimRotation += 360
elif self.aimRotation > 180:
self.aimRotation -= 360
def getState(self, deepfeel, rotation, direct): # 极端诡异和空格于制表符显示问题
rotation = math.radians(rotation)
sliceRes = self.sliceWindow(deepfeel)
aimRotation = math.radians(self.aimRotation)
xDirect = round(math.cos(rotation), 6)
yDirect = round(math.sin(rotation), 6)
aimDirectX = round(math.cos(aimRotation), 6)
aimDirectY = round(math.sin(aimRotation), 6)
alphadirect = aimDirectX * yDirect - aimDirectY * xDirect
alphacos = aimDirectX * xDirect + aimDirectY * yDirect # 直接用目标与行进方向夹角sin cos作为状态
if alphacos < 0 and alphadirect > 0:
alphadirect = 1
if alphacos < 0 and alphadirect < 0:
alphadirect = -1
'''
judgeState = self.judgeForceWall(deepfeel, alphacos)
if judgeState == True:
self.chTarget(direct)
aimRotation = math.radians(self.aimRotation)
xDirect = round(math.cos(rotation), 6)
yDirect = round(math.sin(rotation), 6)
aimDirectX = round(math.cos(aimRotation), 6)
aimDirectY = round(math.sin(aimRotation), 6)
alphadirect = aimDirectX * yDirect - aimDirectY * xDirect
alphacos = aimDirectX * xDirect + aimDirectY * yDirect # 直接用目标与行进方向夹角sin cos作为状态
if alphacos < 0 and alphadirect > 0:
alphadirect = 1
if alphacos < 0 and alphadirect < 0:
alphadirect = -1
'''
free = 0
if self.forceWallMenory > 0:
free = 5 + self.forceWallMenory
stop = 0
if self.stoptime >= self.stoplong:
stop = 5
timenow = datetime.datetime.now()
internaltime = (timenow - self.lasttime).total_seconds()
other = [
xDirect, yDirect, self.lastaction[0], self.lastaction[1],
alphadirect, alphacos, self.lastLeftRightFeel[0],
self.lastLeftRightFeel[1], free, stop, self.lastalphadirect,
self.lastalphacos
]
nextstate = []
# print(f"other {other}")
for i in deepfeel:
nextstate.append(i)
for i in other:
nextstate.append(i)
for i in sliceRes:
nextstate.append(i)
for i in range(self.saveTimes):
queue = self.saveDeepFeels.get()
for j in queue:
nextstate.append(j)
self.saveDeepFeels.put(queue)
for i in range(self.saveTimes):
queue = self.savelastActions.get()
for j in queue:
nextstate.append(j)
self.savelastActions.put(queue)
for i in range(self.saveTimes):
queue = self.savelastAngles.get()
for j in queue:
nextstate.append(j)
self.savelastAngles.put(queue)
self.savelastAngles.get()
self.savelastAngles.put([alphadirect, alphacos])
self.lasttime = timenow
self.lastLeftRightFeel = [nextstate[0], nextstate[63]]
self.saveDeepFeels.get()
self.saveDeepFeels.put(deepfeel.copy())
self.savelastActions.get()
self.savelastActions.put([self.lastaction[0], self.lastaction[1]])
if self.forceWallMenory > 0:
self.forceWallMenory -= 1
if self.forceWallMenory <= 0:
self.aimRotaion = self.finalAimRotaion.copy()
print("曾经感受到墙")
self.lastalphadirect = alphadirect
self.lastalphacos = alphacos
return nextstate
def caculateObs(self, state, uprange=25, downrange=36): # 压缩转成线
imageCompact = []
for i in range(uprange, downrange):
imageCompact.append(state[i][:])
imageCompact = np.array(imageCompact)
power = np.min(imageCompact, axis=0)
# print(power)
for i in range(len(power)):
if power[i] > 8:
power[i] = 8
elif power[i] < 0.2 and power[i] > 0.000001:
power[i] = 0.2
elif power[i] == 0:
power[i] = 7
# print(power)
return power