env = CarlaEnv()
break
except Exception as e:
print(e)
max_episodes = 100
max_steps = 1800
control = Controller()
for i in range(int(max_episodes)):
s = env.reset()
counter = 0
while True:
if counter == 0:
a = control.action(s[0], s[3], s[1], 0.001, controller_type="LQR")
else:
a = control.action(s[0], s[3], s[1], s[2], controller_type="LQR")
s2, r, terminal, info = env.step(a[0])
s = s2
counter += 1
if terminal:
history_pos = last_info["history"]
history_xte = last_info["xte_history"]
history_vel = last_info["velError_history"]
with open("stored_data/car_history_H2_2.pkl", "wb") as hand:
pickle.dump(history_pos, hand)
with open("stored_data/car_history_H2_2_xte.pkl", 'wb') as hand:
pickle.dump(history_xte, hand)
finally:
# 断开服务器链接
if robot.connected:
# 关闭机械臂
robot.robot_shutdown()
# 断开机械臂链接
robot.disconnect()
print("Disconnected")
# 释放库资源
WuziRobot.uninitialize()
logger.info("{0} test completed.".format(WuziRobot.get_local_time()))
# Stop streaming
pipeline.stop()
import time
time.sleep(1)
robot_first = True
controller = Controller(True)
if robot_first:
controller.action(None)
new_chess = controller.wait_human()
while 1:
if controller.action(new_chess) is None:
break
new_chess = controller.wait_human()
controller.disconnect()