'\' connecting to {} port {} for output.'.format(*server_address_out))
sock_output.connect(server_address_out)
module_running = True
try:
iter = 0
while module_running:
######################################################
# WHEN YOU ARE READY, START READING INCOMING DATA
# TO PROCESS IT. THIS FUNCTION CALL IS BLOCKING,
# IT WILL WAIT UNTIL THERE'S DATA. THE DATA WILL
# BE WAITING AND PILE UP UNTIL YOU READ IT.
######################################################
msg = tcplib.receiveData(sock_input)
print('message:', msg)
path, yellow, blue = get_path(msg)
######################################################
# PERFORM SOME CALCULATIONS.
# WHEN YOU GET A RESULT YOU WISH TO SEND,
# CONTINUE WITH THE CODE BELOW
######################################################
my_results = path
tcplib.sendData(sock_output, my_results)
######################################################
# IF YOU'RE DONE HANDLING THE DATA, THE LOOP
# WILL TAKE YOU BACK TO WAITING ON INCOMING DATA.
print('INFO: \'', MY_MODULE_NAME,
'\' connecting to {} port {} for output.'.format(*server_address_out))
sock_output.connect(server_address_out)
module_running = True
try:
while module_running:
######################################################
# WHEN YOU ARE READY, START READING INCOMING DATA
# TO PROCESS IT. THIS FUNCTION CALL IS BLOCKING,
# IT WILL WAIT UNTIL THERE'S DATA. THE DATA WILL
# BE WAITING AND PILE UP UNTIL YOU READ IT.
######################################################
cone_data = tcplib.receiveData(sock_input_perception)
motion_data = tcplib.receiveData(sock_input)
built_map = SLAM.main(cone_data, motion_data)
######################################################
# PERFORM SOME CALCULATIONS.
# WHEN YOU GET A RESULT YOU WISH TO SEND,
# CONTINUE WITH THE CODE BELOW
######################################################
tcplib.sendData(sock_output, built_map)
######################################################
# IF YOU'RE DONE HANDLING THE DATA, THE LOOP
# WILL TAKE YOU BACK TO WAITING ON INCOMING DATA.
# IF YOUR MODULE ENCOUNTERS AN ERROR OR NEEDS TO
# TERMINATE, CONTINUE WITH THE CODE BELOW
sock_output = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
server_address_out = (IP_ADDR, VEHICLE_PORT)
print('INFO: \'', MY_MODULE_NAME,
'\' connecting to {} port {} for output.'.format(*server_address_out))
sock_output.connect(server_address_out)
module_running = True
try:
iter = 0
while module_running:
# Since for now we receive path and state at really fequence
# We assume that at each iteration we will update our current paramaters
# That won't be the case at the end we should only update our aprameters if we get new ones
new_state = tcplib.receiveData(sock_motion)
new_path = mpc_car_model.local_to_global(tcplib.receiveData(sock_path),
new_state)
# Computation of the output given the new input (asked A. Deherse to be sure about argument order)
# new_state = (new_state[0], new_state[1], new_state[5])
# previous_state = (new_state[3], new_state[5])
# #mpc_module.previous_state = previous_state
mpc_module.acquire_path(new_path)
mpc_module.set_state(new_state)
output = mpc_module.run_MPC()
#Sending to new command to the car
tcplib.sendData(sock_output, output)
if False:
socket_control = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
server_address_out_control = (IP_ADDR, IP_PORT_OUT_CONTROL)
print(
'INFO: \'', MY_MODULE_NAME,
'\' connecting to {} port {} for output.'.format(
*server_address_out_control))
socket_control.connect(server_address_out_control)
module_running = True
first_mes = True
try:
while module_running:
speed = tcplib.receiveData(socket_speed)
imu_frame = tcplib.receiveData(socket_imu)
measurements = np.matrix([[
imu_frame[0], imu_frame[1], imu_frame[4], speed, imu_frame[3],
imu_frame[5]
]]).T
if first_mes:
x = measurements
first_mes = False
#i+=1
#x, P = kalman_update(x, measurements[:,i], P, Q, R, I, GPS)
x, P = kalman_update(x, measurements, P, Q, R, I, dt)
to_send = x.A1