def PID_Go(pShip, pAim, yawShip, gz): # 直行段PID
global thrust_last # 上一时刻油门值
PID_P = 0.5
# PID_D = 0 在下面if结构中定义
yawAim = utils.getAimYaw(pShip, pAim) # 期望方向角
error = utils.angleDiff(yawAim, yawShip) # 方向角误差
dist = utils.getDistance(pShip, pAim) # 距离误差
if error * gz > 0:
PID_D = -0.8
else:
PID_D = 0.8
dire = PID_P * error + gz * PID_D
dire = max(dire, -100)
dire = min(dire, 100)
thrust = 100 - abs(error) * 3
if dist < 10: # 接近段
tar_speed = dist / 8 - abs(error) / 20 # 目标速度
tar_speed = max(tar_speed, 0.2) # 目标速度限幅在0.2到1之间
tar_speed = min(tar_speed, 1)
err_speed = tar_speed - speed
thrust = thrust_last + err_speed * 50
thrust = abs(thrust)
thrust = max(thrust, 10) # 下限是10
thrust = min(thrust, 100) # 上限100
thrust_last = thrust
return int(dire), thrust
def PID_Turn_old(pShip, pAim, yawOpt, yawShip, gz): # 原地转弯PID
# 错误！缺少return 0
global gz_last # 上一时刻角速度
PID_P_gz = 5
# PID_D_gz 在下面if结构中定义
yawAim = utils.getAimYaw(pShip, pAim) # 期望方向角
error = utils.angleDiff(yawAim, yawShip) # 角度差
gzAim = 10 * utils.symbol(error) # 期望角速度
if abs(error) > 100:
dire = 100 * utils.symbol(error) # 角度差大于100度时全速转弯
elif abs(error) < 20:
gzAim = gzAim * abs(error) / 20 * 0.5
error_gz = gzAim - gz # 角速度误差
dire = PID_P_gz * error_gz
if dire < 0:
dire = min(dire, -10)
else:
dire = max(dire, 10)
else:
error_gz = gzAim - gz # 角速度误差
d_gz = gz - gz_last # 角速度增量
gz_last = gz
if error_gz * d_gz > 0:
PID_D_gz = -1
else:
PID_D_gz = 1
dire = PID_P_gz * error_gz + 10 * utils.symbol(error)
gz_last = gz
dire = min(dire, 60)
dire = max(dire, -60)
return dire
def PID_Go(pShip,
pAim,
yawShip,
gz,
yawOpt,
slowSpeed=True,
isFirst=True): # 直行段PID
global thrust_last # 上一时刻油门值
PID_P = 0.5
# PID_D = 0 在下面if结构中定义
if isFirst == True:
yawAim = utils.getAimYaw(pShip, pAim) # 期望方向角
else:
yawAim = yawOpt
# *********************补偿项***************注意，距离小的时候不准
# if utils.getAimYaw(pShip, pAim)>5:
# weight=np.array([0.7,0.3])
# yawAim=angleWeightSum(np.array(yawOpt,utils.getAimYaw(pShip, pAim)),weight)
# else:
# yawAim=yawOpt
error = utils.angleDiff(yawAim, yawShip) # 方向角误差
dist = utils.getDistance(pShip, pAim) # 距离误差
if error * gz > 0:
PID_D = -0.8
else:
PID_D = 0.8
dire = PID_P * error + gz * PID_D
dire = max(dire, -100)
dire = min(dire, 100)
thrust = (100 - abs(error) * 3) * 0.8
tar_speed = 0
if dist < 0.8 and slowSpeed == True:
tar_speed = dist / 1.5 - abs(error) / 20 # 目标速度
tar_speed = max(tar_speed, 0.2) # 目标速度限幅在0.2到1之间
tar_speed = min(tar_speed, 1)
err_speed = tar_speed - speed
thrust = thrust_last + err_speed * 50
thrust = abs(thrust)
thrust = max(thrust, 10) # 下限是10
thrust = min(thrust, 100) # 上限100
thrust_last = thrust
log.debug("PID_Go 目标速度：{}\t实际速度: {}\tthrust: {}\tdist: {}".format(
tar_speed, speed, thrust, dist))
return int(dire), thrust
def PID_Turn(pShip,
pAim,
yawShip,
gz,
yawOpt,
isFirst=True,
error=None,
isLeft=None,
routeType=0): # 原地转弯PID
PID_P_gz = 5
# PID_D_gz 在下面if结构中定义
if error == None:
if isFirst == True:
yawAim = utils.getAimYaw(pShip, pAim) # 期望方向角
else:
yawAim = yawOpt
error = utils.angleDiff(yawAim, yawShip) # 角度差
# if isLeft != None and abs(error) > 120:
# if (isLeft and error > 0):
# error -= 360
# elif not isLeft and error < 0:
# error += 360
if routeType == 0 or routeType == -1:
# 循迹
gzAim = 15 * utils.symbol(error) # 期望角速度
if abs(error) > 100:
dire = 100 * utils.symbol(error) # 角度差大于100度时全速转弯
elif abs(error) < 20:
gzAim = gzAim * abs(error) / 20 / 2
error_gz = gzAim - gz # 角速度误差
dire = PID_P_gz * error_gz
if dire < 0:
dire = min(dire, -20)
else:
dire = max(dire, 20)
if abs(error) < 5:
return 0
else:
error_gz = gzAim - gz # 角速度误差
dire = PID_P_gz * error_gz + 10 * utils.symbol(error)
# elif routeType == 1:
# # 区域
# gzAim = 20*utils.symbol(error) # 期望角速度
# if abs(error) > 60:
# dire = 100*utils.symbol(error) # 角度差大于100度时全速转弯
# elif abs(error) < 20:
# gzAim = gzAim*abs(error)/20/2
# error_gz = gzAim-gz # 角速度误差
# dire = PID_P_gz * error_gz
# if dire < 0:
# dire = min(dire, -30)
# else:
# dire = max(dire, 30)
# if abs(error) < 5:
# return 0
# else:
# error_gz = gzAim-gz # 角速度误差
# dire = PID_P_gz * error_gz+10*utils.symbol(error)
if routeType == -1:
dire = min(dire, 60)
dire = max(dire, -60)
else:
dire = min(dire, 100)
dire = max(dire, -100)
log.debug("PID_Turn 角度差：{}\tdire: {}".format(error, dire))
return int(dire)
def test_one_image(self, image, label):
# Returns criterion loss and actual error when applying the model to an image and comparing output to label
outputs = self.net.forward(image.float())
#print('Outputs: {}'.format(outputs.float().detach().cpu().numpy()[0]))
#print('Labels: {}'.format(label.float().detach().cpu().numpy()[0]))
loss = self.criterion(outputs.float(), label.float())
#print('Loss: {}'.format(loss.item()))
if self.label_style == 'Reward':
state_error = 0
estim_reward = UnnormalizeLabel(
self.environment, self.label_style,
outputs.float().detach().cpu().numpy()[0][0])
real_reward = UnnormalizeLabel(
self.environment, self.label_style,
label.float().detach().cpu().numpy()[0][0])
rew_error = estim_reward - real_reward # (outputs.float() - label.float()).detach().cpu().numpy()[0][0]
pred_state = [0, 0]
elif self.label_style == 'State':
state_estim = UnnormalizeLabel(
self.environment, self.label_style,
outputs.float().detach().cpu().numpy()[0])
state_gt = UnnormalizeLabel(
self.environment, self.label_style,
label.float().detach().cpu().numpy()[0])
hat_x = state_estim[0]
hat_theta = state_estim[1]
x = state_gt[0]
pred_state = [hat_x, hat_theta]
theta = state_gt[1]
state_error = [hat_x - x, ut.angleDiff(hat_theta, theta)]
if self.environment == 'cartpole':
real_reward = cp_modenv.state2reward(x, theta)
estim_reward = cp_modenv.state2reward(hat_x, hat_theta)
elif self.environment == 'duckietown':
real_reward = dt_modenv.state2reward(x, theta)
estim_reward = dt_modenv.state2reward(hat_x, hat_theta)
rew_error = estim_reward - real_reward # (outputs.float() - label.float()).detach().cpu().numpy()[0][0]
elif self.label_style == 'Angle':
angle_estim = UnnormalizeLabel(
self.environment, self.label_style,
outputs.float().detach().cpu().numpy()[0])
angle_gt = UnnormalizeLabel(
self.environment, self.label_style,
label.float().detach().cpu().numpy()[0])
hat_theta = angle_estim[0]
pred_state = [0, hat_theta]
theta = angle_gt[0]
state_error = [0, ut.angleDiff(hat_theta, theta)]
rew_error = 0
real_reward = 0
estim_reward = 0
elif self.label_style == 'Angle_droneAngle':
angles_estim = UnnormalizeLabel(
self.environment, self.label_style,
outputs.float().detach().cpu().numpy()[0])
angles_gt = UnnormalizeLabel(
self.environment, self.label_style,
label.float().detach().cpu().numpy()[0])
hat_theta = angles_estim[0]
theta = angles_gt[0]
hat_drone_angle = angles_estim[1]
drone_angle_gt = angles_gt[1]
pred_state = [0, hat_theta, hat_drone_angle]
state_error = [
0,
ut.angleDiff(hat_theta, theta),
ut.angleDiff(hat_drone_angle, drone_angle_gt)
]
rew_error = 0
real_reward = 0
estim_reward = 0
elif self.label_style == 'Distance':
dist_estim = UnnormalizeLabel(
self.environment, self.label_style,
outputs.float().detach().cpu().numpy()[0])
dist_gt = UnnormalizeLabel(self.environment, self.label_style,
label.float().detach().cpu().numpy()[0])
hat_x = dist_estim[0]
pred_state = [hat_x, 0]
x = dist_gt[0]
state_error = [hat_x - x, 0]
rew_error = 0
real_reward = 0
estim_reward = 0
elif self.label_style == 'State_droneAngle':
state_estim = UnnormalizeLabel(
self.environment, self.label_style,
outputs.float().detach().cpu().numpy()[0])
state_gt = UnnormalizeLabel(
self.environment, self.label_style,
label.float().detach().cpu().numpy()[0])
hat_x = state_estim[0]
hat_theta = state_estim[1]
hat_drone_angle = state_estim[2]
pred_state = [hat_x, hat_theta, hat_drone_angle]
x = state_gt[0]
theta = state_gt[1]
drone_angle_gt = angles_gt[1]
state_error = [
hat_x - x,
ut.angleDiff(hat_theta, theta),
ut.angleDiff(hat_drone_angle, drone_angle_gt)
]
real_reward = dt_modenv.state2reward(x, theta)
estim_reward = dt_modenv.state2reward(hat_x, hat_theta)
rew_error = estim_reward - real_reward # (outputs.float() - label.float()).detach().cpu().numpy()[0][0]
elif self.label_style == 'droneAngle':
drone_angle_estim = UnnormalizeLabel(
self.environment, self.label_style,
outputs.float().detach().cpu().numpy()[0])
drone_angle_label = UnnormalizeLabel(
self.environment, self.label_style,
label.float().detach().cpu().numpy()[0])
hat_drone_angle = drone_angle_estim[0]
pred_state = [0, 0, hat_drone_angle]
drone_angle_gt = drone_angle_label[0]
state_error = [0, 0, ut.angleDiff(hat_drone_angle, drone_angle_gt)]
rew_error = 0
real_reward = 0
estim_reward = 0
return loss.item(
), pred_state, state_error, rew_error, real_reward, estim_reward