Skip to content
Snippets Groups Projects
Commit edd2e894 authored by Midras Lappe's avatar Midras Lappe
Browse files

added Client from Midras Lappe

parent 48ec0c12
Branches
No related tags found
No related merge requests found
# -*- coding: utf-8 -*-
"""
Created on Tue Jan 12 17:17:49 2021
@author: theob
"""
import asyncio
import Gym_new
import time
import nest_asyncio
import numpy as np
import random
import math
import matplotlib.pyplot as plt
async def main():
env = Gym_new.Gym()
await env.connect_websocket()
command = ""
#observation, reward, done, info = await env.reset(0.017)
# observation, reward, done, info = await env.step("accelerate,steer_right", 0.17)
# #actionen sind accelerate, steer_left, steer_right, brake, reverse
# print(observation)
# print(reward)
# print(done)
for i in range(random.randrange(0, 50)):
#print(i)
observationO, rewardO, doneO, infoO = await env.step(command, 0.2)
step_width = 0.02
#observationO, rewardO, doneO, infoO = await env.step(command, 0.02)
observation, reward, done, info = await env.reset(step_width)
speedO = observationO[0]
steering_angleO = observationO[1]
sensorsO = np.array(observationO[2])
sensors_dir = []
for i in range(len(sensorsO)):
x = (-np.sin(-i*2*np.pi/len(sensorsO)))
y = (np.cos(-i*2*np.pi/len(sensorsO)))
sensors_dir.append([x,y])
sensors_dir =np.array(sensors_dir)
sensors_dir_len=sensors_dir*sensorsO[:,np.newaxis]
#print(sensors_dir_len)
sollwinkel=0
sollgeschwindigkeit = 0
sensors_old =np.zeros([len(sensorsO), 5])
speed_old = np.zeros([1,5])
#print(sensors_old)
sensors_old[:,0]=sensorsO
sensors_old = np.roll(sensors_old,1)
#print(sensors_old)
zubringer = True
abstand_rechts = 2
aufgefahren = False
change_speed_log = np.array([[0,0,0,0,0,0,0,0,0],[0,0,0,0,0,0,0,0,0]])
timeg = 0;
i=1
while(True):
observation, reward, done, info = await env.step(command, step_width)
command = ""
speed = observation[0]
steering_angle = observation[1]
sensors = np.array(observation[2])
sensors_old = np.roll(sensors_old,1)
sensors_old[:,0]=sensors
speed_old = np.roll(speed_old,1)
speed_old[:,0]=speed
speed_mean = np.mean(speed_old)
#print("Durchschnittsgeschwindigkeit"+str(speed_mean))
sensor_change_speed = (sensors_old[:,4]-sensors_old[:,0])/(5*step_width)
change_speed_log = np.append(change_speed_log,([np.append(timeg,sensor_change_speed)]),axis = 0)
timeg = timeg+step_width
#print("Change Speed" + str(sensor_change_speed))
#print(sensors_old)
sensors_sum = np.sum(np.absolute(sensors))
# print("Summe der Sensoren"+str(sensors_sum))
sensors_dir_len=sensors_dir*sensors[:,np.newaxis]
sensors_dir_speed = sensors_dir*sensor_change_speed[:,np.newaxis]
sensors_dir_speed[:,0]-=speed_mean/3.6
#print("Sensors Speed mit x Y"+str(sensors_dir_speed))
#print(sensors_dir_len)
dir_sum = np.sum(sensors_dir_len,axis=0)
#print(dir_sum)
#aufgefahren = False
#print("Sensors Sum = "+str(sensors_sum))
if(zubringer): #Steuerung für die Auffahrt (sensors_sum < 90) &
#sollgeschwindigkeit = 90*(2.2*sensors[0]-sensors[4])/13 #1.9 1.7
#sollgeschwindigkeit = 90*(2.2*sensors[0]-5)/13
sollgeschwindigkeit = 16*(sensors[0]-2.3)
try:
sollwinkel = 3.7*((dir_sum[0])/13+((sensors[1]+sensors[5])-(sensors[3]+sensors[7]))/13)*(speed/abs(speed))#3.3 3.7
except Exception as e:
sollwinkel = 3.7*((dir_sum[0])/13+((sensors[1]+sensors[5])-(sensors[3]+sensors[7]))/13)
#print("Sollwinkel auffahrt "+str(sollwinkel))
#print('Sensor Change speed'+ str( sensor_change_speed[7]))
#if((sensors_sum>=90)):
if((sensor_change_speed[7]<-13)&(reward[0]<247)): #&(sensors_sum>=90)
zubringer = False
if(not zubringer):
weight_s1_s3 = 1
weight_s2 = 1
#if()
#sollgeschwindigkeit = (50*(2.2*sensors[0]-5)/13)
sollgeschwindigkeit = 8.5*(sensors[0]-2.3)
if((not aufgefahren)&(sensors[7]>3.5)&(sensors[6]>3)&(sensors[5]>2)&(abstand_rechts<6)):
abstand_rechts += 0.1#0.06
elif(not aufgefahren):
#sollgeschwindigkeit=130*(1-np.exp(-sensors[0]/5))
sollgeschwindigkeit = 80 + (sensors[7]-sensors[5])*5
if((sensors[2]>5)&(abstand_rechts>5)):#sensor[2]>5.5 #not aufgefahren & (abs(sensor_change_speed[1])<10)
abstand_rechts = 5.5
aufgefahren = True
#print("Sensor 3 change_spped"+str(abs(sensor_change_speed[1])))
if(aufgefahren&(abs(sensor_change_speed[1])>8)):#&(abstand_rechts>4.5)): #&
abstand_rechts = 1#1.5 #0.3*sensor_change_speed[1]/60
weight_s1_s3 = 0
weight_s2 = 0
#time.sleep(1)
#time.sleep(0.5)
#sollwinkel = 1.3*(dir_sum[0]+16)/13
#print("Abstand rechts soll "+str(abstand_rechts))
try:
sollwinkel = 3.3*(weight_s1_s3*(sensors[1]-sensors[3])/3+weight_s2*(sensors[2]-abstand_rechts)/3)*(speed/abs(speed))#(sensors[7]-sensors[5])/13)
except Exception as e:
sollwinkel = 3.3*(weight_s1_s3*(sensors[1]-sensors[3])/3+weight_s2*(sensors[2]-abstand_rechts)/3)
# print("Sensor 1 "+str(sensors[1]))
# print("Sensor 3 "+str(sensors[3]))
#print("Sensor 2 "+str(sensors[2]))
#print("Sensor 7 "+str(sensors[7]))
#print("Sollwinkel"+str(sollwinkel))
if(sollwinkel>steering_angle):
# print("steer_right")
command +=("steer_right")
elif(sollwinkel<steering_angle):
# print("steer_left")
command +=("steer_left")
#print("Sollgeschwindigkeit"+str(sollgeschwindigkeit))
if(sollgeschwindigkeit>speed):
command += ("accelerate")
elif((speed>0) & (sollgeschwindigkeit<speed)):
command +=("brake")
elif(sollgeschwindigkeit<speed):
command +=("reverse")
if(sensors_sum==0):
command +=("reset")
# if reward[1] < 0:
# env.reset(0.01)
#if speed < 50 :
# command += ("accelerate")
# if(sensorsO[1]-sensors[1]>0 ):
# command += (",steer_left" )
# elif(sensorsO[7]-sensors[7]>0):
# command +=("steer_right")
# elif ( (sensors[1] + 2 < sensors[7])&(abs(steering_angle)<3) ): #
# command += (",steer_left" )
# elif ( (sensors[1] > sensors[7]+2)&(abs(steering_angle)<3)): #(sensors[1] > sensors[7]+1)&
# command +=("steer_right")
# else:
# if ((sensors[4]-sensorsO[4]>0) & (steering_angle != 0) & (sensors[6] > 2.5) & (sensors[0]>10)):
# command +=("steer_left")
# elif ((sensors[5]-sensorsO[5]>0) & (steering_angle != 0) & (sensors[2] > 2.5)& (sensors[0]>10)):
# command +=("steer_right")
#observation, reward, done, info = await env.reset(0.017)
observationO, rewardO, doneO, infoO = observation, reward, done, info
#print(observation)
#print(reward)
#print(done)
time.sleep(0.05)
if(done or (reward[1]>50)or (reward[0]<170)):
# print(change_speed_log)
# print(change_speed_log[:,0])
# print(change_speed_log[:,1])
# for i in range(1,8):
# plt.plot(change_speed_log[:,0],change_speed_log[:,i]) # plotting by columns
# plt.plot(change_speed_log[:,0],change_speed_log[:,[2,8]])
# plt.legend(['Sensor 1','Sensor 7'])
# plt.xlabel('t in Sekunden')
# plt.ylabel('ds/dt')
# plt.xlim(0.01,12)
# plt.show()
# f=open('ClientV4Changespeed.csv','ab')
# np.savetxt(f,change_speed_log)
# f.close()
#break
f=open('ClientV7TestDone_all_maps.csv','ab')
np.savetxt(f,[np.append(done,reward[0])])
print(np.append(done,reward[0]))
f.close()
aufgefahren = False
zubringer = True
observation, reward, done, info = await env.reset(step_width)
observation, reward, done, info = await env.reset(step_width)
if i == 100:
break
i+=1
if __name__ == "__main__":
nest_asyncio.apply()
loop = asyncio.get_event_loop()
loop.run_until_complete(main())
\ No newline at end of file
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment