def rotate(self, angle: float):
"""
:param angle:
:return:
"""
rotation_matrix = np.array([[np.cos(angle), -np.sin(angle)],
[np.sin(angle),
np.cos(angle)]])
ul = rotation_matrix @ self.upper_left.to_array().T
lr = rotation_matrix @ self.lower_right.to_array().T
return Rectangle([geom.Point(ul[0], ul[1]), geom.Point(lr[0], lr[1])])
def find_starting_beacons(own_colour: TeamColor, clusters: List[Cluster]):
b1, b2, b3 = define_point_beacons(own_colour)
if own_colour.value == TeamColor.orange.value:
starting_position = geom.Point(-1210, 1400)
starting_orientation = 0
elif own_colour.value == TeamColor.purple.value:
starting_position = geom.Point(1210, 1400)
starting_orientation = np.pi
else:
return [None, None, None]
p_b1_lidar = geom.from_theoretical_table_to_lidar(b1, starting_position,
starting_orientation)
p_b2_lidar = geom.from_theoretical_table_to_lidar(b2, starting_position,
starting_orientation)
p_b3_lidar = geom.from_theoretical_table_to_lidar(b3, starting_position,
starting_orientation)
found_b1, found_b2, found_b3 = None, None, None
min_d_cluster_beacon1 = 3000
min_d_cluster_beacon2 = 3000
min_d_cluster_beacon3 = 3000
threshold_closest_beacon = 500
for cluster in clusters:
closest = cluster.get_closest_point_to_robot()
if closest is not None:
d_cluster_beacon1 = dr.distance_array(closest,
p_b1_lidar.to_array())
d_cluster_beacon2 = dr.distance_array(closest,
p_b2_lidar.to_array())
d_cluster_beacon3 = dr.distance_array(closest,
p_b3_lidar.to_array())
if d_cluster_beacon1 < min_d_cluster_beacon1 and d_cluster_beacon1 < threshold_closest_beacon:
min_d_cluster_beacon1 = d_cluster_beacon1
found_b1 = closest.copy()
if d_cluster_beacon2 < min_d_cluster_beacon2 and d_cluster_beacon2 < threshold_closest_beacon:
min_d_cluster_beacon1 = d_cluster_beacon2
found_b2 = closest.copy()
if d_cluster_beacon3 < min_d_cluster_beacon3 and d_cluster_beacon3 < threshold_closest_beacon:
min_d_cluster_beacon1 = d_cluster_beacon3
found_b3 = closest.copy()
return [found_b1, found_b2, found_b3]
def __init__(self, positions: List[geom.Point]):
center = geom.Point(0, 0)
for position in positions:
center = center + position
center = center.multiply_by(1 / 4.)
super().__init__(center)
self.positions = positions
assert len(positions) == 4
def find_beacons_with_odometry(clusters: List[Cluster], odometry_state,
own_colour: TeamColor, logger_name):
"""
Function which uses raw estimation of encoders for robot's position to find a priori positions of beacons.
:param logger_name:
:param clusters:
:param odometry_state:
:param own_colour:
:return:
"""
logger = logging.getLogger(logger_name)
odometry_position = geom.Point(odometry_state[0], odometry_state[1])
odometry_orientation = odometry_state[2]
b1, b2, b3 = define_point_beacons(own_colour)
logger.debug("position balise 1 table : " + str(b1))
logger.debug("position balise 2 table : " + str(b2))
logger.debug("position balise 3 table : " + str(b3))
p_b1_lidar = geom.from_theoretical_table_to_lidar(b1, odometry_position,
odometry_orientation)
p_b2_lidar = geom.from_theoretical_table_to_lidar(b2, odometry_position,
odometry_orientation)
p_b3_lidar = geom.from_theoretical_table_to_lidar(b3, odometry_position,
odometry_orientation)
logger.debug("position balise 1 lidar : " + str(p_b1_lidar))
logger.debug("position balise 2 lidar : " + str(p_b2_lidar))
logger.debug("position balise 3 lidar : " + str(p_b3_lidar))
found_b1, found_b2, found_b3 = None, None, None
for cluster in clusters:
closest = cluster.get_closest_point_to_robot()
if closest is not None:
d1 = dr.distance_array(closest, p_b1_lidar.to_array())
d2 = dr.distance_array(closest, p_b2_lidar.to_array())
d3 = dr.distance_array(closest, p_b3_lidar.to_array())
if d1 < 200:
found_b1 = closest.copy()
# print("mean et b1 : ", mean, p_b1_lidar)
if d2 < 200:
found_b2 = closest.copy()
# print("mean et b2 : ", mean, p_b2_lidar)
if d3 < 200:
found_b3 = closest.copy()
# print("mean et b3 : ", mean, p_b3_lidar)
return [found_b1, found_b2, found_b3]
def find_robots_in_purple_zone(clusters: List[Cluster]):
"""
:param clusters:
:return: x, y, radius, index, timestamp
"""
robot_position = geom.Point(PURPLE_SELF_X, PURPLE_SELF_Y)
robot_orientation = PURPLE_SELF_THETA
purple_zone = ta.Rectangle(P_PURPLE_START_ZONE)
if isinstance(clusters[0], Cluster):
return find_robots_in_zone(purple_zone, clusters, robot_position, robot_orientation)
return find_robots_in_zone(purple_zone, clusters, robot_position, robot_orientation)
def is_cluster_in_rectangle(self, cluster: Union[Cluster, List]):
if isinstance(cluster, Cluster):
for point in cluster.points:
if not self.is_point_in_rectangle(point):
return False
return len(cluster.points) > 0
elif type(cluster) == list:
for point in cluster:
print(point)
if not self.is_point_in_rectangle(
geom.Point(point[0], point[1])):
return False
return len(cluster) > 0
return False
def change_basis(rp: geom.Point, ori: float, measures: List):
"""
:param rp: robot position
:param ori: robot orietation
:param measures: list of angles and distances
:return:
"""
# measures = outr.one_turn_to_cartesian_points(measures)
new_measures = []
for measure in measures:
p = geom.Point(measure[0], measure[1])
p.rotate(-ori)
new_measure = p + rp
new_measures.append([new_measure.x, new_measure.y])
return new_measures
def rotate(self, angle: float):
"""
>>> v = geom.Vector()
>>> v.set_coordinates(2., 3.)
>>> v.rotate(np.pi)
>>> np.isclose(v.x, -2)
True
>>> np.isclose(v.y, -3)
True
:param angle:
:return:
"""
rotation_matrix = np.array([[np.cos(angle), -np.sin(angle)],
[np.sin(angle),
np.cos(angle)]])
positions = []
for position in self.positions:
res = rotation_matrix @ np.array([position.x, position.y]).T
point = geom.Point(res[0], res[1])
positions.append(point)
return Square(positions)
def __init__(self, limits: List[geom.Point]):
self.upper_left, self.lower_right = limits
center = geom.Point(0, 0)
for position in limits:
center = center + position
self.center = center.multiply_by(1 / 2.)
def take_symmetric(self):
self.positions = [
geom.Point(-position.x, position.y) for position in self.positions
]
self.center = geom.Point(-self.center.x, self.center.y)
def set_by_upper_left_and_lower_right(self, upper_left, lower_right):
self.x_center = (upper_left[0] + lower_right[0]) / 2
self.y_center = (upper_left[1] + lower_right[1]) / 2
self.center = geom.Point(self.x_center, self.y_center)
def set_parameters(self, x: int, y: int, r: int, i: int):
self.x_center = x
self.y_center = y
self.radius = r
self.index = i
self.center = geom.Point(x, y)
import numpy as np
import matplotlib.pyplot as pl
import lidarproc.main.geometry as geom
import lidarproc.main.table as ta
__author__ = "Clément Besnier"
table = ta.Table()
# for orange
beacon_1 = ta.Square([geom.Point(-1500 - 100, 2000), geom.Point(-1500, 2000), geom.Point(-1500, 2000 - 100),
geom.Point(-1500 - 100, 2000 - 100)])
beacon_2 = ta.Square([geom.Point(1500, 1000 + 50), geom.Point(1500 + 100, 1000 + 50),
geom.Point(1500 + 100, 1000 - 50), geom.Point(1500, 1000 - 50)])
beacon_3 = ta.Square([geom.Point(-1500 - 100, 0 + 100), geom.Point(-1500, 0 + 100), geom.Point(-1500, 0),
geom.Point(-1500 - 100, 0)])
beacon_1.take_symmetric()
beacon_2.take_symmetric()
beacon_3.take_symmetric()
table.add_square_obstacle(beacon_1)
table.add_square_obstacle(beacon_2)
table.add_square_obstacle(beacon_3)
table.add_edge_point(geom.Point(-1500, 0))
table.add_edge_point(geom.Point(1500, 0))
table.add_edge_point(geom.Point(1500, 2000))
table.add_edge_point(geom.Point(-1500, 2000))
def compute_own_state(beacons: List[np.ndarray], own_colour: TeamColor, logger_name) \
-> Tuple[Union[geom.Point, None], Union[float, None]]:
"""
The ultimate function which computes the position of the robot with the LiDAR!
:param logger_name:
:param beacons: clusters which are measures of beacons
:param own_colour: colour defined at the beginning of the match
:return:
"""
logger = logging.getLogger(logger_name)
# positions of beacons in the table basis
b1, b2, b3 = define_point_beacons(own_colour)
if beacons[0] is not None and beacons[2] is not None:
beacon1, beacon3 = beacons[0], beacons[2]
logger.debug("balises 1 et 3 mesurées : " + str(beacon1) + " " +
str(beacon3))
p_beacon1 = geom.Point.from_array(beacon1)
p_beacon3 = geom.Point.from_array(beacon3)
# region computes robot position
d13 = 1900 # dr.distance_array(beacon1, beacon3)
d1 = np.sqrt(beacon1 @ beacon1.T) + FIX_BEACON_RADIUS
d3 = np.sqrt(beacon3 @ beacon3.T) + FIX_BEACON_RADIUS
logger.debug("d1 d3 d13 : " + str(d1) + ' ' + str(d3) + ' ' + str(d13))
angle1 = np.arccos((d1**2 + d13**2 - d3**2) / (2 * d1 * d13))
angle3 = np.arccos((d3**2 + d13**2 - d1**2) / (2 * d3 * d13))
# logger.debug("angle alpha 1 "+str(np.rad2deg(angle1)))
# logger.debug("angle alpha 3 "+str(np.rad2deg(angle3)))
# logger.debug("angle alpha 1 rad " + str(angle1))
# logger.debug("angle alpha 3 rad " + str(angle3))
# theta_r = np.pi/2 - angle3 +
if own_colour == TeamColor.purple:
# robot_x = b3.x - np.sin(angle3)*d3
# robot_y = b3.y + np.cos(angle3)*d3
#
# print("From b3", geom.Point(robot_x, robot_y))
logger.debug("b1 : x et y" + str(b1.x) + " " + str(b1.y))
logger.debug("b3 : x et y" + str(b3.x) + " " + str(b3.y))
robot_x = b1.x - np.sin(angle1) * d1
robot_y = b1.y - np.cos(angle1) * d1
robot_position = geom.Point(robot_x, robot_y)
logger.debug("From b1 " + str(robot_position))
# robot_x = b1.x - np.sin(angle3) * d3
# robot_y = b1.y - np.cos(angle3) * d3
#
# robot_position = geom.Point(robot_x, robot_y)
# logger.debug("From b3 " + str(robot_position))
elif own_colour == TeamColor.orange:
robot_x = b1.x + np.sin(angle1) * d1
robot_y = b1.y - np.cos(angle1) * d1
# print("From b1", geom.Point(robot_x, robot_y))
# robot_x = b3.x + np.sin(angle3) * d3
# robot_y = b3.y - np.cos(angle3) * d3
robot_position = geom.Point(robot_x, robot_y)
logger.debug("From b1 " + str(robot_position))
else:
logger.debug("La couleur donnée n'est pas bonne")
return None, None
# region # computes robot orientation
v_table_beacons = geom.Vector()
v_table_beacons.set_by_points(b1, b3)
v_lidar_beacons = geom.Vector()
v_lidar_beacons.set_by_points(p_beacon1 - robot_position,
p_beacon3 - robot_position)
lidar_beacons_angle = v_lidar_beacons.compute_basis_angle()
# logger.debug("angle table beacons angle "+str(np.rad2deg(table_beacons_angle)))
# logger.debug("angle table beacons angle rad "+str(table_beacons_angle))
# logger.debug("angle lidar beacons angle "+str(np.rad2deg(lidar_beacons_angle)))
# logger.debug("angle lidar beacons angle rad"+str(lidar_beacons_angle))
robot_orientation = lidar_beacons_angle % (2 * np.pi)
logger.debug("Orientation du robot : " + str(robot_orientation))
# endregion
return robot_position, robot_orientation
else:
if beacons[0] is None:
logger.debug("La balise 1 n'est pas détectée")
if beacons[2] is None:
logger.debug("La balise 3 n'est pas détectée")
return None, None
clusters to the expected positions of beacons are searched.
"""
from typing import List, Tuple, Union, Iterable
import numpy as np
from lidarproc.main.constants import *
from lidarproc.main.clustering import Cluster, Beacon
import lidarproc.main.geometry as geom
import lidarproc.main.data_retrieval as dr
import lidarproc.main.table as table
__author__ = "Clément Besnier"
p_beacons_orange = [[geom.Point(point[0], point[1]) for point in limits]
for limits in beacons_orange]
p_beacons_purple = [[geom.Point(point[0], point[1]) for point in limits]
for limits in beacons_purple]
def define_point_beacons(own_colour: TeamColor):
"""
Gets the positions of the beacons according to our colour in the table basis.
:param own_colour:
:return:
"""
if own_colour.value == TeamColor.orange.value:
bo1 = table.Rectangle(p_beacons_orange[0]).get_center()
bo2 = table.Rectangle(p_beacons_orange[1]).get_center()
bo3 = table.Rectangle(p_beacons_orange[2]).get_center()
"""
import time
from typing import List
import numpy as np
import lidarproc.main.geometry as geom
from lidarproc.main.constants import *
from lidarproc.main.clustering import Cluster
import lidarproc.main.table as ta
__author__ = "Clément Besnier"
P_PURPLE_START_ZONE = [geom.Point(p[0], p[1]) for p in PURPLE_START_ZONE]
P_ORANGE_START_ZONE = [geom.Point(o[0], o[1]) for o in ORANGE_START_ZONE]
def find_robots_in_zone(zone: ta.Rectangle, clusters: List[Cluster], robot_position: geom.Point,
robot_orientation: float):
"""
:param zone:
:param clusters:
:param robot_position:
:param robot_orientation:
:return:
"""
robot_paramters = []