diff --git a/client_V7.py b/client_V7.py new file mode 100755 index 0000000000000000000000000000000000000000..4b4fbf88985fa8985ccaad184bcce9dabbce6c74 --- /dev/null +++ b/client_V7.py @@ -0,0 +1,217 @@ +# -*- 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