def optimized_rear_sensor(self, car, act_mask, display_obstacle_on_sensor=False):
"""
rear visual sensor
:param car:
:param object_mask:
:param act_mask:
:param display_obstacle_on_sensor:
:return:
"""
# act_mask is a separate image where you can only see what the sensor sees
position = self.global_cars_positions[car.car_tag]
center = Collision.calculate_center_for_car(car, position)
if car.car_tag == self.current_car:
center = Collision.center_rect(self.screen_width, self.screen_height)
mid_of_rear_axle = Collision.point_rotation(car, 65, 16, center)
arc_points = get_arc_points(mid_of_rear_axle, 150, radians(-90 + car.angle), radians(90 + car.angle),
self.sensor_size)
draw_center = Collision.center_rect(self.screen_width, self.screen_height)
draw_mid_rear_axle = Collision.point_rotation(car, 65, 16, draw_center)
draw_arc_points = get_arc_points(draw_mid_rear_axle, 150, radians(-90 + car.angle), radians(90 + car.angle),
self.sensor_size)
offroad_edge_points = []
draw_offroad_edge_points = []
for end_point, draw_end_point in zip(arc_points, draw_arc_points):
points_to_be_checked = list(get_equidistant_points(mid_of_rear_axle, end_point, 25))
draw_points_to_be_checked = list(get_equidistant_points(draw_mid_rear_axle, draw_end_point, 25))
check = False
for line_point, draw_line_point in zip(points_to_be_checked, draw_points_to_be_checked):
try:
if np.array_equal(self.object_mask.get_at((int(line_point[0]), int(line_point[1]))), self.bkd_color):
check = True
break
except:
check = True
break
if check is False:
offroad_edge_points.append(end_point)
draw_offroad_edge_points.append(draw_end_point)
else:
offroad_edge_points.append(line_point)
draw_offroad_edge_points.append(draw_line_point)
for index in range(0, len(arc_points)):
if offroad_edge_points[index] == arc_points[index]:
pygame.draw.line(self.screen, (0, 255, 0), mid_of_rear_axle, arc_points[index], True)
pygame.draw.line(act_mask, (0, 255, 0), draw_mid_rear_axle, draw_arc_points[index], True)
else:
pygame.draw.line(self.screen, (0, 255, 0), mid_of_rear_axle, offroad_edge_points[index], True)
pygame.draw.line(act_mask, (0, 255, 0), draw_mid_rear_axle, draw_offroad_edge_points[index], True)
if display_obstacle_on_sensor is True:
pygame.draw.line(self.screen, (255, 0, 0), offroad_edge_points[index], arc_points[index], True)
pygame.draw.line(act_mask, (255, 0, 0), draw_offroad_edge_points[index], draw_arc_points[index], True)
def enable_rear_sensor(self, car, draw_screen, rays_nr):
"""
rear distance sensor
:param car:
:param data_screen:
:param draw_screen:
:param rays_nr:
:return:
"""
position = self.global_cars_positions[car.car_tag]
center = Collision.calculate_center_for_car(car, position)
if car.car_tag == self.current_car:
center = Collision.center_rect(self.screen_width, self.screen_height)
mid_of_rear_axle = Collision.point_rotation(car, 65, 16, center)
distance = np.array([])
for angle_index in range(120, 240, int(round(120/rays_nr))):
distance = np.append(distance,
self.compute_sensor_distance(car, mid_of_rear_axle, 200, angle_index, self.object_mask,
draw_screen, side='rear'))
return distance
def draw_mpc_prediction(self, *args, mpc_solution, car_tag):
"""
:param args: args[0] -> mpc_traj_points_carX, args[1] -> mpc_angle_carX
:param mpc_solution:
:param car_tag:
:return:
"""
if car_tag == self.current_car:
center_screen = (int(self.screen_width / 2), int(self.screen_height / 2))
else:
position = self.global_cars_positions[car_tag]
center_screen = Collision.calculate_center_for_car(self.kinematic_cars[car_tag], position)
args[0].clear()
for index in range(2, 20, 2):
delta_position = (
mpc_solution[index] * cos(np.deg2rad(args[1])) + mpc_solution[index + 1] * sin(np.deg2rad(args[1])),
mpc_solution[index] * (-sin(np.deg2rad(args[1]))) + mpc_solution[index + 1] * cos(np.deg2rad(args[1])))
x_point = center_screen[0] - int(delta_position[0] * self.ppu)
y_point = center_screen[1] - int(delta_position[1] * self.ppu)
traj_point = (x_point, y_point)
args[0].append(traj_point)
def draw_trajectory(self, car, car_data):
# draw trajectory
"""
:param car:
:param car_data:
:return:
"""
if car.car_tag == self.current_car:
center_screen = (int(self.screen_width / 2), int(self.screen_height / 2))
else:
position = self.global_cars_positions[car.car_tag]
center_screen = Collision.calculate_center_for_car(car, position)
trajectory_points = []
waypoints = []
min = 1000
idx = -1
if car.car_tag == 'car_1':
prev_ref_index = self.prev_ref_index_car1
elif car.car_tag == 'car_2':
prev_ref_index = self.prev_ref_index_car2
else:
raise ValueError("Car tag not defined")
for elem in range(prev_ref_index-40, prev_ref_index+40):
dx = car_data[elem][0] - car.position[0]
dy = car_data[elem][1] - car.position[1]
d = abs(math.sqrt(dx**2+dy**2))
if d < min:
min = d
idx = elem
prev_ref_index = idx
if car.car_tag == 'car_1':
self.prev_ref_index_car1 = prev_ref_index
elif car.car_tag == 'car_2':
self.prev_ref_index_car2 = prev_ref_index
else:
raise ValueError("Car tag not defined")
for add_elem in range(idx, idx + 150, 15):
if add_elem < len(car_data):
delta_position = (
car.position[0] - car_data[add_elem][0],
car.position[1] - car_data[add_elem][1])
x_point = center_screen[0] + int(delta_position[0] * self.ppu)
y_point = center_screen[1] + int(delta_position[1] * self.ppu)
traj_point = (x_point, y_point)
trajectory_points.append(traj_point)
if len(waypoints) < 9:
waypoints.append((car_data[add_elem][0], car_data[add_elem][1]))
# draw each trajectory point
pygame.draw.circle(self.screen, (255, 255, 0), traj_point, 2, 2)
# draw lines between trajectory points
for traj_point, next_traj_point in zip(trajectory_points, trajectory_points[1:]):
pygame.draw.aaline(self.screen, (255, 255, 0), traj_point, next_traj_point, 10)
self.prepare_mpc_input(car, waypoints)
if car.car_tag == 'car_1':
if len(self.mpc_trajectory_points_car1) > 0:
for traj_point, next_traj_point in zip(self.mpc_trajectory_points_car1, self.mpc_trajectory_points_car1[1:]):
pygame.draw.aaline(self.screen, (0, 255, 0), traj_point, next_traj_point, 10)
elif car.car_tag == 'car_2':
if len(self.mpc_trajectory_points_car2) > 0:
for traj_point, next_traj_point in zip(self.mpc_trajectory_points_car2, self.mpc_trajectory_points_car2[1:]):
pygame.draw.aaline(self.screen, (0, 255, 0), traj_point, next_traj_point, 10)
