validate_results.py

import calculate_parallel_curves
import agent
import simulation
import calculate_nearpoint_farpoint
import calculate_visual_angles
import matplotlib.pyplot as plt
import numpy as np
import math
import initialize_simulation

# Set simulation and agent objects =================================================================
# Agent, nearPoint and farPoint initial positions
center_X,center_Y = calculate_parallel_curves.x, calculate_parallel_curves.final_sine
current_pos =30
start_position_x,start_position_y = center_X[current_pos],center_Y[current_pos] # start at the front center of the road

# current position
current_x,current_y = start_position_x,start_position_y # because this is a static plot, we use start_x and start_y as the current position

# Agent parameters/variables
agent_size,color,velocity = 10,'red',16.9
phi_gain,derivative_nearPoint_gain,derivative_farPoint_gain,proportional_nearPoint_gain = 1,30,13.5,36

# Agent object instantiated
agent1 = agent.Agent(start_position_x,start_position_y,
                        agent_size,color,velocity,
                        phi_gain,derivative_nearPoint_gain,derivative_farPoint_gain,proportional_nearPoint_gain)

# Simulation parameteres/variables
delta_time,number_of_frames = 0.05,20
start_delta_alpha,start_alpha,start_phi = 0,0,np.radians(45)

# Simulation object instantiated
simulation1 = simulation.Simulation(delta_time,number_of_frames,start_delta_alpha,start_alpha,start_phi)
simulation.append_arrays((start_delta_alpha,simulation1.delta_alpha_array),(start_alpha,simulation1.alpha_array),(start_phi,simulation1.phi_array))
# Append arrays for initial positions of agent, nearPoint and farPoint
simulation.append_arrays((start_position_x,simulation1.agent_x_position_array),(start_position_y,simulation1.agent_y_position_array),(start_position_x,simulation1.nearPoint_x_array),(start_position_y,simulation1.nearPoint_y_array),(start_position_x,simulation1.farPoint_x_array),(start_position_y,simulation1.farPoint_y_array))

# road plot
plt.plot(calculate_parallel_curves.x,calculate_parallel_curves.final_sine,c='yellow',ls='--')
plt.plot(calculate_parallel_curves.left_x_array,calculate_parallel_curves.left_y_array, color = "black")
plt.plot(calculate_parallel_curves.right_x_array,calculate_parallel_curves.right_y_array, color = "black")

# =======================================================================================================
# Validify placement of nearPoint and farPoint
# current_x,current_y = calculate_parallel_curves.x[current_pos],calculate_parallel_curves.final_sine[current_pos]
# np_x, np_y = calculate_nearpoint_farpoint.get_point(current_x,current_y,calculate_parallel_curves.x,calculate_parallel_curves.final_sine,20)
# fp_x, fp_y = calculate_nearpoint_farpoint.get_point(current_x,current_y,calculate_parallel_curves.x,calculate_parallel_curves.final_sine,50)

# plt.scatter(current_x,current_y, s = 150,c='red')
# plt.scatter(np_x, np_y,c='green')
# plt.scatter(fp_x, fp_y,c='blue')
# plt.show()
# ==========================================================================================================

# =========================================================================================================
# Validate nearPoint and farPoint angles

# use north vector as reference for heading vector angle
north_terminal_point_x,north_terminal_point_y = current_x, current_y+20

# nearPoint and farPoint positions
np_x, np_y = calculate_nearpoint_farpoint.get_point(current_x,current_y,calculate_parallel_curves.x,calculate_parallel_curves.final_sine,5)
fp_x, fp_y = calculate_nearpoint_farpoint.get_point(current_x,current_y,calculate_parallel_curves.x,calculate_parallel_curves.final_sine,20)
simulation.append_arrays((np_x,simulation1.nearPoint_x_array),(np_y,simulation1.nearPoint_y_array),
                                    (fp_x,simulation1.farPoint_x_array),(fp_y,simulation1.farPoint_y_array))

# plot nearPoint and farPoint 
plt.scatter(current_x,current_y,s=200,c='red')
plt.scatter(np_x, np_y,c='green')
plt.scatter(fp_x, fp_y,c='blue')

# determine previous_position for angle calculation
i=2
simulation1.agent_x_position_array.append(center_X[current_pos-1])
simulation1.agent_y_position_array.append(center_Y[current_pos-1])
simulation1.agent_x_position_array.append(center_X[current_pos])
simulation1.agent_y_position_array.append(center_Y[current_pos])

#  WHEN F > 0 (IN THE LOOP) --------------------------------------------------------------------------------
# # calculate visual angles and print them
# np_visual_angle,past_position = calculate_visual_angles.calculate_visual_angle(i,simulation1,current_x,current_y,np_x,np_y)
# fp_visual_angle,past_position = calculate_visual_angles.calculate_visual_angle(i,simulation1,current_x,current_y,fp_x,fp_y)
# north_visual_angle, past_position = calculate_visual_angles.calculate_visual_angle(i,simulation1,current_x,current_y,north_terminal_point_x,north_terminal_point_y)
# past_x = past_position[0]
# past_y = past_position[1]

# delta_x = current_x - past_x
# delta_y = current_y - past_y

# heading_terminal_x = delta_x + current_x
# heading_terminal_y = delta_y + current_y

# WHEN F = 0 (OUTSIDE OF LOOP) ---------------------------------------------------------------------
# calculate visual angles and print them
np_visual_angle,future_position = initialize_simulation.init_visual_angle(simulation1,agent1,current_x,current_y,np_x,np_y)
fp_visual_angle,future_position = initialize_simulation.init_visual_angle(simulation1,agent1,current_x,current_y,fp_x,fp_y)
north_visual_angle, future_position = initialize_simulation.init_visual_angle(simulation1,agent1,current_x,current_y,north_terminal_point_x,north_terminal_point_y)

delta_x = future_position[0] - current_x
delta_y = future_position[1] - current_y

heading_terminal_x = delta_x + current_x
heading_terminal_y = delta_y + current_y

# plot visualPoint vectors ------------------------------------------------------------------
heading_vector_x, heading_vector_y = [current_x, heading_terminal_x+delta_x*20], [current_y,heading_terminal_y+delta_y*20]
nearPoint_vector_x, nearPoint_vector_y = [current_x,np_x], [current_y,np_y]
farPoint_vector_x, farPoint_vector_y = [current_x,fp_x], [current_y,fp_y]
north_vectorline_x, north_vectorline_y = [current_x, north_terminal_point_x], [current_y,north_terminal_point_y]

plt.plot(north_vectorline_x, north_vectorline_y, c="purple", zorder = 4)
plt.plot(nearPoint_vector_x, nearPoint_vector_y, c="green", zorder = 3)
plt.plot(farPoint_vector_x, farPoint_vector_y, c="blue", zorder = 3)
plt.plot(heading_vector_x, heading_vector_y, c="black", zorder = 4,linewidth=2)
# plt.scatter(heading_vector_x, heading_vector_y,c='black')
# print("agent position",current_x,current_y)

print("near point:",math.degrees(np_visual_angle))
print("far point:",math.degrees(fp_visual_angle))
print("heading relative to north:",math.degrees(north_visual_angle))

plt.show()

print(current_x,current_y)
print(future_position[0],future_position[1])
# -------------------------------------------------------------------------------------------------
# ==============================================================================================