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)